1. 为什么相机模型是 VIO 系统的第一道生死关

很多人读 OPENVINS 源码，习惯从 IMU 预积分或者滑动窗口优化切入，觉得那才是"算法核心"。我一开始也这么想，直到有一次用一套广角镜头跑数据集，轨迹整体往一个方向缓慢漂移，调了三天 IMU 噪声参数都没用，最后发现问题出在相机模型上——畸变系数标定得不准，特征点的归一化坐标本身就是歪的，后面优化再精细也是白搭。从那以后我就养成了一个习惯：拿到任何一套 VIO 数据，先看相机模型和去畸变这一环。

OPENVINS 作为目前学术界和工程界都认可的开源 VIO 框架，它的相机模型模块设计得相当克制但完整。它没有像某些框架那样把相机模型写死成针孔加径向畸变，而是抽象出了一套可扩展的模型体系，支持RadTan（径向切向畸变）、Equidistant（等距/鱼眼模型）、Kannala-Brandt等多种模型。这一篇我们就专门把cam这个模块从源码层面拆开，搞清楚它到底怎么把一张畸变图像上的像素点，变成优化器能用的归一化坐标，以及在这个过程中有哪些坑是文档里不会写的。

这篇文章适合已经能跑通 OPENVINS、想深入理解内部机制的人，也适合正在自己写 VIO 或者 SLAM 系统、需要设计相机模型抽象层的工程师。如果你只是想知道怎么改配置文件跑通，那看官方文档就够了；但如果你想搞清楚"为什么改了这个参数轨迹就飘了"，那这篇值得往下读。

2. OPENVINS 相机模型模块的整体设计思路

2.1 从需求反推设计：为什么不能只写一个针孔模型

先想清楚一个问题：一个 VIO 系统对相机模型到底有什么需求？表面上看，就是把像素坐标转成归一化坐标，再乘个内参矩阵就完事了。但实际工程里远不止这么简单。

第一，不同镜头的畸变特性差异巨大。普通工业相机用 RadTan 模型就够了，但广角、鱼眼镜头用 RadTan 拟合会非常勉强，尤其是视场角超过 120 度之后，径向畸变的多项式模型会失效，必须用等距模型或者 Kannala-Brandt 这类专门为鱼眼设计的模型。第二，去畸变和投影必须是可逆的。VIO 前端做特征跟踪时需要去畸变得到归一化坐标，后端做重投影误差计算时需要把 3D 点投影回像素平面，这两个过程必须严格互逆，否则优化会引入系统性偏差。第三，性能要可控。去畸变是每个特征点每帧都要做的操作，如果迭代求解太慢，整个前端就废了。

OPENVINS 的设计思路是：定义一个抽象基类CamBase，把"投影"和"反投影（去畸变）"这两个核心操作作为纯虚函数，然后每个具体模型继承它实现自己的逻辑。这样上层代码只依赖基类接口，换模型不用改一行前端代码。这个设计看起来朴素，但非常实用，我在自己的项目里也照搬了这套结构。

2.2 类继承体系与文件组织

打开ov_core/src/cam/目录，你会看到这样几个文件：CamBase.h定义抽象接口，CamRadtan.h/cpp实现径向切向畸变模型，CamEquidistant.h/cpp实现等距模型，还有CamKannalaBrandt等。每个模型都是自包含的，头文件里声明参数和接口，cpp 里实现具体的数学运算。

CamBase里定义的核心接口主要有这么几个：project把归一化坐标投影到像素坐标，undistort把像素坐标去畸变成归一化坐标，还有compute_distortion计算畸变量。注意这里有个容易混淆的点：OPENVINS 里的"归一化坐标"指的是去畸变之后、还没乘内参矩阵的坐标，也就是理想针孔模型下的(x/z, y/z)。这个定义一定要记牢，后面看代码时很多地方会绕晕。

提示：读源码时建议先把CamBase.h从头到尾看一遍，把每个虚函数的输入输出语义写在纸上，再去看具体实现。直接扎进CamRadtan.cpp很容易迷失在公式里。

