【点群登録アルゴリズム】【NDT】

0 はじめに

• この記事の目的は、点群登録アルゴリズムの学習を記録することです. ICP や PnP などは以前に学習したことがあり、NDT アルゴリズムについては後で説明しますので、記録します。

1 NDT (正規分布変換アルゴリズム)

1.1 NDT アルゴリズムの紹介

• 正規分布変換アルゴリズム。**NDT (正規分布変換)** アルゴリズムとも呼ばれます。アルゴリズムは、1 回限りの初期化ジョブです。ICP アルゴリズムと比較すると、NDT アルゴリズムのレジストレーション効果は ICP アルゴリズムと似ており、その改善は、別の従来の点群レジストレーション アルゴリズムである ICP アルゴリズムのラスター化と本質的に同等です。アルゴリズムの本質は、点群と点群の間の姿勢変換を計算することによって最適な一致を決定することです。点群が点群間の最適な一致であるかどうかを判断する方法は、標準的な最適化手法に基づいています。

• NDT アルゴリズムの一般的なプロセスは次のとおりです。
  ソース点群 P とターゲット点群 Q という 2 つの点群があることが知られています。
  (1) ソース点群 P が位置する空間を 1 単位と 1 単位のグリッドに分割します (つまり、2 次元空間への 3 次元空間の射影)。
  (2) 分割された単位グリッド内の点の分布に従って、単位グリッドの正規分布 PDF パラメータを計算します。
  (3) 伝達行列に従ってターゲット点群 Q 内の点を変換する;
  (4) ソース点群 P が配置されている空間分割グリッド内のターゲット点群点の数をカウントし、以下に従って対応する確率分布を計算するポイント関数の分布に;
  (5) すべてのポイントの最適値を解決します。つまり、ターゲット ポイント クラウドとソース ポイント クラウドの間の剛体変換を解決します。

• 概要:
  (1) ICP アルゴリズムとは異なり、NDT アルゴリズムでは、登録プロセス中に距離が正しくないポイント ペアを削除する必要があります。ただし、各反復プロセスの計算コストが高いため、すべての隣接点を検索する必要があります。
  (2) 登録時に、NDT アルゴリズムは対応点の特徴を経由せずに登録と計算を直接実行できます。したがって、隣接検索マッチングポイントを計算するときにあまりコストを消費せず、確率密度関数の計算は比較的簡単で、アルゴリズムの効率が大幅に向上します。したがって、時間計算量の観点からは、NDT アルゴリズムは ICP アルゴリズムよりも優れています。

• 参考：古典的な点群登録アルゴリズム：Normal Distribution Transformation Algorithm NDT (Normal Distributions Transform)

1.2 PCL ライブラリでの NDT アルゴリズムの使用

1.2.1 データボクセルフィルタリング処理

• ボクセル グリッド フィルター VoxelGrid ダウンサンプリングは、グリッド内のポイントの数を減らして、重心が変わらないようにします。pcl::VoxelGrid

pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Source(new pcl::PointCloud<pcl::PointXYZ>());

pcl::VoxelGrid<pcl::PointXYZ> cloud_filter_;//创建滤波对象
cloud_filter_.setLeafSize(1.3, 1.3, 1.3);/设置滤波时创建的体素体积为1.3m的立方体
cloud_filter_.setInputCloud(cloud_Source);//设置需要过滤的点云(指针)　给滤波对象
cloud_filter_.filter(*filtered_cloud_ptr);//执行滤波处理，存储输出

• 参考：pcl点群フィルタリングコード解析と原理紹介(1)

1.2.2 NDT処理を行う

//初始化NDT对象
	pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

	//根据输入数据的尺寸设置相关参数
	ndt.setTransformationEpsilon(0.0001);//为终止条件设置最小转换差异
	ndt.setStepSize(0.1); //为more-thuente线搜索设置最大步长
	ndt.setResolution(0.01); //设置NDT网格网格结构的分辨率（voxelgridcovariance）
	ndt.setMaximumIterations(100);//设置最大迭代次数

	ndt.setInputSource(filtered_cloud_ptr);//源点云
	//cloud_Target 同样也是 pcl::PointCloud<pcl::PointXYZ>::Ptr 格式
	ndt.setInputTarget(cloud_Target);//目标点云

	// 设置使用机器人测距法得到的粗略初始变换矩阵结果
	Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
	Eigen::Translation3f init_translation(1.79387, 0.72047, 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);

//获取匹配的位姿
//Ttarget_source
Eigen::Matrix4f pose = ndt.getFinalTransformation();

//获得匹配的的分
float score = ndt.getFitnessScore();