SLAM 算法阅读记录：Traj-LO

• 纯雷达slam，类似ct-icp的连续时间思想
• 项目地址：https://github.com/kevin2431/Traj-LO
• 论文地址：https://ieeexplore.ieee.org/document/10387726

---

1. 代码阅读

1.1 主函数

run_trajlo.cpp 文件就是主节点，根据运行时输入的命令的参数 argv[1] 来获得配置文件的路径

1.1.1 配置类 TrajConfig

• 实例化一个 配置类 traj::TrajConfig config; 对象， 将命令行中的配置文件的路径传入这个类中：config.load(config_path);，功能就是读取配置文件中的参数的内容；

1.1.2 雷达里程计类 TrajLOdometry

• 实例化一个雷达里程计类对象指针 traj::TrajLOdometry::Ptr trajLOdometry(new traj::TrajLOdometry(config)); ，采用传参构造，传入的配置类对象来获取配置文件中的部分需要的参数；

   TrajLOdometry::TrajLOdometry(const TrajConfig& config)
      : config_(config), isFinish(false) {
   min_range_2_ = config.min_range * config.min_range;//设置点云的最小距离阈值（平方值）,过滤掉距离太近的激光点，避免自遮挡或噪声点
   converge_thresh_ = config.converge_thresh;//优化迭代的收敛判断阈值,当位姿增量小于此值时，认为优化收敛，停止迭代
  
   laser_data_queue.set_capacity(100);//设置激光雷达数据队列的最大容量
  
   init_interval_ = config.init_interval; //系统启动时的第一个时间窗口大小
   window_interval_ = config.seg_interval; //每个时间段的持续时间
   max_frames_ = config.seg_num; //滑动窗口中最多保留的帧数,控制内存使用和计算复杂度
  
   map_.reset(new MapManager(config.ds_size, config.voxel_size,
                              config.planer_thresh, config.max_voxel_num,
                              config.max_range));
   //ds_size：点云降采样网格大小; voxel_size：体素地图的体素边长; planer_thresh：平面拟合的阈值; 
   //max_voxel_num：地图中最大体素数量; max_range：激光雷达的最大有效距离
  
   // setup marginalization
   marg_H.setZero(POSE_SIZE, POSE_SIZE);
   marg_b.setZero(POSE_SIZE); //初始化边缘化的Hessian矩阵和梯度向量  //维度：POSE_SIZE = 6（3个旋转 + 3个平移）
   double init_pose_weight = config.init_pose_weight; 
   marg_H.diagonal().setConstant(init_pose_weight);
   //设置初始位姿的先验信息权重,为第一个位姿提供稳定性约束,权重越大，初始位姿越稳定
   }
  

  1.1.3 数据载入类 DataLoader

• 实例化一个数据载入类指针对象为空指针： traj::DataLoader::Ptr dataLoader = nullptr; ;
• 然后通过配置文件中的雷达的类型：config.type 来根据不同的雷达的类型，来采用不同的字段读取点云；
• 当读取后如果 DataLoader 中的tbb::concurrent_bounded_queue<Scan::Ptr> *laser_queue = nullptr; 该指针非空指针，就将读取的点云数据利用对应的 pub 函数存入到这个指针中； 该指针构造时是空指针，当用户点击可视化窗口的对应的数据集的相关按键时才会进行读取存储点云，不点击时不进行任何操作。
• 发布

1.1.4 可视化类 Visualizer

• 通过之前的三个类 trajLOdometry, config, dataLoader 来实例化一个可视化类指针，进行一些窗口的初始化操作；
• 主循环：visualizer->mainLoop();，其中 { drawPanel(); }// 绘制主要的控制面板和3D场景 这部分中实现了点击对应的部分来设置代码中里程计中的一些标志位，启用里程计：odometry->Start();; 并且还有上文中存储点云的空指针的赋值：dataset->laser_queue = &odometry->laser_data_queue; ,此时点云开始发布到里程计的点云队列 odometry->laser_data_queue 中去.

