Lego_Loam--源码分析
0 整体框架分析
翻看 LEGO-Loam 的代码目录，首先进入到launch 文件中，看到：
<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"/>
luanch 文件启动了四个 node ，所以整个程序执行了四个 node 的基础上完成了整体功能的实现。
然后再打开 CmakeList.txt 可以看到这些 node 的一些依赖
add_executable(imageProjection src/imageProjection.cpp)
add_dependencies(imageProjection ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp)
target_link_libraries(imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
add_executable(featureAssociation src/featureAssociation.cpp)
add_dependencies(featureAssociation ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp)
target_link_libraries(featureAssociation ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
add_executable(mapOptmization
src/mapOptmization.cpp
src/Scancontext.cpp
)
target_link_libraries(mapOptmization
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${OpenCV_LIBRARIES}
gtsam
Eigen3::Eigen
)
add_executable(transformFusion src/transformFusion.cpp)
target_link_libraries(transformFusion ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
简单截取了以上部分，每一个节点之间链接了那些库可以看的很清楚。接下来的内容，按照这四个 node 的顺序 逐步对整个代码进行分析
1 ImageProjection – 将激光点云处理成图像
在这个node 中，cpp 文件实现了 激光点云特征提取，标号，分割等功能
第一步，先打开main 函数：
int main(int argc, char **argv)
{
ros::init(argc, argv, "lego_loam");
ImageProjection IP;
ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");
ros::spin();
return 0;
}
在 main 函数中，只是实例化了一个对象 IP ，所以 一旦创建了一个对象，就进入到了 Class 中去执行构造函数。
ImageProjection() : nh("~")
{
// 订阅原始的激光点云
subLaserCloud = nh.subscribe(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
// 转换成图片的点云
pubFullCloud = nh.advertise("/full_cloud_projected", 1);
// 转换成图片的并带有距离信息的点云
pubFullInfoCloud = nh.advertise("/full_cloud_info", 1);
// 发布提取的地面特征
pubGroundCloud = nh.advertise("/ground_cloud", 1);
// 发布已经分割的点云
pubSegmentedCloud = nh.advertise("/segmented_cloud", 1);
// 具有几何信息的分割点云
pubSegmentedCloudPure = nh.advertise("/segmented_cloud_pure", 1);
pubSegmentedCloudInfo = nh.advertise("/segmented_cloud_info", 1);
// 含有异常信息的点云
pubOutlierCloud = nh.advertise("/outlier_cloud", 1);
nanPoint.x = std::numeric_limits::quiet_NaN();
nanPoint.y = std::numeric_limits::quiet_NaN();
nanPoint.z = std::numeric_limits::quiet_NaN();
nanPoint.intensity = -1;
allocateMemory();
resetParameters();
}
class 中的构造函数中，订阅了原始的激光点云数据（subLaserCloud）然后就进入到了 回调函数（ImageProjection::cloudHandler）对点云数据进行处理。
// 订阅激光雷达点云信息之后的回调函数
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
// 1. 使用 pcl库
copyPointCloud(laserCloudMsg);
// 2. 找到开始时刻和结束时刻的方向角度
findStartEndAngle();
// 3.将点云信息投影到 16 * 1800 分辨率的图像上（点云阵列上）
projectPointCloud();
// 4. 根据上下线束 俯仰角 判断是否是 地面 （角度小于10度 为地面）
groundRemoval();
// 5. 点云分割 首先对点云进行聚类标记 然后通过聚类完成的标签 对点云分块存储
cloudSegmentation();
剥洋葱
// 6. Publish all clouds 发布各类型的点云
publishCloud();
// 7. 重启
resetParameters();
}
回调函数中 按照以上 7 个逻辑步骤 对点云进行处理 个人认为这个回调函数写的相当nice 主要的逻辑步骤通过一个个封装好的函数呈现出来。按照总分的结构下面将逐步对这几个步骤进行分析。
1.1 ROS 消息转PCL
由于激光雷达点云消息在传递过程中使用的是ROS 类型的消息，所以在处理的过错中统一需要对消息类型转换，通常的办法就是使用 PCL 库
void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
// 1. 读取ROS点云转换为PCL点云
cloudHeader = laserCloudMsg->header;
cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
//2.移除无效的点云 Remove Nan points
std::vector indices;
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
// 3. have "ring" channel in the cloud or not
// 如果点云有"ring"通过，则保存为laserCloudInRing
// 判断是不是使用了 velodyne 的激光雷达
if (useCloudRing == true)
{
pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
if (laserCloudInRing->is_dense == false)
{
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
}
}
这个函数主体内容 分为三个步骤，其中的第三步 注意区分 自己使用的激光雷达是否存在 CloudRing 没有这个的激光雷达 接下来的步骤都不会去执行，所以在实际自己跑这个框架的时候一定要注意到这个地方
1.2 寻找方向角
void findStartEndAngle()
{
// 1.开始点和结束点的航向角 (负号表示顺时针旋转)
segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
// 加 2 * M_PI 表示已经转转了一圈
segMsg.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
laserCloudIn->points[laserCloudIn->points.size() - 1].x) +
2 * M_PI;
// 2.保证 所有角度 落在 [M_PI , 3M_PI] 上
if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI)
{
segMsg.endOrientation -= 2 * M_PI;
}
else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
segMsg.endOrientation += 2 * M_PI;
segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
}
这个函数在 LOAM 代码里面也存在。首先通过一个反正切函数 找到一帧激光点云起始时刻和结束时刻的航向角 第二步的作用 我个人认为是将一帧激光点云的航向角度限制在【pi , 3pi 】 之间，方便后续计算出一帧激光点云的时间。
1.3 激光点云图片化
// 3.点云消息处理成图像方式的阵列 将一帧点云变成 16* 1800 的图片
// rangeMat.at(rowIdn, columnIdn) = range; 关键的代码
void projectPointCloud()
{
// 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)
{
// 提取点云中 x y z 坐标数值
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
// 判断是不是使用了 velodyne 的雷达
if (useCloudRing == true)
{
// 提取激光雷达线束到 rowIdn
rowIdn = laserCloudInRing->points[i].ring;
}
// 是其他的雷达 就通过俯仰角 确定当前的激光点是来自哪个线束 index
else
{
verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
// idx = (theta +15.1)/2
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
}
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
// 保证 columnIdn 的大小在 [0 , 1800)
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;
// 如果距离小于 1米 则过滤掉 通常是过滤自身（距离传感器比较近的点）
range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
if (range < sensorMinimumRange)
continue;
// 将计算下来的距离传感器的 数值保存到 rangeMat 中
// 这是一个 16 * 1800 的矩阵 rowIdn为线束数值 columnIdn 是 一圈圆形 滩平之后的数值
// range 是特征点云点到原点的数值
// 这样就将一个三维的坐标 转换到一个 矩阵中了.
rangeMat.at(rowIdn, columnIdn) = range;
// 将 index 和 横坐标存储在 intensity 中 整数部分是线束数值 小数部分是方向角度
thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
// //深度图的索引值 index = 列号 + 行号 * 1800
index = columnIdn + rowIdn * Horizon_SCAN;
// 个人理解是 fullCloud 存放的是坐标信息（二维的）， fullInfoCloud 增加了点到传感器的距离信息（三维的）
fullCloud->points[index] = thisPoint;
fullInfoCloud->points[index] = thisPoint;
// intensity 中存放的是点云点到传感器的距离
fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
}
}
此处对激光点云的处理是 LEGO-LOAM 和 LOAM 的第一个区别。这里将一帧激光点云处理成一个 16 * 1800 像素的图片。 16代表的是激光雷达的线束，1800代表的是激光扫描一圈时候是 间隔0.5度发射一个激光束 所以 360 * 0.5 = 1800
再者，rangeMat.at(rowIdn, columnIdn) 这里调用了一个 opencv 的多维数组，数组中的行与列 对应的就是图片的长和宽，然后数组中存放的数值就是图片相应位置上面的点到激光雷达的距离。
最后将点云的坐标信息存储在 intensity 中，采用的方法是：整数+小数 存储。整数部分存储的是激光线束数值，小数部分存储的水平航向值。
1.4 滤除地面点
首先。判断地面点的标志就是相邻两个激光线束扫描到点的坐标，如果两个坐标之间的差值 或者两个坐标之间的斜率大于一个设定的值，则将该点
判断是地面点。所以此处用到了激光点云图片的深度信息
// 4.滤除地面点
void groundRemoval()
{
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)
{
// 前7个激光雷达扫描线束足够满足地面点的检测 所以只遍历 7 次
for (size_t i = 0; i < groundScanInd; ++i)
{
lowerInd = j + (i)*Horizon_SCAN;
upperInd = j + (i + 1) * Horizon_SCAN;
// 如果之前计算过的 intensity 是 -1 则直接默认为是一个无效点
if (fullCloud->points[lowerInd].intensity == -1 ||
fullCloud->points[upperInd].intensity == -1)
{
// 对这个无效点直接进行贴标签
groundMat.at(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(i, j) = 1;
groundMat.at(i + 1, j) = 1;
}
}
}
// 给地面点 标记一个符号 为 -1
for (size_t i = 0; i < N_SCAN; ++i)
{
for (size_t j = 0; j < Horizon_SCAN; ++j)
{
if (groundMat.at(i, j) == 1 || rangeMat.at(i, j) == FLT_MAX)
{
labelMat.at(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(i, j) == 1)
groundCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]);
}
}
}
}
1.5 点云分割
1.5.1 分割函数
从论文中可以看到 分割函数的来源参考了两篇论文 （ Effificient Online Segmentation for Sparse 3D Laser Scans ，另外一篇 Fast Range Image-Based Segmentation of Sparse 3D Laser Scans for Online Operation）的方法 。方法的原理可以参考知乎文章 《地面点障碍物快速分割聚类》 网址是：https://zhuanlan.zhihu.com/p/72932303 里面详细的说明了点云分割的原理。此处只对分割函数进行逐步分析。
void 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};
// 传进来的两个参数，按照坐标不同 分别给他们放到 X 与 Y 的数组中
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(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(thisIndX, thisIndY) != 0)
continue;
d1 = std::max(rangeMat.at(fromIndX, fromIndY),
rangeMat.at(thisIndX, thisIndY));
d2 = std::min(rangeMat.at(fromIndX, fromIndY),
rangeMat.at(thisIndX, thisIndY));
if ((*iter).first == 0)
alpha = segmentAlphaX;
else
alpha = segmentAlphaY;
angle = atan2(d2 * sin(alpha), (d1 - d2 * cos(alpha)));
// ROS_INFO_STREAM("angle=" << (angle * 180 / M_PI));
if (angle > segmentTheta)
{
queueIndX[queueEndInd] = thisIndX;
queueIndY[queueEndInd] = thisIndY;
++queueSize;
++queueEndInd;
labelMat.at(thisIndX, thisIndY) = labelCount;
lineCountFlag[thisIndX] = true;
allPushedIndX[allPushedIndSize] = thisIndX;
allPushedIndY[allPushedIndSize] = thisIndY;
++allPushedIndSize;
}
}
}
// check if this segment is valid
bool feasibleSegment = false;
// 如果是 allPushedIndSize 累加的数量增加到了30 个 则判断这部分点云属于一个聚类
if (allPushedIndSize >= 30)
feasibleSegment = true;
// 如果垂直 方向上点的数量大于 5个 默认是一个有效的聚类
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
// 无效的点 则标号999999
for (size_t i = 0; i < allPushedIndSize; ++i)
{
labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999;
}
}
}
这里设计到了 BFS 算法，有兴趣可以深入研究一下。
1.5.2 点云分割过程
void cloudSegmentation()
{
// segmentation process 分割过程
for (size_t i = 0; i < N_SCAN; ++i) // 16线的 一个线束一个的遍历
for (size_t j = 0; j < Horizon_SCAN; ++j) // 水平 的 1800
if (labelMat.at(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(i, j) > 0 || groundMat.at(i, j) == 1)
{
// outliers that will not be used for optimization (always continue)
// 勾勒出优化过程中不被使用的值
// 1. 如果label为999999则跳过
if (labelMat.at(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
// 2. 如果为地，跳过index不是5的倍数的点
if (groundMat.at(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(i, j) == 1);
// mark the points' column index for marking occlusion later
segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
// save range info
segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at(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;
}
2 FeatureAssociation – 特征点提取与匹配
进入到这个cpp 文件中，看到主函数的内容：主函数内部做了两件事情，一个是实例化一个对象，一个是循环执行 类方法
FA.runFeatureAssociation(); 下面的内容将对这两个部分展开分析。
int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");
ROS_INFO("\033[1;32m---->\033[0m Feature Association Started.");
FeatureAssociation FA;
ros::Rate rate(200);
while (ros::ok())
// while ( 1 )
{
ros::spinOnce();
FA.runFeatureAssociation();
rate.sleep();
}
ros::spin();
return 0;
}
主函数里面首选实例化了一个对象 FA ，所以和之前的ImageProjection 处理过程一样，执行构造函数的内容，所以在构造函数中会订阅上一个cpp 文件中 pub 出来的消息。然后转到回调函数中处理数据，此处的回调函数都是将相应的信息存储到buff 中 所以不做详细分析。
FeatureAssociation():
nh("~")
{
// 订阅了分割之后的点云
subLaserCloud = nh.subscribe("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
// 订阅的分割点云含有图像的深度信息
subLaserCloudInfo = nh.subscribe("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
// 非聚类的点
subOutlierCloud = nh.subscribe("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
// IMU传感器的消息
subImu = nh.subscribe(imuTopic, 50, &FeatureAssociation::imuHandler, this);
// 发布面特征点 和 边特征点
pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 1);
pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 1);
pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 1);
pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 1);
pubLaserCloudCornerLast = nh.advertise("/laser_cloud_corner_last", 2);
pubLaserCloudSurfLast = nh.advertise("/laser_cloud_surf_last", 2);
pubOutlierCloudLast = nh.advertise("/outlier_cloud_last", 2);
pubLaserOdometry = nh.advertise ("/laser_odom_to_init", 5);
initializationValue();
}
进入到函数runFeatureAssociation（）的主体部分，函数通过里面完成了两个大的功能：特征提取和特征匹配，和之前上一个章节一致，按照顺序执行每一个函数。
void runFeatureAssociation()
{
if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05){
newSegmentedCloud = false;
newSegmentedCloudInfo = false;
newOutlierCloud = false;
}else{
return;
}
/**
1. Feature Extraction
*/
// 1-1去除运动畸变
adjustDistortion();
// 1-2 计算曲率
calculateSmoothness();
// 1-3 标记出远点和离散点
markOccludedPoints();
// 1-4 特征提取
extractFeatures();
// 1-5 发布提取的特征点云消息
publishCloud(); // cloud for visualization
/**
2. Feature Association
*/
if (!systemInitedLM) {
checkSystemInitialization();
return;
}
//2-1 更新初始位姿
updateInitialGuess();
//2-2 特征点匹配
updateTransformation();
// 2-3坐标变换
integrateTransformation();
// 2-4发布雷达里程计消息
publishOdometry();
// 2-5发布用于图优化的点云
publishCloudsLast(); // cloud to mapOptimization
}
};
2.1 特征点提取
与loam 相比较 在特征点提取之前，对特征点进行预处理。
2.1.1 运动畸变去除
从代码中看，他的运动畸变去除依靠的是IMU 传感器，但是这一点在论文中没有体现出来。
有兴趣的伙伴可以移步至 LEGO-LOAM源码解析 【一个废物】https://zhuanlan.zhihu.com/p/242559124
这里观看详细的分析和公式的推导。
2.1.2 曲率计算
作者对曲率的计算方式相对比较简单。并没有严格按照论文上的公式，但是求解曲率的目的也是为了寻找特征点，咱们也没有必要吹毛求疵。代码部分和loam 相比较没有明显的改动，参照loam 即可。
2.1.3 标记瑕疵点
这个函数算是 对A-loam 代码的一个交代了， loam 论文中指出了一些遮挡点和缝隙点，由于LEGO-Loam 中 已经对点云聚类处理了，所以，此处标记的瑕疵点，更多就是遮挡点了，而在此处被标记的点将不会被纳入到特征点的提取中。
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(float(segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i]));
float diff2 = std::abs(float(segInfo.segmentedCloudRange[i+1] - segInfo.segmentedCloudRange[i]));
// 如果一个点突变很远，则也被标记成 1
if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])
cloudNeighborPicked[i] = 1;
}
}
从代码中分析，在激光点云图像中（原始点云图像化处理之后）如果一个点和左右相邻的两个点之间的距离比较大，那么就认为这个很有可能是一个噪声点，然后提取特征时候尽量避免这个点。
2.1.4 特征点提取
特征点的提取过程和loam 一样。提取曲率最大的两个点作为边特征点，然后在剩下的里面找到 20个作为备选的20个边特征点。但是面特征点选取不同，此处的面特征点选取放在了被标记的地面点中，在里面选取了曲率最小的4个点作为面特征点，剩下的作为备选点，如果在某处点云过于密集，则会降采样处理，这样就保证了选取的特征点不在过于密集的地方。
void extractFeatures()
{
// 初始化对应的指针
cornerPointsSharp->clear();
cornerPointsLessSharp->clear();
surfPointsFlat->clear();
surfPointsLessFlat->clear();
for (int i = 0; i < N_SCAN; i++) {
surfPointsLessFlatScan->clear();
// 将一帧激光图像分成 6个子图 提取的特征点相对均匀
for (int j = 0; j < 6; j++) {
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;
// 按照曲率的大小排序
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
int largestPickedNum = 0;
// 边特征点的选择
for (int k = ep; k >= sp; k--) {
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > edgeThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == false) {
largestPickedNum++;
// 挑选出曲率最大的点
if (largestPickedNum <= 2) {
cloudLabel[ind] = 2;
cornerPointsSharp->push_back(segmentedCloud->points[ind]);
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
// 把剩下的 20 个备选的边特征点选取出来
} else if (largestPickedNum <= 20) {
cloudLabel[ind] = 1;
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else {
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;
}
}
}
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]);
smallestPickedNum++;
// 选取超过4个之后 跳出 只选4个点
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]);
}
}
}
// 降低采样频率 防止选取过于密集
surfPointsLessFlatScanDS->clear();
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.filter(*surfPointsLessFlatScanDS);
*surfPointsLessFlat += *surfPointsLessFlatScanDS;
}
}
2.2 特征点匹配
特征点的匹配问题，和LOAM 分析差不多。从作者的论文里面分析，他的原理也是 通过从上一帧激光点云中的 角点和面点 ，找到当前帧的角点和面点之间的为位姿匹配。
作者在特征点匹配中使用到了 两次L -M 的优化。通过翻译原文可以知道两步 L-M 的过程如下：
第一步： 通过匹配当前帧的面特征点与上一帧面特征点之间的对应关系来估计 【Z ， Roll , Pitch】 这三个变量。
第二步：剩下的【x ， y ， yaw】 使用当前帧的边特征点与上一帧中的边特征点的关系进行估计，同时使用 【x ， y ， yaw】作为约束。
注意： 第一步优化也能得到 【x ， y ， yaw】但是在第二步没有用到 整个过程中得到的 6D 转换是两步得到的数据融合得到的。
通过作者在论文中的描述，他将特征匹配的过程分为了两个部分。接下来先分析面特征点中只用到了【Z ， Roll , Pitch】
如下图所示，在同一个地面上，用 A 与B 两个圆表示两个点云，他们想要在同一个地面上，就得保证 【Z ， Roll , Pitch】三个数值是相同的，意味着就是这已经在这三个维度上匹配好了。作者也说了 【x ， y ， yaw】 也可以得到，但是他没有使用。
在代码中，可以进行逐步分析.
2.2.1更新初始位姿
这个函数主要功能是对激光里程计初始化，在loam 代码中，激光里程计是将第一帧作为参考帧。如果系统没有初始化，则一直将当前帧作为参考帧。函数根据 IMU积分的结果，计算出一个初始位姿transformCur，这个位姿指的是雷达旋转一圈后发生的相对位姿变换。—本文中涉及到 IMU 的内容都没有看懂。
2.2.2 特征点匹配
边特征点和面特征点之间的匹配，这个过程和 LOAM 一致。
void updateTransformation(){
// 如果角特征点数量 和 面特征点数量较少 返回不执行匹配过程
if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
return;
// 面特征匹配
for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
laserCloudOri->clear();
coeffSel->clear();
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;
}
}
2.2.3坐标变换
这里主要是对函数 integrateTransformation() 分析。网上检索到的信息说是 将IMU信息融入到位姿更新当中
分析一下代码：
void integrateTransformation(){
float rx, ry, rz, tx, ty, tz;
//transformSum 是 IMU累计的变化量 ，0，1，2 分别代表的是 pitch yaw roll 的内容，
//transformCur 是当前的分量，函数的作用是局部坐标转换成全局坐标
AccumulateRotation(transformSum[0], transformSum[1], transformSum[2],
-transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);
float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX)
- sin(rz) * (transformCur[4] - imuShiftFromStartY);
float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX)
+ cos(rz) * (transformCur[4] - imuShiftFromStartY);
float z1 = transformCur[5] - imuShiftFromStartZ;
float x2 = x1;
float y2 = cos(rx) * y1 - sin(rx) * z1;
float z2 = sin(rx) * y1 + cos(rx) * z1;
tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
ty = transformSum[4] - y2;
tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
加入 imu 当前数据 更新位姿
PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart,
imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);
transformSum[0] = rx;
transformSum[1] = ry;
transformSum[2] = rz;
transformSum[3] = tx;
transformSum[4] = ty;
transformSum[5] = tz;
}
2.2.4发布里程计消息
此处发布了两个消息，第一个是publishOdometry 发布激光里程计位姿和 TF 变换。这里的坐标变换实际上是odom 相对于地图原点的，所以会有累计误差出现。第二个消息是publishCloudsLast 由于上一帧的点云已经被统一到了当前帧的起始时刻，匹配的是当前帧的终止位姿相对于起始位姿的转换，此处的内容是按照优化后的结果，那当前帧的点云统一到终止时刻，然后将特征点云发布出去。
3 mapOptimization --图的优化
首先，作者在论文中描述的了图优化的过程，他将边特征点和面特征点与上一个时刻的点云地图进行匹配。作者说明了在这个环节中与loam 的区别，点云地图的存储是以特征点集合的方式完成的（只存储集合，不存储点云地图）。
文中定义了一个
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-lv3sXIhr-1648563653925)(C:\Users\92837\AppData\Roaming\Typora\typora-user-images\image-20220324134303782.png)]
这个集合中保存了所有的特征点云的集合。M 中的每一个子特征点集合，与扫描的传感器的位姿有关系。
然后 定义了 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-NQfac3Wg-1648563653926)(C:\Users\92837\AppData\Roaming\Typora\typora-user-images\image-20220324135334294.png)]
k 决定了 Q 的大小，Q的获取
进入到这个node 首先看到的是主函数的内容；主函数实例化了一个对象 MO 之后，然后开启了两个线程，第一个线程是执行回环检测的功能，第二个线程是用来可视化作用。最后在函数中循环执行 run（） 函数。
int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");
ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
mapOptimization MO;
// 回环检测线程和 可视化线程
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
ros::Rate rate(200);
while (ros::ok())
// while ( 1 )
{
ros::spinOnce();
MO.run();
rate.sleep();
}
loopthread.join();
visualizeMapThread.join();
return 0;
}
由于前文的分析可以知道，在前端完成了 点云信息的预处理，激光里程计的帧间匹配完成实现了激光里程计功能。但是前端实现的功能中会出现累计误差，所以这个结点的作用就是通过回环检测的方式，将累计误差降低。
最后我们进入到 run（）函数中：
void run(){
// 判断进来的点云消息是否更新了 更新才往后执行
if (newLaserCloudCornerLast && std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
newLaserCloudSurfLast && std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
newLaserOdometry)
{
newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;
std::lock_guard lock(mtx);
// 以相对较慢的速度建图
if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
timeLastProcessing = timeLaserOdometry;
// 3-1转换到 map 坐标系下
transformAssociateToMap();
// 3-2 提取周围关键帧
extractSurroundingKeyFrames();
// 3-3 降采样
downsampleCurrentScan();
// 3-4 scan -- map 之间的优化
scan2MapOptimization();
// 3-5 关键帧保存
saveKeyFramesAndFactor();
// 3-6 位姿矫正
correctPoses();
// 3-7 发布坐标变换
publishTF();
// 3-8 发布关键帧和姿态
publishKeyPosesAndFrames();
// 3-9 点云清除
clearCloud();
}
}
}
};
3.1坐标系变换
3.2 关键帧提取
从代码的含义中分析，整个过程对是否使用回环检测进行判断，如果使用了回环检测，则提取上一时刻的 50个关键帧。
如果是没有使用图优化，则提取当前欧氏距离 内 最近的 50个关键帧。
void extractSurroundingKeyFrames(){
if (cloudKeyPoses3D->points.empty() == true)
return;
// 判断是否进行了回环检测
if (loopClosureEnableFlag == true){
// only use recent key poses for graph building
// 只使用最近的关键姿态进行图的构建
// 如果关键帧队列数量少于 50
if (recentCornerCloudKeyFrames.size() < surroundingKeyframeSearchNum){ // queue is not full (the beginning of mapping or a loop is just closed)
// clear recent key frames queue
// 清除关键帧序列
recentCornerCloudKeyFrames. clear();
recentSurfCloudKeyFrames. clear();
recentOutlierCloudKeyFrames.clear();
int numPoses = cloudKeyPoses3D->points.size();
for (int i = numPoses-1; i >= 0; --i){
int thisKeyInd = (int)cloudKeyPoses3D->points[i].intensity;
PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
updateTransformPointCloudSinCos(&thisTransformation);
// extract surrounding map
recentCornerCloudKeyFrames. push_front(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
recentSurfCloudKeyFrames. push_front(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
recentOutlierCloudKeyFrames.push_front(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
if (recentCornerCloudKeyFrames.size() >= surroundingKeyframeSearchNum)
break;
}
}else{ // queue is full, pop the oldest key frame and push the latest key frame
// 更新存储的关键帧，把旧的弹出，新的添加进去
if (latestFrameID != cloudKeyPoses3D->points.size() - 1){ // if the robot is not moving, no need to update recent frames
recentCornerCloudKeyFrames. pop_front();
recentSurfCloudKeyFrames. pop_front();
recentOutlierCloudKeyFrames.pop_front();
// push latest scan to the end of queue
latestFrameID = cloudKeyPoses3D->points.size() - 1;
PointTypePose thisTransformation = cloudKeyPoses6D->points[latestFrameID];
updateTransformPointCloudSinCos(&thisTransformation);
recentCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[latestFrameID]));
recentSurfCloudKeyFrames. push_back(transformPointCloud(surfCloudKeyFrames[latestFrameID]));
recentOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[latestFrameID]));
}
}
// 点云拼接
for (int i = 0; i < recentCornerCloudKeyFrames.size(); ++i){
*laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *recentSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *recentOutlierCloudKeyFrames[i];
}
}else{ // 如果没有回环检测
surroundingKeyPoses->clear();
surroundingKeyPosesDS->clear();
// extract all the nearby key poses and downsample them
// 找到最近的 50个点 然后提取到这里
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
kdtreeSurroundingKeyPoses->radiusSearch(currentRobotPosPoint, (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis, 0);
for (int i = 0; i < pointSearchInd.size(); ++i)
surroundingKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchInd[i]]);
downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
// delete key frames that are not in surrounding region
// 删除 不在周围的关键帧
int numSurroundingPosesDS = surroundingKeyPosesDS->points.size();
for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i){
bool existingFlag = false;
for (int j = 0; j < numSurroundingPosesDS; ++j){
if (surroundingExistingKeyPosesID[i] == (int)surroundingKeyPosesDS->points[j].intensity){
existingFlag = true;
break;
}
}
if (existingFlag == false){
surroundingExistingKeyPosesID. erase(surroundingExistingKeyPosesID. begin() + i);
surroundingCornerCloudKeyFrames. erase(surroundingCornerCloudKeyFrames. begin() + i);
surroundingSurfCloudKeyFrames. erase(surroundingSurfCloudKeyFrames. begin() + i);
surroundingOutlierCloudKeyFrames.erase(surroundingOutlierCloudKeyFrames.begin() + i);
--i;
}
}
// add new key frames that are not in calculated existing key frames
// 添加新的关键帧
for (int i = 0; i < numSurroundingPosesDS; ++i) {
bool existingFlag = false;
for (auto iter = surroundingExistingKeyPosesID.begin(); iter != surroundingExistingKeyPosesID.end(); ++iter){
if ((*iter) == (int)surroundingKeyPosesDS->points[i].intensity){
existingFlag = true;
break;
}
}
if (existingFlag == true){
continue;
}else{
int thisKeyInd = (int)surroundingKeyPosesDS->points[i].intensity;
PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
updateTransformPointCloudSinCos(&thisTransformation);
surroundingExistingKeyPosesID. push_back(thisKeyInd);
surroundingCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
surroundingSurfCloudKeyFrames. push_back(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
surroundingOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
}
}
// 点云拼接
for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) {
*laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingOutlierCloudKeyFrames[i];
}
}
// 降低特征点云的采样速度
// Downsample the surrounding corner key frames (or map)
downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->points.size();
// Downsample the surrounding surf key frames (or map)
downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->points.size();
}
SUM :前面的几个函数，转换到map 坐标系，提取关键帧，降采样等 主要的作用就是方便后续的图优化过程，提高图优化的速度。
3.3 图优化
这个函数的作用主要是 使用 scan --> map 的位姿匹配过程，机器人当前的位姿 transformTobeMapped 与之前的激光里程计构建的地图匹配，最后通过非线性优化，使得两者之间的误差最小，多次迭代之后，误差变得更小。在优化的同时，插入 IMU 信息然后对 roll 和 pitch 修正，对transformTobeMapped 进行中值滤波，最后获取最终的机器人位姿。
虽然，在 scan --> scan 的匹配之后，又进行了 scan --> map 的匹配，但是只是在局部的实验中完成了回环检测的过程，所以实际意义上，这依旧是没有修正累计误差。
void scan2MapOptimization(){
// laserCloudCornerFromMapDSNum是extractSurroundingKeyFrames()函数最后降采样得到的coner点云数
// laserCloudSurfFromMapDSNum是extractSurroundingKeyFrames()函数降采样得到的surface点云数
if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {
// laserCloudCornerFromMapDS 与 laserCloudSurfFromMapDS 来源有两种情况
// 第一种：来自 recentCornerCloudKeyFrames 此时有回环检测
// 第二种：来自 surroundingCornerCloudKeyFrames 此时没有使用回环检测
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
for (int iterCount = 0; iterCount < 10; iterCount++) {
laserCloudOri->clear();
coeffSel->clear();
cornerOptimization(iterCount);
surfOptimization(iterCount);
if (LMOptimization(iterCount) == true)
break;
}
// 此时迭代结束，更新转移矩阵
transformUpdate();
}
}
通过代码可以分析出，这里对是否进行回环检测做了判断，与前面的图优化预处理阶段的数据来源对应上了。整体的框架主要干活的是 cornerOptimization() 与 surfOptimization() 这两个函数。
3.4 保存关键帧和因子图
saveKeyFramesAndFactor() 这个函数的主要作用就是选取关键帧。如果当前帧与上一帧之间的欧氏距离 大于 0.3 米 则认为是一个新的关键帧。此时需要计算出当前帧与与上一帧之间的约束情况。 这种约束可以认为是小回环，加入到后端中去优化。将优化后的结果作为关键帧的位姿与点云，然后同步到 scan --> map 过程中。
void scan2MapOptimization(){
// laserCloudCornerFromMapDSNum是extractSurroundingKeyFrames()函数最后降采样得到的coner点云数
// laserCloudSurfFromMapDSNum是extractSurroundingKeyFrames()函数降采样得到的surface点云数
if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {
// laserCloudCornerFromMapDS 与 laserCloudSurfFromMapDS 来源有两种情况
// 第一种：来自 recentCornerCloudKeyFrames 此时有回环检测
// 第二种：来自 surroundingCornerCloudKeyFrames 此时没有使用回环检测
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
for (int iterCount = 0; iterCount < 10; iterCount++) {
laserCloudOri->clear();
coeffSel->clear();
cornerOptimization(iterCount);
surfOptimization(iterCount);
if (LMOptimization(iterCount) == true)
break;
}
// 此时迭代结束，更新转移矩阵
transformUpdate();
}
}
void saveKeyFramesAndFactor(){
currentRobotPosPoint.x = transformAftMapped[3];
currentRobotPosPoint.y = transformAftMapped[4];
currentRobotPosPoint.z = transformAftMapped[5];
bool saveThisKeyFrame = true;
// 如果当前帧和前一帧的距离小于 0.3米 则不更新关键帧
if (sqrt((previousRobotPosPoint.x-currentRobotPosPoint.x)*(previousRobotPosPoint.x-currentRobotPosPoint.x)
+(previousRobotPosPoint.y-currentRobotPosPoint.y)*(previousRobotPosPoint.y-currentRobotPosPoint.y)
+(previousRobotPosPoint.z-currentRobotPosPoint.z)*(previousRobotPosPoint.z-currentRobotPosPoint.z)) < 0.3){ // save keyframe every 0.3 meter
saveThisKeyFrame = false;
}
if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())
return;
previousRobotPosPoint = currentRobotPosPoint;
/**
* update grsam graph
*/
// 把当前的 pose 加入到 grsam graph
if (cloudKeyPoses3D->points.empty()){
gtSAMgraph.add(PriorFactor(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));
initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));
for (int i = 0; i < 6; ++i)
transformLast[i] = transformTobeMapped[i];
}
else{
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),
Point3(transformLast[5], transformLast[3], transformLast[4]));
gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));
gtSAMgraph.add(BetweenFactor(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise));
initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4])));
}
/**
* update iSAM
*/
isam->update(gtSAMgraph, initialEstimate);
isam->update();
gtSAMgraph.resize(0);
initialEstimate.clear();
/**
* save key poses
*/
PointType thisPose3D;
PointTypePose thisPose6D;
Pose3 latestEstimate;
isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at(isamCurrentEstimate.size()-1);
thisPose3D.x = latestEstimate.translation().y();
thisPose3D.y = latestEstimate.translation().z();
thisPose3D.z = latestEstimate.translation().x();
thisPose3D.intensity = cloudKeyPoses3D->points.size(); // this can be used as index
cloudKeyPoses3D->push_back(thisPose3D);
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity; // this can be used as index
thisPose6D.roll = latestEstimate.rotation().pitch();
thisPose6D.pitch = latestEstimate.rotation().yaw();
thisPose6D.yaw = latestEstimate.rotation().roll(); // in camera frame
thisPose6D.time = timeLaserOdometry;
cloudKeyPoses6D->push_back(thisPose6D);
/**
* save updated transform
*/
if (cloudKeyPoses3D->points.size() > 1){
transformAftMapped[0] = latestEstimate.rotation().pitch();
transformAftMapped[1] = latestEstimate.rotation().yaw();
transformAftMapped[2] = latestEstimate.rotation().roll();
transformAftMapped[3] = latestEstimate.translation().y();
transformAftMapped[4] = latestEstimate.translation().z();
transformAftMapped[5] = latestEstimate.translation().x();
for (int i = 0; i < 6; ++i){
transformLast[i] = transformAftMapped[i];
transformTobeMapped[i] = transformAftMapped[i];
}
}
pcl::PointCloud::Ptr thisCornerKeyFrame(new pcl::PointCloud());
pcl::PointCloud::Ptr thisSurfKeyFrame(new pcl::PointCloud());
pcl::PointCloud::Ptr thisOutlierKeyFrame(new pcl::PointCloud());
pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame);
/*
Scan Context loop detector
- ver 1: using surface feature as an input point cloud for scan context (2020.04.01: checked it works.)
- ver 2: using downsampled original point cloud (/full_cloud_projected + downsampling)
*/
bool usingRawCloud = true;
if( usingRawCloud ) { // v2 uses downsampled raw point cloud, more fruitful height information than using feature points (v1)
pcl::PointCloud::Ptr thisRawCloudKeyFrame(new pcl::PointCloud());
pcl::copyPointCloud(*laserCloudRawDS, *thisRawCloudKeyFrame);
scManager.makeAndSaveScancontextAndKeys(*thisRawCloudKeyFrame);
}
else { // v1 uses thisSurfKeyFrame, it also works. (empirically checked at Mulran dataset sequences)
scManager.makeAndSaveScancontextAndKeys(*thisSurfKeyFrame);
}
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);
} // saveKeyFramesAndFactor
3.5 发布消息
参考内容
1 LeGO-LOAM分析 【王方浩】https://zhuanlan.zhihu.com/p/384902839
2 lego-loam阅读理解笔记 二【高飞 007】 https://blog.csdn.net/qingdu007/article/details/108307913
3 SC-LEGO-LOAM 扩展以及深度解析 【lovely_yoshino】 https://www.guyuehome.com/34083
4 LEGO-LOAM源码解析 【一个废物】https://zhuanlan.zhihu.com/p/242559124
5 三维SLAM算法LeGO-LOAM源码阅读（二） https://www.codeleading.com/article/9834466048/
6 SC-Lego-LOAM解析 【激光SLAM小白】 https://zhuanlan.zhihu.com/p/348281520
7 开源SLAM系统：LEGO-LOAM源码解析 http://xchu.net/2020/09/20/52lego/
8 LeGOLOAM代码阅读(3)——mapOptmization.cpp 【shuang_yu_ 】 https://blog.csdn.net/shuang_yu_/article/details/107826532