从零手撸Ceres BA：SLAM后端优化实战与避坑指南

1. 从零手撸Ceres BA：为什么我不建议你直接抄开源代码

搞SLAM的人迟早会撞上BA（Bundle Adjustment，光束法平差）这堵墙。不管你前面前端里程计跑得多漂亮，回环检测做得多灵敏，最后位姿和地图点能不能收敛到一块儿去，全看后端优化这一哆嗦。而Ceres Solver，基本就是这个环节里绕不开的工业级工具。很多人第一次接触Ceres BA，都是直接扒开源框架里的backend.cpp，改改参数就跑，结果一旦发散或者收敛慢，完全不知道从哪儿下手。我自己也是这么过来的，踩了无数坑之后才明白：Ceres BA这东西，你得自己从零搭一遍，把每个残差块、每个雅可比、每个求解器选项都亲手摸过，才算真正会用。

这篇内容就是把我自己反复搭Ceres BA的经验完整拆开。核心目标很明确：给定一组带噪声的位姿和地图点观测，用Ceres构建一个能跑通、能收敛、能调优的BA问题。适合谁看？如果你已经写过简单的Ceres Hello World，知道AutoDiffCostFunction大概怎么用，但一到SLAM的BA场景就懵，那这篇就是给你准备的。我会从整体设计思路讲起，把残差构建、参数块管理、求解器配置、调试排查全部过一遍，最后给你一份可以直接抄作业的完整流程。全程不依赖任何特定开源框架，纯Ceres + Eigen，你拿过去改改就能塞进自己的项目里。

2. 整体设计思路：BA到底在优化什么

2.1 先想清楚BA的数学本质

BA的本质是一个大规模非线性最小二乘问题。我们有相机位姿（旋转+平移）和三维地图点，每个观测是某个位姿下看到某个地图点产生的二维像素坐标。理想情况下，把三维点投影到该位姿的成像平面上，应该正好落在观测到的像素位置。但现实是位姿有误差、地图点有误差、观测本身也有噪声，所以投影位置和观测位置之间总有残差。BA要做的就是调整所有位姿和地图点，让这些残差的平方和最小。

用公式说，假设有m个位姿、n个地图点、k次观测，优化变量就是所有位姿的参数（每个6自由度）加上所有地图点的坐标（每个3自由度），总共6m+3n维。目标函数是：

min Σ || π(T_i, P_j) - z_ij ||²_Σ

其中π是投影函数，T_i是第i个位姿，P_j是第j个地图点，z_ij是观测到的像素坐标，Σ是观测的协方差（信息矩阵）。这个问题的规模可以非常大——几千个位姿、几十万个点，变量维度轻松上百万。所以BA的核心挑战不是“怎么定义残差”，而是“怎么在这么大的规模下高效求解”。

2.2 为什么选Ceres而不是手写高斯牛顿

你完全可以手写高斯牛顿或者LM来解BA，事实上很多教学代码就是这么干的。但到了实际项目里，手写求解器很快就会遇到几个绕不过去的问题。第一是稀疏性利用，BA的Hessian矩阵是高度稀疏的，位姿和位姿之间、位姿和点之间才有非零块，点与点之间没有直接关联。手写的话你得自己实现Schur消元，把点边缘化掉，先解位姿，再回代求点，这个逻辑写起来容易出错。第二是数值稳定性，LM的阻尼因子怎么调、信赖域怎么更新，这些细节直接影响收敛性。第三是自动微分，手写雅可比在投影函数复杂一点的时候就是噩梦。

Ceres把这些全包了。它内置了稀疏求解器（SPARSE_NORMAL_CHOLESKY、SPARSE_SCHUR等），支持自动微分，LM和DogLeg都有成熟实现。你只需要把残差定义清楚，剩下的交给它。当然，代价是你得理解它的参数块管理机制和求解器选项，不然调参的时候就是盲人摸象。

2.3 参数块与流形的设计考量

Ceres里每个优化变量都要组织成参数块（ParameterBlock）。位姿的旋转部分如果直接用旋转矩阵的9个元素表示，会有冗余和正交约束问题，Ceres提供了流形（Manifold，旧版本叫LocalParameterization）来处理。对于SO(3)，标准做法是用四元数（4维）作为参数块，然后挂一个EigenQuaternionManifold，让优化在切空间（3维）里进行。平移就是3维向量，直接作为参数块。

