一、ndt mapping源碼解析

0）基礎(chǔ)知識：正態(tài)分布變換算法NDT

NDT，Normal Distributions Transform正態(tài)分布變換算法的簡稱，其是一種統(tǒng)計學模型。如果一組隨機向量滿足正態(tài)分布，那么它的概率密度函數(shù)為：
$p(x)=\frac{1}{(2\pi)^{\frac{D}{2}} \sqrt{\Sigma}} e^{-\frac{(x-\vec \mu)^T \Sigma^{-1} (x-\vec \mu)}{2 \sigma^2}}$

其中D表示維度， $\vec \mu$ 表示均值向量， $\Sigma$ 表示隨機向量的協(xié)方差矩陣。由于掃描得到的激光點云數(shù)據(jù)點是三維空間點坐標，所以需要采用三維正態(tài)分布。NDT能夠通過概率的形式描述點云的分部情況，這有利于減少配準所需要的時間。

正態(tài)分布變換NDT算法的流程

1）網(wǎng)格化目標點云
首先需要將目標點云網(wǎng)格化grid。點云配準一般是對兩個點云數(shù)據(jù)進行兩兩配準，需要先固定一個點云數(shù)據(jù)，另外一個點云數(shù)據(jù)再通過旋轉(zhuǎn)平移來和固定點云進行匹配拼接。這里的固定點云就是目標點云，平移旋轉(zhuǎn)的點云就是源點云。網(wǎng)格化目標點云主要是利用立方體將激光點云所在空間進行均勻劃分，使得激光點處于相應(yīng)的立方體中，這一步作為NDT算法的第一步非常重要。
2）注冊激光點云數(shù)據(jù)
目標點云劃分網(wǎng)格后，將點云加載到網(wǎng)格內(nèi)，首先計算均值向量
$\vec \mu = \frac{1}{m} \sum_{k=1}^{m} \vec y_k$
其中 $\vec y_{k=1,2,3,...,m}$ 表示網(wǎng)格中所有掃描點的坐標。
然后，計算協(xié)方差矩陣：
$\Sigma \frac{1}{m} \sum_{k=1}^m (\vec y_k-\vec \mu)(\vec y_k - \vec \mu)^T$
最后，求出每個網(wǎng)格內(nèi)的正態(tài)分布概率密度函數(shù)：
$p(\vec x) = \frac{1}{2^{\frac{3}{2}}\sqrt{|\Sigma|}} e^{-\frac{(\vec x - \vec \mu)^T \Sigma^{-1}((\vec x - \vec \mu))}{2}}$
由于需要用到協(xié)方差矩陣的逆矩陣，所以每個格子中激光點不可少于三個，一般至少要保證有五個點。
3）求出源點云相對于目標點云的初始坐標變換參數(shù)
坐標變換通常涉及到平移與旋轉(zhuǎn)，平移通過平移向量表示，旋轉(zhuǎn)則可以通過旋轉(zhuǎn)矩陣表示，旋轉(zhuǎn)是關(guān)于自身zyx三個固定軸的旋轉(zhuǎn)，轉(zhuǎn)角分別用 $\alpha$ ， $\beta$ ， $\gamma$ 表示，分別與yaw，pitch，roll對應(yīng)。

z	y	x
yaw	pitch	roll
$\alpha$	$\beta$	$\gamma$

旋轉(zhuǎn)矩陣R可以表示為： $R_{3 \times 3} = [\delta, \epsilon,\eta]$ 。
其中
$\delta=\left[ \begin{matrix} cos \beta cos \alpha \\ sin \alpha cos \beta \\ -sin \beta \end{matrix} \right]$
$\epsilon=\left[ \begin{matrix} cos \alpha sin \beta sin \gamma - sin \alpha cos \gamma \\ cos \alpha cos \gamma + sin \alpha sin \beta sin \gamma\\ cos \beta sin \gamma \end{matrix} \right]$
$\eta=\left[ \begin{matrix} cos \alpha sin \beta cos \gamma + sin \alpha sin \gamma \\ sin \alpha sin \beta cos \gamma - cos \alpha sin \gamma\\ cos \gamma cos \beta \end{matrix} \right]$

平移向量t為：
$t = [t_x,t_y,t_z]^T$

