lego-loam mapOptmization 源码注释（二）

看过了main函数，我们来看mapOptmization的正题：

MO.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)、
表面点云数据 (newLaserCloudSurfLast)、离群点云数据 (newLaserCloudOutlierLast) 
和里程计数据 (newLaserOdometry)。同时，还会检查这些数据的时间戳与最新的里程计数据的
时间戳之间的差异是否小于0.005秒。这是为了确保所有数据都是在相近的时间点采集的，
从而可以认为是同步的。{newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;
//如果所有数据都已就绪，会将所有新数据的标志位重置为 false，表示这些数据已经被处理。std::lock_guard<std::mutex> lock(mtx);
//使用 std::lock_guard 和 std::mutex 来保护共享数据，确保线程安全。
//这是为了防止多个线程同时访问和修改共享数据，可能会导致数据不一致。if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {//extern const double mappingProcessInterval = 0.3;//检查自上次处理以来是否已经过了设定的时间间隔 (mappingProcessInterval)。//这是为了避免过于频繁的处理，从而节省计算资源。timeLastProcessing = timeLaserOdometry;//更新时间，为下一次判断做准备transformAssociateToMap();extractSurroundingKeyFrames();downsampleCurrentScan();scan2MapOptimization();saveKeyFramesAndFactor();correctPoses();publishTF();publishKeyPosesAndFrames();clearCloud();}}}

下面我们来看第一个函数，transformAssociateToMap();

transformAssociateToMap();

void transformAssociateToMap(){float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);float y1 = transformBefMapped[4] - transformSum[4];float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);float x2 = x1;float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;transformIncre[5] = z2;float sbcx = sin(transformSum[0]);float cbcx = cos(transformSum[0]);float sbcy = sin(transformSum[1]);float cbcy = cos(transformSum[1]);float sbcz = sin(transformSum[2]);float cbcz = cos(transformSum[2]);float sblx = sin(transformBefMapped[0]);float cblx = cos(transformBefMapped[0]);float sbly = sin(transformBefMapped[1]);float cbly = cos(transformBefMapped[1]);float sblz = sin(transformBefMapped[2]);float cblz = cos(transformBefMapped[2]);float salx = sin(transformAftMapped[0]);float calx = cos(transformAftMapped[0]);float saly = sin(transformAftMapped[1]);float caly = cos(transformAftMapped[1]);float salz = sin(transformAftMapped[2]);float calz = cos(transformAftMapped[2]);float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz)- cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)- cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx);transformTobeMapped[0] = -asin(srx);float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)- cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx)- cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz)+ (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly)+ cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz)+ (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly);float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)- cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx)+ cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)+ (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly)- cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly)+ (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly);transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]), crycrx / cos(transformTobeMapped[0]));float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)- (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)+ cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)- (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)+ cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]), crzcrx / cos(transformTobeMapped[0]));x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4];z1 = transformIncre[5];x2 = x1;y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;transformTobeMapped[3] = transformAftMapped[3] - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);transformTobeMapped[4] = transformAftMapped[4] - y2;transformTobeMapped[5] = transformAftMapped[5] - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);}

我们要尝试理解transformAssociateToMap();就要先理解：transformSum，transformBefMapped，transformAftMapped，transformTobeMapped分别是什么？

transformSum是当前帧雷达里程计的结果，transformBefMapped是上一帧雷达里程计的结果。

这在transformUpdate()函数中有所体现。transformAftMapped是上一帧优化后的位姿，transformTobeMapped是当前帧优化后的位姿。

第一次运行时，除transformSum，其他均初始化为0。

		for (int i = 0; i < 6; i++) {transformBefMapped[i] = transformSum[i];transformAftMapped[i] = transformTobeMapped[i];}

所以要得到当前帧优化后的位姿，就要用上一帧优化后的位姿乘上一帧雷达里程计结果的逆，再乘当前帧雷达里程计的结果。公式如下：

参考自LOAM中的坐标变化公式推导_loam公式推导-CSDN博客

从xyz1到transformIncre的过程就对应当前帧雷达里程计结果的逆乘（上一帧雷达里程计的平移量-当前帧雷达里程计的平移量）。公式如下：

