打开Ceres-solver的examples目录很多人第一眼看到pose_graph_3d.cc时会愣一下代码能编译运行完也能输出优化后的位姿文件但如果让你说清楚“四元数参数化到底解决了什么问题”“残差为什么那样写”“信息矩阵是怎么参与计算的”大多数人就卡壳了。我在SLAM后端里泡了几年回头看这个示例发现它其实是理解位姿图优化最完整也最精炼的一个范本——节点是SE(3)位姿边是相对位姿约束整张图通过非线性最小二乘把累积漂移压掉。这篇笔记我会从数学模型、代码实现到调试经验把pose_graph_3d完整拆一遍适合正在学Ceres、准备把前端里程计和回环拼成后端优化器的人参考。1. 这个示例到底在解一个什么优化问题1.1 从SLAM的“闭环漂移”说起先想一个具体场景机器人在一条走廊里来回走视觉里程计或激光里程计给出了每一帧的位姿增量把这些增量一串起来就得到了一条连续的轨迹。问题是每个增量都有噪声这些噪声会沿着轨迹一路累积走得越远轨迹离真实位置偏差越大。等机器人回到起点时回环检测告诉你“你回到原点啦”可轨迹上的终点和起点之间可能有很大一段距离——这就是闭环漂移。位姿图优化要做的是把轨迹上的所有关键帧位姿当成优化变量把里程计相邻帧之间的相对位姿约束和回环检测给出的闭环约束都当作边一起扔进最小二乘框架里。目标是找到一组位姿让所有边上的“预测相对位姿”和“实际测量相对位姿”尽量一致。翻译成Ceres能听懂的术语节点的位姿是参数块ParameterBlock边是残差块ResidualBlock优化是求解一个大规模稀疏非线性最小二乘问题。pose_graph_3d示例用的数据就是这种位姿图每个节点保存一个三维位姿每条边保存两个节点之间的相对平移和相对旋转外加一个信息矩阵用来描述这条边的可信度。整个示例不涉及前端不涉及特征匹配纯粹聚焦后端优化本身非常适合作为学习Ceres的第二个示例——第一个是helloworld第三个是bundle_adjustment中间这个位姿图正好让你接触流形优化和稀疏求解。1.2 主流程其实只有四步不管代码里有多少行pose_graph_3d的主流程就四步读数据、建Problem、求解、写结果。第一步用ReadG2oFile读取g2o格式的位姿图文件把节点和边分别存进std::mapint, Pose3d和std::vectorConstraint3d同时记录每个边的信息矩阵。第二步遍历所有节点把平移向量和四元数分别构造成两个独立的参数块加入Ceres Problem遍历所有边为每条边创建一个6维残差块并且对四元数参数块加上Manifold老版本叫LocalParameterization。第三步配置Solver用SPARSE_NORMAL_CHOLESKY这类适合大规模稀疏问题的线性求解器做优化迭代若干次直到收敛。第四步把优化后的位姿写到文件里方便后续可视化或者喂给下游模块。代码骨架大概是这个样子// 1. 读取g2o数据 std::mapint, Pose3d poses; std::vectorConstraint3d constraints; ReadG2oFile(FLAGS_input, poses, constraints); // 2. 构建Problem ceres::Problem problem; for (const auto pose : poses) { // 平移块 四元数块 problem.AddParameterBlock(t, 3); problem.AddParameterBlock(q, 4); // 固定第一个节点消除零空间 if (pose.first 0) { problem.SetParameterBlockConstant(t); problem.SetParameterBlockConstant(q); } } for (const auto constraint : constraints) { problem.AddResidualBlock( cost_function, // 自动微分误差项 new ceres::HuberLoss(1.0), // 鲁棒核函数 t_begin, q_begin, t_end, q_end); } // 3. 求解 ceres::Solver::Options options; options.linear_solver_type ceres::SPARSE_NORMAL_CHOLESKY; options.minimizer_progress_to_stdout true; ceres::Solve(options, problem, summary); // 4. 输出优化位姿不要小看这四步每一步都藏着若干坑四元数的存储顺序、信息矩阵的上三角读取、参数块的固定策略、Manifold的添加方式。这些坑我后面逐个展开。1.3 2D和3D版本的分水岭Ceres examples里还有一个pose_graph_2d和3D版本唯一本质的区别是位姿表示方式2D位姿是(x, y, theta)三个参数优化时直接当普通欧氏向量加增量即可没有任何流形问题。3D位姿包含旋转旋转是三维流形不能用普通加法直接更新。这就是为什么3D版本的代码复杂性比2D高一大截——大部分额外代码都是在处理四元数的参数化、残差定义和存储顺序。理解了这一点你就抓住了学习pose_graph_3d的核心线索这个示例的重点不是位姿图优化本身而是“如何在优化框架里正确处理旋转”。2. SE(3)表示与四元数存储约定先算清旋转这笔账2.1 为什么最后选四元数3D位姿属于特殊欧氏群SE(3)由旋转部分SO(3)和平移部分组成。平移部分简单就是三维向量。旋转部分则有好几种表示方式欧拉角、旋转矩阵、旋转向量、四元数。每一种都能表达旋转但放进优化器里的代价差别很大。欧拉角最直观滚转、俯仰、偏航三个角一听就懂。可它有万向锁问题而且旋转是有顺序的不同坐标系定义、不同旋转顺序写出来的Rx * Ry * Rz完全不一样团队协作时特别容易出bug。旋转矩阵有9个参数但真正自由度只有3个优化器做增量加法后矩阵很容易不再正交你必须额外施加6个正交约束求解难度和工作量都直线上升。旋转向量即李代数so(3)在理论上非常优雅但在Ceres里需要自己定义完整的李群李代数操作。四元数的优势是4个参数表示3个自由度除了单位范数约束外没有别的耦合配合Ceres的QuaternionManifold增量可以定义在三维切空间上优化时不需要额外约束。而且在Ceres里四元数是最成熟的旋转参数化方案官方API直接支持省去很多手写李代数的麻烦。所以pose_graph_3d选四元数是务实的选择。2.2 Ceres 与 g2o 里的四元数顺序不一样这是我在实际项目里坑过别人的一个点。四元数有两种常见的内存顺序一种是(w, x, y, z)实部在前另一种是(x, y, z, w)实部在后。Ceres内部约定参数块里的四元数顺序是实部在前即(w, x, y, z)。而g2o文件格式里四元数的顺序是(qx, qy, qz, qw)实部在最后。看一行g2o数据VERTEX_SE3:QUAT 1 0.5 0.0 0.0 0.0 0.0 0.0 1.0这行节点表示ID为1的位姿平移是(0.5, 0.0, 0.0)四元数部分是(0.0, 0.0, 0.0, 1.0)也就是实部1放在最后。如果直接把这7个数按(x, y, z, qx, qy, qz, qw)塞进Ceres参数块Ceres会把这个四元数当成(0, 0, 0, 1)相当于实部0、虚部(0,0,1)即一百八十度绕Z轴旋转轨迹直接乱套。正确做法是在读取后把四元数重排成Ceres顺序pose_params[0] qw; // w pose_params[1] qx; // x pose_params[2] qy; // y pose_params[3] qz; // z同样的坑也出现在输出文件里。如果你要把优化结果写回给其他SLAM系统记得再转回(qx, qy, qz, qw)的顺序。2.3 单位范数与双覆盖带来的麻烦四元数表示旋转要求范数为1这是它的“流形约束”。如果优化器直接对四元数的四个分量做加减法范数很快就会飘走轻则收敛变慢重则发散。Ceres解决这个问题的方式就是Manifold后面专门讲。还有一个四元数特有的坑q和-q表示同一个旋转。这意味着四元数平面上有两个点对应同一个姿态优化器在迭代过程中可能从q跳到-q看起来残差值没变但数值上产生了跳变。Ceres的QuaternionManifold处理增量计算时会在一定范围内避免这个问题但如果你自己手动处理四元数记得检查实部符号尽量让所有四元数保持在同一半球减小跳变概率。3. 误差项的设计相对位姿残差是怎么来的3.1 位置残差把测量变换到世界系再相减假设一条边连接节点i和节点j测量值是这样一组相对约束在节点i的局部坐标系下节点j相对节点i的平移是p_ij旋转是q_ij。现在优化器给了一组候选位姿(t_i, q_i)和(t_j, q_j)问题来了怎么判断这组候选位姿符不符合这条边的测量思路是“预测-实测”。先用节点i的位姿把相对测量转换到世界坐标系得到一个预测的节点j位置p_predicted q_i * p_ij t_i然后和节点j的实际位置t_j相减r_pos t_j - p_predicted这就是位置残差。如果t_j和预测完全一致残差为0说明这条边的约束被完美满足了。代码里PoseGraph3dErrorTerm的operator()中位置残差的计算逻辑就是这样只不过换成了模板类型T让Ceres可以用自动微分对t_i、q_i、t_j求导。3.2 旋转残差误差四元数取虚部乘2旋转残差比位置残差稍微绕一点但理解后你会发现它很自然。预测的节点j朝向是q_c q_i * q_ij真实的节点j朝向是q_j。两者之间的“误差旋转”可以用四元数除法共轭乘法表示q_d q_c.conjugate() * q_j如果q_c和q_j完全一致q_d是单位四元数(1, 0, 0, 0)残差应该是0。如果有一点小偏差q_d接近单位四元数它的虚部近似等于旋转向量的一半。所以代码里取q_d的虚部并乘以2得到一个三维旋转残差向量residuals_raw[3] 2.0 * q_d.x(); residuals_raw[4] 2.0 * q_d.y(); residuals_raw[5] 2.0 * q_d.z();这个乘2是有讲究的小角度下四元数虚部约等于0.5 * theta * axis乘以2后就还原成了旋转向量theta * axis和位置的3维残差拼起来正好是6维残差向量。这样设计的另一个好处是残差的量纲是“角度”和位置残差的“长度”不会差好几个数量级信息矩阵加权时也更合理。3.3 信息矩阵残差的“权重”从哪来位姿图优化和目标函数不是简单的“所有残差平方和”每条边还要带权重。信息矩阵是协方差矩阵的逆对角线越大表示该维度测量越可信优化时希望该维度的残差压得越小。引入信息矩阵后单条边的代价是马氏距离e^T * Omega * eCeres最小化的是残差的平方和||r||^2没法直接吃一个矩阵进来所以要把马氏距离转换成普通二范数的形式。做法是取信息矩阵的平方根因子让残差向量左乘这个因子r sqrt_information * e这样||r||^2 e^T * Omega * e如果不考虑分解方向的细微差别直觉上就是把“加权残差”喂给Ceres。代码里对信息矩阵做Cholesky分解取matrixL()作为平方根信息矩阵。这里有个严谨性提示Cholesky分解满足Omega L * L^T数学上严格成立的是残差乘L^T后得到e^T * L * L^T * e e^T * Omega * e。官方示例直接用了matrixL()等效于把信息矩阵当成L^T * L来用其实也是对称正定的工程上一般影响不大。如果你自己写代码时想严格对应原始信息矩阵可以取llt.matrixU()或matrixL().transpose()。3.4 用AutoDiff让Ceres自己算雅可比位姿图残差是典型的“计算表达式麻烦、求解析导更麻烦”的模型。你当然可以手推雅可比但PoseGraph3dErrorTerm用的是Ceres最推荐的方式自动微分AutoDiff。声明一个代价函数时需要指定残差维度和每个参数块维度new ceres::AutoDiffCostFunctionPoseGraph3dErrorTerm, 6, 3, 4, 3, 4( new PoseGraph3dErrorTerm(t_ab, q_ab, sqrt_information))模板参数的含义是残差6维第一个参数块3维i的平移第二个参数块4维i的四元数第三个参数块4维j的平移注意这里顺序第四个参数块4维j的四元数。这里容易搞混的一点是示例里边的参数块顺序是t_i, q_i, t_j, q_j而代价函数模板参数顺序是6, 3, 4, 3, 4对应上去就清楚了两个节点各占两个参数块。AutoDiff的原理是让残差函数写成模板Ceres通过重载的T类型在计算残差时自动记录微分链最终得到精确到机器精度的雅可比。对初学者来说你只需要把残差表达式写对求导的事情交给Ceres。这也是为什么PoseGraph3dErrorTerm::operator()是模板函数而且内部用了Eigen::QuaternionT而不是double——这样Ceres才能做自动微分。4. 四元数的局部参数化优化器真正“动”的东西4.1 普通加法为什么会毁掉四元数走到这一步代码里最关键的技巧出现了。Ceres的迭代优化本质上是“当前值 增量”不断更新对普通向量空间来说这很自然x_new x delta。但对四元数来说四个分量加起来之后范数大概率不等于1了你等于把旋转表示带离了流形表面。短期看优化会变慢长期看会发散。更微妙的问题是四元数只有3个自由度如果优化器把4个分量都当成自由变量那整个问题就多了一个自由度Hessian矩阵会出现退化线性求解阶段就会出问题。解决思路是把增量定义在三维切空间里每个迭代步优化一个三维旋转向量delta然后通过一个“映射”把它变成一个单位四元数增量再乘到当前四元数上。这个“映射”就是Manifold做的核心工作。4.2 把位姿拆成两个Parameter Block前面提到Ceres不能直接对7维参数块说“前3维用普通加法后4维用四元数加法”因为Manifold是作用在整个参数块上的。示例代码的解决办法很直接把位姿拆成两个独立参数块一个3维的平移块一个4维的四元数块。double* t_block new double[3]; double* q_block new double[4]; t_block[0] pose.p.x(); t_block[1] pose.p.y(); t_block[2] pose.p.z(); q_block[0] pose.q.w(); q_block[1] pose.q.x(); q_block[2] pose.q.y(); q_block[3] pose.q.z(); problem.AddParameterBlock(t_block, 3); problem.AddParameterBlock(q_block, 4);这样平移块保持普通欧氏空间四元数块单独加上四元数Manifold各司其职。代价函数连接4个参数块时顺序是t_i, q_i, t_j, q_j对应AutoDiffCostFunction模板参数中的3, 4, 3, 4。这也是我在看代码时觉得最值得借鉴的设计当一个问题里同时存在普通向量和流形变量时不要把两类变量混在一个参数块里拆开反而更干净。4.3 Plus与JacobianManifold的核心Ceres的Manifold概念抽象了流形上变量更新的方式核心是Plus运算q_new Plus(q, delta)其中delta是三维向量表示切空间里的增量。四元数版本的Plus大致是把delta的范数作为旋转角度方向归一化后构造一个单位四元数增量delta_q然后右乘当前四元数并归一化// 伪代码 theta ||delta||; delta_q.w cos(theta / 2); delta_q.vec sin(theta / 2) * delta / theta; q_new (q * delta_q).normalized();Jacobian则是Plus在q处对delta的导数维度是4x3。这部分Ceres已经帮你实现好了你直接用ceres::QuaternionManifold或老版本的ceres::QuaternionParameterization即可。在老版本Ceres里代码是这样ceres::LocalParameterization* quat_param new ceres::QuaternionParameterization; problem.AddParameterBlock(q_block, 4); problem.SetParameterization(q_block, quat_param);新版本Ceres 2.2推荐使用Manifold接口ceres::Manifold* quat_manifold new ceres::QuaternionManifold; problem.AddParameterBlock(q_block, 4, quat_manifold);示例代码里用宏做了版本兼容我们学习时最好能同时认识这两个名字因为网上大量旧教程还在用QuaternionParameterization你遇到代码时得能看懂。4.4 忘加Manifold会怎样我建议每个学习这个示例的人都亲自做一次实验把四元数Manifold去掉只用AddParameterBlock(q_block, 4)然后跑优化。运行后你会看到Ceres日志里出现一条警告大意是检测到四元数参数块没有有效流形继续使用普通加法更新。优化结果通常是你依然能拿到一个“看起来收敛”的解但四元数的范数各不一样轨迹会发生明显的扭曲回环约束也闭合不上。这个实验比看十遍文档都有用。它让你直观理解Manifold在流形优化里不是可选项而是必选项。5. 从g2o文件到Ceres Problem数据流与求解器配置5.1 g2o 3D格式与信息矩阵的读法g2o格式是SLAM社区通用的位姿图存储格式pose_graph_3d直接用它作为输入。3D版本有两种常见类型VERTEX_SE3:QUAT id x y z qx qy qz qw EDGE_SE3:QUAT id1 id2 x y z qx qy qz qw边那一行在节点四元数之后还有21个数值这是6x6信息矩阵上三角部分按行排列的结果。上三角元素对应关系如下位置序号对应矩阵元素1Omega(0,0)2Omega(0,1)3Omega(0,2)4Omega(0,3)5Omega(0,4)6Omega(0,5)7Omega(1,1)8Omega(1,2)9Omega(1,3)10Omega(1,4)11Omega(1,5)12Omega(2,2)13Omega(2,3)14Omega(2,4)15Omega(2,5)16Omega(3,3)17Omega(3,4)18Omega(3,5)19Omega(4,4)20Omega(4,5)21Omega(5,5)读取时逐行处理把上三角的21个数还原成一个6x6对称矩阵。这一部看起来很简单但很容易写错上三角的索引计算错一位整个信息矩阵就是错乱的优化结果虽然不会报错但明显不合理。我在自己的项目里用g2o::SparseOptimizer加载过同一份文件和Ceres读出来的结果对比过数值确认顺序没有歧义后才放心。建议你调试时也可以打印一条边的信息矩阵看看对角线是不是预期量级的倒数。5.2 固定第一帧破坏绝对参考系的“零空间”位姿图优化目标函数有一个天然的对称性把全图所有位姿同时左乘同一个刚性变换世界系整体变换所有边的残差完全不变。这意味着问题存在一个“零空间”Hessian矩阵至少有一组零特征值线性求解器处理起来很麻烦解也不唯一。解决办法是固定住至少一个节点的位姿通常固定第一帧。代码里if (id 0) { problem.SetParameterBlockConstant(t_block); problem.SetParameterBlockConstant(q_block); }如果不固定SPARSE_NORMAL_CHOLESKY通常也能解出一个结果但轨迹可能在整体平移和旋转方向飘移。特别是当数据本身有较多噪声时不固定第一个节点优化出来的绝对位置可能和你预期的参考系差很远。如果你有多传感器融合需求、希望输出在特定坐标系下可以额外固定其它参考系已知的节点。5.3 Solver配置面向稀疏图问题的组合位姿图规模一般从几百个节点到几十万个节点不等Hessian矩阵是典型的稀疏模式。示例里的求解器配置是ceres::Solver::Options options; options.linear_solver_type ceres::SPARSE_NORMAL_CHOLESKY; options.minimizer_progress_to_stdout true;SPARSE_NORMAL_CHOLESKY用稀疏Cholesky分解求解正规方程非常适合位姿图这种稀疏结构。对于更小规模的问题可以直接用DENSE_QR或DENSE_NORMAL_CHOLESKY但对于学习这个示例的场景保持默认的稀疏求解器就好。还有一个值得开的选项是options.max_num_iterations默认值对大多数位姿图够用但如果你发现输出里显示迭代到了上限还没收敛可以调大。Solver::Summary里有很多字段可以研究比如termination_type、initial_cost、final_cost、num_successful_steps。跑完示例后我建议把Summary打印出来完整看一遍尤其是最终代价下降了多少这比肉眼看轨迹更快判断优化是否正常。5.4 遗留的内存管理与编译事项示例代码里每个节点都new了平移和四元数数组优化结束后需要手动delete[]。Ceres的Problem在构造时不会接管这些原始指针的所有权所以忘记释放会造成内存泄漏。我见过不少把示例改成自己项目的人跑完优化直接退出没管几天后程序循环优化时内存爆掉才发现问题。如果你不想手管内存可以用std::unique_ptrdouble[]之类的方式包一层。编译上Ceres本身通过CMake构建examples可以单独编译确保Eigen和gflags依赖装好就行。如果你在Ubuntu上用apt install libceres-dev装的Ceres一般自带examples的源代码放在/usr/share/doc/libceres-dev/examples附近。找不到就直接去Gitee或GitHub上拉Ceres源码仓库目录examples/slam/pose_graph_3d下面有完整工程文件。6. 跑通后的调参、可视化与踩坑记录6.1 如何编译并运行示例拿到Ceres源码后进入examples/slam/pose_graph_3d目录用CMake构建mkdir build cd build cmake .. make -j$(nproc)构建完成后运行需要指定输入文件。官方示例默认写死了pose_graph_3d.txt所以一般这样./pose_graph_3d -input /path/to/pose_graph_3d.txt -output /tmp/pose_graph_3d_optimized.txt如果你的Ceres版本比较老flags名可能不带-input而是-input_file运行./pose_graph_3d --help看下就行。启动后终端会滚动输出优化迭代日志核心关注这几项每轮的cost值是否单调下降最后termination_type是CONVERGENCE还是NO_CONVERGENCE。6.2 可视化优化前后轨迹输出文件里每一行是id x y z qx qy qz qw。我习惯写一个很小的Python脚本把原始文件和优化后文件读进来用matplotlib画成三维轨迹线节点的颜色按ID渐变这样能一眼看出回环是否闭合。import numpy as np import matplotlib.pyplot as plt def load_pose_file(path): data np.loadtxt(path) ids data[:, 0].astype(int) positions data[:, 1:4] return ids, positions ids, pos_opt load_pose_file(pose_graph_3d_optimized.txt) _, pos_raw load_pose_file(pose_graph_3d.txt) fig plt.figure(figsize(10, 10)) ax fig.add_subplot(111, projection3d) ax.plot(pos_raw[:, 0], pos_raw[:, 1], pos_raw[:, 2], r-, linewidth0.8, labelbefore) ax.plot(pos_opt[:, 0], pos_opt[:, 1], pos_opt[:, 2], b-, linewidth0.8, labelafter) ax.legend() plt.show()优化前你会看到轨迹在回环闭合处明显断开优化后头尾基本能接上。如果优化后轨迹反而更乱那大概率是数据读取顺序或信息矩阵哪里出了问题。6.3 调参经验核函数、迭代次数、信息矩阵缩放示例里的HuberLoss(1.0)值得多说两句。位姿图数据不是完美的回环检测可能给出一条错误约束里程计偶尔也会跳变如果直接最小化平方误差一条大误差边会把整个轨迹拉坏。Huber核在残差小于阈值时保持二次函数残差大于阈值后退化为线性增长相当于把异常边的权重降下来。默认阈值1.0在很多场景下偏小或偏大你可以根据残差量级调整。运行示例后观察每条边的最终残差如果最大残差异常大把Huber阈值调大一些让更多边保持正常权重如果整体残差都很小阈值可以调小增强对微小噪声的敏感性。信息矩阵的数值大小同样影响结果。如果你手头的数据是从某个SLAM系统导出的信息矩阵已经是协方差逆直接使用即可。如果自己构造数据建议让平移部分和旋转部分的残差量级匹配避免某个维度占用绝对主导权。比如平移单位是米旋转残差单位是弧度信息矩阵的对应对角线块应该粗略反映这两类测量的可信比例否则优化器会倾向满足权重大的一方。迭代次数方面位姿图优化一般迭代几十次就能收敛但如果你的图很大或者初始值远离真值有可能需要几百次。不要盲目调大迭代上限先检查初始cost是否合理、第一轮cost下降是否明显。初始cost说明你的初值离最优解多远如果在同一个数量级说明数据或者参数块构造有问题。6.4 我踩过的几个坑第一个坑是四元数顺序。读完g2o文件后没调整顺序就塞进Ceres结果优化后的轨迹在少数节点上出现明显“打结”排查了很久才发现是虚部顺序反了。从那以后我写了一个单元测试把Ceres参数块里的四元数转回g2o格式和原始数据做比较确保两个生态的转换没有歧义。第二个坑是信息矩阵的上三角读取。21个数看起来不多但行优先和列优先搞反后矩阵虽然不是对称的Ceres依然会照算不误只是加权方向错了。我调试时会把读进来的信息矩阵打印出来检查对称性和对角线正负号这一步能省很多时间。第三个坑是忘了固定第一帧。有一版代码为了测试可重复性把所有节点设为可优化结果每次运行出来的绝对轨迹都不一样因为整体旋转/平移没有参考。后来固定了第一帧问题立刻消失。第四个坑和Manifold的版本兼容有关。老项目用的Ceres 1.14只认LocalParameterization新环境装的是Ceres 2.2老接口虽然能用但会打印弃用警告。如果你的项目长期维护建议统一到新版Manifold API并写一个小的编译宏做版本过渡就像官方示例做的那样。这些坑本质上都不是Ceres的问题而是“SLAM后端的数据约定”和“Ceres的API约定”之间的映射问题。遇到时不要慌先打印关键数据再对照格式定义逐项检查一般都能快速定位。最后再分享一个我实际项目里的心得把这个示例跑通只是起点。我常常把HuberLoss换成LossFunction的自定义实现或者把四元数残差改成基于李代数so(3)的版本对比两者收敛速度和轨迹精度也会把单一位姿图扩展成多传感器融合的形式加入GPS位置先验和地面约束。pose_graph_3d给的这套“拆参数块、加Manifold、用AutoDiff写残差、配权重”的骨架可以复用到很多优化问题上。如果你正在写自己的图优化模块拿这个示例当模板改比自己从零搭要快得多也稳得多。