0. 简介
精确的长期有效定位是机器人实现自主导航并与变化环境互动的关键，特别是在大规模、地下和工业探索等场景中。终生定位可以被视为一个多会话建图问题，其中给定的中央会话（例如图 1 中的会话 A）不断被利用和更新，同时为其他附属会话（例如图 1 中的会话 B 和 C）提供长期定位能力。尽管基于LiDAR的和基于结构化的一般 SLAM 算法在单一会话建图中表现良好，但由于它们在处理多会话环境中的空间和时间相关性方面能力有限，将这些方法扩展到长期定位中仍然具有挑战性。为了应对这些局限性，本文介绍一种多功能的基于图优化的长期定位框架，支持灵活的模式切换机制，称为 LiLoc。该框架由四个模块组成：自适应子图拼接、位姿初始化、自中心因子图（EFG）和模式切换。自适应子图拼接策略旨在实现中央会话与附属会话之间的交互，动态管理（生成、选择和更新）中央会话的先验子图，减少系统内存消耗，同时保留先验知识的时效性。位姿初始化为附属会话提供准确的初始位姿，作为随后的基于图优化的估计的先验值。自中心因子图（EFG）以集中方式构建，紧密集成了 IMU 预积分、LiDAR 里程计和扫描匹配因子。特别是在处理扫描匹配因子时，EFG 引入了一种创新的传播模型。与其他基于因子图的方法=相比，该传播模型通过将当前扫描与先前地图之间的相对位姿作为边，传递到相关的先前位姿节点，并根据关键帧配准误差加权噪声。通过联合优化先验和当前因子图，这种方法不仅保留了先验约束对位姿估计的限制影响，还减轻了先验约束不确定性对精度的负面影响。与此同时，基于重叠的模式切换机制允许灵活的定位模式，使得该系统在多会话任务中更加有效。相关代码在Github上已经开源了
1. 自适应子图拼接
这部分算法作者写的比较零散，在dataLoader.hpp和anchorOptimize.hpp都有涉及，分别是来管理重定位模式下G2O的初次加载以及在mapOptmization.cpp条件下的保存重定位关键帧中使用
void addNewPrior() {
static PointTypePose lastPose; // 上一个姿态
static int end = priorSession_->KeyPoses6D_->size(); // 记录之前关键位姿数量的变量
static int count = 0; // 用于计算添加次数
if (first_add) {
// 如果是第一次添加优先节点
curPose_.intensity = priorSession_->KeyPoses6D_->size(); // 当前姿态强度设置为当前关键点数量
priorSession_->KeyPoses6D_->push_back(curPose_); // 添加当前姿态
CloudPtr copy(new Cloud());
pcl::copyPointCloud(*curCloud_, *copy); // 拷贝当前点云数据到新的拷贝
priorSession_->keyCloudVec_.push_back(copy); // 将拷贝的点云添加入会话
int this_node_id = genGlobalNodeIdx(priorSession_->index_, (int)curPose_.intensity); // 生成全局节点索引
gtsam::Pose3 poseFrom = isamCurrentEstimate_.at<gtsam::Pose3>(this_node_id - 1); // 获取上一个状态估计
gtsam::Pose3 poseTo = pclPointTogtsamPose3(curPose_); // 将点云转换为GTSAM Pose3格式的目标姿态
initialEstimate_.insert(this_node_id, poseTo); // 插入初始估计
gtsam::Pose3 poseRel = poseFrom.between(poseTo); // 计算两个姿态之间的相对偏差
gtSAMgraph_.add(gtsam::BetweenFactor<gtsam::Pose3>(this_node_id - 1, this_node_id, poseRel, odomNoise_)); // 在图中添加边（因子）
increNodePtIds_.push_back(curPose_.intensity); // 增加新节点ID到更新列表，这是增量节点的ID列表
count++; // 更新添加计数
first_add = false; // 后续添加将不再被视为第一次添加
lastPose = curPose_; // 更新最后的姿态
} else {
// 如果不是第一次添加，即发现当前点在searchNearestSubMapAndVertex中关键点数量都超过了十个，则重新创建子图
Eigen::Affine3f transStart = pclPointToAffine3f(lastPose); // 将上一位姿转化为仿射变换
Eigen::Affine3f transFinal = pclPointToAffine3f(curPose_); // 将当前位姿转化为仿射变换
Eigen::Affine3f transBetween = transStart.inverse() * transFinal; // 计算两次变换间的相对变换
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw); // 从变换中提取平移和旋转角度
if (abs(roll) < surroundingkeyframeAddingAngleThreshold && // 判断与上一帧的姿态变化是否在阈值范围内
abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
sqrt(x * x + y * y + z * z) < surroundingkeyframeAddingDistThreshold)
{
return ; // 若变化小，直接返回，不添加新优先节点
}
curPose_.intensity = priorSession_->KeyPoses6D_->size(); // 更新当前姿态强度
priorSession_->KeyPoses6D_->push_back(curPose_); // 添加到关键位姿集合
CloudPtr copy(new Cloud());
pcl::copyPointCloud(*curCloud_, *copy); // 拷贝当前点云
priorSession_->keyCloudVec_.push_back(copy); // 存入会话
int this_node_id = genGlobalNodeIdx(priorSession_->index_, (int)curPose_.intensity); // 生成当前节点ID
gtsam::Pose3 poseFrom = isamCurrentEstimate_.at<gtsam::Pose3>(this_node_id - 1); // 获取前一节点姿态
gtsam::Pose3 poseTo = pclPointTogtsamPose3(curPose_); // 转换为GTSAM的Pose3格式
initialEstimate_.insert(this_node_id, poseTo); // 插入初始估计
gtsam::Pose3 poseRel = poseFrom.between(poseTo); // 计算相对姿态差
gtSAMgraph_.add(gtsam::BetweenFactor<gtsam::Pose3>(this_node_id - 1, this_node_id, poseRel, odomNoise_)); // 加入图优化过程
increNodePtIds_.push_back(curPose_.intensity); // 更新节点ID列表
count++; // 增加计数器
first_add = false; // 设置标志以避免再次复用此逻辑
lastPose = curPose_; // 更新lastPose为当前姿态
if (count % priorSession_->submap_size == 0) {
// 检查统计数量是否达到子地图大小，认为需要10个，然后就生成一个子地图
CloudPtr submap(new Cloud()); // 创建新的子地图
TrajectoryPtr vertexCloud(new Trajectory()); // 保存对应顶点集
float xx = 0.0, xy = 0.0, xz = 0.0;
for (int i = end; i < priorSession_->keyCloudVec_.size(); i++) {
// 遍历从end开始的新加入的关键位姿
*submap += *transformPointCloud(priorSession_->keyCloudVec_[i], &priorSession_->KeyPoses6D_->points[i]); // 变换并合并到子地图
xx += priorSession_->KeyPoses6D_->points[i].x; // 聚合所有X坐标
xy += priorSession_->KeyPoses6D_->points[i].y; // 聚合所有Y坐标
xz += priorSession_->KeyPoses6D_->points[i].z; // 聚合所有Z坐标
vertexCloud->push_back(priorSession_->KeyPoses6D_->points[i]); // 把当前顶点推入数组
}
PointTypePose centeriod; // 中心点实例
centeriod.x = xx / (float)priorSession_->submap_size; // 计算中心点
centeriod.y = xy / (float)priorSession_->submap_size;
centeriod.z = xz / (float)priorSession_->submap_size;
priorSession_->SubMapCenteriod_->push_back(centeriod); // 存储到子图中心点集合
priorSession_->subMapVertexCloudVec_.push_back(vertexCloud); // 推入存储结构
CloudPtr submap_copy(new Cloud());
downSizeFilterSurf.setLeafSize(0.2, 0.2, 0.2); // 设置下采样过滤参数
downSizeFilterSurf.setInputCloud(submap); // 设置输入子图点云
downSizeFilterSurf.filter(*submap); // 执行过滤操作
pcl::copyPointCloud(*submap, *submap_copy); // 复制处理后的子图
priorSession_->subMapCloudVec_.push_back(submap_copy); // 添加到子图集合
// *priorSession_->globalMap_ += *submap_copy; // 全局地图更新，可选项（注释掉了）
end = priorSession_->KeyPoses6D_->size(); // 更新结束位置为当前
count = 0; // 重置计数器
submap->clear(); // 清空当前子图
}
}
}
而文中也提到在 ILM，则生成新子图，在mapOptmization.cpp也明确显示了两个函数，并根据公式（2）更新先前子图数据库
scan2MapOptimization(); // 执行扫描到地图的优化过程
double t1 = time.toc("1"); // 统计第一阶段时间
reg_time.push_back(t1); // 记录注册阶段时间
if (mode == ModeType::RELO) {
saveRELOKeyFramesAndFactor(); // 如果是RELO模式，则保存关键帧和因子
}
else if (mode == ModeType::LIO) {
saveLIOKeyFramesAndFactor(); // 如果是LIO模式，则保存关键帧和因子
}
ILM的相关代码如下，首先会在上一帧根据GTSAM在saveLIOKeyFramesAndFactor函数中将cloudKeyPoses3D和surfCloudKeyFrames这些压入数组中，而这个初始位置就是在laserCloudInfoHandler中使用点云匹配提供的一个初始位置
让后主要的优化还是在scan2MapOptimization这个函数下，然后输入的数据在downsampleCurrentScan函数下计数，确保有足够的点来完成scan-to-map的逻辑，而数据是从extractCloud函数拿到的laserCloudSurfFromMap指的是根据搜索来得到的附近帧。最后得到一个比较优的相对位移。
// 保存帧信息
bool saveFrame() {
if (cloudKeyPoses3D->points.empty()) // 确认有效点集
return true;
Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
Eigen::Affine3f transBetween = transStart.inverse() * transFinal; // 得到两个坐标变换之间的关系
float x, y, z, roll, pitch, yaw; // 位姿各向量初始化
pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw); // 转回到欧拉角表达形式
// 验证跟进（添加）阈值
if (abs(roll) < surroundingkeyframeAddingAngleThreshold &&
abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
sqrt(x * x + y * y + z * z) < surroundingkeyframeAddingDistThreshold)
return false; // 返回false，不产生其他关键帧
return true; // 可以形成新的帧
}
// 添加里程计因子
void addOdomFactor() {
if (cloudKeyPoses3D->points.empty()) {
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished()); // 参数设定
gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise)); // 为 GTSAM 图添加初始先验
initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped)); // 初始化插入
writeVertex(0, trans2gtsamPose(transformTobeMapped), vertices_str); // 写入顶点文件型式
}
else {
// 对于普通的增量因子
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);
gtsam::Pose3 poseRel = poseFrom.between(poseTo);
initialEstimate.insert(cloudKeyPoses3D->size(), poseTo); // 写入最大尺寸
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size(), poseRel, odometryNoise)); // 新增影响因子至图中
writeVertex(cloudKeyPoses3D->size(), poseTo, vertices_str);
writeEdge({
cloudKeyPoses3D->size() - 1, cloudKeyPoses3D->size()}, poseRel, edges_str); // 边信息写入
}
}
// 向扫描映射优化过程
void scan2MapOptimization() {
if (cloudKeyPoses3D->points.empty()) // 如果未读取姿态数据则退出
return;
if (laserCloudSurfLastDSNum > 30) {
// 至少需要有一定数量的数据
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS); // 设置输入源
for (int iterCount = 0; iterCount < 20; iterCount++) {
// 最大迭代次数限制
laserCloudOri->clear(); // 清空目标集合
coeffSel->clear(); // 清空参数保存集合
surfOptimization(); // 执行表面优化操作
combineOptimizationCoeffs(); // 合并优化得到的系数
if (LMOptimization(iterCount) == true) // 查找最佳估计
break;
}
transformUpdate(); // 更新变换参数
}
else {
ROS_WARN("Not enough features! Only %d planar features available.", laserCloudSurfLastDSNum); // 报警玄机
}
}
// 保存LIO关键帧和因子
void saveLIOKeyFramesAndFactor() {
if (saveFrame() == false) // 检查是否成功保存帧
return;
ros_time_tum.push_back(timeLaserInfoCur);
// 添加新的里程计因子
addOdomFactor();
// 更新iSAM实时系统
isam->update(gtSAMgraph, initialEstimate);
isam->update(); // 执行一次高效的精确化更新
gtSAMgraph.resize(0); // 清空图以便下一次使用
initialEstimate.clear(); // 清空初始估计
// 保存关键位姿
PointType thisPose3D; // 当前三维位姿定义
PointTypePose thisPose6D; // 当前六维位姿定义（包含旋转信息）
Pose3 latestEstimate; // 最新的位姿预测
isamCurrentEstimate = isam->calculateEstimate(); // 计算当前的位姿估计
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1); // 获取最新的位姿估计
thisPose3D.x = latestEstimate.translation().x(); // 获取x坐标
thisPose3D.y = latestEstimate.translation().y(); // 获取y坐标
thisPose3D.z = latestEstimate.translation().z(); // 获取z坐标
thisPose3D.intensity = cloudKeyPoses3D->size(); // 用作索引，表示当前关键点数量
cloudKeyPoses3D->push_back(thisPose3D); // 将当前三维位姿添加到关键帧列表中
thisPose6D.x = thisPose3D.x; // 设置六维位姿的x坐标
thisPose6D.y = thisPose3D.y; // 设置六维位姿的y坐标
thisPose6D.z = thisPose3D.z; // 设置六维位姿的z坐标
thisPose6D.intensity = thisPose3D.intensity; // 六维位姿的强度
thisPose6D.roll = latestEstimate.rotation().roll(); // 获取滚转角
thisPose6D.pitch = latestEstimate.rotation().pitch(); // 获取俯仰角
thisPose6D.yaw = latestEstimate.rotation().yaw(); // 获取偏航角
thisPose6D.time = timeLaserInfoCur; // 当前时间戳
cloudKeyPoses6D->push_back(thisPose6D); // 添加六维位姿到关键帧列表中
poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size() - 1); // 计算并存储位姿协方差
// 保存变换参数
transformTobeMapped[0] = latestEstimate.rotation().roll(); // 滚转角
transformTobeMapped[1] = latestEstimate.rotation().pitch(); // 俯仰角
transformTobeMapped[2] = latestEstimate.rotation().yaw(); // 偏航角
transformTobeMapped[3] = latestEstimate.translation().x(); // x坐标
transformTobeMapped[4] = latestEstimate.translation().y(); // y坐标
transformTobeMapped[5] = latestEstimate.translation().z(); // z坐标
// 获取表面点云
pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>()); // 创建新的点云对象
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame); // 拷贝上一个层面点云数据
surfCloudKeyFrames.push_back(thisSurfKeyFrame); // 添加到关键框架列表中
updatePath(thisPose6D); // 更新路径
}
2. 粗到精的位姿初始化
