使用g2o进行点云ICP求解

继上文ceres实现点云求解ICP
使用G2O进行ICP求解的代码如下:

#include <Eigen/Core>
#include <Eigen/src/Core/Matrix.h>
#include <iostream>

#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include <g2o/core/optimization_algorithm_factory.h>
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/core/base_binary_edge.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/io.h>
#include <pcl/io/ply_io.h>
#include <sophus/se3.hpp>
using namespace std;

G2O_USE_OPTIMIZATION_LIBRARY(pcg)
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
G2O_USE_OPTIMIZATION_LIBRARY(csparse)

using Vector6d = Eigen::Vector<double,6>;
class VertexParams : public g2o::BaseVertex<6,Vector6d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    VertexParams()= default;

    bool read(std::istream & /*is*/) override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    bool write(std::ostream & /*os*/) const override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    void setToOriginImpl() override {
        _estimate = Vector6d(1,1,1,1,1,1);
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    //更新优化之后的顶点
    void oplusImpl(const double *update) override {
    //这里其实可以使用Sophus::SE3d，但是对应的顶点和边需要改为G2O提供的SE3对应类型，本文为了和上文ceres的操作相对应，采用Vector6d类存储旋转和平移。
    //相比于使用SE3d，Vector6d的更新需要分开，对于平移部分，直接相加即可。但是对于旋转部分需要使用矩阵乘法。
        Vector6d dx(0,0,0,0,0,0);
        Eigen::Vector3d dx_rot(update[0],update[1],update[2]);
        Eigen::Vector3d dx_trans(update[3],update[4],update[5]);

        Eigen::Vector3d pre_rot(_estimate(0),_estimate(1),_estimate(2));
        Eigen::Vector3d pre_treans(_estimate(3),_estimate(4),_estimate(5));

        Eigen::AngleAxisd dx_rollAngle(Eigen::AngleAxisd(dx_rot(0),Eigen::Vector3d::UnitX()));
        Eigen::AngleAxisd dx_pitchAngle(Eigen::AngleAxisd(dx_rot(1),Eigen::Vector3d::UnitY()));
        Eigen::AngleAxisd dx_yawAngle(Eigen::AngleAxisd(dx_rot(2),Eigen::Vector3d::UnitZ()));

        Eigen::AngleAxisd pre_rollAngle(Eigen::AngleAxisd(pre_rot(0),Eigen::Vector3d::UnitX()));
        Eigen::AngleAxisd pre_pitchAngle(Eigen::AngleAxisd(pre_rot(1),Eigen::Vector3d::UnitY()));
        Eigen::AngleAxisd pre_yawAngle(Eigen::AngleAxisd(pre_rot(2),Eigen::Vector3d::UnitZ()));

        Eigen::Matrix3d dx_rotation_matrix;
        Eigen::Matrix3d pre_rotation_matrix;
        dx_rotation_matrix=dx_rollAngle*dx_pitchAngle*dx_yawAngle;  //欧拉角转旋转矩阵
        pre_rotation_matrix=pre_rollAngle*pre_pitchAngle*pre_yawAngle;
        Eigen::Matrix3d after_rot = pre_rotation_matrix*dx_rotation_matrix;
        Eigen::Vector3d after_eulerAngle=after_rot.eulerAngles(0,1,2);
        dx(0) = after_eulerAngle(0);
        dx(1) = after_eulerAngle(1);
        dx(2) = after_eulerAngle(2);
        dx(3) = pre_treans(0) + dx_trans(0);
        dx(4) = pre_treans(1) + dx_trans(1);
        dx(5) = pre_treans(2) + dx_trans(2);
        _estimate = dx;
        //std::cout<<_estimate;
    }
};


