任務動機：梳理cartographer處理激光雷達運動畸變的原理，並針對特殊數據特性的雷達數據作相應適配，進而提高建圖效果。算法

任務描述：查閱cartographer源碼中激光雷達運動畸變的處理流程，對比分析實際使用中特定激光雷達數據處理時可能產生的問題。針對於不一樣的實際問題做相應修改後，進行對比建圖實驗驗證。後端

1. 激光雷達運動畸變的緣由

一幀激光點雲由多個激光點組成，而這些激光點不是同一時刻產生的，因此在雷達運動過程當中，一幀內的各個激光點基準不一樣，就會產生運動畸變。例如：當機器人旋轉速度爲1.57rad/s，此時雷達若頻率爲10hz，那麼一幀激光點內的首尾兩點基準會相差1.57 * 0.1 =0.157rad，即約爲9°，在點雲幀間匹配時該偏差不能忽視。優化

2. Cartographer中對雷達運動畸變的處理

Cartographer中對於激光雷達的運動補償大體爲如下流程，具體細節可從代碼（pose_extrapolator）中查看。spa

對於一幀激光數據中的每一點都賦予對應的時間戳；
假設勻速運動模型，經過前面幀間匹配的位移除以時間間隔求出速度；
根據每個激光點對應的時間戳進行運動補償。

3. Cartographer處理運動畸變中可能出現的問題

Cartographer中處理雷達點雲運動畸變時已經默認拿到的雷達數據已經足夠準確，使用雷達驅動中發出的time_increament直接對單幀內的沒一點賦值時間戳。而現實中激光雷達因爲硬件設備問題，會出現兩種異常狀況：調試

3.1 問題一

雷達點雲打包發出的順序與真實電機旋轉掃描時的數據正好相反，對於cartographer計算過程當中每一個點對應時間戳剛好相反。即處理運動畸變時第一個激光點被假設成了最後一個激光點，結果拔苗助長。code

3.2 問題二

激光雷達的頻率不穩定，例如官方標定爲10HZ，可是實際運行中數據間隔可能徘徊與0.08-0.12s之間，只能保證平均間隔爲0.1s，但在cartographer中收到的time_increament多數狀況下是固定的，導致處理運動畸變時，單幀激光數據中的每一個點沒法對應其準確的時間戳，例以下面幾個雷達數據（rostopic記錄）連續七幀時間戳的對比。blog

	雷達一	雷達二	雷達三
掃描頻率	15hz	13hz	28hz
理論幀間間隔（s）	0.066667	0.076923	0.03571
時間戳1（stamp.nsecs）	138875719	217470838	574852851
時間戳2（stamp.nsecs）	205275613	297503257	611164101
時間戳3（stamp.nsecs）	271705863	369562690	646372642
時間戳4（stamp.nsecs）	338044979	449569886	681654101
時間戳5（stamp.nsecs）	404755814	529576631	717150226
時間戳6（stamp.nsecs）	471542889	601488252	752839434
時間戳7（stamp.nsecs）	538550579	681515123	788386892

能夠看出，雷達一數據間隔爲0.066四、0.0664三、0.0663四、0.0667一、0.0667九、0.06701，雷達二時間幀間間隔爲0.08003s、0.07206s、0.08s、0.07192s、0.08003s，雷達三數據間隔爲0.03631s、0.03521s、0.0352s、0.0355s、0.0356九、0.03555s，相對於雷達一和雷達三，雷達二數據波動更爲明顯。對於這種波動，從數據源上影響cartographer建圖效果。索引

4. 問題解決方法與實驗對比

4.1 問題一解決方法

