pcl 點雲配準 Registration 正態分佈變換NDT配準 迭代最近點ICP配準 非線性ICP 配準

點雲配準 Registration

博文末尾支持二維碼讚賞哦 _

在逆向工程，計算機視覺，文物數字化等領域中，由於點雲的不完整，旋轉錯位，平移錯位等，
使得要得到的完整的點雲就需要對局部點雲進行配準，爲了得到被測物體的完整數據模型，
需要確定一個合適的座標系，將從各個視角得到的點集合併到統一的座標系下形成一個完整的點雲，
然後就可以方便進行可視化的操作，這就是點雲數據的配準。

實質就是把不同的座標系中測得到的數據點雲進行座標系的變換，以得到整體的數據模型，
問題的關鍵是如何讓得到座標變換的參數R（旋轉矩陣）和T（平移向量），
使得兩視角下測得的三維數據經座標變換後的距離最小，

目前配准算法按照過程可以分爲整體配準和局部配準。
PCL中有單獨的配準模塊，實現了配準相關的基礎數據結構，和經典的配准算法如ICP。 

給定兩個來自不同座標系的三維數據點集，找到兩個點集空間的變換關係，
使得兩個點集能統一到同一座標系統中，即配準過程

求得旋轉和平移矩陣
P2 = R*P1  + T　　　　[R t]

點雲配準的概念也可以類比於二維圖像中的配準，

只不過二維圖像配準獲取得到的是x，y，alpha，beta等放射變化參數

三維點雲配準可以模擬三維點雲的移動和對其，也就是會獲得一個旋轉矩陣和一個平移向量，
通常表達爲一個4×3的矩陣，其中3×3是旋轉矩陣，1*3是平移向量。
嚴格說來是6個參數，因爲旋轉矩陣也可以通過羅格里德斯變換轉變成1*3的旋轉向量。

 

常用的點雲配准算法有兩種：

    1. 正態分佈變換方法  NDT  正態分佈變換進行配準（normal Distributions Transform） 
    2. 和著名的 迭代最近點 Iterative Closest Point （ICP） 點雲配準，

此外還有許多其它算法 列舉如下：

    ICP：穩健ICP、point to plane ICP、point to line ICP、MBICP、GICP
    NDT: NDT 3D、Multil-Layer NDT
    FPCS、KFPSC、SAC-IA
    Line Segment Matching、ICL

兩個數據集的計算步驟：

  1. 識別最能代表兩個數據集中的場景的興趣點（interest points）（即關鍵點 keypoints）
  2. 在每個關鍵點處，計算特徵描述符;
  3. 從特徵描述符集合以及它們在兩個數據集中的x,y,z位置，基於特徵和位置之間的相似性來估計對應關係;
  4. 假設數據被認爲包含噪聲的，並不是所有的對應關係都是有效的，
     所以捨棄對配準過程產生負面影響的那些負影響對應關係;
  5. 利用剩餘的正確的對應關係來估算剛體變換，完整配準。

迭代最近點 Iterative Closest Point （ICP）

ICP算法本質上是基於最小二乘法的最優配準方法。
該算法重複進行選擇對應關係點對，計算最優剛體變換這一過程，直到滿足正確配準的收斂精度要求。
算法的輸入：參考點雲和目標點雲，停止迭代的標準。
算法的輸出：旋轉和平移矩陣，即轉換矩陣。

使用點匹配時，使用點的XYZ的座標作爲特徵值，針對有序點雲和無序點雲數據的不同的處理策略：
      1. 窮舉配準（brute force matching）;
      2. kd樹最近鄰查詢（FLANN）;
      3. 在有序點雲數據的圖像空間中查找;
      4. 在無序點雲數據的索引空間中查找.
特徵描述符匹配：
      1. 窮舉配準（brute force matching）;
      2. kd樹最近鄰查詢（FLANN）。

in cloud B for every point in cloud A .

錯誤對應關係的去除（correspondence rejection）:

    由於噪聲的影響，通常並不是所有估計的對應關係都是正確的，
    由於錯誤的對應關係對於最終的剛體變換矩陣的估算會產生負面的影響，
    所以必須去除它們，可以採用隨機採樣一致性估計，或者其他方法剔除錯誤的對應關係，
    最終只使用一定比例的對應關係，這樣既能提高變換矩陣的估計京都也可以提高配準點的速度。

