(02)Cartographer源码无死角解析-(74) 2D后端优化→OptimizationProblem2D-里程计、local位姿、GPU残差

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

上一篇博客介绍的 landmark 数据的残差与优化，最后本人分享了如下：

$\color{red}核心理解:$ 在上一篇博客中介绍的约束残差(节点相对与子图位姿的残差)，其主是优化每个节点以及每个子图的位姿。可想而知，其是离散的。但是轨迹是一条连续平滑的曲线，所以本质上我们希望通过插值获取到的位姿也是准确的。恰好， landmark帧的时间点，也激光雷达数据帧的时间点是不一样的，所以如果 landmark帧的时间点位于两个激光雷达数据帧的时间点，那么可以把landmark帧相对于机器人的位姿作为一个约束，对两节点插值之后的位姿进行优化，从而广播到两个节点，间接对两个节点进行优化。

现在回到 OptimizationProblem2D::Solve() 函数，到目前为止，已经把：

  // Add cost functions for landmarks.
  // Step: landmark数据 与 通过2个节点位姿插值出来的相对位姿 的差值作为残差项
  AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
                           &problem, options_.huber_scale());

讲解完成了，不过看起来，任务量还是很多的，似乎还剩下很多代码都没有讲解。起始不然，因为后面的代码比较简单，另外再前面已经接触过类似的代码，分析起来就没有那么困难。

二、残差准备

接着代码 OptimizationProblem2D::Solve() 函数调用的 AddLandmarkCostFunctions() 之后继续分析。首先其又运行了一个for循环，且其中也嵌套一个层for循环，先来看第一层循环，代码如下：

  // 遍历多个轨迹, 添加里程计与local结果的残差
  for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
    // 获取每个节点的轨迹id
    const int trajectory_id = node_it->id.trajectory_id;
    // 获取这条轨迹的最后一个位置的迭代器
    const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
    // 如果轨迹是frozen的, 则无需处理直接跳过
    if (frozen_trajectories.count(trajectory_id) != 0) {
      node_it = trajectory_end;
      continue;
    }

    auto prev_node_it = node_it;

需要注意，这里的for循环时对轨迹遍历，并不是对节点的遍历，如果 node_data_ 中包含了多条轨迹的节点，node_data_.begin() 表示 trajectory_id==0 的第一个节点，而 node_data_.end() 表示 trajectory_id 等于最大轨迹的第一个节点。先获得轨迹起始节点 node_it 对应轨迹标识 trajectory_id。然后获得这条轨迹结束位姿的迭代器，记录在 trajectory_end，接着判断一下目前节点对应的轨迹是否为冻结状态，如果为冻结状态则 continue 跳出第一层循环。然后把当前 node_it 记录给 prev_node_it，也就是 prev_node_it 记录的目前轨迹的起始节点。接着代码分析，可以看到其嵌套了一个循环：

    // 遍历一个轨迹的所有节点, 添加里程计与local结果的残差
    for (++node_it; node_it != trajectory_end; ++node_it) {
      const NodeId first_node_id = prev_node_it->id;
      const NodeSpec2D& first_node_data = prev_node_it->data;
      prev_node_it = node_it;

      const NodeId second_node_id = node_it->id;
      const NodeSpec2D& second_node_data = node_it->data;

      // 如果节点的索引不连续, 跳过
      if (second_node_id.node_index != first_node_id.node_index + 1) {
        continue;
      }

该循环就是对一条轨迹的节点进行遍历，prev_node_it 主要用于记录上一节点，然后判断当前节点与上一节点索引是否连续，如果不连续，则会 continue 跳出循环，该节点不参与下面的残差计算。

另外，其还获取了当前节点与上一节点的节点数据，分别存储于 first_node_data 与 second_node_data。

三、里程计插值

首先根据两个连续的节点数据 first_node_data 与 second_node_data 的时间，然后对里程计数据进行插值处理，获得里程计在这两个时间点的 local 系下的位姿，里程计数据的线性插值，被调代码如下：

      // Add a relative pose constraint based on the odometry (if available).
      // 根据里程计数据进行插值得到的2个节点间的坐标变换
      std::unique_ptr<transform::Rigid3d> relative_odometry =
          CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
                                        second_node_data);

