slam中ceres的常见用法总结

ceres的使用过程基本可以总结为:
1、创建优化问题与损失核函数。

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()，该函数常用的重载有两个 ——

void AddParameterBlock(double* values, int size);
void AddParameterBlock(double* values,
                         int size,
                         LocalParameterization* local_parameterization);

区别是若添加的参数更新方式需要另外定义，如四元数，则需要用ceres::LocalParameterization对象重新定义该参数更新的广义加法并作为参数传入AddParameterBlock()。
例如 VINS中的POSE，首先对Pose重新参数化-
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
然后再添加
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);

3、添加残差块
一个优化问题可以看成通过调整参数将一大堆各种各样的残差降到最小，因此，残差的提供是至关重要的，一个残差的构建离不开残差的数学定义以及关联的参数，ceres添加残差块通过 AddResidualBlock()完成 ， 有两个重载貌似最为常用，

template 
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0,
                                   Ts*... xs)
ResidualBlockId AddResidualBlock(
      CostFunction* cost_function,
      LossFunction* loss_function,
      const std::vector& parameter_blocks);                                   

也就是需要提供三种参数 —— cost_function对象、鲁棒核函数对象、 该残差的关联参数 。
其中重点是cost_function对象的给定，它有两种常见的提供方式：
(1)、 创建一个派生于CostFunction的Factor对象, 如 IMUFactor，然后需要自己实现Evaluate()函数，直接完成jacobian的运算，参考VINS代码。
(2)、 采用ceres::AutoDiffCostFunction对象，即自动数值求导对象，AutoDiffCostFunction对象也是CostFunction的子类, 自定义一个Factor对象，并且重载operator()，在其中完成残差的计算，然后将该Factor对象作为参数即可构造ceres::AutoDiffCostFunction对象。这样的好处是不需要自己计算jacobian，但是效率可能会低点，下面是实例-
创建 ceres::AutoDiffCostFunction对象

// 创建 ceres::AutoDiffCostFunction 对象并返回  
	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
									   const Eigen::Vector3d last_point_b_, const double s_)
	{   
		// 自动求导     残差维度 3   优化变量维度  7 
		return (new ceres::AutoDiffCostFunction(new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
	}

添加到AddResidualBlock

// 先求出代价函数    static 函数  
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
// 添加残差   代价函数  核函数   优化变量 
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);

4、设置优化参数，ceres::Solve求解即可。