class EdgePointOnCurve : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexParams> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Eigen::Vector3d m_pos;
    EdgePointOnCurve(Eigen::Vector3d &pos):m_pos(pos){}

    bool read(std::istream & /*is*/) override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    bool write(std::ostream & /*os*/) const override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    //边的误差计算
    void computeError() override {

        const VertexParams *T = dynamic_cast<const VertexParams *>(vertex(0));//顶点
        Eigen::Vector3d rot(T->estimate()(0),T->estimate()(1),T->estimate()(2));
        Eigen::Vector3d trans(T->estimate()(3),T->estimate()(4),T->estimate()(5));
        //计算变换后的点
        Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(rot(0),Eigen::Vector3d::UnitX()));
        Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(rot(1),Eigen::Vector3d::UnitY()));
        Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(rot(2),Eigen::Vector3d::UnitZ()));
        Eigen::Matrix3d rotation_matrix;
        rotation_matrix=yawAngle*pitchAngle*rollAngle;  //欧拉角转旋转矩阵
        Eigen::Vector3d target_pos = rotation_matrix*m_pos + trans;
        _error(0) = std::abs(target_pos(0) - _measurement(0));
        _error(1) = std::abs(target_pos(1) - _measurement(1));
        _error(2) = std::abs(target_pos(2) - _measurement(2));
    }
};

int main(int argc, char **argv) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPLYFile("../../clouds/face.ply",*cloud_source);
    pcl::io::loadPLYFile("../../clouds/new_face.ply",*cloud_target);
    if(!cloud_source || !cloud_target){
        std::cout<<"error cloud"<<std::endl;
        return 0;
    }
    g2o::SparseOptimizer optimizer;
    // 优化器类型
    string solver_type = "gn_pcg";
    // 优化器生成器
    g2o::OptimizationAlgorithmFactory *solver_factory = g2o::OptimizationAlgorithmFactory::instance();
    // 存储优化器性质
    g2o::OptimizationAlgorithmProperty solver_property;
    // 生成优化器
    solver_factory->listSolvers(std::cout);
    g2o::OptimizationAlgorithm *solver = solver_factory->construct(solver_type, solver_property);
    optimizer.setAlgorithm(solver);
    if (!optimizer.solver()) {
       std::cout << "G2O 优化器创建失败！" << std::endl;
    }
    VertexParams *vertex_pose = new VertexParams(); // camera vertex_pose
    vertex_pose->setId(0);//定义节点编号
    vertex_pose->setEstimate(Vector6d(1,1,1,1,1,1));//定义节点初始值
    optimizer.addVertex(vertex_pose);
    for(int i = 0; i<10;i++){
        auto &p1 = cloud_source->points[i];
        auto &p2 = cloud_target->points[i];
        Eigen::Vector3d source_p(p1.x,p1.y,p1.z);
        Eigen::Vector3d target_p(p2.x,p2.y,p2.z);
        EdgePointOnCurve *e = new EdgePointOnCurve(source_p);
        e->setId(i); 
        e->setMeasurement(target_p);
        e->setVertex(0, vertex_pose);
        e->setInformation(Eigen::Matrix3d::Identity());       //信息矩阵
        optimizer.addEdge(e);
    }
    optimizer.initializeOptimization();
    optimizer.computeInitialGuess();
    optimizer.computeActiveErrors();
    optimizer.setVerbose(true);
    optimizer.optimize(100);
    //std::cout<<vertex_pose->estimate();
    Eigen::Vector3d rot(vertex_pose->estimate()(0),vertex_pose->estimate()(1),vertex_pose->estimate()(2));
    Eigen::Vector3d trans(vertex_pose->estimate()(3),vertex_pose->estimate()(4),vertex_pose->estimate()(5));
    //计算变换后的点
    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(rot(0),Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(rot(1),Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(rot(2),Eigen::Vector3d::UnitZ()));
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix=yawAngle*pitchAngle*rollAngle;  //欧拉角转旋转矩阵
    std::cout << "Rotation:"<<std::endl;
    std::cout<<rotation_matrix<<std::endl;
    std::cout << "Translation:"<<std::endl;
    std::cout<<trans(0)<<" "<<trans(1)<<" "<<trans(2)<<" "<<std::endl;
    return 0;
}