ceres的常见用法总结

1. ceres 使用流程

ceres的使用过程基本可以总结为:

1.1 创建优化问题与损失核函数

ceres::Problem problem;
ceres::LossFunction *loss_function;         // 损失核函数 
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0); // 柯西核函数   

1.2 添加参数块，在创建的problem中添加优化问题的优化变量

添加优化变量 problem.AddParameterBlock()，该函数常用的重载有两个

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

注意：这里添加的优化变量并不一定是ceres内部运算时采用的优化变量，例如我们通常会采用四元数+平移也就是SE3作为外部维护的状态，在ceres优化中也被成为global parameter，但通常会对se3(local parameter)求解jacobian，那么此时我们必须要告诉ceres，求解出se3的优化增量后如何对SE3进行更新，这个就是通过指定参数化方式完成的，即传入LocalParameterization的子类对象。

例如 VINS中的POSE，创建POSE的ceres参数化对象-

ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();

然后再添加

problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);

问：参数化类中需要实现什么？

一是定义该参数优化过程中，求解出来的Local parameter对Global parameter进行更新的方法，

virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const

二是定义Global parameter对Local parameter的jacobian,

virtual bool ComputeJacobian(const double* x, double* jacobian) const

注意：因为ceres优化时，只能设置残差关于李群的jacobian(通过实现ceres::SizedCostFunction子类完成)，但我们需要的其实是残差关于李代数的jacobian，因此通过这个函数传入李群与对应李代数的jacobian，从而实现转换。

1.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对象、鲁棒核函数对象、 该残差的关联参数 。
其中重点是cost_function对象的给定，它有两种常见的提供方式：

1.3.1 手动求导

创建一个派生于CostFunction的Factor对象, 如 IMUFactor，然后需要自己实现Evaluate()函数，直接完成jacobian的运算，参考VINS代码。
例如：class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
其中 15表示该Factor残差的维度， 7，9，7，9表示该Factor内部所有参数块的李群维度。

1.3.2 自动求导

采用ceres::AutoDiffCostFunction对象，即自动求导对象，AutoDiffCostFunction对象也是CostFunction的子类, 自定义一个Factor对象，并且重载operator()，在其中完成残差的计算，然后将该Factor对象作为参数即可构造ceres::AutoDiffCostFunction对象。这样的好处是不需要自己计算jacobian，但是效率可能会低点，有不少开源方案采用这种模式,例如：ALOAM

1. 创建 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<LidarEdgeFactor, 3, 4, 3>(new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
	}

2. 添加到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);

loam-livox 的ICP的ceres 自动求导实现:

// p2p with motion deblur 点-点 ICP
template <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 ) ) };   
        // 当前点经过变换后的位置     为什么还要乘个T_last ?????????????????????????????????
        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>(     // 残差3   参数维度 4,3 
            new ceres_icp_point2point_mb( current_pt, closest_pt, motion_blur_s ) ) );
    }
};

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

参考

1. SLAM不dunk的博客
2. chengwei0019的博客
3. chengwei0024的开源项目testICP
4. 知乎陌归