PCL Trimmed ICP实现点云精配准(抄袭狗别再抄错版)

标签：错版配准 target viewer 狗别 ICP source pcl icp

一、算法原理

1、原理概述

对于源点云 P P P 中点的个数不等目标点云 Q Q Q点个数的两个点集，源点云中的一些点在目标点集中可能没有对应关系。假设可以配对的数据点的最小保证率为 σ \sigma σ ，那么,可以配对的数据点数量为 N p o N_{po} Npo= σ N \sigma N σN，当 σ = 1 \sigma=1 σ=1 时就是ICP。
(1)对于 P P P 的每个点，找出 Q Q Q中最近的点，并计算各个点对之间的距离 d i 2 d_i^2 di2 ;
(2) d i 2 d_i^2 di2 按升序排列，选择前 N p o N_{po} Npo个点对并计算它们的和 s u m ′ sum' sum′；
(3)满足约束条件（与ICP相同）则退出，否则令 s u m = s u m ′ sum=sum' sum=sum′ 并继续；
(4)为选定的 N p o N_{po} Npo 对对应点对计算最小化 s u m ′ sum' sum′ 的最佳变换 ( R , T ) (R,T) (R,T) ；
(5)进行变换并循环。
Trimmed ICP算法已经成功应用于初始相对旋转达30°的配准，算法总是相对于修剪的均方误差目标函数单调收敛到局部最小值。收敛到全局最小值取决于点的初始位置。为了避免局部极小值，通常在不同的条件下运行几次ICP。或者改变 σ \sigma σ 的值在不同的条件下运行Trimmed ICP选择最佳结果。

2、参考文献

[1] Chetverikov D , Svirko D , Stepanov D , et al. The Trimmed Iterative Closest Point Algorithm[C]// International Conference on Pattern Recognition. IEEE, 2002.
[2] 李鑫,莫思特,黄华,杨世基.自动计算重叠度的多源点云配准方法[J/OL].红外与激光工程:1-10[2021-05-16].

二、代码实现

抄袭狗记得来抄最新代码哦，还能抄错就不只是丢人的问题了。

#include <iostream>
#include <Eigen/Dense>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/recognition/ransac_based/trimmed_icp.h> // trimmed_icp头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>  // 利用控制台计算时间

using namespace std;

void  visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& target, pcl::PointCloud<pcl::PointXYZ>::Ptr& icp)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer(u8"配准结果"));
    int v1 = 0;
    int v2 = 1;
    viewer->createViewPort(0, 0, 0.5, 1, v1);
    viewer->createViewPort(0.5, 0, 1, 1, v2);
    viewer->setBackgroundColor(0, 0, 0, v1);
    viewer->setBackgroundColor(0.05, 0, 0, v2);
    viewer->addText("Raw point clouds", 10, 10, "v1_text", v1);
    viewer->addText("Registed point clouds", 10, 10, "v2_text", v2);
    //原始点云绿色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source, 255, 0, 0);
    //目标点云蓝色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 0, 0, 255);
    //转换后的源点云红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transe(icp, 255, 0, 0);
    //viewer->setBackgroundColor(255, 255, 255);
    viewer->addPointCloud(source, src_h, "source cloud", v1);
    viewer->addPointCloud(target, tgt_h, "target cloud", v1);
    viewer->addPointCloud(target, tgt_h, "target cloud1", v2);
    viewer->addPointCloud(icp, transe, "pcs cloud", v2);
    //添加坐标系
    //viewer->addCoordinateSystem(0.1);
    //viewer->initCameraParameters();
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(10000));
    }
}

int main(int argc, char* argv[])
{
    // -------------------------------加载源点云----------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *source) == -1)
    {
        PCL_ERROR("Couldn't read the PCD file!\n");
        return -1;
    }
    
    // -----------------------------加载目标点云----------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *target) == -1)
    {
        PCL_ERROR("Couldn't read the PCD file!\n");
        return -1;
    }

    int iterations = 35;
    pcl::console::TicToc time;
    time.tic();
    // ------------------------------ICP配准------------------------------------
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(source);               // 设置源点云
    icp.setInputTarget(target);               // 设置目标点云
    icp.setMaximumIterations(iterations);           // 设置最大迭代次数
    icp.setMaxCorrespondenceDistance(15);           // 设置最大对应点距离
    icp.setTransformationEpsilon(1e-10);            // 设置精度
    icp.setEuclideanFitnessEpsilon(0.01);           // 设置收敛条件
    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    icp.align(*icp_cloud);                          // 执行ICP配准
    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();  // 变换矩阵
    if (icp.hasConverged())
    {
        std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
        transformation_matrix = icp.getFinalTransformation().cast<double>();  // 获取变换矩阵
    }
    else
    {
        PCL_ERROR("\nICP has not converged.\n");
        exit(-1);
    }
   
    // ----------------------Trimmed-ICP精细化处理----------------------------
    pcl::recognition::TrimmedICP<pcl::PointXYZ, double> Tricp;
    Tricp.init(target);                         // 初始化目标点云
    float sigma = 0.96;                         // 设置内点比例
    int Np = source->size();                    // 源点云点数量
    int Npo = Np * sigma;                       // 参与配准的点对数量
    Tricp.setNewToOldEnergyRatio(sigma);        // 设置内点能量比例
    Tricp.align(*icp_cloud, Npo, transformation_matrix);  // 执行Trimmed ICP配准
    // --------------------------结果可视化----------------------------------
    visualize_registration(source, target, icp_cloud);

    return 0;
}

三、结果展示

在这里插入图片描述

标签：错版,配准,target,viewer,狗别,ICP,source,pcl,icp
From： https://blog.csdn.net/qq_36686437/article/details/143103985