学习文章：
[1]LeGO-LOAM分析之建图（三）
[2] LeGo-LOAM 源码解析-WinFrom控件库
[3] LeGO-LOAM批注版
[4]LeGO-LOAM 源码阅读笔记（mapOptmization.cpp）

整合拼起来的，自用
关于transformFusion.cpp 融合粗、精配准的里程计信息
学习文章：LeGo-LOAM源码阅读笔记(五)—transformFusion.cpp

mapOptimization

整体介绍

整体功能分为回环检测、可视化以及位姿全局优化，核心是位姿优化。

主体流程：订阅特征提取后的点云、里程计数据 -> 计算当前姿态优化的初始值 -> 提取局部关键帧 -> 降采样 -> scan-to-map地图优化（线特征、面特征、L-M）-> 保存关键帧与因子图优化 -> 闭环检测 -> 发布TF变换、点云

main()

主函数分为2个主要的功能，一是启动单独的线程进行回环检测，二是在循环中执行主流程。

int main(int argc, char** argv)
{ros::init(argc, argv, "lego_loam");ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");// 调用构造函数，订阅话题上一帧的surf，corner，outlier点云，imu，和laser_odommapOptimization MO;// std::thread 构造函数，将MO作为参数传入构造的线程中使用// 进行闭环检测与闭环的功能//1.回环检测std::thread loopthread(&mapOptimization::loopClosureThread, &MO);// 该线程中进行的工作是publishGlobalMap(),将数据发布到ros中，可视化//2.可视化线程std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);ros::Rate rate(200);while (ros::ok())       //200hz循环调用{ros::spinOnce();// 全局姿态优化的主要处理部分//3.主流程MO.run();rate.sleep();}loopthread.join();visualizeMapThread.join();return 0;
}

lidar制图在构造函数中订阅消息，订阅的消息主要是以下5个。主要是接收特征提取之后的点云，lidar里程计和IMU消息。

        subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, &mapOptimization::laserCloudCornerLastHandler, this);subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, &mapOptimization::laserCloudSurfLastHandler, this);subOutlierCloudLast = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2, &mapOptimization::laserCloudOutlierLastHandler, this);subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &mapOptimization::laserOdometryHandler, this);subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 50, &mapOptimization::imuHandler, this);

loopthread()

上面主要的performLoopClosure()函数流程：
1.先进行闭环检测detectLoopClosure()，如果返回true,则可能可以进行闭环，否则直接返回，程序结束。
2.接着使用icp迭代进行对齐。
3.对齐之后判断迭代是否收敛以及噪声是否太大，是则返回并直接结束函数。否则进行迭代后的数据发布处理。
4.接下来得到latestSurfKeyFrameCloud和nearHistorySurfKeyFrameCloudDS之间的位置平移和旋转。
5.然后进行图优化过程。

RANSAC（Random Sample Consensus）是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。

visualizeMap Thread

visualizeGlobalMapThread()代码：

void visualizeGlobalMapThread(){ros::Rate rate(0.2);while (ros::ok()){rate.sleep();publishGlobalMap();}
}

publishGlobalMap()主要进行了3个步骤：
1.通过KDTree进行最近邻搜索;
2.通过搜索得到的索引放进队列;
3.通过两次下采样，减小数据量;

run()

