【LVI-SAM】激光雷达点云地图优化LIO-SAM 之mapOptimization实现细节

1. mapOptimization 地图优化

定义 mapOptimization 类，它是一个用于地图优化的ROS节点，主要用于处理LiDAR SLAM（Simultaneous Localization and Mapping）中的地图构建和优化。以下是代码的流程和功能的详细总结：

1.1 小结

整体而言，mapOptimization 类实现了一个完整的LiDAR SLAM系统的后端，包括地图构建、优化、环路闭合和全局地图可视化。通过GTSAM库进行状态估计和优化，确保了系统的准确性和鲁棒性。

1.2 流程和功能：

1. 初始化：
   • 在构造函数中，初始化ROS订阅者、发布者和服务。
   • 设置IMU、GPS和环路闭合检测的订阅者。
   • 初始化GTSAM的ISAM2优化器和相关的因子图和值。
2. 数据处理：
   • 通过回调函数处理LiDAR点云、GPS和里程计数据。
   • 调用 laserCloudInfoHandler 处理LiDAR点云信息。
   • 调用 gpsHandler 处理GPS数据。
   • 调用 loopInfoHandler 处理环路闭合信息。
3. 地图优化：
   • 在 scan2MapOptimization 函数中，执行特征提取、点云配准和优化。
   • 使用GTSAM进行状态估计和优化，更新地图和姿态。
4. 保存关键帧和因子：
   • 在 saveKeyFramesAndFactor 函数中，保存关键帧和优化后的因子图。
5. 环路闭合：
   • 在 loopClosureThread 函数中，执行环路闭合检测和校正。
6. 全局地图可视化：
   • 在 visualizeGlobalMapThread 函数中，可视化全局地图。
7. 发布信息：
   • 发布优化后的地图、关键帧、路径和SLAM信息。

1.3 各个接口的目的：

• laserCloudInfoHandler：处理LiDAR点云信息，执行特征提取、点云配准和优化。
• gpsHandler：处理GPS数据，用于辅助定位和地图校正。
• loopInfoHandler：处理环路闭合信息，用于检测和校正环路闭合。
• saveMapService：提供服务来保存当前构建的地图。
• publishOdometry：发布优化后的里程计信息。
• publishFrames：发布关键帧和全局地图。
• publishGlobalMap：发布全局地图的可视化。
• loopClosureThread：在单独的线程中执行环路闭合。
• visualizeGlobalMapThread：在单独的线程中可视化全局地图。

2. mapOptimization 地图优化接口详细梳理

• 初始化ROS节点。
• 创建 mapOptimization 类的实例。
• 启动地图优化、环路闭合和地图可视化的线程。
• 进入ROS事件循环，处理回调函数。
• 在ROS事件循环结束后，等待所有线程结束。
  以下是将脑图改为左右格式的方向显示：

mapOptimization 类的主要方法和回调函数从左到右展开，展示了它们之间的关系和调用顺序。这样的布局有助于清晰地展示从构造函数到各个回调函数和线程的流程。

2.1 laserCloudInfoHandler接口功能和实现细节

• 激光云信息处理函数
• 该函数负责处理接收到的激光云信息，包括时间戳提取、信息和特征云提取、
• 下采样、优化、保存关键帧、修正位姿以及发布相关数据。

