3. 数学基础（下）：针孔相机模型、畸变模型、双目视觉模型、PnP问题与ICP算法简介

好，咱们接着聊。上一节我们把旋转矩阵、四元数这些“玩具”拆了一遍，这一节终于要上真家伙了——相机到底是怎么看到世界的？

说实话，我当年刚接触SLAM时，觉得相机模型不就是小孔成像嘛，有啥好学的？结果第一次做标定，图像边缘的柱子全弯了，我才意识到畸变这东西有多坑。嗯，咱们一步步来。

3.1 针孔相机模型：最朴素的“眼睛”

针孔相机模型，说白了就是初中物理课上的小孔成像。光从物体上反射，穿过一个小孔，在后面的成像平面上形成倒像。但咱们做SLAM时，通常把成像平面“虚拟”地挪到相机前方，这样图像就是正的了，数学上也好处理。

这里有几个关键坐标系，我建议你记牢：

  • 世界坐标系：你放相机的地方，原点你说了算。
  • 相机坐标系：原点在相机光心，Z轴朝前。
  • 图像坐标系：在成像平面上，单位是毫米。
  • 像素坐标系：最终你看到的图像，单位是像素。

从世界点到像素点，经历了四次变换：

  1. 世界坐标 → 相机坐标（刚体变换，外参）
  2. 相机坐标 → 归一化平面（投影）
  3. 归一化平面 → 图像坐标（内参缩放）
  4. 图像坐标 → 像素坐标（加主点偏移）

数学公式长这样：

Z * [u, v, 1]^T = K * [R|t] * [X, Y, Z, 1]^T

其中K是内参矩阵：

K = [fx, 0, cx;
     0, fy, cy;
     0,  0,  1]

fx、fy是焦距的像素表示，cx、cy是主点坐标。我在项目中见过有人把cx、cy直接设成图像中心，结果标定出来的地图总是歪的——千万别偷懒，老老实实做标定。

我的小习惯： 拿到一个新相机，第一件事就是标定内参。哪怕厂家给了参数，我也要自己跑一遍。因为镜头批次不同，fx、fy可能有1-2%的偏差，这在SLAM里足以让地图漂移。

3.2 畸变模型：为什么直线会变弯？

你想想看，针孔模型是理想情况。现实中的镜头是透镜组，光线穿过时会发生折射，导致图像变形。这就是畸变。

畸变主要分两种：

  • 径向畸变：光线在透镜边缘弯曲更厉害，表现为“桶形”或“枕形”。
  • 切向畸变：透镜和成像平面不平行，导致图像倾斜。

数学上，我们用多项式来矫正：

x_corrected = x * (1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x*y + p2*(r^2 + 2*x^2)
y_corrected = y * (1 + k1*r^2 + k2*r^4 + k3*r^6) + p1*(r^2 + 2*y^2) + 2*p2*x*y

其中k1、k2、k3是径向畸变参数，p1、p2是切向畸变参数。r是点到中心的距离。

我曾经踩过的坑： 有一次用鱼眼镜头做SLAM，只用了k1、k2两个参数矫正，结果图像边缘的特征点匹配全乱了。后来才发现鱼眼镜头需要k3甚至更高阶参数。所以，畸变模型的选择一定要和镜头匹配，别图省事。

3.3 双目视觉模型：如何用两只眼睛看深度？

单目相机缺深度信息，这是硬伤。双目相机通过左右眼的视差，可以算出深度。原理很简单——你伸出一根手指放在眼前，轮流闭眼，手指的位置会跳。这个“跳”的量就是视差。

双目模型的核心公式：

depth = (f * baseline) / disparity

其中：

  • f：焦距（像素单位）
  • baseline：左右相机光心之间的距离
  • disparity：同一个点在左右图像中的像素差

你看，深度和视差成反比。物体越近，视差越大；物体越远，视差趋近于0。所以双目相机对近处物体精度高，远处就无能为力了。

实际项目中，双目匹配是个大难题。我记得有一次在室外做实验，阳光太强，左右图像的亮度不一致，匹配算法直接崩了。后来加了直方图均衡化才稳住。

关键点： 双目相机的基线越长，测距精度越高，但体积也越大。做嵌入式产品时，这是个典型的trade-off。我一般建议室内机器人用5-8cm基线，室外无人车用15-30cm。

3.4 PnP问题：已知3D点，求相机位姿

PnP（Perspective-n-Point）问题，说白了就是：我知道一些3D点的世界坐标，也知道它们在图像上的2D投影，求相机的位置和姿态。

这是SLAM里最常用的算法之一。比如你建好地图后，新来一帧图像，匹配到地图上的3D点，就能算出当前相机在哪。

常见的解法有：

  • P3P：只用3个点，速度快，但容易受噪声影响。
  • EPnP：用n个点，精度高，是工程中最常用的。
  • DLT（直接线性变换）：简单粗暴，但需要至少6个点。
  • BA优化：把PnP问题放到非线性优化里，精度最高，但计算量大。

我个人习惯先用EPnP算一个初值，再用BA精化。这样既保证了速度，又保证了精度。有一次在嵌入式平台上，EPnP跑了2ms，BA跑了15ms，帧率完全扛得住。

避坑指南： 做PnP时，3D点的分布很重要。如果所有点都在一个平面上，或者集中在图像的一角，解算结果会非常不稳定。我曾经因为特征点全集中在画面左侧，导致位姿估计偏了10度。所以，尽量让特征点均匀分布在整个图像上。

3.5 ICP算法：点云对齐的利器

ICP（Iterative Closest Point）算法，用于对齐两组点云。在SLAM里，它常用于RGB-D相机或激光雷达的场景。

ICP的核心思想很简单：

  1. 找到两组点云中最近的匹配点对。
  2. 根据匹配点对，计算旋转和平移。
  3. 应用变换，重复迭代，直到收敛。

数学上，它最小化的是这样一个误差：

E(R, t) = (1/N) * Σ || p_i - (R * q_i + t) ||^2

其中p_i和q_i是匹配点对。

ICP的缺点也很明显：

  • 需要好的初始值，否则容易陷入局部最优。
  • 对噪声敏感，尤其是离群点。
  • 计算量大，点云多的时候跑不动。

我在做嵌入式移植时，对ICP做了几个优化：

  • 用体素滤波降采样，把点云从几万点降到几千点。
  • 用KD树加速最近邻搜索。
  • 只迭代10-15次，不追求完全收敛。

效果还不错，在ARM Cortex-A72上，一帧点云的对齐时间从200ms降到了30ms。

注意： ICP假设两组点云已经粗略对齐。如果你直接用ICP做全局匹配，大概率会失败。正确的做法是：先用里程计或视觉特征给出一个初值，再用ICP精化。我见过有人直接用ICP做重定位，结果点云对到了墙上——嗯，教训深刻。

3.6 小结

这一节的内容，是SLAM数学基础的“下半场”。从相机如何成像，到如何从图像中恢复3D信息，再到如何求解位姿，环环相扣。

说实话，这些模型看起来公式多，但真正用起来，你会发现它们就是SLAM的“砖瓦”。每一块都扎实了，后面的系统才不会塌。

下一节，咱们开始讲特征点提取与匹配——这是SLAM前端的第一步，也是决定系统成败的关键。到时候我会分享一些在嵌入式平台上做特征提取的优化技巧，敬请期待。

← 上一章 📖 返回目录 下一章 →