视觉SLAM基础：特征点法（ORB特征）、对极几何、PnP、ICP
做SLAM这么多年，我越来越觉得，视觉SLAM就像是在玩一场「拼图游戏」。你拿着相机在未知环境里走一圈，每张图片都是一块拼图碎片。怎么把这些碎片拼起来？靠的就是特征点法、对极几何、PnP和ICP这些核心工具。
今天咱们就把这四个东西掰开揉碎了讲。嗯，先从最基础的特征点说起。
ORB特征：又快又稳的「视觉指纹」
你想想看，两张图片里怎么找到同一个点？总不能一张一张像素去比对吧。我们需要一种「视觉指纹」——特征点。ORB就是目前最流行的方案之一。
ORB全称是Oriented FAST and Rotated BRIEF。说白了，它干了两件事：
- 用FAST算法找角点——快速定位那些「与众不同的像素」
- 用BRIEF算法算描述子——给每个角点生成一串「二进制指纹」
我个人习惯用ORB而不是SIFT或SURF，原因很简单：实时性。在嵌入式设备上，ORB能跑到30帧以上，而SIFT可能只有5帧。做SLAM，实时性就是生命线。
核心要点：ORB特征具有旋转不变性和尺度不变性。旋转不变性靠的是「灰度质心法」算主方向，尺度不变性靠的是「图像金字塔」。
我曾经在一个室内机器人项目里吃过亏。当时直接用OpenCV默认参数提取ORB特征，结果在纹理稀疏的走廊里，特征点数量不够，导致后续匹配失败。后来我调整了FAST阈值，从默认的20降到10，问题就解决了。
避坑指南：ORB的FAST阈值不要死板。纹理丰富场景用20-30，纹理稀疏场景用5-10。我一般会写个自适应阈值函数，根据图像梯度均值动态调整。
对极几何：两张图之间的「几何约束」
有了特征点匹配，接下来要回答一个问题：相机在两张图之间动了多少？
对极几何就是干这个的。它描述了同一场景点在两幅图像上的投影关系。核心公式就一个：
x₂ᵀ · F · x₁ = 0其中F是基础矩阵，x₁和x₂是匹配点对。这个公式告诉我们：左图上的一个点，在右图上的匹配点一定位于一条直线上——这就是「极线约束」。
我记得刚入行时，总觉得对极几何很抽象。后来在调试一个双目视觉项目时，我亲手画出了极线，才真正理解它的物理意义。说白了，它把二维搜索降成了一维搜索，大大减少了匹配的计算量。
注意：对极几何只适用于「纯旋转」或「平移+旋转」的情况。如果相机只有旋转没有平移，对极几何会退化，无法恢复深度信息。我在做全景拼接时就踩过这个坑。
实际应用中，我们通常用8点法或5点法求解基础矩阵F。5点法更稳定，但计算复杂一些。我个人推荐用RANSAC+8点法，鲁棒性足够好。
PnP：从3D到2D的「定位神器」
对极几何解决的是「两张2D图之间」的关系。但如果我们已经知道一些3D点的位置，想求相机的位置呢？这时候就要用PnP了。
PnP的全称是Perspective-n-Point。输入是n个3D点及其在图像上的2D投影，输出是相机的位姿（旋转R和平移t）。
常用的解法有：
- P3P——只用3个点，速度快但容易受噪声影响
- EPnP——用n个点，精度更高，我比较常用
- DLT——直接线性变换，简单粗暴
实战经验：在视觉SLAM中，PnP通常用于「跟踪阶段」。我们已经有了前一帧的3D地图点，通过特征匹配找到当前帧的2D点，然后用PnP求出当前帧的位姿。这个过程每帧都要做，所以效率很重要。
我曾经在一个AR项目中，用EPnP配合RANSAC，在手机上实现了60fps的实时位姿估计。关键点在于：RANSAC的迭代次数不要设太高，50次就够了，再多就是浪费。
// 伪代码示例：EPnP求解相机位姿
cv::Mat rvec, tvec;
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec,
false, cv::SOLVEPNP_EPNP);ICP：点云配准的「终极武器」
最后说说ICP。如果说PnP是「3D-2D」的匹配，那ICP就是「3D-3D」的匹配。
ICP的全称是Iterative Closest Point。它的核心思想很简单：不断迭代，让两组点云之间的距离最小化。
在视觉SLAM中，ICP常用于：
- RGB-D SLAM——直接用深度图生成3D点云，然后配准
- 激光雷达与视觉融合——把视觉重建的稀疏点云和激光雷达的稠密点云对齐
ICP的数学本质是最小二乘优化。目标函数是：
min Σ || R·p_i + t - q_i ||²其中p_i是源点云中的点，q_i是目标点云中最近的点。R和t就是我们要找的旋转和平移。
实用技巧：ICP对初始值非常敏感。如果初始位姿偏差太大，很容易陷入局部最优。我一般先用PnP或里程计给一个粗略的初始值，再用ICP做精调。这样既快又准。
另外，ICP的变种很多。Point-to-Point ICP适合噪声小的场景，Point-to-Plane ICP适合有平面结构的场景。我在做室内建图时，通常用Point-to-Plane，因为室内环境墙壁多，收敛更快。
知识体系总览
说了这么多，我画了一张图帮你理清思路：
这张图展示了视觉SLAM前端的核心流程。从左到右看：先提取ORB特征，然后通过特征匹配建立对应关系。如果是初始化阶段，用对极几何求初始位姿；如果是跟踪阶段，用PnP求当前帧位姿；如果有深度信息，还可以用ICP做精配准。
这四个方法不是孤立的。在实际系统中，它们经常配合使用。比如在ORB-SLAM中，初始化用对极几何，跟踪用PnP，局部优化用ICP的思想做BA（Bundle Adjustment）。
总结一下：特征点法是视觉SLAM的基石，ORB是特征点的「最佳拍档」，对极几何、PnP、ICP是求解位姿的「三驾马车」。把这四个东西吃透了，视觉SLAM的入门就算完成了80%。
好了，这一章就到这里。下一章我们会深入讲一讲光流法和直接法，看看它们和特征点法有什么不同，又该怎么选型。