Apollo自動駕駛創建點雲地圖流程分析

前言

Apollo爲了實現點雲與慣性導航的融合定位這個最終的功能，開放了一系列的工具，包括點雲數據過濾，與GNSS的時間對齊，生成點雲地圖，使用點雲地圖與Ins慣性導航系統做融合定位，可視化定位效果，以及對最終的定位結果的評估等等這一些列的工具套件，從整體上來看，這些工具可以分爲兩個大類別，一是創建點雲地圖，二是使用點雲地圖與Ins(慣性導航系統)做融合定位。

創建點雲地圖

下圖描述了創建點雲地圖的主要流程

創建點雲地圖是進行點雲融合定位的第一步，作爲先驗性的地圖信息參考。
要想創建一個點雲地圖，需要兩個重要的信息，一個是點雲信息，另一個是定位信息。
點雲信息，來自激光LiDAR，是對環境掃描的點雲信息， 包括XYZIT（X,Y,Z,強度,時間戳）， 是相對於車身的相對座標。
定位信息，來自全球定位導航系統(GNSS)，GNSS融合了GPS+RTK+IMU 的數據，能夠精準定位（精度在10cm以內）， 包括Position（位置），Orientation(姿態)和Vel（速度）信息，在創建點雲地圖時只需要Position和Orientation 信息。
最終點雲地圖中的每一個點，是包含了準確的定位信息的點，所以，點雲地圖實際上就是對點雲數據與定位數據的一個融合的結果。
接下來我們就來深入的探討一下，這個融合過程是怎麼實現的。

數據採集與解析

首先我們來看數據提取與解析。

    輸入數據：使用Cyber_record 錄製的數據包
    輸出數據： 
		LiDAR 數據
		odometry 數據

要創建某個區域的地圖，首先需要對這個區域進行數據採集，主要採集LiDAR和GNSS數據，可以是用cyber_record庫對這兩個topic消息進行解析，對於LiDAR點雲數據，我們需要使用PCL庫將每一幀的點雲數據其轉換成PCD文件並保存，同時也要記錄每一幀的索引文件，每行記錄的內容是：index（pcd索引值）, timestamp(時間戳), 保存到索引文件pcd_timestamp.txt。
對於Odometry定位數據，也同樣提取一份索引文件，沒行內容爲： index(位置索引), timestamp（時間戳）,位置(
x, y, z) , 姿態( qx, qy, qz, qw) ,方差( std_x, std_y, std_z) , 並保存至文件odometry_loc.txt。
此外，在此次解析過程中，還有三個輸出，分別是gnss_loc.txt， lidar_loc.txt和fusion_loc.txt， 這三個文件分別保存融合後的數據輸出，所以，在第一次創建時，都爲空。
有了pcd_timestamp.txt 和odometry_loc.txt 這兩個索引文件以及所有的點雲pcd數據，就可以做點雲與GNSS時間對齊了。

點雲數據與GNSS定位融合

點雲數據是由若干幀組成的，我們的激光雷達採集頻率是10Hz，即每秒10幀，每一幀是由若干個離散的點組成，目前每一幀原始點大概10萬個點左右。爲了能夠方便有效的對這些點進行計算，通常採用pcd格式的文件對點雲數據進行存儲，每一幀存儲爲一個pcd文件。
而在算法層面，知行者中，對於點雲的處理主要依賴於PCL庫，這個庫提供了大部分公開的點雲處理算法，在融合過程中使用的關鍵性的算法，均出自PCL庫。
地理位置信息和點雲數據是相對獨立的，要想把它們融合在一起，就需要找到它們的關聯點，唯一能把它們關聯起來的，只有時間戳，莫急，我們一步一步往下看。
時間對齊與座標轉換插值

輸入： 
    GNSS定位數據索引文件
    點雲pcd數據
    激光雷達外參
輸出： 
    時間對齊後的位置

GNSS定位信息的更新頻率是100Hz，點雲的更新頻率是10Hz，顯然不在一個頻段上。也就是說，GNSS在各個時刻與點雲的各個時刻是沒有重合點的，要想獲取激光雷達在某一時刻的位置，必須先要將激光雷達與定位系統做時間對齊，找到激光雷達距離定位系統最近的時間點，在通過激光雷達附近的定位點做插值計算，最終得到激光點雲所在的位置信息。

我們先來看下準備工作：

	遍歷 odometry_loc.txt 定位文件{
		對Pos(x,y,z)做座標平移+旋轉變換得到仿射矩陣
		獲取GNSS位置的時間戳作爲對齊參考時間 和 方差
 		std::vector<Eigen::Affine3d>* poses
		...
		Eigen::Translation3d trans(Eigen::Vector3d(x, y, z));
      		Eigen::Quaterniond quat(qr, qx, qy, qz);
      		poses->push_back(trans * quat);
      		timestamps→push_back(timestamp);

 		Eigen::Vector3d std;
      		std << std_x, std_y, std_z;
      		stds->push_back(std);
	}

