slam_toolbox Ceres 求解器

06 Ceres 求解器

源码：solvers/ceres_solver.hpp、solvers/ceres_solver.cpp。
通过 pluginlib 注册为 karto::ScanSolver（solver_plugins.xml）。

G2O / SPA / GTSAM 的代码还在树里，默认不编译。作者对比后认为 Ceres + SPARSE_NORMAL_CHOLESKY + SCHUR_JACOBI 足够。

6.1 插件如何挂上

SlamToolbox::setSolver()：

solver_plugin 默认 "solver_plugins::CeresSolver"
solver_loader_.createSharedInstance(...)
solver_->Configure(shared_from_this())   // 读 YAML
smapper_->getMapper()->SetScanSolver(solver_.get())

反序列化时 loadSerializedPoseGraph：Reset → 遍历顶点 AddNode → 遍历边 AddConstraint → SetScanSolver → Compute 一次，把图落到优化器里。

6.2 问题定义

每个图节点是 2D 位姿 (x, y, θ)，存在 nodes_[unique_id] = Eigen::Vector3d。
θ 用 AngleManifold 参数化，避免角度跳变。

每条边是 LinkInfo：相对位姿 GetPoseDifference() + 协方差。
信息矩阵 = cov.Inverse()，再 llt().matrixU() 得到 sqrt_information，残差是标准 PoseGraph2dErrorTerm（ceres_utils.h）。

第一个节点的 (x,y,θ) 在第一次 Compute 时被 SetParameterBlockConstant，钉住规范，消除刚体自由度。

6.3 何时 Optimize

• AddNode / AddConstraint：只往 ceres::Problem 里加块，不算
• TryCloseLoop → CorrectPoses → ScanSolver::Compute：ceres::Solve
• 成功则 corrections_ 填所有节点新位姿，Karto 写回 LocalizedRangeScan::SetCorrectedPose

所以日常前端匹配不跑 Ceres，只有回环或加载旧图才跑。这是能在弱 CPU 上建大图的原因之一。

6.4 mode: localization 的含义

YAML 的 mode 主要给 Ceres 看，不是选节点类型。

mode == "localization" 时：Problem::Options.enable_fast_removal = true，方便窗口过期时 RemoveNode / RemoveConstraint。内存大约翻倍。
建图 YAML 应写 mapping，定位 YAML 写 localization。和用哪个可执行文件是两件事，但应对齐。

6.5 推荐与可调项

参数	推荐	说明
ceres_linear_solver	SPARSE_NORMAL_CHOLESKY	大图默认；可开 dynamic_sparsity
ceres_preconditioner	SCHUR_JACOBI（配置里常见）或 JACOBI	作者 benchmark 偏好 SCHUR_JACOBI
ceres_trust_strategy	LEVENBERG_MARQUARDT	DOGLEG 也可，LM 资料多
ceres_loss_function	None	里程计好时用平方损失；打滑用 HuberLoss

求解容差约 function_tolerance=1e-3（相对 5 cm 栅格是 50 倍）。num_threads=50 只是上限，实际按核数。

6.6 二次开发

换求解器：改 solver_plugin，不要改 Mapper.cpp。
真要改残差：动 PoseGraph2dErrorTerm，属于 L5。
ISlam 管理层不要直接碰 Ceres。