1.2 里程计的启动函数 void TrajLOdometry::Start()

• 这里是一个lambda函数，死循环中执行着核心的功能；

1.2.1 点云分割

• 从之前从传感器中读取的点云数据队列 laser_data_queue 中 获取当前最新的点云数据 curr_scan，然后通过一个布尔值 first_scan 来判断是否是第一次扫描：

  如果是第一次扫描，这个初始分段点云的起止时间分别为：初始扫描的的开始时间戳 last_begin_t_ns_ = curr_scan->timestamp;、开始时间加上配置文件中的初始时间间隔last_end_t_ns_ = last_begin_t_ns_ + init_interval_;

• 调用 void TrajLOdometry::PointCloudSegment(Scan::Ptr scan, Measurement::Ptr measure) 函数，功能：

  将传入的扫描点云 curr_scan 中的点进行筛选时间在 last_begin_t_ns_ 和 last_end_t_ns_ 之间的，(之所以在代码中不考虑 last_begin_t_ns_ 之后的，是因为代码的逻辑已经保证了不会有“过早”的点遗留，点云处理是单调时间递增的，数据的上游经过按照时间排序的处理，因此每个点的时间戳只会往后，不会出现“比 last_begin_t_ns_ 还早”的点。)

  如果点在时间段之间，就将点存入 points_cache 中，然后点超出了时间范围，就将 之前存的那个时间段的 points_cache; 存入 measure中，并设置对应的起止时间为对应的时间 measure->tp = {last_begin_t_ns_, last_end_t_ns_};；

  再将 measure 存入 measure_cache 中去，之后清空 points_cache，measure，并重设下一次的起止时间，下一次的开始时间是这一次的结束时间，下一次的结束时间是开始时间加上配置中的窗口时间间隔。

• 然后measure_cache存的一个一个measure就是不同时间段的点

1.2.2 计算时间插值因子

• 去掉一些坐标为nan 的点，筛选到雷达距离为配置文件中设置的距离的点，len < min_range_2_
• 从 measure_cache 弹出最新的测量点云 measure ，计算这一段点云每个点的时间在整个时间段的占比，即时间插值因子，
• 将符合条件的点以及对应的插值因子 alpha 存入 points 中 points.emplace_back(Eigen::Vector4d(p.x, p.y, p.z, alpha));

1.2.3 初始化地图

• 如果没有初始化地图，就对初始时间段的点云 measure 中提取的有效点points组织为体素网格结构，进行初始化地图
• 将当前帧的位姿状态存储在frame_poses_映射中，然后存的键是时间对的结束时间，
• 将初始位姿添加到轨迹容trajectory_中，初始化假设静止，T_wc_curr = Sophus::SE3d();初始化当前位姿为单位变换，这表示世界坐标系与当前帧坐标系重合（无旋转、无平移）
• 将初始化标志位设置为true。

1.2.4 PoseStateWithLin结构体

• 作用：用于管理SLAM系统中位姿的线性化状态和增量更新

1.2.4.1 核心设计理念

• PoseStateWithLin 的核心思想是分离线性化点和当前估计值：

线性化点：优化过程中固定的参考点 当前位姿：基于线性化点加上增量的实时估计值 这种设计解决了非线性优化中的关键问题：当优化变量在迭代过程中变化时，如何保持目标函数线性化的稳定性。

1.2.4.2 关键成员变量

bool linearized;          // 是否已线性化（标记线性化点是否已固定）
VecN delta;               // 从线性化点开始的累积增量（李代数形式）
PoseState<Scalar> pose_linearized; // 线性化时刻的位姿状态（时间戳+位姿）
SE3 T_w_i_current;       // 当前位姿（世界坐标系到当前帧坐标系）

1.2.4.3 关键函数解析

applyInc(const VecN& inc) 函数