2.3 参数是怎么传进来的

相机参数不是硬编码的，而是通过配置文件读入。在 OPENVINS 的 YAML 配置里，你会看到类似这样的段落：

cam0: cam_model: "radtan" intrinsics: [458.65, 457.29, 367.21, 248.37] distortion_coeffs: [-0.283, 0.074, 0.0002, -0.0001] resolution: [752, 480]

intrinsics是四个内参：焦距 fx、fy 和主点 cx、cy。distortion_coeffs对 RadTan 来说是四个系数 k1、k2、p1、p2。这些参数在系统初始化时被读入，构造对应的相机模型对象。这里有个细节：OPENVINS 支持双相机，所以配置里会有 cam0 和 cam1 两段，每个相机独立构造自己的模型对象。如果你的两个镜头型号不同，完全可以一个用 radtan 一个用 equidistant，框架层面是支持的。

3. RadTan 模型源码逐行拆解

3.1 畸变模型的数学原理

RadTan 全称是 Radial-Tangential，即径向畸变加切向畸变。径向畸变来自镜头曲率，表现为图像边缘的点向外或向内偏移；切向畸变来自镜头和成像平面不平行，表现为图像产生轻微的梯形形变。数学上，给定归一化平面上的理想点 $(x, y)$，畸变后的坐标是：

$$r^2 = x^2 + y^2$$

径向畸变因子：$1 + k_1 r^2 + k_2 r^4 + k_3 r^6$

切向畸变项：$2p_1 xy + p_2(r^2 + 2x^2)$ 和 $p_1(r^2 + 2y^2) + 2p_2 xy$

最终畸变坐标 $x_d = x \cdot radial + tangential_x$，$y_d$ 同理。然后乘内参矩阵得到像素坐标。

这个公式本身不难，难的是反过程：给定畸变后的像素坐标，怎么求原始的理想归一化坐标？这个反解没有解析形式，必须迭代。OPENVINS 用的是牛顿迭代法，这也是我要重点讲的部分。

3.2 project 函数：从归一化坐标到像素

先看正向投影，这个简单。CamRadtan::project接收归一化坐标，先算 $r^2$，再算径向和切向畸变项，得到畸变坐标，最后乘内参矩阵加上主点，返回像素坐标。代码逻辑非常直白，基本就是把上面的公式翻译成 C++。

但这里有个容易忽略的细节：OPENVINS 在投影时会检查畸变后的点是否还在图像范围内，如果超出边界会返回 false。这个检查很重要，因为后端优化时如果投影点跑到图像外，重投影误差就不应该被计算，否则会引入错误的约束。我在自己实现时一开始没做这个检查，结果优化器经常被一些跑到图像外的点带偏。

3.3 undistort 函数：牛顿迭代去畸变的核心

这是整个模块最核心也最容易出问题的部分。给定像素坐标 $(u, v)$，先减去主点、除以焦距，得到畸变归一化坐标 $(x_d, y_d)$。然后要解方程：找到 $(x, y)$ 使得畸变后的坐标等于 $(x_d, y_d)$。

OPENVINS 的做法是构造一个残差函数 $f(x, y) = distort(x, y) - (x_d, y_d)$，然后用牛顿法迭代求解。每次迭代计算雅可比矩阵，求解线性方程得到增量，更新估计值，直到残差足够小或者达到最大迭代次数。

雅可比矩阵的推导是这里的难点。对 $x$ 求偏导，需要用到链式法则，把径向项和切向项分别对 $x$ 求导。OPENVINS 在代码里把这个雅可比写成了显式的表达式，看起来很长，但其实就是逐项求导的结果。我建议你自己拿纸笔推导一遍，推完之后再看代码会豁然开朗。

注意：牛顿迭代的收敛性依赖初值。OPENVINS 直接用畸变坐标作为初值，对于畸变不太大的镜头（比如普通工业镜头）通常两三次迭代就收敛了。但如果是鱼眼镜头，畸变非常大，这个初值可能离真实解很远，迭代次数会明显增加，甚至不收敛。这也是为什么鱼眼镜头要用专门的模型，而不是硬套 RadTan。