我的理解就是，当前的平移量差值△r，是在上一次位姿优化后，由于位姿变换产生的，我们先把它变换回上一次的位姿，也就是左乘当前帧雷达里程计结果的逆，然后再乘transformTobeMapped，变换到当前帧优化后的位姿上。

        transformSum[0] = -pitch;transformSum[1] = -yaw;transformSum[2] = roll;
//那为什么公式中没有求逆的过程呢？对于旋转矩阵来说，yaw-pitch-roll的逆就是按roll-pitch-yaw顺序旋转。

transformTobeMapped 的公式推导就是矩阵乘法的展开式，类似于PluginIMURotation的公式推导，用恒等关系解决，具体推导见：LOAM中的坐标变化公式推导_loam公式推导-CSDN博客

extractSurroundingKeyFrames();

void extractSurroundingKeyFrames(){if (cloudKeyPoses3D->points.empty() == true)return;	}

only use recent key poses for graph building只使用临近帧的位姿去建图。

初始的cloudKeyPoses3D就是空的，它的值会在saveKeyFramesAndFactor()函数中更新，所以可以把cloudKeyPoses3D简单理解为上一帧的点云。

！！但是其实cloudKeyPoses3D不是点云，它只是一个上一帧机器人的位置，但是这个位置可以代替一帧点云，这样运行时就大大减少了内存使用！！

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);

当 extractSurroundingKeyFrames() 中执行 return 后，run() 函数中的代码会继续执行，并不会因为 extractSurroundingKeyFrames() 返回就中断 run() 的执行。具体来说，extractSurroundingKeyFrames() 函数中如果 cloudKeyPoses3D->points.empty() 为 true，会直接返回，跳过当前函数的其他内容。但因为这是 extractSurroundingKeyFrames() 内部的 return，仅仅是提前结束了该函数的执行，不会影响 run() 中其后的代码执行。所以在 extractSurroundingKeyFrames() 执行 return 后，run() 函数会继续执行 downsampleCurrentScan()、scan2MapOptimization() 等后续函数。

所以我们按cloudKeyPoses3D就是空的，先看return之后的代码，也就是下一个函数downsampleCurrentScan()；。

当cloudKeyPoses3D->points.empty() == false，我们来看下面的代码：

如果需要回环检测：loopClosureEnableFlag == true

