slam中ceres的用法解析

1. 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内部运算时采用的优化变量，例如我们通常会采用四元数+平移也就是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，从而实现转换。

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)、 创建一个派生于CostFunction的Factor对象, 如 IMUFactor，然后需要自己实现Evaluate()函数，直接完成jacobian的运算，参考VINS代码。
例如：class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
其中 15表示该Factor残差的维度， 7，9，7，9表示该Factor内部所有参数块的李群维度。
(2)、 采用ceres::AutoDiffCostFunction对象，即自动求导对象，AutoDiffCostFunction对象也是CostFunction的子类, 自定义一个Factor对象，并且重载operator()，在其中完成残差的计算，然后将该Factor对象作为参数即可构造ceres::AutoDiffCostFunction对象。这样的好处是不需要自己计算jacobian，但是效率可能会低点，有不少开源方案采用这种模式, 下面是实例-
ALOAM
创建 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_)));
	}

添加到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求解即可。

2. ICP实例

给出一个基于手动jacobian的PP ICP，重点在于优化参数化子类以及Factor类的编写。

2.1 优化状态参数化

       设定POSE用SE3表示，即 四元数 + XYZ，而优化时使用se3，即$[\varphi ,\rho ]$，ICP优化的数学推导如下，首先优化的代价函数为：$C={\Vert \begin{array}{c}f(T)\end{array}\Vert }^2$ ，T为优化的对象-变换矩阵，但是T作为SE3同SO3一样是存在约束的，而se3是不存在约束的，所以在增量优化时我们应该求解一个se3而非T。
       增量优化的求解思路梳理如下，采用扰动法，先给T一个左乘扰动，扰动后的函数为：$$f(T\oplus \delta T)=f(\delta TT)=f(exp([\delta \xi {]}^{\wedge })T)$$ 其中$\delta \xi$为左扰动对应的se3李代数。优化的目标即找到一个合适的$\delta \xi$使得代价函数尽可能缩小，根据高斯牛顿优化，我们需要将一个扰动后的函数一阶泰勒展开为$f(X\oplus \delta x)=f(X)+J\delta x$的形式, 但是由于姿态矩阵T是流形，因此对于$f(T)$函数，若将其在T处展开，是不能得到我们要的形式的，如下：
$$f(T\oplus \delta \xi )=f(exp([\delta \xi {]}^{\wedge })T)=f(T)+\frac{\partial f}{\partial T}(\delta TT-T)\ne f(T)+\frac{\partial f}{\partial T}\delta T$$但是，若将$f$看作$\delta \xi$的函数，则可以很好的解决，如下：
$$\begin{gathered} f(exp([\delta \xi {]}^{\wedge })T)=F(\delta \xi )=F(0)+J\delta \xi \\ J=\frac{\partial F(\delta \xi )}{\partial \delta \xi }{|}_{\delta \xi =0} \end{gathered}$$ 通过迭代优化方法求解出增量$\delta \xi$即可，求解出se3增量后，接着需要用这个增量对我们的POSE进行更新，因为我们的POSE是SE3，所以首先将se3转换为SE3，即：
$$\begin{gathered} se3:\delta \xi =\left[\begin{array}{c}\delta \rho \\ \delta \varphi \end{array}\right] \\ SE3:exp([\delta \xi {]}_{\times })=\left[\begin{array}{cc}exp([\delta \varphi {]}_{\times })& J\rho \\ 0^T& 1\end{array}\right] \end{gathered}$$
其中$J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }$
由于我们的POSE中姿态用四元数表示，因此将so3-$\delta \varphi$转换为四元数，$\delta \varphi =u\delta \theta$,
$Q=exp(\frac{u\delta \theta }{2})=\left[\begin{array}{c}cos\frac{\theta }{2}\\ usin\frac{\theta }{2}\end{array}\right]$,当扰动很小时，近似为：$\left[\begin{array}{c}1\\ \frac{u\theta }{2}\end{array}\right]$
将se3转换为四元数以及XYZ后，接着就可以更新POSE了，记得我们构建优化问题时扰动是通过左乘的形式添加的，因此，我们也需要用左乘对POSE进行更新，即：
$$\begin{gathered} \delta TT=\left[\begin{array}{cc}\delta R& \delta t\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right] \\ =\left[\begin{array}{cc}\delta RR& \delta Rt+\delta t\\ 0^T& 1\end{array}\right] \end{gathered}$$
上述过程需要通过参数化ceres::LocalParameterization的子类传给ceres，代码如下，上述更新部分即类中的Plus()函数。

class SE3PoseLocalParameterization : public ceres::LocalParameterization {
public:
    SE3PoseLocalParameterization() {}
    virtual ~SE3PoseLocalParameterization() {}

