1. 从一次轨迹漂移说起为什么BA是SLAM后端的命门去年帮一个做割草机器人定位的朋友排查问题时遇到一个特别典型的场景机器人在小院子里跑前几百米轨迹还挺准一旦绕到灌木丛边上、特征点变稀疏回环之后整条轨迹就像被人拽了一把歪出去半米多。前端光流跟踪看着没问题回环检测也触发了可优化完的位姿就是不对。最后定位到根因——后端Bundle Adjustment的鲁棒核函数参数设得太激进几个误匹配的外点把整个优化带偏了。这件事让我重新把Ceres Solver这套BA工具链从头到尾捋了一遍。SLAMSimultaneous Localization and Mapping同时定位与建图这个领域前端负责“看得见”后端负责“算得准”而BABundle Adjustment光束法平差就是后端里最核心的那台发动机。不管你是做视觉SLAM、激光SLAM还是视觉惯性里程计只要涉及位姿图优化和地图点联合优化Ceres几乎是绕不开的选择——它稳定、接口干净、自动微分省心工业界和学术界都在用。这篇内容我打算把Ceres BA在SLAM里的完整落地路径讲透从问题建模、残差构建、雅可比推导到Ceres的工程配置、求解器调参、鲁棒核选择再到实际调试中那些文档里不会写的坑。适合已经了解SLAM基本概念、想动手把后端优化跑通的同学也适合做过前端但后端一直调不明白的工程师。看完你应该能自己搭一套可用的BA优化模块并且知道出问题时该往哪个方向查。2. BA到底在优化什么把问题拆到残差层面2.1 重投影误差BA的唯一主角BA的本质其实一句话能说清调整相机位姿和三维地图点让每个地图点投影到图像上的位置尽量接近它实际被观测到的像素位置。这个“接近程度”就是重投影误差也是我们要最小化的目标。假设世界坐标系下有一个三维点 $P_w [X, Y, Z]^T$某一帧相机的位姿是 $T_{cw}$从世界到相机的变换那么它在该帧相机坐标系下的坐标是$$P_c R_{cw} \cdot P_w t_{cw}$$再经过针孔相机模型投影到归一化平面加上内参 $K$ 得到像素坐标$$p_{pred} K \cdot \frac{P_c}{Z_c}$$而实际观测到的像素是 $p_{obs}$那么残差就是$$r p_{obs} - p_{pred}$$整个BA问题就是所有观测残差的平方和最小化。听起来简单但真正落地时有三个关键决策点每一个都会直接影响优化结果。2.2 三个必须想清楚的问题第一个问题是参数化方式。位姿用旋转矩阵加平移向量还是四元数还是李代数这不是数学洁癖而是直接影响优化稳定性的工程问题。旋转矩阵有正交约束直接优化会破坏约束四元数有单位模长约束而李代数 $\mathfrak{se}(3)$ 是无约束的最适合无约束优化器。Ceres里通常用Sophus或者自己写的SE3参数块配合LocalParameterization新版本叫Manifold来处理。第二个问题是雅可比怎么求。残差对位姿、对地图点的偏导手推容易出错尤其是涉及旋转的部分。Ceres提供了三种方式解析雅可比、数值雅可比、自动微分。自动微分最省心但计算量比解析大解析雅可比最快但推导和实现成本高。实际项目里我一般先用自动微分把流程跑通性能瓶颈出现后再针对性替换成解析雅可比。第三个问题是外点怎么处理。SLAM前端给的匹配不可能100%正确总有误匹配。如果直接用最小二乘一个外点的残差可能是内点的几百倍平方之后直接把优化拉飞。所以必须用鲁棒核函数Robust Kernel把大残差的权重压下去。Huber、Cauchy、Tukey是三个最常用的选哪个、阈值设多少后面会详细讲。2.3 稀疏性是BA能实时跑的底层原因有人会问一张图几百个特征点几十帧参数动辄上千维直接解正规方程不是要爆炸吗答案是稀疏性。BA的Hessian矩阵有个特殊结构每个残差只关联一个相机位姿和一个地图点所以Hessian里位姿块和地图点块之间是稀疏的只有对角块和关联块非零。利用这个结构可以用Schur Complement先把地图点边缘化掉只解位姿再回代求地图点。这就是所谓的Bundle Adjustment稀疏求解也是Ceres内部自动帮你做的事。理解这一点很重要因为它解释了为什么Ceres的SPARSE_NORMAL_CHOLESKY和DENSE_SCHUR、SPARSE_SCHUR这些求解器选项存在以及什么时候该选哪个。小规模问题用稠密求解没问题规模一上来必须走Schur路径。3. Ceres工程落地从零搭一个可用的BA模块3.1 环境准备与依赖选择Ceres的安装本身不复杂但依赖版本容易踩坑。我一般用这套组合Eigen 3.3以上Ceres强依赖、glog或gflags日志可选、SuiteSparse稀疏求解必需、CXSparse轻量替代。Ubuntu下直接apt装最省事sudo apt-get install libceres-dev libeigen3-dev libsuitesparse-dev如果要自己编译最新版注意Ceres 2.0之后API有变化LocalParameterization被标记为废弃改用Manifold。很多老教程还在用旧接口照抄会编译报错。我建议新项目直接用2.1以上版本用AutoDiffManifold或者Sophus配合EigenQuaternionManifold。提示如果你用的是ROS注意ROS自带的Ceres版本可能和系统版本冲突编译时用find_package(Ceres REQUIRED)并确认CERES_INCLUDE_DIRS指向的是你想要的那个。3.2 残差块的构建一个完整的重投影残差Ceres的核心是CostFunction。对于重投影误差我通常定义一个仿函数functor输入是相机位姿7维四元数平移、地图点3维、观测像素2维输出是2维残差。struct ReprojectionError { ReprojectionError(double observed_x, double observed_y) : observed_x_(observed_x), observed_y_(observed_y) {} template typename T bool operator()(const T* const camera, const T* const point, T* residuals) const { // camera: [qw, qx, qy, qz, tx, ty, tz] T p[3]; // 四元数旋转 Eigen::QuaternionT q(camera[0], camera[1], camera[2], camera[3]); Eigen::Mapconst Eigen::MatrixT,3,1 pt(point); Eigen::MapEigen::MatrixT,3,1 p_map(p); p_map q * pt; p[0] camera[4]; p[1] camera[5]; p[2] camera[6]; // 针孔投影 T xp p[0] / p[2]; T yp p[1] / p[2]; // 内参 const T fx T(520.9); const T fy T(521.0); const T cx T(325.1); const T cy T(249.7); residuals[0] fx * xp cx - T(observed_x_); residuals[1] fy * yp cy - T(observed_y_); return true; } static ceres::CostFunction* Create(double observed_x, double observed_y) { return new ceres::AutoDiffCostFunctionReprojectionError, 2, 7, 3( new ReprojectionError(observed_x, observed_y)); } double observed_x_, observed_y_; };这里有几个细节值得说。第一四元数在Ceres里作为参数块时必须配EigenQuaternionManifold旧版是QuaternionParameterization否则优化过程中四元数会失去单位模长约束。第二AutoDiffCostFunction的模板参数Functor, 残差维度, 参数块1维度, 参数块2维度必须和实际一致写错了编译能过但运行结果错。第三内参如果也要优化就再加一个参数块但一般标定好的内参固定住更稳。3.3 参数块与Manifold配置构建Problem的时候参数块的添加方式决定了优化器怎么看待这些变量ceres::Problem problem; for (auto obs : observations) { ceres::CostFunction* cost ReprojectionError::Create(obs.px, obs.py); problem.AddResidualBlock(cost, new ceres::HuberLoss(1.0), obs.camera_pose, obs.point); } // 固定第一帧位姿消除规范自由度 problem.SetParameterBlockConstant(poses[0]);这里有两个关键操作。一是SetParameterBlockConstant固定第一帧因为BA存在规范自由度整体旋转平移不影响重投影误差不固定的话Hessian奇异求解器会报错或者给出无意义解。二是四元数参数块要设置Manifoldproblem.SetManifold(quaternion_block, new ceres::EigenQuaternionManifold());注意如果你用的是Ceres 2.0以下版本对应接口是SetParameterization和QuaternionParameterization别混用。3.4 求解器选项每个参数都有理由Solver::Options里参数几十个但真正影响结果的就那么几个。我一般这么配ceres::Solver::Options options; options.linear_solver_type ceres::SPARSE_SCHUR; options.trust_region_strategy_type ceres::LEVENBERG_MARQUARDT; options.max_num_iterations 50; options.function_tolerance 1e-6; options.gradient_tolerance 1e-10; options.parameter_tolerance 1e-8; options.num_threads 4; options.minimizer_progress_to_stdout true;SPARSE_SCHUR是BA场景的默认选择利用稀疏结构加速。LEVENBERG_MARQUARDT比DOGLEG更稳尤其在初始值不太好的时候。max_num_iterations别设太大BA一般10到20次迭代就收敛了设50是给难例留余量。num_threads根据机器来Ceres的并行主要在残差求值和部分线性求解上4到8线程收益明显。4. 调参实战鲁棒核、初值与规模控制4.1 鲁棒核函数怎么选、阈值怎么定鲁棒核是BA抗外点的第一道防线。三个常用核的特性差异很大核函数对大残差的抑制适用场景阈值经验值Huber线性增长温和外点比例低10%1.0~2.0像素Cauchy对数增长较强外点比例中等0.5~1.5像素Tukey超过阈值直接归零外点比例高3~5像素阈值单位是残差的尺度。重投影残差单位是像素所以阈值直接按像素设。我踩过的坑是阈值设太小内点被误伤优化欠拟合设太大外点没压住轨迹漂移。经验做法是先统计内点残差的分布取中位数的2到3倍作为Huber阈值。实操心得如果发现优化后残差分布有明显长尾说明还有外点没处理干净这时候换Cauchy或者调小阈值比加迭代次数管用。4.2 初值质量决定收敛速度BA是非凸优化初值好坏直接决定能不能收敛到全局最优。前端给的位姿和三角化点如果误差太大BA要么收敛慢要么掉进局部极小。我的做法是先用PnP或者对极几何给一个粗略初值跑一次只有位姿的优化固定地图点再放开地图点联合优化。这种分步策略比一上来全放开稳得多。另外地图点的初值深度很关键。深度太小的点投影误差对位姿特别敏感容易主导优化方向。我一般会过滤掉深度小于某个阈值的点或者给深度加一个先验约束。4.3 问题规模与求解器切换小规模几十帧、几百点用DENSE_SCHUR甚至DENSE_QR都行代码简单。规模上去之后必须切SPARSE_SCHUR否则内存和时间都扛不住。判断标准很简单参数维度超过几百或者Hessian非零元素占比低于10%就该走稀疏路径。如果发现求解特别慢先看minimizer_progress_to_stdout输出的每次迭代耗时。如果线性求解占大头检查是不是Schur补的顺序没优化好Ceres有ordering选项可以调AMD或COLAMD。5. 常见问题排查那些文档不会告诉你的坑5.1 优化不收敛或残差反弹最常见的原因是外点没处理好或者初值太差。排查顺序先看每次迭代的残差变化如果残差先降后升多半是LM的阻尼策略问题可以调initial_trust_region_radius如果残差一直不降检查雅可比是不是推错了用数值微分对比一下自动微分的结果。5.2 轨迹整体漂移但局部一致这种通常是规范自由度没固定或者固定错了帧。固定第一帧是标准做法但如果第一帧本身观测质量差可以固定一个观测最多的帧。另外检查是不是有孤立的地图点只被一帧观测到这种点约束太弱容易带偏优化建议过滤掉观测数少于2的点。5.3 求解速度突然变慢先看问题规模是不是涨了再看是不是有大量外点导致迭代次数增加。还有一个隐蔽原因参数块的内存布局不连续导致缓存命中率低。Ceres对参数块的内存对齐有要求用Eigen::aligned_allocator分配或者确保参数块是连续存储的。5.4 常见问题速查表现象可能原因排查方向残差不下降雅可比错误、初值太差数值微分对比、分步优化残差先降后升阻尼策略、外点调LM参数、换鲁棒核轨迹整体漂移规范自由度未固定固定首帧或高质量帧求解慢规模大、外点多、内存布局差切稀疏求解、过滤外点结果随机多线程竞争、未初始化检查线程安全、初始化变量避坑技巧调试BA时先把num_threads设为1排除并行带来的不确定性确认逻辑正确后再开多线程。6. 性能优化与工程化建议6.1 从自动微分到解析雅可比自动微分开发快但每个残差都要构建表达式树计算量比解析大几倍。当BA成为性能瓶颈时把热点残差换成解析雅可比是收益最明显的优化。重投影误差的解析雅可比推导不算复杂网上有成熟公式关键是注意旋转部分的链式法则别漏项。6.2 边缘化与滑动窗口如果要做长时间运行的SLAM不能把所有历史帧都留在优化窗口里否则规模无限增长。滑动窗口加边缘化是标准方案固定窗口大小把滑出的帧通过Schur补边缘化成先验。Ceres本身不直接提供边缘化接口需要自己实现或者用GTSAM这类专门做因子图的库。但如果只是做局部BACeres足够。6.3 增量式优化每次新来一帧就全量优化太浪费。增量式BA只优化受影响的局部区域其余固定。Ceres可以通过SetParameterBlockConstant灵活控制哪些块参与优化实现简单的增量策略。7. 我个人在实际操作中的几点体会调BA这件事数学推导只占三成剩下七成是工程细节和调试经验。我见过太多人公式推得漂亮代码一跑就崩问题往往出在参数块配置、鲁棒核阈值、初值处理这些“脏活”上。我的建议是先把最小可运行版本跑通用仿真数据验证正确性再上真实数据。真实数据的外点分布、初值质量、观测噪声都和仿真差很远直接上真机会让你怀疑人生。另外Ceres的日志一定要开minimizer_progress_to_stdout输出的每次迭代信息是排查问题的第一手资料比任何猜测都靠谱。最后分享一个小技巧如果优化结果时好时坏先固定随机种子、关掉多线程把问题变成确定性的再逐步放开。很多所谓的“随机bug”其实是并行竞争或者未初始化内存导致的确定性环境下复现一次问题就清楚一半了。