ndt算法學習

　　NDT算法原理：

　　　　NDT算法的基本思想是先根據參考數據（reference scan）來構建多維變量的正態分布， 如果變換參數能使得兩幅激光數據匹配的很好，那么變換點在參考系中的概率密度將會很大。

       因此，可以考慮用優化的方法求出使得概率密度之和最大的變換參數，此時兩幅激光點雲數 據將匹配的最好。

 

        算法流程：

1. 將空間（reference scan）划分成各個格子cell
2. 將點雲投票到各個格子

3. 計算格子的正態分布PDF參數

   

4. 將第二幅scan的每個點按轉移矩陣T的變換

5. 第二幅scan的點落於reference的哪個 格子，計算響應的概率分布函數

   

6. 求所有點的最優值，目標函數為

    NDT算法關鍵點：

    1、將二維空間划分為固定大小網格，每個網格至少包括3個點（一般5個）

    2、計算網格中點集的均值μ

    3、計算網格中點集的協方差矩陣Σ

    4、網格中的觀測到點x的概率p(x)服從正態分布N( μ,Σ)。

 

    NDT關鍵代碼示例：

    

    1、加載輸入點雲和目標點雲

    auto target_cloud = read_cloud_point("cloud1.pcd");

    auto input_cloud = read_cloud_point("cloud2.pcd");

 

    2、點雲濾波

    approximate_voxel_filter.setLeafSize(0.5, 0.5, 0.5);

    approximate_voxel_filter.setInputCloud(input_cloud);

    approximate_voxel_filter.filter(*filtered_cloud);

    3、配置點雲參數

    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

    ndt.setTransformationEpsilon(0.01); //收斂數

    ndt.setStepSize(0.1); //步長

    ndt.setResolution(1.0); //格子邊長

    ndt.setMaximumIterations(30); //迭代次數

    ndt.setInputSource(filtered_cloud);

    ndt.setInputTarget(target_cloud);

   

    4、設置初始值

    Eigen::AngleAxisf init_rotation(0.0, Eigen::Vector3f::UnitZ());

    Eigen::Translation3f init_translation (0, 0, 0);

    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

    5、開始配准

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);

    ndt.align(*output_cloud, init_guess);

    6、保存配准的點雲圖

    pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());

    pcl::io::savePCDFileASCII("cloud3.pcd", *output_cloud);