3.4 迭代次数与收敛阈值的选择

代码里有两个关键参数：最大迭代次数和收敛阈值。OPENVINS 默认最大迭代次数是 5 次，收敛阈值是残差的平方和小于某个很小的值。这两个参数的选择是有讲究的。

迭代次数太少，去畸变不准确，特征点坐标有偏差，直接影响 VIO 精度。迭代次数太多，浪费时间，而且对于已经收敛的点，多迭代几次也没意义。5 次是一个经验值，对于大多数镜头够用。收敛阈值设得太松，迭代提前退出，精度不够；设得太紧，可能永远达不到，白白浪费迭代次数。

我在实际项目里做过测试：对于畸变系数 k1 在 -0.3 左右的普通镜头，3 次迭代后残差就已经降到 1e-6 以下了，5 次是绰绰有余。但如果 k1 到了 -0.5 以上，5 次迭代后残差可能还在 1e-4 量级，这时候要么增加迭代次数，要么换模型。

4. 去畸变在 VIO 前端中的实际调用链路

4.1 特征点从像素到归一化的完整流程

搞清楚单个函数的实现之后，我们要看它在整个系统里是怎么被调用的。VIO 前端处理一帧图像的流程大致是：提取特征点（FAST、Shi-Tomasi 等），对每个特征点调用undistort得到归一化坐标，然后用归一化坐标做光流跟踪或者描述子匹配。

这里有个性能上的关键点：去畸变是逐点操作的，如果一帧有 200 个特征点，每个点迭代 5 次，每次迭代涉及十几次浮点运算，加起来就是上万次运算。对于 30 帧每秒的实时系统，这个开销不能忽视。OPENVINS 的做法是在特征提取阶段就记录像素坐标，去畸变只在需要的时候做，而且尽量批量处理以提高缓存命中率。

我在自己的系统里做过一个优化：把去畸变的迭代过程向量化，一次处理多个点，利用 SIMD 指令加速。实测下来对于 200 个点的场景，去畸变耗时从 0.8ms 降到了 0.3ms 左右。当然这是进阶优化，初期先把正确性保证好更重要。

4.2 去畸变与光流跟踪的配合

光流跟踪是在像素层面做的还是归一化层面做的？这个问题很多人搞不清楚。OPENVINS 的做法是：光流跟踪在去畸变后的归一化坐标上做。也就是说，先把上一帧的特征点去畸变，然后在当前帧的去畸变图像上跟踪。

等等，这里有个矛盾：光流需要图像像素，但去畸变后的坐标已经不是整数像素了。OPENVINS 的处理方式是维护一个去畸变后的"虚拟图像"概念，实际上是在原始图像上采样，但采样位置由去畸变坐标决定。这个细节在代码里体现为undistort之后再做双线性插值。

提示：如果你发现光流跟踪经常跟丢，除了检查图像质量，也要确认去畸变是否正确。去畸变错了，归一化坐标就错了，光流在错误的坐标上搜索，自然容易丢。

4.3 后端重投影误差中的相机模型

后端优化时，重投影误差的计算需要把 3D 地图点投影到像素平面，和观测到的像素坐标比较。这个投影过程用的就是project函数。注意这里有个对称性：前端用undistort把像素转成归一化坐标，后端用project把归一化坐标转回像素，两者必须严格互逆。

如果去畸变和投影不互逆，会发生什么？假设去畸变有微小偏差，前端得到的归一化坐标偏了一点，后端投影回去得到的像素坐标也偏了一点，重投影误差就不为零，优化器会试图调整位姿来消除这个误差，但误差来源是模型本身，调位姿没用，结果就是轨迹产生系统性漂移。这就是我开头说的那个坑的数学本质。

5. 不同相机模型的对比与选型实战

5.1 RadTan、Equidistant、Kannala-Brandt 的适用边界

选相机模型不是拍脑袋决定的，要看镜头的实际视场角和畸变特性。我整理了一个对比表，是我在实际项目中总结出来的经验值：