• 1. 未线性化状态,基准位姿直接更新：pose_linearized.T_w_i 代表当前真实位姿,增量处理：每次调用 applyInc() 时，增量直接应用到 pose_linearized.T_w_i,使用场景：初始化阶段或未加入优化问题的普通帧
• 1. 已线性化状态,基准位姿固定：pose_linearized.T_w_i 被冻结，不再改变,增量累积：所有增量累加到 delta,当前位姿计算：T_w_i_current = pose_linearized.T_w_i * exp(delta),使用场景：已加入优化问题的关键帧

getPose() const 函数

if (!linearized) {
    return pose_linearized.T_w_i;  // 直接返回基准位姿
} else {
    return T_w_i_current;          // 返回基于线性化点+增量的当前位姿
}

1.2.5 地图已初始化后

1.2.5.1 先验信息设置

• 这一时间段的测量点云的先验设置为上一时间段点云从开始时刻到结束时刻的相对位姿变化 measure->pseudoPrior = T_prior;
• 预测这一时间段点云结束时刻的位姿 Sophus::SE3d T_w_pred = frame_poses_[tp.first].getPose() * T_prior;，开始时刻的位姿乘相对位姿变化
• 设置初始位姿初始猜想值：frame_poses_[tp.second] =PoseStateWithLin<double>(tp.second, T_w_pred);//未线性化，非线性优化需要有一个良好的初始值猜想，才能够更快地收敛下去。

1.2.5.2 点云降采样

• map_->PreProcess(points, tp);函数

1.2.5.3 运动状态判断

• 判断是否已离开初始位置（平移距离>0.5m），防止在初始位置微小抖动导致地图初始化失败

if (!isMove_) {//判断是否远离初始点
            isMove_ = (T_w_pred).translation().norm() > 0.5;
          } else {
            map_->ComputeThreshold();//根据误差来动态计算配准阈值
          }

• 动态阈值计算：一旦开始移动，根据当前配准误差动态调整配准阈值，使系统对不同运动状态有自适应能力

  计算当前模型误差：model_deviation_ ：上一时间段的预测与实际偏差,这个偏差反映了运动模型的准确性

  运动显著性过滤：当机器人几乎静止时，微小的测量噪声可能导致较大的相对误差,这些样本不能反映真实的模型精度,只有当运动足够大时（超过min_motion_th_），才认为误差样本是有效的

  动态阈值计算：计算历史模型误差的均方根（RMS）,这是统计学中衡量误差大小的标准方法,核心思想：阈值应该与系统的历史表现相匹配

1.2.5.4 优化 Optimize()

• 绝对顺序映射构建，为每个位姿变量创建索引映射，将离散的位姿转换为连续的优化变量。假设有N个位姿，则优化变量维度为6N，根据索引来确定优化的是哪个变量

• 迭代优化循环 初始化全局Hessian矩阵、梯度向量；

