cartographer后端优化求解器解读

根据前面分析的图优化基本结构可知，已知节点、约束和闭环约束条件后，则可进行超定方程组求解，即求解所有节点的最佳位姿，使得超定方程组误差最小。optimization_problem_则是优化求解器，采用的是SPA(Sparse Pose Adjustment)方法进行优化，较早的开源SLAM算法kator slam后端优化采用相同的思想进行优化。由于目前许多优化库的出现，例如g2o、ceres、gtsam等，使得slam后端优化问题变得十分简单。而cartographer采用的ceres库进行实现的。

OptimizationProblem2D成员

  optimization::proto::OptimizationProblemOptions options_;         // 优化器的配置信息
  MapById<NodeId, NodeSpec2D> node_data_;                           // 节点信息列表
  MapById<SubmapId, SubmapSpec2D> submap_data_;                     // submap列表信息
  std::map<std::string, transform::Rigid3d> landmark_data_;         // 依据时间记录各类传感器信息
  sensor::MapByTime<sensor::ImuData> imu_data_;                     
  sensor::MapByTime<sensor::OdometryData> odometry_data_;
  sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
  std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_; // 轨迹信息

其中主要被优化的就是node_data_和submap_data_列表，其列表中定义的结构体如下所示。

struct NodeSpec2D {
  common::Time time;
  transform::Rigid2d local_pose_2d;
  transform::Rigid2d global_pose_2d;
  Eigen::Quaterniond gravity_alignment;
};

struct SubmapSpec2D {
  transform::Rigid2d global_pose;
};

节点信息中包含局部位置和绝对位置，以及重力方向。而submap的信息仅包含其绝对位姿信息。

构造函数

OptimizationProblem2D::OptimizationProblem2D(
    const proto::OptimizationProblemOptions& options)
    : options_(options) {}

OptimizationProblem2D::~OptimizationProblem2D() {}

构造函数非常简单，仅传递了全局配置。

优化对象接口

主要功能是将节点信息、submap信息、传感器信息等放入到全局变量中，十分简单。如下几个例子。

   1.加入submap信息

void OptimizationProblem2D::AddSubmap(
    const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
  submap_data_.Append(trajectory_id, SubmapSpec2D{global_submap_pose});
}

2.加入node信息

void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
                                              const NodeSpec2D& node_data) {
  node_data_.Append(trajectory_id, node_data);
  trajectory_data_[trajectory_id];
}

优化求解实现Solve

void OptimizationProblem2D::Solve(
    const std::vector<Constraint>& constraints,
    const std::map<int, PoseGraphInterface::TrajectoryState>&
        trajectories_state,
    const std::map<std::string, LandmarkNode>& landmark_nodes)

求解输入为位姿图的约束、轨迹状态、路标点，而被求解放在全局变量中。

  // 若无节点数据，则无需优化
  if (node_data_.empty()) {
    // Nothing to optimize.
    return;
  }

  // 遍历记录下所有FROZEN状态的轨迹列表
  std::set<int> frozen_trajectories;
  for (const auto& it : trajectories_state) {
    if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) {
      frozen_trajectories.insert(it.first);
    }
  }

由于核心就是优化节点的位姿，因此如节点列表为空，则无需优化。由于优化都是相对位置，因此必须某些节点必须提供固定值，即不可优化节点，否则会出现无穷解。而cartographer又支持多条轨迹，假设第一条轨迹建图完成，现需在原地图基础上扩展，则不能改变已有的地图数据，可进行锁定。在进行优化时可根据轨迹的状态记录所锁定状态，防止被优化。

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

  // Set the starting point.
  // TODO(hrapp): Move ceres data into SubmapSpec.
  // 优化用的临时变量
  MapById<SubmapId, std::array<double, 3>> C_submaps;
  MapById<NodeId, std::array<double, 3>> C_nodes;
  std::map<std::string, CeresPose> C_landmarks;

创建优化问题对象，采用ceres库。由于优化是迭代不断调整正确解，这里提前定义需要优化的临时变量。

  bool first_submap = true;
  for (const auto& submap_id_data : submap_data_) {
    //查询是否为冻结轨迹
    const bool frozen =
        frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
    // 临时submap变量插入id和全局位置
    C_submaps.Insert(submap_id_data.id,
                     FromPose(submap_id_data.data.global_pose));
    // 优化器中插入需要优化的数据
    problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);
    // 第一帧submap和冻结的则设为常量
    if (first_submap || frozen) {
      first_submap = false;
      // Fix the pose of the first submap or all submaps of a frozen
      // trajectory.
      problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
    }
  }

