SLAM学习笔记（三）——基于Ceres库实现对李代数的非线性最小二乘优化求解

Ceres库，Google开发的用于最小二乘求解最优解的通用非线性优化库，作为SLAM非常重要的工具是绕不开的。本文不再赘叙相关的原理以及具体每个函数的使用方法，旨在通过相关案例能够让读者快速地使用Ceres解决相应的问题。

本文求解的问题与前面的两篇文章解决的问题一样，即给定匹配点坐标的情况下如何使用Ceres求解出最优的变换矩阵。本文中对变换矩阵求解时用李代数进行的表示，通过自定义雅可比矩阵，同时自定义了更新变量的规则。

首先我们继承LocalParameterization类自定义新的变量更新方式

class SE3update:public ceres::LocalParameterization{
    public:
    virtual ~SE3update(){}
    virtual bool Plus(const double* x,
        const double* delta,
        double* x_plus_delta) const {
        Eigen::Map<const Eigen::Matrix<double,6,1>> se3_x(x);
        Sophus::SE3d T_x=Sophus::SE3d::exp(se3_x);
        Eigen::Map<const Eigen::Matrix<double,6,1>> se3_delta(delta);
        Sophus::SE3d T_delta=Sophus::SE3d::exp(se3_delta);
        Sophus::SE3d T_x_plus_delta=T_delta*T_x;
        Eigen::Matrix<double,6,1> se3_x_plus_delta=T_x_plus_delta.log();
        for(int i=0;i<6;i++){
            x_plus_delta[i]=se3_x_plus_delta[i];
        }
        return true;
    }

    virtual bool ComputeJacobian(const double* x,
        double* jacobian) const override {
        Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> jacobian_matrix(jacobian);
        jacobian_matrix.setIdentity();

        return true;
    }

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

Plus函数就是如何将求出的加到变量x上完成更新。这里都是先把他们转成变换矩阵再进行相乘，再转回李代数完成更新。由于LocalParameterization中除了MultiplyByJacobian，其它函数全是虚函数，因为任何从纯虚基类派生的类都必须实现所有的纯虚函数，除非该派生类也被声明为纯虚基类，否则无法实例化，所以其它函数都需要在子类中实现。注意ComputeJacobian中的雅可比矩阵，参考文章Ceres中LocalParameterization参数Jacobian矩阵计算过程，我们这里将它赋值为单位矩阵。

然后我们定义损失函数

class ICPcostFunction: public ceres::SizedCostFunction<3,6>{
    public:
    ICPcostFunction(Eigen::Vector3d S_p_,Eigen::Vector3d T_p_):S_p(S_p_),T_p(T_p_){}
    virtual bool Evaluate(double const *const *parameters, double *residuals, 
        double **jacobians) const {

        Eigen::Map<const Eigen::Matrix<double,6,1>> se3(*parameters);
        Sophus::SE3d T=Sophus::SE3d::exp(se3);

        Eigen::Vector3d S_p_trans=T*S_p;

        residuals[0]=S_p_trans(0)-T_p(0);
        residuals[1]=S_p_trans(1)-T_p(1);
        residuals[2]=S_p_trans(2)-T_p(2);

        if (jacobians && jacobians[0]) {
            Eigen::Map<Eigen::Matrix<double, 3, 6, Eigen::RowMajor>> jacobian(jacobians[0]);

            jacobian(0,0)=1;
            jacobian(0,1)=0;
            jacobian(0,2)=0;
            jacobian(0,3)=0;
            jacobian(0,4)=S_p_trans(2);
            jacobian(0,5)=-1*S_p_trans(1);

            jacobian(1,0)=0;
            jacobian(1,1)=1;
            jacobian(1,2)=0;
            jacobian(1,3)=-1*S_p_trans(2);
            jacobian(1,4)=0;
            jacobian(1,5)=S_p_trans(0);

            jacobian(2,0)=0;
            jacobian(2,1)=0;
            jacobian(2,2)=1;
            jacobian(2,3)=S_p_trans(1);
            jacobian(2,4)=-1*S_p_trans(0);
            jacobian(2,5)=0;
        }
        
        return true;
    }
    private:
    Eigen::Vector3d S_p;
    Eigen::Vector3d T_p;
};