这里有个关键选择：位姿是用四元数+平移分开两个参数块，还是打包成一个7维数组？我建议分开。分开的好处是流形只挂在四元数上，平移不需要流形，逻辑清晰。打包的话你得自己写一个复合流形，容易出错。地图点就是3维向量，直接作为参数块，不需要流形。

另一个考量是参数块的顺序。Ceres的稀疏求解器对参数块顺序敏感，因为Schur消元需要知道哪些是“位姿块”、哪些是“点块”。通常把位姿放在前面，点放在后面，这样Ceres在SPARSE_SCHUR模式下能自动识别并做消元。如果你顺序反了，性能可能差很多。

3. 核心细节解析：残差、雅可比与信息矩阵

3.1 重投影残差怎么定义才不出错

重投影残差是整个BA的基石，定义错了后面全白搭。假设相机内参矩阵K已知（焦距fx、fy，主点cx、cy），位姿T = [R | t]，地图点P在世界坐标系下。投影过程分三步：先把P转到相机坐标系，P_cam = R * P + t；然后归一化，得到归一化坐标(u, v) = (P_cam.x / P_cam.z, P_cam.y / P_cam.z)；最后乘内参，像素坐标 = (fx * u + cx, fy * v + cy)。残差就是预测像素减去观测像素，二维向量。

这里有几个容易踩的坑。第一是坐标系约定，R是world-to-camera还是camera-to-world，t的符号，这些必须和你的前端保持一致，不然残差方向反了，优化直接发散。第二是P_cam.z的符号，如果点在相机后面，投影是无效的，残差应该被丢弃或者给一个大的惩罚。第三是内参是否参与优化，如果内参也标定得不准，可以把fx、fy、cx、cy也作为参数块加进去，但一般SLAM里内参是预先标定好的，不参与BA。

