利用同名点求不同坐标系下的转换矩阵并进行评估

//实现汽车扫描的算法
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/registration/transformation_estimation_lm.h>
#include<pcl/registration/warp_point_rigid_3d.h>
#include<pcl/registration/transformation_estimation_dual_quaternion.h>
#include<pcl/registration/transformation_estimation_svd.h>
#include <pcl\registration\icp.h>
#include <pcl\registration\gicp.h>

#include<pcl/registration/transformation_estimation_svd.h>
using namespace pcl::registration;

#include <pcl/common/distances.h>

int main()
{
    
    
    // 创建两个点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt(new pcl::PointCloud<pcl::PointXYZ>);
    cloud_src->width = 5;
    cloud_src->height = 1;
    cloud_src->points.resize(cloud_src->width * cloud_src->height);
    for (size_t i = 0; i < cloud_src->points.size(); ++i)
    {
    
    
        cloud_src->points[i].x = static_cast<float>(1024 * rand() / (RAND_MAX + 1.0));
        cloud_src->points[i].y = static_cast<float>(1024 * rand() / (RAND_MAX + 1.0));
        cloud_src->points[i].z = static_cast<float>(1024 * rand() / (RAND_MAX + 1.0));
        std::cout << cloud_src->points[i].x << "," << cloud_src->points[i].y << "," << cloud_src->points[i].z << std:: endl;
    }
    std::cout << "=====================" << std::endl;
    cloud_tgt->width = 5;
    cloud_tgt->height = 1;
    cloud_tgt->points.resize(cloud_tgt->width * cloud_tgt->height);

    Eigen::Affine3d transform = Eigen::Affine3d::Identity();
    transform.translate(Eigen::Vector3d(11, 23, 0)); // 平移
    transform.rotate(Eigen::AngleAxisd(8 * M_PI / 180, Eigen::Vector3d::UnitX())); // 绕x轴旋转
    transform.rotate(Eigen::AngleAxisd(2 * M_PI / 180, Eigen::Vector3d::UnitY())); // 绕y轴旋转
    transform.rotate(Eigen::AngleAxisd(0 * M_PI / 180, Eigen::Vector3d::UnitZ())); // 绕z轴旋转
    transform.scale(Eigen::Vector3d(1, 1, 1)); // 缩放
    transform.matrix();
    pcl::transformPointCloud(*cloud_src, *cloud_tgt, transform.matrix());

    for (size_t i = 0; i < cloud_tgt->points.size(); ++i)
    {
    
    
        cloud_tgt->points[i].x = cloud_tgt->points[i].x + (float)rand() / RAND_MAX;
        cloud_tgt->points[i].y = cloud_tgt->points[i].y + (float)rand() / RAND_MAX;
        cloud_tgt->points[i].z = cloud_tgt->points[i].z + (float)rand() / RAND_MAX;
        std::cout << cloud_tgt->points[i].x << "," << cloud_tgt->points[i].y << "," << cloud_tgt->points[i].z << std::endl;
    }
    std::cout << "===============================" << std::endl;

#if 1
#pragma region 使用icp
   //定义ICP对象
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    // 设置ICP参数
    icp.setInputSource(cloud_src);
    icp.setInputTarget(cloud_tgt);
    icp.setMaximumIterations(1000000);
    //icp.setMaxCorrespondenceDistance(0.2);
    /*icp.setMaximumIterations(1000000);
    icp.setTransformationEpsilon(1e-6);
    icp.setMaxCorrespondenceDistance(1e-3);*/
    
    // 计算变换矩阵
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    icp.align(*transformed_cloud);
    std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << "最终的迭代次数" << icp.getMaximumIterations() << std::endl;
    // 输出变换矩阵
    std::cout << "Transformation matrix:" << std::endl << icp.getFinalTransformation() << std::endl;
#pragma endregion
#endif

#if 0
    // 定义TransformationEstimationSVD对象
    TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;
    // 求解转换矩阵
    Eigen::Matrix4f transformationMatrix;
    svd.estimateRigidTransformation(*cloud_src, *cloud_tgt, transformationMatrix);
    // 输出转换矩阵
    std::cout << "Transformation Matrix: " << std::endl << transformationMatrix << std::endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*cloud_src, *transformed_cloud, transformationMatrix);
#endif
    double error{
    
     0.0 };
    for (size_t i = 0; i < transformed_cloud->points.size(); ++i)
    {
    
    
        std::cout << transformed_cloud->points[i].x << "," << transformed_cloud->points[i].y << "," << transformed_cloud->points[i].z << std::endl;
        error+=pcl::squaredEuclideanDistance(transformed_cloud->points[i],cloud_tgt->points[i]);
    }
    std::cout << "最终svd的误差和是：" << error << std::endl;

    return 0;
}