PCL如何使用ICP(iterative closest point)算法

ICP介紹

假如一個點雲只是另一個點雲的剛性變換的話，可以通過不斷縮小兩個點雲點之間的距離得到兩個點雲之間的剛性變換矩陣，從而使兩個點雲可以完全拼接在一起，而這種迭代最近點的算法就是ICP.

Why ICP 算法？

ICP主要用在2D重建，機器人定位並實現最佳路徑規劃以及3D模型的registration等應用場合。

如何使用PCL裏的ICP算法？

 1. 添加必要頭文件：

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>  

 2. 如下代碼示例所示：實例化icp並設定參數，setInputSource指定變換前的點雲，setInputTarget指定InputSource變換後的點雲,新定義的Final點雲用來存儲應用ICP算法之後所得到的點雲結果，如果變換前後點雲正確Align的話（即變換點雲通過剛性變換之後幾乎和變換後點雲完全重合），則 icp.hasConverged() = 1 (true)，然後輸出fitness得分和其他一些相關信息。

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputSource(cloud_in);
  icp.setInputTarget(cloud_out);
  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);
  std::cout << "has converged:" << icp.hasConverged() << " score: " <<
  icp.getFitnessScore() << std::endl;
  std::cout << icp.getFinalTransformation() << std::endl;

 此外還可以通過設置setMaximumIterations來確定初次配準時的迭代次數，當初次應用ICP算法時，只可以設置的稍微大一點，以使兩個點雲的點可以儘可能的接近。

完整工程示例

開發環境 Visual 2015&PCL1.8.1

PCL1.8.1下載地址： https://github.com/PointCloudLibrary/pcl/releases/tag/pcl-1.8.1

PCL頭文件目錄：

$(PCL_ROOT)
$(PCL_ROOT)\include\pcl-1.8
$(PCL_ROOT)\3rdParty\Eigen\eigen3
$(PCL_ROOT)\3rdParty\FLANN\include
$(PCL_ROOT)\3rdParty\VTK\include\vtk-8.0
$(PCL_ROOT)\3rdParty\Boost\include\boost-1_64

 PCL庫目錄：

$(PCL_ROOT)\lib\*.lib
$(PCL_ROOT)\3rdParty\VTK\lib\*.lib
$(PCL_ROOT)\3rdParty\FLANN\lib\*.lib
$(PCL_ROOT)\3rdParty\Boost\lib\*.lib

具體代碼可以參考我的github:  ICP Example

        裏面有兩個代碼，Source.cpp是PCL官方的例子，mian.cpp是我參考3改的測試例子, 運行某個例子時需要把另外一個Exclude掉。編譯並運行，每點擊一次空格鍵做一次ICP算法，結果如下：

應用ICP之前，

ICP算法迭代60次之後，

實際變換矩陣：

Applying this rigid transformation to: cloud_in -> cloud_icp
Rotation matrix :
    |  0.707 -0.707  0.000 |
R = |  0.707  0.707  0.000 |
    |  0.000  0.000  1.000 |
Translation vector :
t = <  0.000,  0.000,  0.400 >

ICP算法得到的轉換矩陣

ICP transformation 0 : cloud_icp -> cloud_in
Rotation matrix :
    |  0.724  0.690 -0.010 |
R = | -0.690  0.723 -0.022 |
    | -0.008  0.023  1.000 |
Translation vector :
t = <  0.010,  0.025, -0.399 >

 對比真實轉換矩陣和位移，已經非常接近了。所以說ICP算法得到的只是一個近似矩陣，迭代次數決定了與真實矩陣的近似程度。

總結

本博文只是測試了PCL官方的兩個ICP的例子，此處使用的點雲均爲同一個點雲，其中點的數目和位置均完全相同，但是在實際應用中，不論是結構光掃描還是激光掃描，同一個3D鏡頭不同時刻拍出點雲的數目是不一樣的，而且如果由多個相同鏡頭不同角度拍攝同一物體，點雲的數目也不一樣，ICP算法在這些情況下的應用效果還需以後驗證。

 

Reference:

1. http://pointclouds.org/documentation/tutorials/iterative_closest_point.php

2. http://pointclouds.org/documentation/tutorials/pcl_visualizer.php

3. http://pointclouds.org/documentation/tutorials/interactive_icp.php