用Ceres的自动微分，残差定义大概长这样：

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 q, const T* const t, const T* const point, T* residuals) const { // q是四元数(w,x,y,z)，t是平移 Eigen::Quaternion<T> q_eigen(q[0], q[1], q[2], q[3]); Eigen::Matrix<T, 3, 1> p_world(point[0], point[1], point[2]); Eigen::Matrix<T, 3, 1> p_cam = q_eigen * p_world + Eigen::Matrix<T, 3, 1>(t[0], t[1], t[2]); if (p_cam.z() <= T(0)) { residuals[0] = T(0); residuals[1] = T(0); return true; } T x = p_cam.x() / p_cam.z(); T y = p_cam.y() / p_cam.z(); residuals[0] = fx_ * x + cx_ - T(observed_x_); residuals[1] = fy_ * y + cy_ - T(observed_y_); return true; } static ceres::CostFunction* Create(double observed_x, double observed_y) { return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 4, 3, 3>( new ReprojectionError(observed_x, observed_y)); } double observed_x_, observed_y_; double fx_ = 500.0, fy_ = 500.0, cx_ = 320.0, cy_ = 240.0; };

注意模板参数<ReprojectionError, 2, 4, 3, 3>，2是残差维度，后面三个是参数块维度：四元数4维、平移3维、地图点3维。这个顺序必须和AddResidualBlock时传入的参数块顺序一致，错一个就编译不过或者运行出错。

3.2 信息矩阵：别再用单位矩阵糊弄了

很多教程为了简单，直接把信息矩阵设为单位矩阵，也就是假设所有观测的噪声方差都是1。这在教学里没问题，但在实际项目里会出大问题。不同特征点的观测精度是不一样的，离相机近的、纹理丰富的点，像素定位精度高；离得远的、模糊的点，精度差。如果一视同仁，优化结果会被差观测拖偏。

信息矩阵是协方差矩阵的逆，通常假设观测噪声是各向同性的高斯分布，协方差是σ²I，那么信息矩阵就是(1/σ²)I。σ的单位是像素，一般取0.5到2.0之间。如果你有更精细的噪声模型，比如考虑像素坐标x和y的相关性，可以用完整的2x2矩阵。Ceres里通过AddResidualBlock的最后一个参数传入，传NULL就是单位矩阵。

ceres::LossFunction* loss = new ceres::HuberLoss(1.0); Eigen::Matrix2d info = Eigen::Matrix2d::Identity() / (sigma * sigma); ceres::CostFunction* cost = ReprojectionError::Create(obs_x, obs_y); problem.AddResidualBlock(cost, loss, q, t, point); problem.SetParameterBlockConstant(...); // 如果需要固定某个参数块

这里还涉及鲁棒核函数的选择。外点（误匹配）在SLAM里是常态，如果不用鲁棒核，一个外点就能把整个优化带偏。HuberLoss是最常用的，阈值一般取1.0到2.0。CauchyLoss更激进，适合外点比例高的情况，但可能把内点也压制了。我一般先用Huber，如果发现外点太多再换Cauchy。

3.3 流形：四元数优化的正确姿势

四元数有4个分量，但实际只有3个自由度，因为模长约束为1。如果直接在4维空间里优化，Ceres会尝试改变模长，导致数值问题。所以必须挂流形，让优化在切空间里进行。Ceres新版本用Manifold，旧版本用LocalParameterization。

// 新版本 problem.AddParameterBlock(q, 4, new ceres::EigenQuaternionManifold()); problem.AddParameterBlock(t, 3); problem.AddParameterBlock(point, 3);

EigenQuaternionManifold的约定是四元数顺序为(w, x, y, z)，和Eigen的Quaternion一致。如果你用的是其他顺序（比如xyzw），得自己写流形或者调整。这个细节很容易忽略，一旦顺序错了，优化结果就是乱的，但残差又不会报错，排查起来很痛苦。

另外，位姿的平移部分不需要流形，直接3维欧氏空间。地图点也不需要。只有旋转需要流形。如果你把位姿打包成7维（四元数+平移），那就得写一个复合流形，分别处理前4维和后3维，麻烦且容易错。所以我强烈建议分开。

4. 实操过程：从零搭建一个可收敛的BA

4.1 数据准备与问题构建

假设你有一组模拟数据：10个位姿，100个地图点，每个位姿看到大约20个点，总共约200次观测。数据格式我建议用简单的结构体：

struct Observation { int pose_id; int point_id; double pixel_x, pixel_y; }; struct Pose { Eigen::Quaterniond q; Eigen::Vector3d t; }; struct Point3D { Eigen::Vector3d p; };

构建问题时，先把所有位姿和地图点转成Ceres的参数块。位姿的四元数存成double[4]，平移存成double[3]，地图点存成double[3]。然后遍历观测，为每个观测创建CostFunction并AddResidualBlock。

ceres::Problem problem; std::vector<double*> pose_q_blocks, pose_t_blocks, point_blocks; for (auto& pose : poses) { double* q = new double[4]{pose.q.w(), pose.q.x(), pose.q.y(), pose.q.z()}; double* t = new double[3]{pose.t.x(), pose.t.y(), pose.t.z()}; problem.AddParameterBlock(q, 4, new ceres::EigenQuaternionManifold()); problem.AddParameterBlock(t, 3); pose_q_blocks.push_back(q); pose_t_blocks.push_back(t); } for (auto& pt : points) { double* p = new double[3]{pt.p.x(), pt.p.y(), pt.p.z()}; problem.AddParameterBlock(p, 3); point_blocks.push_back(p); } for (auto& obs : observations) { ceres::CostFunction* cost = ReprojectionError::Create(obs.pixel_x, obs.pixel_y); problem.AddResidualBlock(cost, new ceres::HuberLoss(1.0), pose_q_blocks[obs.pose_id], pose_t_blocks[obs.pose_id], point_blocks[obs.point_id]); }

这里有个重要细节：第一个位姿通常要固定，否则整个系统有7自由度（6自由度刚体变换+1尺度）的规范自由度，优化会漂移。固定第一个位姿的q和t：

problem.SetParameterBlockConstant(pose_q_blocks[0]); problem.SetParameterBlockConstant(pose_t_blocks[0]);

如果你有先验信息（比如IMU或者GPS），也可以给第一个位姿加一个先验残差块，而不是硬固定。硬固定简单但可能引入偏差，先验更灵活但需要额外定义残差。

4.2 求解器配置：让收敛快且稳

Ceres的求解器选项很多，但BA场景下真正关键的就几个。首先是线性求解器类型，小规模问题（几千个变量以内）用DENSE_SCHUR就行，大规模必须用SPARSE_SCHUR。SPARSE_SCHUR会利用BA的稀疏结构，先Schur消元掉地图点，解位姿，再回代，效率高很多。

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.min_linear_solver_iterations = 10; options.max_linear_solver_iterations = 500; options.num_threads = 4; options.logging_type = ceres::PER_MINIMIZER_ITERATION;

LEVENBERG_MARQUARDT比DOGLEG更稳，适合BA这种残差非线性的问题。max_num_iterations一般设50就够，BA通常10到20次迭代就收敛了。function_tolerance是目标函数变化量的阈值，gradient_tolerance是梯度范数的阈值，parameter_tolerance是参数变化量的阈值，三个满足一个就停。num_threads根据你的CPU核数设，Ceres的自动微分和部分线性求解器支持多线程。

还有一个容易忽略的选项是use_nonmonotonic_steps，设为true可以让LM在非单调方向上试探，有时候能跳出局部极小。但也会增加不稳定性，我一般先不开，遇到收敛困难再试。

4.3 迭代过程监控与收敛判断

跑起来之后，Ceres会打印每次迭代的cost、梯度范数、信赖域半径等信息。你要关注几个信号。第一是cost是否单调下降，如果震荡或者上升，说明LM阻尼不够或者残差定义有问题。第二是梯度范数是否趋近于零，如果降到1e-10以下基本就是收敛了。第三是信赖域半径，如果一直缩小到很小，说明步长被压制，可能陷入局部极小。

ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); std::cout << summary.FullReport() << std::endl;

