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 度4-5 个好普通工业相机、手机主摄Equidistant100-180 度4 个中等广角、轻度鱼眼Kannala-Brandt 180 度4-5 个较好鱼眼、全景RadTan 的问题在于当视场角超过 100 度后径向畸变的多项式模型需要很高阶才能拟合而高阶项在数值上不稳定容易过拟合。Equidistant 模型假设入射角和成像半径成正比这个假设对广角镜头更合理。Kannala-Brandt 则是更通用的鱼眼模型用入射角的多项式来拟合。5.2 标定参数不准时的症状与排查相机模型选对了参数标定不准照样出问题。我遇到过几种典型的症状分享出来供大家对照第一种轨迹整体缩放。如果你发现估计的轨迹比真实轨迹大了一圈或者小了一圈很可能是焦距标定不准。焦距偏大归一化坐标偏小深度估计偏大轨迹就放大了。第二种轨迹弯曲。直线运动估计成弧线通常是径向畸变系数不准。k1 偏大边缘点被过度校正特征点位置系统性偏移导致轨迹弯曲。第三种特定区域特征点跟踪差。如果只有图像边缘的特征点容易丢中心区域正常那基本可以确定是畸变模型的问题要么模型选错了要么高阶系数标定不准。排查方法很简单拿一张棋盘格图片用标定工具重新标定对比新旧参数。如果差异很大说明原来的标定有问题。另外可以用重投影误差作为指标好的标定重投影误差应该在 0.1 像素以内。5.3 双相机系统的模型一致性双相机系统有个额外的问题两个相机的模型必须一致吗答案是不必须但外参标定必须和模型匹配。如果 cam0 用 RadTancam1 用 Equidistant那么外参标定时必须用各自的模型去畸变否则外参会有偏差。OPENVINS 支持两个相机用不同模型配置里分别指定即可。但我在实际使用中发现如果两个相机型号相同最好用相同的模型和相近的参数这样外参标定的鲁棒性更好。如果两个相机型号差异很大那外参标定要格外小心建议多标几组取平均。6. 实操从零验证相机模型是否正确6.1 准备测试数据与工具验证相机模型不需要跑完整的 VIO只需要几张标定图片和一组已知的 3D-2D 对应点。我通常用棋盘格因为角点位置可以精确提取而且棋盘格平面上的 3D 坐标是已知的。工具方面OpenCV 的calibrateCamera可以标定参数projectPoints可以做投影验证。但要注意OpenCV 的畸变模型和 OPENVINS 的 RadTan 在系数顺序上可能不同用的时候要核对清楚。OpenCV 的系数顺序是 k1, k2, p1, p2, k3OPENVINS 也是这个顺序但有些工具会用 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 点假设棋盘在 z0 平面 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迭代发散检查畸变系数是否合理减小迭代步长坐标数值巨大主点或焦距错误核对配置文件确认单位边缘点坐标异常高阶畸变系数过大重新标定或降低模型阶数所有点坐标偏移主点标定不准用棋盘格重新标定主点NaN 是最危险的因为它会传播到整个优化过程导致系统崩溃。OPENVINS 在去畸变后会检查坐标是否有限如果出现 NaN 会跳过该点。但这个检查只是兜底根本解决还是要找到 NaN 的来源。7.2 迭代不收敛的处理方法迭代不收敛通常发生在畸变很大的情况下。处理方法有几种一是增加最大迭代次数给迭代更多机会二是改用更鲁棒的求解方法比如用阻尼牛顿法三是换模型如果 RadTan 不收敛试试 Equidistant。我在处理一个 180 度鱼眼镜头时RadTan 怎么调都不收敛换成 Kannala-Brandt 后一次就通过了。这说明模型选择比参数调优更重要。不要在一个不合适的模型上死磕。7.3 性能瓶颈的定位与优化如果发现去畸变耗时占比很高可以用 profiling 工具定位。我通常用perf或者简单的计时器统计去畸变函数的调用次数和总耗时。如果确实是大头可以考虑几个优化方向减少迭代次数在精度允许的前提下、向量化处理、预计算雅可比矩阵中不变的部分。预计算是个很实用的技巧雅可比矩阵中有些项只和畸变系数有关和具体的点无关可以提前算好。这样每次迭代只需要做少量运算。我在一个高帧率项目里用这个技巧把去畸变耗时降低了 40%。8. 我在实际项目里踩过的坑与经验总结第一个坑是忽略了去畸变和投影的互逆性验证。我一开始只验证了去畸变没验证投影结果后端优化时重投影误差始终不为零调了很久才发现是投影函数里有个符号写反了。后来我养成了习惯每次改相机模型代码都要写一个单元测试随机生成归一化坐标投影再去畸变看能不能回到原值。第二个坑是标定参数的单位问题。焦距的单位是像素但有些人标定时用的是毫米直接填进去就错了。主点的单位也是像素而且要注意是从图像左上角还是中心开始算。OPENVINS 用的是左上角为原点和 OpenCV 一致但有些标定工具用中心为原点转换时要小心。第三个坑是双相机时间同步对去畸变的影响。双相机如果时间不同步同一时刻两个相机看到的场景不一样去畸变后的特征点匹配就会出错。这个问题表面上看是去畸变的问题实际上是时间同步的问题。排查时要先确认时间戳是否对齐。第四个坑是图像分辨率改变后忘记更新参数。如果图像做了降采样焦距和主点都要按比例缩放畸变系数不变。我有一次把图像从 752x480 降到 376x240忘了改焦距结果轨迹完全不对。这个坑很隐蔽因为降采样后图像看起来正常只有跑 VIO 才会发现问题。最后分享一个实用技巧在配置文件里加注释记录每个参数的标定时间和标定条件。这样过几个月回头看能快速回忆起参数是怎么来的。相机参数不是一劳永逸的镜头摔过、温度变化大、重新对焦后都可能需要重新标定。养成记录的习惯能省很多排查时间。