變換矩陣的估算（transormation estimation）的步驟如下:

      1. 在對應關係的基礎上評估一些錯誤的度量標準
      2. 在攝像機位姿（運動估算）和最小化錯誤度量標準下估算一個剛體變換(  rigid  transformation )
      3. 優化點的結構  (SVD奇異值分解 運動估計;使用Levenberg-Marquardt 優化 運動估計;)
      4. 使用剛體變換把源旋轉/平移到與目標所在的同一座標系下，用所有點，點的一個子集或者關鍵點運算一個內部的ICP循環
      5. 進行迭代，直到符合收斂性判斷標準爲止。


===================================================

迭代最近點算法（Iterative CLosest Point簡稱ICP算法）:

ICP算法對待拼接的2片點雲，首先根據一定的準則確立對應點集P與Q，
其中對應點對的個數，然後通過最小二乘法迭代計算最優的座標變換，
即旋轉矩陣R和平移矢量t，使得誤差函數最小，

ICP處理流程分爲四個主要的步驟：

    1. 對原始點雲數據進行採樣(關鍵點 keypoints(NARF, SIFT 、FAST、均勻採樣 UniformSampling)、
       特徵描述符　descriptions，NARF、 FPFH、BRIEF 、SIFT、ORB )
    2. 確定初始對應點集(匹配 matching )
    3. 去除錯誤對應點對(隨機採樣一致性估計 RANSAC )
    4. 座標變換的求解

Feature based registration 配準

    1. SIFT 關鍵點 (pcl::SIFT…something)
    2. FPFH 特徵描述符  (pcl::FPFHEstimation)  
    3. 估計對應關係 (pcl::CorrespondenceEstimation)
    4. 錯誤對應關係的去除( pcl::CorrespondenceRejectionXXX )  
    5. 座標變換的求解

PCL類的相關的介紹:

對應關係基類　　   pcl::CorrespondenceGrouping< PointModelT, PointSceneT >
幾何相似性對應　   pcl::GeometricConsistencyGrouping< PointModelT, PointSceneT >
相似性度量　　　   pcl::recognition::HoughSpace3D
多實例對應關係　   pcl::Hough3DGrouping< PointModelT, PointSceneT, PointModelRfT, PointSceneRfT >
CRH直方圖　　　   pcl::CRHAlignment< PointT, nbins_ >
隨機採樣一致性估計 pcl::recognition::ObjRecRANSAC::Output
      pcl::recognition::ObjRecRANSAC::OrientedPointPair
      pcl::recognition::ObjRecRANSAC::HypothesisCreator
      pcl::recognition::ObjRecRANSAC

      pcl::recognition::ORROctree::Node::Data
      pcl::recognition::ORROctree::Node
      pcl::recognition::ORROctree
      pcl::recognition::RotationSpace

GITHUB

 

#include <iostream>                 //標準輸入輸出頭文件
#include <pcl/io/pcd_io.h>          //I/O操作頭文件
#include <pcl/point_types.h>        //點類型定義頭文件
#include <pcl/registration/icp.h>   //ICP配準類相關頭文件
#include <sstream> 

int
 main (int argc, char** argv)
{ 
  //創建兩個pcl::PointCloud<pcl::PointXYZ>共享指針，並初始化它們
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

  float x_trans = 0.7;
  if(argc>=2) {
   std::istringstream xss(argv[1]);
   xss >> x_trans;

  }

  // 隨機填充點雲
  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->points.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);
  }
  std::cout << "Saved " << cloud_in->points.size () << " data points to input:"//打印處點雲總數
      << std::endl;
// 打印座標
  for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << "    " << //打印處實際座標
      cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
      cloud_in->points[i].z << std::endl;
  *cloud_out = *cloud_in;
  std::cout << "size:" << cloud_out->points.size() << std::endl;
//實現一個簡單的點雲剛體變換，以構造目標點雲
  for (size_t i = 0; i < cloud_in->points.size (); ++i)
    cloud_out->points[i].x = cloud_in->points[i].x + x_trans;//　x軸正方向偏移　0.7米
  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;

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;//創建IterativeClosestPoint的對象
  icp.setInputCloud(cloud_in);                 //cloud_in設置爲點雲的源點
  icp.setInputTarget(cloud_out);               //cloud_out設置爲與cloud_in對應的匹配目標
  pcl::PointCloud<pcl::PointXYZ> Final;        //存儲經過配準變換點雲後的點雲
  icp.align(Final);  
    