FullReport会给出详细的迭代记录和最终状态。如果summary.termination_type是CONVERGENCE，说明正常收敛；如果是NO_CONVERGENCE，说明达到最大迭代次数还没收敛，需要调参或者检查数据；如果是FAILURE，通常是数值问题，比如Hessian矩阵奇异。

我自己的经验是，BA第一次跑不通，90%的问题出在残差定义或者坐标系约定上。先把第一个位姿固定，把地图点也固定，只优化位姿，看看能不能收敛。如果能，再放开地图点。逐步放开变量，比一上来全放开容易定位问题。

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

5.1 优化发散的几种典型原因

BA发散是最常见的问题，表现是cost越来越大或者变成NaN。我整理了几种典型原因和排查方法。

除零问题特别隐蔽，因为自动微分在p_cam.z接近0时会产生巨大的梯度。我的做法是在残差函数里加一个判断，如果p_cam.z小于某个阈值（比如0.01），直接把残差设为零并返回，相当于丢弃这个观测。虽然有点粗暴，但比NaN好。

5.2 收敛慢的性能调优

如果BA能收敛但很慢，比如每次迭代要好几秒，那通常是线性求解器的锅。首先确认你用的是SPARSE_SCHUR而不是DENSE_SCHUR。其次检查参数块顺序，位姿块必须在点块前面，否则Schur消元无法正确识别。第三，如果问题规模特别大（几十万点），可以考虑用ITERATIVE_SCHUR配合CGNR，用迭代法解线性系统，内存占用小但收敛慢一些。

还有一个技巧是options.preconditioner_type，对于ITERATIVE_SCHUR，用SCHUR_JACOBI预条件子能显著加速。但如果是SPARSE_SCHUR，预条件子不适用，因为它是直接法。

// 大规模问题的迭代求解配置 options.linear_solver_type = ceres::ITERATIVE_SCHUR; options.preconditioner_type = ceres::SCHUR_JACOBI; options.max_linear_solver_iterations = 200;

另外，num_threads别设太大，超过物理核数反而会因为线程调度开销变慢。一般设成物理核数或者物理核数的一半。

5.3 外点处理与鲁棒核调参

外点在SLAM里太常见了，误匹配、动态物体、遮挡都会产生外点。鲁棒核的阈值调不好，要么外点没压住，要么内点被误伤。我的经验是先用Huber，阈值从1.0开始试。如果发现优化结果被少数几个点带偏，说明外点比例高，换Cauchy，阈值降到0.5。如果换了Cauchy之后内点也被压制，收敛变慢，说明阈值太小，往上调。

还有一个更彻底的做法是先用RANSAC或者P3P做一轮粗筛，把明显的外点剔掉，再用BA精调。但这样会增加前端复杂度，看你的项目需求。

注意：鲁棒核的阈值单位是残差的范数，而残差是像素误差。所以阈值1.0意味着像素误差超过1个像素的观测会被降权。如果你的观测噪声本身就大于1个像素，阈值要相应调大。

5.4 参数块固定与先验的取舍

固定第一个位姿是最简单的规范自由度处理方法，但有个副作用：如果第一个位姿本身误差大，整个优化会被它锚定在错误的位置。更好的做法是给第一个位姿加一个弱先验，比如一个均值为当前估计、协方差较大的高斯先验。这样既消除了规范自由度，又允许第一个位姿在小范围内调整。

