任务动机：梳理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中对激光雷达运动畸变的处理方法分析