cartographer 子图和节点的优化

1、涉及到的类型

1.  MapById<SubmapId, SubmapSpec2D> submap_data_;

SubmapId              int trajectory_id; int submap_index;

struct SubmapSpec2D {transform::Rigid2d global_pose;};

2.transform::Rigid2d   translation_;   Rotation2D rotation_;   两个平移量和一个旋转量

   transform::Rigid3d    Vector translation_;    Quaternion rotation_;  三个平移量和四元素

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

2、具体形式调用

1.pose_graph_2d.cc   void PoseGraph2D::RunOptimization()函数中调用：

  optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                               data_.landmark_nodes);

参数：：data_.constraints  所有数据的约束

GetTrajectoryStates()  轨迹的状态 ACTIVE, FINISHED, FROZEN, DELETED

data_.landmark_nodes   landmark的所有数据

2.optimization_problem_2d.cc    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)

中涉及到的子图节点的优化：

1.for (const auto& submap_id_data : submap_data_)      //for start

 首先在冻结轨迹中寻找该子图id是否存在，确定该子图是否为冻结子图

2.C_submaps.Insert(submap_id_data.id, FromPose(submap_id_data.data.global_pose));

3.添加一个参数块  problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);

4.如果为第一个子图或者是冻结子图，则固定子图的数据，也就是位姿。  //for end

problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());

5. for (const auto& node_id_data : node_data_)     //for start

首先在冻结轨迹中寻找该node是否存在，确定该node_id_data是否为冻结node

6.添加一个参数块 problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);

7.如果为冻结node数据，则将该node_id_data数据设为固定值     //for end

8.为子图内和子图间的约束添加代价函数 for (const Constraint& constraint : constraints)  //for start

9.CreateAutoDiffSpaCostFunction(constraint.pose)   

ceres::CostFunction* CreateAutoDiffSpaCostFunction(
    const PoseGraphInterface::Constraint::Pose& observed_relative_pose) {
  return new ceres::AutoDiffCostFunction<SpaCostFunction2D, 3 /* residuals */,
                                         3 /* start pose variables */,
                                         3 /* end pose variables */>(
      new SpaCostFunction2D(observed_relative_pose));
}

 

SpaCostFunction2D中  私有成员observed_relative_pose_      重要成员函数： operator()函数

  template <typename T>
  bool operator()(const T* const start_pose, const T* const end_pose,
                  T* e) const {
    const std::array<T, 3> error =
        ScaleError(ComputeUnscaledError(
                       transform::Project2D(observed_relative_pose_.zbar_ij),
                       start_pose, end_pose),
                   observed_relative_pose_.translation_weight,
                   observed_relative_pose_.rotation_weight);
    std::copy(std::begin(error), std::end(error), e);
    return true;
  }

static std::array<T, 3> ComputeUnscaledError( const transform::Rigid2d& relative_pose, const T* const start,
    const T* const end) 计算偏差错误

首先首先计算start到end的位姿   然后计算 相对位姿与其所求相对位姿的參差

.

std::array<T, 3> ScaleError(const std::array<T, 3>& error,double translation_weight, double rotation_weight) {
  return {{  error[0] * translation_weight,  error[1] * translation_weight,error[2] * rotation_weight}};

    //for end

10.添加违反测距或连续节点之间更改的惩罚如果没有里程计

  for (auto node_it = node_data_.begin(); node_it != node_data_.end();)   //for start

11.如果该node_it 所对应的轨迹冻结，则该node_it迭代器指向下一个迭代器，continue

12. auto prev_node_it = node_it;

13.    for (++node_it; node_it != trajectory_end; ++node_it)   //for start

14.   求数据   const NodeId first_node_id = prev_node_it->id;  second_node_id
    const NodeSpec2D& first_node_data = prev_node_it->data;  second_node_data

      if (second_node_id.node_index != first_node_id.node_index + 1) {continue;}   //保证在同一个轨迹

15.根据里程计（如果可用）添加相对姿势约束。   

  std::unique_ptr<transform::Rigid3d> relative_odometry =
          CalculateOdometryBetweenNodes(trajectory_id, first_node_data, second_node_data)

如果有轨迹则执行，否则返回nullptr   计算first_node_odometry  second_node_odometry 位姿

    const std::unique_ptr<transform::Rigid3d> first_node_odometry =InterpolateOdometry(trajectory_id, first_node_data.time);

求出该时间的里程计位姿，并且求出前一时刻里程计位姿，而 这进行插值求出位姿 Interpolate(pose1,pose2).transform

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

16.创建里程计的代价    跟上述一致

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

17.基于连续的本地SLAM姿势添加相对姿势约束。   

  const transform::Rigid3d relative_local_slam_pose =
          transform::Embed3D(first_node_data.local_pose_2d.inverse() * second_node_data.local_pose_2d);

18.优化   ceres::Solve(common::CreateCeresSolverOptions(options_.ceres_solver_options()), &problem, &summary);

上面的所有 约束都加到   problem中  进行非线性优化 ceres

19.存储结果  跟新submap_data_的全局位姿 node_data_的全局位姿 landmark_data_的位姿（全局）

//for end //for end