函数 CalculateOdometryBetweenNodes() 实现于 optimization_problem_2d.cc 文件中，代码注释如下：

/**
 * @brief 根据里程计数据算出两个节点间的相对坐标变换
 * 
 * @param[in] trajectory_id 轨迹的id
 * @param[in] first_node_data 前一个节点数据
 * @param[in] second_node_data 后一个节点数据
 * @return std::unique_ptr<transform::Rigid3d> 两个节点的坐标变换
 */
std::unique_ptr<transform::Rigid3d>
OptimizationProblem2D::CalculateOdometryBetweenNodes(
    const int trajectory_id, const NodeSpec2D& first_node_data,
    const NodeSpec2D& second_node_data) const {

  if (odometry_data_.HasTrajectory(trajectory_id)) {
    // 插值得到time时刻的里程计数据
    const std::unique_ptr<transform::Rigid3d> first_node_odometry =
        InterpolateOdometry(trajectory_id, first_node_data.time);
    const std::unique_ptr<transform::Rigid3d> second_node_odometry =
        InterpolateOdometry(trajectory_id, second_node_data.time);

    if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
      // 根据里程计数据算出的相对坐标变换
      // 需要注意的是, 实际上在optimization_problem中, node的位姿都是2d平面上的
      // 而odometry的pose是带姿态的, 因此要将轮速计插值出来的位姿转到平面上
      transform::Rigid3d relative_odometry =
          transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *
          first_node_odometry->inverse() * (*second_node_odometry) *
          transform::Rigid3d::Rotation(
              second_node_data.gravity_alignment.inverse());

      return absl::make_unique<transform::Rigid3d>(relative_odometry);
    }
  }
  return nullptr;
}

其代码也比较简单，就是先判断一下 odometry_data_ 中有没有 trajectory_id 对应的里程计数据，如果有就根据 first_node_data 与 second_node_data 获得需要插值求得位姿的时刻，然后进行插值。这里的插值函数 InterpolateOdometry() 其权重依旧由 time 与前后两个里程计节点的时间距离来决定的。求得 first_node_data.time 与 second_node_data.time 对应插值里程计位姿之后的 local 位姿之后，再进一步获得两节点基于里程计的相对位姿变换。

四、里程计残差

      // Step: 第三种残差 节点与节点间在global坐标系下的相对坐标变换 与 通过里程计数据插值出的相对坐标变换 的差值作为残差项
      // 如果存在里程计则可增加一个残差
      if (relative_odometry != nullptr) {
        problem.AddResidualBlock(
            CreateAutoDiffSpaCostFunction(Constraint::Pose{
                *relative_odometry, options_.odometry_translation_weight(),
                options_.odometry_rotation_weight()}),
            nullptr /* loss function */, 
            C_nodes.at(first_node_id).data(),
            C_nodes.at(second_node_id).data());
      }

如果存在里程计则可增加一个残差，该残差以 relative_odometry 作为帧值(根据里程计计算出来两节点的位姿变换)，然后让根据两节点global系下的节点位姿计算出两节点的相对位姿，优化过程就是让后者向前者靠近。

四、local残差

