Build two-dimensional laser SLAM from scratch --- code implementation based on ceres-based back-end optimization

In the previous article, we analyzed how to use g2o to optimize the pose graph.

Since g2o naturally optimizes the pose graph, it is very suitable for the interface of Karto's pose graph. You only need to assign the corresponding vertices and constraints to the past.

In this article, let's take a look at another commonly used optimization library, Ceres solver.

1 Introduction to ceres

Ceres solver is a library developed by Google for nonlinear optimization. It has a large number of applications in the optimization of laser SLAM and V-SLAM, such as Cartographer, VINS and so on.

To study ceres, you can directly go to its official website to read the tutorials. The documentation of the library produced by Google is very good. I won’t say more here.
http://ceres-solver.org/tutorial.html

At present, many tutorials and articles on the Internet are translated official websites, it is better to read the first-hand information.

I made a note when I read the official document before, and interested students can also take a look.

Ceres notes
https://blog.csdn.net/tiancailx/article/details/117601117

2 Code explanation of ceres-based back-end optimization

Next, let's take a look at how to use ceres to solve the back-end optimization.

The first is the header file, the header file is basically the same, that is, the inheritance of the interface, so I won’t say much.

2.1 Adding nodes to the pose graph

Since ceres is not specially used for pose graph optimization, the essence of ceres is to solve the least squares problem.

The essence of the pose graph optimization in the current slam problem depends on the least squares, so the pose graph optimization can be converted into a least squares problem.

There is no concept of nodes and edges inside ceres, so in the AddNode function, unlike g2o, you can directly add nodes to g2o. Here, you first save the nodes, wait until the time of optimization to perform calculations, and then pass them into ceres.

Since the data type in ceres is double, the format conversion is performed when saving.

// 添加节点
void CeresSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex)
{
    
    
  karto::Pose2 pose = pVertex->GetObject()->GetCorrectedPose();
  int pose_id = pVertex->GetObject()->GetUniqueId();
  Pose2d pose2d;
  pose2d.x = pose.GetX();
  pose2d.y = pose.GetY();
  pose2d.yaw_radians = pose.GetHeading();
  poses_[pose_id] = pose2d;
}

struct Pose2d
{
    
    
  double x;
  double y;
  double yaw_radians;
};

2.2 Adding edges (constraints) to the pose graph

The implementation of the AddConstraint function is similar to the AddNode function.

Since the edge cannot be directly added to ceres, the data format is converted first, and then the edge is saved.

The data saved here are the IDs of the two nodes and the pose transformation between the two nodes.

The inverse operation is performed when saving the covariance matrix of the pose, so what is saved is the information matrix.

// 添加约束
void CeresSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge)
{
    
    
  karto::LocalizedRangeScan *pSource = pEdge->GetSource()->GetObject();
  karto::LocalizedRangeScan *pTarget = pEdge->GetTarget()->GetObject();
  karto::LinkInfo *pLinkInfo = (karto::LinkInfo *)(pEdge->GetLabel());

  karto::Pose2 diff = pLinkInfo->GetPoseDifference();
  karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse();
  Eigen::Matrix<double, 3, 3> info;
  info(0, 0) = precisionMatrix(0, 0);
  info(0, 1) = info(1, 0) = precisionMatrix(0, 1);
  info(0, 2) = info(2, 0) = precisionMatrix(0, 2);
  info(1, 1) = precisionMatrix(1, 1);
  info(1, 2) = info(2, 1) = precisionMatrix(1, 2);
  info(2, 2) = precisionMatrix(2, 2);
  Eigen::Vector3d measurement(diff.GetX(), diff.GetY(), diff.GetHeading());

  Constraint2d constraint2d;
  constraint2d.id_begin = pSource->GetUniqueId();
  constraint2d.id_end = pTarget->GetUniqueId();
  constraint2d.x = measurement(0);
  constraint2d.y = measurement(1);
  constraint2d.yaw_radians = measurement(2);
  constraint2d.information = info;
  constraints_.push_back(constraint2d);
}

