PCL學習記錄-12-Registration點雲配準-1 (ICP,NDT方法)

本博文主要介紹關于點雲配準的ICP和NDT兩種方法的實作代碼,并貼出具有參考意義的博文位址.

關于點雲配準的較好的參考部落格 :

http://robot.czxy.com/docs/pcl/chapter03/registration/#ndt

https://blog.csdn.net/sru_alo/article/details/88285135

https://blog.csdn.net/yuxuan20062007/article/details/80914102

一. ICP 點雲配準方法-疊代最近點算法(ICP)

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>  //!!Mar26: ICP method head file

/*Demo for Point cloud registration-ICP method
*       ICP: Iterative Closest Point
*       Step By Step: 1. Establish two saperate point cloud data with off-set
*                     2. Using ICP method to find the transformatio matrix between the two Point cloud data
*/
using namespace std;



int main(int argc, char **argv){
  
  // Step1: Establish two point cloud data with off-set
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
  


  // Step2: Assign random points into the point cloud data and show coordinate
  cloud_in ->width =5;
  cloud_in ->height =1;
  cloud_in ->is_dense =false;
  cloud_in ->points.resize(cloud_in ->width * cloud_in ->height);

  for(size_t i=0; i< cloud_in ->size(); ++i){
      cloud_in ->points[i].x = 1024 * rand() /(RAND_MAX +1.0f);
      cloud_in ->points[i].y = 1024 * rand() /(RAND_MAX +1.0f);
      cloud_in ->points[i].z = 1024 * rand() /(RAND_MAX +1.0f);
  }
  
  cout << "Saved " << cloud_in ->points.size() << "data points to input: " <<endl;
  for(size_t i=0; i< cloud_in ->size(); ++i){
      std::cout << "    " <<
                  cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
                  cloud_in->points[i].z << std::endl;
  }


   // Step3 : Copy the point_in data into point_out data
    *cloud_out = *cloud_in;
    std::cout << "size:" << cloud_out->points.size() << std::endl;


   // Step4 : Apply off-set to the point cloud data of 0.7 meter
    for (size_t i = 0; i < cloud_in->points.size(); ++i){
        cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
        std::cout << "Transformed " << cloud_in->points.size() << " data points:" << std::endl;
    }

    for (size_t i = 0; i < cloud_out->points.size(); ++i){
        std::cout << "    " << cloud_out->points[i].x << " " <<
                  cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
    }

   // Step5 : Improtant!! Using ISP to compare these two point cloud
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; //ISP method<PointFormat1, PointFormat2>
    icp.setInputSource(cloud_in);
    icp.setInputTarget(cloud_out);

    pcl::PointCloud<pcl::PointXYZ> Final_output;                    //注意,這個是點雲對象而不是指針
    icp.align(Final_output);

    std::cout << "has converged:" << icp.hasConverged()             //注意!! 若比對正常,hasCpnverged輸出值為1(true)
              << " score: "  << icp.getFitnessScore() 
    << std::endl;


    const pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 &matrix = icp.getFinalTransformation(); 
                                                                    //注意!! 擷取兩個點雲的轉換關系(4x4 的矩陣)
    std::cout << matrix << std::endl;
    return (0);

}

ICP 點雲配準方法-效果如下:

PCL學習記錄-12-Registration點雲配準-1 (ICP,NDT方法)

二. NDT 點雲配準方法-正态分布變換配準(NDT)

#include <iostream>
#include <thread>

#include <pcl/io/pcd_io.h>                        //輸入輸出及點雲類型頭檔案
#include <pcl/point_types.h>


#include <pcl/registration/ndt.h>                 //NDT配準頭檔案
#include <pcl/filters/approximate_voxel_grid.h>   //體素網格降采樣頭檔案

#include <pcl/visualization/pcl_visualizer.h>     //PCL可視化頭檔案

/*點雲配準: NDT正态分布配準方法
*   功能:使用正态分布變幻配準方法, 比對兩個點雲,并且将點雲轉化到正确的位置
*   步驟: 讀取及存儲點雲-> 将輸入點雲用體素網格方式進行降采樣 -> NDT配準 -> 将點雲轉化到正确位置
*    
*/
using namespace std;
using namespace std::chrono_literals;