这是一种比较好理解的残差，再前端求得节点位姿时，使用了扫描匹配，以及Ceres优化，尽量让一帧点云全部打在障碍物上，很明显其时比较优的解，这里我们认为其时正确的，对于相邻的两个节点，都是通过这种方式估算出来的 local 位姿，因为间隔时间短，他们之间的累计误差时可以忽略的，也就是说， local 系下两个节点的相对变换基本同理基本时正确的，可以把其作为一个约束来对待，让 global 系下两个节点之间的位姿变换通过Ceres优化，逼近于local系下对应两个节点的位姿变换。代码如下所示：

      // Add a relative pose constraint based on consecutive local SLAM poses.
      // 计算相邻2个节点在local坐标系下的坐标变换
      const transform::Rigid3d relative_local_slam_pose =
          transform::Embed3D(first_node_data.local_pose_2d.inverse() *
                             second_node_data.local_pose_2d);
      // Step: 第四种残差 节点与节点间在global坐标系下的相对坐标变换 与 相邻2个节点在local坐标系下的相对坐标变换 的差值作为残差项
      problem.AddResidualBlock(
          CreateAutoDiffSpaCostFunction(
              Constraint::Pose{relative_local_slam_pose,
                               options_.local_slam_pose_translation_weight(),
                               options_.local_slam_pose_rotation_weight()}),
          nullptr /* loss function */, 
          C_nodes.at(first_node_id).data(),
          C_nodes.at(second_node_id).data());
    }
  }

五、GPS残差-准备工作与约束计算

对于GPS残差部分，又是两个for循环进行嵌套的代码，且与前面十分相似如下，第一层for循环依旧是对轨迹的遍历。

 // 遍历多个轨迹, 添加gps的残差
  std::map<int, std::array<double, 3>> C_fixed_frames;
  for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
    const int trajectory_id = node_it->id.trajectory_id;
    const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
    if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) {
      node_it = trajectory_end;
      continue;
    }

    const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
    bool fixed_frame_pose_initialized = false;

其先判断一下该轨迹是否存在GPU数据，没有没有，则 continue 遍历下一条轨迹。如果存在则获得该轨迹信息，轨迹信息主要包含了如下内容：

  struct TrajectoryData {
    double gravity_constant = 9.8;
    std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
    absl::optional<transform::Rigid3d> fixed_frame_origin_in_map;
  };

先把 fixed_frame_pose_initialized 设置为 false，接着就是对轨迹的每个节点进行遍历了：

    // 遍历一个轨迹的所有节点, 添加gps的残差
    for (; node_it != trajectory_end; ++node_it) {
      const NodeId node_id = node_it->id;
      const NodeSpec2D& node_data = node_it->data;

      // 根据节点的时间对gps数据进行插值, 获取这个时刻的gps数据的位姿
      const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
          Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);
      // 要找到第一个有效的数据才能进行残差的计算
      if (fixed_frame_pose == nullptr) {
        continue;
      }
      // gps数据到gps第一个数据间的坐标变换
      const Constraint::Pose constraint_pose{
          *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),
          options_.fixed_frame_pose_rotation_weight()};

先获得遍历轨迹节的time，利用 FixedFramePoseData 数据进行插值，获得 GPS该时刻的位姿 fixed_frame_pose，需要注意 gps数据是到gps第一个数据间的坐标变换，那么可以为这个变换创建一个约束constraint_pose，其中 constraint_pose.zbar_ij 就是 *fixed_frame_pose。

五、GPS残差-global系到gps系变换

在建立好约束 constraint_pose 之后，如果 fixed_frame_pose_initialized 不为true，也就说目前遍历的是该轨迹的第一个节点，那么则对GPS位姿进行一个初始化处理：

      // 计算gps坐标系原点在global坐标系下的坐标
      if (!fixed_frame_pose_initialized) {
        transform::Rigid2d fixed_frame_pose_in_map;
        // 如果设置了gps数据的原点
        if (trajectory_data.fixed_frame_origin_in_map.has_value()) {
          fixed_frame_pose_in_map = transform::Project2D(
              trajectory_data.fixed_frame_origin_in_map.value());
        } 
        // 第一次优化之前执行的是这里
        else {
          // 计算gps第一个数据在global坐标系下的坐标, 相当于gps数据的坐标系原点在global坐标系下的坐标
          fixed_frame_pose_in_map =
              node_data.global_pose_2d *
              transform::Project2D(constraint_pose.zbar_ij).inverse();
        }

        // 保存gps坐标系原点在global坐标系下的坐标
        C_fixed_frames.emplace(trajectory_id,
                               FromPose(fixed_frame_pose_in_map));
        fixed_frame_pose_initialized = true;
      }