void extractSurroundingKeyFrames(){if (cloudKeyPoses3D->points.empty() == true)return;	if (loopClosureEnableFlag == true){
//extern const bool loopClosureEnableFlag = false;
//默认值是false,也就是默认不进行回环检测。// only use recent key poses for graph buildingif (recentCornerCloudKeyFrames.size() < surroundingKeyframeSearchNum){ 
// queue is not full (the beginning of mapping or a loop is just closed)
//extern const int surroundingKeyframeSearchNum = 50; 
// submap size (when loop closure enabled)// clear recent key frames queuerecentCornerCloudKeyFrames. 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 maprecentCornerCloudKeyFrames. push_front(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));recentSurfCloudKeyFrames.   push_front(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));recentOutlierCloudKeyFrames.push_front(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));if (recentCornerCloudKeyFrames.size() >= surroundingKeyframeSearchNum)
//显然当我们刚开始建图的时候，附近的关键帧是不够的，需要积累一段时间后才会break。
//积累达到的条件就是子地图中有50个关键帧。break;}}

如果我们的关键帧满了，pop the oldest key frame and push the latest key frame。

但是如果机器人没有移动，没有新的关键帧产生，这时就不需要pop the oldest key frame and push the latest key frame。也就是latestFrameID == cloudKeyPoses3D->points.size() - 1的情况。

else{  // queue is full, pop the oldest key frame and push the latest key frameif (latestFrameID != cloudKeyPoses3D->points.size() - 1){  // if the robot is not moving, no need to update recent framesrecentCornerCloudKeyFrames. pop_front();recentSurfCloudKeyFrames.   pop_front();recentOutlierCloudKeyFrames.pop_front();// push latest scan to the end of queuelatestFrameID = 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];//将周围关键帧中的点添加到子地图中。}}

如果不需要回环检测：loopClosureEnableFlag == false

这种情况是代码中设置的默认情况：也就是loam的做法

else{surroundingKeyPoses->clear();surroundingKeyPosesDS->clear();
//将 surroundingKeyPoses 和 surroundingKeyPosesDS 清空，为提取新的周围关键帧做好准备。// extract all the nearby key poses and downsample themkdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
//用之前的机器人位置来建树kdtreeSurroundingKeyPoses->radiusSearch(currentRobotPosPoint, (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis, 0);
//extern const float surroundingKeyframeSearchRadius = 50.0; 距离是50
// key frame that is within n meters from current pose will be considerd for scan-to-map optimization (when loop closure disabled)for (int i = 0; i < pointSearchInd.size(); ++i)surroundingKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchInd[i]]);
//将找到的点添加到 surroundingKeyPoses 中。对提取到的 surroundingKeyPoses 进行降采样，得到 surroundingKeyPosesDS。降采样后可以减少计算量。downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);// delete key frames that are not in surrounding regionint 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 framesfor (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];}}

这里增加和删减关键帧的代码很有意思，我们来看一下：

先看增加的：

// add new key frames that are not in calculated existing key framesfor (int i = 0; i < numSurroundingPosesDS; ++i) {bool existingFlag = false;//首先将existingFlag都初始化为false。for (auto iter = surroundingExistingKeyPosesID.begin(); iter != surroundingExistingKeyPosesID.end(); ++iter){if ((*iter) == (int)surroundingKeyPosesDS->points[i].intensity){existingFlag = true;break;}}
//这一段代码是为了防止重复添加，如果关键帧已经在surroundingExistingKeyPosesID中，那么就break。if (existingFlag == true){continue;
//如果这一帧已经存在了，再去循环下一帧。}else{
//如果都不存在，那就添加进来，比如一开始的时候，surroundingExistingKeyPosesID没有值，肯定要添加！！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];
//离群点也加到面点中去了。}}

再看删除的：

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;}}

最后需要回环的和不需要回环的，得到的结果统一进行降采样，得到

laserCloudCornerFromMapDS和laserCloudSurfFromMapDS。

// 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();

downsampleCurrentScan()；

void downsampleCurrentScan(){//downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);
//downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);
//downSizeFilterOutlier.setLeafSize(0.4, 0.4, 0.4);laserCloudCornerLastDS->clear();downSizeFilterCorner.setInputCloud(laserCloudCornerLast);downSizeFilterCorner.filter(*laserCloudCornerLastDS);laserCloudCornerLastDSNum = laserCloudCornerLastDS->points.size();laserCloudSurfLastDS->clear();downSizeFilterSurf.setInputCloud(laserCloudSurfLast);downSizeFilterSurf.filter(*laserCloudSurfLastDS);laserCloudSurfLastDSNum = laserCloudSurfLastDS->points.size();laserCloudOutlierLastDS->clear();downSizeFilterOutlier.setInputCloud(laserCloudOutlierLast);downSizeFilterOutlier.filter(*laserCloudOutlierLastDS);laserCloudOutlierLastDSNum = laserCloudOutlierLastDS->points.size();laserCloudSurfTotalLast->clear();laserCloudSurfTotalLastDS->clear();*laserCloudSurfTotalLast += *laserCloudSurfLastDS;*laserCloudSurfTotalLast += *laserCloudOutlierLastDS;downSizeFilterSurf.setInputCloud(laserCloudSurfTotalLast);downSizeFilterSurf.filter(*laserCloudSurfTotalLastDS);laserCloudSurfTotalLastDSNum = laserCloudSurfTotalLastDS->points.size();}

对角点和面点按照不同大小的格子采样，格子越小采样点越多，越细致，所以角点要更密一些。

scan2MapOptimization();

 void scan2MapOptimization(){if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);//为角点建立kd树kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);//为面点建立kd树for (int iterCount = 0; iterCount < 10; iterCount++) {laserCloudOri->clear();coeffSel->clear();cornerOptimization(iterCount);surfOptimization(iterCount);if (LMOptimization(iterCount) == true)break;              }transformUpdate();}}

似曾相识的操作，和FeatureAssociation中的updateTransformation()函数很像。一开始laserCloudCornerFromMapDSNum == 0 ，因为laserCloudCornerFromMapDSNum的值是在extractSurroundingKeyFrames();中更新的！！所以我们先看下一个函数saveKeyFramesAndFactor()；。

经过extractSurroundingKeyFrames();，现在laserCloudCornerFromMapDSNum ！=0，我们接着看scan2MapOptimization();。

cornerOptimization(iterCount);

updatePointAssociateToMapSinCos();

void updatePointAssociateToMapSinCos(){cRoll = cos(transformTobeMapped[0]);sRoll = sin(transformTobeMapped[0]);cPitch = cos(transformTobeMapped[1]);sPitch = sin(transformTobeMapped[1]);cYaw = cos(transformTobeMapped[2]);sYaw = sin(transformTobeMapped[2]);tX = transformTobeMapped[3];tY = transformTobeMapped[4];tZ = transformTobeMapped[5];}

pointAssociateToMap(&pointOri, &pointSel);

 void pointAssociateToMap(PointType const * const pi, PointType * const po){float x1 = cYaw * pi->x - sYaw * pi->y;float y1 = sYaw * pi->x + cYaw * pi->y;float z1 = pi->z;float x2 = x1;float y2 = cRoll * y1 - sRoll * z1;float z2 = sRoll * y1 + cRoll * z1;po->x = cPitch * x2 + sPitch * z2 + tX;po->y = y2 + tY;po->z = -sPitch * x2 + cPitch * z2 + tZ;po->intensity = pi->intensity;}//将点云转换到当前优化之后的位姿！！

 void cornerOptimization(int iterCount){updatePointAssociateToMapSinCos();for (int i = 0; i < laserCloudCornerLastDSNum; i++) {pointOri = laserCloudCornerLastDS->points[i];//一定要区分开laserCloudCornerLastDS和laserCloudCornerFromMapDSNum！！
//laserCloudCornerLastDS 和 laserCloudCornerFromMapDSNum 是分别处理“当前帧”和“地图”中角点云数据的两个变量
//laserCloudCornerLastDS 表示当前激光帧中降采样后的角点云。//这是在当前帧的点云中选取的角点特征，通过降采样 (DownSample) 后存入 laserCloudCornerLastDS，以减少计算量和数据冗余。
//该点云用于与地图数据匹配的优化过程。//所以是用地图中的角点建树，用当前帧的角度查询！！！pointAssociateToMap(&pointOri, &pointSel);kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);//找到当前点相邻的五个点if (pointSearchSqDis[4] < 1.0) {//如果距离最大的那个点的距离都小于1，那么证明五个相邻点均满足要求。float cx = 0, cy = 0, cz = 0;for (int j = 0; j < 5; j++) {cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;}cx /= 5; cy /= 5;  cz /= 5;//cx，cy,cz是中心坐标。float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;for (int j = 0; j < 5; j++) {float 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;}a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;//方差协方差的平均值//matA1是五个点到中心点距离的协方差矩阵的均值矩阵，也就是五个协方差矩阵的平均。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;cv::eigen(matA1, matD1, matV1);
//cv::eigen(matA1, matD1, matV1); 是 OpenCV 提供的一个函数，用于计算矩阵的特征值和特征向量。
//具体来说：
//    matA1 是输入矩阵，通常是一个方阵。
//    matD1 是输出矩阵，用于存储 matA1 的特征值。
//    matV1 是输出矩阵，用于存储 matA1 的特征向量。
//在执行后，matD1 中包含的是 matA1 的特征值，而 matV1 的列向量则是对应的特征向量。if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {float x0 = pointSel.x;float y0 = pointSel.y;float z0 = pointSel.z;float x1 = cx + 0.1 * matV1.at<float>(0, 0);float 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);float y2 = cy - 0.1 * matV1.at<float>(0, 1);float z2 = cz - 0.1 * matV1.at<float>(0, 2);
//这段代码的作用是基于特征向量 matV1 在三维空间中计算两个点 (x1, y1, z1) 和 (x2, y2, z2) 的位置。
//这两个点位于以 (cx, cy, cz) 为中心、沿特征向量方向的一小段距离（0.1 倍特征向量长度）上。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)));float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));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;float ld2 = a012 / l12;float s = 1 - 0.9 * fabs(ld2);
//以上代码在FeatureAssociation中的updateTransformation()函数注释过，可以去之前的博客查看，
//具体代码作用就是找到当前帧角点到“地图”上的线特征的垂线方向和距离。coeff.x = s * la;coeff.y = s * lb;coeff.z = s * lc;coeff.intensity = s * ld2;if (s > 0.1) {laserCloudOri->push_back(pointOri);coeffSel->push_back(coeff);}}}}}

if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1))，matD1.at<float>(0, 0) 表示第一个特征值，matD1.at<float>(0, 1) 表示第二个特征值，确认数据集中一个方向的变化比其他方向大得多，这里是判断5个相邻点是不是在一条直线上。

surfOptimization(iterCount);

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;}cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);float pa = matX0.at<float>(0, 0);float pb = matX0.at<float>(1, 0);float pc = matX0.at<float>(2, 0);float pd = 1;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;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;if (s > 0.1) {laserCloudOri->push_back(pointOri);coeffSel->push_back(coeff);}}}}}

