位姿图优化(Ceres& G2O& GTSAM)

作为SLAM中常用的方法，其原因是因为SLAM观测不只考虑到当前帧的情况，而需要加入之前状态量的观测。就比如一个在二维平面上移动的机器人，机器人可以使用一组传感器，例如车轮里程计或激光测距仪。从这些原始测量值中，我们想要估计机器人的轨迹并构建环境地图。为了降低问题的计算复杂度，位姿图方法将原始测量值抽象出来。具体来说，它创建了一个表示机器人姿态的节点图，以及表示两个节点之间的相对变换（增量位置和方向）的边。边缘是源自原始传感器测量值的虚拟测量值，例如通过集成原始车轮里程计或对齐从机器人获取的激光范围扫描。对上图的具体描述如下

之前作者已经已经在《SLAM本质剖析-Ceres》中详细的介绍了SLAM中对Ceres的使用。使用 Ceres Solver求解非线性优化问题，主要包括以下几部分：

与g2o优化中直接在边上添加information（信息矩阵）不同，Ceres无法直接实现此功能。 由于Ceres只接受最小二乘优化就，即$min(r^{^{T}}r)$;若要对残差加权，使用马氏距离即$min (r^{^{T}}\Sigma r)$,则要对信息矩阵（协方差矩阵）$\Sigma$做Cholesky分解：$LL^{^{T}}=\Sigma$，则$r^{^{T}}\Sigma r=r^{^{T}}(LL^{^{T}})r=(L^{^{T}}r)^{T}(L^{T}r)$，令$r’=L^{T}r$,最终得到纯种的最小二乘优化：$min(r’^{^{T}}r’)$。

这一块的内容在前文中已经详细阐述了，这里就不详细分析了，下面直接贴一下每个部分的代码以及结构的组成。

（1）AutoDiffCostFunction（自动求导）

模板参数分别是：代价结构体仿函数类型名，残差维度，第一个优化变量维度，第二个….；函数参数（new costFunc）是代价结构体（或类）对象，即此对象重载的 () 函数的函数指针

（2）NumericDiffCostFunction（数值求导）

（3）自定义CostFunction

class PoseGraphCFunc : public ceres::SizedCostFunction{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;PoseGraphCFunc(const Sophus::SE3 measure,const Matrix6d covariance):_measure(measure),_covariance(covariance){}~PoseGraphCFunc(){}virtual bool Evaluate(double const_ const_ parameters,double _residuals,double **jacobians)const{//创建带有参数的SE3Sophus::SE3 pose_i=Sophus::SE3::exp(Vector6d(parameters[0]));//Eigen::MtatrixXd可以通过double指针来构造Sophus::SE3 pose_j=Sophus::SE3::exp(Vector6d(parameters[1]));Eigen::Map residual(residuals);//Eigen::Map(ptr)函数的作用是通过传入指针将c++数据映射为Eigen上的Matrix数据，有点与&相似//通过LLT分解得到信息矩阵residual=(_measure.inverse()_pose_i.inverse()_pose_j).log();Matrix6d sqrt_info=Eigen::LLT(_covariance).matrixLLT().transpose();//计算雅克比矩阵if(jacobians){if(jacobians[0]){Eigen::Map jacobian_i(jacobians[0]);Matrix6d Jr=JRInv(Sophus::SE3::exp(residual));jacobian_i=(sqrt_info)_(-Jr)_pose_j.inverse().Adj();}if(jacobians[1]){Eigen::Map jacobian_j(jacobians[1]);Matrix6d J = JRInv(Sophus::SE3::exp(residual));jacobian_j = sqrt_info _ J _ pose_j.inverse().Adj();}}residual=sqrt_info_residual;return true;second.x, &pose_end_iter->second.y, &pose_end_iter->second.yaw_radians); // 设置参数化，这里设置边的两个顶点的pose的旋转角度为 local_parameterization problem->SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization); problem->SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization); } // The pose graph optimization problem has three DOFs that are not fully // constrained. This is typically referred to as gauge freedom. You can apply // a rigid body transformation to all the nodes and the optimization problem // will still have the exact same cost. The Levenberg-Marquardt algorithm has // internal damping which mitigate this issue, but it is better to properly // constrain the gauge freedom. This can be done by setting one of the poses // as constant so the optimizer cannot change it. std::map::iterator pose_start_iter = poses->begin(); CHECK(pose_start_iter != poses->end()) SetParameterBlockConstant(&pose_start_iter->second.x); problem->SetParameterBlockConstant(&pose_start_iter->second.y); problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians); } 2. G2O的位姿图优化

G2O这部分也在我们的系列文章《SLAM本质剖析-G2O》中详细的阐述了。这里不详细的去写了。G2O本身就是经常被用于位姿图优化的，所以这里我们直接给出例子。算法的整个流程：1、构造g2o优化问题并执行优化；2、每次优化完成后，对地图点是否为外点进行卡方检验，检验到为内点的加入下次优化，否则下次不优化；3、得到优化后的当前帧的位姿（此函数只优化当前帧位姿）

6）更新isam和计算位姿

7）访问最终矫正后的位姿

https://blog.csdn.net/Curryfun/article/details/105894863https://blog.csdn.net/qq_42731705/article/details/120723600https://blog.csdn.net/ljtx200888/article/details/114278683https://xiaotaoguo.com/p/ceres-usage/https://blog.csdn.net/Walking_roll/article/details/119817174https://blog.csdn.net/lzy6041/article/details/107658568

原创文章作者：敢敢のwings。如若转载，请注明出处：古月居 http://www.guyuehome.com/41992

打赏 0

点赞 0

收藏 0

分享