Ceres-solver examples之pose_graph_2d学习笔记

简要说明

ceres-solver库是google的非线性优化库，可以对slam问题，机器人位姿进行优化，使其建图的效果得到改善。pose_graph_2d是官方给出的二维平面上机器人位姿优化问题，需要读取一个g2o文件，运行程序后返回一个poses_original.txt和一个poses_optimized.txt，大家按字面意思理解，内部格式长这样：

pose_id x y yaw_radians
pose_id x y yaw_radians
pose_id x y yaw_radians
...

得到这两个文件后，用官方提供的plot_results.py可以画出原始和优化后的位姿地图，类似下图：

变量说明

重要变量为以下几个：
constraints：vector，放入变量的类型为Constraint2d， 含义为机器人两个pose之间的限制，Constraint2d包括两个pose的id，相对坐标x，y，和协方差阵。这个变量描述的是观测量测量量measurement，即机器人认为自己感知到的正确的数据，这些数据其实是有噪声的。

poses: map类指针，键值对为id和 Pose2d ，Pose2d是一个由id，世界坐标x，y，yaw角。这个变量描述的是实际机器人的世界坐标位置，是根据观测量来计算出的量。由于误差的存在，这个量会随着时间的推移误差逐渐加大，最后通过回环检测来得到矛盾关系从而进行优化。

关键步骤

    // Ceres will take ownership of the pointer.
    //将需要的参数传入，设置残差，构造costfunction，使用自动求导方式
    ceres::CostFunction* cost_function = PoseGraph2dErrorTerm::Create(
        constraint.x, constraint.y, constraint.yaw_radians, sqrt_information);

详情见下面的Costfunction的搭建。

一、Costfunction的搭建

使用ceres库的关键是构造 costfunction ，ceres官方搭建的costfunction，同样有一个类表示，名为PoseGraph2dErrorTerm，具体如下所示：

class PoseGraph2dErrorTerm {
   
 public:
  PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians,
                       const Eigen::Matrix3d& sqrt_information)
      : p_ab_(x_ab, y_ab),
        yaw_ab_radians_(yaw_ab_radians),
        sqrt_information_(sqrt_information) {
   }
  template <typename T>
  //x_a,y_a(p_a)x_b,y_b(p_b)是世界下的ab坐标
  bool operator()(const T* const x_a, const T* const y_a, const T* const yaw_a,
                  const T* const x_b, const T* const y_b, const T* const yaw_b,
                  T* residuals_ptr