    /
    /**
     * @brief: GOBAL：SE3， LOCAL ：se3 
     * @param x : SE3 四元数 + XYZ 
     * @param delta se3 
     * @param x_plus_delta  SE3 
     */    
    virtual bool Plus(const double *x,  const double *delta, double *x_plus_delta) const 
    {
        Eigen::Map<const Eigen::Vector3d> _p(x+4);
        Eigen::Map<const Eigen::Quaterniond> _q(x);   // 传入序列 (x, y, z, w)构造eigen 四元数   注意eigen 内部存储四元数的顺序是  (x, y, z, w) 
        Eigen::Quaterniond delta_q;  
        Eigen::Vector3d delta_t;  
        Eigen::Map<const Eigen::Vector3d> delta_se3_omega(delta);    // se3中旋转部分
        Eigen::Map<const Eigen::Vector3d> delta_se3_upsilon(delta+3);    // se3中t    

        Eigen::Vector3d unit_delta_se3_omega = delta_se3_omega.normalized(); 
        double theta = delta_se3_omega.norm();                            

        if(theta<1e-10)
        {
            delta_q = Eigen::Quaterniond(1, delta_se3_omega.x() / 2, 
                                            delta_se3_omega.y() / 2, 
                                            delta_se3_omega.z() / 2);
        }
        else
        {
            double sin_half_theta = sin(0.5*theta);
            delta_q = Eigen::Quaterniond(cos(0.5*theta), unit_delta_se3_omega.x()*sin_half_theta, 
                                          unit_delta_se3_omega.y()*sin_half_theta, 
                                          unit_delta_se3_omega.z()*sin_half_theta);
        }

        Eigen::Matrix3d J;

        if (theta<1e-10)
        {
            J = delta_q.matrix();   // 当theta很小时， 近似为罗德里格斯公式 
        }
        else
        {
            double c = sin(theta) / theta;
            J = Eigen::Matrix3d::Identity()*c + (1 - c)*unit_delta_se3_omega*unit_delta_se3_omega.transpose() + 
                    (1 - cos(theta))*Utility::getSkewMatrix(unit_delta_se3_omega) / theta ; 
        }

        delta_t = J*delta_se3_upsilon;  
        Eigen::Map<Eigen::Vector3d> p(x_plus_delta+4);
        Eigen::Map<Eigen::Quaterniond> q(x_plus_delta);
        q = (delta_q*_q).normalized();
        p = delta_q * _p + delta_t; 
        return true;
    }

    /
    virtual bool ComputeJacobian(const double *x, double *jacobian) const 
    {
        Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
        (j.topRows(6)).setIdentity();
        (j.bottomRows(1)).setZero();

        return true;
    }

    virtual int GlobalSize() const { return 7;};
    virtual int LocalSize() const { return 6; };
};

2.2 jacobian矩阵的求解

根据前面的推导，jacobian矩阵即求解：$$J=\frac{\partial F(\delta \xi )}{\partial \delta \xi }{|}_{\delta \xi =0}$$对于ICP来说，用函数$f(TP)$表示点P经过变换T后，与匹配点的距离，则：
$$\begin{gathered} F(\delta \xi )=f(exp([\delta \xi {]}^{\wedge })TP) \\ J=\frac{\partial f(exp([\delta \xi {]}^{\wedge })TP)}{\partial \delta \xi }{|}_{\delta \xi =0} \\ \underset{exp([\delta \xi {]}^{\wedge })TP=P^{\prime }}{⟹}\frac{\partial f(P^{\prime })}{\partial P^{\prime }}\frac{\partial exp([\delta \xi {]}^{\wedge })TP}{\partial \delta \xi } \\ =n\frac{\partial (I+[\delta \xi {]}^{\wedge })TP}{\partial \delta \xi } \\ =n\frac{\partial [\delta \xi {]}^{\wedge }TP}{\partial \delta \xi } \\ =n\frac{\partial \left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ 0& 0\end{array}\right]\left[\begin{array}{c}Rp+t\\ 1\end{array}\right]}{\partial \delta \xi } \\ =n\frac{\partial \left[\begin{array}{c}\delta {\varphi }^{\wedge }(Rp+t)+\delta \rho \\ 0\end{array}\right]}{\partial \left[\begin{array}{cc}\delta \varphi & \delta \rho \end{array}\right]} \\ =n\left[\begin{array}{cc}-(Rp+t{)}^{\wedge }& I\end{array}\right] \end{gathered}$$
其中n为匹配点距离向量，残差即为匹配点之间的距离。
创建一个ceres::SizedCostFunction的Factor子类，将这个jacobian与残差设置到该子类重写的Evaluate()函数中即可。

// 手动求解jacobian 
class PointPointFactor3D : public ceres::SizedCostFunction<1, 7>     // 1: 残差维度        7 ： global size   
{
    private:
        Eigen::Vector3d source_point_;
        Eigen::Vector3d target_point_;
    public:
        PointPointFactor3D(const Eigen::Vector3d &source_point, const Eigen::Vector3d &target_point) 
                                                    : source_point_(source_point),  target_point_(target_point)
        {
        }
        ~PointPointFactor3D()
        {
        }
        
        virtual bool Evaluate(double const * const * parameters, 
                            double *residuals, double **jacobians) const
        {
            Eigen::Map<const Eigen::Quaterniond> q_source_curr(parameters[0]);
            Eigen::Map<const Eigen::Vector3d> t_source_curr(parameters[0] + 4);

            Eigen::Vector3d target_point_after_trans = q_source_curr * target_point_ + t_source_curr;  
            Eigen::Vector3d n = ( target_point_after_trans - source_point_ ).transpose();
            double distance =  n.norm(); 
            if (distance ==0 )return false; 
            
            // 残差  
            residuals[0] = (source_point_ - target_point_after_trans).norm();   

            if( jacobians != nullptr )
            {   
                // 注意这里的jacobian 在维度上是  error 关于 李群的， 但是其实这是对旋转的李代数以及平移求导  
                if( jacobians[0] != nullptr )
                {
                    Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor>> J(jacobians[0]);  
                    J.setZero();  
                    J.block<1, 3>(0, 0) = (( target_point_after_trans - source_point_ ).transpose() 
                                                                    / distance) * (-Utility::getSkewMatrix(target_point_after_trans)) ;     
                    J.block<1, 3>(0, 3) = ( target_point_after_trans - source_point_ ).transpose() 
                                                                / distance;          
                }
            }
            return true;
        }

};

注：内容纯属自己的归纳总结，若发现问题，请留言指出 谢谢