这段用特征值判断是面特征的代码很有意思，拿出来看一下：

 cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);float pa = matX0.at<float>(0, 0);float pb = matX0.at<float>(1, 0);float pc = matX0.at<float>(2, 0);float pd = 1;float ps = sqrt(pa * pa + pb * pb + pc * pc);pa /= ps; pb /= ps; pc /= ps; pd /= ps;//将pa,pb,pc，pd归一化。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;}}

cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);

matA0 = cv::Mat (5, 3, CV_32F, cv::Scalar::all(0));
matB0 = cv::Mat (5, 1, CV_32F, cv::Scalar::all(-1));
matX0 = cv::Mat (3, 1, CV_32F, cv::Scalar::all(0));

使用 cv::solve 函数解线性方程组，得到平面方程的法向量（平面参数）结果 matX0。该平面方程满足 ax + by + cz + d = 0。

对拟合平面的 5 个点逐个进行检查。
距离计算：对于每个点，计算它到平面的距离，即代入平面方程 ax + by + cz + d = 0 后的结果。这个值的绝对值表示该点到平面的“偏离程度”。
判断条件：如果任意一个点的偏离值大于 0.2（即距离平面超过 0.2 米），则认为该平面不可靠，将 planeValid 设为 false 并跳出循环。

LMOptimization(iterCount);

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();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];float 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;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;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;}cv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);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));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;}

