NDT演算法詳解與C++實現

Gaowaly發表於2024-08-12

　　點雲匹配在感知環節是一個很重要的資訊獲取手段，而其中的演算法也有幾個比較經典了，例如ICP（Iterative Closest Point，迭代最近點）演算法，而本文決定記錄學習的是NDT演算法，也就是Normal Distribution Transform，正態分佈變換演算法。什麼是正態分佈變換演算法呢，簡言之，就是把空間中的點雲進行整理，整理成具有分佈特性的塊，然後根據這些塊來對不同幀的點雲做匹配，求解這些分佈塊的位姿變換。

但實際上，還是要好好學習其原理以及實現才行。

0.本文大綱

以下給出本文大綱，一共分為學習部分以及總結部分：

NDT演算法的基本原理學習
NDT演算法C++程式碼實現
總結

1.NDT演算法的基本原理學習

　　首先需要明確一點，任何演算法都是一系列邏輯和數值運算組成的針對某一問題的解決方案。NDT也不例外，因此其演算法原理就要從提出這一系列運算的作者文章和原始程式碼中去學習了。而此處使用的是IEEE的論文：《The Normal Distributions Transform: A New Approach to Laser Scan Matching》-Peter Biber.

1.1 NDT演算法核心思想與論文學習

　　正態分佈變換（NDT）演算法是一個匹配演算法，可以較好的獲取前後兩個目標之間的姿態變動關係以及匹配度等資訊，故常用於匹配定位，地圖構建等，而NDT最經典的應用就是鐳射點雲的匹配，獲取位姿變換，也就是旋轉平移變化引數

核心思想

　　其核心思想在於把目標點雲（Target）分成若干個小立方塊（常稱作“網格”，論文或程式碼常用“cell”表示），根據設定好的引數，求解每個網格的多維正態分佈並計算其機率分佈模型，當處在同坐標下的源點雲（Source）進入時，根據正態分佈引數計算每個轉換點在對應網格中的機率，累計所有網格的機率得到

　　還有一個關鍵點在於如何使機率

論文學習—— Abstract-摘要

第一部分：摘要（重點）表示：^[1]文章提出了一種新的鐳射匹配方式，而這種匹配方式可以較好的應用在SLAM方案中，實時建圖且不依賴於里程計資料；

第二部分：隨後在介紹和先前工作中介紹了SLAM現有成果，以及NDT演算法在SLAM方案中的效果和優勢，包括對比了ICP匹配方法以及一些複雜的工況討論；

第三部分：NDT演算法流程（與前述核心思想對照）

主要包括點雲網格化，求均值，協方差矩陣，計算正態分佈引數等；

第四部分：掃描匹配流程（前提是在同坐標系，或應用起來認為是相同一個感測器的相鄰資料幀）

　　主要包括：

　　在第一幀Target上建立NDT關係（獲取到計算引數），初始化匹配引數（這一步可以用0或者里程計資料給出，後續會詳細分析區別），

　　在第二幀Source對映到第一幀資料的座標系中，計算每個點的正態分佈情況，獲得各網格的機率得分及總分，透過後端最佳化來獲取一個最好的引數估計值，持續計算每個點的正態分佈情況直到收斂；

第五部分：後端最佳化方法，文中選取的是牛頓法，求解

第六部分：位姿跟蹤與地圖構建，這一部分主要描述了NDT定位的以及建圖的一些效果。

建圖效果

論文整體理解難度不高，可以結合核心思想學習，此處不再贅述。

1.2 NDT演算法詳細流程分解與公式學習

　　第二小節的主要目標是把NDT演算法中每一個流程出現的步驟再細化，出現公式的步驟學習清楚。

第一個流程：求解NDT（注意理解什麼是NDT）。步驟如下：

　　①將空間劃分為一個個網格，網格大小可以自擬，文中用的1m × 1m的2D網格，這個網格大小可以根據環境來確認，例如鐳射掃描200m範圍時，可以取到更大的網格；遍歷這些網格，保留至少包含3個點的網格（這個單元格中的最小點數為3是確保正態分佈可計算，實際中可以取大於3的值，例如在某些室外環境中可以取到十幾到幾十上百個點）；

