回环检测
在LOAM系列中回环检测主要存在有四种方法
- 传统的领域距离搜索+ICP匹配
- 基于scan context系列的粗匹配+ICP精准匹配的回环检测
- 基于scan context的回环检测
- 基于Intensity scan context+ICP的回环检测
在参考很多大佬的比对结果中我们发现，传统的领域距离搜索+ICP匹配是这三个方法中最耗时的，相较于基于scan context的回环检测而言利用类梯度下降法做ICP匹配需要消耗大量的计算资源。在使用基于scan context的回环检测时，会出现误检和漏检情况，而基于Intensity scan context的回环检测，对于scan context建图精度未有太大提升，但漏检的情况要少很多。
SC-LeGO-LOAM算法详解
SC-LeGO-LOAM是在LeGO-LOAM的基础上新增了基于Scan context的回环检测，在回环检测的速度上相较于LeGO-LOAM有了一定的提升。下面我们将会对SC-LeGO-LOAM算法的代码进行详细分析。
首先我们从代码的launch文件开始入手进行分析，README文件中提示，我们需要从run.launch文件入手。
<launch>
........
<!--- LeGO-LOAM -->
<node pkg="lego_loam" type="imageProjection" name="imageProjection" output="screen"/>
<node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
<node pkg="lego_loam" type="mapOptmization" name="mapOptmization" output="screen"/>
<node pkg="lego_loam" type="transformFusion" name="transformFusion" output="screen"/>
</launch>
从中我们可以看到launch文件中主要依赖四个node，其中最后一个node主要输出了一些数据坐标系的转换，对文章的理解影响不会很大，主要功能的是由前面3个node来实现的，而且是存在数据流的依次传递和处理。
imageProjection.cpp
首先我们先来看一下imageProjection这一个node节点，这个部分主要是对激光雷达数据进行预处理。包括激光雷达数据获取、点云数据分割、点云类别标注、数据发布。
该文件中订阅了激光雷达发布的数据，并发布了多个分类点云结果，初始化lego_loam::ImageProjection构造函数中，nodehandle使用~来表示在默认空间中。
ImageProjection::ImageProjection() : nh("~") {
// init params
InitParams();
// subscriber
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic.c_str(), 1,
&ImageProjection::cloudHandler, this);
// publisher
pubFullCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_projected", 1);
pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_info", 1);
pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2>("/ground_cloud", 1);
pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud", 1);
pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud_pure", 1);
pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info>("/segmented_cloud_info", 1);
pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud", 1); // 离群点或异常点
nanPoint.x = std::numeric_limits<float>::quiet_NaN();
nanPoint.y = std::numeric_limits<float>::quiet_NaN();
nanPoint.z = std::numeric_limits<float>::quiet_NaN();
nanPoint.intensity = -1;
allocateMemory();
resetParameters();
}
在构造函数中除了使用allocateMemory对点云进行reset、resize、assign等重置、赋值等操作以外。主要的函数是ImageProjection::cloudHandler，该函数内部清晰记录了激光雷达数据流的走向
void ImageProjection::cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) {
// 1. Convert ros message to pcl point cloud
copyPointCloud(laserCloudMsg);
// 2. Start and end angle of a scan
findStartEndAngle();
// 3. Range image projection
projectPointCloud();
// 4. Mark ground points
groundRemoval();
// 5. Point cloud segmentation
cloudSegmentation();
// 6. Publish all clouds
publishCloud();
// 7. Reset parameters for next iteration
resetParameters();
}
该回调函数调用了七个函数，完成了对单帧激光雷达数据的处理。下面我们对该流程进行梳理，并详细介绍地面分割方法。
- copyPointCloud：该函数主要的功能是通过
pcl::fromROSMsg函数将ROS的PointCloud保存成PCL的PointCloud，并通过pcl::removeNaNFromPointCloud函数对nan噪点进行了滤除，从而避免了后面的计算中出现各种异常情况。 - findStartEndAngle：这个函数主要为了计算起止角度范围。因为运动，会导致多线激光雷达的第一个点和最后一个点并不是严格的360°，会存在一定的畸变。为此需要计算出起止角度，并将差值放在
segMsg变量中，成员变量segMsg的类型cloud_msgs::cloud_info是作者自定义的。它保存了当前帧的一些重要信息，包括起至角度，每个线的起至序号，及成员变量fullCloud中每个点的状态。 -
projectPointCloud：该函数中将激光点云按照角度展开成图像的形式，计算所在行列和深度。其中行表示激光线束数量，列表示每个线上同一时刻扫描到的点。这里以
x轴的负方向开始逆时针列序列号逐渐递增，即图像中的从左到右。以Mat图像保存深度，其中点集的行用fullCloud 和 fullInfoCloud表示和深度用fullInfoCloud表示。这里的fullInfoCloud内部存放该帧下的所有数据，该处的点云相比于copyPointCloud函数输入的点云数据，主要的不同之处在于，当前的点云根据计算出来的行列，重新排列了顺序。例如，激光雷达本身可能是先列后行的方向输出的点云，或者是livox雷达那种非重复扫描出来的结果，这里都会被重新组织成先行后列的顺序保存。void ImageProjection::projectPointCloud() { // 激光点云投影成二维图像，行表示激光线束数量，列表示每一个线上扫描到的点（0.1s扫描一圈，一个圆圈摊平就是360度） // 计算点云深度，保存到深度图中 // range image projection float verticalAngle, horizonAngle, range; size_t rowIdn, columnIdn, index, cloudSize; PointType thisPoint; cloudSize = laserCloudIn->points.size(); for (size_t i = 0; i < cloudSize; ++i) { thisPoint.x = laserCloudIn->points[i].x; thisPoint.y = laserCloudIn->points[i].y; thisPoint.z = laserCloudIn->points[i].z; // find the row and column index in the iamge for this point // 计算竖直方向上的点的角度以及在整个雷达点云中的哪一条水平线上 if (useCloudRing == true) { // 用vlp的时候有ring属性 rowIdn = laserCloudInRing->points[i].ring; } else { // 其他lidar需要根据计算垂直方向的俯仰角？ verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI; rowIdn = (verticalAngle + ang_bottom) / ang_res_y; } if (rowIdn < 0 || rowIdn >= N_SCAN) continue; // 水平方向上的角度，一行1800个像素点 horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; // round是四舍五入 columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2; if (columnIdn >= Horizon_SCAN) columnIdn -= Horizon_SCAN; if (columnIdn < 0 || columnIdn >= Horizon_SCAN) continue; // 每个点的深度 range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z); if (range < sensorMinimumRange) continue; // 在rangeMat矩阵中保存该点的深度，保存单通道像素值 rangeMat.at<float>(rowIdn, columnIdn) = range; thisPoint.intensity = (float) rowIdn + (float) columnIdn / 10000.0; index = columnIdn + rowIdn * Horizon_SCAN; fullCloud->points[index] = thisPoint; fullInfoCloud->points[index] = thisPoint; fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity" } } -
groundRemoval：该步骤主要是提前滤除地面，从贴近地面的几个线中提取地面点。取七个扫描圈，每两个圈之间进行一次比较，角度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加入到
groundCloud点云中。地面的点云会在groundMat中标记为1，labelMat中标记-1 ，不会参与后面的分割。
void ImageProjection::groundRemoval() { // 利用不同的扫描圈来表示地面，进而检测地面是否水平。 // 取七个扫描圈，每两个圈之间进行一次比较，角度相差10°以内的我们可以看做是平地。 // 并且将扫描圈中的点加入到groundCloud点云 size_t lowerInd, upperInd; float diffX, diffY, diffZ, angle; // groundMat // -1, no valid info to check if ground of not // 0, initial value, after validation, means not ground // 1, ground for (size_t j = 0; j < Horizon_SCAN; ++j) { // 每行 for (size_t i = 0; i < groundScanInd; ++i) { // 每列 // 只用7个光束检测地面 lowerInd = j + (i) * Horizon_SCAN; upperInd = j + (i + 1) * Horizon_SCAN; // intensity在投影的时候已经归一化 if (fullCloud->points[lowerInd].intensity == -1 || fullCloud->points[upperInd].intensity == -1) { // no info to check, invalid points groundMat.at<int8_t>(i, j) = -1; continue; } diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x; diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y; diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z; angle = atan2(diffZ, sqrt(diffX * diffX + diffY * diffY)) * 180 / M_PI; // 相邻圈小于10度 if (abs(angle - sensorMountAngle) <= 10) { groundMat.at<int8_t>(i, j) = 1; groundMat.at<int8_t>(i + 1, j) = 1; } } } // extract ground cloud (groundMat == 1) // mark entry that doesn't need to label (ground and invalid point) for segmentation // note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan for (size_t i = 0; i < N_SCAN; ++i) { for (size_t j = 0; j < Horizon_SCAN; ++j) { if (groundMat.at<int8_t>(i, j) == 1 || rangeMat.at<float>(i, j) == FLT_MAX) { labelMat.at<int>(i, j) = -1; } } } if (pubGroundCloud.getNumSubscribers() != 0) { for (size_t i = 0; i <= groundScanInd; ++i) { for (size_t j = 0; j < Horizon_SCAN; ++j) { if (groundMat.at<int8_t>(i, j) == 1) groundCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]); } } } }-
cloudSegmentation：这一部分主要是将地面点与异常点排除之后，对非地面点云的分割，并生成局部特征的步骤。这个函数主要完成了两个任务，一是通过广度优先搜索，从非地面点中找出所有连成片的入射角比较小的patch上的点，并在labelMat标注patch的编号（从1开始）
void ImageProjection::labelComponents(int row, int col) { // use std::queue std::vector std::deque will slow the program down greatly // 特征检测，检测点与其临近点的特征 float d1, d2, alpha, angle; int fromIndX, fromIndY, thisIndX, thisIndY; bool lineCountFlag[N_SCAN] = {false}; queueIndX[0] = row; queueIndY[0] = col; int queueSize = 1; int queueStartInd = 0; int queueEndInd = 1; allPushedIndX[0] = row; allPushedIndY[0] = col; int allPushedIndSize = 1; while (queueSize > 0) { // Pop point fromIndX = queueIndX[queueStartInd]; fromIndY = queueIndY[queueStartInd]; --queueSize; ++queueStartInd; // Mark popped point labelMat.at<int>(fromIndX, fromIndY) = labelCount; //检查上下左右四个邻点 // Loop through all the neighboring grids of popped grid for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter) { // new index thisIndX = fromIndX + (*iter).first; thisIndY = fromIndY + (*iter).second; // index should be within the boundary if (thisIndX < 0 || thisIndX >= N_SCAN) continue; // at range image margin (left or right side) if (thisIndY < 0) thisIndY = Horizon_SCAN - 1; if (thisIndY >= Horizon_SCAN) thisIndY = 0; // prevent infinite loop (caused by put already examined point back) if (labelMat.at<int>(thisIndX, thisIndY) != 0) continue; // d1与d2分别是该点与某邻点的深度 d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), rangeMat.at<float>(thisIndX, thisIndY)); d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), rangeMat.at<float>(thisIndX, thisIndY)); if ((*iter).first == 0) alpha = segmentAlphaX; else alpha = segmentAlphaY; // angle其实是该特点与某邻点的连线与XOZ平面的夹角，这个角代表了局部特征的敏感性 angle = atan2(d2 * sin(alpha), (d1 - d2 * cos(alpha))); // 如果夹角大于60°，则将这个邻点纳入到局部特征中，该邻点可以用来配准使用 if (angle > segmentTheta) { queueIndX[queueEndInd] = thisIndX; queueIndY[queueEndInd] = thisIndY; ++queueSize; ++queueEndInd; labelMat.at<int>(thisIndX, thisIndY) = labelCount; lineCountFlag[thisIndX] = true; allPushedIndX[allPushedIndSize] = thisIndX; allPushedIndY[allPushedIndSize] = thisIndY; ++allPushedIndSize; } } } // check if this segment is valid // 当邻点数目达到30后，则该帧雷达点云的几何特征配置成功 bool feasibleSegment = false; if (allPushedIndSize >= 30) feasibleSegment = true; else if (allPushedIndSize >= segmentValidPointNum) { int lineCount = 0; for (size_t i = 0; i < N_SCAN; ++i) if (lineCountFlag[i] == true) ++lineCount; if (lineCount >= segmentValidLineNum) feasibleSegment = true; } // segment is valid, mark these points if (feasibleSegment == true) { ++labelCount; } else { // segment is invalid, mark these points for (size_t i = 0; i < allPushedIndSize; ++i) { labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999; } } }
二是把所有地面点和刚分割出来的labelCount上的点合并保存在segmentedCloud中，此时当前帧的点已经有效的分割成多个类，这也是该node需要传递给下一个node进行特征提取和匹配的点云。并在segMsg中对应位置处保存每个点的属性（该点是不是地面，深度，属于第几列）。同时由于我们不需要那么多地面的点云，所以使用5个采一个的策略来实现地面点的降采样。void ImageProjection::cloudSegmentation() { // segmentation process // /在排除地面点与异常点之后，逐一检测邻点特征并生成局部特征 for (size_t i = 0; i < N_SCAN; ++i) for (size_t j = 0; j < Horizon_SCAN; ++j) if (labelMat.at<int>(i, j) == 0) labelComponents(i, j); int sizeOfSegCloud = 0; // extract segmented cloud for lidar odometry for (size_t i = 0; i < N_SCAN; ++i) { segMsg.startRingIndex[i] = sizeOfSegCloud - 1 + 5; for (size_t j = 0; j < Horizon_SCAN; ++j) { // 如果是特征点或者是地面点，就可以纳入被分割点云 if (labelMat.at<int>(i, j) > 0 || groundMat.at<int8_t>(i, j) == 1) { // outliers that will not be used for optimization (always continue) if (labelMat.at<int>(i, j) == 999999) { if (i > groundScanInd && j % 5 == 0) { outlierCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]); continue; } else { continue; } } // majority of ground points are skipped // 地面点云每隔5个点纳入被分割点云 if (groundMat.at<int8_t>(i, j) == 1) { if (j % 5 != 0 && j > 5 && j < Horizon_SCAN - 5) continue; } // mark ground points so they will not be considered as edge features later segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i, j) == 1); // mark the points' column index for marking occlusion later segMsg.segmentedCloudColInd[sizeOfSegCloud] = j; // save range info segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at<float>(i, j); // save seg cloud 把当前点纳入分割点云中 segmentedCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]); // size of seg cloud ++sizeOfSegCloud; } } segMsg.endRingIndex[i] = sizeOfSegCloud - 1 - 5; } // extract segmented cloud for visualization // 在当前有节点订阅便将分割点云的几何信息也发布出去 if (pubSegmentedCloudPure.getNumSubscribers() != 0) { for (size_t i = 0; i < N_SCAN; ++i) { for (size_t j = 0; j < Horizon_SCAN; ++j) { if (labelMat.at<int>(i, j) > 0 && labelMat.at<int>(i, j) != 999999) { segmentedCloudPure->push_back(fullCloud->points[j + i * Horizon_SCAN]); segmentedCloudPure->points.back().intensity = labelMat.at<int>(i, j); } } } } } - publishCloud：下面j就是与ROS相关的发布任务了。包括该帧的segMsg，完整点云fullCloud/fullInfoCloud（步骤3），地面点云（步骤4），发从非地面提取出来的点和降采样的地面点（步骤5），外点（步骤5,实际为比较小的patch上的点【因此两个叠加会导致角度差较大，且点云数不满足需求】）。
void ImageProjection::publishCloud() { // 1. Publish Seg Cloud Info segMsg.header = cloudHeader; pubSegmentedCloudInfo.publish(segMsg); // 2. Publish clouds sensor_msgs::PointCloud2 laserCloudTemp; pcl::toROSMsg(*outlierCloud, laserCloudTemp); laserCloudTemp.header.stamp = cloudHeader.stamp; laserCloudTemp.header.frame_id = "base_link"; pubOutlierCloud.publish(laserCloudTemp); // segmented cloud with ground pcl::toROSMsg(*segmentedCloud, laserCloudTemp); laserCloudTemp.header.stamp = cloudHeader.stamp; laserCloudTemp.header.frame_id = "base_link"; pubSegmentedCloud.publish(laserCloudTemp); // projected full cloud if (pubFullCloud.getNumSubscribers() != 0) { pcl::toROSMsg(*fullCloud, laserCloudTemp); laserCloudTemp.header.stamp = cloudHeader.stamp; laserCloudTemp.header.frame_id = "base_link"; pubFullCloud.publish(laserCloudTemp); } // original dense ground cloud if (pubGroundCloud.getNumSubscribers() != 0) { pcl::toROSMsg(*groundCloud, laserCloudTemp); laserCloudTemp.header.stamp = cloudHeader.stamp; laserCloudTemp.header.frame_id = "base_link"; pubGroundCloud.publish(laserCloudTemp); } // segmented cloud without ground if (pubSegmentedCloudPure.getNumSubscribers() != 0) { pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp); laserCloudTemp.header.stamp = cloudHeader.stamp; laserCloudTemp.header.frame_id = "base_link"; pubSegmentedCloudPure.publish(laserCloudTemp); } // projected full cloud info if (pubFullInfoCloud.getNumSubscribers() != 0) { pcl::toROSMsg(*fullInfoCloud, laserCloudTemp); laserCloudTemp.header.stamp = cloudHeader.stamp; laserCloudTemp.header.frame_id = "base_link"; pubFullInfoCloud.publish(laserCloudTemp); } }- resetParameters：清空成员变量，准备下一次的处理。
-
评论（0）
您还未登录，请登录后发表或查看评论