这段代码实现了一个基于 LM（Levenberg-Marquardt）优化的位姿调整函数 LMOptimization，其中每次迭代会根据点云的配准误差，调整并优化位姿参数 transformTobeMapped。

matA 存储每个点的雅可比矩阵，用于表达平移和旋转对误差的影响。
matB 存储误差值，表示点到平面/线的距离误差。

配准的优化目标是最小化点 pointOri 到目标平面的距离，因此误差函数可以定义为：
error=n⃗⋅(R(transformTobeMapped)⋅p⃗Ori+t⃗)−d
其中：
n⃗为平面法向量，即 coeff 的 x, y, z 分量。
p⃗Ori为点 pointOri。
R 和 t 为旋转和平移量。
d 是平面到原点的距离。
优化时，误差对各位姿参数的偏导数就构成了雅可比矩阵的各个元素。

求解线性方程 matAtA * matX = matAtB 得到 matX，即位姿更新值。

公式的具体推导如下：

1. 建立误差方程在位姿优化中，对于每个观测点，定义误差 eiei：
ei=f(pi,X)−qi
ei=f(pi,X)−qi其中：pipi 是观测的点。XX 是待优化的位姿（旋转和平移）参数。f(pi,X)f(pi,X) 是在位姿参数 XX 下点 pipi 在地图中的预测位置。qiqi 是点 pipi 应该满足的目标位置（或平面约束）。2. 最小化平方误差为了找到最优的 XX，最小化所有观测点的误差平方和，即目标函数：
E(X)=∑iei2
E(X)=i∑ei2将上式展开，变成优化问题。
3. 线性化误差方程LM 优化是一种非线性优化方法，它首先将误差方程线性化，以便能够用线性方程来近似求解更新量。假设当前位姿估计为 XX，那么误差可以用泰勒展开近似表示：
ei(X+ΔX)≈ei(X)+∂ei∂XΔX
ei(X+ΔX)≈ei(X)+∂X∂eiΔX定义雅可比矩阵 Ji=∂ei∂XJi=∂X∂ei，可以简化为：
e≈JΔX+b
e≈JΔX+b其中 bb 是当前误差值。
4. 通过正规方程求解最优解将上面的误差向量放入最小化问题，并对 ΔXΔX 求导，得到正规方程：
JTJΔX=JTb
JTJΔX=JTb其中：JTJJTJ 对应 matAtA。JTbJTb 对应 matAtB。将这个方程用 QR 分解或其他解线性方程的方式求出 ΔXΔX，即为位姿更新量

