NDT算法介紹

NDT正態(tài)分布點(diǎn)云算法，即Normal Distribution Transform，是將參考點(diǎn)云轉(zhuǎn)換成多維變量的正態(tài)分布來進(jìn)行配準(zhǔn)。如果變換參數(shù)能夠使得兩幅激光數(shù)據(jù)匹配得很好，那么變換點(diǎn)在參考點(diǎn)云中對(duì)應(yīng)的概率密度將會(huì)很大。因此，要使得兩幅激光點(diǎn)云數(shù)據(jù)匹配最優(yōu)，只需用優(yōu)化方法迭代計(jì)算使得概率密度之和最大的轉(zhuǎn)換矩陣。

現(xiàn)實(shí)中，LiDAR掃描得到的點(diǎn)云可能和參考點(diǎn)云（如高精地圖點(diǎn)云）存在細(xì)微的區(qū)別，此偏差可能來自于測(cè)量誤差，也有可能是“場(chǎng)景”發(fā)生了一下變化（比如說行人，車輛）。NDT配準(zhǔn)可以忽略細(xì)微變化，因此可用于解決這些細(xì)微的偏差問題。同時(shí)，NDT相對(duì)與ICP，它耗時(shí)穩(wěn)定，跟初值相關(guān)不大，初值誤差大時(shí)，也能很好得糾正過來。計(jì)算正態(tài)分布是個(gè)一次性的工作，因?yàn)槠湓谂錅?zhǔn)中不利用對(duì)應(yīng)點(diǎn)的特征計(jì)算和匹配，不需要消耗大量代價(jià)計(jì)算最鄰近點(diǎn)，所以時(shí)間較快。

NDT算法流程

?

?NDT公式推導(dǎo)

NDT算法的基本思想是先根據(jù)參考數(shù)據(jù)（reference scan）來構(gòu)建多維變量的正態(tài)分布，如果變換參數(shù)能使得兩幅激光數(shù)據(jù)匹配的很好，那么變換點(diǎn)在參考系中的概率密度將會(huì)很大。因此，可以考慮用優(yōu)化的方法求出使得概率密度之和最大的變換參數(shù)，此時(shí)兩幅激光點(diǎn)云數(shù)據(jù)將匹配的最好。

?

NDT缺點(diǎn)：格子參數(shù)最重要，太大導(dǎo)致精度不高，太小導(dǎo)致內(nèi)存過高，并且只有兩幅圖像相差不大的情況才能匹配。

改進(jìn)：

八叉樹建立，格子有大有小 ，迭代，每次使用更精細(xì)的格子

K聚類，有多少個(gè)類就有多少個(gè)cell，格子大小不一

三線插值 平滑相鄰的格子cell導(dǎo)致的不連續(xù)，提高精度

NDT代碼實(shí)戰(zhàn)

#ifdef USE_FAST_PCL#include <fast_pcl/registration/ndt.h>#else#include <pcl/registration/ndt.h>#endif #ifdef CUDA_FOUND#include <fast_pcl/ndt_gpu/NormalDistributionsTransform.h>#endif...........核心算法代碼，構(gòu)建地圖使用ndt算法static std::shared_ptr<gpu::GNormalDistributionsTransform> new_gpu_ndt_ptr = std::make_shared<gpu::GNormalDistributionsTransform>();//創(chuàng)建NDT轉(zhuǎn)換矩陣類new_gpu_ndt_ptr->setResolution(ndt_res);//網(wǎng)格大小設(shè)置new_gpu_ndt_ptr->setInputTarget(map_ptr);//參考幀new_gpu_ndt_ptr->setMaximumIterations(max_iter);//迭代最大次數(shù)new_gpu_ndt_ptr->setStepSize(step_size);//牛頓法優(yōu)化的最大步長(zhǎng)new_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);//連續(xù)變換之間允許的最大差值pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());pcl::PointXYZ dummy_point;dummy_scan_ptr->push_back(dummy_point);//構(gòu)建地圖mapnew_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);//當(dāng)前幀new_ndt.omp_align(*output_cloud, Eigen::Matrix4f::Identity());fitness_score = ndt.omp_getFitnessScore();t = ndt.getFinalTransformation();// Update localizer_poselocalizer_pose.x = t(0, 3);localizer_pose.y = t(1, 3);localizer_pose.z = t(2, 3);. .........-------------使用ndt算法與地圖參考幀進(jìn)行匹配---------------舉一個(gè)列子gpu_ndt_ptr->setInputSource（），其他和地圖一致#ifdef CUDA_FOUND//在代碼中標(biāo)記可以用GPU并行加速 if (_use_gpu == true) {gpu_ndt_ptr->setInputSource(filtered_scan_ptr);//參考點(diǎn)云 } else { #endifif (_use_fast_pcl){cpu_ndt.setInputSource(filtered_scan_ptr);}else{ndt.setInputSource(filtered_scan_ptr);} #ifdef CUDA_FOUND } #endif//NDT默認(rèn)值設(shè)置 static int max_iter = 30; // 最大迭代次數(shù) static float ndt_res = 1.0; // 分辨率1m static double step_size = 0.1; // 步長(zhǎng)0.1m static double trans_eps = 0.01; // 變換矩陣差值

其中 ndt.setTransformationEpsilon() 即設(shè)置變換的 ?（兩個(gè)連續(xù)變換之間允許的最大差值），這是判斷我們的優(yōu)化過程是否已經(jīng)收斂到最終解的閾值。ndt.setStepSize(0.1) 即設(shè)置牛頓法優(yōu)化的最大步長(zhǎng)。ndt.setResolution(1.0) 即設(shè)置網(wǎng)格化時(shí)立方體的邊長(zhǎng)，網(wǎng)格大小設(shè)置在NDT中非常重要，太大會(huì)導(dǎo)致精度不高，太小導(dǎo)致內(nèi)存過高，并且只有兩幅點(diǎn)云相差不大的情況才能匹配。ndt.setMaximumIterations(30) 即優(yōu)化的迭代次數(shù)，我們這里設(shè)置為35次，即當(dāng)?shù)螖?shù)達(dá)到35次或者收斂到閾值時(shí)，停止優(yōu)化。
由于NDT算法不需要匹配各個(gè)點(diǎn)計(jì)算速度較ICP快，官方建議定位模塊使用NDT算法，然后通過使用CUDA實(shí)現(xiàn)的 fast_pcl package實(shí)現(xiàn)了對(duì)NDT優(yōu)化過程的并行加速。

參考鏈接：

NDT點(diǎn)云配準(zhǔn)算法_海清河宴的博客-CSDN博客_ndt配準(zhǔn)

無人駕駛學(xué)習(xí)筆記-NDT 配準(zhǔn)_ppipp1109的博客-CSDN博客_ndt配準(zhǔn)

無人駕駛算法學(xué)習(xí)（五）：激光里程計(jì)之幀間匹配算法_su揚(yáng)帆啟航的博客-CSDN博客_幀間匹

總結(jié)

以上是生活随笔為你收集整理的NDT点云配准的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。