PCL 之 ICP 算法實現

ICP是什麼？

ICP（Iterative Closest Point），即最近點迭代算法，是最爲經典的數據配准算法。其特徵在於，經過求取源點雲和目標點雲之間的對應點對，基於對應點對構造旋轉平移矩陣，並利用所求矩陣，將源點雲變換到目標點雲的座標系下，估計變換後源點雲與目標點雲的偏差函數，若偏差函數值大於閥值，則迭代進行上述運算直到知足給定的偏差要求.

ICP算法採用最小二乘估計計算變換矩陣，原理簡單且具備較好的精度，可是因爲採用了迭代計算，致使算法計算速度較慢，並且採用ICP進行配準計算時，其對待配準點雲的初始位置有必定要求，若所選初始位置不合理，則會致使算法陷入局部最優。

PCL+ICP

PCL點雲庫已經實現了多種點雲配准算法，結合pcl，本次配準的主要目的是：

1. 對PCL中ICP算法進行一些註解
2. 建立可視化窗口，經過設置鍵盤迴調函數，控制迭代過程，觀察ICP算法的計算過程

PCL中ICP的官方參考文檔 http://pointclouds.org/docume...

具體代碼實現

首先看一下pcl中ICP的主要代碼：

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;  //建立ICP的實例類
 icp.setInputSource(cloud_sources);
 icp.setInputTarget(cloud_target);
 icp.setMaxCorrespondenceDistance(100);  
 icp.setTransformationEpsilon(1e-10); 
 icp.setEuclideanFitnessEpsilon(0.001); 
 icp.setMaximumIterations(100);   
 icc.align(final);

須要說明的是：

其一：PCL中的ICP算法是基於SVD（Singular Value Decomposition）實現的.

其二：使用pcl的ICP以前要set幾個參數：

1. setMaximumIterations， 最大迭代次數，icp是一個迭代的方法，最多迭代這些次（若結合可視化並逐次顯示，可將次數設置爲1）；
2. setEuclideanFitnessEpsilon， 設置收斂條件是均方偏差和小於閾值， 中止迭代；
3. setTransformtionEpsilon, 設置兩次變化矩陣之間的差值（通常設置爲1e-10便可）；
4. setMaxCorrespondenaceDistance,設置對應點對之間的最大距離（此值對配準結果影響較大）。

若是僅僅運行上述代碼，並設置合理的的預估計參數，即可實現利用ICP對點雲數據進行配準計算.

其次爲了更深刻的瞭解ICP的計算過程，即本試驗的第二個目的，繼續添加如下代碼：

boost::shared_ptr<pcl::visualization::PCLVisualizer> view(new pcl::visualization::PCLVisualizer("icp test"));  //定義窗口共享指針
int v1 ; //定義兩個窗口v1，v2，窗口v1用來顯示初始位置，v2用以顯示配準過程
int v2 ;
view->createViewPort(0.0,0.0,0.5,1.0,v1);  //四個窗口參數分別對應x_min,y_min,x_max.y_max.
view->createViewPort(0.5,0.0,1.0,1.0,v2);

pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> sources_cloud_color(cloud_in,250,0,0); //設置源點雲的顏色爲紅色
view->addPointCloud(cloud_in,sources_cloud_color,"sources_cloud_v1",v1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_cloud_color (cloud_target,0,250,0);  //目標點云爲綠色
view->addPointCloud(cloud_target,target_cloud_color,"target_cloud_v1",v1); //將點雲添加到v1窗口

view->setBackgroundColor(0.0,0.05,0.05,v1); //設着兩個窗口的背景色
view->setBackgroundColor(0.05,0.05,0.05,v2);

view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2,"sources_cloud_v1");  //設置顯示點的大小
view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2,"target_cloud_v1");

pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>aligend_cloud_color(Final,255,255,255);  //設置配準結果爲白色
view->addPointCloud(Final,aligend_cloud_color,"aligend_cloud_v2",v2);
view->addPointCloud(cloud_target,target_cloud_color,"target_cloud_v2",v2);

view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2,"aligend_cloud_v2");
view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2,"target_cloud_v2");

view->registerKeyboardCallback(&keyboardEvent,(void*)NULL);  //設置鍵盤迴調函數
int iterations = 0; //迭代次數
while(!view->wasStopped())
{
        view->spinOnce();  //運行視圖
        if (next_iteration)
        {
                icp.align(*Final);  //icp計算
                cout <<"has conveged:"<<icp.hasConverged()<<"score:"<<icp.getFitnessScore()<<endl;
                cout<<"matrix:\n"<<icp.getFinalTransformation()<<endl;
                cout<<"iteration = "<<++iterations;
                /*... 若是icp.hasConverged=1,則說明本次配準成功，icp.getFinalTransformation()可輸出變換矩陣   ...*/
                if (iterations == 1000)  //設置最大迭代次數
                        return 0;
                view->updatePointCloud(Final,aligend_cloud_color,"aligend_cloud_v2");
                
        }
        next_iteration = false;  //本次迭代結束，等待觸發

}

最後還須要設置如下鍵盤迴調函數，用以控制迭代進程：

bool next_iteration = false;
//設置鍵盤交互函數
void keyboardEvent(const pcl::visualization::KeyboardEvent &event,void *nothing)
    {
            if(event.getKeySym() == "space" && event.keyDown())
                    next_iteration = true;
    }
    /*... 上述函數表示當鍵盤空格鍵按下時，纔可執行ICP計算 ... */

實例練習

將上述代碼組合，添加相應頭文件：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/parse.h>  //pcl控制檯解析

根據不一樣模型設置合適的參數，即可實現最初目的。實例展現：

ICP之變種

ICP有不少變種，有point-to-point的，也有point-to-plane的，通常後者的計算速度快一些，是基於法向量的，須要輸入數據有較好的法向量，具體使用時建議根據本身的須要及可用的輸入數據選擇具體方法。

• pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget > Class Template Reference
• pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference
• pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar > Class Template Reference
• pcl::IterativeClosestPointNonLinear< PointSource, PointTarget, Scalar > Class Template Reference
• pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference
• pcl::registration::IncrementalICP< PointT, Scalar > Class Template Reference