其先判断一下该轨迹是否设置了GPS原点，也就是 trajectory_data.fixed_frame_origin_in_map 参数是否设置，如果设置了，则把其映射到2D平面赋值给 fixed_frame_pose_in_map。

如果没有设置，node_data.global_pose_2d 表示global下机器人位姿，ransform::Project2D(constraint_pose.zbar_ij) 表示机器人在GPS下的位姿，

          fixed_frame_pose_in_map =
              node_data.global_pose_2d *
              transform::Project2D(constraint_pose.zbar_ij).inverse();

那么 fixed_frame_pose_in_map 就表示GPS系相对于 Global系的位姿了。也就是说
fixed_frame_pose_in_map 表示GPU第一个数据在global系下的位姿，计算出来之后，fixed_frame_pose_initialized 赋值为 true，就不会再为该轨迹计算 fixed_frame_pose_in_map 了。求得 fixed_frame_pose_in_map 会被添加到 C_fixed_frames 之中。

六、GPS残差-残差构建

最后就是最核心的残差构建部分了，主体代码如下所示：

      // Step: 第五种残差 节点与gps坐标系原点在global坐标系下的相对坐标变换 与 通过gps数据进行插值得到的相对坐标变换 的差值作为残差项
      problem.AddResidualBlock(
          CreateAutoDiffSpaCostFunction(constraint_pose),
          options_.fixed_frame_pose_use_tolerant_loss()
              ? new ceres::TolerantLoss(
                    options_.fixed_frame_pose_tolerant_loss_param_a(),
                    options_.fixed_frame_pose_tolerant_loss_param_b())
              : nullptr,
          C_fixed_frames.at(trajectory_id).data(), // 会自动调用AddParameterBlock
          C_nodes.at(node_id).data());
    }

constraint_pose 是利用GPS数据插值出来当前遍历节点 node_it 对应的位姿。ceres::TolerantLoss() 同样用于残差结果的抑制异常点。根据 C_fixed_frames.at(trajectory_id).data() 表示GPU系在global系下的位姿，
C_nodes.at(node_id).data()) 表示当前节点在global系下的位姿，那么可以求得当前节点相对于GPS系的位姿。再与约束 constraint_pose 做残差。

七、开始优化，且更新数据

  // Solve. 进行求解
  ceres::Solver::Summary summary;
  ceres::Solve(
      common::CreateCeresSolverOptions(options_.ceres_solver_options()),
      &problem, &summary);

  // 如果开启了优化的log输出, 就输出ceres的报告
  if (options_.log_solver_summary()) {
    LOG(INFO) << summary.FullReport();
  }

  // 将优化后的所有数据进行更新 Store the result.
  for (const auto& C_submap_id_data : C_submaps) {
    submap_data_.at(C_submap_id_data.id).global_pose =
        ToPose(C_submap_id_data.data);
  }
  for (const auto& C_node_id_data : C_nodes) {
    node_data_.at(C_node_id_data.id).global_pose_2d =
        ToPose(C_node_id_data.data);
  }
  for (const auto& C_fixed_frame : C_fixed_frames) {
    trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map =
        transform::Embed3D(ToPose(C_fixed_frame.second));
  }
  for (const auto& C_landmark : C_landmarks) {
    landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
  }

八、结语

到这里位姿，可以说后端优化或者位姿图的讲解基本就完成了，总的来说，后端优化中共有五种残差，分别如下所示：

1、基于节点与子图(子图内，子图间)约束的残差
2、基于Landmark的残差
3、基于odometry里程计的残差
4、基于节点local系下的残差
5、基于GPS数据的残差

下一篇博客，就带代价对后端优化进行一个整理的复盘，让大家的理解更加