点击上方“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<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 向位姿图中添加边(约束)

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

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

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

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

// 添加约束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 优化求解

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

之前都是在保存数据, 真正的计算就在这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<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 搭建模型

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<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 角度更新方式

通过重载 () 定义了角度的更新方式, 限定了角度更新后依然处于 [-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: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 残差的计算

首先通过 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<T>() 就是传入的这2个位姿间的坐标变换, 通过让2者相减得到 xy的残差.

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

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

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_NEWprivate:// 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 进行求解

这块的代码就比较简单了.

首先设置了求解时的参数, 然后调用 ceres::Solve() 进行求解, 并将求解结果的简要报告打印出来.

// 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 运行

3.1 依赖

这篇文章的代码是需要依赖 1.13.0 版本的ceres库, 如果没装ceres的需要先安装一下.
我将ceres库的安装包放在了 工程Creating-2D-laser-slam-from-scratch/TrirdParty文件夹内, 可以直接解压安装.

安装的方法可以看一下 install_dependence.sh 脚本中的安装指令, 也可以直接执行这个脚本进行所有依赖项的安装.

安装完了ceres之后, 编译代码, 如果一切顺利的话是可以编译通过的.

这里要说一下, 很多同学说代码在ubuntu 18 或者其他的ubuntu版本上编译不过, 这我也没啥办法, 我一直用的是 1604, 没试过其他的版本. 如果实在编译不过就看看文章吧, 或者看看源码.

3.2 运行

本篇文章对应的数据包, 请在我的公众号中回复 lesson6 获得，并将launch中的bag_filename更改成您实际的目录名。

我将之前使用过的数据包的链接都放在腾讯文档里了, 腾讯文档的地址如下:
https://docs.qq.com/sheet/DVElRQVNlY0tHU01I?tab=BB08J2

通过如下命令运行本篇文章对应的程序
roslaunch lesson6 karto_slam_outdoor.launch solver_type:=ceres_solver

3.3 结果分析

启动之后, 会显示出使用的优化器的具体类型.

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

在运行前期, 由于没有找到回环, 所以一直没有进行优化. 在最后阶段产生回环时, 会调用基于ceres的优化, 并打印出如下的log.

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

可以看到, 优化前的cost是 e-02 量级的, 优化后的cost是 e-15 量级的, 效果十分明显.

优化前

优化后

最终的地图

最终的cost

看一下最终的cost, 可以看到, 之前优化后的cost都是 e-15 这种级别的, 但是最后一次的cost是 e-02, 变大了很多. 原因还不清楚.

猜测可能是由于数据数据突然结束了, 导致其实还存在优化的空间???

有了解的同学可以评论告诉我一下, 十分感谢.

[ 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总结

通过这篇文章, 我们知道了如何使用ceres进行后端优化问题的搭建与计算, 并体验了优化前后的地图效果与cost值.

本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

11.自动驾驶中的深度学习模型部署实战

12.相机模型与标定(单目+双目+鱼眼）

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~