void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn){// 提取时间戳// 提取信息和特征云extract info and feature cloud// 加锁// 如果当前时间与上次处理时间之差大于等于地图处理间隔// 更新初始猜测updateInitialGuess();// 提取周围关键帧extractSurroundingKeyFrames();// 对当前扫描进行下采样downsampleCurrentScan();// 扫描到地图的优化scan2MapOptimization();// 保存关键帧和因子saveKeyFramesAndFactor();// 修正位姿correctPoses();// 发布里程计publishOdometry();// 发布帧publishFrames();}

2.1.1 updateInitialGuess 接口功能和实现细节

1. 接口功能：

updateInitialGuess 函数的目的是更新初始的姿态估计，以便在SLAM系统中进行优化。这个函数根据IMU数据和里程计数据来估计当前的位姿。

2. 实现流程

代码是用于更新初始位置估计的函数，它结合了IMU（惯性测量单元）和里程计（odometry）的数据来更新机器人的姿态（位置和方向）。

• 保存当前变换：首先，函数通过trans2Affine3f(transformTobeMapped)将当前待映射的变换（可能包含位置和旋转）转换为一个Eigen::Affine3f类型的变换矩阵，并将其保存在incrementalOdometryAffineFront中，以便后续处理。

• 初始化：如果cloudKeyPoses3D（一个可能保存关键帧姿态的点云）为空，说明这是第一次调用此函数。此时，它会使用IMU的初始滚转角、俯仰角和偏航角来初始化transformTobeMapped（一个可能包含位置和旋转的数组）。如果不使用IMU航向初始化，则将偏航角设置为0。然后，它会保存IMU的初始变换到lastImuTransformation中，并返回。

• 使用里程计预积分估计姿态：如果cloudInfo.odomAvailable为真，说明有可用的里程计数据。此时，它会计算从上次到当前的里程计变换（transBack），并与上一次保存的预变换（lastImuPreTransformation）进行比较，以得到增量变换（transIncre）。然后，将这个增量变换应用到当前的待映射变换上，得到最终的变换（transFinal）。最后，从这个最终的变换中提取出新的位置和旋转，并更新lastImuPreTransformation和lastImuTransformation，然后返回。

• 使用IMU增量估计姿态（仅旋转）：如果cloudInfo.imuAvailable为真但cloudInfo.odomAvailable为假，说明只有IMU数据可用。此时，它会计算从IMU初始状态到当前状态的变换（transBack），并与上次保存的IMU变换（lastImuTransformation）进行比较，以得到增量变换（transIncre）。然后，将这个增量变换（仅旋转部分）应用到当前的待映射变换上，得到最终的变换（transFinal）。最后，从这个最终的变换中提取出新的位置和旋转，并更新lastImuTransformation，然后返回。

注意几个关键点：

• trans2Affine3f和pcl::getTransformation函数是假设存在的，用于在数组和Eigen::Affine3f类型的变换矩阵之间进行转换。

• 这个函数通过结合IMU和里程计的数据来更新机器人的姿态估计，但在实际应用中，这些数据可能来源于不同的传感器或算法，并且需要进行相应的时间同步和校准。

• transformTobeMapped数组可能包含位置和旋转信息，但在这个函数中，只更新了旋转部分（当只有IMU数据时），或者同时更新了位置和旋转（当有里程计数据时）。

• 这个函数使用了静态变量来保存上一次的状态（如lastImuTransformation和lastImuPreTransformation），以便在后续调用中计算增量变换。这要求这个函数在整个程序的生命周期内被正确地调用和管理。

在更新过程中，代码只更新了transformTobeMapped中的姿态信息（即横滚、俯仰和偏航角），而没有更新位置信息（即x、y、z坐标）。这是因为在短时间内，IMU的加速度计数据（用于估计位置变化）相比激光雷达的测量来说，噪声较大且容易积分漂移。而IMU的陀螺仪数据（用于估计旋转变化）则相对更可靠，尤其是在初始化阶段，可以帮助系统快速收敛到正确的姿态。因此，在这个特定的代码段中，主要关注的是IMU的旋转估计，而不是位置估计。

总结来说，代码只更新旋转信息是因为IMU的旋转估计在短时间内更可靠，且有助于系统快速收敛到正确的姿态。

3. 输入：

• cloudInfo：包含IMU和里程计数据的结构体，包括初始的姿态估计和IMU的可用性。
• transformTobeMapped：一个浮点数组，用于存储当前的位姿变换参数。

4. 输出：

• 更新后的 transformTobeMapped 数组，包含估计的位姿变换参数。

5. 全局变量：

• incrementalOdometryAffineFront：存储当前的位姿变换。
• lastImuTransformation：存储上一次IMU变换。
• lastImuPreTransformation：存储上一次预积分的IMU变换。
• lastImuPreTransAvailable：标志，指示是否有可用的预积分IMU变换。

    void updateInitialGuess(){// save current transformation before any processingincrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);static Eigen::Affine3f lastImuTransformation;// initializationif (cloudKeyPoses3D->points.empty()){transformTobeMapped[0] = cloudInfo.imuRollInit;transformTobeMapped[1] = cloudInfo.imuPitchInit;transformTobeMapped[2] = cloudInfo.imuYawInit;if (!useImuHeadingInitialization)transformTobeMapped[2] = 0;lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;return;}// use imu pre-integration estimation for pose guessstatic bool lastImuPreTransAvailable = false;static Eigen::Affine3f lastImuPreTransformation;if (cloudInfo.odomAvailable == true){Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX,    cloudInfo.initialGuessY,     cloudInfo.initialGuessZ, cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);if (lastImuPreTransAvailable == false){lastImuPreTransformation = transBack;lastImuPreTransAvailable = true;} else {Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);Eigen::Affine3f transFinal = transTobe * transIncre;pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);lastImuPreTransformation = transBack;lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;return;}}// use imu incremental estimation for pose guess (only rotation)if (cloudInfo.imuAvailable == true){Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);Eigen::Affine3f transFinal = transTobe * transIncre;pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;return;}}

这个函数是SLAM系统中初始化位姿估计的关键步骤，它确保了在优化过程开始之前有一个合理的初始猜测。这对于优化算法的收敛速度和准确性至关重要。

2.1.2 extractSurroundingKeyFrames 提取周围关键帧

 * @brief 提取周围关键帧* 从给定的三维关键点集合中提取周围的关键帧。

输入输出

1. extractNearby 输入：
   无显式输入参数，但依赖全局变量中的点云数据和关键帧姿态信息。
2. extractNearby 输出：
   无显式返回值。处理后的点云数据存储在全局变量中，供后续过程使用。
3. extractCloud 输入：
   cloudToExtract：下采样后的关键姿态点云。
4. extractCloud 输出：
   无显式返回值。处理后的局部地图数据存储在全局变量中，供后续过程使用。

使用的全局变量
点云与关键帧相关：
pcl::PointCloud::Ptr cloudKeyPoses3D：3D关键姿态的点云。
pcl::PointCloud::Ptr cloudKeyPoses6D：6D关键姿态的点云，包含位姿和时间信息。
cornerCloudKeyFrames：存储每一帧的角点云。
surfCloudKeyFrames：存储每一帧的面点云。
kd-tree 和下采样相关：
pcl::KdTreeFLANN::Ptr kdtreeSurroundingKeyPoses：用于查找附近关键帧的kd-tree。
pcl::VoxelGrid downSizeFilterSurroundingKeyPoses：下采样过滤器，用于对周围关键帧进行下采样。
pcl::VoxelGrid downSizeFilterCorner 和 pcl::VoxelGrid downSizeFilterSurf：用于对角点和面点进行下采样。
局部地图构建相关：
laserCloudCornerFromMap 和 laserCloudSurfFromMap：用于存储提取到的角点云和面点云。
laserCloudCornerFromMapDS 和 laserCloudSurfFromMapDS：存储下采样后的角点云和面点云。
laserCloudMapContainer：缓存已经处理并变换后的局部地图点云，避免重复计算。
其他：
surroundingKeyframeSearchRadius：搜索半径。
timeLaserInfoCur：当前时间戳，用于与关键帧时间进行比较。

实现流程

1. extractNearby 实现流程：

• 创建点云容器：为周围关键姿态和下采样后的关键姿态创建点云指针。
• kd-tree 半径搜索：使用 kd-tree 对当前点进行半径搜索，提取周围关键姿态。
• 下采样处理：对提取到的周围关键姿态进行下采样，得到下采样后的关键姿态点云。
• 最近邻查找：对于下采样后的点云，通过最近邻查找，为点云中的每个点赋值强度信息。
• 提取最新关键帧：在时间差小于10秒的情况下，提取一些最新的关键帧，以应对机器人在原地旋转的情况。
• 调用 extractCloud：将提取到的周围关键姿态点云传入 extractCloud 函数，进一步处理。

2. extractCloud 实现流程：

• 清空局部地图点云：清空局部地图中的角点云和面点云。
• 提取并变换点云：
• 对每个提取到的关键姿态点，根据其索引从缓存中提取变换后的点云。
• 如果缓存中没有变换后的点云，则对该点进行点云变换，并将变换后的角点云和面点云添加到局部地图中。
• 更新缓存，避免重复计算。
• 下采样局部地图：对角点云和面点云分别进行下采样。
• 清理缓存：如果缓存大小超过1000个元素，清理缓存以节省内存。
  

void extractNearby(){// 将搜索到的点添加到周围关键姿态的点云中// 为下采样过滤器设置输入点云downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);// 执行下采样操作，并将结果保存到周围关键姿态的下采样点云中downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);for(auto& pt : surroundingKeyPosesDS->points){// 在下采样后的点云中查找最近的点kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);// 将最近点的强度赋值给当前点pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;}// 如果机器人在一个位置旋转，则也提取一些最新的关键帧// extract some latest key frames in case the robot rotates in one positionint numPoses = cloudKeyPoses3D->size();for (int i = numPoses-1; i >= 0; --i){// 如果当前关键帧的时间与当前激光信息的时间差小于10秒if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)// 将该关键帧添加到周围关键姿态的下采样点云中surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);else// 超出时间范围，跳出循环break;}// 提取周围关键姿态的点云extractCloud(surroundingKeyPosesDS);}

2.1.3 downsampleCurrentScan 对当前扫描进行下采样

 * @brief 对当前扫描的云数据进行下采样* 对当前扫描的云数据进行下采样操作。首先清空上一次下采样后的角点云数据和曲面云数据，* 然后设置下采样滤波器的输入云数据，并进行下采样操作。下采样后的结果分别存储在激光角点云数据和激光曲面云数据的下采样版本中。* 最后获取下采样后角点云数据和曲面云数据的大小。

    void downsampleCurrentScan(){// 对当前扫描的云数据进行下采样// 清空上一次下采样后的角点云数据// Downsample cloud from current scanlaserCloudCornerLastDS->clear();// 设置下采样滤波器的输入云数据为上一次角点云数据downSizeFilterCorner.setInputCloud(laserCloudCornerLast);// 进行下采样，结果存储在激光角点云数据的下采样版本中downSizeFilterCorner.filter(*laserCloudCornerLastDS);// 获取下采样后角点云数据的大小laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();// 清空上一次下采样后的曲面云数据laserCloudSurfLastDS->clear();// 设置下采样滤波器的输入云数据为上一次曲面云数据downSizeFilterSurf.setInputCloud(laserCloudSurfLast);// 进行下采样，结果存储在激光曲面云数据的下采样版本中downSizeFilterSurf.filter(*laserCloudSurfLastDS);// 获取下采样后曲面云数据的大小laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();}

2.1.4 scan2MapOptimization 扫描到地图的优化

/*** @brief 优化扫描结果到地图的映射** 如果给定的三维关键点集合为空，则直接返回。如果激光点云中的角点数量大于最小有效角点数量10，* 且平面特征数量大于最小有效平面特征数量100，则进行以下操作：* 设置 kdtreeCornerFromMap 的输入云为 laserCloudCornerFromMapDS，设置 kdtreeSurfFromMap 的输入云为 laserCloudSurfFromMapDS。* 然后进行最多 30 次迭代，每次迭代中：* 1. 清空 laserCloudOri 和 coeffSel。* 2. 调用角点优化函数 cornerOptimization。* 3. 调用平面特征优化函数 surfOptimization。* 4. 合并优化系数 combineOptimizationCoeffs。* 5. 如果 LMOptimization 函数返回 true，则跳出循环。* 最后调用 transformUpdate 更新变换。* 如果角点或平面特征数量不足，则输出警告信息。*/void scan2MapOptimization(){// 如果 cloudKeyPoses3D 的点集为空，则返回if (cloudKeyPoses3D->points.empty())return;// 如果激光点云中的角点数量大于最小有效角点数量，且平面特征数量大于最小有效平面特征数量if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum){// 设置 kdtreeCornerFromMap 的输入云为 laserCloudCornerFromMapDSkdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);// 设置 kdtreeSurfFromMap 的输入云为 laserCloudSurfFromMapDSkdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);// 迭代 30 次for (int iterCount = 0; iterCount < 30; iterCount++){// 清空 laserCloudOri 和 coeffSellaserCloudOri->clear();coeffSel->clear();// 调用角点优化函数cornerOptimization();// 调用平面特征优化函数surfOptimization();// 合并优化系数combineOptimizationCoeffs();// 如果 LMOptimization 函数返回 true，则跳出循环if (LMOptimization(iterCount) == true)break;              }// 更新变换transformUpdate();} else {// 输出警告信息，表示特征数量不足ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);}}

2.1.5 saveKeyFramesAndFactor 保存关键帧和因子

    /*** @brief 保存关键帧和因子** 该函数用于保存关键帧和相关的因子信息。* 如果保存帧失败，则直接返回。* 接着，函数会添加里程计因子、GPS因子和回环因子。* 然后，函数会更新ISAM并进行关键帧的保存和姿态的提取。* 最后，函数会将更新的变换和接收到的边缘和曲面点保存到相应的变量中。*/void saveKeyFramesAndFactor(){// odom factor添加里程计因子到因子图中，这些因子通常表示机器人相邻时刻间的运动约束。addOdomFactor();// gps factor 如果可用，添加GPS因子到因子图中，这些因子提供了全局位置约束。addGPSFactor();// loop factor如果检测到闭环（即机器人回到了之前访问过的地方），则添加闭环因子。addLoopFactor();// update iSAMisam->update(gtSAMgraph, initialEstimate);isam->update();if (aLoopIsClosed == true){isam->update();}isamCurrentEstimate = isam->calculateEstimate();latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);// cout << "****************************************************" << endl;// isamCurrentEstimate.print("Current estimate: ");poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);// save updated transform// save all the received edge and surf pointspcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());pcl::copyPointCloud(*laserCloudCornerLastDS,  *thisCornerKeyFrame);pcl::copyPointCloud(*laserCloudSurfLastDS,    *thisSurfKeyFrame);// save key frame cloudcornerCloudKeyFrames.push_back(thisCornerKeyFrame);surfCloudKeyFrames.push_back(thisSurfKeyFrame);// save path for visualizationupdatePath(thisPose6D);}

1. 功能概述：

该函数是SLAM系统中的关键部分，负责保存关键帧、更新因子图，并使用GTSAM库进行状态估计和优化。

2. 输入：

• cloudKeyPoses3D：存储关键帧的3D位置信息的点云。
• cloudKeyPoses6D：存储关键帧的6D位置和姿态信息的点云。
• gtSAMgraph：当前的因子图。
• initialEstimate：初始估计值。

3. 输出：

• 更新后的因子图和状态估计。
• 保存的关键帧和路径信息。

4. 全局变量：

• isam：GTSAM的ISAM2对象，用于状态估计和优化。
• poseCovariance：存储最新姿态估计的协方差矩阵。
• transformTobeMapped：存储最新姿态变换参数的数组。

5. 实现流程：

1. 保存关键帧检查：

• 调用 saveFrame() 函数检查是否可以保存当前帧。如果返回 false，则不保存并返回。

2. 添加因子：

• addOdomFactor()：添加里程计因子，表示机器人相邻时刻间的运动约束。
• addGPSFactor()：添加GPS因子，提供全局位置约束。
• addLoopFactor()：添加闭环因子，用于处理环路闭合检测。

3. 更新ISAM：

• 使用 isam->update(gtSAMgraph, initialEstimate) 更新ISAM后端。
• 如果检测到闭环，多次调用 isam->update() 以加强闭环的约束。

4. 清空因子图和初始估计：

• 清空因子图和初始估计，为下一次迭代做准备。

5. 保存关键姿态：

• 从ISAM后端获取最新的姿态估计，并保存为3D和6D姿态。

6. 保存姿态协方差：

• 获取最新姿态的协方差矩阵，并保存。

7. 保存变换：

• 保存最新姿态的旋转和平移参数。

8. 保存点云：

• 复制最新的角点云和曲面点云，并添加到相应的容器中。

9. 更新路径：

• 更新可视化路径，显示机器人的移动轨迹。

2.1.6 correctPoses 修正位姿

 * @brief 校正位姿* 如果检测到闭环，则清除地图缓存和路径，并更新关键帧位姿。* 如果关键帧位姿为空，则直接返回。

    void correctPoses(){// 如果检测到闭环，执行以下操作// 清除地图缓存// clear map cachelaserCloudMapContainer.clear();// 清除路径// clear pathglobalPath.poses.clear();// 更新关键帧位姿// update key posesint numPoses = isamCurrentEstimate.size();// 更新3D位姿cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();// 更新6D位姿// 更新路径updatePath(cloudKeyPoses6D->points[i]);}

2.1.7 publishOdometry 发布里程计

@brief 发布里程计信息
将里程计信息发布到ROS中，包括全局里程计和增量里程计。
全局里程计将里程计信息转换为nav_msgs::Odometry消息，并发布到全局坐标系下。
增量里程计根据当前变换矩阵与上一次发布的变换矩阵计算得到，并发布到ROS中。

2.1.8 publishFrames 发布帧

@brief 发布帧
如果 cloudKeyPoses3D 中的点集为空，则直接返回。
发布关键帧姿态，发布周围的关键帧，发布已注册的关键帧，发布已注册的高分辨率原始点云，发布路径，
发布 SLAM 信息以供第三方使用。

2.2