在第一次迭代时，要判断matAtA的奇异性，处理退化情况。

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));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]) {//将特征值小于100，对应的特征向量置零。for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}matP = matV.inv() * matV2;//matV.inv() 是 matV 的逆矩阵,matP对应的就是除了退化的列为0，其他的列均为1。}

transformUpdate();

void transformUpdate(){if (imuPointerLast >= 0) {float imuRollLast = 0, imuPitchLast = 0;while (imuPointerFront != imuPointerLast) {if (timeLaserOdometry + scanPeriod < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeLaserOdometry + scanPeriod > imuTime[imuPointerFront]) {imuRollLast = imuRoll[imuPointerFront];imuPitchLast = imuPitch[imuPointerFront];} else {int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;float ratioFront = (timeLaserOdometry + scanPeriod - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeLaserOdometry - scanPeriod) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);imuRollLast = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;imuPitchLast = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;}transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast;transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast;}for (int i = 0; i < 6; i++) {transformBefMapped[i] = transformSum[i];transformAftMapped[i] = transformTobeMapped[i];}}

saveKeyFramesAndFactor()；

void saveKeyFramesAndFactor(){currentRobotPosPoint.x = transformAftMapped[3];currentRobotPosPoint.y = transformAftMapped[4];currentRobotPosPoint.z = transformAftMapped[5];
//第一次运行时，transformAftMapped初始化为0，xyz均为0。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;
//如果两帧之间的距离太小，就不保存！！}
//所以第一次运行时，saveThisKeyFrame的值肯定是false!!if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())return;
//所以关键点在这，第一次运行时，也不会return，因为cloudKeyPoses3D->points.empty()。previousRobotPosPoint = currentRobotPosPoint;/*** update grsam graph*/if (cloudKeyPoses3D->points.empty()){gtSAMgraph.add(PriorFactor<Pose3>(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<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])));}/*** 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<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 indexcloudKeyPoses3D->push_back(thisPose3D);thisPose6D.x = thisPose3D.x;thisPose6D.y = thisPose3D.y;thisPose6D.z = thisPose3D.z;thisPose6D.intensity = thisPose3D.intensity; // this can be used as indexthisPose6D.roll  = latestEstimate.rotation().pitch();thisPose6D.pitch = latestEstimate.rotation().yaw();thisPose6D.yaw   = latestEstimate.rotation().roll(); // in camera framethisPose6D.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<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);    
}

我们仔细看一下这段代码，这段代码考虑第一次优化时的情况：

 if (cloudKeyPoses3D->points.empty()){gtSAMgraph.add(PriorFactor<Pose3>(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));
//这里将一个先验因子添加到因子图 gtSAMgraph 中，以指定第一个关键帧的位置和姿态。这是为了固定第一个关键帧，作为优化的参考基准。
//    PriorFactor<Pose3>(0, ...) 以第一个关键帧（索引 0）为对象，提供先验位姿。
//    Pose3(Rot3::RzRyRx(...), Point3(...)) 用于表示位姿，其中 Rot3::RzRyRx(...) 表示旋转角度，Point3(...) 表示平移。
//    priorNoise 是因子的噪声模型，用于定义先验约束的置信度。initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));
//将第一个关键帧位姿插入到 initialEstimate 中，作为优化的初始估计值。for (int i = 0; i < 6; ++i)transformLast[i] = transformTobeMapped[i];}

当我们的因子图中有第一个因子之后，就可以添加到cloudKeyPoses3D和cloudKeyPoses6D中。

注意这里又做了一次坐标轴的变换，x->y,y->z,z->x。

cloudKeyPoses3D代表点，也就是位置，position。

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);

cloudKeyPoses6D代表位姿，也就是变换，transform。

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);

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);
//使用 pcl::copyPointCloud 将 laserCloudCornerLastDS、laserCloudSurfLastDS 和 
//laserCloudOutlierLastDS 点云数据复制到各自的关键帧中：
//    laserCloudCornerLastDS 是最新一帧经过降采样后的角点点云数据。
//    laserCloudSurfLastDS 是最新一帧经过降采样后的平面点数据。
//    laserCloudOutlierLastDS 是最新一帧经过降采样后的离群点数据。
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);

