4、点云地图优化：回环检测与图优化
点云地图建完了，看着挺漂亮。但别高兴太早。
我见过太多团队，建图时跑得顺风顺水，一拿到实际场景就崩了。为什么？因为里程计漂移。说白了，你走一圈回来，起点和终点对不上。这就是所谓的「闭环误差」。
这一章，我们就来解决这个问题。用回环检测找到误差，用图优化把它抹平。
4.1 回环检测：找到「走歪了」的证据
回环检测，名字听着高大上，其实就一件事：判断机器人是否回到了之前去过的地方。
你想想看，如果我能确认当前位置和之前某个位置是同一个地方，那我就能算出这两帧之间的相对位姿。这个相对位姿，就是约束。有了约束，就能修正整条轨迹。
4.1.1 基于ICP的回环检测
最直接的方法：把当前帧和候选帧做ICP配准。如果配准得分很高，就认为是一个回环。
我个人习惯用点到面ICP，比点到点稳定很多。尤其是在结构化场景里，点到面ICP能利用平面法向量信息，收敛更快。
核心思路：
- 提取当前关键帧的局部点云
- 在历史关键帧中搜索空间近邻（KD-Tree）
- 用ICP计算相对位姿
- 如果匹配分数低于阈值，接受为回环
我的经验： 别只依赖ICP。我曾在一条长走廊里吃过亏——两边都是白墙，ICP直接收敛到错误位置。后来我加了视觉词袋做初筛，先确认场景相似性，再做ICP精配。效果好了很多。
4.1.2 基于Scan Context的回环检测
ICP有个毛病：依赖初始值。如果初始位姿差太远，ICP会掉进局部最优。
这时候，Scan Context就派上用场了。它把点云投影成鸟瞰图，然后提取全局描述子。匹配时直接算描述子距离，不需要初始位姿。
// Scan Context 匹配伪代码
Eigen::MatrixXd scan_context = GenerateScanContext(cloud);
int best_match_id = FindNearestInDatabase(scan_context);
double distance = CalculateDistance(scan_context, database[best_match_id]);
if (distance < THRESHOLD) {
// 发现回环
LoopCandidate candidate;
candidate.source_id = current_id;
candidate.target_id = best_match_id;
candidate.relative_pose = ICPRefine(cloud_current, cloud_target);
}注意： Scan Context对旋转敏感。如果机器人掉了个头，描述子会变。我建议在匹配时做旋转对齐，或者干脆存多个角度的描述子。
4.2 图优化：把误差「摊」到每一帧
回环检测找到了约束，接下来就是优化。图优化的本质，就是调整所有位姿，让所有约束的误差总和最小。
4.2.1 位姿图的构建
位姿图由两部分组成：
- 节点：每一帧的位姿（通常是关键帧）
- 边：两个节点之间的相对位姿约束
边有两种：
- 里程计边：相邻帧之间的约束，来自ICP或IMU预积分
- 回环边：非相邻帧之间的约束，来自回环检测
嗯，这里要注意：回环边的信息矩阵（权重）通常设得比里程计边大。因为回环约束更「可信」，它修正的是全局漂移。
4.2.2 基于GTSAM的位姿图优化
GTSAM是我个人比较喜欢的优化库。它基于因子图，接口清晰，调试方便。
// GTSAM 位姿图优化示例
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/BetweenFactor.h>
// 1. 创建因子图
NonlinearFactorGraph graph;
// 2. 添加里程计因子（相邻帧约束）
for (int i = 0; i < n_poses - 1; ++i) {
Pose3 relative_pose = odom_poses[i+1] - odom_poses[i];
auto noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << 0.05, 0.05, 0.05, 0.01, 0.01, 0.01).finished());
graph.add(BetweenFactor<Pose3>(i, i+1, relative_pose, noise));
}
// 3. 添加回环因子
for (auto &loop : loop_closures) {
auto noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << 0.01, 0.01, 0.01, 0.001, 0.001, 0.001).finished());
graph.add(BetweenFactor<Pose3>(loop.source_id, loop.target_id,
loop.relative_pose, noise));
}
// 4. 固定第一个节点（消除自由度）
Values initial;
for (int i = 0; i < n_poses; ++i) {
initial.insert(i, odom_poses[i]);
}
graph.add(PriorFactor<Pose3>(0, odom_poses[0],
noiseModel::Diagonal::Sigmas(Vector::Constant(6, 1e-6))));
// 5. 优化
LevenbergMarquardtOptimizer optimizer(graph, initial);
Values result = optimizer.optimize();避坑指南： 我曾经在优化后地图反而变差了。排查了半天，发现是信息矩阵设置不合理。回环边的噪声方差设得太小，导致优化器强行把轨迹扭曲去迎合回环约束。正确的做法是：根据回环匹配的置信度动态调整信息矩阵。
4.2.3 基于Ceres的位姿图优化
如果你不想引入GTSAM，Ceres也是个好选择。它更轻量，而且对自定义残差支持更好。
// Ceres 位姿图优化（核心残差定义）
struct PoseGraphResidual {
PoseGraphResidual(const Eigen::Vector3d &t_ab,
const Eigen::Quaterniond &q_ab)
: t_ab_(t_ab), q_ab_(q_ab) {}
template <typename T>
bool operator()(const T* const pose_a,
const T* const pose_b,
T* residuals) const {
// 计算相对位姿误差
// ... (SE3 流形上的误差计算)
return true;
}
private:
Eigen::Vector3d t_ab_;
Eigen::Quaterniond q_ab_;
};Ceres的优势在于：你可以轻松加入自定义约束。比如GPS先验、地面平面约束等。我在一个项目中就加了路面高程约束，效果出奇的好。
4.3 全局一致性调整
图优化完成后，我们得到了优化后的位姿。但点云地图还没更新。
全局一致性调整，就是把每一帧点云按照优化后的位姿重新拼接。这一步看似简单，但有几个坑：
- 体素滤波：重新拼接前，建议对每帧点云做体素滤波。不然累积的点云会非常稠密，内存爆炸。
- 离群点剔除：优化后的位姿可能有微小误差，导致同一位置出现「重影」。用统计滤波可以去掉这些离群点。
- 增量式更新：如果地图很大，别一次性全部重新拼接。可以只更新回环附近的局部区域。
我的建议： 全局一致性调整后，一定要做一次可视化检查。看看墙角是否对齐、地面是否平整。我见过一个项目，优化后地图的「数学误差」很小，但视觉上明显有错位。原因是回环检测本身就有偏差，优化器只是把偏差「摊平」了而已。
4.4 本章知识体系
下面这张图，概括了本章的核心流程：
4.5 本章小结
回环检测与图优化，是点云地图从「能用」到「好用」的关键一步。
- 回环检测帮你找到「走歪了」的证据
- 图优化帮你把误差「摊」到每一帧
- 全局一致性调整让地图看起来「舒服」
我个人觉得，这三步里最容易出问题的反而是第一步——回环检测。检测不到，后面都是白搭；检测错了，优化器会把地图搞得更乱。所以，宁缺毋滥，回环检测的阈值设严一点，宁可漏检也不要误检。
好了，这一章就到这里。下一章我们聊聊点云地图的语义标注——让地图不仅知道「哪里有墙」，还知道「墙是什么颜色、什么材质」。