run()是mapOptimization类的一个成员变量
run()的运行流程：
1.判断是否有新的数据到来并且时间差值小于0.005；
2.如果timeLaserOdometry - timeLastProcessing >= mappingProcessInterval，则进行以下操作：
2.1.将坐标转移到世界坐标系下，得到可用于建图的Lidar坐标，即修改transformTobeMapped的值；
2.2.抽取周围的关键帧；
2.3.下采样当前scan；
2.4.当前scan进行图优化过程；
2.5.保存关键帧和因子；
2.6.校正位姿；
2.7.发布Tf；
2.8.发布关键位姿和帧数据；

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){// 标志位重新置为falsenewLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;// 创建对象 lock 对mtx进行上锁,当lock被析构时,自动解锁std::lock_guard<std::mutex> lock(mtx);// 全局姿态优化的时间判断条件：距离上次优化已经过去0.3s//mappingProcessInterval是0.3秒，以低频进行建图if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {timeLastProcessing = timeLaserOdometry;//把点云坐标均转换到世界坐标系下（scan2map）transformAssociateToMap();// 根据当前和上一次全局姿态优化时的里程计 transformSum transformBefMapped// 以及上一次全局姿态优化的结果 transformAftMapped// 计算当前姿态优化的初始值，赋值给 transformTobeMapped//由于帧数的频率大于建图的频率，因此需要提取关键帧进行匹配extractSurroundingKeyFrames();// 函数功能：根据当前位置，提取局部关键帧集合,以及对应的三个关键帧点云集合// 步骤：//     1. 在关键帧位置集合cloudKeyPoses3D 中//          检索当前位置currentRobotPosPoint 附近的姿态点//          获得局部位置点，赋值给 局部位置点集合surroundingKeyPoses//     2. 根据 局部位置点集合surroundingKeyPoses 更新//          局部关键帧集合 surroundingExistingKeyPosesID//          局部关键帧 角点点云集合surroundingCornerCloudKeyFrames//          局部关键帧 平面点点云集合surroundingSurfCloudKeyFrames//          局部关键帧 离群点点云集合surroundingOutlierCloudKeyFrames//          增加新进入局部的关键帧、并删除离开局部的关键帧。（滑窗）//     3. 为局部点云地图赋值//          laserCloudCornerFromMap     所有局部关键帧的角点集合//          laserCloudSurfFromMap       所有局部关键帧平面点和离群点的集合//降采样匹配以及增加地图点云，回环检测downsampleCurrentScan();// 对来自msg的点云进行降采样// laserCloudCornerLast   //角点点云// laserCloudSurfLast     // 平面点点云// laserCloudOutlierLast  // 离群点点云// laserCloudSurfTotalLast // 平面点+离群点// 当前扫描进行边缘优化，图优化以及进行LM优化的过程,// 根据现有地图与最新点云数据进行配准,从而更新机器人精确位姿与融合建图，// 它分为角点优化、平面点优化、配准与更新等部分。//使用 scan-to-model 的策略优化了 变换 transformTobeMappedscan2MapOptimization();     //优化求位姿// 使用scan-to-model位姿优化，获得当前时间点机器人的位姿 transformTobeMapped// 参考IMU的姿态 对 transformTobeMapped 进行中值滤波,获得最终的机器人位姿// 为 transformBefMapped 赋值为 里程计位姿，即scan-to-model优化前的位姿// 为 transformAftMapped 赋值为 transformTobeMapped 即scan-to-model优化后的位姿//保存轨迹与位姿图,为回环检测做准备saveKeyFramesAndFactor();// 1选定关键帧// 2根据新的关键帧更新因子图// 3经过因子图优化后，更新当前位姿// transformAftMapped// transformTobeMapped// 并用因子图优化后的位姿 创建关键帧位置点和位姿点,添加到集合// cloudKeyPoses3D  关键帧位置点集合// cloudKeyPoses6D  关键帧位姿点集合// 并为 transformLast 赋值// 更新// cornerCloudKeyFrames 关键帧角点点云集合// surfCloudKeyFrames   关键帧平面点点云集合// outlierCloudKeyFrames关键帧离群点点云集合//correctPoses是回环检测成功后将位姿图的数据依次更新//闭环处理矫正、暂不考虑correctPoses();//发送tf变换publishTF();// 主题"/aft_mapped_to_init"发布优化后的位姿信息// msg.header.stamp 取的是 里程计时间信息// msg.pose 存储本坐标系位姿"/camera_init"  经过全局位姿优化的姿态// msg.twist 存储子坐标系位姿"/aft_mapped"  未经过全局优化的姿态,即里程计信息// 发布tf变换// msg.frame_id_ = "/camera_init"// msg.child_frame_id_ = "/aft_mapped";// msg 的姿态变换为 transformAftMapped 即优化后的机器人位姿距离原点的位姿变换publishKeyPosesAndFrames();// 主题 "/key_pose_origin" 发布关键帧位姿点集合// 主题 "/recent_cloud" 发布 局部平面点点云// 主题 "/registered_cloud" 发布 当前帧点云clearCloud();}}}

extractSurroundingKeyFrames()

如果使能了回环检测，则添加过去50个关键帧，如果没有使能回环检测，则添加离当前欧式距离最近的50个关键帧，然后拼接成点云。

void extractSurroundingKeyFrames(){if (cloudKeyPoses3D->points.empty() == true)return;	// loopClosureEnableFlag 这个变量另外只在loopthread这部分中有用到if (loopClosureEnableFlag == true){// recentCornerCloudKeyFrames保存的点云数量太少，则清空后重新塞入新的点云直至数量够if (recentCornerCloudKeyFrames.size() < surroundingKeyframeSearchNum){recentCornerCloudKeyFrames. clear();recentSurfCloudKeyFrames.   clear();recentOutlierCloudKeyFrames.clear();int numPoses = cloudKeyPoses3D->points.size();for (int i = numPoses-1; i >= 0; --i){// cloudKeyPoses3D的intensity中存的是索引值?// 保存的索引值从1开始编号；int thisKeyInd = (int)cloudKeyPoses3D->points[i].intensity;PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];updateTransformPointCloudSinCos(&thisTransformation);// 依据上面得到的变换thisTransformation，对cornerCloudKeyFrames，surfCloudKeyFrames，surfCloudKeyFrames// 进行坐标变换recentCornerCloudKeyFrames. push_front(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));recentSurfCloudKeyFrames.   push_front(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));recentOutlierCloudKeyFrames.push_front(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));if (recentCornerCloudKeyFrames.size() >= surroundingKeyframeSearchNum)break;}}else{// recentCornerCloudKeyFrames中点云保存的数量较多// pop队列最前端的一个，再push后面一个if (latestFrameID != cloudKeyPoses3D->points.size() - 1){recentCornerCloudKeyFrames. pop_front();recentSurfCloudKeyFrames.   pop_front();recentOutlierCloudKeyFrames.pop_front();// 为什么要把recentCornerCloudKeyFrames最前面第一个元素弹出?// (1个而不是好几个或者是全部)?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){// 两个pcl::PointXYZI相加?// 注意这里把recentOutlierCloudKeyFrames也加入到了laserCloudSurfFromMap*laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i];*laserCloudSurfFromMap   += *recentSurfCloudKeyFrames[i];*laserCloudSurfFromMap   += *recentOutlierCloudKeyFrames[i];}}else{// 下面这部分是没有闭环的代码// surroundingKeyPoses->clear();surroundingKeyPosesDS->clear();//cloudKeyPoses3D虽说是点云，但是是为了保存机器人在建图过程中的轨迹，其中的点就是定周期采样的轨迹点，//                                     这一点是在saveKeyFramesAndFactor中计算出的，在第一帧时必然是空的//surroundingKeyframeSearchRadius是50米，也就是说是在当前位置进行半径查找，得到附近的轨迹点//距离数据保存在pointSearchSqDis中kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);// // 进行半径surroundingKeyframeSearchRadius内的邻域搜索，// currentRobotPosPoint：需要查询的点（当前位置，当前帧位姿点）// pointSearchInd：搜索完的邻域点对应的索引// pointSearchSqDis：搜索完的每个领域点点与传讯点之间的欧式距离// 0：返回的邻域个数，为0表示返回全部的邻域点//                                                                                          需要查询的点                                                                           搜索完的邻域点（帧？）对应的索引      搜索完的每个领域点点与传讯点之间的欧式距离kdtreeSurroundingKeyPoses->radiusSearch(currentRobotPosPoint, (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis, 0);for (int i = 0; i < pointSearchInd.size(); ++i)// 取邻域内的关键帧存入surroundingKeyPoses（surroundingKeyPoses属于cloudKeyPoses3D）surroundingKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchInd[i]]);//  对附近轨迹点的点云进行降采样，轨迹具有一定间隔downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);        //  局部关键帧位置集合——目标点附近的位置点集合downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);                   // 上者的降采样结果int numSurroundingPosesDS = surroundingKeyPosesDS->points.size();// surroundingExistingKeyPosesID.size()根据删除动态减小的for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i){bool existingFlag = false;for (int j = 0; j < numSurroundingPosesDS; ++j){// 双重循环，不断对比surroundingExistingKeyPosesID[i]和surroundingKeyPosesDS的点的index// 如果能够找到一样的，说明存在相同的关键点(因为surroundingKeyPosesDS从cloudKeyPoses3D中筛选而来)if (surroundingExistingKeyPosesID[i] == (int)surroundingKeyPosesDS->points[j].intensity){existingFlag = true;break;}}if (existingFlag == false){// 如果surroundingExistingKeyPosesID[i]对比了一轮的已经存在的关键位姿的索引后（intensity保存的就是size()）// 没有找到相同的关键点，那么把这个点从当前队列中删除// 否则的话，existingFlag为true，该关键点就将它留在队列中surroundingExistingKeyPosesID.erase(surroundingExistingKeyPosesID.   begin() + i);surroundingCornerCloudKeyFrames.erase(surroundingCornerCloudKeyFrames. begin() + i);surroundingSurfCloudKeyFrames.erase(surroundingSurfCloudKeyFrames.   begin() + i);surroundingOutlierCloudKeyFrames.erase(surroundingOutlierCloudKeyFrames.begin() + i);--i;}}// 上一个两重for循环主要用于删除数据，此处的两重for循环用来添加数据for (int i = 0; i < numSurroundingPosesDS; ++i) {bool existingFlag = false;for (auto iter = surroundingExistingKeyPosesID.begin(); iter != surroundingExistingKeyPosesID.end(); ++iter){// *iter就是不同的cloudKeyPoses3D->points.size(),？（我觉得是cloudKeyPoses3D->points[i].intensity）// 把surroundingExistingKeyPosesID内没有对应的点放进一个队列里// 这个队列专门存放周围存在的关键帧，但是和surroundingExistingKeyPosesID的点没有对应的，也就是新的点if ((*iter) == (int)surroundingKeyPosesDS->points[i].intensity){existingFlag = true;break;}}if (existingFlag == true){continue;}else{// surroundingExistingKeyPosesID的点没有对应的点，将新点更新进去//这类情况是初次处理时将点云变换到当前坐标系下//cloudKeyPoses6D中的数据来源自thisPose6D，我们可以看到thisPose6D根据isam库进行先验后得到，// 所以在下面容器中存放的都是已经粗配准完毕的点云下标int thisKeyInd = (int)surroundingKeyPosesDS->points[i].intensity;      // 取局部关键帧的IDPointTypePose 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];}}// 进行两次下采样// 最后的输出结果是laserCloudCornerFromMapDS和laserCloudSurfFromMapDSdownSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->points.size();downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->points.size();}

前面的几个函数，转换到map 坐标系，提取关键帧，降采样等 主要的作用就是方便后续的图优化过程，提高图优化的速度。

scan到map优化

scan2MapOptimization()

通过最小二乘法，添加当前的扫描帧到map

void scan2MapOptimization(){// laserCloudCornerFromMapDSNum是extractSurroundingKeyFrames()函数最后降采样得到的coner点云数// laserCloudSurfFromMapDSNum是extractSurroundingKeyFrames()函数降采样得到的surface点云数if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {// laserCloudCornerFromMapDS和laserCloudSurfFromMapDS的来源有2个：// 当有闭环时，来源是：recentCornerCloudKeyFrames，没有闭环时，来源surroundingCornerCloudKeyFrameskdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);for (int iterCount = 0; iterCount < 10; iterCount++) {// 用for循环控制迭代次数，最多迭代10次laserCloudOri->clear();coeffSel->clear();cornerOptimization(iterCount);      //线特征的关联surfOptimization(iterCount);        //面特征的关联// LM优化if (LMOptimization(iterCount) == true)break;              }// 迭代结束更新相关的转移矩阵transformUpdate();}}

cornerOptimization()

角特征化

void cornerOptimization(int iterCount){updatePointAssociateToMapSinCos();for (int i = 0; i < laserCloudCornerLastDSNum; i++) {pointOri = laserCloudCornerLastDS->points[i];           //laserCloudCornerLastDS（来自 odoOptimization 的*下采样*角点特征集）// 进行坐标变换,转换到全局坐标中去（世界坐标系）// pointSel:表示选中的点，point select// 输入是pointOri，输出是pointSelpointAssociateToMap(&pointOri, &pointSel);// 进行5邻域搜索，// pointSel为需要搜索的点，// pointSearchInd搜索完的邻域对应的索引// pointSearchSqDis 邻域点与查询点之间的距离//                                               当前角点点云（对应线特征）// 1. 查找最近的5个点kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); //从周围map里面提取了一些线特征的点（构成了一坨点）// 一坨点共线// 只有当最远的那个邻域点的距离pointSearchSqDis[4]小于1m时才进行下面的计算// 以下部分的计算是在计算点集的协方差矩阵，Zhang Ji的论文中有提到这部分// 2. 上述点距离小于1mif (pointSearchSqDis[4] < 1.0) {// 先求5个样本的平均值float cx = 0, cy = 0, cz = 0;for (int j = 0; j < 5; j++) {cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;     //laserCloudCornerFromMapDS：局部角点点云的降采样  (每轮进行清空)cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;}// 先求该坨点的中心点// 3. 5个点的质心cx /= 5; cy /= 5;  cz /= 5;// 下面在求矩阵matA1=[ax,ay,az]^t*[ax,ay,az]// 更准确地说应该是在求协方差matA1float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;for (int j = 0; j < 5; j++) {// 一坨点（所有点）减去中心点// ax代表的是x-cx,表示均值与每个实际值的差值，求取5个之后再次取平均，得到matA1float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;a11 += ax * ax; a12 += ax * ay; a13 += ax * az;a22 += ay * ay; a23 += ay * az;a33 += az * az;}// 4. 协方差a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;// 求正交阵的特征值和特征向量// 对协方差矩阵的特征向量最直观的解释之一是它总是指向数据方差最大的方向// 特征值：matD1，特征向量：matV1（本质上包含了线的方程）中cv::eigen(matA1, matD1, matV1);     //解矩阵// 边缘：与较大特征值相对应的特征向量代表边缘线的方向（一大两小，大方向）// 以下这一大块是在计算点到边缘的距离，最后通过系数s来判断是否距离很近// 如果距离很近就认为这个点在边缘上，需要放到laserCloudOri中if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) //从线的方程中虚拟出两个点来（并非点云中实际存在的点）{// 这样又有三个点了（可以按照以前求残差的方式）1float x0 = pointSel.x;float y0 = pointSel.y;float z0 = pointSel.z;float x1 = cx + 0.1 * matV1.at<float>(0, 0);    //  中心点往左一定距离      2float y1 = cy + 0.1 * matV1.at<float>(0, 1);float z1 = cz + 0.1 * matV1.at<float>(0, 2);float x2 = cx - 0.1 * matV1.at<float>(0, 0);    //中心点往右一段距离        3float y2 = cy - 0.1 * matV1.at<float>(0, 1);float z2 = cz - 0.1 * matV1.at<float>(0, 2);// 这边是在求[(x0-x1),(y0-y1),(z0-z1)]与[(x0-x2),(y0-y2),(z0-z2)]叉乘得到的向量的模长// 这个模长是由0.2*V1[0]和点[x0,y0,z0]构成的平行四边形的面积// 因为[(x0-x1),(y0-y1),(z0-z1)]x[(x0-x2),(y0-y2),(z0-z2)]=[XXX,YYY,ZZZ],// [XXX,YYY,ZZZ]=[(y0-y1)(z0-z2)-(y0-y2)(z0-z1),-(x0-x1)(z0-z2)+(x0-x2)(z0-z1),(x0-x1)(y0-y2)-(x0-x2)(y0-y1)]float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))* ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))* ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))* ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));// l12表示的是0.2*(||V1[0]||)// 也就是平行四边形一条底的长度float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));// 求叉乘结果[la',lb',lc']=[(x1-x2),(y1-y2),(z1-z2)]x[XXX,YYY,ZZZ]// [la,lb,lc]=[la',lb',lc']/a012/l12// LLL=[la,lb,lc]是0.2*V1[0]这条高上的单位法向量。||LLL||=1；float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;// 计算*点pointSel*到直线的距离// 这里需要特别说明的是ld2代表的是点pointSel到过点[cx,cy,cz]的方向向量直线的距离float ld2 = a012 / l12;// 如果在最理想的状态的话，ld2应该为0，表示点在直线上// 最理想状态s=1；float s = 1 - 0.9 * fabs(ld2);// coeff代表系数的意思// coff用于保存距离的方向向量coeff.x = s * la;coeff.y = s * lb;coeff.z = s * lc;// intensity本质上构成了一个核函数，ld2越接近于1，增长越慢// intensity=(1-0.9*ld2)*ld2=ld2-0.9*ld2*ld2coeff.intensity = s * ld2;// 所以就应该认为这个点是边缘点// s>0.1 也就是要求点到直线的距离ld2要小于1m// s越大说明ld2越小(离边缘线越近)，这样就说明点pointOri在直线上if (s > 0.1) {laserCloudOri->push_back(pointOri);coeffSel->push_back(coeff);}}}}}

surfOptimization()

面特征优化

void surfOptimization(int iterCount){updatePointAssociateToMapSinCos();for (int i = 0; i < laserCloudSurfTotalLastDSNum; i++) {pointOri = laserCloudSurfTotalLastDS->points[i];pointAssociateToMap(&pointOri, &pointSel); kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);if (pointSearchSqDis[4] < 1.0) {for (int j = 0; j < 5; j++) {matA0.at<float>(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;matA0.at<float>(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;matA0.at<float>(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;}// matB0是一个5x1的矩阵// matB0 = cv::Mat (5, 1, CV_32F, cv::Scalar::all(-1));// matX0是3x1的矩阵// 求解方程matA0*matX0=matB0// 公式其实是在求由matA0中的点构成的平面的法向量matX0cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);// [pa,pb,pc,pd]=[matX0,pd]// 正常情况下（见后面planeValid判断条件），应该是// pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +// pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +// pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z = -1// 所以pd设置为1float pa = matX0.at<float>(0, 0);float pb = matX0.at<float>(1, 0);float pc = matX0.at<float>(2, 0);float pd = 1;// 对[pa,pb,pc,pd]进行单位化float ps = sqrt(pa * pa + pb * pb + pc * pc);pa /= ps; pb /= ps; pc /= ps; pd /= ps;// 求解后再次检查平面是否是有效平面bool planeValid = true;for (int j = 0; j < 5; j++) {if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {planeValid = false;break;}}if (planeValid) {float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;// 后面部分相除求的是[pa,pb,pc,pd]与pointSel的夹角余弦值(两个sqrt，其实并不是余弦值)// 这个夹角余弦值越小越好，越小证明所求的[pa,pb,pc,pd]与平面越垂直float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;// 判断是否是合格平面，是就加入laserCloudOriif (s > 0.1) {laserCloudOri->push_back(pointOri);coeffSel->push_back(coeff);}}}}}

LMOptimization()

LM优化过程

// 这部分的代码是基于高斯牛顿法的优化，不是zhang ji论文中提到的基于L-M的优化方法// 这部分的代码使用旋转矩阵对欧拉角求导，优化欧拉角，不是zhang ji论文中提到的使用angle-axis的优化bool LMOptimization(int iterCount){float srx = sin(transformTobeMapped[0]);float crx = cos(transformTobeMapped[0]);float sry = sin(transformTobeMapped[1]);float cry = cos(transformTobeMapped[1]);float srz = sin(transformTobeMapped[2]);float crz = cos(transformTobeMapped[2]);int laserCloudSelNum = laserCloudOri->points.size();// laser cloud original 点云太少，就跳过这次循环if (laserCloudSelNum < 50) {return false;}cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));for (int i = 0; i < laserCloudSelNum; i++) {pointOri = laserCloudOri->points[i];coeff = coeffSel->points[i];// 求雅克比矩阵中的元素，距离d对roll角度的偏导量即d(d)/d(roll)// 更详细的数学推导参看wykxwyc.github.iofloat arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;// 同上，求解的是对pitch的偏导量float ary = ((cry*srx*srz - crz*sry)*pointOri.x + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x+ ((-cry*crz - srx*sry*srz)*pointOri.x + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;/*在求点到直线的距离时，coeff表示的是如下内容[la,lb,lc]表示的是点到直线的垂直连线方向，s是长度coeff.x = s * la;coeff.y = s * lb;coeff.z = s * lc;coeff.intensity = s * ld2;在求点到平面的距离时，coeff表示的是[pa,pb,pc]表示过外点的平面的法向量，s是线的长度coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;*///  scan2map里面用的是6自由度matA.at<float>(i, 0) = arx;matA.at<float>(i, 1) = ary;matA.at<float>(i, 2) = arz;// 这部分是雅克比矩阵中距离对平移的偏导matA.at<float>(i, 3) = coeff.x;matA.at<float>(i, 4) = coeff.y;matA.at<float>(i, 5) = coeff.z;// 残差项matB.at<float>(i, 0) = -coeff.intensity;}// 将矩阵由matA转置生成matAt// 先进行计算，以便于后边调用 cv::solve求解cv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;// 利用高斯牛顿法进行求解，// 高斯牛顿法的原型是J^(T)*J * delta(x) = -J*f(x)// J是雅克比矩阵，这里是A，f(x)是优化目标，这里是-B(符号在给B赋值时候就放进去了)// 通过QR分解的方式，求解matAtA*matX=matAtB，得到解matXcv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);// iterCount==0 说明是第一次迭代，需要初始化if (iterCount == 0) {cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));// 对近似的Hessian矩阵求特征值和特征向量，cv::eigen(matAtA, matE, matV);matV.copyTo(matV2);isDegenerate = false;float eignThre[6] = {100, 100, 100, 100, 100, 100};for (int i = 5; i >= 0; i--) {if (matE.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}matP = matV.inv() * matV2;}if (isDegenerate) {cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}transformTobeMapped[0] += matX.at<float>(0, 0);transformTobeMapped[1] += matX.at<float>(1, 0);transformTobeMapped[2] += matX.at<float>(2, 0);transformTobeMapped[3] += matX.at<float>(3, 0);transformTobeMapped[4] += matX.at<float>(4, 0);transformTobeMapped[5] += matX.at<float>(5, 0);float deltaR = sqrt(pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(5, 0) * 100, 2));// 旋转或者平移量足够小就停止这次迭代过程if (deltaR < 0.05 && deltaT < 0.05) {return true;}return false;}

saveKeyFramesAndFactor()

保存关键帧和因子
通过前后2帧进行优化，保存优化后的位姿
saveKeyFramesAndFactor() 这个函数的主要作用就是选取关键帧。如果当前帧与上一帧之间的欧氏距离 大于 0.3 米 则认为是一个新的关键帧。此时需要计算出当前帧与与上一帧之间的约束情况。 这种约束可以认为是小回环，加入到后端中去优化。将优化后的结果作为关键帧的位姿与点云，然后同步到 scan -> map 过程中。

// scan2map以后要判断，这一帧是不是关键帧（用于gtsam图优化）void saveKeyFramesAndFactor(){// 上一次全局优化（经因子图修正）后的结果（位移量）currentRobotPosPoint.x = transformAftMapped[3];currentRobotPosPoint.y = transformAftMapped[4];currentRobotPosPoint.z = transformAftMapped[5];bool saveThisKeyFrame = true;// 满足相应距离条件，才存为关键帧（即此处： 不满足距离条件不存为关键帧）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){saveThisKeyFrame = false;}if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())return;previousRobotPosPoint = currentRobotPosPoint;// --------------------------------------------------------在保存关键帧因子中，先是添加固定先验因子--------------------------------------------if (cloudKeyPoses3D->points.empty()){// static Rot3 	RzRyRx (double x, double y, double z),Rotations around Z, Y, then X axes// RzRyRx依次按照z(transformTobeMapped[2])，y(transformTobeMapped[0])，x(transformTobeMapped[1])坐标轴旋转// Point3 (double x, double y, double z)  Construct from x(transformTobeMapped[5]), y(transformTobeMapped[3]), and z(transformTobeMapped[4]) coordinates. // Pose3 (const Rot3 &R, const Point3 &t) Construct from R,t. 从旋转和平移构造姿态// NonlinearFactorGraph增加一个PriorFactor因子(加入先验因子PriorFactor就是固定这个顶点)gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));// initialEstimate的数据类型是Values,其实就是一个map，这里在0对应的值下面保存了一个Pose3initialEstimate.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    //scan2map匹配完以后，要判断这一帧是不是关键帧，是关键帧就把他加入优化，构建一个相邻帧之间的边（这样才能和回环一起构成一个图结构去优化）{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]));// 构造函数原型:BetweenFactor (Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model)gtSAMgraph.add(BetweenFactor<Pose3>(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])));}// -------------------------------------------------------------------------------------------------------------------------------------------------------------// -----------------------------------------------------------更新isan和计算位姿-------------------------------------------------------------------// gtsam::ISAM2::update函数原型:// ISAM2Result gtsam::ISAM2::update	(	const NonlinearFactorGraph & 	newFactors = NonlinearFactorGraph(),// const Values & 	newTheta = Values(),// const std::vector< size_t > & 	removeFactorIndices = std::vector<size_t>(),// const boost::optional< FastMap< Key, int > > & 	constrainedKeys = boost::none,// const boost::optional< FastList< Key > > & 	noRelinKeys = boost::none,// const boost::optional< FastList< Key > > & 	extraReelimKeys = boost::none,// bool 	force_relinearize = false )	// gtSAMgraph是新加到系统中的因子// initialEstimate是加到系统中的新变量的初始点isam->update(gtSAMgraph, initialEstimate);// update 函数为什么需要调用两次？isam->update();// 删除内容?gtSAMgraph.resize(0);initialEstimate.clear();PointType thisPose3D;PointTypePose thisPose6D;Pose3 latestEstimate;// Compute an estimate from the incomplete linear delta computed during the last update.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();      //这里当真存的是当前关键帧的数量？（索引？）cloudKeyPoses3D->push_back(thisPose3D);thisPose6D.x = thisPose3D.x;thisPose6D.y = thisPose3D.y;thisPose6D.z = thisPose3D.z;thisPose6D.intensity = thisPose3D.intensity;thisPose6D.roll  = latestEstimate.rotation().pitch();thisPose6D.pitch = latestEstimate.rotation().yaw();thisPose6D.yaw   = latestEstimate.rotation().roll();thisPose6D.time = timeLaserOdometry;cloudKeyPoses6D->push_back(thisPose6D);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(const pcl::PCLPointCloud2 &cloud_in,pcl::PCLPointCloud2 &cloud_out )   pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame);cornerCloudKeyFrames.push_back(thisCornerKeyFrame);surfCloudKeyFrames.push_back(thisSurfKeyFrame);outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);}

correctPoses()

校正位姿
回环检测如果成功，会设置 aLoopIsClosed 为 true，才会进行这一步。将 isam 优化后的位姿替换之前的位姿。

// 位姿修正// 对位姿做优化（LOAM中没有），加了回环以后历史帧位姿发生了变化，就要去对位姿做更正（优化完以后，gtsam吐出来的位姿把历史帧的位姿一个一个附回去）void correctPoses(){if (aLoopIsClosed == true){recentCornerCloudKeyFrames. clear();recentSurfCloudKeyFrames.   clear();recentOutlierCloudKeyFrames.clear();// ---------------------------------------------------------------访问最终矫正后的位姿-------------------------------------------------int numPoses = isamCurrentEstimate.size();for (int i = 0; i < numPoses; ++i){cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().y();cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().z();cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().x();cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;// cloudKeyPoses6D->points[i].roll  = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();cloudKeyPoses6D->points[i].yaw   = isamCurrentEstimate.at<Pose3>(i).rotation().roll();}// ---------------------------------------------------------------------------------------------------------------------------------------------aLoopIsClosed = false;}}

回环检测

回环检测可以消除漂移(drift)，通过ICP算法对比当前帧和之前帧是否匹配，如果匹配则进行图优化。

回环检测在loopClosureThread中进行

// 主要功能进行闭环检测和闭环修正void loopClosureThread(){if (loopClosureEnableFlag == false)return;ros::Rate rate(1);while (ros::ok()){rate.sleep();performLoopClosure();}}

下面在看回环检测的过程。判断是否进入回环在detectLoopClosure中进行，判断条件是首尾之间的距离小于7米，并且时间相差30s以上。

// 闭环检测bool detectLoopClosure(){latestSurfKeyFrameCloud->clear();nearHistorySurfKeyFrameCloud->clear();nearHistorySurfKeyFrameCloudDS->clear();// 资源分配时初始化// 在互斥量被析构前不解锁std::lock_guard<std::mutex> lock(mtx);std::vector<int> pointSearchIndLoop;std::vector<float> pointSearchSqDisLoop;kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);// 进行半径historyKeyframeSearchRadius内的邻域搜索，// currentRobotPosPoint：需要查询的点，// pointSearchIndLoop：搜索完的邻域点对应的索引// pointSearchSqDisLoop：搜索完的每个邻域点与当前点之间的欧式距离// 0：返回的邻域个数，为0表示返回全部的邻域点kdtreeHistoryKeyPoses->radiusSearch(currentRobotPosPoint, historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);closestHistoryFrameID = -1;for (int i = 0; i < pointSearchIndLoop.size(); ++i){int id = pointSearchIndLoop[i];// 两个（找到的帧与当前帧）时间差值大于30秒（不大于30s的话，当前帧的闭环帧就是上一帧）if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0){closestHistoryFrameID = id;     //最近关键帧的idbreak;}}if (closestHistoryFrameID == -1){// 找到的点和当前时间上没有超过30秒的return false;}// // save latest key frameslatestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1;// 点云的xyz坐标进行坐标系变换(分别绕xyz轴旋转)// 在历史帧（相当于一个点云）里面找到关键帧，将这些关键帧拼接在一起*latestSurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);*latestSurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure],   &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);// latestSurfKeyFrameCloud中存储的是下面公式计算后的index(intensity):// thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;// 滤掉latestSurfKeyFrameCloud中index<0的点??? index值会小于0?pcl::PointCloud<PointType>::Ptr hahaCloud(new pcl::PointCloud<PointType>());int cloudSize = latestSurfKeyFrameCloud->points.size();for (int i = 0; i < cloudSize; ++i){// intensity不小于0的点放进hahaCloud队列// 初始化时intensity是-1，if ((int)latestSurfKeyFrameCloud->points[i].intensity >= 0){hahaCloud->push_back(latestSurfKeyFrameCloud->points[i]);}}latestSurfKeyFrameCloud->clear();       //滤掉那些点*latestSurfKeyFrameCloud   = *hahaCloud;// historyKeyframeSearchNum在utility.h中定义为25，前后25个点进行变换for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j){if (closestHistoryFrameID + j < 0 || closestHistoryFrameID + j > latestFrameIDLoopCloure)continue;// 要求closestHistoryFrameID + j在0到cloudKeyPoses3D->points.size()-1之间,不能超过索引*nearHistorySurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]);*nearHistorySurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[closestHistoryFrameID+j],   &cloudKeyPoses6D->points[closestHistoryFrameID+j]);}// 下采样滤波减少数据量downSizeFilterHistoryKeyFrames.setInputCloud(nearHistorySurfKeyFrameCloud);downSizeFilterHistoryKeyFrames.filter(*nearHistorySurfKeyFrameCloudDS);if (pubHistoryKeyFrames.getNumSubscribers() != 0){sensor_msgs::PointCloud2 cloudMsgTemp;pcl::toROSMsg(*nearHistorySurfKeyFrameCloudDS, cloudMsgTemp);cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);cloudMsgTemp.header.frame_id = "/camera_init";pubHistoryKeyFrames.publish(cloudMsgTemp);}return true;}

如果检测到回环之后，接着进行ICP匹配，然后进行图优化。作者同时提醒回环检测的ICP算法当里程计漂移太大时经常失败。对于更高级的闭环方法，建议采样 SC-LeGO-LOAM ，它的特征采用的是点云描述符。

/*函数流程：1、先进行闭环检测detectLoopClosure()，如果返回true,则可以进行闭环，否则直接返回，程序结束。2、接着使用icp迭代进行对齐。3、对齐之后判断迭代是否收敛以及噪声是否太大，是则返回并直接结束函数。否则进行迭代后的数据发布处理。4、接下来得到latestSurfKeyFrameCloud和nearHistorySurfKeyFrameCloudDS之间的位置平移和旋转。5、然后进行图优化过程。*/void performLoopClosure(){if (cloudKeyPoses3D->points.empty() == true)return;if (potentialLoopFlag == false){if (detectLoopClosure() == true){potentialLoopFlag = true;timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry;}if (potentialLoopFlag == false)return;}potentialLoopFlag = false;// 调用pcl库的icp（pcl_icp）(用icp得到当前帧7 和目标帧1 之间的相对位姿)pcl::IterativeClosestPoint<PointType, PointType> icp;icp.setMaxCorrespondenceDistance(100);icp.setMaximumIterations(100);icp.setTransformationEpsilon(1e-6);icp.setEuclideanFitnessEpsilon(1e-6);// 设置RANSAC运行次数icp.setRANSACIterations(0);icp.setInputSource(latestSurfKeyFrameCloud);// 使用detectLoopClosure()函数中下采样刚刚更新nearHistorySurfKeyFrameCloudDSicp.setInputTarget(nearHistorySurfKeyFrameCloudDS);pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());// 进行icp点云对齐icp.align(*unused_result);      //unused_result是source乘icp求得的变换矩阵后的点云// 为什么匹配分数高直接返回???分数高代表噪声太多if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)return;// publish corrected cloud// 以下在点云icp收敛并且噪声量在一定范围内进行if (pubIcpKeyFrames.getNumSubscribers() != 0){pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());// icp.getFinalTransformation()的返回值是Eigen::Matrix<Scalar, 4, 4>//                                                                  输入                                            输出                                    刚体变换矩阵信息pcl::transformPointCloud (*latestSurfKeyFrameCloud, *closed_cloud, icp.getFinalTransformation());sensor_msgs::PointCloud2 cloudMsgTemp;pcl::toROSMsg(*closed_cloud, cloudMsgTemp);cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);cloudMsgTemp.header.frame_id = "/camera_init";pubIcpKeyFrames.publish(cloudMsgTemp);}   // 接下来得到latestSurfKeyFrameCloud和nearHistorySurfKeyFrameCloudDS之间的位置平移和旋转。float x, y, z, roll, pitch, yaw;Eigen::Affine3f correctionCameraFrame;correctionCameraFrame = icp.getFinalTransformation();   //相对位姿是准的（此时有一个7'-准的 ）// 得到平移和旋转的角度pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch);Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(cloudKeyPoses6D->points[latestFrameIDLoopCloure]);Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// -----------------------------------在回环检测线程中，当找到闭环，就添加回环因子（优化7'（准）的和7（有累计误差的）之间的残差）---------------------------------------------// 设置图优化参数pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);    //pcl::getTranslationAndEulerAngles的功能是根据仿射矩阵计算x,y,z,roll,pitch,yawgtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));     //current posegtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]);     //last posegtsam::Vector Vector6(6);float noiseScore = icp.getFitnessScore();Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;constraintNoise = noiseModel::Diagonal::Variances(Vector6);// 添加回环因子，然后进行图优化过程。std::lock_guard<std::mutex> lock(mtx);gtSAMgraph.add(BetweenFactor<Pose3>(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));isam->update(gtSAMgraph);isam->update();gtSAMgraph.resize(0);// ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------aLoopIsClosed = true;}