简介这是一份基于LM列文伯格-马夸尔特算法的相机标定MATLAB脚本面向计算机视觉、图像处理方向的研究者与开发者用于求解相机内参与外参的非线性最小二乘问题。脚本完整实现了相机标定的核心流程包括棋盘格角点数据预处理、参数初始化、迭代优化中的残差计算、雅可比矩阵求解、阻尼因子自适应调整以及终止判断与结果验证。LM算法结合梯度下降与高斯-牛顿法的优点在接近线性时快速收敛非线性较强时通过阻尼因子保持稳定适合相机标定这类复杂参数估计问题。压缩包内共一个文件为.m格式源代码整体大小约2KB结构简洁便于直接运行或二次修改。目前已有680人学习下载适用于自动驾驶、机器人导航、增强现实等需要精确标定的实际场景。通过研读代码可快速搭建自己的标定框架掌握LM算法在非线性优化中的调参思路与工程实现技巧。1. 标定里的LM算法为什么说它是非线性标定的事实默认项把一块棋盘格放在相机前面拍二十张照片然后去解焦距、主点、畸变——这套流程所有做视觉的人都不陌生。可真正让重投影误差从几个像素压到零点几个像素的不是拍照而是背后的迭代优化。这个优化器十有八九是LM算法Levenberg–Marquardt也就是标题里lm标定指向的那个核心。LM解决的问题很具体给定一组待标定参数内参、畸变、每帧外参或者手眼标定里的变换矩阵以及每个参数组合下的残差函数怎么迭代让残差平方和最小。它不要求你提供可靠的二阶导数也不像普通梯度下降那样在上百个参数面前慢慢爬。工程上相机内参标定、机器人手眼标定、多传感器联合标定的主流实现内部走的都是LM这一族阻尼最小二乘。这篇笔记就围绕标定lm算法这条主线展开先讲LM的原理和阻尼参数怎么选再给一个能跑通的相机内参标定最小实现接着讨论外参和手眼标定里的位姿参数化问题最后列出四类高频翻车场景。如果你只是想让标定收敛得更稳、看得懂迭代日志里lambda在干什么或者正准备自己写一个标定优化器这篇内容应该能直接帮上忙。2. 从高斯牛顿到LM标定为什么绕不开阻尼最小二乘标定问题看起来五花八门落到数学上却是同一个形式最小化残差平方和。要理解LM为什么是标定的默认选项得先从高斯牛顿说起再看LM在哪一步做了补救。2.1 标定问题的数学本质重投影残差与J^TJ近似海塞相机标定的目标是让所有角点的重投影误差最小。每观测到一个角点X相机模型会把它投影到像素平面得到一个预测位置和实际检测到的像素位置做差这个二维差值就是一个残差块。把所有帧、所有角点的残差堆成一个向量r(θ)标定就变成了minimize_θ ||r(θ)||²这里的θ包含内参fx、fy、cx、cy、畸变和每帧外参旋转向量和平移向量。手眼标定、多相机外参联合优化同样可以写成这个形式区别只是残差的定义方式。高斯牛顿的思路是把r在当前位置做一阶泰勒展开r(θ δ) ≈ r(θ) Jδ。代进目标函数后求导令其为零得到正规方程(J^T J) δ −J^T rJ^T J 是海塞矩阵的近似。好处是不用算二阶导坏处是J^T J可能病态甚至奇异。标定里这种情况很常见比如所有标定板图像都只覆盖了画面中央外参之间强相关再比如同时优化内参和几十帧外参时某些参数在数据里几乎没有约束。J^T J的条件数一旦差到一定程度高斯牛顿一步就把参数送到十万八千里外重投影误差直接爆炸。还有一个尺度问题容易被忽略。相机内参fx常见上千而旋转向量的分量通常只有0.1量级平移量级看场景可能是0.01到0.5米。这几十个参数在同一个向量空间里做加法J^T J里对角线元素能差七八个数量级。这是标定里很多迭代发散的真正原因而不是算法本身写错了。2.2 LM的阻尼机制λ如何让梯度下降和高斯牛顿自动切换LM在高斯牛顿的正规方程里加了一项阻尼(J^T J λ·diag(J^T J)) δ −J^T r注意用的是diag(J^T J)而不是单位阵I。这是Marquardt当年提出的改进目的是对角缩放让每一个参数维度按自己的曲率尺度被阻尼避免大尺度参数被小尺度参数拖累。λ小的时候这个方程逼近高斯牛顿收敛快λ大的时候diag项主导步长被压短方向接近带缩放的梯度下降保证每一步都在下降。λ的更新靠增益比ρ。每走一步计算实际下降量和二次模型预测的下降量之比ρ (cost_old − cost_new) / predpred是模型预测下降值恒为非负。工程上常用的Nielsen更新规则是ρ 0.75λ除以3ρ 0.25λ乘以3ρ小于等于0直接拒绝这一步。这样在平坦区域残差曲面像缓坡自动增大阻尼、小步试探在陡峭区域自动减小阻尼、大步加速。λ的初值一般取 τ · max(diag(J^T J))τ在1e-3到1之间。标定实践中取1e-3通常够用因为内参初值虽然不准但方向通常是对的。下面是我常用的参数表参数推荐值说明λ初值比例τ1e-3 ~ 1数据噪声大、初值差时取大λ增大倍率×3 ~ ×10拒绝步长时用越大越激进λ减小倍率÷3ρ0.75 时用避免收敛过慢ρ接受阈值0大于0就接受残差在降ρ增/减λ阈值0.75 / 0.25超出就调λ终止条件ftol1e-8步长相对变化小于ftol即停2.3 为什么标定不用纯梯度下降、也不用牛顿法纯梯度下降δ −α·J^T r在参数上百个的标定问题里收敛速度感人。原因很简单梯度下降只用了梯度的一阶信息且在J^T J条件数不好的方向上来回震荡。标定的数据量动辄几千个残差项参数又耦合严重纯梯度下降经常跑到一千轮还没收敛。牛顿法则需要真实海塞矩阵残差函数的非线性跨过几十帧图像之后二阶信息不稳定且计算昂贵。工程上值得为标定构建真实海塞的场景几乎没有。共轭梯度、BFGS这类拟牛顿方法也能用但它们不利用最小二乘的J^T J结构每次迭代要维护一个近似的逆海塞规模一上来内存和收敛速度都不如LM。LM正好卡在中间阻尼项保证它永远不会像高斯牛顿那样一步跳过悬崖阻尼减小时又恢复超线性收敛。这个打不过就加阻尼、打得过就放开冲的特性让它成为标定场景里最不需要操心的优化器。代价是你要给它一个不离谱的初值——这一点比算法本身更决定成败后面专门讲。3. 相机内参标定的LM落地最小可运行的实现与参数解读原理讲完就该动手。这一章用一个纯NumPy实现的LM配合针孔相机模型把相机内参标定的最小链路跑通。代码不依赖OpenCV的标定函数重点看LM每一步在做什么。3.1 建模与优化变量把标定板角点写进残差函数先定义投影模型。这里用带径向畸变和切向畸变的针孔模型畸变只取k1、k2、p1、p2鱼眼镜头的大畸变暂时不讨论。投影函数输入一个世界系三维点、当前帧的旋转向量和平移输出像素坐标import numpy as np from scipy.spatial.transform import Rotation def project_with_dist(K, dist, rvec, tvec, pts_3d): # K: [fx, fy, cx, cy] # dist: [k1, k2, p1, p2] fx, fy, cx, cy K k1, k2, p1, p2 dist R Rotation.from_rotvec(rvec).as_matrix() pts_cam pts_3d R.T tvec # 世界系到相机系 z pts_cam[:, 2] x pts_cam[:, 0] / z y pts_cam[:, 1] / z r2 x * x y * y xd x * (1 k1 * r2 k2 * r2 * r2) 2 * p1 * x * y p2 * (r2 2 * x * x) yd y * (1 k1 * r2 k2 * r2 * r2) p1 * (r2 2 * y * y) 2 * p2 * x * y u fx * xd cx v fy * yd cy return np.stack([u, v], axis1)旋转用旋转向量表示是因为LM在向量空间做加减法。如果直接用旋转矩阵的9个元素作为优化变量每一步迭代后矩阵不再满足正交性标定结果没法用。旋转向量3个参数无约束是LM最顺手的表达方式。z除零的问题在实际标定中不会出现——标定板一定在相机前方但如果自己拍数据时标定板太偏导致z接近0需要在外面把那一帧剔除不要指望LM处理这类不连续。优化变量是每帧外参加内参前5个是fx、fy、cx、cy、k1、k2后面每帧追加旋转向量3维和平移3维。构建残差的函数按帧遍历把所有角点的预测和观测之差拉平成一个大向量def make_residual(pts_2d_list, pts_3d): n_frames len(pts_2d_list) def residual(x): K x[0:4] dist x[4:6] res [] for i, pts2d in enumerate(pts_2d_list): rvec x[6 i * 6 : 9 i * 6] tvec x[9 i * 6 : 12 i * 6] proj project_with_dist(K, dist, rvec, tvec, pts_3d) res.append((pts2d - proj).ravel()) return np.concatenate(res) return residual这样设置的好处是后续换参数化方式时只需要改索引映射LM核心完全不用动。注意残差单位是像素像素误差的量级通常在0.1到几个像素之间这个尺度对LM是友好的。如果做双目或深度相机标定残差被统一到米或毫米后权重和终止阈值都要跟着调。3.2 LM核心迭代纯NumPy实现不调优化库下面这段是LM的主循环。它不依赖任何优化库把阻尼更新、增益比计算、数值雅可比全部摊开方便逐行调试def lm_solve(residual, x0, scale1e-3, max_iter50, ftol1e-8): x np.array(x0, dtypefloat) r residual(x) cost 0.5 * (r r) def num_jac(x): # 自适应步长的中心差分避免大参数和小参数共用固定h h np.sqrt(np.finfo(float).eps) * np.maximum(1.0, np.abs(x)) J np.empty((r.size, x.size)) for i in range(x.size): xp x.copy(); xp[i] h[i] xm x.copy(); xm[i] - h[i] J[:, i] (residual(xp) - residual(xm)) / (2 * h[i]) return J J num_jac(x) A J.T J g J.T r lam scale * np.max(np.diag(A)) for _ in range(max_iter): diagA np.diag(A).copy() # 尝试解正规方程奇异时增大lambda再试 while True: try: delta np.linalg.solve(A lam * np.diag(diagA), -g) except np.linalg.LinAlgError: lam * 10 if lam 1e12: return x continue break x_new x delta r_new residual(x_new) cost_new 0.5 * (r_new r_new) pred -0.5 * delta g # 二次模型预测的下降量恒非负 rho (cost - cost_new) / pred if pred 1e-12 else -1 if rho 0: x x_new r r_new cost cost_new J num_jac(x_new) A J.T J g J.T r if rho 0.75: lam / 3 else: lam * 3 if lam 1e12: return x if np.linalg.norm(delta) ftol * (np.linalg.norm(x) ftol): break return x这段代码的核心逻辑分成四块。第一块是数值雅可比步长h取sqrt(eps)·max(1,|x|)这样fx上千时h在1e-13量级旋转向量0.1时h在1e-8量级各自都在合适的微分尺度上。第二块是阻尼正规方程用np.linalg.solve解线性系统如果矩阵奇异地步把λ乘10重试这是高斯牛顿不具备的兜底能力。第三块是增益比pred的表达式来自对二次模型的解析它在数学上恒非负所以rho大于0就能放心接受步长。第四块是终止判断用步长相对变化量比单纯看cost绝对值更可靠。调用时x0怎么给有讲究。fx初值取图像宽度量级比如1920cx取图像中心坐标cy同理畸变全部置0每帧外参的旋转向量置0、平移给一个合理的估计比如相机到标定板的距离。这个初值烂但方向正确LM在几十轮内就能收敛。如果fx初值给成1.0残差曲面离最优解太远梯度的方向信息已经失真LM再厉害也救不回来。3.3 库实现与手写怎么选scipy和OpenCV的边界工程上大多数时候不需要手写LM。scipy的least_squares提供了稳定的LM实现直接传残差和初值就行from scipy.optimize import least_squares res least_squares(residual, x0, methodlm, max_nfev200, xtol1e-10, gtol1e-12) # res.x 是优化后的完整参数向量 # res.cost 是优化后的目标值可以直接当标定质量的粗参考注意methodlm在scipy里不支持bounds和x_scale因为它内部走的是MINPACK自带了一套尺度处理逻辑。OpenCV的calibrateCamera内部实现的也是LM类方法你只需要提供角点坐标、世界系棋盘格点和图像尺寸剩下的它自己处理。我的选择标准是数据量小、残差简单、需要快速验证方案时用scipy或OpenCV要往残差里加自定义约束比如内参先验、畸变上下限、外参平滑项、或者要把LM嵌进一个更大的优化框架时再手写。还有一个场景必须手写——排查问题。当迭代不收敛时手写版能随时打印A、g、λ、rho库实现把这些都封装死了你只能看到算法未收敛六个字。4. 外参标定里的LM位姿参数化与雅可比的工程选择相机内参标定只是LM的一个舞台。机器人手眼标定、多相机外参联合标定同样依赖LM但这里的坑不在阻尼参数而在怎么表示旋转。4.1 手眼标定AXXB为什么要绕到最小二乘再上LM手眼标定要求解方程AXXB其中A是机器人末端位姿变化B是相机观测到的标定板位姿变化X是相机坐标系到机器人工具坐标系的固定变换。经典的两步解析法先求解旋转、再求解平移速度快但对噪声敏感某些位姿组合下结果断崖式变差。我一般会把这个问题转成非线性最小二乘再用LM。做法是把每个有效动作的两端位姿A和CC是相机到标定板的位姿拿来构建残差T_eb · T_bc · T_ce − I让LM去同时优化T_bc。这样所有观测一次性参与优化噪声会被平方和摊薄比两步法稳定得多。这个思路同样适用于激光雷达和相机外参标定只是残差换成点云匹配误差。解析法不是没用它很适合做LM的初值。先用两步法解一个X出来当x0再用LM精修十轮比直接随机初始化稳得多这是标定工程里的标准操作。4.2 旋转参数化欧拉角、四元数、旋转向量LM该选哪个LM在向量空间做加法所有参数化都要保证迭代后的值仍然是合法旋转。欧拉角三个参数无约束看起来没问题但万向锁会导致特定姿态下自由度退化J^T J直接奇异四元数四个参数带模长约束LM每次加完delta还要手动归一化残差函数里又多了约束惩罚项实现繁琐旋转向量李代数so3最合适3个参数无约束指数映射到旋转矩阵天然合法加法迭代后结构自动保持。参数化参数数量约束LM适配度主要问题欧拉角3无中万向锁、角度跳变四元数4单位模长低需归一化/惩罚项旋转向量3无高超出π后映射回绕旋转矩阵9正交行列式1极低迭代后非法旋转向量在超过π时会出现回绕但标定场景中相邻帧的姿态变化通常不会超过90度这个边界基本踩不到。多传感器标定里如果出现接近180度的初始姿态先把初值转成正则范围再进LM不要指望优化器帮你处理周期性。4.3 李代数扰动 LM最小可用的外参联合优化示例下面给一个手眼标定的LM求解骨架用scipy的least_squares驱动残差里混着旋转误差和平移误差。注意旋转误差用轴角取出来而不是直接flatten旋转矩阵的9个元素后者会给LM引入冗余自由度from scipy.optimize import least_squares from scipy.spatial.transform import Rotation def pose_to_matrix(rvec, tvec): R Rotation.from_rotvec(rvec).as_matrix() T np.eye(4) T[:3, :3] R T[:3, 3] tvec return T def make_handeye_residual(observations): # observations: [(T_eb, T_ce), ...] # T_eb: 机器人基座到末端的位姿 # T_ce: 相机到标定板的位姿来自PnP求解 def residual(x): # x的前6个参数rvec_bc, tvec_bc即待求的相机在基座下的位姿 T_bc pose_to_matrix(x[:3], x[3:6]) res [] for A, C in observations: Err A T_bc C - np.eye(4) rot_err Rotation.from_matrix(Err[:3, :3]).as_rotvec() pos_err Err[:3, 3] * 100.0 # 平移误差加权毫米量级 res.append(rot_err) res.append(pos_err) return np.concatenate(res) return residual # x0 由两步解析法给出解析旋转 解析平移作为初值 res least_squares(make_handeye_residual(obs), x0, methodlm, max_nfev200, xtol1e-10, gtol1e-12) T_bc pose_to_matrix(res.x[:3], res.x[3:6])残差里旋转误差单位是弧度平移误差单位是米直接拼接会让LM的J^T J里平移列的比例尺远小于旋转列。这里乘100是把米变成厘米量级让两个量的数值范围接近。实际场景里这个权重值得认真调机器人定位误差大但视觉解算稳定的放大平移权重视觉PnP外点多的放大旋转权重。调权重没有银弹我习惯先做一次优化看两类残差各自的平均值再按它们反比设定权重。切向畸变和旋转参数化是外参标定里最容易出问题的地方。LM提供的梯度方向本质上依赖残差的平滑性PnP解出来的C一旦有大误匹配那一帧的残差会把整个优化拖偏。所以进入LM之前先把观测数据做一轮外点剔除用当前模型预测每一帧的C把预测误差大于3倍中位数的帧丢掉再跑优化。这个前置清理比调任何LM参数都重要。5. LM标定避坑发散、病态与初始化的四类翻车现场LM在标定任务里翻车绝大多数不是算法的问题而是数据、初值或实现细节的问题。以下四条边界我都实际踩过按现象→原因→解决记录在这里。5.1 重投影误差从几万开始迭代几十轮还在几千初值和归一化现象第一轮cost大得离谱LM走了几步后下降明显变慢最终停在比较大的残差值上看起来像陷入局部最优。原因内参初值给得太随意比如fx给了1.0、cx给了0。此时残差曲面在初始点附近是高度非凸的LM的局部模型二次近似和真实曲面差太远增益比ρ反复为负λ被一路放大最后变成小步梯度下降困在一个坏区域里出不来。解决fx初值给图像宽度量级cx、cy给图像中心畸变清0。这是标定的基线初值几乎不可能错。如果这样仍不收敛把数据裁剪一下先只用画面中央区域的角点跑一轮LM拿到内参后再加入全部角点精修。分阶段优化比一步到位鲁棒得多这是最不该省的操作。5.2 迭代没几步就报LinAlgError或step非有限雅可比里有NaN现象lm_solve在np.linalg.solve那行抛异常或者delta里出现NaN程序直接崩。有时不崩但cost变成nan。原因数值雅可比的中心差分采样到了残差函数的奇点。最典型的是投影函数里某点z坐标接近0或负数除以z产生无穷大另一个常见原因是畸变模型在参数太大时多项式爆炸比如k1给了0.5这种离谱初值r²乘出来的畸变量直接把坐标推到几万像素。解决投影前过滤z 0的点给这些点赋一个固定大残差比如1e6不能留NaN。数值差分步长用sqrt(eps)·max(1,|x|)的自适应策略不要用固定1e-6——fx上千和旋转向量0.1共用一个步长时必然出错。还有一点每次调用residual都要传入完整状态向量残差函数内部不要依赖任何可变全局变量否则一次差分采到不一致的数据NaN也是这么来的。5.3 误差曲线水平不动但λ一直增大阻尼更新规则写错现象cost下降几轮后进入平台期每条日志里λ都在乘以3、乘以10但cost纹丝不动像是优化器放弃了。原因增益比的分母算错了。常见的错误是把pred写成delta (lam * diagA * delta - g)这个表达式在数学上不恒等于二次模型预测下降量符号和大小都可能失真导致rho为负一步步拒绝本来可以接受的更新。另一个错误是rho阈值用反ρ 0.25时减λ、ρ 0.75时增λ让阻尼一直停在保守区域步长永远变小。解决pred统一用-0.5 * delta g这是从正规方程推导出来的闭合解不会出错。更新规则按Nielsenρ 0.75减λρ 0.25增λ其他情况维持。如果λ已经涨到1e12还在拒绝步长大概率不是λ的问题回到5.1检查初值或数据里有外点别对着λ死磕。5.4 训练残差0.02像素换个相机角度就崩过拟合与外点污染现象标定用的二十帧图像重投影误差都很漂亮但拿它去矫正新拍的图像直线边缘还是有肉眼可见的弯曲。原因两个叠加因素。一是畸变参数开得太多——普通镜头最多用k1、k2就够把k3、k4也放开后LM会用高阶分量去拟合某些帧的偶然噪声训练残差很低但泛化极差。二是参与优化的标定板图像姿态太单一都朝一个方向摆标定板覆盖的画面范围有限外参之间出现强耦合等价于自由度不足。解决先固定k3等参数为0跑一轮看残差是否仍能压低如果压低不了说明数据不够支撑更多参数。标定板的拍摄姿态必须在画面里铺开四个角落各拍几张远近结合标定板倾斜角度控制在30度上下。收敛后把训练集里误差大于2像素的那几帧拉出来看是什么原因——如果集中在某条边往往是角点检测在低对比度区域偏了0.5像素这类系统误差LM再优秀也洗不掉。6. 验证标定结果与工程习惯从残差直方图到参数置信度LM收敛之后工作还没结束。真正的检验标准不是训练集上的重投影误差而是新数据的表现。我拿到一个标定结果先做留出法验证。把二十帧数据分成两份用十九帧做LM优化留一帧不参与优化完算留出帧的重投影误差。每一帧轮流留一次得到一组独立误差。这个过程能暴露过拟合——训练误差0.02但留出误差1.0像素的说明模型复杂度超过了数据的信息量降畸变阶数或补充拍摄姿态是必然选择。这个操作成本不高但能避免把一块没有泛化能力的结果交出去。第二个习惯是读参数置信度。LM收敛后J^T J的逆再乘上残差方差就是参数的协方差近似 σ² ≈ 2·cost / (n − p)Cov ≈ σ²·(J^T J)⁻¹ 看对角线开方得到每个参数的标准差。如果fx的标准差大过几十个像素说明标定板在画面里没有覆盖足够的径向范围内参的可信度其实很低。这个信息比cost本身诚实得多。第三个习惯是画残差分布直方图。收敛后把所有残差列出来看它是不是接近零均值的钟形分布。如果直方图明显双峰说明有一批角点被系统性地误检了如果拖着长尾说明个别帧还藏着外点。我在标定流程里固定保留这个环节它帮我在十分钟内判断再跑一轮LM还是回炉重新拍数据。帧数超过几百时稠密的J^T J求解开始变慢我会切换到带Schur补的稀疏LM实现主流优化库默认就是这条路残差定义和参数逻辑完全不换迁移成本很低。但无论底层换成什么那几个习惯不会变先看初值靠不靠谱再看训练残差之外的数据表现最后才信cost数字。希望帮到你。本文还有配套的精品资源点击获取