基于ceres的后端优化的代码实现

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

来源丨从零开始搭SLAM

作者丨李太白lx

由于g2o天然是进行位姿图优化的, 所以十分契合karto的位姿图的接口, 只需要将对应的顶点和约束分别赋值过去就可以了.

这篇文章我们来看一下另一个比较常用的优化库 Ceres solver.

1 ceres简介

Ceres solver 是谷歌开发的一款用于非线性优化的库, 在激光SLAM和V-SLAM的优化中均有着大量的应用, 如Cartographer, VINS 等等.

ceres的学习可以直接去它的官网上看教程, 谷歌出品的库的文档都很棒. 我在这里就不多说了. http://ceres-solver.org/tutorial.html

目前网上的很多教程及文章都是翻译的官网, 还是去看第一手资料比较好.

我之前看官方文档的时候做过一个笔记, 感兴趣的同学也可以看看.

ceres笔记 https://blog.csdn.net/tiancailx/article/details/117601117

2 基于ceres的后端优化的代码讲解

接下来就来看一看, 如何使用ceres进行后端优化的求解.

首先是头文件, 头文件基本上一样的, 就是对接口的继承, 就不多说了.

2.1 向位姿图中添加节点

由于ceres不是专门用于位姿图优化的, ceres的本质是求解最小二乘问题.

而目前的slam问题中的位姿图优化的本质都是依赖于最小二乘的, 所以位姿图优化是可以转成最小二乘问题的.

ceres内部没有节点和边的概念, 所以在AddNode函数中, 不像g2o那样可以直接将节点添加到g2o中, 这里首先将节点保存起来, 等到优化的时候再进行计算, 再传入到ceres中.

由于ceres中的数据类型是double, 所以在保存的时候进行了格式的转换.

// 添加节点void CeresSolver::AddNode(karto::Vertex*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 向位姿图中添加边(约束)

AddConstraint这个函数中的实现和AddNode函数是差不多的.

由于不能直接将边添加到ceres中, 所以在这先做了数据格式的转换, 然后将这个边保存起来.

这里保存的数据分别是 2个节点的 ID, 2个节点间的位姿变换.

在保存位姿的协方差矩阵时进行了求逆的操作, 所以保存的是信息矩阵.

// 添加约束void CeresSolver::AddConstraint(karto::Edge*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 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 优化求解

进行求解, 并将优化后的位姿进行保存.

之前都是在保存数据, 真正的计算就在这2个函数里了, 分别是 BuildOptimizationProblem() 与 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::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 搭建模型 2.4.1 搭建最小二乘问题

简单解释一下代码.

首先, 通过AngleLocalParameterization::Create()函数定义了角度的更新方式.

这里要注意, 这个 ceres::LocalParameterization *angle_local_parameterization 指针的所有权是 在 problem->SetParameterization() 函数中 交给了 problem 的了, 也就是说, angle_local_parameterization会在problem析构的时候销毁掉.

注意, 这个指针是放在for循环的上边的, 可知, 同一个指针多次传入 AddResidualBlock() 与 SetParameterization() 是可以的. 只是在 problem 析构之后, 这个指针就被销毁了, 不能再使用了.

我在第一次代码写的时候将 angle_local_parameterization 这个指针放在类的成员变量中了, 并只在构造函数中 new 了一次, 当第二次进行优化的时候程序就崩了. 值得注意一下.

之后, 通过 PoseGraph2dErrorTerm::Create() 进行了 CostFunction 的构造.

然后, 将已有的约束信息通过 AddResidualBlock() 添加到优化问题中, 同时, 规定了角度值的更新方式.

最后, 将第一个节点的位姿设置成固定的, 不进行优化和改变.

void CeresSolver::BuildOptimizationProblem(const std::vector&constraints, std::map *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::const_iterator constraints_iter = constraints.begin();       constraints_iter != constraints.end(); ++constraints_iter)  {const Constraint2d &constraint = *constraints_iter;    std::map::iterator pose_begin_iter = poses->find(constraint.id_begin);    std::map::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::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 角度更新方式

通过重载 () 定义了角度的更新方式, 限定了角度更新后依然处于 [-pi, pi] 之间.代码很简单就不多说了.

之后, 通过 problem->SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization); 将这个角度更新方式的限定与角度变量关联起来.

// Defines a local parameterization for updating the angle to be constrained in// [-pi to pi).class AngleLocalParameterization{public:templatebool 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);  }};// Normalizes the angle in radians between [-pi and pi).templateinline 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 残差的计算

首先通过 PoseGraph2dErrorTerm::Create() 函数进行 ceres::CostFunction 的声明, 并传入 约束的 x, y, theta 以及开方后的信息矩阵 到 PoseGraph2dErrorTerm类的构造函数中将数据进行保存.

之后, 通过重载 () 定义了残差的计算方式. 仿函数的参数就是通过 problem->AddResidualBlock() 传入进来的, 分别是2个位姿的xy与角度.

2个位姿间的残差, 可以这样计算. 构造时传入了这2个位姿间的坐标变换, 而通过这两个节点的位姿又可以算出一个坐标变换, 在正常情况下这2个坐标变换应该是相等的, 所以, 可以让这两个坐标变换的差作为残差.

代码中的 RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) 就是根据2个节点的位姿算出的坐标变换, p_ab_.cast() 就是传入的这2个位姿间的坐标变换, 通过让2者相减得到 xy的残差.

同理, 角度相减得到角度的残差.

这里, 在计算完成之后, 又将残差左乘了开方后的信息矩阵, 通过协方差矩阵来对计算出的残差添加噪声.

templateEigen::Matrix RotationMatrix2D(T yaw_radians){const T cos_yaw = ceres::cos(yaw_radians);const T sin_yaw = ceres::sin(yaw_radians);  Eigen::Matrix 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)  {  }templatebool 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 p_a(*x_a, *y_a);const Eigen::Matrix p_b(*x_b, *y_b);    Eigen::Map