4、视觉里程计（VO）：2D-2D对极几何、3D-2D PnP、3D-3D ICP，以及三角化
视觉里程计，说白了就是让机器人通过摄像头知道自己动了多少。我刚开始做SLAM那会儿，觉得这东西玄乎得很——几帧图像就能算出运动？后来亲手调过几轮代码，才发现核心就那么几个几何关系。
今天咱们就把VO里最关键的四个模块捋清楚：2D-2D对极几何、3D-2D PnP、3D-3D ICP，还有三角化。每个我都会结合项目里的坑来讲。
核心逻辑图：下面这张图是我自己画的知识脉络，建议你先扫一眼，再往下看细节。
4.1 2D-2D对极几何：初始化阶段的看家本领
先说说对极几何。这玩意儿是VO初始化的核心。你想想看，刚开机的时候，我们没有任何3D信息，只有两帧图像上的2D匹配点。怎么算出相机运动？
对极几何告诉我们：两帧图像之间，存在一个本质矩阵E或者基础矩阵F。通过匹配点对，我们可以解出这个矩阵，然后分解出旋转R和平移t。
我的经验：在实际项目中，我建议你用本质矩阵E而不是基础矩阵F。为什么？因为E只需要内参已知的相机，而F需要未知内参。咱们做SLAM的，相机内参一般都标定好了，直接用E更稳定。我曾经在一个项目中用了F，结果平移方向老是反的，折腾了两天才发现是内参没传进去。
具体步骤其实不复杂：
- 特征匹配：ORB、SIFT或者光流法，找到两帧之间的匹配点对
- 求解本质矩阵：用五点法或者八点法（我一般用五点法，更鲁棒）
- 分解R,t：对E做SVD分解，得到四组解，用三角化筛选出正确的那组
// 伪代码：对极几何求解R,t
cv::Mat E = cv::findEssentialMat(points1, points2, K, cv::RANSAC);
cv::Mat R, t;
cv::recoverPose(E, points1, points2, K, R, t);
// 注意：这里的t是单位向量，尺度不确定！
⚠️ 避坑指南：我曾经在一个室内小场景里，对极几何算出来的平移方向完全反了。后来发现是匹配点质量太差——纯旋转运动下，对极几何会退化。记住：对极几何要求有足够的平移，纯旋转是解不出来的。
4.2 三角化：从2D到3D的桥梁
有了两帧之间的R,t，我们就可以做三角化了。说白了，就是利用两帧的相机位姿，把同一个3D点的两条视线相交，算出它的空间坐标。
嗯，这里要注意：三角化不是万能的。如果两个相机之间的平移太小，或者点在极线方向上，三角化的深度误差会非常大。
核心公式：对于匹配点x1和x2，以及已知的投影矩阵P1和P2，三角化就是求解线性方程组：
x1 × (P1 * X) = 0
x2 × (P2 * X) = 0
// 用SVD求解齐次方程，得到3D点X我个人的习惯是，三角化之后一定要做一步深度检查：如果算出来的深度是负数，或者深度值异常大（比如超过100米），直接丢掉这个点。这些点往往是误匹配，留着只会污染后续的优化。
4.3 3D-2D PnP：后续帧的主力
初始化完成之后，我们有了3D地图点。后续每一帧新图像，我们只需要做3D-2D匹配——把已知的3D点投影到当前帧，找到对应的2D特征，然后求解当前帧的位姿。
这就是PnP（Perspective-n-Point）问题。常用的方法有：
- EPnP：效率高，适合在线使用。我大部分项目都用它
- DLT：直接线性变换，简单但精度一般
- BA优化：在PnP之后，用非线性优化微调位姿和3D点
// 实际项目中的PnP调用
cv::Mat rvec, tvec;
cv::solvePnP(objectPoints, imagePoints, K, distCoeffs, rvec, tvec, false, cv::SOLVEPNP_EPNP);
// 然后可以用高斯牛顿或者LM做一步优化
我的建议：别直接用solvePnP的结果就完事了。我习惯在它后面加一步重投影误差检查——把3D点投影回图像，如果误差超过2个像素，就剔除这个点。然后再做一次优化。这样位姿精度能提升不少。
4.4 3D-3D ICP：当你有深度数据时
如果你用的是RGB-D相机或者激光雷达，那就有3D-3D的匹配数据了。这时候用ICP（Iterative Closest Point）是最直接的。
ICP的核心思想很简单：找到两组点云之间的旋转和平移，使得对应点的距离之和最小。
但ICP有个大坑——它需要好的初始值。如果初始位姿差得太远，ICP会陷入局部最优。我遇到过好几次，ICP把点云对齐到完全错误的位置，就是因为初始值给得太随意。
⚠️ 避坑指南：我曾经在一个AGV项目里，直接用单位矩阵作为ICP的初始值，结果点云对齐后，机器人位置偏移了半米。后来我改用PnP的结果作为ICP的初始值，问题就解决了。记住：ICP不是万能的，它需要好的初值。
常用的ICP变体：
- 点对点ICP：最小化对应点之间的距离，适合稠密点云
- 点对面ICP：最小化点到平面的距离，收敛更快，适合平面场景
- 广义ICP：结合了概率模型，鲁棒性更好
4.5 四种方法的选型指南
说了这么多，到底什么时候用哪个？我整理了一个表格，方便你对照：
| 方法 | 输入数据 | 输出 | 适用场景 | 注意事项 |
|---|---|---|---|---|
| 2D-2D对极几何 | 两帧2D匹配点 | R,t（尺度不确定） | 初始化、纯视觉 | 需要足够平移，纯旋转失效 |
| 三角化 | 2D匹配点+位姿 | 3D空间点 | 初始化后建图 | 小平移时深度误差大 |
| 3D-2D PnP | 3D点+2D投影 | R,t（尺度确定） | 后续帧跟踪 | 需要好的3D-2D匹配 |
| 3D-3D ICP | 两组3D点云 | R,t（精确对齐） | RGB-D/激光SLAM | 需要好的初始值 |
我个人在实际项目中的流程是这样的：
- 初始化阶段：用2D-2D对极几何 + 三角化，建立初始地图
- 跟踪阶段：每来一帧新图像，先用PnP估计位姿，再用三角化补充新3D点
- 如果有深度数据：在PnP之后，用ICP做一次精配准，提高精度
- 定期做局部BA：把最近几帧的位姿和3D点一起优化，消除累积误差
最后说一句：视觉里程计的核心就是几何约束。不管用哪种方法，本质上都是在利用图像之间的几何关系来反推运动。你把这个本质抓住了，代码怎么写都不会跑偏。
公众号:蓝海资料掘金营，微信deep3321