int main(int argc, char **argv){
    /*---------------------------------------------------------------
       Step1: 建立點雲對象, 并且讀取點雲檔案
    *----------------------------------------------------------------*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if(pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan1.pcd", *target_cloud) == -1){
        PCL_ERROR("Can not find the file room_scan1.pcd");
        return (-1);
    }
    std::cout << "Loaded :" << target_cloud ->size() << " from the room_scan1.pcd file" <<std::endl;


    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if(pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan2.pcd", *input_cloud) == -1){
        PCL_ERROR("Can not find the file room_scan2.pcd");
        return (-1);
    }
    std::cout << "Loaded :" << input_cloud ->size() << " from the room_scan2.pcd file" <<std::endl;

    /*---------------------------------------------------------------
        Step2: 使用體素網格對輸入點雲進行将采樣(目的:提升配準效率)
    *----------------------------------------------------------------*/

    pcl::PointCloud<pcl::PointXYZ>::Ptr  voxel_filtered_input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> voxel_filter;

    voxel_filter.setLeafSize(0.2,0.2,0.2);
    voxel_filter.setInputCloud(input_cloud);            //注意:此處需要輸入的是指針對象
    voxel_filter.filter(*voxel_filtered_input_cloud);   //注意:此處需要輸出的是真實對象

    std::cout << "The size of the filtered point cloud is : " << voxel_filtered_input_cloud ->size() << std::endl;



    /*---------------------------------------------------------------
        Step3: 重點! 使用正态分布變換配準方法(NDT),進行點雲配準!!
    *----------------------------------------------------------------*/

    pcl::NormalDistributionsTransform <pcl::PointXYZ,pcl::PointXYZ> ndt;
    ndt.setTransformationEpsilon(0.01);                 // 設定NDT方法,決定了變換向量[x, y, z, roll, pitch, yaw]最小允許的增量（以米為機關）。
                                                        // 當增量變化低于這個門檻值，配準過程結束。

    /*Step Size parameter 規定了MT線性搜尋（More-Thuente line search）方法允許的最大步長。
    算法會采用在規定值之下最适合的步長，在接近優化目标時減小步長。
    過大的最大值能減少疊代次數，但也面臨着 overshooting 和在不希望的局部最小值停止的風險。*/
    ndt.setStepSize(0.1);

    /*Resolution parameter規定NDT算法資料結構體素的大小。每個體素資料包括點雲統計資料，平均值，協方差等。
    這些統計資料用來建構多元高斯分布并且用來計算和優化點在體素區域的機率密度分布。
    這個參數和點雲規模息息相關。每個體素要至少填充6個點，但又得小一些，滿足能特異描述環境的功能。*/
    ndt.setResolution(1.0);

    /*最後一個參數規定最大疊代次數，在絕大多數情況，疊代應該因為Transformation Epsilon終止.
    而并非達到疊代的最大值。這個參數防止程式運作時間太長。*/
    ndt.setMaximumIterations(35);

    ndt.setInputCloud(voxel_filtered_input_cloud);
    ndt.setInputTarget(target_cloud);

    /*---------------------------------------------------------------
        Step4: 讀取經過NDT方法尋找到的兩個點雲的左邊轉換關系
    ----------------------------------------------------------------*/
    Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());        //設定假設的rotation matrix
    Eigen::Translation3f init_translation (1.79387, 0.720047, 0);               //設定假設的translation vector
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();  //将上邊的旋轉矩陣+translation vector 轉化為4 x 4的轉換矩陣


    /*---------------------------------------------------------------
        Step5: 我們接下定義點雲之間粗比對的變換矩陣。雖然算法可以不需要這個參數，但它可以讓我們得到更好的結果，在點雲差距很大時效果更加明顯。
    ----------------------------------------------------------------*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    ndt.align (*output_cloud, init_guess);

    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
                << " score: " << ndt.getFitnessScore () << std::endl;



    /*---------------------------------------------------------------
        Step6: 使用NDT方法提取到的轉換矩陣, 将未經體素網格降采樣的點雲,調整到正确的位置,并儲存
    ----------------------------------------------------------------*/
    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());
    pcl::io::savePCDFileASCII("Transformed_pointcloud.pcd", *output_cloud);
    

    /*---------------------------------------------------------------
        Step7: 可視化顯示出來
    ----------------------------------------------------------------*/
    //設定視窗
    pcl::visualization::PCLVisualizer::Ptr view_final (new pcl::visualization::PCLVisualizer("3D Viewer"));
    view_final -> setBackgroundColor(0,0,0);

    //設定目标點雲顔色,設定目标點雲的大小,并加載到可視化視窗中
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>  target_color(target_cloud, 255, 0, 0);
    view_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
    view_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");

    //設定輸出點雲顔色,設定輸出點雲的大小,并加載到可視化視窗中
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>  output_color(target_cloud, 0, 255, 0);
    view_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
    view_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");



    // 開始可視化視窗
    view_final->addCoordinateSystem(1.0, "global");
    view_final->initCameraParameters();

    // 循環,直至視窗被關閉
    while (!view_final->wasStopped()) {
        view_final->spinOnce(100);
        std::this_thread::sleep_for(100ms);
    }

    return (0);

}

NDT 點雲配準方法-效果如下:

PCL學習記錄-12-Registration點雲配準-1 (ICP,NDT方法)

繼續閱讀

PCL - MLS代碼研讀（一）- MLS測試前言測試曲面重建測試SAMPLE_LOCAL_PLANE上採樣方法測試VOXEL_GRID_DILATION上採樣方法主程式MLS module結構

PCL - MLS代碼研讀（八）- MovingLeastSquares

PCL - MLS代碼研讀（十二）- addProjectedPointNormal及copyMissingFields函數

PCL - ICP代碼研讀（六） - IterativeClosestPoint架構

PCL - ICP代碼研讀（十三） - CorrespondenceEstimationBase實現

PCL點雲庫——PCA粗配準

《4-Points Congruent Sets for Robust Pairwise Surface Registration》論文研讀

PCL點雲學習（1）——pcd檔案的生成和讀取

PCL1.6.0學習筆記（一）

ROS 中bag 包

66.[PCL]PCD檔案格式簡介PCD版本

Matlab讀取.pcd格式HDmap環境

velodyne 擷取32線 csv 轉換成 pcd 檔案32線 csv 轉換成 pcd 檔案 C++

PCL 讀取PCD格式并可視化

ROS+opencv實踐-二維碼識别

PCL環境配置--win10+VS2019+PCL1.11.1