前面提过优化主要是调整位姿图中节点位姿，而cartographer同时优化submap和nodeid两种图节点。这里先遍历所有submap，并且全局的第一个submap为锁定值，即无需优化。同时判断此submap的所在轨迹是否锁定状态，如果为锁定状态，则应节点信息设置为常量，否则将加入优化对象中，计算残差。

  // 遍历每个节点
  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());
    }
  }

路径中的节点与submap处理方式基本类似，若所处的trajectory被锁定，则被加入常量，否则被插入优化对象中，计算残差。

  // 增加约束，即submap与node之间的相对位置，采用自动求导
  for (const Constraint& constraint : constraints) {
    problem.AddResidualBlock(
        CreateAutoDiffSpaCostFunction(constraint.pose),
        // Loop closure constraints should have a loss function.
        // 若不是插入本submap的约束（即为闭环的约束），需要添加一个损失函数
        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());
  }

遍历所有节点与submap之间的约束，其实可认为是g2o优化中的边。只是这里有个特殊操作，即如果约束是闭环得到的则需要增加一个HuberLoss损失函数。`而``CreateAutoDiffSpaCostFunction```则是计算代价函数。

  // Add cost functions for landmarks.
  AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
                           &problem, options_.huber_scale());

添加一个路标计算代价函数，由于测试时从未提供过具体的landmark信息，不详细分析。

for (auto node_it = node_data_.begin(); node_it != node_data_.end();){
....
}

遍历所有节点信息，主要功能是处理相邻两个节点的约束信息，具体如下

    // 获取每个节点的trajectory——id
    const int trajectory_id = node_it->id.trajectory_id;
    // 获取这trajectoryid的最后一个node
    const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
    //如果本轨迹是锁定的，则无需处理跳过
    if (frozen_trajectories.count(trajectory_id) != 0) {
      node_it = trajectory_end;
      continue;
    }

将遍历的一个nodeid获取所在trajectory_id，并且获取此trajectory_id中最后一个node节点。目的是为了遍历当前节点直到此轨迹最后一个节点，而下面的循环则是处理相邻的两个节点之间的约束关系。

   auto prev_node_it = node_it;
    //遍历给trajectoryid里所有的nodeid
    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;

      //仅处理相邻的两个node
      if (second_node_id.node_index != first_node_id.node_index + 1) {
        continue;
      }

      // Add a relative pose constraint based on the odometry (if available).
      // 获取相邻两个node的 相对里程计位置差
      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());
      }

      // Add a relative pose constraint based on consecutive local SLAM poses.
      // 增加采用local slam的相对位置
      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());
    }
  }

相邻的两个轨迹节点，首先可存在直接的odometer推移的关系，如果存在则需要添加优化约束对象。而相邻的两个节点一定存在由local slam获取的相对位置。如此可获取所有相邻节点之间的约束。而上节分析约束构造器仅计算了submap和node之间的约束，这里补全了所有约束。

  // 里面是固定的位置node， 估计是类似与gps的全局点
  std::map<int, std::array<double, 3>> C_fixed_frames;
  for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
...
  }

以上再次遍历所有轨迹节点，主要是将中间的全局确定位置点进行锁定，例如来自gps的位姿节点，由于我测试时未添加过全局位置，因此会被continue掉，因此不再详述。

  // Solve. 求解器
  ceres::Solver::Summary summary;
  // 创建求解器并求解
  ceres::Solve(
      common::CreateCeresSolverOptions(options_.ceres_solver_options()),
      &problem, &summary);
  if (options_.log_solver_summary()) {
    LOG(INFO) << summary.FullReport();
  }

已有所有计算残差，调用ceres优化求解器并求解。

  // 获得优化值，更新优化后的值
  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();
  }

最后将计算的结果进行更新。

总结

本节无太多原理性讲解，主要是根据约束和节点信息如何调用ceres库进行优化求解，即完成图优化最后的计算过程，而优化求解的真正算法部分则由ceres库完成。

免责声明：本文系网络转载或改编，未找到原创作者，版权归原作者所有。如涉及版权，请联系删