struct Constraint2d
{
    
    
  int id_begin;
  int id_end;

  double x;
  double y;
  double yaw_radians;

  Eigen::Matrix3d information;
};

2.3 Optimization solution

Solve and save the optimized pose.

Before saving the data, the real calculation is in these two functions, which are BuildOptimizationProblem() and SolveOptimizationProblem().

// 优化求解
void CeresSolver::Compute()
{
    
    
  corrections_.clear();

  ROS_INFO("[ceres] Calling ceres for Optimization");
  ceres::Problem problem;
  BuildOptimizationProblem(constraints_, &poses_, &problem);
  SolveOptimizationProblem(&problem);
  ROS_INFO("[ceres] Optimization finished\n");

  for (std::map<int, Pose2d>::const_iterator pose_iter = poses_.begin(); pose_iter != poses_.end(); ++pose_iter)
  {
    
    
    karto::Pose2 pose(pose_iter->second.x, pose_iter->second.y, pose_iter->second.yaw_radians);
    corrections_.push_back(std::make_pair(pose_iter->first, pose));
  }
}

2.4 Build the model

2.4.1 Building the least squares problem

Simply explain the code.

First, define how to update the angle through the AngleLocalParameterization::Create() function.

It should be noted here that the ownership of the ceres::LocalParameterization *angle_local_parameterization pointer is handed over to the problem in the problem->SetParameterization() function, that is, angle_local_parameterization will be destroyed when the problem is destructed.

Note that this pointer is placed above the for loop. It can be seen that it is possible to pass the same pointer to AddResidualBlock() and SetParameterization() multiple times. Only after the problem is destroyed, the pointer is destroyed and cannot be used again up.

When I wrote the code for the first time, I put the pointer angle_local_parameterization in the member variable of the class, and only added new once in the constructor, and the program crashed when I optimized it for the second time. It is worth noting.

After that, CostFunction is constructed by PoseGraph2dErrorTerm::Create().

Then, the existing constraint information is added to the optimization problem through AddResidualBlock(), and at the same time, the update method of the angle value is stipulated.

Finally, set the pose of the first node to be fixed, without optimization and change.

void CeresSolver::BuildOptimizationProblem(const std::vector<Constraint2d> &constraints, std::map<int, Pose2d> *poses,
                                           ceres::Problem *problem)
{
    
    
  if (constraints.empty())
  {
    
    
    std::cout << "No constraints, no problem to optimize.";
    return;
  }

  ceres::LocalParameterization *angle_local_parameterization = AngleLocalParameterization::Create();

  for (std::vector<Constraint2d>::const_iterator constraints_iter = constraints.begin();
       constraints_iter != constraints.end(); ++constraints_iter)
  {
    
    
    const Constraint2d &constraint = *constraints_iter;
    std::map<int, Pose2d>::iterator pose_begin_iter = poses->find(constraint.id_begin);
    std::map<int, Pose2d>::iterator pose_end_iter = poses->find(constraint.id_end);

    // 对information开根号
    const Eigen::Matrix3d sqrt_information = constraint.information.llt().matrixL();

    // Ceres will take ownership of the pointer.
    ceres::CostFunction *cost_function =
        PoseGraph2dErrorTerm::Create(constraint.x, constraint.y, constraint.yaw_radians, sqrt_information);
    
    problem->AddResidualBlock(cost_function, nullptr,
                              &pose_begin_iter->second.x,
                              &pose_begin_iter->second.y,
                              &pose_begin_iter->second.yaw_radians,
                              &pose_end_iter->second.x,
                              &pose_end_iter->second.y,
                              &pose_end_iter->second.yaw_radians);
    problem->SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization);
    problem->SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization);
  }
  std::map<int, Pose2d>::iterator pose_start_iter = poses->begin();
  problem->SetParameterBlockConstant(&pose_start_iter->second.x);
  problem->SetParameterBlockConstant(&pose_start_iter->second.y);
  problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
}

2.4.2 Angle update method

