Cartographer代码阅读笔记：后端优化器

前沿

Cartographer的后端优化器是通过SPA（Sparse Pose Adjustment）的技术，根据路径节点和子图之间的约束关系，最终优化路径节点和子图的世界坐标。路径节点和子图之间的约束关系是由约束构建器通过回环检测方法得到的。而所谓的SPA技术，本质上还是通过LM（Levenberg-Marquardt）方法进行非线性寻优，并且由于不是所有的子图和路径节点之间都能成功构建约束，以至于LM的增量方程中的H矩阵是一个稀疏矩阵，这样就可以通过使用一些优化手段来降低对内存的需求，提高求解效率。

相关文件在：cartographer/mapping/internal/optimization/optimization_problem_2d.cc

Cartographer有关其他分析文章：回到目录

接下来探讨一下每个函数的具体实现过程：

Solve()函数分析

/** 
 * @brief 后端优化核心函数，通过Ceres优化库，调整子图和路径节点的世界位姿
 * @param constraints 位姿图的约束
 * @param frozen_trajectories 运动轨迹
 * @param landmark_nodes 路标点
 */
void OptimizationProblem2D::Solve(
    const std::vector<Constraint>& constraints,
    const std::set<int>& frozen_trajectories,
    const std::map<std::string, LandmarkNode>& landmark_nodes) {

  // 路径节点为空就没有必要进行优化
  if (node_data_.empty()) {
    return;
  }

  // 创建优化问题对象
  ceres::Problem::Options problem_options;
  ceres::Problem problem(problem_options);

  // 创建一些临时变量，用于描述优化问题
  MapById<SubmapId, std::array<double, 3>> C_submaps;
  MapById<NodeId, std::array<double, 3>> C_nodes;
  std::map<std::string, CeresPose> C_landmarks;
  bool first_submap = true;
  bool freeze_landmarks = !frozen_trajectories.empty(); 

  // 遍历所有子图
  for (const auto& submap_id_data : submap_data_) {
    const bool frozen =
        frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;

    // 将子图的全局位姿放置到临时容器C_submaps中。
    C_submaps.Insert(submap_id_data.id,
                     FromPose(submap_id_data.data.global_pose));

    // 将子图全局位姿作为优化参数告知对象problem。
    problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);
    
    // 如果是第一幅子图，或者已经冻结，就通过SetParameterBlockConstant将对应的参数设定为常数，
    // 这样在Ceres在迭代求解的过程中将不会改变这些参数。
    if (first_submap || frozen) {
      first_submap = false;
      problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
    }
  }

  // 遍历所有路径节点，将他们的全局位姿作为优化参数告知对象problem。
  // 如果该节点所在的路径被冻结了，就将它所对应的优化参数设为常数。
  for (const auto& node_id_data : node_data_) {
    const bool frozen =
        frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
    C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
    problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
    if (frozen) {
      problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
    }
  }

  // 遍历所有的约束，描述优化问题的残差块。
  // 函数CreateAutoDiffSpaCostFunction用于提供对应约束的SPA代价计算。
  // 如果是通过闭环检测构建的约束，则为之提供一个Huber的核函数，用于降低错误的闭环检测对最终的优化结果带来的负面影响。
  for (const Constraint& constraint : constraints) {
    problem.AddResidualBlock(
        CreateAutoDiffSpaCostFunction(constraint.pose),
        // Only loop closure constraints should have a loss function.
        constraint.tag == Constraint::INTER_SUBMAP
            ? new ceres::HuberLoss(options_.huber_scale())
            : nullptr,
        C_submaps.at(constraint.submap_id).data(),
        C_nodes.at(constraint.node_id).data());
  }

  // 根据路标点添加残差项。
  AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
                           &C_nodes, &C_landmarks, &problem);

  // 遍历所有的路径节点，为相邻节点之间添加约束
  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 (frozen_trajectories.count(trajectory_id) != 0) {
      node_it = trajectory_end;
      continue;
    }

    auto prev_node_it = node_it;
    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;
      }

      // Add a relative pose constraint based on the odometry (if available).
      std::unique_ptr<transform::Rigid3d> relative_odometry =
          CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
                                        second_node_data);

      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());
      }

      // 根据Local SLAM以及里程计等局部定位信息建立相邻的路径节点之间的位姿变换关系。
      // 并将之描述为残差项提供给problem对象。
      const transform::Rigid3d relative_local_slam_pose =
          transform::Embed3D(first_node_data.local_pose_2d.inverse() *
                             second_node_data.local_pose_2d);
      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());
    }
  }

  // 在描述完优化参数和残差计算方法之后，就可以通过ceres求解优化问题。对象summary将记录整个求解过程。
  ceres::Solver::Summary summary;
  ceres::Solve(
      common::CreateCeresSolverOptions(options_.ceres_solver_options()),
      &problem, &summary);
  if (options_.log_solver_summary()) {
    LOG(INFO) << summary.FullReport();
  }

  // 最后记录优化的结果
  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_landmark : C_landmarks) {
    landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
  }
}

返回顶部 or 回到目录