这时咱们就可以回头看extractSurroundingKeyFrames();了。

看完上述所有代码，咱们继续看saveKeyFramesAndFactor()；

我们来看更新因子图的代码，之前我们看过图初始化的代码，现在我们来看初始化完成后的情况：

/**

* update grsam graph

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<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])));}

transformLast相当于上上帧优化的位姿，transformAftMapped是上一帧优化的位姿，

添加因子：

gtSAMgraph.add(BetweenFactor<Pose3>(...))：这一行代码将一个"位姿间因子"添加到因子图中。
BetweenFactor 用于表示两个位姿之间的约束（即 poseFrom 和 poseTo 之间的相对位姿）。这里使用 between 方法来计算两个位姿之间的相对变换，并将这个因子添加到图中，同时还包含了 odometryNoise 作为噪声模型。

odometryNoise = noiseModel::Diagonal::Variances(Vector6);

noiseModel::Diagonal::Variances：

这是GTSAM库中用于定义噪声模型的一个函数。噪声模型用于表示在测量过程中可能存在的不确定性，通常用于优化问题。
Diagonal表示这个噪声模型是对角线形式，意味着每个维度的噪声是独立的。

初始估计的插入：
- initialEstimate.insert(...)：这行代码将 poseTo（表示后映射的位姿）插入到初始估计中。
- 这里使用的旋转和平移量与 poseTo 中使用的相同，确保初始估计与因子图中的因子一致。

总的来说，这段代码的功能是从给定的转换数据（transformLast 和 transformAftMapped）中提取位姿信息，构建两个位姿之间的约束，并将这些约束添加到优化图中，同时为新位姿提供初始估计。

/*** 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<Pose3>(isamCurrentEstimate.size()-1);

问：这种优化的逻辑是什么？为什么要这样优化？

优化的逻辑

因子图模型：
- 在因子图中，节点通常表示变量（如机器人或传感器的位姿），边（因子）表示观测或约束（如里程计数据、传感器测量等）。
- 每个因子包含关于变量之间关系的信息，以及它们的噪声模型，提供了从一个状态（位姿）到另一个状态的测量或预测。
最小化误差：
- 优化过程的目标是最小化所有因子的误差函数。每个因子的误差反映了当前位姿估计与实际观测之间的差距。
- 通过调整位姿估计，使得所有因子的误差尽可能小，从而得到一个全局一致的状态估计。（我觉得这一点很重要，全局一致）
非线性最小二乘：
- GTSAM通常使用非线性最小二乘法（如高斯-牛顿法或莱文伯格-马夸尔特法）来求解这个优化问题。
- 该方法迭代地调整变量以最小化残差，直到达到收敛条件。

correctPoses()

void correctPoses(){if (aLoopIsClosed == true){recentCornerCloudKeyFrames. clear();recentSurfCloudKeyFrames.   clear();recentOutlierCloudKeyFrames.clear();// update key posesint 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;}}

aLoopIsClosed = false; aLoopIsClosed的初值是false，经过回环检测函数performLoopClosure()会变成true，我们先看回环检测。