The update method of the angle is defined by overloading (), and it is limited that the angle is still between [-pi, pi] after the update. The code is very simple and I won’t say much.

Afterwards, problem->SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization);by associating the definition of this angle update method with the angle variable.

// Defines a local parameterization for updating the angle to be constrained in
// [-pi to pi).
class AngleLocalParameterization
{
    
    
public:
  template <typename T>
  bool operator()(const T *theta_radians, const T *delta_theta_radians, T *theta_radians_plus_delta) const
  {
    
    
    *theta_radians_plus_delta = NormalizeAngle(*theta_radians + *delta_theta_radians);

    return true;
  }

  static ceres::LocalParameterization *Create()
  {
    
    
    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization, 1, 1>);
  }
};

// Normalizes the angle in radians between [-pi and pi).
template <typename T>
inline T NormalizeAngle(const T &angle_radians)
{
    
    
  // Use ceres::floor because it is specialized for double and Jet types.
  T two_pi(2.0 * M_PI);
  return angle_radians - two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
}

2.4.3 Calculation of residuals

First, declare the ceres::CostFunction through the PoseGraph2dErrorTerm::Create() function, and pass in the constrained x, y, theta and the information matrix after the square root to the constructor of the PoseGraph2dErrorTerm class to save the data.

After that, the residual calculation method is defined by overloading (). The parameters of the functor are problem->AddResidualBlock()passed in, which are the xy and angle of the two poses.

The residual between the two poses can be calculated in this way.
The coordinate transformation between the two poses is passed in during construction, and a coordinate transformation can be calculated through the poses of the two nodes. Under normal circumstances, the two The two coordinate transformations should be equal, so the difference between the two coordinate transformations can be used as the residual.

In the code RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a)is the coordinate transformation calculated based on the poses of the two nodes, p_ab_.cast<T>()which is the coordinate transformation between the two incoming poses, and the xy residual is obtained by subtracting the two.

Similarly, subtracting the angles yields the residual of the angles.

Here, after the calculation is completed, the residual is multiplied by the information matrix after the square root, and noise is added to the calculated residual through the covariance matrix.

template <typename T>
Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians)
{
    
    
  const T cos_yaw = ceres::cos(yaw_radians);
  const T sin_yaw = ceres::sin(yaw_radians);

  Eigen::Matrix<T, 2, 2> rotation;
  rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
  return rotation;
}

// Computes the error term for two poses that have a relative pose measurement
// between them. Let the hat variables be the measurement.
//
// residual =  information^{1/2} * [  r_a^T * (p_b - p_a) - \hat{p_ab}   ]
//                                 [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
//
// where r_a is the rotation matrix that rotates a vector represented in frame A
// into the global frame, and Normalize(*) ensures the angles are in the range
// [-pi, pi).
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>
  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) const
  {
    
    
    const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
    const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);

    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);

    residuals_map.template head<2>() = RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
    residuals_map(2) = NormalizeAngle((*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));

    // Scale the residuals by the square root information matrix to account for
    // the measurement uncertainty.
    residuals_map = sqrt_information_.template cast<T>() * residuals_map;

    return true;
  }

  static ceres::CostFunction *Create(double x_ab, double y_ab, double yaw_ab_radians,
                                     const Eigen::Matrix3d &sqrt_information)
  {
    
    
    return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
        new PoseGraph2dErrorTerm(x_ab, y_ab, yaw_ab_radians, sqrt_information)));
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
  // The position of B relative to A in the A frame.
  const Eigen::Vector2d p_ab_;
  // The orientation of frame B relative to frame A.
  const double yaw_ab_radians_;
  // The inverse square root of the measurement covariance matrix.
  const Eigen::Matrix3d sqrt_information_;
};

2.5 Solving

This piece of code is relatively simple.

First set the parameters of the solution, then call ceres::Solve() to solve, and print out a brief report of the solution results.