能夠從硬件驅動層面上來處理，經過雷達廠家上位機軟件或者驅動程序使雷達輸出反序。若是不便於進行上述處理，對於問題1從算法層面修改，能夠從rostopic讀到的原始數據調整。cartographer中的對於每個激光點point都帶有一個時間戳，其計算方式爲每一幀從0開始，計算方式爲i * msg.time_increment，其中i爲一幀激光點中的點索引，msg.time_increment爲相鄰兩激光點間時間增量。詳細代碼可在cartographer_ros_extended-master/cartographer_ros/cartographer_ros/msg_conversion.cc中對於laserscan數據處理中可查到。從rostopic中讀到的msg.time_increment多爲固定值，不能適應於頻率波動狀況。能夠修改msg_conversion.cc中的代碼以下：rem

angle += msg.angle_increment;
}
/********************** Adjust timestamp for point reverse ********************/
std::vector<float>time_tempx;
std::vector<float>time_tempy;
for (auto& point :point_cloud.points) {
    time_tempx.push_back(point.position.x());
    time_tempy.push_back(point.position.y());
}
int i_time = 0;
for (auto& point :point_cloud.points) {
    point.position.x() =  time_tempx[point_cloud.points.size()-1-i_time];
    point.position.y() =  time_tempy[point_cloud.points.size()-1-i_time];
    i_time++;
}
/********************** end *********************/ 
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);

對應修改如上代碼，可解決數據反序問題。以下兩圖爲未調整反序的建圖效果（後端優化關閉）源碼

點雲反序調整後建圖效果（其他參數同樣）

第一幅對比圖爲雷達原地旋轉情形，能夠看出爲調序的激光旋轉一圈後地圖出現明顯重影，調整點雲反序後原地旋轉一圈激光與地圖基本可以對齊。第二幅地圖爲使用雷達創建的一個辦公室場景（無迴環），能夠看出在調整點雲反序後建圖效果有明顯提高。

4.2 問題二解決方法

因爲雷達在實際運行中頻率不穩定致使建圖受到影響，主要緣由是多數雷達驅動發過來的topic中的time_increament是一個固定的值，而真實值卻實時變化。解決方法爲使用連續兩幀之間的時間戳差值求出的實時time_increament代替原topic收到的固定值。修改msg_conversion.cc代碼以下。

PointCloudWithIntensities point_cloud;
float angle = msg.angle_min;
/************Adjust timestamp for point frequency *********/
static double timestamp_current = 0 ;
static double timestamp_last = 0 ;
double time_increment_vary;
timestamp_last = timestamp_current;
timestamp_current = msg.header.stamp.toSec();
if(timestamp_last != 0) {
    time_increment_vary = (timestamp_current - timestamp_last)/(6.2831852/msg.angle_increment);
}
else time_increment_vary = msg.time_increment;
/************ end *********/
for (size_t i = 0; i < msg.ranges.size(); ++i) {
    const auto& echoes = msg.ranges[i];

使用求解出的time_increment_vary代替源碼中的msg.time_increment 以下

const cartographer::sensor::TimedRangefinderPoint point {
    rotation * (first_echo * Eigen::Vector3f::UnitX()), i * time_increment_vary
};//i * msg.time_increment
point_cloud.points.push_back(point);
if (msg.intensities.size() > 0)

以下圖爲使用某雷達未調整實時頻率建圖效果

調整實時頻率建圖效果

能夠看出長方形邊有細微差異，調整實時頻率後的建圖邊緣較爲光滑，無明顯重影。對於不一樣的雷達建圖可能表現不一樣，某些雷達自身輸出的頻率較爲穩定，波動不明顯，則無需實時校準幀間時間間隔。對於幀間頻率波動較大的激光雷達，建圖效果會有提高。

5. 總結

因爲雷達自身硬件與數據輸出問題致使建圖受到影響，尤爲是時間戳間隔的問題，每每會疏於考慮。博主也是在調試時偶然發現，這裏進行梳理與總結，拋磚引玉，針對於問題二也須要更多款型的雷達進行試驗，文中若有不妥之處，還望指正。若是以前沒注意到這一點，調參很久也沒有獲得理想的地圖，那麼，快去查看本身使用的雷達的頻率吧！

Cartographer中對激光雷達運動畸變的處理方法分析