PCL點雲配準-正態分佈變換

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

配准算法提供了點雲配準變換的初始估：雖然算法不指定初值也可以運行，但是有了它，易於得到更好的結果，尤其是當兩塊點雲差異較大時。

配準分數通過計算output cloud與target cloud對應的最近點歐式距離的平方和得到，得分越小說明匹配效果越好。 

/*
使用正態分佈變換進行配準的實驗 。其中room_scan1.pcd  room_scan2.pcd這些點雲包含同一房間360不同視角的掃描數據
*/
#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);
}