1. 从零手搓一个视觉SLAM后端为什么我劝你别跳过状态估计这一关很多人学视觉SLAM前端特征匹配、光流追踪、PnP位姿求解跑得飞起一到后端就卡住了。BA优化、位姿图、因子图、Huber核、Schur消元、边缘化这些词单拎出来都认识串在一起就不知道从哪下手。我自己当初也是这样前端能跑通就觉得自己“会SLAM”了结果一到回环检测之后位姿跳变、轨迹漂移、地图重影才发现后端才是决定整个系统能不能用的分水岭。这篇东西就是围绕“SLAM后端”这个主题把我从零搭一个后端优化模块的完整思路、踩过的坑、调参的经验从头到尾捋一遍。核心关键词就几个状态估计、非线性最小二乘、Bundle Adjustment、位姿图优化、鲁棒核函数、稀疏求解、边缘化。适合已经了解前端基本流程、想真正把后端跑起来的人也适合刚接触SLAM、想知道后端到底在干什么的读者。我不会只讲公式而是把每个选择背后的“为什么”讲清楚让你看完能自己动手写一个能用的后端。先说清楚后端到底解决什么问题。前端给你的是带噪声的观测特征点像素坐标、帧间相对位姿、回环约束。这些观测单独看都有误差累积起来轨迹就飘了。后端要做的事就是把这些所有观测放在一起找一个全局一致的位姿和地图点配置让整体误差最小。说白了前端是“猜”后端是“算总账”。这个总账怎么算用什么代价函数怎么解怎么保证解得快、解得稳就是后端全部的内容。我下面会按四个大块来讲整体设计思路、核心细节与实操要点、完整实现流程、常见问题排查。每一块都尽量给到能直接抄的代码结构和参数也会说清楚哪些地方是我踩过坑之后才明白的。2. 后端整体设计与方案选型拆解2.1 为什么后端本质是一个非线性最小二乘问题先把最核心的认知建立起来SLAM后端在数学上就是一个非线性最小二乘Non-linear Least Squares问题。你有一堆待优化的变量比如每个关键帧的位姿旋转平移6自由度、每个地图点的三维坐标3自由度。你还有一堆观测比如某个地图点在某个关键帧里被观测到的像素坐标。理想情况下把地图点投影到那个关键帧的成像平面上应该正好落在观测到的像素位置。但因为有噪声投影位置和观测位置有偏差这个偏差就是重投影误差。后端的目标就是调整所有位姿和地图点让所有重投影误差的平方和最小。写成公式就是min Σ || z_ij - π(T_i, P_j) ||²其中 z_ij 是观测π 是投影函数T_i 是位姿P_j 是地图点。这个式子看起来简单但它是非线性的因为投影函数里包含旋转和平移而且旋转是流形上的量不能直接线性加减。所以要用迭代的方法从初始值出发一步步逼近最优解。这里有个关键选择用迭代最近点还是高斯牛顿/列文伯格-马夸尔特。SLAM后端几乎不用ICP那套因为观测模型是重投影不是点对点距离。主流就是高斯牛顿Gauss-Newton, GN和列文伯格-马夸尔特Levenberg-Marquardt, LM。GN收敛快但在初始值不好或者残差很大的时候容易发散LM加了阻尼因子更稳但每步计算量大一点。我自己的经验是前端给的初始值一般不会太离谱用GN就够了但如果要做回环后的全局优化LM更保险。实际工程里很多库默认用LM比如Ceres、g2o都可以选。2.2 为什么选稀疏求解而不是稠密求解这是后端实现里最容易被忽略但影响最大的一个点。假设你有1000个关键帧每个帧观测200个点那变量总数大概是1000×6 200×1000×3也就是60万维左右。如果直接构造一个60万×60万的稠密矩阵去解内存直接爆掉计算量也是天文数字。但SLAM的BA问题有一个非常好的结构每个观测只关联一个位姿和一个地图点所以雅可比矩阵和海森矩阵都是高度稀疏的。具体来说海森矩阵H可以分块成H [ B E ] [ E^T C ]其中B是位姿块C是地图点块E是位姿和地图点之间的关联块。因为每个地图点通常只被少数几个位姿观测到C是对角块矩阵求逆非常便宜。这就是**Schur消元Schur Complement**的基础先把地图点边缘化掉得到一个只关于位姿的缩减矩阵S B - E C⁻¹ E^T解出位姿增量再回代求地图点增量。这样求解规模从几十万维降到几千维速度提升几个数量级。我实测过同样一个1000帧的BA问题稠密求解要几分钟甚至跑不动用Schur消元加稀疏Cholesky分解几秒钟就出结果。所以如果你自己写后端一定要用稀疏求解器别用Eigen的稠密LLT或者LDLT去硬解。Eigen有SparseMatrix和SimplicialLDLT配合Schur消元是入门级后端最实用的组合。2.3 位姿图优化和BA到底怎么选很多人分不清BA和位姿图优化Pose Graph Optimization, PGO。简单说BA同时优化位姿和地图点PGO只优化位姿把地图点边缘化掉用位姿之间的相对约束代替。BA精度高但计算量大适合局部窗口或者小规模场景。PGO计算量小适合大规模全局优化尤其是回环检测之后。实际系统里通常是分层后端前端附近跑一个局部BA窗口里放最近几十个关键帧和它们看到的地图点实时优化回环检测触发后跑一个全局PGO把所有关键帧的位姿调整一遍消除累积漂移。这样既保证了局部精度又控制了全局计算量。我自己的项目里局部BA窗口设的是20个关键帧全局PGO在回环时触发。这个20不是拍脑袋定的是试出来的窗口太小局部精度不够轨迹抖动明显窗口太大单次优化时间超过100ms影响实时性。你可以根据自己设备的算力调整但一般10到30之间比较合理。2.4 鲁棒核函数为什么必不可少前端匹配不可能100%正确总有误匹配。这些误匹配在后端里就是外点outlier如果直接用平方误差一个外点的残差可能比内点大几百倍优化会被它带偏导致整个轨迹崩掉。所以必须用鲁棒核函数Robust Kernel把大残差的权重降下来。常用的核函数有Huber、Cauchy、Tukey。Huber是分段函数残差小的时候是二次的残差大的时候变成线性的这样外点不会主导优化。Cauchy更激进对大残差的抑制更强。Tukey更狠直接给大残差零权重。我一般用Huber因为它在内外点之间过渡比较平滑不容易把一些边缘内点也误杀。阈值设多少经验值是1到2个像素具体要看你的重投影误差分布。你可以先跑一遍不带核函数的优化看看残差直方图把阈值设在95%分位数附近。注意鲁棒核函数不是万能的如果外点比例超过50%再好的核函数也救不回来。这种情况下要先做RANSAC或者更严格的外点剔除再进后端。3. 核心细节解析与实操要点3.1 状态变量怎么定义旋转的表示与更新后端里最麻烦的就是旋转的表示。旋转矩阵有9个元素但只有3个自由度还有正交约束不能直接当普通向量优化。四元数有4个元素也有单位长度约束。所以主流做法是在切空间里做增量更新当前旋转用旋转矩阵或四元数存优化时求一个3维的增量δθ然后用指数映射更新。具体来说如果当前旋转是R增量是δθ三维向量更新方式是R_new R * exp(δθ^)其中exp是SO(3)的指数映射δθ^是δθ的反对称矩阵。这样每次迭代只优化3维增量既没有冗余参数也不用担心约束被破坏。代码里怎么实现如果你用Ceres它自带AngleAxisRotatePoint和LocalParameterization你只需要定义旋转的局部参数化就行。如果自己写可以用Sophus库它封装了SO(3)和SE(3)的指数映射、对数映射、伴随等操作非常方便。我强烈建议自己写后端的话用Sophus省去大量推导和调试时间。实操心得旋转的增量更新一定要用右乘还是左乘这个要统一。我习惯用右乘也就是R_new R * exp(δθ^)对应的是在局部坐标系下的扰动。如果你用左乘那雅可比的推导会不一样混用会导致优化不收敛。这个坑我踩过调了一整天以为是自己雅可比推错了结果是左右乘搞混了。3.2 雅可比矩阵的推导与代码实现雅可比是后端优化的核心。重投影误差对位姿和地图点的导数直接决定了优化方向对不对。误差定义是e z - π(T, P)其中z是观测像素坐标2维π是投影函数。对地图点P的雅可比是2×3的矩阵对位姿T的雅可比是2×6的矩阵。推导过程涉及链式法则从像素坐标到归一化平面再到相机坐标系再到世界坐标系。具体公式我就不在这里全推了网上资料很多。但我要强调的是代码实现时的几个细节第一投影函数里要判断深度是否为正。如果地图点在相机后面投影会出错这时候要直接把这个观测的雅可比设为零或者给一个很大的残差让优化把它排除掉。第二像素坐标的归一化。如果你用的是内参矩阵K那投影是u fx * X/Z cx v fy * Y/Z cy对X、Y、Z的偏导是∂u/∂X fx/Z ∂u/∂Y 0 ∂u/∂Z -fx*X/Z² ∂v/∂X 0 ∂v/∂Y fy/Z ∂v/∂Z -fy*Y/Z²这些在代码里要仔细写符号错了优化方向就反了。第三位姿的雅可比要用伴随或者链式法则。如果误差对相机坐标系下的点求导得到J_cam那对位姿的雅可比是J_cam * ∂(TP)/∂δθ。这个∂(TP)/∂δθ在SE(3)上有标准形式Sophus里有现成的函数。我自己的做法是先用数值求导验证解析雅可比。具体就是给每个变量加一个小扰动看误差变化和解析雅可比对比。如果误差在1e-6量级说明推导正确。这个步骤看起来笨但能省掉大量调试时间。3.3 信息矩阵与权重为什么不能所有观测一视同仁每个观测的可信度不一样。比如特征点匹配质量高的像素误差应该给更大权重匹配质量低的权重小一点。这个权重在最小二乘里就是信息矩阵通常是协方差矩阵的逆。实际工程里最简单的做法是给所有观测同样的权重也就是信息矩阵是单位阵。但这样效果往往不好因为不同特征点的定位精度确实不同。更好的做法是用特征点提取时的尺度或者响应值来估计协方差。比如ORB特征尺度大的点定位精度低协方差应该大一点信息矩阵就小一点。我自己的项目里信息矩阵设的是对角阵对角线元素是1/σ²σ根据特征点尺度线性插值。具体来说第0层金字塔σ1像素第3层σ3像素中间线性插值。这样优化时更信任高分辨率的点轨迹精度提升明显。注意信息矩阵不要随便设得太大或太小否则会导致海森矩阵条件数变差求解不稳定。一般σ在0.5到5像素之间比较合理。3.4 边缘化怎么在滑动窗口里保持信息不丢失滑动窗口BA里旧的关键帧要移出窗口但不能直接丢掉否则会丢失约束导致轨迹漂移。这时候要用边缘化Marginalization把旧帧的信息“压缩”成先验加到剩余变量上。边缘化的数学操作就是Schur消元。假设你要边缘化变量x_m保留x_r那海森矩阵分块H [ H_mm H_mr ] [ H_rm H_rr ]边缘化后剩余变量的海森矩阵变成H_rr H_rr - H_rm * H_mm⁻¹ * H_mr同时右端项也要相应更新。这样旧变量的信息就转移到了新变量的先验里。边缘化最大的坑是填充fill-in原本稀疏的矩阵边缘化后可能变稠密导致求解变慢。所以边缘化的顺序很重要一般先边缘化观测少的地图点再边缘化旧位姿。另外边缘化会引入线性化误差如果边缘化时用的线性化点和后续优化点差太远先验就不准了。所以边缘化要尽量在局部线性化点附近进行不要拖太久。我自己的做法是滑动窗口里保留最近10个关键帧每次新帧进来边缘化最旧的那个帧和它独有的地图点。边缘化后的先验用Ceres的EvaluationCallback或者自定义的CostFunction加进去。实测下来不加边缘化轨迹跑100米就飘得没法看加了边缘化跑500米还能保持厘米级精度。4. 完整实操流程与核心环节实现4.1 环境准备与依赖选型我用的环境是Ubuntu 20.04Eigen 3.3SophusCeres Solver 2.0。为什么选Ceres而不是g2oCeres的自动求导和数值求导很方便适合快速原型g2o更轻量但需要自己写雅可比。如果你刚开始写后端我建议先用Ceres把流程跑通再考虑用g2o或者自己写求解器优化性能。安装Ceressudo apt-get install libceres-devSophus需要自己编译因为Ubuntu源里的版本可能不兼容git clone https://github.com/strasdat/Sophus.git cd Sophus mkdir build cd build cmake .. make -j4 sudo make installEigen一般系统自带如果没有sudo apt-get install libeigen3-dev4.2 数据结构设计关键帧、地图点、观测后端的数据结构要清晰不然后面优化时到处找变量很麻烦。我定义三个核心结构struct MapPoint { int id; Eigen::Vector3d position; bool is_outlier; }; struct KeyFrame { int id; Sophus::SE3d pose; std::vectorstd::shared_ptrMapPoint observations; std::vectorEigen::Vector2d pixels; }; struct Observation { int keyframe_id; int mappoint_id; Eigen::Vector2d pixel; Eigen::Matrix2d info; };关键帧里存位姿和它看到的点地图点里存三维坐标观测单独存一份方便构建优化问题。这个结构看起来简单但实际用的时候要注意地图点可能被多个关键帧看到所以用shared_ptr共享观测的像素坐标和关键帧里的pixels要对应别搞乱顺序。4.3 构建BA问题从观测到代价函数用Ceres构建BA问题核心是定义一个CostFunction。我写一个ReprojectionErrorclass ReprojectionError { public: ReprojectionError(const Eigen::Vector2d pixel, const Eigen::Matrix2d info) : pixel_(pixel), info_(info) {} template typename T bool operator()(const T* const pose, const T* const point, T* residual) const { Eigen::MatrixT,3,1 p(point[0], point[1], point[2]); Eigen::MatrixT,3,1 pc Sophus::SE3T::exp( Eigen::MatrixT,6,1(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]) ) * p; T u pc[0] / pc[2]; T v pc[1] / pc[2]; residual[0] T(pixel_[0]) - u; residual[1] T(pixel_[1]) - v; // 乘以信息矩阵的平方根 Eigen::MatrixT,2,2 sqrt_info info_.castT().llt().matrixL(); Eigen::MatrixT,2,1 r(residual[0], residual[1]); r sqrt_info * r; residual[0] r[0]; residual[1] r[1]; return true; } private: Eigen::Vector2d pixel_; Eigen::Matrix2d info_; };这里有几个细节第一位姿用6维向量表示前3维是平移后3维是旋转增量用Sophus::SE3::exp转成变换矩阵。第二信息矩阵要开平方根因为Ceres的残差是r^T * r你要让最终代价是r^T * info * r所以残差要乘以sqrt_info。第三深度pc[2]要判断正负如果是负的直接返回false或者给一个大残差。然后构建Problemceres::Problem problem; for (const auto obs : observations) { ceres::CostFunction* cost_function new ceres::AutoDiffCostFunctionReprojectionError, 2, 6, 3( new ReprojectionError(obs.pixel, obs.info)); problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0), keyframes[obs.keyframe_id].pose.data(), mappoints[obs.mappoint_id].position.data()); }HuberLoss的阈值设1.0这是像素单位的你可以根据实际误差调整。4.4 求解与迭代参数配置与收敛判断Ceres的求解选项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;linear_solver_type选SPARSE_SCHUR这就是前面说的Schur消元加稀疏求解。trust_region_strategy用LM稳。max_num_iterations设50一般20到30步就收敛了50是上限。function_tolerance和gradient_tolerance控制收敛条件设太小会浪费时间设太大会提前停止。我一般用1e-6和1e-10。求解完检查ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); std::cout summary.BriefReport() std::endl;如果summary.termination_type是CONVERGENCE说明正常收敛。如果是NO_CONVERGENCE可能是迭代次数不够或者问题本身有问题。如果是FAILURE检查一下雅可比或者数据。4.5 位姿图优化的实现全局PGO和BA类似但变量只有位姿约束是位姿之间的相对变换。我定义一个PoseGraphErrorclass PoseGraphError { public: PoseGraphError(const Sophus::SE3d measurement, const Eigen::Matrixdouble,6,6 info) : measurement_(measurement), sqrt_info_(info.llt().matrixL()) {} template typename T bool operator()(const T* const pose_i, const T* const pose_j, T* residual) const { Sophus::SE3T Ti Sophus::SE3T::exp(Eigen::MatrixT,6,1(pose_i[0],...,pose_i[5])); Sophus::SE3T Tj Sophus::SE3T::exp(Eigen::MatrixT,6,1(pose_j[0],...,pose_j[5])); Sophus::SE3T Tij Ti.inverse() * Tj; Sophus::SE3T error measurement_.castT().inverse() * Tij; Eigen::MatrixT,6,1 e error.log(); e sqrt_info_.castT() * e; for (int i 0; i 6; i) residual[i] e[i]; return true; } };这里measurement是回环检测给出的相对位姿info是信息矩阵一般回环的info比里程计的小因为回环不确定性大。求解时用SPARSE_NORMAL_CHOLESKY或者SPARSE_SCHUR都行PGO的矩阵比BA小很多。5. 常见问题与排查技巧实录5.1 优化不收敛或者发散怎么办这是最常见的问题。我遇到过的原因有几种第一初始值太差。前端位姿如果飘得厉害后端直接优化可能发散。解决办法是先跑几轮高斯牛顿用小的阻尼因子或者先用PGO把位姿粗调一遍再进BA。第二雅可比符号错了。这个最隐蔽因为优化会往错误方向走残差可能先降后升。排查方法是数值求导对比前面说过。第三外点太多。如果误匹配超过30%Huber也救不回来。这时候要先做RANSAC剔除或者用更严格的匹配阈值。第四信息矩阵设得不对。如果信息矩阵太大海森矩阵条件数爆炸求解不稳定。检查一下σ是不是设得太小。5.2 轨迹漂移严重怎么调漂移一般来自累积误差。如果局部BA窗口太小漂移会快如果边缘化没做好信息丢失也会漂。我的经验是局部BA窗口至少10帧最好20帧。边缘化时保留足够的先验信息不要一次边缘化太多变量。回环检测要灵敏回环后立刻跑全局PGO。信息矩阵根据特征点质量调整别用单位阵。我实测过同样数据窗口10帧漂移0.5米窗口20帧漂移0.1米窗口30帧漂移0.08米但计算时间翻倍。所以20帧是个平衡点。5.3 计算时间太长怎么优化后端计算时间主要花在求解线性方程上。优化方向用SPARSE_SCHUR而不是稠密求解。减少迭代次数用更好的初始值。边缘化旧变量控制问题规模。用多线程Ceres的num_threads设成CPU核心数。如果还慢考虑用GPU加速但那是另一个话题了。我自己的项目1000帧的全局PGO用SPARSE_SCHUR加4线程大概2秒。局部BA 20帧大概20毫秒完全能满足实时性。5.4 常见问题速查表问题现象可能原因排查方法解决办法优化发散初始值差、雅可比错、外点多数值求导、检查残差直方图先PGO粗调、修正雅可比、RANSAC剔除轨迹漂移窗口小、边缘化不当、回环少对比不同窗口大小、检查先验增大窗口、改进边缘化、增加回环计算慢稠密求解、迭代多、变量多计时各环节稀疏求解、减少迭代、边缘化残差降不下去信息矩阵错、核函数阈值大检查信息矩阵、调阈值调整σ、减小Huber阈值地图重影位姿不准、地图点外点可视化轨迹和点云优化位姿、剔除外点最后分享一个小技巧后端调参时先把所有观测的info设为单位阵Huber阈值设大一点比如10让优化先跑通。然后再逐步加info、调阈值、加边缘化。这样每步变化的影响都能看清楚不会一上来就一堆参数互相干扰。这个后端模块我前后改了大概两个月从最开始的稠密求解跑不动到后来稀疏Schur加边缘化能实时跑中间踩的坑基本都写在这了。你如果自己写建议先从小的仿真数据开始比如用g2o或者Ceres的样例数据把BA跑通再换成自己的数据。别一上来就上真实场景那样出了问题都不知道是数据的问题还是代码的问题。