简介面向计算机视觉研究者的Matlab束集调整工具包聚焦三维重建、视觉SLAM、相机自标定与图像拼接中的高精度优化问题。包内实现从特征匹配、相机参数估计到非线性优化与后处理的全流程包含莱文贝格-马夸特等优化思路的Matlab代码以及Ceres求解器的混合编译入口既能帮助初学者理解BA数学模型也为进阶开发者提供可直接改写的实验基座。资源共35个文件涵盖14个m脚本残差计算、数据集生成、旋转矩阵、相机绘制、数据集读写等、8个h与4个hxx头文件、3个cpp源文件并配备CeresBA编译脚本、测试问题文本与README说明整体压缩包仅387KB轻量且结构清晰。目前已有118人学习浏览适合结合公开数据集或自采图片进行重投影误差分析、相机位姿校正并借助可运行脚本快速复现标准BA流程、对比不同初始值对优化结果的影响。1. 拿到 Bundle Adjustment for Matlab 的压缩包先想清楚里面该有什么解压一个名为 Bundle Adjustment for Matlab.zip 的包直接跑 demo 很容易但要真正用起来得先想清楚这类包里必然装了什么把相机位姿和三维点拼成状态向量的数据约定、算重投影误差的残差函数、能利用稀疏结构的 Levenberg-Marquardt 迭代。Bundle Adjustment 是运动恢复结构、视觉 SLAM 与摄影测量里最后一道精化工序本质是大规模稀疏非线性最小二乘问题。它适合两类人做三维视觉课题、需要读懂 BA 源码的研究生以及要评估 MATLAB 原型与 C 方案差异的工程师。理解了这份代码回头看 ceres 和 g2o 的 cost function 语义会很轻松。2. 从重投影误差到稀疏结构Bundle Adjustment 的数学模型与 MATLAB 表述Bundle Adjustment 的数学目标一句话就能说清同时优化 N 个相机位姿和 M 个三维点让每个可见点的重投影位置与观测位置之差在二范数意义下最小。假设第 k 条观测来自相机 c、三维点 p残差为 r_k [u,v]^T_投影 - [u,v]^T_观测其中投影通过相机内参 K、旋转矩阵 R_c 和平移 t_c 计算。总代价函数是 F(x) 0.5 * Σ_k ||r_k||²BA 的一切优化都围绕这个标量展开。MATLAB 里实现 BA 的第一个决定不是选算法而是选状态参数化。旋转用 3 参数轴角而不是 3x3 矩阵因为 9 参数带正交约束正规方程几乎必然奇异平移用 3 参数三维点用 3 参数于是每个相机贡献 6 个自由度。这个选择会在后面雅可比的所有列索引里反复出现所以在写任何代码之前先把状态向量的排布写进注释。2.1 状态向量里相机位姿与三维点怎么排布状态向量 x 的长度是 6N 3M不含内参时排布约定直接决定雅可比列顺序。常见做法是前 6N 个元素按相机序排每 6 个一组 [rx, ry, rz, tx, ty, tz]后面 3M 个元素按点序排每 3 个一组 [X, Y, Z]。这份代码里先列相机、后列点这样后面做 Schur 补分块时索引天然连续不必再做列置换。nCam max(obs(:,1)); nc 6 * nCam; % 相机部分长度 x_cam x(1:nc); x_pts x(nc1:end);观测表 obs 是 BA 的另一个核心约定每行一条观测四列分别是 [camId, ptId, u, v]所有 id 从 1 开始。用 max(obs(:,1)) 和 max(obs(:,2)) 取相机数和点数比单独传参更不容易出现索引越界。注意列顺序别调成 [u, v, camId, ptId]这种小错误在几百条观测时看不出来数据一上千就难查。2.2 Bundle Adjustment 雅可比的分块结构哪个观测动哪个变量每条观测的残差只和相机 c 的 6 个参数、点 p 的 3 个参数有关所以雅可比 J 是 2K x (6N3M) 的大矩阵但每两行最多只有 9 个非零块。于是 JJ 呈现明确的分块形态正规方程分块相机列块 (6N)点列块 (3M)相机行块 (6N)对角块稠密非对角由共视关系决定有共视处非零点行块 (3M)与右上对称严格块对角点行块为什么严格对角因为三维点之间没有直接观测约束两个点不发生投影关系时 H 对应块一定是零。这个性质不是巧合而是 BA 可解的根本原因。像素坐标量级在几百点坐标量级在零点几到几两者乘积导致 H 的条件数很大后面 4.1 的数值归一化就是为压这个问题。2.3 用 MATLAB 符号工具箱验证雅可比再写解析版手推雅可比容易在链式法则上出错我一般先用 Symbolic Math Toolbox 生成一次解析式作为后续手工实现的标尺syms rx ry rz tx ty tz X Y Z fx fy cx cy real R rotz(rz) * roty(ry) * rotx(rx); % 顺序必须与实现的旋转函数一致 Pc R * [X; Y; Z] [tx; ty; tz]; u fx * Pc(1) / Pc(3) cx; v fy * Pc(2) / Pc(3) cy; J_sym jacobian([u; v], [rx ry rz tx ty tz X Y Z]); matlabFunction(J_sym, File, block_jacobian_auto.m);matlabFunction 会把符号表达式转成独立的 .m 函数输入是旋转矩阵、相机系坐标和内参输出 2x9 雅可比块。验证时把数值差分离散求导的结果和它对比两者差超过 1e-6基本就是旋转顺序写反或者残差减号方向反了。注意 rotz、roty、rotx 的连乘顺序必须和业务代码里的旋转实现完全一致MATLAB 里绕固定轴和绕自身轴的结果不同这是验证最容易翻车的点。提示符号工具生成的 matlabFunction 不参与主循环调用只在模型变更时用来重新生成解析雅可比。3. 手写稀疏 LM 主循环Bundle Adjustment 的数据组织、雅可比拼装与阻尼更新理论立住之后决定 MATLAB 版 BA 成败的是数据组织。我重构这类包内代码时只保留三个对象cams 位姿数组、pts 点数组、obs 观测表。位姿和点都按行展开进状态向量观测表负责把残差函数和二者的对应关系固定下来。如果残差函数不是在遍历 obs 时同时算残差和雅可比而是在外面用 find 反复查索引数据量一上来性能会差一个数量级。3.1 观测表与索引映射把数据组织成 camId、ptId、像素三列观测表读进来第一件事是类型转换和去 NaN。uint16 的 id 在减法操作里容易被当成无符号数截断我一般直接转成 double。去 NaN 用 isnan 过滤整行否则雅可比里残留全零行LM 会得到一个看似正常但实际上步长异常的更新。obs 列含义类型与注意col 1camIddouble从 1 开始col 2ptIddouble从 1 开始col 3-4u, vdouble像素坐标obs double(obs); obs(any(isnan(obs), 2), :) []; nCam max(obs(:,1)); nPts max(obs(:,2)); nc 6 * nCam;这里强调 id 从 1 开始如果源数据来自 C 且从 0 起导入时统一 obs(:,1:2) obs(:,1:2) 1。这个约定在 2.1 定好后面所有函数共用避免每个函数各写一套偏移。3.2 用 sparse 拼装稀疏雅可比与残差函数残差与雅可比写在一个函数里返回值给 3.3 的 LM 循环用function [r, J] ba_fun(x, obs, K) nCam max(obs(:,1)); nc 6 * nCam; cams reshape(x(1:nc), 6, []); pts reshape(x(nc1:end), 3, []); nObs size(obs, 1); r zeros(2*nObs, 1); I zeros(2*nObs, 1); Jc zeros(2*nObs, 1); V zeros(2*nObs, 1); for k 1:nObs c obs(k,1); p obs(k,2); R axis_angle_to_so3(cams(1:3,c)); % 轴角转旋转矩阵 Pc R * pts(:,p) cams(4:6,c); % 变换到相机系 uvh K * [Pc(1)/Pc(3); Pc(2)/Pc(3); 1]; r(2*k-1:2*k) uvh(1:2) - obs(k,3:4); [Jcam, Jpt] block_jacobian_auto(R, Pc, K); col [(c-1)*61:c*6, nc (p-1)*31:nc p*3]; I(2*k-1:2*k) [2*k-1; 2*k]; Jc(2*k-1:2*k) col; V(2*k-1:2*k) [Jcam, Jpt]; end J sparse(I, Jc, V, 2*nObs, numel(x)); endsparse(I, Jc, V, m, n) 按三元组坐标拼装相同位置的元素默认累加但这里每个观测的行块互不重叠不存在重复问题。预先分配三个数组再填值比在循环里反复调用 sparse 快得多。block_jacobian_auto 就是 2.3 生成的函数输出 2x6 相机块与 2x3 点块。这里刻意没用有限差分2K x (6N3M) 的问题里差分每步都要重新算残差代价太高。3.3 LM 阻尼迭代主循环与正规方程求解LM 的迭代骨架一般是这样写function [x, hist] ba_lm(x0, obs, K) lambda 1e-3; x x0; for it 1:80 [r, J] ba_fun(x, obs, K); cost 0.5 * (r * r); hist(it) cost; A J * J; g J * r; D spdiags(diag(A), 0, size(A,1), size(A,2)); while true d -(A lambda * D) \ g; xt x d; rt ba_fun(xt, obs, K); % 只需 rt重算一次 J 略浪费 ct 0.5 * (rt * rt); if ct cost lambda max(lambda * 0.5, 1e-10); x xt; break; end lambda lambda * 2; if lambda 1e12 return; end end if norm(d) 1e-8 break; end end end阻尼放在 H 的对角上而不是单位阵上是 Marquardt 的常用变体每个参数尺度差异大时这种缩放比固定系数更容易收敛。A JJ 返回稀疏矩阵MATLAB 的 \ 对稀疏对称正定矩阵会自动选择合适的分解路径前提是你别在中间加一句 full()。收敛判据用增量范数 norm(d) 1e-8配合代价函数相对变化做双保险。内循环里重算一次 ba_fun 确实浪费工程化时可以把残差与雅可比拆开只重算残差。4. 参数配置与优化工具箱对照让 MATLAB 里的 Bundle Adjustment 稳、快、可复现手写循环能跑通之后大多数人会陷入一个误区反复调 lambda指望靠阻尼参数救回发散的优化。我一般把参数分成两组看待一组是收敛控制另一组是数值健康。matlab 优化工具箱里的 lsqnonlin 是验证手写实现最好的对照组两边结果对上才能确认不是自己把符号写错了。4.1 BA 必调参数表阻尼初值、缩放因子与收敛阈值参数典型取值作用与调节建议lambda 初值1e-3太大收敛慢太小易在初始几步震荡失败缩放x2代价没下降时快速加大步长惩罚成功缩放x0.5代价下降后放松阻尼加快沿谷底移动增量阈值1e-8norm(d) 低于该值时判定收敛最大迭代50~100大场景调大超过 200 步不收敛多半是初始化问题表里的经验值是稀疏 BA 最常见的起始点。lambda 持续翻倍到 1e8 以上仍不下降基本可以确定是残差函数有 bug 或初始值离谱不用继续等。数值归一化是 BA 里最值得做的一项预处理像素坐标动辄几百到几千点坐标可能只有零点几一个常见做法是把 u、v 减去主点后除以焦距让投影误差变成无量纲量雅可比各列量纲差距缩小一两个数量级。对应的 K 也要换成归一化版本观测和内参同步修改残差定义不变。4.2 用 lsqnonlin JacobPattern 快速对照手写实现lsqnonlin 的优势是不用自己写阻尼循环劣势是它对大规模问题仍然按稠密方式处理所以必须提供稀疏结构。严格说 JacobPattern 只影响性能不影响最终解但没它的话N、M 到几百时中间变量直接撑爆内存。对照实现如下pat spalloc(2*size(obs,1), 6*nCam 3*nPts, 2*size(obs,1)*9); for k 1:size(obs,1) c obs(k,1); p obs(k,2); pat(2*k-1:2*k, (c-1)*61:c*6) 1; pat(2*k-1:2*k, 6*nCam (p-1)*31 : 6*nCam p*3) 1; end opts optimoptions(lsqnonlin, ... SpecifyObjectiveGradient, true, ... JacobPattern, pat, ... MaxIterations, 100, ... Display, final); x_opt lsqnonlin((x) ba_fun(x, obs, K), x0, [], [], opts);SpecifyObjectiveGradient 置 true 表示残差函数第二返回值是解析雅可比lsqnonlin 不再做有限差分。注意 lsqnonlin 默认用信任域反射算法在没有边界约束时可以直接用。对照方式同一份 obs 和 x0手写 LM 的最终 cost 与 lsqnonlin 的最终 cost 应一致到小数点后 8 位以上不一致时先查 2.3 的符号雅可比再查旋转顺序最后看残差符号是否差了个负号。注意JacobPattern 必须覆盖所有可能的非零位置宁可多标不可漏标漏标会被当成恒零元素直接改掉稀疏结构。4.3 常见误用与排查列几个我在 MATLAB 版 BA 里见过最多的坑。第一把 J 或 A 转成 full() 调试N300、M1000 时密集 A 是 4800 阶方阵内存还能撑住再大三四个量级就直接崩调试稀疏结构用 spy(A) 而不是 full(A)。第二残差不除以焦距像素误差的数值让雅可比某些列占据主导收敛轨迹呈锯齿状。第三内参 K 是 3x3 但投影代码里写成只取前两行导致齐次坐标被丢弃投影结果差一个尺度。第四tol 设成 1e-12 这种过严的值数值噪声下增量永远降不到阈值白白多跑几十轮。排查顺序一般是这样先用 4.2 的 lsqnonlin 拿到参考解再把手写 LM 的每步 cost 打出来对比下降曲线。手写实现前几步下降慢但后面追上多半是阻尼缩放系数问题如果从头就发散优先怀疑雅可比符号。5. 验证与进阶合成场景自检再用 Schur 补消元5.1 合成场景自检误差降到噪声水平才算收敛判断 BA 收敛不能看 cost 是否变成零要看它是否降到噪声水平。常见做法是先生成带真值的数据立方体内随机布点球面上放相机并朝向原点投影后加 1 像素高斯噪声再把位姿和点扰动 5% 作为初值。rng(1); Xgt 5 * randn(3, 500); % 生成相机位姿、投影观测加 sigma1px 噪声 x0 perturb(xgt, 0.05); x_opt ba_lm(x0, obs, K); [~, J] ba_fun(x_opt, obs, K); C J * J; unc sqrt(diag(C \ speye(size(C))));期望的最终代价约等于噪声方差之和的一半即 0.5 * 2K * sigma^2。若最终 cost 比这个值高一个数量级说明优化没有真正收敛若低得多则可能是噪声加得不够或过度拟合。这里求协方差属于验证用途大规模场景不要直接求逆用稀疏 Cholesky 分解或只取对角。5.2 Schur 补消元把点变量先消掉相机规模减半的经典技巧最后一个实用技巧是 Schur 补消元。正规方程分块写成 [C W; W P]P 对应三维点部分且是块对角所以可以先把点增量表示成相机增量的函数消去 P 块。这个操作在 ceres 和 g2o 里叫 marginalization也是经典 SBA 库的核心步骤。C A(1:nc, 1:nc); W A(1:nc, nc1:end); P A(nc1:end, nc1:end); gC g(1:nc); gP g(nc1:end); S C - W * (P \ W); dxC S \ (gC - W * (P \ gP)); dxP P \ (gP - W * dxC);P 是块对角P\W 等价于对每个 3x3 块分别反解代价几乎线性S 的维度是 6N远小于原问题。把这段替换 3.3 循环里的 d -(A lambda*D)\g每步迭代都能明显提速观测数上万的点云上尤其明显。自己写的 BA 到这一步基本就具备往增量式 BA 和滑动窗口平滑推进的基础了。本文还有配套的精品资源点击获取 MATLAB中的Bundle Adjustment：稀疏LM优化与Schur补实现
简介面向计算机视觉研究者的Matlab束集调整工具包聚焦三维重建、视觉SLAM、相机自标定与图像拼接中的高精度优化问题。包内实现从特征匹配、相机参数估计到非线性优化与后处理的全流程包含莱文贝格-马夸特等优化思路的Matlab代码以及Ceres求解器的混合编译入口既能帮助初学者理解BA数学模型也为进阶开发者提供可直接改写的实验基座。资源共35个文件涵盖14个m脚本残差计算、数据集生成、旋转矩阵、相机绘制、数据集读写等、8个h与4个hxx头文件、3个cpp源文件并配备CeresBA编译脚本、测试问题文本与README说明整体压缩包仅387KB轻量且结构清晰。目前已有118人学习浏览适合结合公开数据集或自采图片进行重投影误差分析、相机位姿校正并借助可运行脚本快速复现标准BA流程、对比不同初始值对优化结果的影响。1. 拿到 Bundle Adjustment for Matlab 的压缩包先想清楚里面该有什么解压一个名为 Bundle Adjustment for Matlab.zip 的包直接跑 demo 很容易但要真正用起来得先想清楚这类包里必然装了什么把相机位姿和三维点拼成状态向量的数据约定、算重投影误差的残差函数、能利用稀疏结构的 Levenberg-Marquardt 迭代。Bundle Adjustment 是运动恢复结构、视觉 SLAM 与摄影测量里最后一道精化工序本质是大规模稀疏非线性最小二乘问题。它适合两类人做三维视觉课题、需要读懂 BA 源码的研究生以及要评估 MATLAB 原型与 C 方案差异的工程师。理解了这份代码回头看 ceres 和 g2o 的 cost function 语义会很轻松。2. 从重投影误差到稀疏结构Bundle Adjustment 的数学模型与 MATLAB 表述Bundle Adjustment 的数学目标一句话就能说清同时优化 N 个相机位姿和 M 个三维点让每个可见点的重投影位置与观测位置之差在二范数意义下最小。假设第 k 条观测来自相机 c、三维点 p残差为 r_k [u,v]^T_投影 - [u,v]^T_观测其中投影通过相机内参 K、旋转矩阵 R_c 和平移 t_c 计算。总代价函数是 F(x) 0.5 * Σ_k ||r_k||²BA 的一切优化都围绕这个标量展开。MATLAB 里实现 BA 的第一个决定不是选算法而是选状态参数化。旋转用 3 参数轴角而不是 3x3 矩阵因为 9 参数带正交约束正规方程几乎必然奇异平移用 3 参数三维点用 3 参数于是每个相机贡献 6 个自由度。这个选择会在后面雅可比的所有列索引里反复出现所以在写任何代码之前先把状态向量的排布写进注释。2.1 状态向量里相机位姿与三维点怎么排布状态向量 x 的长度是 6N 3M不含内参时排布约定直接决定雅可比列顺序。常见做法是前 6N 个元素按相机序排每 6 个一组 [rx, ry, rz, tx, ty, tz]后面 3M 个元素按点序排每 3 个一组 [X, Y, Z]。这份代码里先列相机、后列点这样后面做 Schur 补分块时索引天然连续不必再做列置换。nCam max(obs(:,1)); nc 6 * nCam; % 相机部分长度 x_cam x(1:nc); x_pts x(nc1:end);观测表 obs 是 BA 的另一个核心约定每行一条观测四列分别是 [camId, ptId, u, v]所有 id 从 1 开始。用 max(obs(:,1)) 和 max(obs(:,2)) 取相机数和点数比单独传参更不容易出现索引越界。注意列顺序别调成 [u, v, camId, ptId]这种小错误在几百条观测时看不出来数据一上千就难查。2.2 Bundle Adjustment 雅可比的分块结构哪个观测动哪个变量每条观测的残差只和相机 c 的 6 个参数、点 p 的 3 个参数有关所以雅可比 J 是 2K x (6N3M) 的大矩阵但每两行最多只有 9 个非零块。于是 JJ 呈现明确的分块形态正规方程分块相机列块 (6N)点列块 (3M)相机行块 (6N)对角块稠密非对角由共视关系决定有共视处非零点行块 (3M)与右上对称严格块对角点行块为什么严格对角因为三维点之间没有直接观测约束两个点不发生投影关系时 H 对应块一定是零。这个性质不是巧合而是 BA 可解的根本原因。像素坐标量级在几百点坐标量级在零点几到几两者乘积导致 H 的条件数很大后面 4.1 的数值归一化就是为压这个问题。2.3 用 MATLAB 符号工具箱验证雅可比再写解析版手推雅可比容易在链式法则上出错我一般先用 Symbolic Math Toolbox 生成一次解析式作为后续手工实现的标尺syms rx ry rz tx ty tz X Y Z fx fy cx cy real R rotz(rz) * roty(ry) * rotx(rx); % 顺序必须与实现的旋转函数一致 Pc R * [X; Y; Z] [tx; ty; tz]; u fx * Pc(1) / Pc(3) cx; v fy * Pc(2) / Pc(3) cy; J_sym jacobian([u; v], [rx ry rz tx ty tz X Y Z]); matlabFunction(J_sym, File, block_jacobian_auto.m);matlabFunction 会把符号表达式转成独立的 .m 函数输入是旋转矩阵、相机系坐标和内参输出 2x9 雅可比块。验证时把数值差分离散求导的结果和它对比两者差超过 1e-6基本就是旋转顺序写反或者残差减号方向反了。注意 rotz、roty、rotx 的连乘顺序必须和业务代码里的旋转实现完全一致MATLAB 里绕固定轴和绕自身轴的结果不同这是验证最容易翻车的点。提示符号工具生成的 matlabFunction 不参与主循环调用只在模型变更时用来重新生成解析雅可比。3. 手写稀疏 LM 主循环Bundle Adjustment 的数据组织、雅可比拼装与阻尼更新理论立住之后决定 MATLAB 版 BA 成败的是数据组织。我重构这类包内代码时只保留三个对象cams 位姿数组、pts 点数组、obs 观测表。位姿和点都按行展开进状态向量观测表负责把残差函数和二者的对应关系固定下来。如果残差函数不是在遍历 obs 时同时算残差和雅可比而是在外面用 find 反复查索引数据量一上来性能会差一个数量级。3.1 观测表与索引映射把数据组织成 camId、ptId、像素三列观测表读进来第一件事是类型转换和去 NaN。uint16 的 id 在减法操作里容易被当成无符号数截断我一般直接转成 double。去 NaN 用 isnan 过滤整行否则雅可比里残留全零行LM 会得到一个看似正常但实际上步长异常的更新。obs 列含义类型与注意col 1camIddouble从 1 开始col 2ptIddouble从 1 开始col 3-4u, vdouble像素坐标obs double(obs); obs(any(isnan(obs), 2), :) []; nCam max(obs(:,1)); nPts max(obs(:,2)); nc 6 * nCam;这里强调 id 从 1 开始如果源数据来自 C 且从 0 起导入时统一 obs(:,1:2) obs(:,1:2) 1。这个约定在 2.1 定好后面所有函数共用避免每个函数各写一套偏移。3.2 用 sparse 拼装稀疏雅可比与残差函数残差与雅可比写在一个函数里返回值给 3.3 的 LM 循环用function [r, J] ba_fun(x, obs, K) nCam max(obs(:,1)); nc 6 * nCam; cams reshape(x(1:nc), 6, []); pts reshape(x(nc1:end), 3, []); nObs size(obs, 1); r zeros(2*nObs, 1); I zeros(2*nObs, 1); Jc zeros(2*nObs, 1); V zeros(2*nObs, 1); for k 1:nObs c obs(k,1); p obs(k,2); R axis_angle_to_so3(cams(1:3,c)); % 轴角转旋转矩阵 Pc R * pts(:,p) cams(4:6,c); % 变换到相机系 uvh K * [Pc(1)/Pc(3); Pc(2)/Pc(3); 1]; r(2*k-1:2*k) uvh(1:2) - obs(k,3:4); [Jcam, Jpt] block_jacobian_auto(R, Pc, K); col [(c-1)*61:c*6, nc (p-1)*31:nc p*3]; I(2*k-1:2*k) [2*k-1; 2*k]; Jc(2*k-1:2*k) col; V(2*k-1:2*k) [Jcam, Jpt]; end J sparse(I, Jc, V, 2*nObs, numel(x)); endsparse(I, Jc, V, m, n) 按三元组坐标拼装相同位置的元素默认累加但这里每个观测的行块互不重叠不存在重复问题。预先分配三个数组再填值比在循环里反复调用 sparse 快得多。block_jacobian_auto 就是 2.3 生成的函数输出 2x6 相机块与 2x3 点块。这里刻意没用有限差分2K x (6N3M) 的问题里差分每步都要重新算残差代价太高。3.3 LM 阻尼迭代主循环与正规方程求解LM 的迭代骨架一般是这样写function [x, hist] ba_lm(x0, obs, K) lambda 1e-3; x x0; for it 1:80 [r, J] ba_fun(x, obs, K); cost 0.5 * (r * r); hist(it) cost; A J * J; g J * r; D spdiags(diag(A), 0, size(A,1), size(A,2)); while true d -(A lambda * D) \ g; xt x d; rt ba_fun(xt, obs, K); % 只需 rt重算一次 J 略浪费 ct 0.5 * (rt * rt); if ct cost lambda max(lambda * 0.5, 1e-10); x xt; break; end lambda lambda * 2; if lambda 1e12 return; end end if norm(d) 1e-8 break; end end end阻尼放在 H 的对角上而不是单位阵上是 Marquardt 的常用变体每个参数尺度差异大时这种缩放比固定系数更容易收敛。A JJ 返回稀疏矩阵MATLAB 的 \ 对稀疏对称正定矩阵会自动选择合适的分解路径前提是你别在中间加一句 full()。收敛判据用增量范数 norm(d) 1e-8配合代价函数相对变化做双保险。内循环里重算一次 ba_fun 确实浪费工程化时可以把残差与雅可比拆开只重算残差。4. 参数配置与优化工具箱对照让 MATLAB 里的 Bundle Adjustment 稳、快、可复现手写循环能跑通之后大多数人会陷入一个误区反复调 lambda指望靠阻尼参数救回发散的优化。我一般把参数分成两组看待一组是收敛控制另一组是数值健康。matlab 优化工具箱里的 lsqnonlin 是验证手写实现最好的对照组两边结果对上才能确认不是自己把符号写错了。4.1 BA 必调参数表阻尼初值、缩放因子与收敛阈值参数典型取值作用与调节建议lambda 初值1e-3太大收敛慢太小易在初始几步震荡失败缩放x2代价没下降时快速加大步长惩罚成功缩放x0.5代价下降后放松阻尼加快沿谷底移动增量阈值1e-8norm(d) 低于该值时判定收敛最大迭代50~100大场景调大超过 200 步不收敛多半是初始化问题表里的经验值是稀疏 BA 最常见的起始点。lambda 持续翻倍到 1e8 以上仍不下降基本可以确定是残差函数有 bug 或初始值离谱不用继续等。数值归一化是 BA 里最值得做的一项预处理像素坐标动辄几百到几千点坐标可能只有零点几一个常见做法是把 u、v 减去主点后除以焦距让投影误差变成无量纲量雅可比各列量纲差距缩小一两个数量级。对应的 K 也要换成归一化版本观测和内参同步修改残差定义不变。4.2 用 lsqnonlin JacobPattern 快速对照手写实现lsqnonlin 的优势是不用自己写阻尼循环劣势是它对大规模问题仍然按稠密方式处理所以必须提供稀疏结构。严格说 JacobPattern 只影响性能不影响最终解但没它的话N、M 到几百时中间变量直接撑爆内存。对照实现如下pat spalloc(2*size(obs,1), 6*nCam 3*nPts, 2*size(obs,1)*9); for k 1:size(obs,1) c obs(k,1); p obs(k,2); pat(2*k-1:2*k, (c-1)*61:c*6) 1; pat(2*k-1:2*k, 6*nCam (p-1)*31 : 6*nCam p*3) 1; end opts optimoptions(lsqnonlin, ... SpecifyObjectiveGradient, true, ... JacobPattern, pat, ... MaxIterations, 100, ... Display, final); x_opt lsqnonlin((x) ba_fun(x, obs, K), x0, [], [], opts);SpecifyObjectiveGradient 置 true 表示残差函数第二返回值是解析雅可比lsqnonlin 不再做有限差分。注意 lsqnonlin 默认用信任域反射算法在没有边界约束时可以直接用。对照方式同一份 obs 和 x0手写 LM 的最终 cost 与 lsqnonlin 的最终 cost 应一致到小数点后 8 位以上不一致时先查 2.3 的符号雅可比再查旋转顺序最后看残差符号是否差了个负号。注意JacobPattern 必须覆盖所有可能的非零位置宁可多标不可漏标漏标会被当成恒零元素直接改掉稀疏结构。4.3 常见误用与排查列几个我在 MATLAB 版 BA 里见过最多的坑。第一把 J 或 A 转成 full() 调试N300、M1000 时密集 A 是 4800 阶方阵内存还能撑住再大三四个量级就直接崩调试稀疏结构用 spy(A) 而不是 full(A)。第二残差不除以焦距像素误差的数值让雅可比某些列占据主导收敛轨迹呈锯齿状。第三内参 K 是 3x3 但投影代码里写成只取前两行导致齐次坐标被丢弃投影结果差一个尺度。第四tol 设成 1e-12 这种过严的值数值噪声下增量永远降不到阈值白白多跑几十轮。排查顺序一般是这样先用 4.2 的 lsqnonlin 拿到参考解再把手写 LM 的每步 cost 打出来对比下降曲线。手写实现前几步下降慢但后面追上多半是阻尼缩放系数问题如果从头就发散优先怀疑雅可比符号。5. 验证与进阶合成场景自检再用 Schur 补消元5.1 合成场景自检误差降到噪声水平才算收敛判断 BA 收敛不能看 cost 是否变成零要看它是否降到噪声水平。常见做法是先生成带真值的数据立方体内随机布点球面上放相机并朝向原点投影后加 1 像素高斯噪声再把位姿和点扰动 5% 作为初值。rng(1); Xgt 5 * randn(3, 500); % 生成相机位姿、投影观测加 sigma1px 噪声 x0 perturb(xgt, 0.05); x_opt ba_lm(x0, obs, K); [~, J] ba_fun(x_opt, obs, K); C J * J; unc sqrt(diag(C \ speye(size(C))));期望的最终代价约等于噪声方差之和的一半即 0.5 * 2K * sigma^2。若最终 cost 比这个值高一个数量级说明优化没有真正收敛若低得多则可能是噪声加得不够或过度拟合。这里求协方差属于验证用途大规模场景不要直接求逆用稀疏 Cholesky 分解或只取对角。5.2 Schur 补消元把点变量先消掉相机规模减半的经典技巧最后一个实用技巧是 Schur 补消元。正规方程分块写成 [C W; W P]P 对应三维点部分且是块对角所以可以先把点增量表示成相机增量的函数消去 P 块。这个操作在 ceres 和 g2o 里叫 marginalization也是经典 SBA 库的核心步骤。C A(1:nc, 1:nc); W A(1:nc, nc1:end); P A(nc1:end, nc1:end); gC g(1:nc); gP g(nc1:end); S C - W * (P \ W); dxC S \ (gC - W * (P \ gP)); dxP P \ (gP - W * dxC);P 是块对角P\W 等价于对每个 3x3 块分别反解代价几乎线性S 的维度是 6N远小于原问题。把这段替换 3.3 循环里的 d -(A lambda*D)\g每步迭代都能明显提速观测数上万的点云上尤其明显。自己写的 BA 到这一步基本就具备往增量式 BA 和滑动窗口平滑推进的基础了。本文还有配套的精品资源点击获取