ceres实现点云点到点的ICP求解位姿变换

最新推荐文章于 2024-07-17 20:43:45 发布

成长的小树

最新推荐文章于 2024-07-17 20:43:45 发布

阅读量555

点赞数 13

文章标签： c++

本文链接：https://blog.csdn.net/qq_22230823/article/details/135495354

版权

本文介绍了如何使用Ceres库中的CeresSolver对两个3D点云进行姿态估计，通过计算残差并求解优化问题，得到旋转和平移参数，实现点云之间的精确配准。

摘要由CSDN通过智能技术生成

#include "ceres/ceres.h"
#include "pcl/impl/point_types.hpp"
#include <Eigen/src/Core/Matrix.h>
#include <ceres/internal/eigen.h>
#include <ceres/jet.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 <ceres/rotation.h>
#include <Eigen/Dense>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/search.h>
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

class CostFunctor {
public:
    CostFunctor(pcl::PointXYZ source,pcl::PointXYZ target){
        m_source_pt = source;
        m_target_pt = target;
    }
    template <typename T>
    bool operator()(const T *const transform,T *residual)const {
        T p[3];
        T point[3];
        point[0] = T(m_source_pt.x);
        point[1] = T(m_source_pt.y);
        point[2] = T(m_source_pt.z);
        ceres::AngleAxisRotatePoint(transform, point, p);
        p[0] += transform[3];
        p[1] += transform[4];
        p[2] += transform[5];
        //计算误差 e=p-（Rp‘+t）
        residual[0] = T(m_target_pt.x) - p[0];
        residual[1] = T(m_target_pt.y) - p[1];
        residual[2] = T(m_target_pt.z) - p[2];
        return true;
    }
private:
    pcl::PointXYZ m_source_pt;
    pcl::PointXYZ m_target_pt;;
};

int main() 
{
    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;
    }
    double transform[6] = {0, 1, 2, 0, 0, 0};
    Problem problem;
    //残差维度为两个点坐标相减，3维；输入维度为旋转3维度(欧拉角)，平移3维度
    for(int i = 0;i<cloud_source->size();i++){
        CostFunction* cost_function =new AutoDiffCostFunction<CostFunctor, 3, 6>(
            new CostFunctor(cloud_source->points[i],cloud_target->points[i]));
            problem.AddResidualBlock(cost_function, nullptr, transform);
    }   
    Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << "\n";
    Eigen::Vector3d rot(transform[0],transform[1],transform[2]);
    
    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<<transform[3]<<" "<<transform[4]<<" "<<transform[5]<<" "<<std::endl;
}