假設(shè)某一個激光點在兩個點云坐標系下的位置坐標分別為：
$X = \left[ \begin{matrix} x_i \\ y_i \\ z_i \end{matrix} \right]$
$X' = \left[ \begin{matrix} x'_i \\ y'_i \\ z'_i \end{matrix} \right]$

那么通過坐標變換公式：
$X' = R_{3 \times 3} X + t_{3 \times 1} = TX$
可以得到同一個激光點云在這兩個點云坐標系下的位置坐標之間的關(guān)系，其中T表示坐標變換參數(shù)變換。

這一步是為了尋找一個合適的初始坐標變換使得源點云大致處于目標點云的坐標平面當中。這一步提供的變換參數(shù)的初值，為下一步變換參數(shù)的迭代提供距離最優(yōu)點較近的初值。在自動駕駛里初始值的提供可以依靠GNSS、Odom或者IMU慣性導航，利用這些傳感器獲取車輛的當前位姿，通過坐標變換得到相對于目標點云的坐標變換參數(shù)，也就會位移t與旋轉(zhuǎn)R。單純依靠里程計或者IMU會產(chǎn)生累計誤差，Autoware利用里程計計算速度確定位置，利用IMU確定姿態(tài)的變化。

4）源點云進行初始坐標變換，并計算在目標點云網(wǎng)格中的概率
源點云根據(jù)初始變換參數(shù)將坐標轉(zhuǎn)換到目標點云中，此時源點云會分布在目標點云網(wǎng)格中，然后轉(zhuǎn)換后的源點云的坐標 $X'$ ，由對應(yīng)所在的網(wǎng)格的正態(tài)分布概率密度函數(shù)，求出激光點坐標為 $X'$ 的概率。
5）求出最大目標似然函數(shù)
將每個點的概率乘積起來作為目標似然函數(shù)。通過似然函數(shù)找到概率乘積最大時候的坐標轉(zhuǎn)換關(guān)系。簡單來說就是由最大的概率找到最優(yōu)的坐標變換。
$\psi = \prod_{k=1}^n p(X')$
此時需要將最大似然函數(shù)做一個變換，最優(yōu)化問題涉及求解一階導數(shù)和二階導數(shù)，乘積的形式不容易求導，因此將它轉(zhuǎn)換為負對數(shù)的形式，就能轉(zhuǎn)換成累加的形式，方便求導的操作了。
$- \log \psi = - \sum_{k=1}^n \log (p(X'))$
6）利用高斯牛頓法進行優(yōu)化，找出最佳變換參數(shù)p完成點云配準
然后利用高斯牛頓法進行求解優(yōu)化目標函數(shù) $- \log \psi = - \sum_{k=1}^n \log (p(X'))$ 的核心是求解 $H\Delta\vec p = -\vec g$ ，其中H是目標函數(shù)的海森矩陣，g是目標函數(shù)的梯度向量。迭代更新公式為 $T = T + \Delta T$ ，其迭代參數(shù)為坐標變換矩陣T

1）Autoware中的定位模塊

Autoware的定位模塊分為基于GNSS定位以及基于激光雷達點云數(shù)據(jù)定位兩個部分。

而基于激光點云數(shù)據(jù)的定位采用NDT Matching配準算法得到自身在激光點云地圖中的位姿信息，其中激光點云地圖由車載激光雷達前期掃描行車環(huán)境得到的激光點云數(shù)據(jù)壓縮拼接而成（NDT Mapping）。

NDT Matching配準的時候，需要手動設(shè)置初始位置，因此Autoware需要借助GNSS定位對其進行初始化，同時基于激光點云數(shù)據(jù)的定位結(jié)果進行修正。

1）點云配準原理

在激光點云地圖建圖過程中，由于激光雷達掃描距離存在限制，一次掃描難以獲取完整的目標環(huán)境，并且距離激光雷達越遠，點云就會變得越稀疏，所以需要經(jīng)過連續(xù)多次掃描，然后將每次掃描的點云數(shù)據(jù)進行配準拼接，最終才能形成連續(xù)完整的激光點云地圖。從不同角度掃描同一場景所得到的的點云數(shù)據(jù)統(tǒng)一轉(zhuǎn)換到同一坐標系的過程叫做點云配準。簡單地說就是將離散的點云數(shù)據(jù)在統(tǒng)一的坐標系下拼接成一整個完整的點云數(shù)據(jù)。通常點云配準算法能夠利用兩個點集之間的最小距離或者利用統(tǒng)計學方法，得到兩個點集之間的變換關(guān)系，使得點云達到變換配準的效果，問題關(guān)鍵在于如何得到激光點云之間的坐標變換矩陣T，也就是得到旋轉(zhuǎn)矩陣R以及平移向量t。通?？梢灾苯永肞CL開源點云庫來對相關(guān)點云數(shù)據(jù)進行處理，PCL中包含了基于NDT正態(tài)分布變換等多種點云配準算法。

ndt_mapping主要利用的是scan_to_map的方式實現(xiàn)激光點云建圖，該方法經(jīng)常使用在SLAM同時定位與建圖中，當激光雷達進行掃描建圖的時候，由于受到掃描距離等因素的限制，使得激光雷達不能一次掃描得到完整的環(huán)境地圖，因此需要進行連續(xù)多次的掃描。

最終掃描得到的整個環(huán)境地圖就是全局地圖map，而其中scan表示當前掃描得到的激光點云數(shù)據(jù)，可以通過固定目標點云地圖submap，然后利用NDT配準算法，將每一幀掃描得到的激光點云數(shù)據(jù)scan變換到目標點云submap中，并使得兩者拼接在一起，最終得到拼接完整的全局地圖map。

具體的步驟如下：
1.激光雷達掃描得到的激光點云數(shù)據(jù)需要去除距離車體較近與較遠的激光點集，然后利用體素濾波過濾剩下的激光點云數(shù)據(jù)，在保持點云統(tǒng)計特征的情況下，降低激光點云數(shù)據(jù)集的尺寸大小，最好將降采樣后的過濾點云作為NDT配準算法的輸入源點云。

2.第一幀激光點云作為全局地圖加載成功，將其作為NDT配準的輸入目標點云。

3.為了快速得到準確的配準結(jié)果，需要給NDT算法提供良好的初始值，該節(jié)點通過IMU、Odom以及兩者聯(lián)合來求得初始位姿估計。

4.將前三者得到的結(jié)果作為參數(shù)傳入到NDT配準算法中進行激光點云配準。

5.最后將1中過濾得到的點云數(shù)據(jù)變換到全局地圖map中，進行地圖更新。

2）源碼解析

根據(jù)圖形化界面，找到Autoware源碼中Computing界面對應(yīng)的配置文件computing.yaml，位置在autoware\src\autoware\utilities\runtime_manager\scripts
打開computing.yaml文件，可以看到清晰的autoware圖形化界面的布局以及控件對應(yīng)執(zhí)行的指令。
根據(jù)computing.yaml中的內(nèi)容，當勾選了ndt_mapping進行建圖的時候，會去cmd運行l(wèi)idar_localizer中的ndt_mapping.launch文件。

ndt_mapping.launch文件如下：

<!-- -->
<launch>
  <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 --> 
  <arg name="method_type" default="0" /> <!-- 默認使用CPU -->
  <arg name="use_odom" default="false" /> <!-- 默認不使用里程計 -->
  <arg name="use_imu" default="false" /> <!-- 默認不使用IMU -->
  <arg name="imu_upside_down" default="false" /> <!-- 默認不進行IMU方向正負變換 -->
  <arg name="imu_topic" default="/imu_raw" />
  <arg name="incremental_voxel_update" default="false" />

  <!-- rosrun lidar_localizer ndt_mapping  -->
  <node pkg="lidar_localizer" type="queue_counter" name="queue_counter" output="screen"/>
  <node pkg="lidar_localizer" type="ndt_mapping" name="ndt_mapping" output="screen">
    <param name="method_type" value="$(arg method_type)" />
    <param name="use_imu" value="$(arg use_imu)" />
    <param name="use_odom" value="$(arg use_odom)" />
    <param name="imu_upside_down" value="$(arg imu_upside_down)" />
    <param name="imu_topic" value="$(arg imu_topic)" />
    <param name="incremental_voxel_update" value="$(arg incremental_voxel_update)" />
  </node>

</launch>

根據(jù)

二、ndt mapping園區(qū)實戰(zhàn)

1）啟動autoware

source ~/Projects/Autoware-All/install/setup.bash
roslaunch runtime_manager runtime_manager.launch

2）切換到Simulation界面

加載rosbag，從零開始不循環(huán)播放，進度到1%的時候，點擊Pause暫停

3）切換到Setup界面

直接點擊Vehicle Model加載黑色的車輛模型，然后點擊TF，加載從車身底盤坐標系到激光雷達坐標系之間的坐標轉(zhuǎn)換關(guān)系，tf_x，tf_y，tf_z是兩者之間的相對位姿關(guān)系。

4）切換到Computing界面

點擊ndt_mapping的app，設(shè)置ndt 的配準參數(shù)，暫時可以使用其默認值即可
設(shè)置完畢參數(shù)后，勾選ndt_mapping

5）回到Simulation界面

點擊Pause，取消暫停，繼續(xù)播放ROSBAG數(shù)據(jù)包，開始NDT 配準建圖，直到結(jié)束為止，可以在終端看到建圖的過程：

也可以在rviz中看到實時建圖的過程（當ROSBAG數(shù)據(jù)包太大時候，打開rviz可視化會非常的卡，fps基本為0，卡的動不了，實際路測的時候，可以關(guān)閉）

等待進度條走到100%ROSBAG數(shù)據(jù)包播放完畢

6）保存全局地圖成pcd文件

當ndt_mapping建圖完畢后，切回到computing界面，再次點擊ndt_mapping右側(cè)的app按鈕，在下方輸入要保存建好的全局地圖的pcd文件的路徑，然后點擊PCD OUTPUT完成點云地圖的保存。

ndt matching點云定位

在經(jīng)過ndt mapping建圖過程后，得到了rosbag包的完整全局點云地圖pcd文件，于是可以通過ndt matching進行點云定位

1）再次播放ROSBAG數(shù)據(jù)包

切換界面到Simulation，從頭開始Play播放數(shù)據(jù)包，1%進度后按暫停

2）加載車輛模型以及車身地盤坐標系到激光雷達坐標系的變換關(guān)系tf

在Setup界面點擊Baselink to Localizer下的TF按鈕，可以使用其默認值，來設(shè)置車身底盤坐標系到激光雷達傳感器坐標系之間的相對位姿關(guān)系。并且點擊Vehicle Model加載車輛模型。

3）加載世界坐標系到全局地圖坐標系之間的變換關(guān)系tf_local

首先在本地某處新建一個tf_local.launch文件，然后在其中寫入如下內(nèi)容用于表示車身世界坐標系到全局地圖的位姿變換關(guān)系（一般世界坐標系world與地圖坐標系是重合的）：

<launch>

    <node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map 10" />

</launch>

仍然在Map界面的下方，點擊 TF右側(cè)ref，加載剛才新建的tf_local.launch文件

4）加載全局地圖

切換界面到Map界面，點擊Point Cloud的Ref按鈕，加載剛才構(gòu)建好的點云地圖pcd文件，點擊Point Cloud按鈕后，知道進度條變?yōu)?00%，右側(cè)出現(xiàn)OK表示加載完畢

5）設(shè)置體素濾波進行點云降采樣（核心思想是將網(wǎng)格voxel內(nèi)的點云數(shù)據(jù)用其質(zhì)心來近似表達）

切換到Sensing界面，點擊Points Downsampler中的voxel_grid_filter右側(cè)的app設(shè)置體素濾波的參數(shù)（可使用默認的參數(shù)值，voxel_leaf_size表示立方體網(wǎng)格的邊長，measurement表示點云的有效距離）

6）進行ndt_matching點云配準定位

切換到Computing界面，點擊Lidar_localizer下的ndt_matching右側(cè)的app對點云配準進行參數(shù)設(shè)置，勾選Initial Pose（默認是單選在GNSS），來設(shè)置激光點云定位前車輛的初始位置，假設(shè)存在GNSS數(shù)據(jù)，則選擇GNSS，采用GNSS作為初始位置數(shù)據(jù)輸入。確定后，勾選ndt_matching按鈕。