PCLのICPアルゴリズム実装

ICPって何?
ICP(Iterative Closest Point)、すなわち最近点反復アルゴリズムは、最も古典的なデータアライメントアルゴリズムである.ソース点群とターゲット点群との対応点対を求めることにより、対応点対に基づいて回転平行行列を構築し、求めた行列を利用して、ソース点群をターゲット点群の座標系に変換し、変換後のソース点群とターゲット点群の誤差関数を推定し、誤差関数値がバルブ値より大きい場合、所定の誤差要件を満たすまで上記演算を反復する.
ICPアルゴリズムは最小二乗推定を用いて変換行列を計算し,原理が簡単で精度が良いが,反復計算を採用したためアルゴリズム計算速度が遅く,ICPを用いてアライメント計算を行う場合,アライメント点群の初期位置に一定の要求があり,選択した初期位置が不合理であればアルゴリズムが局所最適に陥る.
PCL+ICP
PCL点群ライブラリはすでに多種の点群アライメントアルゴリズムを実現して、pclを結合して、今回のアライメントの主な目的は:

PCLにおけるICPアルゴリズムのいくつかの注釈

可視化ウィンドウを作成し、キーボードコールバック関数を設定することによって反復プロセスを制御し、ICPアルゴリズムの計算プロセス

を観察する.
PCLにおけるICPの公式参考文書http://pointclouds.org/docume...
具体的なコード実装
まずpclのICPの主なコードを見てみましょう.

pcl::IterativeClosestPoint<: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を使用する前にいくつかのパラメータをセットすることです.

1. setMaximumIterations，       ，icp        ，       （           ，       1）；
2. setEuclideanFitnessEpsilon，                 ，     ；
3. setTransformtionEpsilon,              （     1e-10  ）；
4. setMaxCorrespondenaceDistance,             （           ）。  

上記コードのみを実行する、合理的な事前推定パラメータを設定すれば、ICPによる点群データのアライメント計算を実現することができる.
次にICPの計算プロセス、すなわち本試験の第2の目的をより深く理解するために、以下のコードを追加し続ける.

boost::shared_ptr<: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<:pointxyz> sources_cloud_color(cloud_in,250,0,0); //           
view->addPointCloud(cloud_in,sources_cloud_color,"sources_cloud_v1",v1);
pcl::visualization::PointCloudColorHandlerCustom<: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<: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 <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 
#include 
#include 
#include 
#include 
#include 
#include   //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