loam代码分析（3）
loam代码分析（3）
- mapOptmization
- 世界坐标系转换
- 提取submap（extractSurroundingKeyFrames）
- 非闭环处理
- 闭环处理
- 当前帧降采样（downsampleCurrentScan）
- scanTOmap优化匹配（scan2MapOptimization）
- 存储当前帧pose信息和cloud(saveKeyFramesAndFactor)
- 闭环检测
- 闭环优化
目前最新开源的3d slam算法lego-loam，为loam的改进版。同时另有高人进行了工程优化。
其原作者github
其工程优化的github
个人对其工程优化后的代码和原理进行了基本分析；
包含：
- imageProjecion
- FeatureAssociation
- mapOptmization
- transformFusion
这四个文件
mapOptmization
FeatureAssociation.cpp同样为独立线程，内部存在三个独立线程执行不同功能；
- slam主线程：
主要功能是依据每帧点云，采用LM匹配历史点云，获取当前帧在世界坐标系下坐标；按时间记录每帧的世界坐标系下的pose和对应的点云； - maping publish线程：
根据顺序存储的每帧pose和对应点云，将点云每个点转换成世界坐标系，形成一个总点云； - loopClosureThread线程：
检测当前位置在历史位置帧中是否有交集（即与历史帧位置接近），如存在构建一个闭环submap，同时匹配获取闭环位置，放入约束；
其整个slam流程如下：
- 当前帧根据odom转换世界坐标系；
- 提取submap用于点云匹配；
- 对当前帧激光点云进行降采样；
- 当前帧对submap进行匹配优化；
- 存储当前帧pose信息和cloud；
- 闭环优化后结果更新；
世界坐标系转换
OdometryToTransform(association.laser_odometry, transformSum); // 4元数转换为旋转量
transformAssociateToMap(); // 坐标转为世界坐标系
未完待续
提取submap（extractSurroundingKeyFrames）
此函数主要对历史存储的pose及其对应点云cloud进行提取，根据是否闭环需求分成两种提取方法；
非闭环处理
由于不做闭环处理，因此闭环检测和优化时间省略，为提高每一步增量匹配更加精准，采用历史缓存中与当前位置距离在一定范围内帧集合，从而形成submap；
kdtreeSurroundingKeyPoses.setInputCloud(cloudKeyPoses3D);
kdtreeSurroundingKeyPoses.radiusSearch(
currentRobotPosPoint, (double)_surrounding_keyframe_search_radius, // 没有闭环时，仅搜索与当前位置在50m内的位置，用于构建submap
pointSearchInd, pointSearchSqDis);
for (int i = 0; i < pointSearchInd.size(); ++i){ // 将50m内所有帧对应位置放入队列中
surroundingKeyPoses->points.push_back(
cloudKeyPoses3D->points[pointSearchInd[i]]);
}
for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) { // 将附近搜索范围内的n组点云形成一个整点云
*laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingOutlierCloudKeyFrames[i];
}
闭环处理
如果需要做闭环处理，则应提高匹配性能，且做简单的增量submap即可，即采用当前位置索引提取历史中前方按顺序索引的N个位置。即采用滑窗方法仅使用最近的N个点云数据形成submap；
if (recentCornerCloudKeyFrames.size() <
_surrounding_keyframe_search_num) { // 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(); // 按照最近遍历每一个轨迹点 location pose放入滑窗队列中
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() >= _surrounding_keyframe_search_num)
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) { // 构建submap的点云
*laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *recentSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *recentOutlierCloudKeyFrames[i];
}
当前帧降采样（downsampleCurrentScan）
由于构建的submap都采用了稀疏点云存储，因此当前需匹配的scan也应降采样；
scanTOmap优化匹配（scan2MapOptimization）
当目标点云数目足够多的时候，调用LM优化算法进行scanTOmap匹配。获取匹配后位姿；
if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) { // 如果当前激光帧特征点个数达到一定个数时，才进行匹配优化
kdtreeCornerFromMap.setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap.setInputCloud(laserCloudSurfFromMapDS);
for (int iterCount = 0; iterCount < 10; iterCount++) { // 优化匹配迭代10次，
laserCloudOri->clear();
coeffSel->clear();
cornerOptimization(iterCount);
surfOptimization(iterCount);
if (LMOptimization(iterCount) == true) break;
}
transformUpdate(); // 获取匹配后当前帧的位姿
具体匹配算法，后续补充；
存储当前帧pose信息和cloud(saveKeyFramesAndFactor)
每帧匹配优化后均会得到最佳匹配pose，基于存储和性能的考虑，则将每0.3m间隔进行存储；
bool saveThisKeyFrame = true;
if (sqrt((previousRobotPosPoint.x - currentRobotPosPoint.x) * // 帧移动的位置在0.3间隔进行保存
(previousRobotPosPoint.x - currentRobotPosPoint.x) +
(previousRobotPosPoint.y - currentRobotPosPoint.y) *
(previousRobotPosPoint.y - currentRobotPosPoint.y) +
(previousRobotPosPoint.z - currentRobotPosPoint.z) *
(previousRobotPosPoint.z - currentRobotPosPoint.z)) < 0.3) {
saveThisKeyFrame = false;
}
if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty()) return; // 不够0.3m位移 无需处理
存储位姿和对应的cloud
/**
* 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, 历史key location pose中的intensity为存储顺序的ID
cloudKeyPoses3D->push_back(thisPose3D); // 记录每帧激光点所在的位置 3d
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); // 记录每帧激光雷达所在位姿 6d
/**
* 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);
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);
闭环检测
- 根据当前点位姿搜索历史位姿可能存在闭环情况，即在搜索范围内最近的历史pose；
// find the closest history key frame
std::vector pointSearchIndLoop;
std::vector pointSearchSqDisLoop;
kdtreeHistoryKeyPoses.setInputCloud(cloudKeyPoses3D); // 当前机器人所在位置附近，搜索历史所有范围在闭环搜索范围内（默认为7m） 的key location，
kdtreeHistoryKeyPoses.radiusSearch(
currentRobotPosPoint, _history_keyframe_search_radius, pointSearchIndLoop,
pointSearchSqDisLoop);
closestHistoryFrameID = -1;
for (int i = 0; i < pointSearchIndLoop.size(); ++i) { // 所有在闭环搜索范围的位置ID
int id = pointSearchIndLoop[i];
if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0) { // 所有范围内location的时刻与现在时刻需在30s以上，否则无需闭环
closestHistoryFrameID = id; // 仅需找到一帧曾经的位置与当前位置在 搜索范围内，且时间满足30s以上
break;
}
}
- 找到最近历史中pose后，将最近pose索引前后5帧激光数据，即共11帧数据构建loop_submap，用于闭环匹配；
// save history near key frames
for (int j = - _history_keyframe_search_num; j <= _history_keyframe_search_num; ++j) { // 将历史中找到的一帧closestHistoryFrameID的前后_history_keyframe_search_num个作为闭环的submap
if (closestHistoryFrameID + j < 0 ||
closestHistoryFrameID + j > latestFrameIDLoopCloure)
continue;
*nearHistorySurfKeyFrameCloud += *transformPointCloud( // 包括 corner 和 surf两种特征点云
cornerCloudKeyFrames[closestHistoryFrameID + j],
&cloudKeyPoses6D->points[closestHistoryFrameID + j]);
*nearHistorySurfKeyFrameCloud += *transformPointCloud(
surfCloudKeyFrames[closestHistoryFrameID + j],
&cloudKeyPoses6D->points[closestHistoryFrameID + j]);
}
- 利用当前cloud点云信息和loop_submap采用ICP进行匹配；
// ICP Settings
pcl::IterativeClosestPoint icp;
icp.setMaxCorrespondenceDistance(100);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
// Align clouds
icp.setInputSource(latestSurfKeyFrameCloud); // 历史帧队列中最新的一帧的点云
icp.setInputTarget(nearHistorySurfKeyFrameCloudDS); // 闭环检测提取的submap
pcl::PointCloud::Ptr unused_result(
new pcl::PointCloud());
icp.align(*unused_result); // icp 匹配
闭环优化
采用gtsam库，对位姿进行优化，与g2o相类似，以位姿pose为节点，两个位姿差为约束条件。当闭环存在时，即出现闭环约束时，进行优化后，更新位姿；