 因为我们要用自定义的雅可比矩阵，所以这里继承SizedCostFunction类，具体的写法可以参考【SLAM】Ceres优化库超详细解析，这里不再赘叙，这一块比较简单

然后就可以在主函数中进行非线性优化求解了，代码如下

int main(int argc, char** argv){
    pcl::PointCloud<pcl::PointXYZ>::Ptr S(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr T(new pcl::PointCloud<pcl::PointXYZ>);
    std::string SnameS = argv[1];
    std::string SnameT = argv[2];
    pcl::io::loadPCDFile(SnameS, *S);
    pcl::io::loadPCDFile(SnameT, *T);

    float align_error=0;
    for(int i=0;i<S->size();i++)
    {
        float dis=pcl::geometry::distance(S->points[i],T->points[i]);
        align_error+=dis;
    }
    std::cout<<"before optimize: "<<align_error<<std::endl;

    Eigen::Matrix3d R=Eigen::Matrix3d::Identity();
    Eigen::Vector3d t(0,0,0);
    Sophus::SE3d T_ini(R,t);
    Eigen::Matrix<double,6,1> se3_ini=T_ini.log();
    Sophus::SE3d TT=Sophus::SE3d::exp(se3_ini);

    ceres::Problem problem;
    ceres::LocalParameterization* local_param=new SE3update();
    problem.AddParameterBlock(se3_ini.data(),6,local_param);
    for(int i=0;i<S->size();i++){
        ceres::CostFunction *costfunction;
        Eigen::Vector3d s_p(S->points[i].x,S->points[i].y,S->points[i].z);
        Eigen::Vector3d t_p(T->points[i].x,T->points[i].y,T->points[i].z);
        costfunction=new ICPcostFunction(s_p,t_p);
        problem.AddResidualBlock(costfunction,nullptr,se3_ini.data());
    }

    ceres::Solver::Options options;
    options.linear_solver_type=ceres::DENSE_NORMAL_CHOLESKY;
    // options.minimizer_progress_to_stdout=true;
    options.max_num_iterations=20;

    ceres::Solver::Summary summary;
    ceres::Solve(options,&problem,&summary);

    // cout<<summary.BriefReport()<<endl;
    Sophus::SE3d T_result=Sophus::SE3d::exp(se3_ini);
    // cout<<"T:"<<endl<<T_result.matrix()<<endl;

    pcl::transformPointCloud(*S,*S,T_result.matrix());
    align_error=0;
    for(int i=0;i<S->size();i++)
    {
        float dis=pcl::geometry::distance(S->points[i],T->points[i]);
        align_error+=dis;
    }
    std::cout<<"after optimize: "<<align_error<<std::endl;
}

运行结果如下

before optimize: 152.927
after optimize: 3.71689

当然也可以添加核函数提高算法的鲁棒性，这里以Huber核函数为例，参照Ceres中的源码，我这里重写了一下该类

class Huber:public ceres::LossFunction{
    public:
    Huber(double a):a_(a),b_(a * a){}
    virtual ~Huber(){}
    virtual void Evaluate(double sq_norm, double out[3]) const{
        if (sq_norm > b_) {
            // Outlier region.
            // 'r' is always positive.
            const double r = sqrt(sq_norm);
            out[0] = 2.0 * a_ * r - b_;
            out[1] = std::max(std::numeric_limits<double>::min(), a_ / r);
            out[2] = -out[1] / (2.0 * sq_norm);
        } else {
            // Inlier region.
            out[0] = sq_norm;
            out[1] = 1.0;
            out[2] = 0.0;
        }
    }
    private:
    double a_,b_;
};

sq_norm是残差的二范数，out[3]则是核函数对残差的相应导数，如out[0]则是0阶导数，out[1]是一阶导数，out[2]是二阶导数。

更改一下main函数

ceres::Problem problem;
ceres::LocalParameterization* local_param=new SE3update();
problem.AddParameterBlock(se3_ini.data(),6,local_param);
ceres::LossFunction* huber_loss=new Huber(0.1);
for(int i=0;i<S->size();i++){
     ceres::CostFunction *costfunction;
     Eigen::Vector3d s_p(S->points[i].x,S->points[i].y,S->points[i].z);
     Eigen::Vector3d t_p(T->points[i].x,T->points[i].y,T->points[i].z);
     costfunction=new ICPcostFunction(s_p,t_p);
     problem.AddResidualBlock(costfunction,huber_loss,se3_ini.data());
}

运行结果

before optimize: 152.927
after optimize: 3.49

确实在相同的迭代次数下精度更高了