转载自：https://mp.weixin.qq.com/s/fKlG9LWlPI52wStUAv18iw
原创 lovely_yoshino 古月居 今天
在了解SLAM的原理、流程后，个人经常实时困惑该如何去从零开始去设计编写一套能够符合我们需求的SLAM框架。作者认为Ceres、Eigen、Sophus、G2O这几个函数库无法避免，尤其是Ceres函数库在激光SLAM和V-SLAM的优化中均有着大量的应用。所以作者从Ceres作为开端，来对手写SLAM开个头，来方便各位后续的开发。
这里分享一位博主写的博客，个人看了感觉写的不错，对SLAM的剖析很细致。
(*https://blog.csdn.net/unlimitedai)
“1
Ceres示例
首先我们需要明确Ceres函数库在SLAM中主要起到的作用是优化。目前Bundle Adjustment 其本质还是离不开最小二乘原理(几乎所有优化问题其本质都是最小二乘),目前Bundle Adjustment 优化框架最为代表的是Ceres solver和G2O(这里主要介绍ceres solver)。Ceres中的优化需要四步，构建优化的残差函数，构建优化问题，在每次获取到数据后添加残差块，总体优化。
构建残差函数：
//构建代价函数结构体，residual为残差。//last_point_a_为这一帧中的点a，curr_point_b_为点a旋转后和上一帧里最近的点//curr_point_c_为点b同线或上线号的点，curr_point_d_为点b下线号的点//b，c，d与a点距离不超过3m//plane_norm为根据向量bc和bd求出的法向量struct CURVE_FITTING_COST{//类似构造函数CURVE_FITTING_COST(Eigen::Vector3d _curr_point_a_, Eigen::Vector3d _last_point_b_,Eigen::Vector3d _last_point_c_, Eigen::Vector3d _last_point_d_):curr_point_a_(_curr_point_a_),last_point_b_(_last_point_b_),last_point_c_(_last_point_c_),last_point_d_(_last_point_d_){plane_norm = (last_point_d_ - last_point_b_).cross(last_point_c_ - last_point_b_);plane_norm.normalize();}template <typename T>//plane_norm点乘向量ab为a点距面bcd的距离，即残差bool operator()(const T* q,const T* t,T* residual)const{Eigen::Matrix<T, 3, 1> p_a_curr{T(curr_point_a_.x()), T(curr_point_a_.y()), T(curr_point_a_.z())};Eigen::Matrix<T, 3, 1> p_b_last{T(last_point_b_.x()), T(last_point_b_.y()), T(last_point_b_.z())};Eigen::Quaternion<T> rot_q{q[3], q[0], q[1], q[2]};Eigen::Matrix<T, 3, 1> rot_t{t[0], t[1], t[2]};Eigen::Matrix<T, 3, 1> p_a_last;p_a_last=rot_q * p_a_curr + rot_t;residual[0]=abs((p_a_last - p_b_last).dot(plane_norm));return true;}const Eigen::Vector3d curr_point_a_,last_point_b_,last_point_c_,last_point_d_;Eigen::Vector3d plane_norm;};
构建优化问题：
//优化问题构建ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);ceres::LocalParameterization *q_parameterization =new ceres::EigenQuaternionParameterization();ceres::Problem::Options problem_options;ceres::Problem problem(problem_options);problem.AddParameterBlock(para_q, 4, q_parameterization);problem.AddParameterBlock(para_t, 3);
每次求出abcd点后，将他们的坐标构建成Eigen::Vector3d数据，添加残差块：
Eigen::Vector3d curr_point_a(laserCloudIn_plane.points[i].x,laserCloudIn_plane.points[i].y,laserCloudIn_plane.points[i].z);Eigen::Vector3d last_point_b(laserCloudIn_plane_last.points[closestPointInd].x,laserCloudIn_plane_last.points[closestPointInd].y,laserCloudIn_plane_last.points[closestPointInd].z);Eigen::Vector3d last_point_c(laserCloudIn_plane_last.points[minPointInd2].x,laserCloudIn_plane_last.points[minPointInd2].y,laserCloudIn_plane_last.points[minPointInd2].z);Eigen::Vector3d last_point_d(laserCloudIn_plane_last.points[minPointInd3].x,laserCloudIn_plane_last.points[minPointInd3].y,laserCloudIn_plane_last.points[minPointInd3].z);problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,4,3>(new CURVE_FITTING_COST(last_point_a,curr_point_b,curr_point_c,curr_point_d)),loss_function,para_q,para_t);
遍历过所有的a点后，就可以优化求解了。
//所有前一帧里的点都当a点遍历过后，进行优化ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_QR;//迭代数options.max_num_iterations = 5;//进度是否发到STDOUToptions.minimizer_progress_to_stdout = false;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);
而在V-SLAM中Ceres则用的更多，我们可以看下下面的例子。
double para_Pose[7];para_Pose[0] = 0.0;para_Pose[1] = 0.0;para_Pose[2] = 0.0;para_Pose[6] = 1.0;para_Pose[3] = 0.0;para_Pose[4] = 0.0;para_Pose[5] = 0.0;int kNumObservations = cur_pts.size();double invDepth[kNumObservations][1];ceres::LossFunction *loss_function;//loss_function = new ceres::HuberLoss(1.0);loss_function = new ceres::CauchyLoss(1.0);// 柯西核函数ceres::LocalParameterization *local_parameterizationP = new PoseLocalParameterization();problem.AddParameterBlock(para_Pose, 7, local_parameterizationP);//对Pose重新参数化for (int i = 0; i < kNumObservations; ++i) {invDepth[i][0] = 1;problem.AddParameterBlock(invDepth[i], 1); //对深度重新参数化if (!invdepths.empty()&&invdepths[i]>0){// cout << "depth observations "<< 1./invdepths[i] <<" "<< invdepths[i] <<endl;invDepth[i][0] = invdepths[i];problem.SetParameterBlockConstant(invDepth[i]);//把任何参数块设为常数，并且使用SetParameterBlockVariable()来撤销这一操作ceres::CostFunction *f_d;//自动求导方法，AutoDiffCostFunctionf_d = new ceres::AutoDiffCostFunction<autoIvDepthFactor, 1,1>(new autoIvDepthFactor(invdepths[i]) );problem.AddResidualBlock(f_d, loss_function, invDepth[i]);}ceres::CostFunction *f;f = new ceres::AutoDiffCostFunction<autoMonoFactor, 3,7,1>(new autoMonoFactor(Vector3d(un_prev_pts[i].x, un_prev_pts[i].y, 1),Vector3d(un_cur_pts[i].x, un_cur_pts[i].y, 1)) );problem.AddResidualBlock(f, loss_function, para_Pose,invDepth[i]);}ceres::Solver::Options options;// options.max_num_iterations = 7;options.linear_solver_type = ceres::DENSE_SCHUR;options.trust_region_strategy_type = ceres::DOGLEG;options.minimizer_progress_to_stdout = false;ceres::Solver::Summary summary;TicToc solveTime;ceres::Solve(options, &problem, &summary);
“2
Ceres常见函数总结
1、创建优化问题与损失核函数
用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等；该参数可以取NULL或nullptr，此时损失函数为单位函数。
ceres::Problem problem;ceres::LossFunction *loss_function; // 损失核函数//loss_function = new ceres::HuberLoss(1.0);loss_function = new ceres::CauchyLoss(1.0); // 柯西核函数
2、在创建的problem中添加优化问题的优化变量 problem.AddParameterBlock()，该函数常用的重载有两个。
用户在调用AddResidualBlock( )时其实已经隐式地向Problem传递了参数模块，但在一些情况下，需要用户显示地向Problem传入参数模块（通常出现在需要对优化参数进行重新参数化的情况）。Ceres提供了Problem::AddParameterBlock( )函数用于用户显式传递参数模块：
void AddParameterBlock(double* values, int size);void AddParameterBlock(double* values,int size,LocalParameterization* local_parameterization);
注：values表示优化变量，size表示优化变量的维度。
其中，第一种函数原型除了会增加一些额外的参数检查之外，功能上和隐式传递参数并没有太大区别。第二种函数原型则会额外传入LocalParameterization参数，用于重构优化参数的维数。
LocalParameterization是在优化Manifold(流形)上的变量时需要考虑的，Manifold上变量是过参数的，即Manifold上变量的维度大于其自由度。这会导致Manifold上变量各个量之间存在约束，如果直接对这些量求导、优化，那么这就是一个有约束的优化，实现困难。为了解决这个问题，在数学上对Manifold在当前变量值处形成的切空间求导，在切空间上优化，最后投影回Manifold。
对于SLAM问题，广泛遇到的Manifold是旋转，旋转仅需要3个量，但实际运用中涉及到万向锁问题，在更高维空间表达旋转，四元数就是在维度4表达3个自由度的三维空间的旋转。
bool ComputeJacobian()计算得到一个4*3的矩阵(global_to_local)，含义是Manifold上变量对Tangent Space上变量的导数，在ceres::CostFunction处提供residuals对Manifold上变量的倒数，乘以这个矩阵，之后变就变成了对Tangent Space上变量的导数。
problem.AddParameterBlock(quaternion, 4);// 直接传递4维参数ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization();problem.AddParameterBlock(quaternion, 4, local_param)//重构参数，优化时实际使用的是3维的等效旋转矢量
3、添加残差块
一个优化问题可以看成通过调整参数将一大堆各种各样的残差降到最小，因此，残差的提供是至关重要的，一个残差的构建离不开残差的数学定义以及关联的参数，ceres添加残差块通过 AddResidualBlock() 完成 ， 有两个重载貌似最为常用。
template <typename... Ts>ResidualBlockId AddResidualBlock(CostFunction* cost_function,LossFunction* loss_function,double* x0,Ts*... xs)ResidualBlockId AddResidualBlock(CostFunction* cost_function,LossFunction* loss_function,const std::vector<double*>& parameter_blocks);
也就是需要提供三种参数 —— cost_function对象、鲁棒核函数对象、 该残差的关联参数 。
代价函数：包含了参数模块的维度信息，内部使用仿函数定义误差函数的计算方式。AddResidualBlock( )函数会检测传入的参数模块是否和代价函数模块中定义的维数一致，维度不一致时程序会强制退出。代价函数模块的详解参见Ceres详解（二） CostFunction。
损失函数：用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等（完整的参数列表参见Ceres API文档）；该参数可以取NULL或nullptr，此时损失函数为单位函数。
参数模块：待优化的参数，可一次性传入所有参数的指针容器vector<double*>或依次传入所有参数的指针double*</double*>。
其中重点是cost_function对象的给定，它有三种常见的提供方式：
自动导数（AutoDiffCostFunction）：由ceres自行决定导数的计算方式，最常用的求导方式。
数值导数（NumericDiffCostFunction）：由用户手动编写导数的数值求解形式，通常在残差函数的计算使用无法直接
调用的库函数，导致调用AutoDiffCostFunction类构建时使用；但手动编写的精度和计算效率不如模板类，因此不到不得已，官方并不建议使用该方法。
解析导数（Analytic Derivatives）：当导数存在闭合解析形式时使用，用于可基于CostFunciton基类自行编写；但由于需要自行管理残差和雅克比矩阵，除非闭合解具有具有明显的精度和效率优势，否则同样不建议使用。
例如loam-livox 的ICP的ceres 自动求导实现：
// p2p with motion deblur 点-点 ICPtemplate <typename _T>struct ceres_icp_point2point_mb{Eigen::Matrix<_T, 3, 1> m_current_pt; // 当前的点Eigen::Matrix<_T, 3, 1> m_closest_pt; // 最近的点_T m_motion_blur_s; // 用于畸变去除// 上一次变换Eigen::Matrix<_T, 4, 1> m_q_last;Eigen::Matrix<_T, 3, 1> m_t_last;_T m_weigh;ceres_icp_point2point_mb( const Eigen::Matrix<_T, 3, 1> current_pt,const Eigen::Matrix<_T, 3, 1> closest_pt,const _T &motion_blur_s = 1.0,Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) ) : m_current_pt( current_pt ),m_closest_pt( closest_pt ),m_motion_blur_s( motion_blur_s ),m_q_last( q_s ),m_t_last( t_s ){m_weigh = 1.0;};// operator() 重载计算残差，通过输入的参数，并返回template <typename T>bool operator()( const T *_q, const T *_t, T *residual ) const{// 上一次的变换Eigen::Quaternion<T> q_last{ ( T ) m_q_last( 0 ), ( T ) m_q_last( 1 ), ( T ) m_q_last( 2 ), ( T ) m_q_last( 3 ) };Eigen::Matrix<T, 3, 1> t_last = m_t_last.template cast<T>();// 畸变去除Eigen::Quaternion<T> q_incre{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };Eigen::Matrix<T, 3, 1> t_incre{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };Eigen::Quaternion<T> q_interpolate = Eigen::Quaternion<T>::Identity().slerp( ( T ) m_motion_blur_s, q_incre );Eigen::Matrix<T, 3, 1> t_interpolate = t_incre * T( m_motion_blur_s );// 当前的点Eigen::Matrix<T, 3, 1> pt{ T( m_current_pt( 0 ) ), T( m_current_pt( 1 ) ), T( m_current_pt( 2 ) ) };// 当前点经过变换后的位置Eigen::Matrix<T, 3, 1> pt_transfromed;pt_transfromed = q_last * ( q_interpolate * pt + t_interpolate ) + t_last;residual[ 0 ] = ( pt_transfromed( 0 ) - T( m_closest_pt( 0 ) ) ) * T( m_weigh );residual[ 1 ] = ( pt_transfromed( 1 ) - T( m_closest_pt( 1 ) ) ) * T( m_weigh );residual[ 2 ] = ( pt_transfromed( 2 ) - T( m_closest_pt( 2 ) ) ) * T( m_weigh );return true;};static ceres::CostFunction *Create( const Eigen::Matrix<_T, 3, 1> current_pt,const Eigen::Matrix<_T, 3, 1> closest_pt,const _T motion_blur_s = 1.0,Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) ){return ( new ceres::AutoDiffCostFunction< // 自动求导ceres_icp_point2point_mb, 3, 4, 3>( // 对应的是operator中的维度new ceres_icp_point2point_mb( current_pt, closest_pt, motion_blur_s ) ) );}};
其中重载操作符()（必有），操作符()是一个模板方法，返回值为bool型，接受参数依次为待优化变量和残差变量，且待优化变量的传入方式应和Problem::AddResidualBlock()一致。
4.其他成员函数
// 设定对应的参数模块在优化过程中保持不变void Problem::SetParameterBlockConstant(double *values)// 设定对应的参数模块在优化过程中可变void Problem::SetParameterBlockVariable(double *values)// 设定优化下界void Problem::SetParameterLowerBound(double *values, int index, double lower_bound)// 设定优化上界void Problem::SetParameterUpperBound(double *values, int index, double upper_bound)// 该函数紧跟在参数赋值后，在给定的参数位置求解Problem，给出当前位置处的cost、梯度以及Jacobian矩阵；bool Problem::Evaluate(const Problem::EvaluateOptions &options,double *cost, vector<double>* residuals,vector<double> *gradient, CRSMatrix *jacobian)
5.求解
ceres::Solve(options, &problem, &summary);参考链接：
https://blog.csdn.net/wzheng92/article/details/79874724；
https://zhuanlan.zhihu.com/p/332237105；
https://blog.csdn.net/unlimitedai/article/details/107701861
参考代码：
#include <iostream>#include <opencv2/core/core.hpp>#include <ceres/ceres.h>#include <chrono>using namespace std;// 代价函数的计算模型struct CURVE_FITTING_COST {CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}// 残差的计算template<typename T>bool operator()(const T *const abc, // 模型参数，待优化的参数，有3维T *residual) const {residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c) //残差，也就是代价函数的输出return true;}const double _x, _y; // x,y数据};int main(int argc, char **argv) {double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值int N = 100; // 数据点double w_sigma = 1.0; // 噪声Sigma值double inv_sigma = 1.0 / w_sigma;cv::RNG rng; // OpenCV随机数产生器vector<double> x_data, y_data; // 数据for (int i = 0; i < N; i++) {double x = i / 100.0;x_data.push_back(x);y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));}double abc[3] = {ae, be, ce};// 构建最小二乘问题ceres::Problem problem;for (int i = 0; i < N; i++) {problem.AddResidualBlock( // 向问题中添加误差项// 使用自动求导，将定义的代价函数结构体传入。模板参数：误差类型，输出维度即残差的维度，输入维度即优化参数的维度，维数要与前面struct中一致new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])),nullptr, // 核函数，这里不使用，为空abc // 待估计参数);}// 配置求解器ceres::Solver::Options options; // 这里有很多配置项可以填options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; // 增量方程如何求解//options.linear_solver_type = ceres::DENSE_QR;options.minimizer_progress_to_stdout = true; // 输出到coutceres::Solver::Summary summary; // 优化信息chrono::steady_clock::time_point t1 = chrono::steady_clock::now();ceres::Solve(options, &problem, &summary); // 开始优化，求解chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);cout << "solve time cost = " << time_used.count() << " seconds. " << endl;// 输出结果cout << summary.BriefReport() << endl; //输出优化的简要信息cout << "estimated a,b,c = ";for (auto a:abc) cout << a << " ";cout << endl;return 0;}
cmakelists.txt:
cmake_minimum_required(VERSION 2.8)project(gaussnewton)set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")find_package(OpenCV REQUIRED)include_directories(${OpenCV_INCLUDE_DIRS})find_package(Ceres REQUIRED)include_directories(${CERES_INCLUDE_DIRS})include_directories("/usr/include/eigen3")set(SOURCE_FILES main.cpp)add_executable(gaussnewton ${SOURCE_FILES})target_link_libraries(gaussnewton ${OpenCV_LIBS})target_link_libraries(gaussnewton ${CERES_LIBRARIES})