网站与域名的区别,广州新际网站建设公司怎么样,网络推广工作任务和职业能力,win7搭建wordpress1.常用的几种loam算法 aloam 纯激光 lego_loam 纯激光 去除了地面 lio_sam imu激光紧耦合 lvi_sam 激光视觉 2.代码思路 2.1.特征点提取scanRegistration.cpp#xff0c;这个文件的目的是为了根据曲率提取4种特征点和对当前点云进行预处理 输入是雷达点云话题 输出是 4种特征点…1.常用的几种loam算法 aloam 纯激光 lego_loam 纯激光 去除了地面 lio_sam imu激光紧耦合 lvi_sam 激光视觉 2.代码思路 2.1.特征点提取scanRegistration.cpp这个文件的目的是为了根据曲率提取4种特征点和对当前点云进行预处理 输入是雷达点云话题 输出是 4种特征点点云和预处理后的当前点云 1带有点云线束id在角度范围所处进度的全部有效点点云因为雷达是旋转的雷达旋转的进度 2曲率比较大的特征点点云2个点 3曲率一般大的特征点点云20个点 4曲率比较小的面点点云 5一般面点点云角点提取剩下的那些点 当输入雷达是16线雷达去计算是哪条线id是通过计算俯仰角每2度算一根线去计算的 角度所处的进度是根据水平角计算获得的 2.1.1选择雷达扫描半径范围内的点 pcl::PointCloudpcl::PointXYZ narrowed_scan;narrowed_scan.header scan.header;if (min_range max_range){ROS_ERROR_ONCE(min_rangemax_range (%lf, %lf), min_range, max_range);return scan;}double square_min_range min_range * min_range;double square_max_range max_range * max_range;for (pcl::PointCloudpcl::PointXYZ::const_iterator iter scan.begin(); iter ! scan.end(); iter){const pcl::PointXYZ p *iter;// 点云点到原点的位置的欧式距离double square_distance p.x * p.x p.y * p.y;if (square_min_range square_distance square_distance square_max_range){narrowed_scan.points.push_back(p);}}return narrowed_scan;2.1.2计算水平角角度范围 int cloudSize laserCloudIn.points.size();
// 起始点角度 atan2范围是-pipi
float startOri -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
// 终止点角度
float endOri -atan2(laserCloudIn.points[cloudSize - 1].y,laserCloudIn.points[cloudSize - 1].x) 2 * M_PI;
// 总有些例外 比如这里大于3pi 和小于pi 就要做一些调整到合理的范围
if (endOri - startOri 3 * M_PI)
{endOri - 2 * M_PI;
}
else if (endOri - startOri M_PI)
{endOri 2 * M_PI;
}
Eigen::Vector2f anglerange;
anglerange[0] startOri;
anglerange[1] endOri;
return anglerange;2.1.3每条线上点的计算范围
// 计算的范围 起始点是从第5个点开始 终止点是倒数第6个点
for (int i 0; i N_SCANS; i)
{// 第一根线就是 05scanStartInd[i] laserCloud-size() 5;*laserCloud laserCloudScans[i];// 第一根线就是第一根线的点数量-6scanEndInd[i] laserCloud-size() - 6;
}2.1.4计算每个点的曲率 // 开始计算曲率for (int i 5; i cloudSize - 5; i){// 每一小段计算了弧长float diffX laserCloud-points[i - 5].x laserCloud-points[i - 4].x laserCloud-points[i - 3].x laserCloud-points[i - 2].x laserCloud-points[i - 1].x - 10 * laserCloud-points[i].x laserCloud-points[i 1].x laserCloud-points[i 2].x laserCloud-points[i 3].x laserCloud-points[i 4].x laserCloud-points[i 5].x;float diffY laserCloud-points[i - 5].y laserCloud-points[i - 4].y laserCloud-points[i - 3].y laserCloud-points[i - 2].y laserCloud-points[i - 1].y - 10 * laserCloud-points[i].y laserCloud-points[i 1].y laserCloud-points[i 2].y laserCloud-points[i 3].y laserCloud-points[i 4].y laserCloud-points[i 5].y;float diffZ laserCloud-points[i - 5].z laserCloud-points[i - 4].z laserCloud-points[i - 3].z laserCloud-points[i - 2].z laserCloud-points[i - 1].z - 10 * laserCloud-points[i].z laserCloud-points[i 1].z laserCloud-points[i 2].z laserCloud-points[i 3].z laserCloud-points[i 4].z laserCloud-points[i 5].z;// 存储起始5到终止-6每个点对应的曲率cloudCurvature[i] diffX * diffX diffY * diffY diffZ * diffZ;// 每个点的索引cloudSortInd[i] i;// 标记cloudNeighborPicked[i] 0;cloudLabel[i] 0;}2.1.4对每个点的曲率进行排序 float t_q_sort 0;
// 遍历每个scan
for (int i 0; i N_SCANS; i)
{// 点云点数小于6个就认为 没有有效的点 就进行continueif (scanEndInd[i] - scanStartInd[i] 6){continue;}// 用来存储不太平整的点pcl::PointCloudPointType::Ptr surfPointsLessFlatScan(new pcl::PointCloudPointType);// 将每个scan分成6等分for (int j 0; j 6; j){// 每个等分的起点和结束点// 起点idint sp scanStartInd[i] (scanEndInd[i] - scanStartInd[i]) * j / 6;// 结束点idint ep scanStartInd[i] (scanEndInd[i] - scanStartInd[i]) * (j 1) / 6 - 1;TicToc t_tmp;// 对点云曲率进行索引排序小的在前大的在后std::sort(cloudSortInd sp, cloudSortInd ep 1, comp);t_q_sort t_tmp.toc();int largestPickedNum 0;// 挑选曲率比较大的部分for (int k ep; k sp; k--){// 排序后顺序就乱了这个时候索引的作用就体现出来了int ind cloudSortInd[k];// 看看这个点是否是有效点同时曲率是否大于阈值if (cloudNeighborPicked[ind] 0 cloudCurvature[ind] 0.1){largestPickedNum;// //目的是为了当前帧大曲率的点和上一帧小一点曲率的点建立约束// 每段选两个曲率大的点if (largestPickedNum 2){cloudLabel[ind] 2;// cornerPointsSharp存放大曲率的点cornerPointsSharp.push_back(laserCloud-points[ind]);cornerPointsLessSharp.push_back(laserCloud-points[ind]);}// 以及20个曲率稍微大一点的点else if (largestPickedNum 20){// label置1表示曲率稍微大一点的点cloudLabel[ind] 1;cornerPointsLessSharp.push_back(laserCloud-points[ind]);}// 超过20个就算了else{break;}// 这个点被选中了pick标志位置1cloudNeighborPicked[ind] 1;// 为了保证特征点不过渡集中将选中的点周围5个点都置为1避免后续会选到for (int l 1; l 5; l){// 查看相邻点距离是否差异过大如果差异过大说明点云在此不连续是特征边缘就会是新的特征因此就不置位了float diffX laserCloud-points[ind l].x - laserCloud-points[ind l - 1].x;float diffY laserCloud-points[ind l].y - laserCloud-points[ind l - 1].y;float diffZ laserCloud-points[ind l].z - laserCloud-points[ind l - 1].z;if (diffX * diffX diffY * diffY diffZ * diffZ 0.05){break;}cloudNeighborPicked[ind l] 1;}// 下面同理for (int l -1; l -5; l--){float diffX laserCloud-points[ind l].x - laserCloud-points[ind l 1].x;float diffY laserCloud-points[ind l].y - laserCloud-points[ind l 1].y;float diffZ laserCloud-points[ind l].z - laserCloud-points[ind l 1].z;if (diffX * diffX diffY * diffY diffZ * diffZ 0.05){break;}cloudNeighborPicked[ind l] 1;}}}// 下面开始挑选面点int smallestPickedNum 0;for (int k sp; k ep; k){int ind cloudSortInd[k];// 确定这个点没有被pack 并且曲率小于阈值if (cloudNeighborPicked[ind] 0 cloudCurvature[ind] 0.1){// -1是认为平坦的点cloudLabel[ind] -1;surfPointsFlat.push_back(laserCloud-points[ind]);smallestPickedNum;// 每个等分取4个比较平坦的面点// 这里不区分平坦和比较平坦因为剩下的点label默认是0就是比较平坦if (smallestPickedNum 4){break;}cloudNeighborPicked[ind] 1;// 下面同理 除非曲率在0.05-0.1之间的点 否则就要标记后5个点for (int l 1; l 5; l){float diffX laserCloud-points[ind l].x - laserCloud-points[ind l - 1].x;float diffY laserCloud-points[ind l].y - laserCloud-points[ind l - 1].y;float diffZ laserCloud-points[ind l].z - laserCloud-points[ind l - 1].z;if (diffX * diffX diffY * diffY diffZ * diffZ 0.05){break;}cloudNeighborPicked[ind l] 1;}// 标记前5个点for (int l -1; l -5; l--){float diffX laserCloud-points[ind l].x - laserCloud-points[ind l 1].x;float diffY laserCloud-points[ind l].y - laserCloud-points[ind l 1].y;float diffZ laserCloud-points[ind l].z - laserCloud-points[ind l 1].z;if (diffX * diffX diffY * diffY diffZ * diffZ 0.05){break;}cloudNeighborPicked[ind l] 1;}}}for (int k sp; k ep; k){// 除了角点 其它的点都是一般平坦的点云if (cloudLabel[k] 0){surfPointsLessFlatScan-push_back(laserCloud-points[k]);}}}pcl::PointCloudPointType surfPointsLessFlatScanDS;pcl::VoxelGridPointType downSizeFilter;// 一般平坦的点比较多所以这里做一个体素滤波downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.setLeafSize(0.2, 0.2, 0.2);downSizeFilter.filter(surfPointsLessFlatScanDS);surfPointsLessFlat surfPointsLessFlatScanDS;
}
printf(sort q time %f \n, t_q_sort);2.1.5 发布4种特征点及插入标志的整体点云 // 分别将当前点云 四种特征的点云发布出去
sensor_msgs::PointCloud2 laserCloudOutMsg;
// 1.整体的点云 对每个点打了标签哪一根线id在角度范围所处的一个进度
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp laserCloudMsg-header.stamp;
laserCloudOutMsg.header.frame_id /map;
pubLaserCloud.publish(laserCloudOutMsg);sensor_msgs::PointCloud2 cornerPointsSharpMsg;
// 2.大曲率特征点
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp laserCloudMsg-header.stamp;
cornerPointsSharpMsg.header.frame_id /map;
pubCornerPointsSharp.publish(cornerPointsSharpMsg);sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
// 3.曲率稍微大一点的特征点
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp laserCloudMsg-header.stamp;
cornerPointsLessSharpMsg.header.frame_id /map;
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);sensor_msgs::PointCloud2 surfPointsFlat2;
// 4.平坦的点
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp laserCloudMsg-header.stamp;
surfPointsFlat2.header.frame_id /map;
pubSurfPointsFlat.publish(surfPointsFlat2);
// 5.一般平坦的点
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp laserCloudMsg-header.stamp;
surfPointsLessFlat2.header.frame_id /map;
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);2.2.激光里程计laserOdometry.cpp 1对特征点提取后的5个点云进行回调并存放到队列里并同时转成pcl点云格式 2两次迭代寻找角点约束和面点约束 3角点约束首先进行雷达运动补偿通过kdtee从上一帧中寻找距离当前帧角点最近的一个点p1根据p1去找不同线上距离最近的点p2 根据点到直线的垂线距离构建残差方程给到ceres可以求解出点到线的约束 (4)面点约束首先进行雷达运动补偿通过kdtee从上一帧中寻找距离当前帧角点最近的一个点p1根据p1去找相同线上距离最近的点p2根据p1去找不同线上距离最近的点p3根据点到平面的距离构建残差方程给到ceres可以求解出点到面的约束 5通过两次迭代进行ceres求解 得到最终的帧间约束结果与之前的坐标约束进行求解就得到了前端激光里程计 2.2.1运动补偿部分 激光雷达运动补偿就是把所有的点补偿到某一时刻这样就可以把本身过去100ms的点收集到一个时间点上去
void TransformToStart(PointType const *const pi, PointType *const po)
{// interpolation ratiodouble s;// 由于kitti数据集上的lidar已经做过了运动补偿因此这里就不做具体补偿了if (DISTORTION)s (pi-intensity - int(pi-intensity)) / SCAN_PERIOD;elses 1.0; // s1s说明全部补偿到点云结束的时刻// s 1;// 所有点的操作方式都是一致的相当于从结束时刻补偿到起始时刻// 这里相当于是一个匀速模型的假设Eigen::Quaterniond q_point_last Eigen::Quaterniond::Identity().slerp(s, q_last_curr);Eigen::Vector3d t_point_last s * t_last_curr;Eigen::Vector3d point(pi-x, pi-y, pi-z);Eigen::Vector3d un_point q_point_last * point t_point_last;po-x un_point.x();po-y un_point.y();po-z un_point.z();po-intensity pi-intensity;
}根据反变换求出结束时刻的点坐标附公式推解
void TransformToEnd(PointType const *const pi, PointType *const po)
{// undistort point firstpcl::PointXYZI un_point_tmp;// 把所有点补偿到起始时刻TransformToStart(pi, un_point_tmp);//再 通过反变换的方式 将起始时刻坐标系下的点 转到 结束时刻坐标系下 Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);//取出起始时刻坐标系下的点//q_last_curr \ t_last_curr 是结束时刻坐标系转到起始时刻坐标系 的 旋转 和 平移 Eigen::Vector3d point_end q_last_curr.inverse() * (un_point - t_last_curr);//通过反变换,求得转到 结束时刻坐标系下 的点坐标po-x point_end.x();po-y point_end.y();po-z point_end.z();// Remove distortion time infopo-intensity int(pi-intensity);
}2.2.2确保5个点云都不为空 // 首先确保5个消息都有有一个队列为空都不行if (!cornerSharpBuf.empty() !cornerLessSharpBuf.empty() !surfFlatBuf.empty() !surfLessFlatBuf.empty() !fullPointsBuf.empty()){return false;}else{return true;}2.2.3通过比较时间戳判断是否是同一帧 // 分别求出队列第一个时间timeCornerPointsSharp cornerSharpBuf.front()-header.stamp.toSec();timeCornerPointsLessSharp cornerLessSharpBuf.front()-header.stamp.toSec();timeSurfPointsFlat surfFlatBuf.front()-header.stamp.toSec();timeSurfPointsLessFlat surfLessFlatBuf.front()-header.stamp.toSec();timeLaserCloudFullRes fullPointsBuf.front()-header.stamp.toSec();// 因为同一帧时间戳是相同的因此这里比较是否是同一帧if (timeCornerPointsSharp ! timeLaserCloudFullRes ||timeCornerPointsLessSharp ! timeLaserCloudFullRes ||timeSurfPointsFlat ! timeLaserCloudFullRes ||timeSurfPointsLessFlat ! timeLaserCloudFullRes){printf(点云消息时间戳不同步!);return true;}else{return false;}2.2.4传感器格式转换成点云格式 // 分别将5个消息取出来同时转成pcl的点云格式
mBuf.lock();
cornerPointsSharp-clear();
// 将第一根元素存放到cornerPointsSharp 就是当前的点云
pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);
// 移除前端的第一个元素 当前待处理的点云
cornerSharpBuf.pop();cornerPointsLessSharp-clear();
pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);
cornerLessSharpBuf.pop();surfPointsFlat-clear();
pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);
surfFlatBuf.pop();surfPointsLessFlat-clear();
pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);
surfLessFlatBuf.pop();laserCloudFullRes-clear();
pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);
fullPointsBuf.pop();
mBuf.unlock();2.2.5 点到线残差构建 for (int i 0; i cornerPointsSharpNum; i)
{// 运动补偿TransformToStart((cornerPointsSharp-points[i]), pointSel);// 在上一帧所有角点构成的kdtee中寻找距离当前帧最近的一个点kdtreeCornerLast-nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd -1, minPointInd2 -1;// 只有小于给定界限才认为是有效约束if (pointSearchSqDis[0] DISTANCE_SQ_THRESHOLD){closestPointInd pointSearchInd[0]; // 对应的最近距离约束的索引取出来// 找到其所对应的线束id 线束信息隐藏在intensity中int closestPointScanID int(laserCloudCornerLast-points[closestPointInd].intensity);double minPointSqDis2 DISTANCE_SQ_THRESHOLD;// 寻找角点在刚刚角点的id上下分别继续寻找目的是找到最近的角点由于其按照约束进行排序所以就是向上找for (int j closestPointInd 1; j (int)laserCloudCornerLast-points.size(); j){// 不找同一根线束的if (int(laserCloudCornerLast-points[j].intensity) closestPointScanID)continue;// 要求找到的线束距离当前线束不能太远if (int(laserCloudCornerLast-points[j].intensity) (closestPointScanID NEARBY_SCAN))break;// 上一帧线的第2个点 到当前帧点的距离double pointSqDis (laserCloudCornerLast-points[j].x - pointSel.x) *(laserCloudCornerLast-points[j].x - pointSel.x) (laserCloudCornerLast-points[j].y - pointSel.y) *(laserCloudCornerLast-points[j].y - pointSel.y) (laserCloudCornerLast-points[j].z - pointSel.z) *(laserCloudCornerLast-points[j].z - pointSel.z);if (pointSqDis minPointSqDis2){minPointSqDis2 pointSqDis;minPointInd2 j;}}// 同样另外一个方向寻找角点for (int j closestPointInd - 1; j 0; --j){if (int(laserCloudCornerLast-points[j].intensity) closestPointScanID)continue;if (int(laserCloudCornerLast-points[j].intensity) (closestPointScanID - NEARBY_SCAN))break;double pointSqDis (laserCloudCornerLast-points[j].x - pointSel.x) *(laserCloudCornerLast-points[j].x - pointSel.x) (laserCloudCornerLast-points[j].y - pointSel.y) *(laserCloudCornerLast-points[j].y - pointSel.y) (laserCloudCornerLast-points[j].z - pointSel.z) *(laserCloudCornerLast-points[j].z - pointSel.z);if (pointSqDis minPointSqDis2){// 取出当前点和上一帧的两个角点minPointSqDis2 pointSqDis;minPointInd2 j;}}}// 最近点所在的线束if (minPointInd2 0){// 当前点Eigen::Vector3d curr_point(cornerPointsSharp-points[i].x,cornerPointsSharp-points[i].y,cornerPointsSharp-points[i].z);// 距离当前点最近的上一帧的点Eigen::Vector3d last_point_a(laserCloudCornerLast-points[closestPointInd].x,laserCloudCornerLast-points[closestPointInd].y,laserCloudCornerLast-points[closestPointInd].z);// 距离上一帧点最近的不同线束上的第二个点 构成棱Eigen::Vector3d last_point_b(laserCloudCornerLast-points[minPointInd2].x,laserCloudCornerLast-points[minPointInd2].y,laserCloudCornerLast-points[minPointInd2].z);double s;if (DISTORTION)// 点在起始点到结束点一周中的进度s (cornerPointsSharp-points[i].intensity - int(cornerPointsSharp-points[i].intensity)) / SCAN_PERIOD;elses 1.0;// 残差项ceres::CostFunction *cost_function LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);// 添加残差块 残差项 损失函数 待优化的变量problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);corner_correspondence;}
}2.2.6 点到面残差构建 for (int i 0; i surfPointsFlatNum; i)
{TransformToStart((surfPointsFlat-points[i]), pointSel);// 先找上一帧距离当前帧最近的面点kdtreeSurfLast-nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd -1, minPointInd2 -1, minPointInd3 -1;// 距离必须小于阈值if (pointSearchSqDis[0] DISTANCE_SQ_THRESHOLD){// 取出找到的上一帧面点的索引closestPointInd pointSearchInd[0];// 取出最近的面点在上一帧的那一条scan线上int closestPointScanID int(laserCloudSurfLast-points[closestPointInd].intensity);double minPointSqDis2 DISTANCE_SQ_THRESHOLD, minPointSqDis3 DISTANCE_SQ_THRESHOLD;// 额外在寻找两个点要求一个点和最近点同一个scan线 另一个点不同的scan// 按照增量方向寻找其它面点for (int j closestPointInd 1; j (int)laserCloudSurfLast-points.size(); j){// 不能和当前找到的上一帧面点线束太远if (int(laserCloudSurfLast-points[j].intensity) (closestPointScanID NEARBY_SCAN))break;// 计算和当前帧该点距离double pointSqDis (laserCloudSurfLast-points[j].x - pointSel.x) *(laserCloudSurfLast-points[j].x - pointSel.x) (laserCloudSurfLast-points[j].y - pointSel.y) *(laserCloudSurfLast-points[j].y - pointSel.y) (laserCloudSurfLast-points[j].z - pointSel.z) *(laserCloudSurfLast-points[j].z - pointSel.z);// 如果是同一根scan且距离最近if (int(laserCloudSurfLast-points[j].intensity) closestPointScanID pointSqDis minPointSqDis2){minPointSqDis2 pointSqDis;minPointInd2 j;}// 如果是其它的线束点else if (int(laserCloudSurfLast-points[j].intensity) closestPointScanID pointSqDis minPointSqDis3){minPointSqDis3 pointSqDis;minPointInd3 j;}}// 同样的方式 按照降序的方式去找这两个点for (int j closestPointInd - 1; j 0; --j){if (int(laserCloudSurfLast-points[j].intensity) (closestPointScanID - NEARBY_SCAN))break;double pointSqDis (laserCloudSurfLast-points[j].x - pointSel.x) *(laserCloudSurfLast-points[j].x - pointSel.x) (laserCloudSurfLast-points[j].y - pointSel.y) *(laserCloudSurfLast-points[j].y - pointSel.y) (laserCloudSurfLast-points[j].z - pointSel.z) *(laserCloudSurfLast-points[j].z - pointSel.z);if (int(laserCloudSurfLast-points[j].intensity) closestPointScanID pointSqDis minPointSqDis2){minPointSqDis2 pointSqDis;minPointInd2 j;}else if (int(laserCloudSurfLast-points[j].intensity) closestPointScanID pointSqDis minPointSqDis3){minPointSqDis3 pointSqDis;minPointInd3 j;}}// 如果找到的另外两个点是有效值就取出它们的3d坐标if (minPointInd2 0 minPointInd3 0){// 当前角点Eigen::Vector3d curr_point(surfPointsFlat-points[i].x,surfPointsFlat-points[i].y,surfPointsFlat-points[i].z);// 上一帧距离当前焦点最近的点Eigen::Vector3d last_point_a(laserCloudSurfLast-points[closestPointInd].x,laserCloudSurfLast-points[closestPointInd].y,laserCloudSurfLast-points[closestPointInd].z);Eigen::Vector3d last_point_b(laserCloudSurfLast-points[minPointInd2].x,laserCloudSurfLast-points[minPointInd2].y,laserCloudSurfLast-points[minPointInd2].z);Eigen::Vector3d last_point_c(laserCloudSurfLast-points[minPointInd3].x,laserCloudSurfLast-points[minPointInd3].y,laserCloudSurfLast-points[minPointInd3].z);double s;if (DISTORTION)s (surfPointsFlat-points[i].intensity - int(surfPointsFlat-points[i].intensity)) / SCAN_PERIOD;elses 1.0;// 构建点到面的约束ceres::CostFunction *cost_function LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);plane_correspondence;}}
}2.2.7发布激光里程计和角点面点降频发送给后端 // 发布雷达里程计结果
nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id /map;
laserOdometry.child_frame_id /laser_odom;
laserOdometry.header.stamp ros::Time().fromSec(timeSurfPointsLessFlat);
// 以四元数和平移向量发布出去
laserOdometry.pose.pose.orientation.x q_w_curr.x();
laserOdometry.pose.pose.orientation.y q_w_curr.y();
laserOdometry.pose.pose.orientation.z q_w_curr.z();
laserOdometry.pose.pose.orientation.w q_w_curr.w();
laserOdometry.pose.pose.position.x t_w_curr.x();
laserOdometry.pose.pose.position.y t_w_curr.y();
laserOdometry.pose.pose.position.z t_w_curr.z();
pubLaserOdometry.publish(laserOdometry);
// 激光里程计路径
geometry_msgs::PoseStamped laserPose;
nav_msgs::Path laserPath;
laserPose.header laserOdometry.header;
laserPose.pose laserOdometry.pose.pose;
laserPath.header.stamp laserOdometry.header.stamp;
laserPath.poses.push_back(laserPose);
laserPath.header.frame_id /map;
pubLaserPath.publish(laserPath);
// 一般角点
pcl::PointCloudPointType::Ptr laserCloudTemp cornerPointsLessSharp;
// 上一帧的一般角点
cornerPointsLessSharp laserCloudCornerLast;
laserCloudCornerLast laserCloudTemp;//
laserCloudTemp surfPointsLessFlat;
surfPointsLessFlat laserCloudSurfLast;
laserCloudSurfLast laserCloudTemp;laserCloudCornerLastNum laserCloudCornerLast-points.size();
laserCloudSurfLastNum laserCloudSurfLast-points.size();// kdtree设置当前帧用来下一帧lidar odom使用
kdtreeCornerLast-setInputCloud(laserCloudCornerLast);
kdtreeSurfLast-setInputCloud(laserCloudSurfLast);
// 一定降频后给后端发送
if (frameCount % skipFrameNum 0)
{frameCount 0;// 一般角点sensor_msgs::PointCloud2 laserCloudCornerLast2;pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);laserCloudCornerLast2.header.stamp ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudCornerLast2.header.frame_id /camera;pubLaserCloudCornerLast.publish(laserCloudCornerLast2);// 面点sensor_msgs::PointCloud2 laserCloudSurfLast2;pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);laserCloudSurfLast2.header.stamp ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudSurfLast2.header.frame_id /camera;pubLaserCloudSurfLast.publish(laserCloudSurfLast2);// 整体点云sensor_msgs::PointCloud2 laserCloudFullRes3;pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);laserCloudFullRes3.header.stamp ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudFullRes3.header.frame_id /camera;pubLaserCloudFullRes.publish(laserCloudFullRes3);
}2.3 scan-map 后端匹配 点云插入地图laserMapping.cpp
2.3.1 传感器数据类型转换成点云 odom转换为eigen类型
// 点云全部转换为pcl的数据格式laserCloudCornerLast-clear();pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);cornerLastBuf.pop();laserCloudSurfLast-clear();pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);surfLastBuf.pop();laserCloudFullRes-clear();pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);fullResBuf.pop();// lidar odom 的结果转成eigen数据格式q_wodom_curr.x() odometryBuf.front()-pose.pose.orientation.x;q_wodom_curr.y() odometryBuf.front()-pose.pose.orientation.y;q_wodom_curr.z() odometryBuf.front()-pose.pose.orientation.z;q_wodom_curr.w() odometryBuf.front()-pose.pose.orientation.w;t_wodom_curr.x() odometryBuf.front()-pose.pose.position.x;t_wodom_curr.y() odometryBuf.front()-pose.pose.position.y;t_wodom_curr.z() odometryBuf.front()-pose.pose.position.z;odometryBuf.pop();// 考虑到实时性就把队列其他的都pop出去不然可能出现处理延时的情况while (!cornerLastBuf.empty()){cornerLastBuf.pop();printf(普通面点未清空 \n);}mBuf.unlock();2.3.2 根据前端结果 得到后端的初始位姿 // q_wodom_curr t_wodom_curr 是雷达的odom// q_w_curr t_w_curr是map坐标系下的位姿q_w_curr q_wmap_wodom * q_wodom_curr;t_w_curr q_wmap_wodom * t_wodom_curr t_wmap_wodom;2.3.3根据位置获得全局地图的中心格子 // 根据初始估计值计算寻找当前位姿在地图中的索引一个各自边长是50m// 后端的地图本质上是一个以当前点为中心的一个栅格地图// 判断在全局栅格的哪一个栅格里一个栅格是50m 栅格中心是25m// t_w_curr 是map坐标系下的位姿 centerCubeI网格中心centerCubeI int((t_w_curr.x() 25.0) / 50.0) laserCloudCenWidth;centerCubeJ int((t_w_curr.y() 25.0) / 50.0) laserCloudCenHeight;centerCubeK int((t_w_curr.z() 25.0) / 50.0) laserCloudCenDepth;// 由于c语言的取整是向0取整 因此如-1.66 成为了-1 但-2才是正确的因此这里自减1if (t_w_curr.x() 25.0 0)centerCubeI--;if (t_w_curr.y() 25.0 0)centerCubeJ--;if (t_w_curr.z() 25.0 0)centerCubeK--;2.3.4 根据机器人位置 更新全局地图范围 其它方向雷同 // 如果当前centerCubeI栅格索引小于3就说明当前点快接近地图边界了需要进行调整相当于地图整体往x正方向移动while (centerCubeI 3){for (int j 0; j laserCloudHeight; j){for (int k 0; k laserCloudDepth; k){// laserCloudWidth是widtch方向栅格总大小21 laserCloudHeight 21int i laserCloudWidth - 1;// 从x最大值开始pcl::PointCloudPointType::Ptr laserCloudCubeCornerPointer // 角点laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];pcl::PointCloudPointType::Ptr laserCloudCubeSurfPointer // 面点laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];// 整体右移for (; i 1; i--){laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCornerArray[i - 1 laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudSurfArray[i - 1 laserCloudWidth * j laserCloudWidth * laserCloudHeight * k];}// 此时i0也就是最左边的格子赋值了之前最右边的格子laserCloudCornerArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeCornerPointer;laserCloudSurfArray[i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k] laserCloudCubeSurfPointer;// 该点云清零由于是指针操作相当于最左边的格子清空了laserCloudCubeCornerPointer-clear();laserCloudCubeSurfPointer-clear();}}// 索引右移centerCubeI;laserCloudCenWidth;}2.3.5根据全局地图中心 提取局部地图每个格子在全局地图中的位置 // 从当前格子为中心,选出地图中一定范围的点云 5*5*3 75个cube
for (int i centerCubeI - 2; i centerCubeI 2; i) // 宽度方向
{for (int j centerCubeJ - 2; j centerCubeJ 2; j) // 高度方向{for (int k centerCubeK - 1; k centerCubeK 1; k) // 深度方向{if (i 0 i laserCloudWidth j 0 j laserCloudHeight k 0 k laserCloudDepth){// 把每个格子序号依次存到对应的索引// i laserCloudWidth * j 二维度平面位置// 每个格子在三维全局地图中的位置laserCloudValidInd[laserCloudValidNum] i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k;// 局部地图格子数量laserCloudValidNum;laserCloudSurroundInd[laserCloudSurroundNum] i laserCloudWidth * j laserCloudWidth * laserCloudHeight * k;laserCloudSurroundNum;}}}
}2.3.6当前帧 根据每个格子的在全局地图中的id将局部地图的每个格子角点和面点分别叠加 laserCloudCornerFromMap-clear();
laserCloudSurfFromMap-clear();
// 开始构建用来这一帧优化的小的局部地图 根据上面得到的索引进行叠加求和
for (int i 0; i laserCloudValidNum; i)
{// 角点叠加// laserCloudValidInd[i] 每个格子的在全局地图中的位置*laserCloudCornerFromMap *laserCloudCornerArray[laserCloudValidInd[i]];// 面点叠加*laserCloudSurfFromMap *laserCloudSurfArray[laserCloudValidInd[i]];
}2.3.7对当前帧角点面点下采样 // 角点
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerStack);
// 面点
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfStack);2.3.8点线残差构建 这里和前端有区别 通过最邻近的5个地图点进行构建协方差矩阵通过协方差矩阵最大特征值与次大特征值判断是否存在直线
int corner_num 0;// 构建角点相关约束for (int i 0; i laserCloudCornerStackNum; i){// 实时角点 点坐标pointOri laserCloudCornerStack-points[i];// 把雷达点转换到map坐标系pointAssociateToMap(pointOri, pointSel);// 局部地图中寻找和该点最近的5个点// pointSearchInd 5个点在局部地图中的索引kdtreeCornerFromMap-nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);// 判断最远的点距离不能超过1m,否则就是无效约束if (pointSearchSqDis[4] 1.0){std::vectorEigen::Vector3d nearCorners;Eigen::Vector3d center(0, 0, 0);for (int j 0; j 5; j){Eigen::Vector3d tmp(laserCloudCornerFromMap-points[pointSearchInd[j]].x,laserCloudCornerFromMap-points[pointSearchInd[j]].y,laserCloudCornerFromMap-points[pointSearchInd[j]].z);// 5个点坐标的叠加center center tmp;// 转存这5个点给nearCornersnearCorners.push_back(tmp);}// 计算这5个点的均值center center / 5.0;Eigen::Matrix3d covMat Eigen::Matrix3d::Zero();// 构建协方差矩阵,5个变量的变化趋势for (int j 0; j 5; j){// 每个点与均值之间的偏移量Eigen::Matrixdouble, 3, 1 tmpZeroMean nearCorners[j] - center;// 该点与该点转置的外积 当前矩阵与当前矩阵的转置 得到3*3的矩阵当前点的协方差矩阵covMat covMat tmpZeroMean * tmpZeroMean.transpose();}// 进行特征值分解Eigen::SelfAdjointEigenSolverEigen::Matrix3d saes(covMat);// 根据特征值分解情况看看是不是真正的线特征// 特征向量就是线特征的方向 Eigen::Vector3d unit_direction saes.eigenvectors().col(2);Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);// 最大特征值大于次大特征值的3倍认为是线特征if (saes.eigenvalues()[2] 3 * saes.eigenvalues()[1]){Eigen::Vector3d point_on_line center;Eigen::Vector3d point_a, point_b;// 根据拟合出来的线特征方向,以平均点为中心构建两个虚拟点//从中心点沿着方向向量向两端移动0.1m使用两个点代替一条直线//这样计算点到直线的距离的形式就跟laserOdometry相似point_a 0.1 * unit_direction point_on_line;point_b -0.1 * unit_direction point_on_line;// 构建约束 和lidar odom 约束一致ceres::CostFunction *cost_function LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);problem.AddResidualBlock(cost_function, loss_function, parameters, parameters 4);corner_num;}}}2.3.9点面残差构建 这里与前端有区别 面的构建通过 最临近当前角点的5个点 通过构建超定方程 qr分解获得的 法向量与点之间的关系
int surf_num 0;
// 构建面点的约束
for (int i 0; i laserCloudSurfStackNum; i)
{// 实时面点坐标pointOri laserCloudSurfStack-points[i];// 把雷达点坐标转到map坐标系pointAssociateToMap(pointOri, pointSel);// 局部地图中搜索距离该点最近的5个点kdtreeSurfFromMap-nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);Eigen::Matrixdouble, 5, 3 matA0;Eigen::Matrixdouble, 5, 1 matB0 -1 * Eigen::Matrixdouble, 5, 1::Ones();// 构建面点方程axbyczd0// 通过构建一个超定方程求解这个平面方程// 判断最远的点距离不能超过1m,否则就是无效约束if (pointSearchSqDis[4] 1.0){for (int j 0; j 5; j){matA0(j, 0) laserCloudSurfFromMap-points[pointSearchInd[j]].x;matA0(j, 1) laserCloudSurfFromMap-points[pointSearchInd[j]].y;matA0(j, 2) laserCloudSurfFromMap-points[pointSearchInd[j]].z;}// 通过eigen接口求解该方程,解就是这个平面的法向量// 豪斯霍尔德变换Eigen::Vector3d norm matA0.colPivHouseholderQr().solve(matB0);double negative_OA_dot_norm 1 / norm.norm();// 法向量归一化norm.normalize();bool planeValid true;// 根据求出来的平面方程进行校验 看看是不是符合平面约束for (int j 0; j 5; j){// 这里相当于求解点到平面的距离if (fabs(norm(0) * laserCloudSurfFromMap-points[pointSearchInd[j]].x norm(1) * laserCloudSurfFromMap-points[pointSearchInd[j]].y norm(2) * laserCloudSurfFromMap-points[pointSearchInd[j]].z negative_OA_dot_norm) 0.2){planeValid false; // 点如果距离平面过远,就认为这是一个拟合的不好的平面break;}}Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);// 如果平面有效就构建平面约束if (planeValid){// 利用平面方程构建约束 和前端构建形式稍有不同ceres::CostFunction *cost_function LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);problem.AddResidualBlock(cost_function, loss_function, parameters, parameters 4);surf_num;}2.2.10通过反变换更新odom-》map的tf关系
// q_wmap_wodom t_wmap_wodom是map到odom之间的关系
q_wmap_wodom q_w_curr * q_wodom_curr.inverse();
t_wmap_wodom t_w_curr - q_wmap_wodom * t_wodom_curr;2.2.11将优化后的当前帧角点加入到局部地图,面点雷同 for (int i 0; i laserCloudCornerStackNum; i)
{// 该点根据位姿投到地图坐标系pointAssociateToMap(laserCloudCornerStack-points[i], pointSel);// 算出这个点所在的格子在全局地图中的索引int cubeI int((pointSel.x 25.0) / 50.0) laserCloudCenWidth;int cubeJ int((pointSel.y 25.0) / 50.0) laserCloudCenHeight;int cubeK int((pointSel.z 25.0) / 50.0) laserCloudCenDepth;// 同样负数做对应的操作if (pointSel.x 25.0 0)cubeI--;if (pointSel.y 25.0 0)cubeJ--;if (pointSel.z 25.0 0)cubeK--;// 如果超过边界的话就算了if (cubeI 0 cubeI laserCloudWidth cubeJ 0 cubeJ laserCloudHeight cubeK 0 cubeK laserCloudDepth){// 当前格子在全局地图中的索引int cubeInd cubeI laserCloudWidth * cubeJ laserCloudWidth * laserCloudHeight * cubeK;// 将当前帧点云角点插入到角点格子中laserCloudCornerArray[cubeInd]-push_back(pointSel);}
}2.2.12把当前帧涉及到的局部地图下采样 for (int i 0; i laserCloudValidNum; i){int ind laserCloudValidInd[i];pcl::PointCloudPointType::Ptr tmpCorner(new pcl::PointCloudPointType());downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);downSizeFilterCorner.filter(*tmpCorner);laserCloudCornerArray[ind] tmpCorner;pcl::PointCloudPointType::Ptr tmpSurf(new pcl::PointCloudPointType());downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);downSizeFilterSurf.filter(*tmpSurf);laserCloudSurfArray[ind] tmpSurf;}2.2.13局部地图发布 // 每隔5帧对外发布一下
if (frameCount % 5 0)
{laserCloudSurround-clear();// 把当前帧相关的局部地图发布出去 laserCloudSurroundNum 坐标点的索引数目for (int i 0; i laserCloudSurroundNum; i){int ind laserCloudSurroundInd[i];*laserCloudSurround *laserCloudCornerArray[ind];*laserCloudSurround *laserCloudSurfArray[ind];}sensor_msgs::PointCloud2 laserCloudSurround3;pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);laserCloudSurround3.header.stamp ros::Time().fromSec(timeLaserOdometry);laserCloudSurround3.header.frame_id /map;pubLaserCloudSurround.publish(laserCloudSurround3);
}2.2.14全局地图发布
// 每隔20帧发布一次全局地图
if (frameCount % 20 0)
{// 21*21*114851pcl::PointCloudPointType laserCloudMap;for (int i 0; i 4851; i){laserCloudMap *laserCloudCornerArray[i];laserCloudMap *laserCloudSurfArray[i];}sensor_msgs::PointCloud2 laserCloudMsg;pcl::toROSMsg(laserCloudMap, laserCloudMsg);laserCloudMsg.header.stamp ros::Time().fromSec(timeLaserOdometry);laserCloudMsg.header.frame_id /map;pubLaserCloudMap.publish(laserCloudMsg);
}2.2.15全局位姿轨迹 tf 发布 // 发布当前位姿
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id /map;
odomAftMapped.child_frame_id /laser_link;
odomAftMapped.header.stamp ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x q_w_curr.x();
odomAftMapped.pose.pose.orientation.y q_w_curr.y();
odomAftMapped.pose.pose.orientation.z q_w_curr.z();
odomAftMapped.pose.pose.orientation.w q_w_curr.w();
odomAftMapped.pose.pose.position.x t_w_curr.x();
odomAftMapped.pose.pose.position.y t_w_curr.y();
odomAftMapped.pose.pose.position.z t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);
// 发布当前轨迹
geometry_msgs::PoseStamped laserAfterMappedPose;
laserAfterMappedPose.header odomAftMapped.header;
laserAfterMappedPose.pose odomAftMapped.pose.pose;
laserAfterMappedPath.header.stamp odomAftMapped.header.stamp;
laserAfterMappedPath.header.frame_id /map;
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
pubLaserAfterMappedPath.publish(laserAfterMappedPath);// 发布tf
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(t_w_curr(0),t_w_curr(1),t_w_curr(2)));
q.setW(q_w_curr.w());
q.setX(q_w_curr.x());
q.setY(q_w_curr.y());
q.setZ(q_w_curr.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, /map, /laser_link));