• 执行点云配准，map_->PointRegistrationNormal()函数，

   void MapManager::PointRegistrationNormal(
    const posePairLin &ppl,        // 优化过程中两帧的位姿(包含是否线性化)
    const tStampPair &tp,          // 时间戳对 (前一帧, 当前帧)
    Eigen::Matrix<double, 12, 12> &H_icp,  // 输出的Hessian矩阵
    Eigen::Matrix<double, 12, 1> &b_icp,   // 输出的残差向量
    double &error,                 // 累积的误差
    double &inliers                // 内点数量
   )
  

  输入：开始和结束时刻的位姿估计、时间段内的点云 输出：Hessian矩阵(H)、梯度向量(b)、误差和内点数量 核心价值：计算点云匹配的残差及其对位姿的导数，用于后续优化

  初始化一些参数：range_thresh：距离阈值（3倍配准阈值），用于筛选有效匹配点；th：鲁棒核函数参数；Weight：Cauchy核函数，残差较小时给予高权重，残差较大时降低权重，减少异常值影响

  获取从开始到结束时的点云位姿增量：`tangent

  并行处理对应时间段中点云的每个点， 通过线性插值，使用之前计算的时间插值因子、位姿增量来计算每个点的位姿，然后将点的坐标由雷达坐标系转换为世界坐标系

   double alpha = ds_points_reg[i].w(); 
   Sophus::SE3d T_b_i = Sophus::se3_expd(alpha * tangent);
   Sophus::SE3d T_w_i = pp.first * T_b_i; //插值后的 中间时刻的位姿
   Eigen::Vector3d point = ds_points_reg[i].head<3>();
   Eigen::Vector3d p_in_world = T_w_i * point; // 变换到世界坐标系
  

  进行体素搜索这个点的世界坐标，找到最近的5个邻近点，进行平面拟合，求解平面的法向量 平面方程： $ Ax+By+Cz=−1 $ 法向量（归一化后）： $[A,B,C]^T$ 然后根据设定的阈值进行判断

  计算点对平面残差：计算目标点云中与当前点最接近的点 和 当前点的向量差residual，在点对平面ICP中，真正的残差点到平面的垂直距离，是计算它在法向量方向的投影。真正的残差应该是： $r=n^T ⋅residual$ 这里可能是作为近似？

  使用之前定义的Cauchy核函数进行鲁棒权重计算，当残差较小时 $(residual2 « th²)：w ≈ 1$ ，给予高权重，当残差较大时 $(residual2 » th²)：w ≈ th²/residual2$ ，权重逐渐减小，这是一种鲁棒核函数，有效降低异常值对优化的影响

  构建信息矩阵 Information：为 $nn^T$

  检查开始或结束点是否被设置为线性化的点，如果设为线性化，则重新计算 这里有个问题，为什么线性化的点坐标不转化为世界坐标系，然后进行体素搜索， ai的答案是：在 “残差点坐标” 和 “Jacobian线性化点” 是两件事：

  点坐标 (p_in_world)： 残差一定要在当前估计位姿下算，这样才能反映迭代过程中点和地图的真实几何关系。 如果用线性化点的位姿去投影点云，那残差就不会收敛到真实值，优化会“死”在旧的线性化点上。 👉 所以残差对应的点坐标必须用 getPose()（当前位姿）计算。

  Jacobian 的线性化点： 边缘化的核心要求：雅可比必须在固定的 getPoseLin() 上计算，保持和之前边缘化矩阵一致。 如果 Jacobian 在新的位姿下重新计算，会破坏 Schur 消元后的信息矩阵，导致整个优化器数值不稳定。 👉 所以 Jacobian 部分必须用 getPoseLin() 来重算。

  计算雅可比矩阵，累积 H 和 b

• 执行运动约束

  计算实际相对运动，T_be是开始时刻到结束时刻的相对运动位姿，然后转为李代数 tau

  计算残差res = tau - Sophus::se3_logd(m.second->pseudoPrior);，pseudoPrior 是 先验运动，通常来自 IMU 预积分或上一帧的运动估计。这里是采用开始时刻的运动计算估计得到，res就是实际运动和先验运动的差异。如果 res ≈ 0，说明运动符合预期；如果 res 很大，说明几何匹配和运动学约束有矛盾。

  处理线性化点，和点云配准那里一样： 残差 res 始终基于当前位姿来计算； 但 Jacobian 必须在固定的线性化点 计算，保持边缘化一致性。 所以如果某帧被线性化了，就用 getPoseLin() 重新计算相对运动 T_be 和 tau。

  计算雅可比矩阵，J_T_w_e：残差对末端位姿的雅可比。J_T_w_b：残差对起始位姿的雅可比。

  把雅可比变换到世界系，rr_b

  组合总雅可比J_be，前 6 列对应 前一帧位姿的增量；后 6 列对应 当前帧位姿的增量。

  累积到 H 和 b，这里还乘了一个权重 alpha_e：config_.kinematic_constrain：人为调节运动约束的重要性；lastInliers：前面 ICP 的有效匹配点数，点数越多说明几何约束越可靠，运动约束可以适当减弱。

• 把局部约束累积到全局 H 和 b

  每个 measurement（二帧之间的约束：ICP 约束、运动约束）在局部构建了一个 Hessian 矩阵 delta_H 和 残差向量 delta_b，大小是12×12 和12×1。这里用 abs_id 找到该局部变量在全局优化向量中的 起始索引，把局部贡献累积到全局 abs_H 和 abs_b。

• 边缘化误差项

  边缘化的思想：当窗口滑动时，最老的帧会被移除，但它对其他帧的约束信息不能丢。于是我们把它的贡献折叠进一个残差（marginalization factor），形式还是H,b。 marg_H, marg_b 就是之前计算好的边缘化误差项（通常通过 Schur complement 消元得到）。

  delta 是被边缘化的帧的累积增量，用来修正残差。

• 求解优化问题

   Eigen::VectorXd update = abs_H.ldlt().solve(abs_b);
      double max_inc = update.array().abs().maxCoeff();
      if (max_inc < converge_thresh_) {
      break;
   }
  

• 应用更新

   for (auto& kv : frame_poses_) {
   int idx = aom.abs_order_map.at(kv.first).first;
   kv.second.applyInc(update.segment<POSE_SIZE>(idx));
   }
  

  遍历每个关键帧，把对应的增量应用到位姿上。applyInc()函数， 如果该帧是未线性化的，直接加在位姿上；如果是线性化的，就累积到 delta。

• 更新运动伪先验

  每个 measurement 对应一段相对运动。 pseudoPrior 是该段运动的“预测值”，在下一次优化迭代时作为 运动连续性约束的参考。 更新方式：用优化后的位姿重新计算相邻两帧之间的相对位姿作为新的 prior。

1.2.5.5 获取优化后的位姿 & 更新模型偏差 & 更新下一时刻的运动先验

T_wc_curr：优化之后的当前位姿。

T_w_pred：先验预测位姿（例如里程计或上一时刻的运动推算）。

model_deviation：预测与优化结果之间的偏差，表示“模型误差”。

map_->UpdateModelDeviation()：更新地图对这种偏差的统计/修正，用于 模型一致性和漂移补偿。

从当前优化结果中提取 相对运动（start → end）。这个 T_prior 会作为 下一时刻的运动先验，给下一次迭代时的约束提供初值。

   T_prior = frame_poses_[tp.first].getPose().inverse() *
            frame_poses_[tp.second].getPose();

1.2.5.6 边缘化最老的帧 Marginalize();

• 只有当窗口中 帧数超过阈值 max_frames_ 时，才会执行边缘化。

• measurements.begin()：map里是按时间排序的，所以 begin() 就是最老的帧段。pp：拿到该段的 起始帧和结束帧位姿。map_->Update(pp, tp)：把这一段的点云信息 整合到全局地图，避免点云丢失。

• 准备边缘化所需的 Hessian 和残差 delta：最老帧的 位姿增量（优化过程中得到的）。

  delta_H, delta_b：该段的约束所对应的 Hessian矩阵 和 残差向量。维度是 12x12，因为这段涉及 起点和终点两个SE3位姿（6+6维）。

• 累积已有的边缘化先验

   marg_H_new.topLeftCorner<POSE_SIZE, POSE_SIZE>() += marg_H;
   marg_b_new.head<POSE_SIZE>() -= marg_b;
   marg_b_new.head<POSE_SIZE>() -= (marg_H * delta);
  

  把历史边缘化信息 合并到新矩阵的左上角块（对应旧帧）。marg_b 和 delta 被用来修正残差，保证一致性。

• 执行舒尔补消元（真正的边缘化）

   Eigen::MatrixXd H_mm_inv =
      marg_H_new.topLeftCorner<6, 6>().fullPivLu().solve(
         Eigen::MatrixXd::Identity(6, 6));
   marg_H_new.bottomLeftCorner<6, 6>() *= H_mm_inv;
  
   marg_H = marg_H_new.bottomRightCorner<6, 6>();
   marg_b = marg_b_new.tail<6>();
   marg_H -= marg_H_new.bottomLeftCorner<6, 6>() * marg_H_new.topRightCorner<6, 6>();
   marg_b -= marg_H_new.bottomLeftCorner<6, 6>() * marg_b_new.head<6>();
  

  把被边缘化的 旧帧变量 (m) 从优化问题里消掉，只保留对剩余帧 (c) 的约束。

• 清理旧数据

  删除最老帧的位姿和测量。

  把它保存到 trajectory_（历史轨迹，用于记录）。

  把边缘化后剩下的帧 tp.second 标记为 线性化帧（在边缘化后保持一致性）。

1.2.5.7 可视化更新

• 隔一定帧数进行可视化

2. 论文阅读

概念定义

雅可比矩阵推导

在高斯-牛顿框架下，线性化得到 $E \approx (e + J\xi)^{\top} W^{-1} (e + J\xi)$ ，并形成正规方程 $H\xi=-b$ ， 其中 $H=J^{!\top}W^{-1}J, b=J^{!\top}W^{-1}e$ 。 根据这几个式子，我们要求的是增量向量 $\xi$（把所有控制位姿的 6D 增量堆叠而成，维度是 $6(K+1)$。步骤是：

• 用当前线性化点 $s=[T_0,\dots,T_K]$ 计算误差 $e$ 与雅可比 $J$ ，装配 $H,b$ ；
• 解线性方程 $H\xi=-b$（通常用稀疏 Cholesky/PCG；若 $H$ 近奇异，会用 LM 衰减 $H+\lambda I$ ；
• 将各块增量分发到位姿：$T_k \leftarrow T_k \oplus \xi_k$ ；
• 迭代重复直至收敛 $||xi||$ 很小或目标下降很小。

能得到的结果：

• 直接解出本次迭代的最优增量 $\xi$ ；
• 由此更新得到一组更优的控制位姿 ${T_k}$；
• 迭代收敛后得到整个时间窗内的连续时间轨迹 $T(t)$ 的最优解（在当前数据与先验下的局部极小）。 同时，$H$ 体现了问题的二阶近似（信息矩阵/曲率），其稀疏块结构对应“每个误差只连少量位姿”的性质，有助于高效求解。

Gauss–Newton 与连续时间配准项的雅可比推导（含 9a–9d, 10）

我们关注的配准误差为（点到平面）： \(e \;\triangleq\; \mathbf{n}^\top \Big(\, \phi_k(t_i)\,T_{LB_j}\,\mathbf{p} \;-\; \mathbf{q} \,\Big),\) 其中 $\mathbf{n}$ 为地图局部平面法向量，$\mathbf{p}$ 是 LiDAR $j$ 坐标系下的原始点，$\mathbf{q}$ 是地图中最近邻点（世界系）， $T_{LB_j}$ 为 LiDAR $j$ $\to$ 参考系外参， $\phi_k(t)$ 是第 $k$ 段的连续时间位姿。第 $k$ 段 内的轨迹参数化为 \(\phi_k(t) \;=\; T_{k-1}\;\Exp\big(\alpha\,\tau^k\big),\qquad \tau^k \;=\; \Log\!\big(T_{k-1}^{-1}T_k\big),\quad \alpha=\tfrac{t-t_{k-1}}{\Delta t_k}.\)

---

9a：链式法则的结构

误差只与当前所在段的两个控制位姿 $T_{k-1},T_k$ 有关， 因此对整堆状态 $s=[T_0,\dots,T_K]$ 的导数呈块稀疏形式。对任意线性化点 $s$ 的右扰动 $s\oplus\xi$ 做一阶展开 \(e(s\oplus\xi)\;\approx\; e(s)\;+\;\frac{\partial e}{\partial s}\,\xi,\) 其中 \(\boxed{\; \frac{\partial e}{\partial s} \;=\; \frac{\partial e}{\partial \phi_k(t_i)} \;\Big[\; \frac{\partial \phi_k(t_i)}{\partial T_{k-1}} \quad \frac{\partial \phi_k(t_i)}{\partial T_k} \;\Big] \;} \tag{9a}\)

这就是文中的 9a：先对中间量 $\phi_k(t_i)$ 求导（“几何项”），再乘上 $\phi_k$ 对端点位姿的导数（“插值项”）。

---

9b： $\displaystyle \frac{\partial e}{\partial \phi_k(t_i)}$ 的推导（几何项）

令 \(\mathbf{X}\;\triangleq\; T_{LB_j}\,\mathbf{p}, \qquad \phi_k(t_i)\;=\;\begin{bmatrix}R & \mathbf{t}\\ \mathbf{0}^\top & 1\end{bmatrix}, \qquad \mathbf{x} \;\triangleq\; R\,\mathbf{X}+\mathbf{t},\) 则 \(e \;=\; \mathbf{n}^\top(\mathbf{x}-\mathbf{q}) \;=\; \mathbf{n}^\top(R\,\mathbf{X}+\mathbf{t}-\mathbf{q}).\) 对位姿采用右扰动 $\phi \mapsto \phi\Exp(\boldsymbol{\xi})$ ， 其中 $\boldsymbol{\xi}=[\boldsymbol{\rho};\boldsymbol{\theta}]\in\mathbb{R}^6$（先平移后旋转的右平凡化）， 有一阶近似： \(\delta \mathbf{x} = \underbrace{R}_{3\times3}\,\boldsymbol{\rho} \;-\; \underbrace{R[\mathbf{X}]_\times}_{3\times3}\,\boldsymbol{\theta}, \quad [\cdot]_\times \text{ 为反对称矩阵}.\) 因此 \(\delta e = \mathbf{n}^\top\,\delta \mathbf{x} = \mathbf{n}^\top\Big( R\,\boldsymbol{\rho} - R[\mathbf{X}]_\times \boldsymbol{\theta}\Big) = \underbrace{\mathbf{n}^\top\!\Big[\; R \;\; -R[\mathbf{X}]_\times \;\Big]}_{1\times 6} \begin{bmatrix}\boldsymbol{\rho}\\ \boldsymbol{\theta}\end{bmatrix}.\) 于是得到 \(\boxed{\; \frac{\partial e}{\partial \phi_k(t_i)} \;=\; \mathbf{n}^\top\!\Big[\; R \;\; -R\,[T_{LB_j}\mathbf{p}]_\times \;\Big] \;} \tag{9b}\) 与文中写法一致（他们将 (\mathbf{n}) 记作 (k n_i^j)，并用 (R_{k t_i^j}) 指代 (R)）。

---

9c / 9d： $\displaystyle \frac{\partial \phi_k(t_i)}{\partial T_{k-1}},\;\frac{\partial \phi_k(t_i)}{\partial T_k}$ 的推导（插值项）

记 \(\phi_k(t) = T_{k-1}\;\Exp(\alpha\,\tau),\qquad \tau=\Log(T_{k-1}^{-1}T_k).\) 对右扰动 $\delta T_{k-1}=T_{k-1}\Exp(\boldsymbol{\xi}_{k-1})$、$\delta T_k=T_k\Exp(\boldsymbol{\xi}_k)$ 求 $\delta\phi$ 的一阶变化。需要三条李群/李代数恒等式（右/左雅可比参见 [33]）：

1. 指数映射的右雅可比微分（在右平凡化下）
   \(\delta\Exp(\alpha\tau) \;\approx\; \Exp(\alpha\tau)\;\Jr(\alpha\tau)\,\alpha\,\delta\tau.\)
2. 相对位姿的对数微分
   \(\delta\tau \;\approx\; \Jl(\tau)^{-1}\big(\,\boldsymbol{\xi}_k \;-\; \boldsymbol{\xi}_{k-1}\,\big).\)

   这是 $\tau=\Log(T_{k-1}^{-1}T_k)$ 在右扰动下的标准一阶近似：前后端点的右扰动在切空间中以 $\Jl(\tau)^{-1}$ 规整。

3. 左乘的扰动传播
   \(\delta\phi \;=\; \underbrace{T_{k-1}\Exp(\alpha\tau)}_{\phi}\,\boldsymbol{\xi}_{k-1} \;+\; T_{k-1}\,\delta\Exp(\alpha\tau).\)

把 1–3 代入并收集 $\boldsymbol{\xi}{k-1}$ 、$\boldsymbol{\xi}{k}$ 的系数，可写成右平凡化的增量映射 \(\delta\phi \;\equiv\; \phi\,\Big( (1-\alpha)\,\Jr\!\big((\alpha-1)\tau\big)\,\Jl(\tau)^{-1}\,\boldsymbol{\xi}_{k-1} \;+\; \alpha\,\Jr(\alpha\tau)\,\Jr(\tau)^{-1}\,\boldsymbol{\xi}_{k} \Big),\) 从而得到对端点位姿的“右雅可比”（去掉前面的 $\phi$ 左乘，因为后续会与 9b 的 $\partial e/\partial\phi$ 相乘）：

\[\boxed{\; \frac{\partial \phi_k(t_i)}{\partial T_{k-1}} = (1-\alpha)\,\Jr\!\big((\alpha-1)\tau\big)\,\Jl(\tau)^{-1} \;} \tag{9c}\] \[\boxed{\; \frac{\partial \phi_k(t_i)}{\partial T_{k}} = \alpha\,\Jr(\alpha\tau)\,\Jr(\tau)^{-1} \;} \tag{9d}\]

这与文中 9c、9d 完全一致。直观理解：$(1-\alpha)$ 与 $\alpha$ 分别给出“靠近起点/终点”的权重，$\Jr,\Jl$ 则负责把端点位姿的右扰动正确地搬运到中间时刻的 $\phi_k(t_i)$ 上。

---

10：运动学（平滑性）约束的雅可比

平滑性误差定义为 \(e^v_k \;=\; (T_k \ominus T_{k-1}) \;-\; \widehat{(T_{k-1}\ominus T_{k-2})} \;=\; \tau^k \;-\; \hat{\tau}^{k-1},\) 其中 $\tau^k=\Log(T_{k-1}^{-1}T_k)$ ，$\hat{\tau}^{k-1}$ 为上个窗口固定先验。对端点右扰动求导（用上面的第 2 条恒等式）： \(\boxed{\; \frac{\partial e^v_k}{\partial T_{k-1}} = -\,\Jl(\tau^k)^{-1}, \qquad \frac{\partial e^v_k}{\partial T_{k}} \;=\; \Jr(\tau^k)^{-1}. \;} \tag{10}\) 这与文中式 (10) 一致。

---

小结：9a–9d 如何拼成最终雅可比

• 先用 9b 得到几何项 $\partial e/\partial\phi$；
• 再用 9c、9d 把 $\phi$ 对端点位姿的导数连锁到 $\partial e/\partial T_{k-1},\partial e/\partial T_k$；
• 将两者按 9a 链式相乘，装配到整堆状态 $s$ 的雅可比行中（其他不相干位姿的列为零）。 这样即可形成稀疏的 $J$，进一步构造 $H=J^\top W^{-1}J$、$b=J^\top W^{-1}e$，进入 Gauss–Newton 的线性求解与迭代更新。

---