　　實際計算過程中，為了避免離散化影響，可以適當採取離散點去除演算法或增加網格屬性，例如文中採取了4重重疊的網格；另外在計算協方差矩陣時，為了防止奇異值出現，應該在求解協方差矩陣後，出現數值過小的應當被手動設定為系統最小，從而完成後續計算。

2.NDT演算法流程與C++程式碼實現

NDT演算法可以直接根據原理寫C++程式碼，也可以呼叫現成的庫來實現，此處就給一個簡單的基於PCL庫的NDT實現方法。

2.1 基於PCL的NDT演算法C++程式碼實現（部分）

//其他的前置部分略去，如果寫成函式，函式輸入輸出應為原始點雲和點雲地圖 
//此處的呼叫寫法基本思想是寫為類(Class)
//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
//1.基本變數定義等 
int max_iter_ = 35;//迭代最大次數
double step_size_ = 0.2;//線搜尋的最大步長，理論上這個值是網格解析度的一半，但也不能過小，當匹配效果不好時，應該放大這個引數，解析度同理
double ndt_res_ = 2.0;//NDT網格解析度
double trans_eps_ = 0.001;//終止條件
double voxel_leaf_size_ = 0.5;//體素濾波size，經驗判斷其值因該是點雲跨度最大除以30
double min_scan_range_ = 2.0;//最小過濾範圍
double max_scan_range_ = 200.0;//最大過濾範圍
//map_是先前已有的點雲，注意這個若是第一幀則需要初始化
pcl::PointCloud<pcl::PointXYZI>::Ptr map_(new pcl::PointCloud<pcl::PointXYZI>);
//scan_ptr是經過預處理後的原始點雲，例如過近過遠的點需要去除
pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
//filtered_scan_ptr是經過降取樣後的點雲
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
//NDT轉換完的點雲
pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
//增廣後的旋轉平移矩陣
Eigen::Matrix4f t_localizer(Eigen::Matrix4f::Identity());
Eigen::Matrix4f t_base_link(Eigen::Matrix4f::Identity());
//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
//2.部分模組初始化
//地圖初始化
if (initial_scan_loaded == 0)
{
  pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, tf_btol_);//tf_btol_此時是一個空陣，即不存在旋轉關係
  *map_ += *transformed_scan_ptr;
  initial_scan_loaded = 1;
}
pcl::PointCloud<pcl::PointXYZI>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZI>(*map_)); //存放第一幀點雲，隨後放置先前的所有地圖
if (is_first_map_ == true)
{
  ndt.setInputTarget(map_ptr);//此處是呼叫PCL庫。如pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> ndt;
  is_first_map_ = false;
  pcl::copyPointCloud(*map_ptr, *final_map);
}
//....
//GPS關係初始化
if (initial_GPS_loaded == 0)
{
  //initial
  current_gps_rtk_pose_.x = gps_rtk.x;
  current_gps_rtk_pose_.y = gps_rtk.y;
  current_gps_rtk_pose_.z = gps_rtk.z
  current_gps_rtk_pose_.roll = gps_rtk.roll;
  current_gps_rtk_pose_.pitch = gps_rtk.pitch;
  current_gps_rtk_pose_.yaw = gps_rtk.yaw;
  //update
  previous_gps_rtk_pose_.x = current_gps_rtk_pose_.x;
  previous_gps_rtk_pose_.y = current_gps_rtk_pose_.y;
  previous_gps_rtk_pose_.z = current_gps_rtk_pose_.z;
  previous_gps_rtk_pose_.roll = current_gps_rtk_pose_.roll;
  previous_gps_rtk_pose_.pitch = current_gps_rtk_pose_.pitch;
  previous_gps_rtk_pose_.yaw = current_gps_rtk_pose_.yaw;
  initial_scan_loaded = 1;
}
//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
//3.NDT部分
//①獲取預處理後的點雲，此處用半徑方式對點雲過濾  
for (unsigned int i = 0; i < input_cloud->size(); i++)
{
  r = pow(pow(input_cloud->points[i].x, 2.0) + pow(input_cloud->points[i].y, 2.0) + pow(input_cloud->points[i].z, 2.0), 0.5);
  if (min_scan_range_ < r && r < max_scan_range_)
  {
    scan_ptr->push_back(input_cloud->points[i]);
  }
}
//②降維處理
pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter_;
voxel_grid_filter_.setInputCloud(scan_ptr);
voxel_grid_filter_.filter(*filtered_scan_ptr);
//③求解與轉換
Eigen::Translation3f init_translation(current_pose_.x, current_pose_.y, current_pose_.z);//初始化第一個位姿
Eigen::AngleAxisf init_rotation_x(current_pose_.roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf init_rotation_y(current_pose_.pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf init_rotation_z(current_pose_.yaw, Eigen::Vector3f::UnitZ());
Eigen::Matrix4f init_guess =
      (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol_;
pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
ndt.align(*output_cloud, init_guess);
t_localizer = ndt.getFinalTransformation();
t_base_link = t_localizer * tf_ltob_;
pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, t_localizer);
//④更新
Eigen::Matrix3d mat_b;
mat_b << static_cast<double>(t_base_link(0, 0)), static_cast<double>(t_base_link(0, 1)), static_cast<double>(t_base_link(0, 2)),
      static_cast<double>(t_base_link(1, 0)), static_cast<double>(t_base_link(1, 1)), static_cast<double>(t_base_link(1, 2)),
      static_cast<double>(t_base_link(2, 0)), static_cast<double>(t_base_link(2, 1)), static_cast<double>(t_base_link(2, 2));
// 更新當前位姿的XYZ
current_pose_.x = t_base_link(0, 3);
current_pose_.y = t_base_link(1, 3);
current_pose_.z = t_base_link(2, 3);
// 更新當前的朝向，使用尤拉角來表示
Eigen::Vector3d euler_angles = mat_b.eulerAngles(0, 1, 2);
current_pose_.roll = double(euler_angles(0));
current_pose_.pitch = double(euler_angles(1));
current_pose_.yaw = double(euler_angles(2));
//如果有必要，根據GPS與RTK模組更新的位姿做一次濾波，此處略，有需要請私信
//⑤地圖更新與NDT更新
double shift = sqrt(pow(current_pose_.x - previous_pose_.x, 2.0) + pow(current_pose_.y - previous_pose_.y, 2.0));
if (shift >= min_add_scan_shift_)
{
  *map_ += *transformed_scan_ptr;//更新地圖
  previous_pose_.x = current_pose_.x;
  previous_pose_.y = current_pose_.y;
  previous_pose_.z = current_pose_.z;
  previous_pose_.roll = current_pose_.roll;
  previous_pose_.pitch = current_pose_.pitch;
  previous_pose_.yaw = current_pose_.yaw;
  std::cout << "map_ptr  is : " << map_ptr->size() << std::endl;
  ndt.setInputTarget(map_);//把先前的地圖作為下一次的匹配目標
  pcl::copyPointCloud(*map_, *final_map);//把更新的地圖傳出去
}
//⑥列印輸出校驗結果
std::cout << "NDT has converged: " << ndt.hasConverged() << std::endl;
std::cout << "Fitness score: " << ndt.getFitnessScore() << std::endl;
std::cout << "Number of iteration: " << ndt.getFinalNumIteration() << std::endl;
std::cout << "(x,y,z,roll,pitch,yaw):" << std::endl;
std::cout << "(" << current_pose_.x << ", " << current_pose_.y << ", " << current_pose_.z << ", " << current_pose_.roll
            << ", " << current_pose_.pitch << ", " << current_pose_.yaw << ")" << std::endl;
std::cout << "Transformation Matrix:" << std::endl;
std::cout << t_localizer << std::endl;
std::cout << "shift: " << shift << std::endl;
std::cout << "final_map: " << final_map->points.size() << std::endl;
//其餘後置部分也做贅述

2.2 NDT演算法流程圖

　　程式碼的實現流程符合前述的原理，此處再給出一個流程圖，流程圖中預設有GPS與RTK資料，實際上也可以只用點雲匹配來進行結果計算和輸出。

　　純手畫，如果覺得有幫助的老鐵還請給一鍵三連支援一下~（轉發）

在GitHub上也有許多開源的解決方案，可以自行搜尋學習。

3.結果展示與總結

3.1 結果展示

以下展示的是基於Kitti資料集的結果。資料集編號為2011_09_26_drive_0009_sync，取用了velodyne_points裡的資料。

基於Kitti資料的NDT地圖結果

3.2 總結

　　前述整理了NDT演算法的相關學習內容。

　　第一部分小結：

　　首先是NDT演算法的理論，那麼從最根本的研讀演算法論文中我們學習到了：

　　NDT的核心思想在於把點雲網格化，計算不同幀間點雲網格的正態分佈機率的匹配程度，求解匹配度最高時的點雲變換關係；

　　其次就是演算法論文中的核心步驟：分為兩大步，一是求解兩幀的NDT，二是用非線性最佳化求解旋轉平移關係。

　　第二部分小結：

　　在第二部分中我們整理了一下相關程式碼，主要包括以下幾個部分：

　　1.點雲及旋轉平移關係的定義；

　　2.各個模組初始化；

　　3.NDT主要部分：

　　　　①預處理；

　　　　②降維；

　　　　③求解與轉換；

　　　　④更新位姿；

　　　　⑤地圖更新與NDT目標更新；

　　　　⑥校驗結果。

　　繪製了流程圖，流程圖中有關於GPS與RTK的部分，在實現時可以選擇性完成，如果不選擇使用，只用點雲也可以獲得結果，但可能精度有所下降。

參考文獻：SLAM演算法工程師之路：NDT演算法詳解與C++實現 - 知乎 (zhihu.com)

DPLL 演算法（求解k-SAT問題）詳解（C++實現）
2022-03-08
演算法C++
遺傳演算法與C++實現
2015-01-06
演算法C++
『資料結構與演算法』棧：詳解與程式碼實現
2021-02-15
資料結構演算法
二叉搜尋樹演算法詳解與Java實現
2016-07-31
演算法Java
遺傳演算法詳解與實驗
2020-07-28
演算法
詳解 dotenv 的使用與實現
2024-08-27
C++實現二分法詳解
2021-07-07
C++
C++實現Prim演算法
2020-10-02
C++演算法
C++實現迷宮的生成與解決
2020-12-20
C++
C++中指標與引用詳解
2021-06-28
C++指標
html 列印相關操作與實現詳解
2020-05-20
HTML
熱修復（一）原理與實現詳解
2019-03-01
最大熵模型詳解與Python實現
2020-11-06
熵模型Python
圖的拓撲排序詳解與實現
2020-11-08
排序
快速排序演算法C++實現
2019-05-11
排序演算法C++
資料結構與演算法——B樹的C++實現
2016-05-13
資料結構演算法C++
C++二叉查詢樹實現過程詳解
2014-10-27
C++
高效字串匹配演算法——BM 演算法詳解（C++）
2023-02-14
字串匹配演算法C++
令牌桶演算法原理及實現(圖文詳解)
2023-02-28
演算法
協同過濾（CF）演算法詳解和實現
2016-06-24
演算法
資料結構與演算法——普通樹的定義與C++實現
2016-04-05
資料結構演算法C++
【模型推理】量化實現分享三：詳解 ACIQ 對稱量化演算法實現
2021-12-29
模型演算法
詳解布隆過濾器原理與實現
2021-12-05
過濾器
Redis Sentinel實現的機制與原理詳解
2017-03-17
Redis
線段樹詳解（原理，實現與應用）
2015-09-09
C++ MiniZip實現目錄壓縮與解壓
2023-11-22
C++
關於C++中物件與類的詳解及其作用詳解
2021-09-25
C++物件
iOS VIPER架構實踐(二)：VIPER詳解與實現
2017-08-21
iOS架構
MD5演算法--C++實現
2016-11-01
演算法C++
最小生成樹的性質與prim演算法（C++實現）
2020-11-29
演算法C++
資料結構與演算法——AVL樹類的C++實現
2016-04-08
資料結構演算法C++
Python程式設計實現蟻群演算法詳解
2020-02-07
Python程式設計演算法
詳細二叉樹實現c++
2020-12-09
二叉樹C++
資料結構與演算法——無權最短路徑演算法的C++實現
2016-05-03
資料結構演算法C++
資料結構與演算法——最短路徑Dijkstra演算法的C++實現
2016-05-04
資料結構演算法C++
資料結構與演算法——不相交集類的C++實現
2016-04-25
資料結構演算法C++
多圖詳解萬星 Restful 框架原理與實現
2021-10-10
REST框架
c++實現的模擬退火演算法
2018-08-27
C++演算法