RadTan 的问题在于，当视场角超过 100 度后，径向畸变的多项式模型需要很高阶才能拟合，而高阶项在数值上不稳定，容易过拟合。Equidistant 模型假设入射角和成像半径成正比，这个假设对广角镜头更合理。Kannala-Brandt 则是更通用的鱼眼模型，用入射角的多项式来拟合。

5.2 标定参数不准时的症状与排查

相机模型选对了，参数标定不准照样出问题。我遇到过几种典型的症状，分享出来供大家对照：

第一种，轨迹整体缩放。如果你发现估计的轨迹比真实轨迹大了一圈或者小了一圈，很可能是焦距标定不准。焦距偏大，归一化坐标偏小，深度估计偏大，轨迹就放大了。

第二种，轨迹弯曲。直线运动估计成弧线，通常是径向畸变系数不准。k1 偏大，边缘点被过度校正，特征点位置系统性偏移，导致轨迹弯曲。

第三种，特定区域特征点跟踪差。如果只有图像边缘的特征点容易丢，中心区域正常，那基本可以确定是畸变模型的问题，要么模型选错了，要么高阶系数标定不准。

排查方法很简单：拿一张棋盘格图片，用标定工具重新标定，对比新旧参数。如果差异很大，说明原来的标定有问题。另外可以用重投影误差作为指标，好的标定重投影误差应该在 0.1 像素以内。

5.3 双相机系统的模型一致性

双相机系统有个额外的问题：两个相机的模型必须一致吗？答案是不必须，但外参标定必须和模型匹配。如果 cam0 用 RadTan，cam1 用 Equidistant，那么外参标定时必须用各自的模型去畸变，否则外参会有偏差。

OPENVINS 支持两个相机用不同模型，配置里分别指定即可。但我在实际使用中发现，如果两个相机型号相同，最好用相同的模型和相近的参数，这样外参标定的鲁棒性更好。如果两个相机型号差异很大，那外参标定要格外小心，建议多标几组取平均。

6. 实操：从零验证相机模型是否正确

6.1 准备测试数据与工具

验证相机模型不需要跑完整的 VIO，只需要几张标定图片和一组已知的 3D-2D 对应点。我通常用棋盘格，因为角点位置可以精确提取，而且棋盘格平面上的 3D 坐标是已知的。

工具方面，OpenCV 的calibrateCamera可以标定参数，projectPoints可以做投影验证。但要注意，OpenCV 的畸变模型和 OPENVINS 的 RadTan 在系数顺序上可能不同，用的时候要核对清楚。OpenCV 的系数顺序是 k1, k2, p1, p2, k3，OPENVINS 也是这个顺序，但有些工具会用 k1, k2, k3, p1, p2，一定要看文档。

6.2 编写验证脚本

我写了一个简单的验证脚本，思路是：用标定得到的参数，把棋盘格角点的 3D 坐标投影到图像上，和实际检测到的角点比较，看重投影误差。如果误差在 0.1 像素以内，说明模型和参数都没问题。

import cv2 import numpy as np # 读取标定参数 K = np.array([[458.65, 0, 367.21], [0, 457.29, 248.37], [0, 0, 1]]) dist = np.array([-0.283, 0.074, 0.0002, -0.0001, 0.0]) # 棋盘格 3D 点（假设棋盘在 z=0 平面） objp = np.zeros((6*9, 3), np.float32) objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2) * 0.025 # 投影 imgpoints, _ = cv2.projectPoints(objp, rvec, tvec, K, dist) # 和实际检测点比较 error = cv2.norm(detected_points, imgpoints, cv2.NORM_L2) / len(imgpoints) print(f"重投影误差: {error:.4f} 像素")

这个脚本跑下来，如果误差大于 0.5 像素，基本可以确定标定有问题。我遇到过误差 2 像素的情况，最后发现是棋盘格尺寸填错了，实际格子是 25mm 填成了 30mm，导致尺度整体偏差。

6.3 在 OPENVINS 里做端到端验证

