第一章：相机模型与坐标系
做视觉SLAM的第一步是什么？
我个人觉得，不是跑算法，不是调参数，而是先把相机怎么成像这件事搞明白。你想想看，我们所有的输入都来自相机，如果连相机模型都理解不透，后面做特征匹配、位姿估计，那都是空中楼阁。
这一章，我们就来聊聊针孔相机模型、内参矩阵、畸变模型，还有那几个绕来绕去的坐标系。嗯，内容不算多，但都是基本功。
1.1 针孔相机模型
针孔相机模型，说白了就是初中物理里的小孔成像。光线穿过一个小孔，在后面的成像平面上形成倒立的像。这个模型虽然简单，但它是所有现代相机模型的基础。
实际相机当然不是真的用针孔，而是用透镜。但数学上，我们仍然用针孔模型来近似。为什么呢？因为它的几何关系特别清晰——所有光线都通过一个点，这个点叫光心。
我在项目中遇到过一件事：有同事直接用针孔模型标定鱼眼相机，结果误差大得离谱。后来才发现，鱼眼相机的视场角太大，针孔模型根本hold不住。所以啊，选模型之前，先搞清楚你的相机是什么类型。
核心公式：
假设空间点 P = [X, Y, Z] 在相机坐标系下，投影到成像平面上的点 p = [x, y]：
x = f * X / Z
y = f * Y / Z其中 f 是焦距。注意，这里成像平面在光心后方，所以像是倒立的。实际处理时，我们通常把成像平面虚拟到光心前方，这样像就是正立的，公式不变。
1.2 内参矩阵
刚才的公式是理想情况。但实际相机里，像素坐标和物理坐标之间还有一层转换。这层转换，就是内参矩阵 K。
内参矩阵长这样：
K = [fx, 0, cx
0, fy, cy
0, 0, 1]其中：
- fx, fy：分别是x和y方向的焦距（单位是像素）
- cx, cy：主点坐标，也就是光轴与成像平面的交点
为什么会有fx和fy？因为像素通常不是正方形，而且传感器可能还有倾斜。不过现在大多数相机的fx和fy很接近，倾斜因子也近似为0。
内参矩阵的作用，就是把相机坐标系下的三维点，投影到像素坐标系。完整的投影公式：
u = fx * X / Z + cx
v = fy * Y / Z + cy写成矩阵形式：
s * [u, v, 1]^T = K * [X, Y, Z]^T这里的 s 是尺度因子，其实就是 Z 值。
我的经验：标定内参时，一定要用棋盘格拍至少20张不同角度的照片。我见过有人只拍5张就标定，结果重投影误差大得没法用。另外，标定板的精度也很重要，淘宝上几块钱的打印棋盘格，精度真的不行。
1.3 畸变模型
实际相机用的不是针孔，而是透镜。透镜会带来畸变。最常见的两种：
- 径向畸变：光线经过透镜边缘时弯曲得更厉害。表现为图像边缘的直线变弯。分为桶形畸变和枕形畸变。
- 切向畸变：透镜和成像平面不平行导致的。表现为图像看起来像被拉伸了。
畸变模型通常用多项式来拟合。径向畸变用 k1, k2, k3 三个参数，切向畸变用 p1, p2 两个参数。
矫正公式：
x_distorted = x * (1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x*y + p2*(r^2 + 2*x^2)
y_distorted = y * (1 + k1*r^2 + k2*r^4 + k3*r^6) + p1*(r^2 + 2*y^2) + 2*p2*x*y其中 r^2 = x^2 + y^2，x, y 是归一化平面上的坐标。
注意：畸变参数是跟内参一起标定出来的。标定完成后，一定要做畸变矫正，否则后续的特征匹配和三角化都会出问题。我曾经在一个项目中忘了做畸变矫正，结果特征点匹配的误差直接导致位姿估计发散，排查了整整两天才找到原因。
1.4 坐标系转换
做视觉SLAM，你会在四个坐标系之间来回切换。搞不清楚它们的关系，代码写出来就是一团浆糊。
四个坐标系：
- 世界坐标系：我们定义的一个固定坐标系。通常以第一帧相机位置为原点，或者以某个已知物体为参考。
- 相机坐标系：以相机光心为原点，Z轴指向相机前方，X轴向右，Y轴向下（注意，这个跟OpenCV的惯例一致）。
- 图像坐标系：在成像平面上，以物理单位（毫米）度量。原点通常在图像中心。
- 像素坐标系：以像素为单位，原点在图像左上角，u轴向右，v轴向下。
它们之间的转换关系：
| 转换 | 公式 | 说明 |
|---|---|---|
| 世界 → 相机 | P_cam = R * P_world + t | R是旋转矩阵，t是平移向量 |
| 相机 → 图像 | x = X/Z, y = Y/Z | 归一化平面坐标 |
| 图像 → 像素 | u = fx*x + cx, v = fy*y + cy | 内参矩阵作用 |
你想想看，整个流程其实就是：世界坐标系下的一个点，先通过外参（R, t）转到相机坐标系，再通过内参投影到像素坐标系。反过来，从像素坐标恢复三维信息，就需要深度值了。
1.5 知识体系总览
下面这张图，把这一章的核心逻辑串起来了。我建议你多看几遍，把每个环节的输入输出搞清楚。
嗯，这张图把整个流程串起来了。从世界坐标系到像素坐标系，中间经过外参、投影、内参三个环节。畸变矫正可以放在投影之后，也可以放在投影之前，取决于你的实现方式。
1.6 实战中的注意事项
最后，分享几个我在实际项目中踩过的坑：
- 坐标系方向：OpenCV和OpenGL的坐标系方向不一样。OpenCV是右手系，Z轴向前；OpenGL是左手系，Z轴向里。混用的时候一定要小心。
- 像素坐标原点：图像坐标系原点在中心，像素坐标系原点在左上角。转换时别忘了加偏移。
- 畸变参数顺序：不同库的畸变参数顺序可能不同。OpenCV是[k1, k2, p1, p2, k3]，有些库是[k1, k2, k3, p1, p2]。标定完一定要确认。
- 浮点精度：内参矩阵里的fx, fy通常很大（几百到几千），计算时尽量用double，避免精度损失。
我的建议：刚开始做视觉SLAM的同学，可以先用OpenCV的calibrateCamera函数走一遍标定流程。把内参和畸变参数打印出来，手动算几个点的投影，看看重投影误差。这样能帮你建立直观理解。
好了，这一章的内容就到这里。相机模型和坐标系转换是视觉SLAM的地基，一定要打牢。下一章我们会聊特征点提取与匹配，到时候这些坐标系知识会反复用到。