// Returns true if the solve was successful.
bool CeresSolver::SolveOptimizationProblem(ceres::Problem *problem)
{
    
    
  assert(problem != NULL);

  ceres::Solver::Options options;
  options.max_num_iterations = 100;
  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

  ceres::Solver::Summary summary;
  ceres::Solve(options, problem, &summary);

  std::cout << summary.BriefReport() << '\n';
  return summary.IsSolutionUsable();
}

3 run

3.1 Dependency

The code of this article needs to rely on the 1.13.0 version of the ceres library. If you have not installed ceres, you need to install it first.
I put the installation package of the ceres library in the project Creating-2D-laser-slam-from-scratch/TrirdParty The file is added, and it can be directly decompressed and installed.

For the installation method, please refer to the installation instructions in the install_dependence.sh script, or you can directly execute this script to install all dependencies.

After installing ceres, compile the code, if everything goes well, it can be compiled and passed.

Here I want to say that many students said that the code cannot be compiled on ubuntu 18 or other ubuntu versions, but I have no choice. I have been using 1604 and have not tried other versions. Go ahead, or look at the source code.

3.2 run

For the data package corresponding to this article, please reply to lesson6 in my official account to get it, and change the bag_filename in launch to your actual directory name.

I put the links of the previously used data packages in the Tencent document, the address of the Tencent document is as follows:
https://docs.qq.com/sheet/DVElRQVNlY0tHU01I?tab=BB08J2

Run the program corresponding to this article through the following command
roslaunch lesson6 karto_slam_outdoor.launch solver_type:=ceres_solver

3.3 Analysis of results

After startup, the specific type of optimizer used will be displayed.

[ INFO] [1637029966.740001825]: ----> Karto SLAM started.
[ INFO] [1637029966.790614727]: Use back end.
[ INFO] [1637029966.790716666]: solver type is CeresSolver.

In the early stage of operation, because no loopback was found, no optimization has been performed. When a loopback is generated in the final stage, the optimization based on ceres will be called, and the following log will be printed.

[ INFO] [1637030162.060933669, 1606808844.440984444]: [ceres] Calling ceres for Optimization
Ceres Solver Report: Iterations: 8, Initial cost: 4.364248e-02, Final cost: 7.465524e-15, Termination: CONVERGENCE
[ INFO] [1637030162.089423589, 1606808844.471175613]: [ceres] Optimization finished

It can be seen that the cost before optimization is on the order of e-02, and the cost after optimization is on the order of e-15, and the effect is very obvious.

before optimization

Optimized

final map

final cost

Looking at the final cost, we can see that the previously optimized cost is at the level of e-15, but the last cost is e-02, which has become much larger. The reason is not clear.

Guessing that it may be due to the sudden end of the data data, there is still room for optimization???

Anyone who knows something can comment and tell me, thank you very much.

[ INFO] [1637032488.825845405, 1606808848.018522836]: [ceres] Calling ceres for Optimization
Ceres Solver Report: Iterations: 8, Initial cost: 6.562447e-02, Final cost: 5.582333e-15, Termination: CONVERGENCE
[ INFO] [1637032488.850353017, 1606808848.038643242]: [ceres] Optimization finished

[ INFO] [1637032489.159521180, 1606808848.351362847]: [ceres] Calling ceres for Optimization
Ceres Solver Report: Iterations: 8, Initial cost: 2.219165e-02, Final cost: 6.116063e-15, Termination: CONVERGENCE
[ INFO] [1637032489.183078270, 1606808848.371497779]: [ceres] Optimization finished

[ INFO] [1637032489.493828053, 1606808848.683609037]: [ceres] Calling ceres for Optimization
Ceres Solver Report: Iterations: 9, Initial cost: 4.919947e+01, Final cost: 1.920770e-02, Termination: CONVERGENCE
[ INFO] [1637032489.519517167, 1606808848.703747883]: [ceres] Optimization finished

4 Summary

Through this article, we know how to use ceres to build and calculate the back-end optimization problem, and experience the map effect and cost value before and after optimization.

The next article will use gtsam to calculate the back-end optimization.