loam代码分析（3）

mapOptmization世界坐标系转换提取submap（extractSurroundingKeyFrames）非闭环处理闭环处理 当前帧降采样（downsampleCurrentScan）scanTOmap优化匹配（scan2MapOptimization）存储当前帧pose信息和cloud(saveKeyFramesAndFactor) 闭环检测闭环优化

目前最新开源的3d slam算法lego-loam，为loam的改进版。同时另有高人进行了工程优化。 其原作者github 其工程优化的github 个人对其工程优化后的代码和原理进行了基本分析； 包含：

imageProjecionFeatureAssociationmapOptmizationtransformFusion 这四个文件

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<Pose3>(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<PointType>::Ptr thisCornerKeyFrame( new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame( new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr thisOutlierKeyFrame( new pcl::PointCloud<PointType>()); 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<int> pointSearchIndLoop; std::vector<float> 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<PointType, PointType> 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<PointType>::Ptr unused_result( new pcl::PointCloud<PointType>()); icp.align(*unused_result); // icp 匹配

闭环优化

采用gtsam库，对位姿进行优化，与g2o相类似，以位姿pose为节点，两个位姿差为约束条件。当闭环存在时，即出现闭环约束时，进行优化后，更新位姿；