//打印經過配準變換點雲後的點雲
std::cout << "Assiend " << Final.points.size () << " data points:"
      << std::endl;
  for (size_t i = 0; i < Final.points.size (); ++i)   
      std::cout << "    " << Final.points[i].x << " " <<
      			     Final.points[i].y << " " << 
			     Final.points[i].z << std::endl;                      
  //打印配準相關輸入信息
  std::cout << "has converged:" << icp.hasConverged() << " score: " <<
  icp.getFitnessScore() << std::endl;
  std::cout << "Transformation: "<< "\n" << icp.getFinalTransformation() << std::endl;

 return (0);
}

正態分佈變換進行配準（normal Distributions Transform）

介紹關於如何使用正態分佈算法來確定兩個大型點雲之間的剛體變換，
正態分佈變換算法是一個配准算法，它應用於三維點的統計模型，
使用標準最優化技術來確定兩個點雲間的最優匹配，
因爲其在配準的過程中不利用對應點的特徵計算和匹配，
所以時間比其他方法比較快.

#include <iostream>
#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>
#include <boost/thread/thread.hpp>

int
main (int argc, char** argv)
{
  // 加載房間的第一次掃描點雲數據作爲目標
  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 ("Couldn't read file room_scan1.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << 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 ("Couldn't read file room_scan2.pcd \n");
    return (-1);
  }
  std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;

//以上的代碼加載了兩個PCD文件得到共享指針，
//後續配準是完成對源點雲到目標點雲的參考座標系的變換矩陣的估計，
//得到第二組點雲變換到第一組點雲座標系下的變換矩陣
// 將輸入的掃描點雲數據過濾到原始尺寸的10%以提高匹配的速度，
//只對源點雲進行濾波，減少其數據量，而目標點雲不需要濾波處理
//因爲在NDT算法中在目標點雲 對應的 體素網格數據結構的 統計計算不使用單個點，
//而是使用包含在每個體素單元格中的點的統計數據
  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
  approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
  approximate_voxel_filter.setInputCloud (input_cloud);// 第二次掃描點雲數據作爲源點雲
  approximate_voxel_filter.filter (*filtered_cloud);
  std::cout << "Filtered cloud contains " << filtered_cloud->size ()
            << " data points from room_scan2.pcd" << std::endl;

  // 初始化正態分佈(NDT)對象
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

  // 根據輸入數據的尺度設置NDT相關參數
  ndt.setTransformationEpsilon (0.01);// 爲終止條件設置最小轉換差異
  ndt.setStepSize (0.1);              // 爲more-thuente線搜索設置最大步長
  ndt.setResolution (1.0);            // 設置NDT網格網格結構的分辨率（voxelgridcovariance）

  //以上參數在使用房間尺寸比例下運算比較好，但是如果需要處理例如一個咖啡杯子的掃描之類更小的物體，需要對參數進行很大程度的縮小

  //設置匹配迭代的最大次數，這個參數控制程序運行的最大迭代次數，一般來說這個限制值之前優化程序會在epsilon變換閥值下終止
  //添加最大迭代次數限制能夠增加程序的魯棒性阻止了它在錯誤的方向上運行時間過長
  ndt.setMaximumIterations (35);

  ndt.setInputSource (filtered_cloud);  //源點雲
  // Setting point cloud to be aligned to.
  ndt.setInputTarget (target_cloud);  //目標點雲

  // 設置使用機器人測距法得到的粗略初始變換矩陣結果
  Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
  Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

  // 計算需要的剛體變換以便將輸入的源點雲匹配到目標點雲
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  ndt.align (*output_cloud, init_guess);
   //這個地方的output_cloud不能作爲最終的源點雲變換，因爲上面對點雲進行了濾波處理
  std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
            << " score: " << ndt.getFitnessScore () << std::endl;

  // 使用創建的變換對爲過濾的輸入點雲進行變換
  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

  // 保存轉換後的源點雲作爲最終的變換輸出
  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

  // 初始化點雲可視化對象
  boost::shared_ptr<pcl::visualization::PCLVisualizer>
  viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer_final->setBackgroundColor (0, 0, 0);  //設置背景顏色爲黑色
 
  // 對目標點雲着色可視化 (red).
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
  target_color (target_cloud, 255, 0, 0);
  viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                  1, "target cloud");

  // 對轉換後的源點雲着色 (green)可視化.
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
  output_color (output_cloud, 0, 255, 0);
  viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
  viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                  1, "output cloud");

  // 啓動可視化
  viewer_final->addCoordinateSystem (1.0);  //顯示XYZ指示軸
  viewer_final->initCameraParameters ();   //初始化攝像頭參數

  // 等待直到可視化窗口關閉
  while (!viewer_final->wasStopped ())
  {
    viewer_final->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  return (0);
}