脚本验证通过后，还要在 OPENVINS 里做端到端验证。方法是：跑一段已知轨迹的数据集（比如 EuRoC），看估计轨迹和真值的误差。如果相机模型有问题，误差会明显偏大，而且往往表现为系统性漂移而不是随机噪声。

我通常的做法是对比两组参数：一组是原始标定参数，一组是重新标定的参数，分别跑同一段数据，比较 ATE（绝对轨迹误差）。如果重新标定后 ATE 明显下降，说明原来的参数确实有问题。这个方法虽然笨，但非常有效。

注意：端到端验证时，要确保其他条件不变，只改相机参数。如果同时改了 IMU 参数，就无法判断误差变化是哪个因素引起的。

7. 常见问题与排查技巧实录

7.1 去畸变后坐标异常的原因排查

去畸变后坐标异常是最常见的问题，表现是归一化坐标出现 NaN 或者数值特别大。我整理了一个排查表：

NaN 是最危险的，因为它会传播到整个优化过程，导致系统崩溃。OPENVINS 在去畸变后会检查坐标是否有限，如果出现 NaN 会跳过该点。但这个检查只是兜底，根本解决还是要找到 NaN 的来源。

7.2 迭代不收敛的处理方法

迭代不收敛通常发生在畸变很大的情况下。处理方法有几种：一是增加最大迭代次数，给迭代更多机会；二是改用更鲁棒的求解方法，比如用阻尼牛顿法；三是换模型，如果 RadTan 不收敛，试试 Equidistant。

我在处理一个 180 度鱼眼镜头时，RadTan 怎么调都不收敛，换成 Kannala-Brandt 后一次就通过了。这说明模型选择比参数调优更重要。不要在一个不合适的模型上死磕。

7.3 性能瓶颈的定位与优化

如果发现去畸变耗时占比很高，可以用 profiling 工具定位。我通常用perf或者简单的计时器，统计去畸变函数的调用次数和总耗时。如果确实是大头，可以考虑几个优化方向：减少迭代次数（在精度允许的前提下）、向量化处理、预计算雅可比矩阵中不变的部分。

预计算是个很实用的技巧：雅可比矩阵中有些项只和畸变系数有关，和具体的点无关，可以提前算好。这样每次迭代只需要做少量运算。我在一个高帧率项目里用这个技巧把去畸变耗时降低了 40%。

8. 我在实际项目里踩过的坑与经验总结

第一个坑是忽略了去畸变和投影的互逆性验证。我一开始只验证了去畸变，没验证投影，结果后端优化时重投影误差始终不为零，调了很久才发现是投影函数里有个符号写反了。后来我养成了习惯，每次改相机模型代码，都要写一个单元测试，随机生成归一化坐标，投影再去畸变，看能不能回到原值。

第二个坑是标定参数的单位问题。焦距的单位是像素，但有些人标定时用的是毫米，直接填进去就错了。主点的单位也是像素，而且要注意是从图像左上角还是中心开始算。OPENVINS 用的是左上角为原点，和 OpenCV 一致，但有些标定工具用中心为原点，转换时要小心。

第三个坑是双相机时间同步对去畸变的影响。双相机如果时间不同步，同一时刻两个相机看到的场景不一样，去畸变后的特征点匹配就会出错。这个问题表面上看是去畸变的问题，实际上是时间同步的问题。排查时要先确认时间戳是否对齐。

第四个坑是图像分辨率改变后忘记更新参数。如果图像做了降采样，焦距和主点都要按比例缩放，畸变系数不变。我有一次把图像从 752x480 降到 376x240，忘了改焦距，结果轨迹完全不对。这个坑很隐蔽，因为降采样后图像看起来正常，只有跑 VIO 才会发现问题。

最后分享一个实用技巧：在配置文件里加注释，记录每个参数的标定时间和标定条件。这样过几个月回头看，能快速回忆起参数是怎么来的。相机参数不是一劳永逸的，镜头摔过、温度变化大、重新对焦后，都可能需要重新标定。养成记录的习惯，能省很多排查时间。