LeGO-LOAM 源码阅读笔记(featureAssociation.cpp)
LeGO-LOAM是一种在LOAM之上进行改进的激光雷达建图方法，建图效果比LOAM要好，但是建图较为稀疏，计算量也更小了。
本文原地址：wykxwyc的博客
github注释后LeGO-LOAM源码：LeGO-LOAM_NOTED
关于代码的详细理解，建议阅读：1.地图优化代码理解
2.图像重投影代码理解
3.特征关联代码理解
4.LeGO-LOAM中的数学公式推导
以上博客会随时更新，如果对你有帮助，请点击注释代码的github仓库右上角star按钮，你的鼓励将给我更多动力。
文章目录
-
-
- featureAssociation.cpp概述
- FeatureAssociation
- laserCloudHandler
- laserCloudInfoHandler
- outlierCloudHandler
- imuHandler
- runFeatureAssociation
- adjustDistortion
- calculateSmoothness
- markOccludedPoints
- extractFeatures
- updateTransformation
- integrateTransformation
-
featureAssociation.cpp概述
featureAssociation.cpp顾名思义，进行特征关联的过程。
FeatureAssociation
FeatureAssociation()构造函数的内容如下：
-
订阅话题:
"/segmented_cloud"(sensor_msgs::PointCloud2)，数据处理函数laserCloudHandler"/segmented_cloud_info"(cloud_msgs::cloud_info)，数据处理函数laserCloudInfoHandler"/outlier_cloud"(sensor_msgs::PointCloud2)，数据处理函数outlierCloudHandlerimuTopic = "/imu/data"(sensor_msgs::Imu)，数据处理函数imuHandler
-
发布话题，这些topic有：
"/laser_cloud_sharp"(sensor_msgs::PointCloud2)"/laser_cloud_less_sharp"(sensor_msgs::PointCloud2)"/laser_cloud_flat"(sensor_msgs::PointCloud2)"/laser_cloud_less_flat"(sensor_msgs::PointCloud2)"/laser_cloud_corner_last"(sensor_msgs::PointCloud2)"/laser_cloud_surf_last"(cloud_msgs::cloud_info)"/outlier_cloud_last"(sensor_msgs::PointCloud2)"/laser_odom_to_init"(nav_msgs::Odometry)
然后初始化各类参数。
laserCloudHandler
laserCloudHandler修改点云数据的时间戳，将点云数据从ROS定义的格式转化到pcl的格式。
laserCloudInfoHandler
函数比较小:
void laserCloudInfoHandler(const cloud_msgs::cloud_infoConstPtr& msgIn){
timeNewSegmentedCloudInfo = msgIn->header.stamp.toSec();
segInfo = *msgIn;
newSegmentedCloudInfo = true;
}
outlierCloudHandler
void outlierCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msgIn){
timeNewOutlierCloud = msgIn->header.stamp.toSec();
outlierCloud->clear();
pcl::fromROSMsg(*msgIn, *outlierCloud);
newOutlierCloud = true;
}
imuHandler
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)接触过很多次，因为它就是LOAM代码里的那个。
函数的实现：
- 通过接收到的imuIn里面的四元素得到roll,pitch,yaw三个角；
- 对加速度进行坐标变换(坐标变换可以参考下面这张图)；
- 将欧拉角，加速度，速度保存到循环队列中；
- 对速度，角速度，加速度进行积分，得到位移，角度和速度(
AccumulateIMUShiftAndRotation())；
加速度坐标交换的示意图：
- 进行加速度坐标交换时将重力加速度去除，然后再进行 x x x到 z z z, y y y到 x x x, z z z到 y y y的变换。
- 去除重力加速度的影响时，需要把重力加速度分解到三个坐标轴上，然后分别去除他们分量的影响，在去除的过程中需要注意加减号（默认右手坐标系的旋转方向来看）。
- 在上面示意图中，可以简单理解为红色箭头实线分解到红色箭头虚线上（根据 p i t c h pitch pitch进行分解）,然后再按找 r o l l roll roll角进行分解。
runFeatureAssociation
void runFeatureAssociation()是featureAssociation.cpp中最主要的函数，它调用这个cpp文件中的其他函数。算法步骤如下：
- 如果有新数据进来则执行，否则不执行任何操作；
- 将点云数据进行坐标变换，进行插补等工作；
- 进行光滑性计算，并保存结果；
- 标记阻塞点(阻塞点：点云中可能出现的互相遮挡的点)；
- 特征抽取，然后分别保存到
cornerPointsSharp等等队列中去； - 发布
cornerPointsSharp等4种类型的点云数据； - 预测位姿；
- 更新变换；
- 积分总变换；
- 发布里程计信息及上一次点云信息；
adjustDistortion
void adjustDistortion()将点云数据进行坐标变换，进行插补等工作。
- 对每个点进行处理，首先进行和laboshin_loam代码中的一样的坐标轴变换过程。
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;
- 针对每个点计算偏航角yaw，然后根据不同的偏航角，可以知道激光雷达扫过的位置有没有超过一半，计算的时候有一部分需要注意一下：
函数原型：
// -atan2(p.x,p.z)==>-atan2(y,x)
// ori表示的是偏航角yaw，因为前面有负号，ori=[-M_PI,M_PI)
// 因为segInfo.orientationDiff表示的范围是(PI,3PI)，在2PI附近
// 下面过程的主要作用是调整ori大小，满足start
float ori = -atan2(point.x, point.z);
这里分为4种情况：
- 没有转过一半，但是
start-ori>M_PI/2 - 没有转过一半，但是
ori-start>3/2*M_PI,说明ori太大，不合理（正常情况在前半圈的话，ori-start范围[0,M_PI]） - 转过一半，
end-ori>3/2*PI,ori太小 - 转过一半，
ori-end>M_PI/2,太大
- 然后进行imu数据与激光数据的时间轴对齐操作。
对齐时候有两种情况，一种不能用插补来优化，一种可以通过插补进行优化。
- 不能通过插补进行优化：imu数据比激光数据早，但是没有更后面的数据(打个比方,激光在9点时出现，imu现在只有8点的)
这种情况下while循环是以imuPointerFront == imuPointerLast结束的：
// while循环内进行时间轴对齐
while (imuPointerFront != imuPointerLast) {
if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
- 可以通过插补来进行数据的优化：
这种情况只有在imu数据充足的情况下才会发生，
进行插补时，当前timeScanCur + pointTime < imuTime[imuPointerFront]，
而且imuPointerFront是最早一个时间大于timeScanCur + pointTime的imu数据指针。
imuPointerBack是imuPointerFront的前一个imu数据指针。
插补的代码：
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
通过上面计算的ratioFront以及ratioBack进行插补,
因为imuRollCur和imuPitchCur通常都在0度左右，变化不会很大，因此不需要考虑超过 2 π 2\pi 2π的情况,
imuYaw转的角度比较大，需要考虑超过 2 π 2\pi 2π的情况。
imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
} else {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}
后面再进行imu的速度插补与位置插补。
另外，针对 i = 0 i=0 i=0 的情况（另一个不同的点云），每次都要用和上面相同的方法判断是否进行插补并且更新 imu 的数据。
更新的数据用途：后面将速度坐标投影过来会用到 i = 0 i=0 i=0 时刻的值。
calculateSmoothness
void calculateSmoothness()用于计算光滑性，这里的计算没有完全按照公式LOAM论文中的进行。
此处的公式计算中没有除以总点数 i i i 及 r [ i ] r[i] r[i] .
注释后的代码如下：
void calculateSmoothness(){
int cloudSize = segmentedCloud->points.size();
for (int i = 5; i < cloudSize - 5; i++) {
float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]
+ segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]
+ segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10
+ segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]
+ segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]
+ segInfo.segmentedCloudRange[i+5];
cloudCurvature[i] = diffRange*diffRange;
// 在markOccludedPoints()函数中对该参数进行重新修改
cloudNeighborPicked[i] = 0;
// 在extractFeatures()函数中会对标签进行修改，
// 初始化为0，surfPointsFlat标记为-1，
// surfPointsLessFlatScan为不大于0的标签
// cornerPointsSharp标记为2，cornerPointsLessSharp标记为1
cloudLabel[i] = 0;
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}
markOccludedPoints
void markOccludedPoints()选择了距离比较远的那些点，并将他们标记为1，还选择了距离变换大的点，并将他们标记为1。
函数代码如下：
void markOccludedPoints(){
int cloudSize = segmentedCloud->points.size();
for (int i = 5; i < cloudSize - 6; ++i){
float depth1 = segInfo.segmentedCloudRange[i];
float depth2 = segInfo.segmentedCloudRange[i+1];
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i+1]
- segInfo.segmentedCloudColInd[i]));
if (columnDiff < 10){
// 选择距离较远的那些点，并将他们标记为1
if (depth1 - depth2 > 0.3){
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}else if (depth2 - depth1 > 0.3){
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
float diff1 = std::abs(segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i]);
float diff2 = std::abs(segInfo.segmentedCloudRange[i+1] - segInfo.segmentedCloudRange[i]);
// 选择距离变化较大的点，并将他们标记为1
if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] &&
diff2 > 0.02 * segInfo.segmentedCloudRange[i])
cloudNeighborPicked[i] = 1;
}
}
extractFeatures
void extractFeatures()进行特征抽取，然后分别保存到cornerPointsSharp等等队列中去。
保存到不同的队列是不同类型的点云，进行了标记的工作，这一步中减少了点云数量，使计算量减少。
函数首先清空了cornerPointsSharp,cornerPointsLessSharp,surfPointsFlat,surfPointsLessFlat
然后对cloudSmoothness队列中sp到ep之间的点的平滑数据进行从小到大的排列。
然后按照不同的要求，将点的索引放到不同的队列中去。
另外还对点进行了标记。
最后，因为点云太多时，计算量过大，因此需要对点云进行下采样，减少计算量。
代码如下：
void extractFeatures(){
cornerPointsSharp->clear();
cornerPointsLessSharp->clear();
surfPointsFlat->clear();
surfPointsLessFlat->clear();
for (int i = 0; i < N_SCAN; i++) {
surfPointsLessFlatScan->clear();
for (int j = 0; j < 6; j++) {
// sp和ep的含义是什么???startPointer,endPointer?
int sp = (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 6;
int ep = (segInfo.startRingIndex[i] * (5 - j) + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
if (sp >= ep)
continue;
// 按照cloudSmoothness.value从小到大排序
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
// 每次ind的值就是等于k??? 有什么意义?
// 因为上面对cloudSmoothness进行了一次从小到大排序，所以ind不一定等于k了
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > edgeThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == false) {
largestPickedNum++;
if (largestPickedNum <= 2) {
// 论文中nFe=2,cloudSmoothness已经按照从小到大的顺序排列，
// 所以这边只要选择最后两个放进队列即可
// cornerPointsSharp标记为2
cloudLabel[ind] = 2;
cornerPointsSharp->push_back(segmentedCloud->points[ind]);
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else if (largestPickedNum <= 20) {
// 塞20个点到cornerPointsLessSharp中去
// cornerPointsLessSharp标记为1
cloudLabel[ind] = 1;
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else {
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
// 从ind+l开始后面5个点，每个点index之间的差值，
// 确保columnDiff<=10,然后标记为我们需要的点
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
// 从ind+l开始前面五个点，计算差值然后标记
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) {
int ind = cloudSmoothness[k].ind;
// 平面点只从地面点中进行选择???为什么要这样做???
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < surfThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == true) {
cloudLabel[ind] = -1;
surfPointsFlat->push_back(segmentedCloud->points[ind]);
// 论文中nFp=4，将4个最平的平面点放入队列中
smallestPickedNum++;
if (smallestPickedNum >= 4) {
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
// 从前面往后判断是否是需要的邻接点，是的话就进行标记
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
// 从后往前开始标记
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
for (int k = sp; k <= ep; k++) {
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);
}
}
}
// surfPointsLessFlatScan中有过多的点云，如果点云太多，计算量太大
// 进行下采样，可以大大减少计算量
surfPointsLessFlatScanDS->clear();
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.filter(*surfPointsLessFlatScanDS);
*surfPointsLessFlat += *surfPointsLessFlatScanDS;
}
}
updateTransformation
void updateTransformation()中主要是两个部分，一个是找特征平面，通过面之间的对应关系计算出变换矩阵。
另一个部分是通过角、边特征的匹配，计算变换矩阵。
该函数主要由其他四个部分组成：findCorrespondingSurfFeatures,calculateTransformationSurf
findCorrespondingCornerFeatures,calculateTransformationCorner
这四个函数分别是对应于寻找对应面、通过面对应计算变换矩阵、寻找对应角/边特征、通过角/边特征计算变换矩阵。
void updateTransformation(){
if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
return;
for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
laserCloudOri->clear();
coeffSel->clear();
// 找到对应的特征平面
// 然后计算协方差矩阵，保存在coeffSel队列中
// laserCloudOri中保存的是对应于coeffSel的未转换到开始时刻的原始点云数据
findCorrespondingSurfFeatures(iterCount1);
if (laserCloudOri->points.size() < 10)
continue;
// 通过面特征的匹配，计算变换矩阵
if (calculateTransformationSurf(iterCount1) == false)
break;
}
for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {
laserCloudOri->clear();
coeffSel->clear();
// 找到对应的特征边/角点
// 寻找边特征的方法和寻找平面特征的很类似，过程可以参照寻找平面特征的注释
findCorrespondingCornerFeatures(iterCount2);
if (laserCloudOri->points.size() < 10)
continue;
// 通过角/边特征的匹配，计算变换矩阵
if (calculateTransformationCorner(iterCount2) == false)
break;
}
}
integrateTransformation
void integrateTransformation()计算了旋转角的累积变化量。
这个函数首先通过AccumulateRotation()将局部旋转左边切换至全局旋转坐标。
然后同坐变换转移到世界坐标系下。
再通过PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);插入imu旋转，更新姿态。
featureAssociation.cpp中还有一些函数在本篇笔记中没有进行说明，但是在我的github仓库LeGO-LOAM_NOTED中写了注释，如果对你有帮助，欢迎star。
(featureAssociation.cpp 完)