struct PosePrior { PosePrior(const Eigen::Quaterniond& q_prior, const Eigen::Vector3d& t_prior, double q_sigma, double t_sigma) : q_prior_(q_prior), t_prior_(t_prior), q_sqrt_info_(1.0 / q_sigma), t_sqrt_info_(1.0 / t_sigma) {} template <typename T> bool operator()(const T* const q, const T* const t, T* residuals) const { Eigen::Quaternion<T> q_eigen(q[0], q[1], q[2], q[3]); Eigen::Quaternion<T> q_prior(q_prior_.w(), q_prior_.x(), q_prior_.y(), q_prior_.z()); Eigen::Quaternion<T> q_diff = q_prior.inverse() * q_eigen; residuals[0] = T(q_sqrt_info_) * q_diff.x(); residuals[1] = T(q_sqrt_info_) * q_diff.y(); residuals[2] = T(q_sqrt_info_) * q_diff.z(); residuals[3] = T(t_sqrt_info_) * (t[0] - T(t_prior_.x())); residuals[4] = T(t_sqrt_info_) * (t[1] - T(t_prior_.y())); residuals[5] = T(t_sqrt_info_) * (t[2] - T(t_prior_.z())); return true; } // ... };

这个先验残差是6维的，旋转部分用四元数的虚部近似，平移部分直接相减。q_sigma和t_sigma控制先验的强度，一般设得比实际观测噪声大一个量级，让先验只起锚定作用，不主导优化。

6. 进阶技巧：让BA更贴近实际项目

6.1 局部BA与全局BA的切换

实际SLAM系统里，BA不是每次都全局跑。前端每来一帧，通常只做局部BA，优化最近几个关键帧和它们看到的地图点。只有回环检测触发时，才做全局BA。局部BA的规模小，实时性好；全局BA规模大，但频率低。

局部BA的实现关键是选好参与优化的位姿集合和点集合。位姿集合一般是最近N个关键帧加上和它们有共视关系的邻居帧。点集合是这些帧看到的所有点。固定集合外的位姿和点，只优化集合内的。这样Hessian矩阵的规模可控，求解快。

std::unordered_set<int> local_pose_ids = GetLocalPoseIds(); std::unordered_set<int> local_point_ids = GetLocalPointIds(); for (int i = 0; i < poses.size(); ++i) { if (local_pose_ids.find(i) == local_pose_ids.end()) { problem.SetParameterBlockConstant(pose_q_blocks[i]); problem.SetParameterBlockConstant(pose_t_blocks[i]); } } for (int j = 0; j < points.size(); ++j) { if (local_point_ids.find(j) == local_point_ids.end()) { problem.SetParameterBlockConstant(point_blocks[j]); } }

全局BA就是所有位姿和点都放开，只固定第一个位姿。全局BA通常跑一次要几秒到几十秒，所以一般放在后台线程，不阻塞前端。

6.2 边缘化与先验的引入

滑动窗口BA里，为了控制规模，会把老的关键帧边缘化掉。边缘化的本质是把老帧的变量消去，但它们对剩余变量的约束不能丢，得转成一个先验信息矩阵加到剩余变量上。这个先验矩阵是边缘化时Hessian矩阵的Schur补。

Ceres本身不直接支持边缘化，但你可以手动计算先验矩阵，然后用Eigen::Matrix构造一个自定义的CostFunction，把先验残差加进去。先验残差的形式是r = J * δx，其中J是先验矩阵的Cholesky分解，δx是当前估计和线性化点的差。这个实现起来比较复杂，但这是滑动窗口BA的核心，绕不过去。

我的建议是先用Ceres把全局BA跑通，理解清楚残差和求解器，再尝试边缘化。边缘化的坑比BA本身还多，尤其是先验矩阵的维护和更新，一不小心就数值不稳定。

6.3 数值精度与float/double的选择

Ceres默认用double，精度足够。但有些嵌入式平台为了省内存和算力，会用float。float的精度在BA里往往不够，因为Hessian矩阵的条件数可能很大，float的舍入误差会累积，导致收敛困难甚至发散。如果非要用float，至少把线性求解器的精度调高，并且用ITERATIVE_SCHUR配合预条件子，减少对Hessian矩阵直接分解的依赖。

另一个精度相关的点是自动微分的步长。Ceres的自动微分用对偶数（dual number）计算导数，理论上是精确的，不存在步长问题。但如果你手写雅可比，用数值差分，步长选不好会引入误差。所以能用自动微分就用自动微分，别手写。

7. 我踩过的那些坑与实操心得

第一个坑是四元数顺序。Ceres的EigenQuaternionManifold要求四元数顺序是(w, x, y, z)，但Eigen的Quaterniond构造函数是(w, x, y, z)，而有些库（比如ROS的tf）用的是(x, y, z, w)。我第一次搭的时候，从tf里拿四元数直接塞给Ceres，结果优化出来的位姿完全不对，排查了一整天才发现是顺序问题。后来我养成了一个习惯：所有四元数进出Ceres的地方，都显式地写一遍顺序转换，哪怕看起来多余。

第二个坑是信息矩阵的尺度。我一开始用单位矩阵，结果优化结果被远处的点带偏。后来把信息矩阵设成(1/σ²)I，σ取1.0，效果好很多。再后来发现不同点的σ其实不一样，就根据点的深度和观测角度给了一个粗略的权重，深度大、角度斜的点权重低。这个权重不用很精确，但有没有差别很大。

第三个坑是固定第一个位姿。我一开始没固定，优化跑完整个轨迹漂了，因为规范自由度没消除。固定之后就好了。但固定也有副作用，如果第一个位姿误差大，整个轨迹会被锚偏。后来我改成加弱先验，效果好很多。

第四个坑是鲁棒核的阈值。我用HuberLoss(1.0)，结果发现有些内点也被降权了，因为我的观测噪声本身就接近1个像素。后来把阈值调到2.0，内点保住了，外点也压住了。所以阈值一定要根据你的实际观测噪声来调，不能照搬教程。

第五个坑是线性求解器的选择。我一开始用DENSE_SCHUR，小规模数据没问题，规模一上来就慢得离谱。换成SPARSE_SCHUR之后，速度快了一个量级。但SPARSE_SCHUR要求参数块顺序正确，位姿在前、点在后，否则它识别不出Schur结构，退化成普通稀疏求解，性能优势就没了。

第六个坑是内存管理。Ceres的AddResidualBlock会接管CostFunction和LossFunction的所有权，你不需要手动delete。但参数块的double数组是你自己new的，Ceres不负责释放，你得在优化结束后自己delete[]。我一开始忘了释放，跑长时间SLAM的时候内存一直涨，后来加了RAII包装才解决。

第七个坑是线程数。我设了num_threads = 8，结果比4还慢，因为我的机器只有4个物理核，超线程反而增加了调度开销。后来改成物理核数，速度就正常了。所以num_threads不是越大越好，得看硬件。

8. 一份可以直接抄的完整配置清单

最后给你一份我常用的Ceres BA配置，小规模到中等规模（几千个位姿、几万个点）都能直接用。你拿过去改改内参和噪声参数就能跑。

ceres::Problem problem; // 参数块添加 for (auto& pose : poses) { double* q = new double[4]{pose.q.w(), pose.q.x(), pose.q.y(), pose.q.z()}; double* t = new double[3]{pose.t.x(), pose.t.y(), pose.t.z()}; problem.AddParameterBlock(q, 4, new ceres::EigenQuaternionManifold()); problem.AddParameterBlock(t, 3); // 存储指针... } for (auto& pt : points) { double* p = new double[3]{pt.p.x(), pt.p.y(), pt.p.z()}; problem.AddParameterBlock(p, 3); // 存储指针... } // 残差添加 for (auto& obs : observations) { ceres::CostFunction* cost = ReprojectionError::Create(obs.pixel_x, obs.pixel_y); ceres::LossFunction* loss = new ceres::HuberLoss(2.0); problem.AddResidualBlock(cost, loss, pose_q_blocks[obs.pose_id], pose_t_blocks[obs.pose_id], point_blocks[obs.point_id]); } // 固定第一个位姿 problem.SetParameterBlockConstant(pose_q_blocks[0]); problem.SetParameterBlockConstant(pose_t_blocks[0]); // 求解器配置 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.logging_type = ceres::PER_MINIMIZER_ITERATION; options.minimizer_progress_to_stdout = true; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); std::cout << summary.BriefReport() << std::endl;

这套配置我在多个项目里用过，收敛稳定，速度也够。如果你的问题规模更大，把SPARSE_SCHUR换成ITERATIVE_SCHUR加SCHUR_JACOBI预条件子。如果外点特别多，把Huber换成Cauchy，阈值降到1.0。如果收敛慢，先检查参数块顺序和线性求解器类型，再考虑调迭代次数和容差。

BA这东西，看再多教程不如自己跑一遍。跑通了再回头看那些公式，会发现每一行都有它的道理。