這段簡單的處理無疑是必要的準備，它可以得到任意時刻每個點odometry(車所在的位置)的仿射變換矩陣，那麼，之後我們只要獲取對應時刻的點雲局部座標值，乘以這一時刻的仿射矩陣，就可以把點雲座標轉換到WGS84座標系下，OK:-)

具體到算法上主要有以下幾個步驟，

一是, 在座標平移的方向根據時間戳做線性插值，即找到距離第一幀點雲數據最近並且在點雲時間之前的定位信息時間，也就是保證第一幀pcd的時間在兩個連續的pos時間之間, 得到線性插值係數 delta_t = (t – t0)/(t1-t0),  
(
輸入位置： odometry， 
輸入位置時間戳: odometry pos timestamps
參考時間戳：pcd的時間戳,
double t = (cur_timestamp - ref_timestamp) / (cur_timestamp – pre_timestamp)
)
根據線性插值係數delta_t , 即可算出座標平移x,y,z三個方向上的座標值，

        Eigen::Translation3d re_transd;
        re_transd.x() = pre_transd.x() * t + cur_transd.x() * (1 - t);
        re_transd.y() = pre_transd.y() * t + cur_transd.y() * (1 - t);
        re_transd.z() = pre_transd.z() * t + cur_transd.z() * (1 - t);

二是，在座標旋轉方向上做插值計算，爲了實現平滑插值的效果，使用的是四元數球面線性插值(slerp)算法, 具體是 
Eigen::Quaterniond res_quatd = pre_quatd.slerp(1 - t, cur_quatd); 

函數來實現的，

out_poses->push_back(re_transd * res_quatd);
…
Eigen::Affine3d pose_tem = out_poses_[i] * velodyne_extrinsic_;

通過時間對齊之後，對點雲的每一幀進行座標平移、旋轉插值變換，再補上激光雷達的外參，就得到了每一幀點雲的pcd在某一時刻的基礎仿射變換矩陣，之後就可以從pcd中獲取當前幀的每一點的位姿信息，乘以這個放射轉換矩陣，最終將點雲座標轉換到車的全局座標系下(WGS84)。
做好每一幀點雲時間對齊之後的索引數據保存至corrected_poses.txt 文件中，內容包括： 幀索引(indexes), 幀時間戳( timestamp), 位置座標(transd.x() transd.y() transd.z()), 姿態四元數( qx qy qz qr )。

創建點雲地圖

至此，我們準備好了以下幾個數據：
點雲的每一幀的pcd文件
點雲幀索引文件corrected_poses.txt，包含了每一幀索引和當前幀的精確base定位信息
激光雷達的外參

有了這兩個數據，我們現在就可以創建地圖了。
分爲無損地圖和有損地圖兩個部分。

創建無損地圖

輸入：
		pcd文件路徑
		點雲索引文件
輸出： 
		無損地圖配置文件
		無損地圖

總結起來，創建地圖也只有兩個關鍵的動作
一是點雲濾波
二是點雲特徵平面分割

點雲濾波，使用的是體素網格濾波(VoxelGrid)，簡單說來的就是將每一幀的點雲空間劃分成若干個小空間，每個小空間包含了一定數量的點，最終計算出這些點的重心點來代表這個小空間，其他的點被過濾掉，從而實現濾波的作用。此處使用的具體算法是PCL::VoxelGrid算法。
特徵平面分割，知行者中，採用的是採樣一致(Ransac)算法，通過提取點雲特徵平面信息，並結合點雲的平均高程信息，最終確認點雲的地面特徵，至此，根據過濾後的結果可以保存此地圖數據，爲無損地圖(lossless map)。

對每一幀做過濾波之後的點雲數據被放入到無損地圖隊列中，等待保存處理。

那麼點雲地圖又是怎麼存儲這些點雲數據的呢，實際上就是做了一個空間上的映射，
首先點雲地圖使用Node節點來作爲管理單元，可以根據當前需要創建點雲的總數來創建Node節點總數，然後根據每個點的座標值來計算Node的索引值，這樣就建立起點雲與存儲之間的映射關係了。

最後每個點雲數據存放在 LosslessMapMatrix 節點中，但此時還沒完，因爲對於水平位置x,y和高程z是分開來處理的，高程值z與點雲強度值是按照給定的閾值範圍內分層次來保存的，也就是說不同範圍的高程數據保存在點雲地圖的不同層上。

而對於有損地圖，就是在每個點雲地圖的Node Cell中，獲取平均高程和灰度的方差值來代表當前cell的數據。

至此，點雲的有損地圖創建完成，也就代表點雲地圖的創建完成。