無人駕駛汽車系統入門（十三）——正态分布變換（NDT）配準與無人車定位
定位即确定無人車在這個世界中的哪個位置，是無人駕駛技術棧中必不可少的一部分。對于無人車而言，對定位的要求極高，一般情況下，我們希望我們的無人車能夠達到 厘米級 的定位精度，單純使用GPS能夠達到米級别的定位，顯然，無人車的定位子產品需要其他的技術支援以進一步提高定位的精度。本文介紹一種依賴于高精度地圖和雷射雷達的定位技術——正态分布變換（Normal Distributions Transform， NDT）。它能實作滿足無人車需求的厘米級的定位需求。
創作不易，轉載請注明出處：http://blog.csdn.net/adamshan/article/details/79230612
無人車系統中的定位問題
無人車的定位問題，實際上就是要找出無人車目前在地圖的那個位置（并且要求精度在厘米級别）。其中有一類方法被稱為即時定位與地圖建構（simultaneous localization and mapping，SLAM），它能夠實作導航定位而不需要地圖，然而，真正實作無人車的SLAM是非常困難的，作為交通工具，遠距離的行駛會使得實時建構地圖的偏差變大。是以，在已經擁有高精度地圖的前提下去做無人車的定位，是更加現實，簡單的。NDT就是一類利用已有的高精度地圖和雷射雷達實時測量資料實作高精度定位的技術。
NDT配準的大緻理念
下圖是一個高精度點雲地圖的例子：
下圖是一個Lidar掃描點雲的例子：
通過正态分布變換配準，能夠确定車輛目前在高精度地圖中的位置，如下圖所示：
放大之後：
可以看出，為了知道我們在這個地圖中的确切位置，我們比較lidar掃描得到的點雲和我們的地圖點雲，其中一個問題在于：lidar掃描得到的點雲可能和地圖的點雲存在細微的差別，這裡的偏差可能來自于測量誤差，也有可能這個“場景”發生了一下變化（比如說行人，車輛）。NDT配準就是用于解決這些細微的偏差問題。
NDT并沒有比較兩個點雲點與點之間的差距，而是先将參考點雲（即高精度地圖）轉換為多元變量的正态分布，如果變換參數能使得兩幅雷射資料比對的很好，那麼變換點在參考系中的機率密度将會很大。是以，可以考慮用優化的方法求出使得機率密度之和最大的變換參數，此時兩幅雷射點雲資料将比對的最好。
NDT算法
多元正态分布
我們知道，如果随機變量X滿足正态分布（即 X∼N(μ,σ) X ∼ N ( μ , σ ) ）,則其機率密度函數為：
f(x)=1σ2π‾‾‾√e−(x−μ)22σ2 f ( x ) = 1 σ 2 π e − ( x − μ ) 2 2 σ 2
其中的 μ μ 為正态分布的均值， σ2 σ 2 為方差，這是對于次元 D=1 D = 1 的情況而言的。對于多元正态分布而言，其機率密度函數可以表示為：
f(x⃗ )=1(2π)D2|Σ|‾‾‾√e−(x⃗ −μ⃗ )TΣ−1(x⃗ −μ⃗ )2 f ( x → ) = 1 ( 2 π ) D 2 | Σ | e − ( x → − μ → ) T Σ − 1 ( x → − μ → ) 2
其中 x⃗ x → 就表示均值向量，而 Σ Σ 表示協方差矩陣,我們知道，協方差矩陣對角元素表示的是對應的元素的方差，非對角元素則表示對應的兩個元素（行與列）的相關性。
下圖表示一個二進制的正态分布，
NDT算法流程
網格化并計算正态分布參數
NDT算法的第一步就是将參考點雲網格化（對于三維地圖來說，即使用一個一個小立方體對整個空間的掃描點進行劃分），對于每一個網格（cell）,基于網格内的點計算其機率密度函數（probability density function, PDF）,
μ⃗ =1m∑k=1my⃗ k, μ → = 1 m ∑ k = 1 m y → k ,
Σ=1m∑k=1m(y⃗ k−μ⃗ )(y⃗ k−μ⃗ )T Σ = 1 m ∑ k = 1 m ( y → k − μ → ) ( y → k − μ → ) T
其中， y⃗ k=1,...,m y → k = 1 , . . . , m 表示一個網格内所有的掃描點。那麼一個網格的機率密度函數則為：
f(x⃗ )=1(2π)32|Σ|‾‾‾√e−(x⃗ −μ⃗ )TΣ−1(x⃗ −μ⃗ )2 f ( x → ) = 1 ( 2 π ) 3 2 | Σ | e − ( x → − μ → ) T Σ − 1 ( x → − μ → ) 2
使用正态分布來表示原本離散的點雲有諸多好處，這種分塊的（通過一個個cell）光滑的表示是連續可導的，每一個機率密度函數可以被認為是一個局部表面（local surface）的近似,它不但描述了這個表面在空間中的位置，同時還包含了這個表面的方向和光滑性等資訊。下圖是一個3D點雲及其網格化效果:
上圖中立方體的邊長為 1 米，其中越明亮的位置表示機率越高。此外，局部表面的方向和光滑性則可以通過協方差矩陣的特征值和特征向量反映出來。我們以三維的機率密度函數為例，如果三個特征值很接近，那麼這個正态分布描述的表面是一個球面，如果一個特征值遠大于另外兩個特征值，則這個正态分布描述的是一條線，如果一個特征值遠小于另外兩個特征值，則這個正态分布描述的是一個平面。下圖描述了協方差矩陣特征值和表面形狀之間的關系：
變換參數和最大似然
當使用NDT配準時，目标是找到目前掃描的姿态，使目前掃描的點位于參考掃描（3D地圖）表面上的可能性最大化。那麼我們需要優化的參數就是對目前掃描的點雲的變換（旋轉，平移等），我們使用一個變換參數 p⃗ p → 來描述。目前掃描為一個點雲 X={x⃗ 1,...,x⃗ n} X = { x → 1 , . . . , x → n } ,給定掃描點集合 X X 和變換參數 p⃗ p→ ，令空間轉換函數 T(p⃗ ,x⃗ k) T ( p → , x → k ) 表示使用使用姿态變換 p⃗ p → 來移動點 x⃗ k x → k ,結合之前的一組狀态密度函數（每個網格都有一個PDF），那麼最好的變換參數 p⃗ p → 應該是最大化似然函數的姿态變換：
Likelihood: Θ=∏k=1nf(T(p⃗ ,x⃗ k)) L i k e l i h o o d : Θ = ∏ k = 1 n f ( T ( p → , x → k ) )
那麼最大化似然也就相當于最小化負對數似然 −logΘ − log Θ ;
−logΘ=−∑k=1nlog(f(T(p⃗ ,x⃗ k))) − log Θ = − ∑ k = 1 n log ( f ( T ( p → , x → k ) ) )
到這裡，就到了我們最熟悉的最優化的部分了。現在的任務就是使用優化算法來調整變換參數 p⃗ p → 來最小化這個負對數似然。NDT算法使用牛頓法進行參數優化。我們不難看出，這裡的機率密度函數 f(x⃗ ) f ( x → ) 其實并不要求一定是正态分布，任何能夠反映掃描表面的結構資訊且對異常掃描點具有魯棒性的機率密度函數都是可以的。
使用C++實作NDT配準
下面我們使用C++實作一個正态分布配準的例子。本例使用Point Cloud Library（PCL）提供的正态分布變換函數來對兩個點雲圖進行配準，點雲資料分别儲存在
build/cloud1.pcd
和
build/cloud2.pcd
檔案中。
PCL是一個用于處理2D/3D圖像和點雲的C++庫，是一個開源項目，其github倉庫：https://github.com/PointCloudLibrary/pcl 安裝：http://www.pointclouds.org/downloads/
讀取pcd檔案中的點雲資訊
我們寫一個函數來讀取對應的pcd檔案：
pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud_point(std::string const &file_path){
// Loading first scan.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (file_path, *cloud) == -)
{
PCL_ERROR ("Couldn't read the pcd file\n");
return nullptr;
}
return cloud;
}
在
main
函數中分别讀取點雲：
auto target_cloud = read_cloud_point("cloud1.pcd");
std::cout << "Loaded " << target_cloud->size () << " data points from cloud1.pcd" << std::endl;
auto input_cloud = read_cloud_point("cloud2.pcd");
std::cout << "Loaded " << input_cloud->size () << " data points from cloud2.pcd" << std::endl;
從兩個pcd檔案中讀取出來的點的數量：
Loaded data points from cloud1.pcd
Loaded data points from cloud2.pcd
過濾輸入點雲
對如此多點做優化是非常耗時的，我們使用
voxel_filter
對輸入的點雲進行過濾，這裡隻對
input_cloud
進行了濾波處理，減少其資料量到10%左右，而
target_cloud
不做濾波處理:
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize(., ., .);
approximate_voxel_filter.setInputCloud(input_cloud);
approximate_voxel_filter.filter(*filtered_cloud);
std::cout<<"Filtered cloud contains "<< filtered_cloud->size() << "data points from cloud2.pcd" << std::endl;
過濾後的掃描點數量隻有原來的10\%
Filtered cloud contains data points from cloud2.pcd
初始化NDT并且設定NDT參數，
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setTransformationEpsilon();
ndt.setStepSize();
ndt.setResolution();
ndt.setMaximumIterations();
ndt.setInputSource(filtered_cloud);
ndt.setInputTarget(target_cloud);
其中
ndt.setTransformationEpsilon()
即設定變換的 ϵ ϵ （兩個連續變換之間允許的最大內插補點），這是判斷我們的優化過程是否已經收斂到最終解的門檻值。
ndt.setStepSize(0.1)
即設定牛頓法優化的最大步長。
ndt.setResolution(1.0)
即設定網格化時立方體的邊長，網格大小設定在NDT中非常重要，太大會導緻精度不高，太小導緻記憶體過高，并且隻有兩幅點雲相差不大的情況才能比對。
ndt.setMaximumIterations(35)
即優化的疊代次數，我們這裡設定為35次，即當疊代次數達到35次或者收斂到門檻值時，停止優化。
初始化變換參數并開始優化
我們對變換參數 p⃗ p → 進行初始化(給一個估計值)，雖然算法不指定初值也可以運作，但是有了它，易于得到更好的結果，尤其是當兩塊點雲差異較大時。變換參數的初始化資料往往來自與測量資料：
Eigen::AngleAxisf init_rotation(, Eigen::Vector3f::UnitZ());
Eigen::Translation3f init_translation (, , );
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);
std::cout << "Normal Distribution Transform has converged:" << ndt.hasConverged()
<< "score: " << ndt.getFitnessScore() << std::endl;
儲存配準以後的點雲圖，輸出到檔案
cloud3.pcd
：
pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());
pcl::io::savePCDFileASCII("cloud3.pcd", *output_cloud);
将配準以後的點雲圖可視化
我們寫一個函數用于現實配準以後的點雲,其中，目标點雲（即我們已有的高精度地圖）用紅點繪制，而輸入點雲用綠點繪制：
void visualizer(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud){
// Initializing point cloud visualizer
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer_final->setBackgroundColor (, , );
// Coloring and visualizing target cloud (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color (target_cloud, , , );
viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
, "target cloud");
// Coloring and visualizing transformed input cloud (green).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color (output_cloud, , , );
viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
, "output cloud");
// Starting visualizer
viewer_final->addCoordinateSystem (, "global");
viewer_final->initCameraParameters ();
// Wait until visualizer window is closed.
while (!viewer_final->wasStopped ())
{
viewer_final->spinOnce ();
boost::this_thread::sleep (boost::posix_time::microseconds ());
}
}
效果：
完整代碼：https://gitee.com/AdamShan/NDT_PCL_demo
後記
在實際運作代碼我們發現我們這個demo的NDT計算比較耗時，其計算量主要集中在使用大量掃描點來優化目标函數上面。我們可以通過一些方法來提高NDT的運算效率，其中比較直接的方案就是使用GPU計算。在Autoware （https://github.com/CPFL/Autoware） 項目中，其定位子產品就是基于NDT算法實作的，其通過使用CUDA實作的
fast_pcl
package實作了對NDT優化過程的并行加速，感興趣的朋友可以關注我，我會在後續的文章中詳細讨論Autoware中的定位子產品。