LIO-SAM中的src/mapOptmization.cpp详细解读
LIO-SAM 中 mapOptimization.cpp 详细解读
mapOptimization.cpp 是 LIO-SAM 里最核心的后端建图优化文件。前面的 imageProjection.cpp 负责点云整理和去畸变，featureExtraction.cpp 负责提取角点和平面点，imuPreintegration.cpp 负责 IMU 预积分和高频 odom 输出，而这个 mapOptimization.cpp 主要负责把当前帧特征点和局部地图进行 scan-to-map 匹配，优化当前 LiDAR 位姿，并把关键帧加入 GTSAM 因子图中进行全局优化。
一句话概括：
mapOptimization.cpp = 局部 scan-to-map 匹配 + 关键帧管理 + 因子图优化 + GPS/回环融合 + 地图发布。它的主流程大概是：
接收 featureExtraction 输出的 cloud_info
↓
根据 IMU / odom 给当前帧位姿初值
↓
提取周围关键帧组成局部地图
↓
当前角点 / 平面点降采样
↓
scan-to-map 优化当前位姿
↓
判断是否保存关键帧
↓
添加 odom / GPS / loop 因子
↓
iSAM2 优化关键帧位姿图
↓
回环后修正历史关键帧
↓
发布 odom、path、局部地图、全局地图、关键帧点云一、头文件和 GTSAM 符号定义
1～18 行：引入 LIO-SAM 消息、服务和 GTSAM 头文件
1 #include "utility.h"
2 #include "lio_sam/cloud_info.h"
3 #include "lio_sam/save_map.h"
4
5 #include <gtsam/geometry/Rot3.h>
6 #include <gtsam/geometry/Pose3.h>
7 #include <gtsam/slam/PriorFactor.h>
8 #include <gtsam/slam/BetweenFactor.h>
9 #include <gtsam/navigation/GPSFactor.h>
10 #include <gtsam/navigation/ImuFactor.h>
11 #include <gtsam/navigation/CombinedImuFactor.h>
12 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
13 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
14 #include <gtsam/nonlinear/Marginals.h>
15 #include <gtsam/nonlinear/Values.h>
16 #include <gtsam/inference/Symbol.h>
17
18 #include <gtsam/nonlinear/ISAM2.h>这一段主要引入三个部分。
第一部分是 LIO-SAM 自己的头文件。utility.h 里面一般包含参数服务器、点云类型、ROS 工具函数、坐标系名称、发布点云函数、距离计算函数等。lio_sam/cloud_info.h 是前端传给后端的综合信息包，里面包含当前帧角点云、平面点云、IMU 姿态初值、IMU 预积分 odom 初值等。lio_sam/save_map.h 是保存地图服务，用于把关键帧地图导出成 PCD 文件。
第二部分是 GTSAM 的几何和因子图模块。Pose3 表示三维位姿，Rot3 表示三维旋转，PriorFactor 表示先验因子，BetweenFactor 表示两个位姿之间的约束，GPSFactor 表示 GPS 位置约束，ISAM2 是增量式图优化器。
第三部分中虽然引入了 ImuFactor 和 CombinedImuFactor，但在这个文件里核心用到的是 PriorFactor、BetweenFactor、GPSFactor 和 ISAM2。IMU 预积分主要在 imuPreintegration.cpp 中完成，这个文件更多是 LiDAR mapping 后端。
20～25 行：GTSAM 符号简写
20 using namespace gtsam;
21
22 using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
23 using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
24 using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
25 using symbol_shorthand::G; // GPS pose这里定义 GTSAM 变量符号。和 imuPreintegration.cpp 不一样，这个文件实际使用更多的是普通整数 key，比如：
gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));也就是说，在 mapOptimization.cpp 中，每个关键帧对应一个 Pose3 位姿变量，key 通常就是关键帧编号：
第 0 个关键帧：Pose3(0)
第 1 个关键帧：Pose3(1)
第 2 个关键帧：Pose3(2)
...这里虽然定义了 X、V、B、G，但这个文件主要优化的是关键帧位姿，不像 imuPreintegration.cpp 那样同时优化 X、V、B。
二、自定义关键帧位姿点类型
30～49 行：PointXYZIRPYT
30 struct PointXYZIRPYT
31 {
32 PCL_ADD_POINT4D
33 PCL_ADD_INTENSITY;
34 float roll;
35 float pitch;
36 float yaw;
37 double time;
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39 } EIGEN_ALIGN16;
40
41 POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
42 (float, x, x) (float, y, y)
43 (float, z, z) (float, intensity, intensity)
44 (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
45 (double, time, time))
46
47 typedef PointXYZIRPYT PointTypePose;这里定义了一个带有 6D 位姿信息的点类型 PointXYZIRPYT。
普通点云点 PointType 一般只包含：
x, y, z, intensity但是关键帧位姿不仅需要位置，还需要姿态和时间。所以这里额外加入了：
roll：横滚角
pitch：俯仰角
yaw：航向角
time：关键帧时间戳intensity 在这里还有一个特殊用途：它常常被用来存关键帧索引。例如后面保存关键帧时：
thisPose3D.intensity = cloudKeyPoses3D->size();也就是说，cloudKeyPoses3D 中的 intensity 不是激光强度，而是关键帧编号。这样后面通过 KD-tree 搜索到某个关键帧位置后，可以通过 intensity 找回它对应的关键帧索引。
三、mapOptimization 类成员变量
50～156 行：后端优化所需的核心变量
50 class mapOptimization : public ParamServer
51 {
52
53 public:
54
55 // gtsam
56 NonlinearFactorGraph gtSAMgraph;
57 Values initialEstimate;
58 Values optimizedEstimate;
59 ISAM2 *isam;
60 Values isamCurrentEstimate;
61 Eigen::MatrixXd poseCovariance;这一段定义 GTSAM 因子图相关变量。
gtSAMgraph 是当前要加入的新因子图。每次保存关键帧时，会往里面添加 odom 因子、GPS 因子、回环因子。initialEstimate 是这些新变量的初始值。isam 是 iSAM2 优化器，它会增量地维护整张关键帧位姿图。isamCurrentEstimate 是当前优化后的全部关键帧位姿结果。poseCovariance 是最新关键帧位姿的边缘协方差，后面 GPS 是否加入会参考这个值。
62～79 行：ROS 发布器和订阅器
62 ros::Publisher pubLaserCloudSurround;
63 ros::Publisher pubLaserOdometryGlobal;
64 ros::Publisher pubLaserOdometryIncremental;
65 ros::Publisher pubKeyPoses;
66 ros::Publisher pubPath;
67
68 ros::Publisher pubHistoryKeyFrames;
69 ros::Publisher pubIcpKeyFrames;
70 ros::Publisher pubRecentKeyFrames;
71 ros::Publisher pubRecentKeyFrame;
72 ros::Publisher pubCloudRegisteredRaw;
73 ros::Publisher pubLoopConstraintEdge;
74
75 ros::Publisher pubSLAMInfo;
76
77 ros::Subscriber subCloud;
78 ros::Subscriber subGPS;
79 ros::Subscriber subLoop;这一部分负责和 ROS 其他模块通信。
核心发布器包括：
pubLaserOdometryGlobal：
发布全局 mapping odom，对应话题 lio_sam/mapping/odometry。
pubLaserOdometryIncremental：
发布增量 odom，对应话题 lio_sam/mapping/odometry_incremental。
这个话题会被 imuPreintegration.cpp 订阅，用作 IMU 预积分校正输入。
pubKeyPoses：
发布关键帧轨迹点云。
pubPath：
发布 nav_msgs::Path，用于 RViz 显示轨迹。
pubRecentKeyFrames：
发布当前局部地图。
pubRecentKeyFrame：
发布当前配准后的关键帧点云。
pubCloudRegisteredRaw：
发布当前配准后的原始去畸变点云。
pubLoopConstraintEdge：
发布回环边可视化。核心订阅器包括：
subCloud：
订阅 lio_sam/feature/cloud_info，也就是前端特征提取后的结果。
subGPS：
订阅 GPS odom。
subLoop：
订阅外部回环检测结果。84～156 行：点云、KD-tree、体素滤波、回环变量和位姿变量
84 vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
85 vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
86
87 pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
88 pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;
89 pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D;
90 pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;cornerCloudKeyFrames 保存每个关键帧的角点特征点云。surfCloudKeyFrames 保存每个关键帧的平面点特征点云。后面构建局部地图、回环检测、保存地图时，都会用这些关键帧点云。
cloudKeyPoses3D 保存关键帧的位置，主要用于 KD-tree 搜索附近关键帧。cloudKeyPoses6D 保存关键帧完整 6D 位姿，包括位置、姿态和时间。copy_cloudKeyPoses3D 和 copy_cloudKeyPoses6D 是回环线程使用的副本，避免回环线程和主线程同时读写同一个点云。
96 pcl::PointCloud<PointType>::Ptr laserCloudOri;
97 pcl::PointCloud<PointType>::Ptr coeffSel;laserCloudOri 存参与优化的原始当前帧特征点。coeffSel 存这些特征点对应的残差系数。它们是 LM 优化的输入。
122 pcl::VoxelGrid<PointType> downSizeFilterCorner;
123 pcl::VoxelGrid<PointType> downSizeFilterSurf;
124 pcl::VoxelGrid<PointType> downSizeFilterICP;
125 pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses;这些是体素滤波器。角点、平面点、ICP 回环点云、周围关键帧位姿都会分别降采样。降采样的目的不是改变算法本质，而是减少点数，提高实时性。
130 float transformTobeMapped[6];这是当前帧待优化的位姿，整个文件非常核心。它的顺序是：
transformTobeMapped[0] = roll
transformTobeMapped[1] = pitch
transformTobeMapped[2] = yaw
transformTobeMapped[3] = x
transformTobeMapped[4] = y
transformTobeMapped[5] = z注意它不是 x,y,z,roll,pitch,yaw，而是先姿态后三维平移。
四、构造函数和内存初始化
157～193 行：构造函数
157 mapOptimization()
158 {
159 ISAM2Params parameters;
160 parameters.relinearizeThreshold = 0.1;
161 parameters.relinearizeSkip = 1;
162 isam = new ISAM2(parameters);构造函数一开始初始化 iSAM2。relinearizeThreshold = 0.1 表示变量变化超过一定阈值时重新线性化，relinearizeSkip = 1 表示每次 update 都检查是否需要重新线性化。iSAM2 的作用是增量式优化关键帧位姿图，每次新关键帧、新 GPS、新回环约束进来时，不需要从头优化整张图，而是在已有结果上增量更新。
164 pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);
165 pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1);
166 pubLaserOdometryGlobal = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry", 1);
167 pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry_incremental", 1);
168 pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);这几行创建主要输出话题。其中 lio_sam/mapping/odometry 是全局优化后的 LiDAR odom，lio_sam/mapping/odometry_incremental 是增量 odom，会传给 IMU 预积分模块。
170 subCloud = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
171 subGPS = nh.subscribe<nav_msgs::Odometry> (gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
172 subLoop = nh.subscribe<std_msgs::Float64MultiArray>("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay());这里订阅三个输入。最核心的是 lio_sam/feature/cloud_info，它是前端送来的当前帧特征信息。GPS 和 loop 是额外约束，不一定每个系统都会启用。
186 downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
187 downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
188 downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
189 downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity);
191 allocateMemory();这里设置各种体素滤波大小。角点和平面点可以使用不同体素大小。最后调用 allocateMemory() 分配点云、KD-tree 和缓存数组。
194～236 行：allocateMemory()
194 void allocateMemory()
195 {
196 cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
197 cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
198 copy_cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
199 copy_cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());这里创建关键帧位姿点云。cloudKeyPoses3D 用于空间搜索，cloudKeyPoses6D 用于保存完整位姿。回环线程使用 copy_cloudKeyPoses3D/6D，避免主线程修改时回环线程读到不一致数据。
207 laserCloudOri.reset(new pcl::PointCloud<PointType>());
208 coeffSel.reset(new pcl::PointCloud<PointType>());这两个变量是 scan-to-map 优化里最关键的输入。laserCloudOri 存当前帧中参与优化的特征点，coeffSel 存每个特征点对应的残差方向和距离。后面的 LMOptimization() 就是根据它们构建线性方程。
210 laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
211 coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
212 laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
213 laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
214 coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
215 laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);这些数组用于并行计算角点和平面点残差。因为 cornerOptimization() 和 surfOptimization() 中用了 OpenMP 并行，如果多个线程同时往同一个点云 push_back，会有线程安全问题。所以代码先用固定大小数组保存每个点的结果和标志位，最后再统一合并。
229 for (int i = 0; i < 6; ++i){
230 transformTobeMapped[i] = 0;
231 }
233 matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));初始化当前待优化位姿为 0。matP 是退化处理时用的投影矩阵，后面在 LM 优化中如果发现某些方向不可观，会用它抑制这些方向的更新。
五、主回调流程：laserCloudInfoHandler
237～272 行：接收前端特征点云并执行 mapping 主流程
237 void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
238 {
239 // extract time stamp
240 timeLaserInfoStamp = msgIn->header.stamp;
241 timeLaserInfoCur = msgIn->header.stamp.toSec();
242
243 // extract info and feature cloud
244 cloudInfo = *msgIn;
245 pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
246 pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);这个函数是 mapping 主入口。每次前端 featureExtraction 发布一帧 cloud_info，这里就会收到。
第 240～241 行保存当前帧时间。第 244 行保存整个 cloudInfo，里面包含 IMU 姿态初值、IMU 预积分 odom 初值、当前帧角点和平面点等。第 245～246 行把 ROS 消息里的角点云和平面点云转换成 PCL 点云。
250 static double timeLastProcessing = -1;
251 if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
252 {
253 timeLastProcessing = timeLaserInfoCur;
254
255 updateInitialGuess();
256
257 extractSurroundingKeyFrames();
258
259 downsampleCurrentScan();
260
261 scan2MapOptimization();
262
263 saveKeyFramesAndFactor();
264
265 correctPoses();
266
267 publishOdometry();
268
269 publishFrames();
270 }这一段就是完整的 mapping 主流程。
mappingProcessInterval 控制 mapping 处理频率。如果 LiDAR 或前端发布太快，后端可以不每帧都处理，避免计算压力过大。
主流程每一步含义如下：
updateInitialGuess：
利用 IMU 姿态或者 IMU 预积分 odom，给当前 scan-to-map 优化一个初始位姿。
extractSurroundingKeyFrames：
从历史关键帧中提取当前附近的关键帧，组成局部地图。
downsampleCurrentScan：
对当前帧角点和平面点降采样，减少优化点数。
scan2MapOptimization：
当前帧特征点和局部地图做匹配，优化 transformTobeMapped。
saveKeyFramesAndFactor：
判断是否保存关键帧，如果保存，就添加 odom/GPS/loop 因子，并用 iSAM2 优化。
correctPoses：
如果发生回环，就根据 iSAM2 结果修正所有历史关键帧位姿。
publishOdometry：
发布当前 mapping odom 和 incremental odom。
publishFrames：
发布关键帧、局部地图、当前配准点云、path 等。这个函数可以理解成整个 mapOptimization.cpp 的调度器。
六、点云坐标变换工具函数
278～285 行：pointAssociateToMap()
278 void pointAssociateToMap(PointType const * const pi, PointType * const po)
279 {
280 po->x = transPointAssociateToMap(0,0) * pi->x + transPointAssociateToMap(0,1) * pi->y + transPointAssociateToMap(0,2) * pi->z + transPointAssociateToMap(0,3);
281 po->y = transPointAssociateToMap(1,0) * pi->x + transPointAssociateToMap(1,1) * pi->y + transPointAssociateToMap(1,2) * pi->z + transPointAssociateToMap(1,3);
282 po->z = transPointAssociateToMap(2,0) * pi->x + transPointAssociateToMap(2,1) * pi->y + transPointAssociateToMap(2,2) * pi->z + transPointAssociateToMap(2,3);
283 po->intensity = pi->intensity;
284 }这个函数把当前帧 LiDAR 坐标系下的点 pi 转换到 map 坐标系下，输出为 po。
这里用的变换矩阵是 transPointAssociateToMap，它由当前待优化位姿 transformTobeMapped 转换而来。scan-to-map 匹配时，需要把当前帧点先按照当前估计位姿投影到地图系，再去局部地图中找最近邻。
286～306 行：transformPointCloud()
286 pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose* transformIn)
287 {
288 pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
289
290 int cloudSize = cloudIn->size();
291 cloudOut->resize(cloudSize);
292
293 Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);
294
295 #pragma omp parallel for num_threads(numberOfCores)
296 for (int i = 0; i < cloudSize; ++i)
297 {
298 const auto &pointFrom = cloudIn->points[i];
299 cloudOut->points[i].x = transCur(0,0) * pointFrom.x + transCur(0,1) * pointFrom.y + transCur(0,2) * pointFrom.z + transCur(0,3);
300 cloudOut->points[i].y = transCur(1,0) * pointFrom.x + transCur(1,1) * pointFrom.y + transCur(1,2) * pointFrom.z + transCur(1,3);
301 cloudOut->points[i].z = transCur(2,0) * pointFrom.x + transCur(2,1) * pointFrom.y + transCur(2,2) * pointFrom.z + transCur(2,3);
302 cloudOut->points[i].intensity = pointFrom.intensity;
303 }
304 return cloudOut;
305 }这个函数把一整帧点云按照某个关键帧位姿转换到 map 坐标系下。
它主要用于两类场景：
1. 构建局部地图：
把附近关键帧的角点云、平面点云从各自 LiDAR 坐标系转换到 map 坐标系。
2. 保存全局地图：
把所有关键帧点云按照优化后的位姿转换到全局坐标系，再拼接成完整地图。第 295 行使用 OpenMP 并行加速，因为点云中每个点的坐标变换互相独立，适合并行处理。
307～354 行：位姿格式转换函数
307 gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint)
308 {
309 return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
310 gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
311 }
312
313 gtsam::Pose3 trans2gtsamPose(float transformIn[])
314 {
315 return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]),
316 gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
317 }这里是不同位姿格式之间的转换。pclPointTogtsamPose3() 把 PointTypePose 转成 GTSAM Pose3。trans2gtsamPose() 把 transformTobeMapped[6] 转成 GTSAM Pose3。
319 Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint)
320 {
321 return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);
322 }
323
324 Eigen::Affine3f trans2Affine3f(float transformIn[])
325 {
326 return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);
327 }这两个函数把位姿转成 Eigen::Affine3f，用于矩阵乘法。特别注意 transformTobeMapped 的顺序是：
roll, pitch, yaw, x, y, z而 pcl::getTransformation() 的输入顺序是：
x, y, z, roll, pitch, yaw所以第 326 行传参顺序必须调换。
七、保存地图和全局地图可视化
355～420 行：saveMapService()
355 bool saveMapService(lio_sam::save_mapRequest& req, lio_sam::save_mapResponse& res)
356 {
357 string saveMapDirectory;
358
359 cout << "****************************************************" << endl;
360 cout << "Saving map to pcd files ..." << endl;
361 if(req.destination.empty()) saveMapDirectory = std::getenv("HOME") + savePCDDirectory;
362 else saveMapDirectory = std::getenv("HOME") + req.destination;这个函数响应保存地图服务。保存目录如果请求里没写，就使用配置里的 savePCDDirectory；如果请求里写了，就保存到指定路径。
367 pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D);
368 pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D);这里先保存轨迹和关键帧位姿。trajectory.pcd 只保存 3D 轨迹点，transformations.pcd 保存完整 6D 位姿。
374 for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) {
375 *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
376 *globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
377 cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
378 }这里遍历所有关键帧，把每个关键帧的角点和平面点转换到全局 map 坐标系，并累加成全局点云地图。
380 if(req.resolution != 0)
381 {
382 cout << "\n\nSave resolution: " << req.resolution << endl;
383
384 downSizeFilterCorner.setInputCloud(globalCornerCloud);
385 downSizeFilterCorner.setLeafSize(req.resolution, req.resolution, req.resolution);
386 downSizeFilterCorner.filter(*globalCornerCloudDS);
387 pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS);如果请求里设置了保存分辨率，就对全局角点和平面点进行体素降采样再保存。否则直接保存原始拼接后的地图。
最后会生成：
trajectory.pcd：关键帧位置轨迹
transformations.pcd：关键帧 6D 位姿
CornerMap.pcd：全局角点地图
SurfMap.pcd：全局平面点地图
GlobalMap.pcd：角点 + 平面点合并地图440～502 行：publishGlobalMap()
440 void publishGlobalMap()
441 {
442 if (pubLaserCloudSurround.getNumSubscribers() == 0)
443 return;
444
445 if (cloudKeyPoses3D->points.empty() == true)
446 return;这个函数用于发布全局地图可视化点云。如果没有订阅者，或者还没有关键帧，就直接返回，避免浪费计算。
459 mtx.lock();
460 kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
461 kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
462 mtx.unlock();它不是每次都发布完整全局地图，而是以当前最后一个关键帧为中心，在一定半径内搜索附近关键帧，只发布附近地图。这样可视化更轻量。
483 int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
484 *globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
485 *globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);这里把搜索到的关键帧点云转换到 map 坐标系，并拼接成用于显示的局部全局地图。
八、回环检测和 ICP 回环约束
503～516 行：loopClosureThread()
503 void loopClosureThread()
504 {
505 if (loopClosureEnableFlag == false)
506 return;
507
508 ros::Rate rate(loopClosureFrequency);
509 while (ros::ok())
510 {
511 rate.sleep();
512 performLoopClosure();
513 visualizeLoopClosure();
514 }
515 }回环检测单独开一个线程运行。如果配置里关闭了回环，就直接返回。如果开启，它会按照 loopClosureFrequency 的频率周期执行 performLoopClosure()，然后调用 visualizeLoopClosure() 发布回环边可视化。
529～609 行：performLoopClosure()
529 void performLoopClosure()
530 {
531 if (cloudKeyPoses3D->points.empty() == true)
532 return;
533
534 mtx.lock();
535 *copy_cloudKeyPoses3D = *cloudKeyPoses3D;
536 *copy_cloudKeyPoses6D = *cloudKeyPoses6D;
537 mtx.unlock();回环线程先复制当前关键帧位姿。这样做是为了避免主线程正在更新关键帧时，回环线程同时读取导致数据不一致。
540 int loopKeyCur;
541 int loopKeyPre;
542 if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)
543 if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)
544 return;这里先尝试外部回环检测，如果没有外部回环结果，就使用距离搜索回环。也就是说，回环候选有两种来源：
1. 外部模块给出当前帧和历史帧的回环关系；
2. 当前关键帧附近搜索到距离近、时间间隔大的历史关键帧。550 loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
551 loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
552 if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
553 return;这里构造当前关键帧局部点云和历史关键帧局部点云。如果点数太少，就不做 ICP，因为点数太少时 ICP 不稳定。
560 static pcl::IterativeClosestPoint<PointType, PointType> icp;
561 icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
562 icp.setMaximumIterations(100);
563 icp.setTransformationEpsilon(1e-6);
564 icp.setEuclideanFitnessEpsilon(1e-6);
565 icp.setRANSACIterations(0);这里配置 ICP 参数。ICP 用当前关键帧点云去对齐历史关键帧点云，验证这个回环是否是真的。
568 icp.setInputSource(cureKeyframeCloud);
569 icp.setInputTarget(prevKeyframeCloud);
570 pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
571 icp.align(*unused_result);
573 if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
574 return;如果 ICP 不收敛，或者 fitness score 大于阈值，就认为回环不可靠，直接返回。
589 Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
591 Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
592 pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
593 gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
594 gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);ICP 得到的是当前帧相对历史帧的校正变换。代码用这个变换修正当前关键帧的错误位姿，得到 poseFrom。poseTo 是历史关键帧位姿。
602 loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
603 loopPoseQueue.push_back(poseFrom.between(poseTo));
604 loopNoiseQueue.push_back(constraintNoise);
608 loopIndexContainer[loopKeyCur] = loopKeyPre;最后把回环约束保存到队列里，后面 addLoopFactor() 会把它加入 GTSAM 因子图。
610～644 行：detectLoopClosureDistance()
610 bool detectLoopClosureDistance(int *latestID, int *closestID)
611 {
612 int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
613 int loopKeyPre = -1;距离回环检测默认使用最新关键帧作为当前回环候选。
623 kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
624 kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);用 KD-tree 搜索空间上靠近当前关键帧的历史关键帧。
628 int id = pointSearchIndLoop[i];
629 if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff)
630 {
631 loopKeyPre = id;
632 break;
633 }这里要求时间差足够大。原因是刚刚经过的位置也可能空间距离很近，但它不是回环。如果没有时间间隔限制，就会把相邻关键帧误认为回环。
九、初始位姿预测
786～846 行：updateInitialGuess()
786 void updateInitialGuess()
787 {
788 // save current transformation before any processing
789 incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);这个函数在每帧 scan-to-map 优化前执行，用来给当前帧一个初始位姿。初值很重要，如果初值太差，scan-to-map 容易匹配失败。
第 789 行先保存当前帧优化前的位姿，后面发布 incremental odom 时会用它和优化后的位姿计算增量。
793 if (cloudKeyPoses3D->points.empty())
794 {
795 transformTobeMapped[0] = cloudInfo.imuRollInit;
796 transformTobeMapped[1] = cloudInfo.imuPitchInit;
797 transformTobeMapped[2] = cloudInfo.imuYawInit;
799 if (!useImuHeadingInitialization)
800 transformTobeMapped[2] = 0;
803 lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
804 return;
805 }如果还没有任何关键帧，说明这是系统刚开始建图。此时直接用 IMU 的 roll、pitch、yaw 初始化姿态。如果不启用 IMU heading 初始化，就把 yaw 设为 0。因为普通 6 轴 IMU 的 yaw 通常不可观，容易漂。
808 static bool lastImuPreTransAvailable = false;
809 static Eigen::Affine3f lastImuPreTransformation;
810 if (cloudInfo.odomAvailable == true)
811 {
812 Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ,
813 cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);如果 IMU 预积分 odom 可用，就优先使用它。cloudInfo.initialGuessX/Y/Z/Roll/Pitch/Yaw 来自前端传入的初始估计，一般由 imuPreintegration.cpp 提供。
818 Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
819 Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
820 Eigen::Affine3f transFinal = transTobe * transIncre;
821 pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
822 transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);这里不是直接把 IMU 预积分 odom 当作最终位姿，而是取两帧之间的增量 transIncre，再叠加到当前 transformTobeMapped 上。这样做可以保持 mapping 坐标系下位姿的连续性。
833 if (cloudInfo.imuAvailable == true)
834 {
835 Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
836 Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;
837
838 Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
839 Eigen::Affine3f transFinal = transTobe * transIncre;如果 IMU 预积分 odom 不可用，但 IMU 姿态可用，就只使用 IMU 的旋转增量来更新初值。这里没有用 IMU 平移，因为单纯 IMU 平移积分容易漂，尤其没有可靠 bias 校正时。
总结一下，初值优先级是：
第一帧：
用 IMU roll/pitch/yaw 初始化。
后续帧：
优先用 IMU 预积分 odom 增量更新完整 6D 初值；
如果没有 odom，就用 IMU 姿态增量更新 roll/pitch/yaw。十、局部地图提取
862～898 行：extractNearby()
862 void extractNearby()
863 {
864 pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
865 pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
866 std::vector<int> pointSearchInd;
867 std::vector<float> pointSearchSqDis;这个函数用于找当前机器人附近的关键帧，用这些关键帧构建局部地图。
870 kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
871 kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
872 for (int i = 0; i < (int)pointSearchInd.size(); ++i)
873 {
874 int id = pointSearchInd[i];
875 surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
876 }以最新关键帧位置为中心，在一定半径内搜索周围关键帧。搜索结果放进 surroundingKeyPoses。
878 downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
879 downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);对周围关键帧位姿做降采样，避免局部地图中关键帧过密
889 int numPoses = cloudKeyPoses3D->size();
890 for (int i = numPoses-1; i >= 0; --i)
891 {
892 if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
893 surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
894 else
895 break;
896 }这里额外加入最近 10 秒内的关键帧。原因是如果机器人原地旋转，空间位移很小，单纯半径搜索和位姿降采样可能会丢掉一些最近帧；加入最近时间内的关键帧可以增强局部地图完整性。
899～939 行：extractCloud()
899 void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
900 {
901 // fuse the map
902 laserCloudCornerFromMap->clear();
903 laserCloudSurfFromMap->clear();
904 for (int i = 0; i < (int)cloudToExtract->size(); ++i)
905 {
906 if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
907 continue;这个函数根据选出的关键帧，真正构建局部角点地图和平面点地图。
909 int thisKeyInd = (int)cloudToExtract->points[i].intensity;
910 if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end())
911 {
912 *laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
913 *laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second;
914 } else {
915 pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
916 pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
917 *laserCloudCornerFromMap += laserCloudCornerTemp;
918 *laserCloudSurfFromMap += laserCloudSurfTemp;
919 laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
920 }如果某个关键帧转换后的点云已经缓存过，就直接从 laserCloudMapContainer 取出来，避免重复变换。如果没有缓存，就调用 transformPointCloud() 把该关键帧的角点和平面点从 LiDAR 坐标系转换到 map 坐标系，并加入局部地图。
925 downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
926 downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
927 laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
929 downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
930 downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
931 laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();局部地图构建完成后，对角点地图和平面点地图分别降采样。后面的 scan-to-map 匹配会在降采样后的局部地图上建立 KD-tree。
十一、当前帧降采样
955～968 行：downsampleCurrentScan()
955 void downsampleCurrentScan()
956 {
957 // Downsample cloud from current scan
958 laserCloudCornerLastDS->clear();
959 downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
960 downSizeFilterCorner.filter(*laserCloudCornerLastDS);
961 laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();
962
963 laserCloudSurfLastDS->clear();
964 downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
965 downSizeFilterSurf.filter(*laserCloudSurfLastDS);
966 laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
967 }这部分很直接，就是对当前帧角点和平面点降采样。
降采样的目的有两个：
1. 减少参与优化的点数，提高实时性；
2. 让点云分布更均匀，避免某些区域点太密导致优化被局部区域主导。十二、角点优化：点到线约束
974～1065 行：cornerOptimization()
974 void cornerOptimization()
975 {
976 updatePointAssociateToMap();
977
978 #pragma omp parallel for num_threads(numberOfCores)
979 for (int i = 0; i < laserCloudCornerLastDSNum; i++)
980 {
981 PointType pointOri, pointSel, coeff;
982 std::vector<int> pointSearchInd;
983 std::vector<float> pointSearchSqDis;这个函数处理当前帧角点。第 976 行先更新当前位姿对应的变换矩阵。第 978 行使用 OpenMP 并行遍历所有降采样后的当前角点。
985 pointOri = laserCloudCornerLastDS->points[i];
986 pointAssociateToMap(&pointOri, &pointSel);
987 kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);当前帧角点 pointOri 先根据当前估计位姿变换到 map 坐标系，得到 pointSel。然后在局部角点地图中搜索最近的 5 个点。
991 if (pointSearchSqDis[4] < 1.0) {
992 float cx = 0, cy = 0, cz = 0;
993 for (int j = 0; j < 5; j++) {
994 cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
995 cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
996 cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
997 }
998 cx /= 5; cy /= 5; cz /= 5;如果第 5 个最近邻距离也小于阈值，说明附近点足够近，可以尝试拟合一条线。这里先计算 5 个近邻点的均值中心。
1014 cv::eigen(matA1, matD1, matV1);
1016 if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {这里对 5 个近邻点的协方差矩阵做特征值分解。如果最大特征值明显大于第二大特征值，说明这 5 个点主要沿一个方向分布，可以认为它们形成一条线。
点到线距离的核心是：
当前点 pointSel 到地图线段 x1-x2 的距离。代码中 ld2 = a012 / l12 就是点到线的距离，其中 a012 表示由当前点和线段两个端点构成的平行四边形面积，l12 表示线段长度。
1052 coeff.x = s * la;
1053 coeff.y = s * lb;
1054 coeff.z = s * lc;
1055 coeff.intensity = s * ld2;
1057 if (s > 0.1) {
1058 laserCloudOriCornerVec[i] = pointOri;
1059 coeffSelCornerVec[i] = coeff;
1060 laserCloudOriCornerFlag[i] = true;
1061 }coeff 保存残差对当前点的约束方向和距离。coeff.x/y/z 可以理解为残差方向，coeff.intensity 保存残差大小。s 是权重，距离越大权重越低。如果权重太小，就认为这个约束不可靠，不参与优化。
这一部分就是典型的 LOAM 点到线残差：
角点约束 = 当前角点到地图中局部直线的距离。十三、平面点优化：点到面约束
1066～1136 行：surfOptimization()
1066 void surfOptimization()
1067 {
1068 updatePointAssociateToMap();
1069
1070 #pragma omp parallel for num_threads(numberOfCores)
1071 for (int i = 0; i < laserCloudSurfLastDSNum; i++)
1072 {这个函数处理当前帧平面点，逻辑和角点类似，也是先把当前点变换到 map 坐标系，然后在局部平面地图中找最近邻。
1078 pointOri = laserCloudSurfLastDS->points[i];
1079 pointAssociateToMap(&pointOri, &pointSel);
1080 kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);当前平面点投影到地图系后，在局部平面点地图中找 5 个最近邻。
1092 for (int j = 0; j < 5; j++) {
1093 matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
1094 matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
1095 matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
1096 }
1098 matX0 = matA0.colPivHouseholderQr().solve(matB0);这里用 5 个点拟合平面。平面方程可以写成：
pa * x + pb * y + pc * z + pd = 0代码里令 pd = 1，然后求解 pa, pb, pc。
1105 bool planeValid = true;
1106 for (int j = 0; j < 5; j++) {
1107 if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
1108 pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
1109 pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
1110 planeValid = false;
1111 break;
1112 }
1113 }拟合出平面后，还要检查 5 个近邻点是否真的都接近这个平面。如果某个点到平面的距离大于 0.2，就认为这个平面不可靠。
1116 float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
1118 float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointOri.x * pointOri.x
1119 + pointOri.y * pointOri.y + pointOri.z * pointOri.z));
1121 coeff.x = s * pa;
1122 coeff.y = s * pb;
1123 coeff.z = s * pc;
1124 coeff.intensity = s * pd2;pd2 是当前点到拟合平面的有符号距离。coeff.x/y/z 是平面法向量方向，coeff.intensity 是距离残差。这个残差后面会进入 LM 优化。
这一部分就是点到面约束：
平面点约束 = 当前平面点到地图中局部平面的距离。十四、合并角点和平面点约束
1137～1157 行：combineOptimizationCoeffs(
1137 void combineOptimizationCoeffs()
1138 {
1139 // combine corner coeffs
1140 for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
1141 if (laserCloudOriCornerFlag[i] == true){
1142 laserCloudOri->push_back(laserCloudOriCornerVec[i]);
1143 coeffSel->push_back(coeffSelCornerVec[i]);
1144 }
1145 }角点优化和平面点优化因为是并行计算，所以结果先分别存到了数组中。这里把有效角点约束加入统一的 laserCloudOri 和 coeffSel。
1147 for (int i = 0; i < laserCloudSurfLastDSNum; ++i){
1148 if (laserCloudOriSurfFlag[i] == true){
1149 laserCloudOri->push_back(laserCloudOriSurfVec[i]);
1150 coeffSel->push_back(coeffSelSurfVec[i]);
1151 }
1152 }
1154 std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
1155 std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);合并平面点约束后，重置标志位，为下一轮迭代做准备。
十五、LM 优化当前位姿
1158～1281 行：LMOptimization()
1158 bool LMOptimization(int iterCount)
1159 {
1160 // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation这个函数是 scan-to-map 的数值优化核心。它根据前面得到的点到线、点到面残差，构建线性方程，求解当前位姿增量。
1172 int laserCloudSelNum = laserCloudOri->size();
1173 if (laserCloudSelNum < 50) {
1174 return false;
1175 }如果有效约束点太少，就不优化。因为点太少时，线性方程不稳定。
1177 cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
1178 cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
1179 cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
1180 cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
1181 cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
1182 cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));这里构建最小二乘问题：
A * x = b其中：
A：残差对位姿六自由度的雅可比矩阵
x：要求解的位姿增量 [droll, dpitch, dyaw, dx, dy, dz]
b：负残差代码中最后求解的是：
AtA * x = AtB也就是正规方程。
1217 matA.at<float>(i, 0) = arz;
1218 matA.at<float>(i, 1) = arx;
1219 matA.at<float>(i, 2) = ary;
1220 matA.at<float>(i, 3) = coeff.z;
1221 matA.at<float>(i, 4) = coeff.x;
1222 matA.at<float>(i, 5) = coeff.y;
1223 matB.at<float>(i, 0) = -coeff.intensity;这里把每个点的约束写进矩阵 A 和 b。前 3 列对应旋转增量，后 3 列对应平移增量。因为 LOAM 原始实现里有 LiDAR 坐标系和 camera 坐标系的转换，所以这里的变量顺序看起来比较绕。
1226 cv::transpose(matA, matAt);
1227 matAtA = matAt * matA;
1228 matAtB = matAt * matB;
1229 cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);这里求解位姿增量 matX。matX 是 6×1 向量，分别对应 roll、pitch、yaw、x、y、z 的增量。
1231 if (iterCount == 0) {
1237 cv::eigen(matAtA, matE, matV);
1240 isDegenerate = false;
1241 float eignThre[6] = {100, 100, 100, 100, 100, 100};第一次迭代时，会判断当前优化是否退化。所谓退化，就是某些方向约束不足，比如走在长走廊里，沿走廊方向位移可能不容易被 LiDAR 约束住。
1243 if (matE.at<float>(0, i) < eignThre[i]) {
1244 for (int j = 0; j < 6; j++) {
1245 matV2.at<float>(i, j) = 0;
1246 }
1247 isDegenerate = true;如果某些特征值小于阈值，说明对应方向不可观，就把该方向的更新抑制掉。
1256 if (isDegenerate)
1257 {
1258 cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
1259 matX.copyTo(matX2);
1260 matX = matP * matX2;
1261 }如果退化，就用投影矩阵 matP 修正增量，避免系统在不可观方向上乱更新。
1263 transformTobeMapped[0] += matX.at<float>(0, 0);
1264 transformTobeMapped[1] += matX.at<float>(1, 0);
1265 transformTobeMapped[2] += matX.at<float>(2, 0);
1266 transformTobeMapped[3] += matX.at<float>(3, 0);
1267 transformTobeMapped[4] += matX.at<float>(4, 0);
1268 transformTobeMapped[5] += matX.at<float>(5, 0);将求解出来的增量加到当前位姿上。
1270 float deltaR = sqrt(
1271 pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
1272 pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
1273 pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
1274 float deltaT = sqrt(
1275 pow(matX.at<float>(3, 0) * 100, 2) +
1276 pow(matX.at<float>(4, 0) * 100, 2) +
1277 pow(matX.at<float>(5, 0) * 100, 2));
1279 if (deltaR < 0.05 && deltaT < 0.05) {
1280 return true; // converged最后判断是否收敛。如果旋转增量和平移增量都很小，就认为优化已经收敛，提前退出迭代。
十六、scan-to-map 主优化流程
1282～1311 行：scan2MapOptimization()
1282 void scan2MapOptimization()
1283 {
1284 if (cloudKeyPoses3D->points.empty())
1285 return;如果还没有关键帧地图，就没法做 scan-to-map，直接返回。
1287 if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
1288 {
1289 kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
1290 kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);只有当前帧角点和平面点数量足够，才进行优化。然后给局部角点地图和平面地图分别建立 KD-tree，方便最近邻搜索。
1292 for (int iterCount = 0; iterCount < 30; iterCount++)
1293 {
1294 laserCloudOri->clear();
1295 coeffSel->clear();
1296
1297 cornerOptimization();
1298 surfOptimization();
1299
1300 combineOptimizationCoeffs();
1301
1302 if (LMOptimization(iterCount) == true)
1303 break;
1304 }
1306 transformUpdate();最多迭代 30 次。每次迭代都重新计算角点残差和平面点残差，然后用 LM 优化更新位姿。如果 LM 已经收敛，就提前退出。最后调用 transformUpdate()，利用 IMU roll/pitch 做轻微融合，并限制姿态和 z 方向。
十七、位姿更新和约束
1312～1343 行：transformUpdate()
1312 void transformUpdate()
1313 {
1314 if (cloudInfo.imuAvailable == true)
1315 {
1316 if (std::abs(cloudInfo.imuPitchInit) < 1.4)
1317 {
1318 double imuWeight = imuRPYWeight;如果 IMU 可用，并且 pitch 没有太夸张，就使用 IMU 姿态对 LiDAR 优化得到的 roll 和 pitch 做融合。这里不融合 yaw，因为普通 IMU 的 yaw 容易漂。
1324 transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
1325 imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
1326 tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
1327 transformTobeMapped[0] = rollMid;这里用球面线性插值 slerp 融合 LiDAR 优化的 roll 和 IMU roll。
1331 transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
1332 imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
1333 tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
1334 transformTobeMapped[1] = pitchMid;同理融合 pitch。
1339 transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
1340 transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
1341 transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
1343 incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);最后对 roll、pitch 和 z 做限制，防止优化结果过大偏移。然后保存优化后的位姿，用于发布 incremental odom。
十八、关键帧判断和因子图添加
1354～1380 行：saveFrame()
1354 bool saveFrame()
1355 {
1356 if (cloudKeyPoses3D->points.empty())
1357 return true;如果还没有关键帧，第一帧必须保存。
1366 Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
1367 Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
1368 transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
1369 Eigen::Affine3f transBetween = transStart.inverse() * transFinal;计算当前帧相对于上一个关键帧的位姿变化。
1373 if (abs(roll) < surroundingkeyframeAddingAngleThreshold &&
1374 abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
1375 abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
1376 sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold)
1377 return false;如果旋转和平移变化都很小，就不保存为关键帧。这样可以避免关键帧太密，减少地图和因子图规模。
1381～1396 行：addOdomFactor()
1381 void addOdomFactor()
1382 {
1383 if (cloudKeyPoses3D->points.empty())
1384 {
1385 noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished());
1386 gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
1387 initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));第一帧添加先验因子，固定图优化的起点。注意这里平移噪声很大，说明对初始平移约束很弱；roll、pitch 相对更可信；yaw 的噪声是 M_PI*M_PI，说明 yaw 不太确定。
1388 }else{
1389 noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
1390 gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
1391 gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);
1392 gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
1393 initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
1394 }后续关键帧添加里程计因子。这个因子连接上一个关键帧和当前关键帧，约束它们之间的相对位姿变化。
1397～1476 行：addGPSFactor()
1397 void addGPSFactor()
1398 {
1399 if (gpsQueue.empty())
1400 return;
1402 if (cloudKeyPoses3D->points.empty())
1403 return;没有 GPS 或系统还没初始化时，不加 GPS 因子。
1412 if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold)
1413 return;如果当前位姿协方差已经很小，说明系统对当前位置很有信心，就没必要加 GPS。只有当 x/y 方向不确定性较大时，GPS 才参与校正。
1429 float noise_x = thisGPS.pose.covariance[0];
1430 float noise_y = thisGPS.pose.covariance[7];
1431 float noise_z = thisGPS.pose.covariance[14];
1432 if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
1433 continue;GPS 自己的协方差太大时跳过，避免坏 GPS 拉偏地图
1446 if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
1447 continue;GPS 如果是未初始化的 (0,0,0)，也跳过。
1466 noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);
1467 gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
1468 gtSAMgraph.add(gps_factor);
1470 aLoopIsClosed = true;最后添加 GPS 因子。这里把 aLoopIsClosed 置为 true，目的是触发后续对历史关键帧位姿的重新修正，虽然名字叫 loop，但 GPS 校正也会导致全局图变化。
1477～1496 行：addLoopFactor()
1477 void addLoopFactor()
1478 {
1479 if (loopIndexQueue.empty())
1480 return;没有回环约束就返回。
1482 for (int i = 0; i < (int)loopIndexQueue.size(); ++i)
1483 {
1484 int indexFrom = loopIndexQueue[i].first;
1485 int indexTo = loopIndexQueue[i].second;
1486 gtsam::Pose3 poseBetween = loopPoseQueue[i];
1487 gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
1488 gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
1489 }把回环检测线程得到的回环约束加入因子图。它本质上也是一个 BetweenFactor<Pose3>，连接当前关键帧和历史关键帧。
十九、关键帧保存和 iSAM2 优化
1497～1582 行：saveKeyFramesAndFactor()
1497 void saveKeyFramesAndFactor()
1498 {
1499 if (saveFrame() == false)
1500 return;如果当前帧变化太小，不满足关键帧条件，就不保存，也不进行新的图优化。
1503 addOdomFactor();
1506 addGPSFactor();
1509 addLoopFactor();依次添加 odom 因子、GPS 因子和回环因子。
1515 isam->update(gtSAMgraph, initialEstimate);
1516 isam->update();调用 iSAM2 更新图优化结果。第一次 update 加入新因子和新变量，第二次 update 进一步更新线性化结果。
1518 if (aLoopIsClosed == true)
1519 {
1520 isam->update();
1521 isam->update();
1522 isam->update();
1523 isam->update();
1524 isam->update();
1525 }如果发生回环或 GPS 校正，就多执行几次 update。因为回环/GPS 是全局约束，可能影响许多历史关键帧，多更新几次可以让优化结果传播得更充分。
1537 isamCurrentEstimate = isam->calculateEstimate();
1538 latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);取出当前所有关键帧的优化结果，并拿到最新关键帧位姿。
1541 thisPose3D.x = latestEstimate.translation().x();
1542 thisPose3D.y = latestEstimate.translation().y();
1543 thisPose3D.z = latestEstimate.translation().z();
1544 thisPose3D.intensity = cloudKeyPoses3D->size();
1545 cloudKeyPoses3D->push_back(thisPose3D);保存最新关键帧 3D 位置，intensity 存关键帧索引。
1547 thisPose6D.x = thisPose3D.x;
1548 thisPose6D.y = thisPose3D.y;
1549 thisPose6D.z = thisPose3D.z;
1551 thisPose6D.roll = latestEstimate.rotation().roll();
1552 thisPose6D.pitch = latestEstimate.rotation().pitch();
1553 thisPose6D.yaw = latestEstimate.rotation().yaw();
1554 thisPose6D.time = timeLaserInfoCur;
1555 cloudKeyPoses6D->push_back(thisPose6D);保存最新关键帧完整 6D 位姿和时间。
1566 pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
1567 pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
1568 pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
1569 pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
1572 cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
1573 surfCloudKeyFrames.push_back(thisSurfKeyFrame);保存当前关键帧的角点和平面点。后面局部地图构建、回环检测、保存全局地图都依赖这些关键帧点云。
二十、回环后修正历史位姿
1583～1615 行：correctPoses()
1583 void correctPoses()
1584 {
1585 if (cloudKeyPoses3D->points.empty())
1586 return;
1588 if (aLoopIsClosed == true)
1589 {
1590 laserCloudMapContainer.clear();
1592 globalPath.poses.clear();如果发生回环或 GPS 全局校正，就需要更新历史关键帧位姿。先清空局部地图缓存和轨迹。
1594 int numPoses = isamCurrentEstimate.size();
1595 for (int i = 0; i < numPoses; ++i)
1596 {
1597 cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
1598 cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
1599 cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();遍历所有关键帧，把它们的位置更新为 iSAM2 优化后的结果。
1603 cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
1604 cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
1605 cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
1607 updatePath(cloudKeyPoses6D->points[i]);同时更新 6D 姿态，并重新生成轨迹。最后把 aLoopIsClosed 置为 false。
二十一、发布 odom 和点云
1633～1703 行：publishOdometry()
1633 void publishOdometry()
1634 {
1635 // Publish odometry for ROS (global)
1636 nav_msgs::Odometry laserOdometryROS;
1637 laserOdometryROS.header.stamp = timeLaserInfoStamp;
1638 laserOdometryROS.header.frame_id = odometryFrame;
1639 laserOdometryROS.child_frame_id = "odom_mapping";
1640 laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
1641 laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
1642 laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
1643 laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
1644 pubLaserOdometryGlobal.publish(laserOdometryROS);发布全局 LiDAR odom，对应 lio_sam/mapping/odometry。
1647 static tf::TransformBroadcaster br;
1648 tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
1649 tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
1650 tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, "lidar_link");
1651 br.sendTransform(trans_odom_to_lidar);发布 odometryFrame -> lidar_link 的 TF。
1654 static bool lastIncreOdomPubFlag = false;
1655 static nav_msgs::Odometry laserOdomIncremental;
1656 static Eigen::Affine3f increOdomAffine;下面开始发布 incremental odom。这个 odom 不是全局位姿直接输出，而是根据每次 scan-to-map 优化前后的位姿增量累积出来。
1664 Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
1665 increOdomAffine = increOdomAffine * affineIncre;这里计算当前帧优化前后位姿的增量，并累积到 increOdomAffine。这个增量 odom 会被 imuPreintegration.cpp 订阅，用来做 IMU 预积分校正。
1696 if (isDegenerate)
1697 laserOdomIncremental.pose.covariance[0] = 1;
1698 else
1699 laserOdomIncremental.pose.covariance[0] = 0;
1701 pubLaserOdometryIncremental.publish(laserOdomIncremental);如果 scan-to-map 退化，就把 covariance[0] 设为 1。后面的 imuPreintegration.cpp 会读取这个标志，如果退化，就降低 LiDAR correction 的权重。
1704～1761 行：publishFrames()
1704 void publishFrames()
1705 {
1706 if (cloudKeyPoses3D->points.empty())
1707 return;
1709 publishCloud(pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);
1711 publishCloud(pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);发布关键帧轨迹和局部地图。这里局部地图只发布平面点 laserCloudSurfFromMapDS，主要用于可视化。
1713 if (pubRecentKeyFrame.getNumSubscribers() != 0)
1714 {
1715 pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
1716 PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
1717 *cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
1718 *cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
1719 publishCloud(pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);
1720 }发布当前配准后的关键帧点云，也就是把当前帧角点和平面点按照优化后的位姿转换到 map 坐标系。
1722 if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
1723 {
1724 pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
1725 pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
1726 PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
1727 *cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
1728 publishCloud(pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame);
1729 }发布当前配准后的原始去畸变点云。它比特征点云更稠密，主要用于可视化。
1731 if (pubPath.getNumSubscribers() != 0)
1732 {
1733 globalPath.header.stamp = timeLaserInfoStamp;
1734 globalPath.header.frame_id = odometryFrame;
1735 pubPath.publish(globalPath);
1736 }发布全局轨迹。
1740 if (pubSLAMInfo.getNumSubscribers() != 0)
1741 {
1742 if (lastSLAMInfoPubSize != cloudKeyPoses6D->size())
1743 {
1744 lio_sam::cloud_info slamInfo;
...
1756 pubSLAMInfo.publish(slamInfo);发布 slam_info，给第三方模块使用。里面包括关键帧点云、关键帧位姿和局部地图。
二十二、main 函数
1762～1779 行：节点启动
1762 int main(int argc, char** argv)
1763 {
1764 ros::init(argc, argv, "lio_sam");
1765
1766 mapOptimization MO;
1767
1768 ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m");
1769
1770 std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
1771 std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
1772
1773 ros::spin();
1774
1775 loopthread.join();
1776 visualizeMapThread.join();
1777
1778 return 0;
1779 }主函数初始化 ROS 节点，创建 mapOptimization 对象，然后启动两个线程：
loopthread：
后台执行回环检测。
visualizeMapThread：
后台发布全局地图可视化，并在配置允许时保存 PCD 地图。主线程执行 ros::spin()，负责处理 cloud_info、GPS、外部回环等回调。
最后详细总结
mapOptimization.cpp 是 LIO-SAM 的后端主模块，它的核心工作不是简单发布点云，而是把前端提取出来的角点和平面点真正用于定位和建图。当前帧点云进入这个文件后，首先会通过 updateInitialGuess() 获得一个比较合理的初始位姿。这个初值可能来自 IMU 姿态，也可能来自 IMU 预积分 odom。初值的作用非常大，因为 scan-to-map 优化是局部优化，如果初值离真实位姿太远，就可能最近邻搜索错误，导致匹配失败。
有了初值之后，系统会从历史关键帧里提取当前机器人附近的关键帧，组成局部地图。局部地图分为角点地图和平面点地图，分别用于点到线和点到面优化。当前帧角点会在局部角点地图里找 5 个近邻，通过特征值判断这些近邻是否形成一条线，如果成立，就构建点到线残差。当前帧平面点会在局部平面地图里找 5 个近邻，通过最小二乘拟合平面，如果平面可靠，就构建点到面残差。角点和平面点残差最终会合并到 laserCloudOri 和 coeffSel 中，送入 LMOptimization() 求解位姿增量。
LMOptimization() 是 scan-to-map 的数值优化核心。它把所有点到线、点到面的残差写成线性方程，求解 6 自由度位姿增量，然后不断更新 transformTobeMapped。第一次迭代时，它还会检查系统是否退化。如果某些方向特征值太小，说明这些方向约束不足，代码会用投影矩阵抑制这些方向的更新。比如长走廊场景中，沿走廊方向可能约束很弱，如果不做退化处理，优化结果可能在这个方向上乱飘。
scan-to-map 得到当前帧位姿后，系统并不会每一帧都保存关键帧，而是通过 saveFrame() 判断当前帧相对上一个关键帧的平移和旋转是否足够大。只有运动超过阈值，才保存成关键帧。保存关键帧时，系统会把当前位姿加入 GTSAM 因子图，同时添加 odom 因子、GPS 因子和 loop 因子。odom 因子来自连续关键帧之间的相对位姿，GPS 因子来自外部 GPS 位置约束，loop 因子来自 ICP 回环检测。iSAM2 会把这些约束统一优化，得到全局一致的关键帧轨迹。
回环部分是这个文件的另一个重点。系统会在后台线程里周期性检测回环。它先通过外部回环结果或距离搜索找候选历史关键帧，然后构建当前关键帧点云和历史关键帧局部点云，用 ICP 做几何验证。如果 ICP 收敛并且 fitness score 合格，就把当前关键帧和历史关键帧之间的相对位姿作为回环因子加入 GTSAM。回环因子加入后，iSAM2 会重新调整历史关键帧位姿，correctPoses() 会根据优化结果更新 cloudKeyPoses3D 和 cloudKeyPoses6D，并清空局部地图缓存，重新生成轨迹。这样地图中的累计漂移就会被拉回。
发布部分也很关键。publishOdometry() 会发布两个 odom：一个是全局 mapping odom，即 lio_sam/mapping/odometry；另一个是 incremental odom，即 lio_sam/mapping/odometry_incremental。后者会被 imuPreintegration.cpp 接收，用来校正 IMU 预积分。也就是说，mapOptimization.cpp 和 imuPreintegration.cpp 是互相配合的：IMU 预积分给 mapping 提供初值，mapping 优化后再给 IMU 预积分提供 LiDAR correction。这个闭环让系统既有 IMU 的高频连续性，也有 LiDAR scan-to-map 的稳定校正能力。
整体来看，mapOptimization.cpp 的主线就是：用 IMU/odom 提供初值，用局部地图做 scan-to-map 匹配，用 LM 优化当前帧位姿，用 GTSAM 管理关键帧图，用 GPS 和回环修正全局漂移，最后发布 odom、轨迹和地图。 这个文件基本就是 LIO-SAM 的“后端大脑”，定位、建图、关键帧、回环、GPS 融合和地图输出都集中在这里。
版权声明： 辛苦码字不易，转载请注明原文出处和作者信息，谢谢理解！
欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。
智能硬件社区聚焦AI智能硬件技术生态，汇聚嵌入式AI、物联网硬件开发者，打造交流分享平台，同步全国赛事资讯、开展 OPC 核心人才招募，助力技术落地与开发者成长。
更多推荐
8
0- 0
已为社区贡献1条内容
所有评论(0)