前言

什么是激光雷达的运动畸变 ?
激光雷达的一帧数据是过去一周期内形成的所有数据,数据仅有一时间戳,而非某个时刻的数据,因此在这一帧时间内的激光雷达或者其载体通常会发生运动,因此,这一帧的原点不一致,会导致一些问题,这个问题就是运动畸变

所以需要去运动畸变,也叫畸变校正

如何进行运动补偿?

运动补偿的目的就是把所有的点云补偿到某一时刻,这样就可以把本身在过去100ms内收集的点云统一到一个时间点上去 这个时间点可以是起始时刻,也可以是结束时刻,也可以是中间的任意时刻

常见的是补偿到起始时刻

Pstart = T_start_current * Pcurrent

畸变校准方法
因此运动补偿需要知道每个点时刻对应的位姿 T_start_current 通常有几种做法

1 如果有高频里程计,可以比较方便的获取每个点相对起始扫描时刻的位姿

2 如果有imu,可以方便的求出每个点相对起始点的旋转

3 如果没有其它传感器,可以使用匀速模型假设,使用上一帧间里程计的结果,作为当前两帧之间的运动,同时假设当前帧也是匀速运动,也可以估计出每个点相对起始时刻的位姿
k-1 到 k 帧 和 k到k+1帧的运动是一至的,用k-1到k帧的位姿变换当做k到k+1帧的位姿变换, 可以求到k到k+1帧的每个点的位姿变换

ALOAM是使用的纯lidar的方式,所以使用的是第3种方式

Code

将帧中间的点转到起始点坐标系下

// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
{

功能函数的名字 : TransformToStart
形参 传入的指针 pi是输入点的点云地址 po是转换后的输出点的点云地址

调用的的时候按下面这种方式用就可以了

 TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //interpolation ratio
    double s;
    //由于kitti数据集上的lidar已经做过了运动补偿,因此这里就不做具体补偿了
    if (DISTORTION)
        // intensity 实数部分存的是 scan上点的 id 虚数部分存的这一点相对这一帧起始点的时间差
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;//求出了点占的时间比率
    else
        s = 1.0;  //s = 1  说明全部补偿到点云结束的时刻

s代表要转换的点在根据时间在这一帧里占的比率
SCAN_PERIOD是一帧的时间,10hz的lidar,那么周期就是0.1s
在前面的点的预处理时将 intensity 附了别的值 实数部分存的是 scan上点的 id 虚数部分存的这一点相对这一帧起始点的时间差

intensity的整体减去实数部分,就是时间差,那么除以周期,也就是时间占比了

有的lidar在内部做了去畸变,那么可以设置DISTORTION为1
则s为1,那么按照上面的定义,就是把全部补偿到点云结束的时刻

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    // 这里相当于是一个匀速模型的假设
    // 把位姿变换分解成平移和旋转  
    Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
    Eigen::Vector3d t_point_last = s * t_last_curr;

做一个匀速模型假设,即上一帧的位姿变换,就是这帧的位姿变换
以此来求出输入点坐标系到起始时刻点坐标系的位姿变换,通过上面求的时间占比s

这里把位姿变换 分解成了 旋转 + 平移 的方式

由于四元数是一个超复数,不是简单的乘法,求它的占比用的 Eigen的slerp函数(球面线性插值)

线性插值和球面线性插值的对比,如上图
两个单位四元数之间进行插值，如左图的线性插值，得到的四元数一定不是单位四元数，我们期望对于旋转的插值应该是不改变长度的，所以显然右图球面（Slerp）插值更为合理。

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    Eigen::Vector3d point(pi->x, pi->y, pi->z);//把当前点的坐标取出
    Eigen::Vector3d un_point = q_point_last * point + t_point_last;//通过旋转和平移将 当前点转到帧起始时刻坐标系下的坐标

上面有了旋转和平移,下面就简单了

把当前点的坐标取出
通过旋转和平移将 当前点转到帧起始时刻坐标系下的坐标

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //将求得的转换后的坐标赋值给输出点
    po->x = un_point.x();
    po->y = un_point.y();
    po->z = un_point.z();
    po->intensity = pi->intensity;//赋值原的intensity
}

将求得的转换后的坐标赋值给输出点
赋值原的intensity

将帧中间的点转到起始点坐标系下

这个是通过反变换的思想,首先把点统一到起始时刻坐标系下,再通过反变换,得到结束时刻坐标系下的点

void TransformToEnd(PointType const *const pi, PointType *const po)
{

功能函数的名字 : TransformToEnd
形参 传入的指针 pi是输入点的点云地址 po是转换后的输出点的点云地址

调用的的时候按下面这种方式用就可以了

TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    // 首先先做畸变校正,都转到起始时刻
    pcl::PointXYZI un_point_tmp;//转到帧起始时刻坐标系下的点
    //先统一到起始时刻
    TransformToStart(pi, &un_point_tmp);

首先先做畸变校正,都转到起始时刻
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //再 通过反变换的方式 将起始时刻坐标系下的点 转到 结束时刻坐标系下 
    Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);//取出起始时刻坐标系下的点
    //q_last_curr  \ t_last_curr 是结束时刻坐标系转到起始时刻坐标系 的 旋转 和 平移  
    Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);//通过反变换,求得转到 结束时刻坐标系下 的点坐标

q_last_curr \ t_last_curr 是结束时刻坐标系转到起始时刻坐标系 的 旋转 和 平移

代码的公式可以推导下:

两边乘 Re-s的逆

合并下

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //将求得的转换后的坐标赋值给输出点
    po->x = point_end.x();
    po->y = point_end.y();
    po->z = point_end.z();

    //移除掉 intensity 相对时间的信息
    po->intensity = int(pi->intensity);
}

将求得的转换后的坐标赋值给输出点
移除掉 intensity 相对时间的信息
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

整体代码:

// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
{
    //interpolation ratio
    double s;
    //由于kitti数据集上的lidar已经做过了运动补偿,因此这里就不做具体补偿了
    if (DISTORTION)
        // intensity 实数部分存的是 scan上点的 id 虚数部分存的这一点相对这一帧起始点的时间差
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;//求出了点占的时间比率
    else
        s = 1.0;  //s = 1  说明全部补偿到点云结束的时刻
    //s = 1;
    //所有点的操作方式都是一致的,相当于从结束时刻补偿到起始时刻
    // 这里相当于是一个匀速模型的假设
    // 把位姿变换分解成平移和旋转  
    Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);//求姿态的球面插值
    Eigen::Vector3d t_point_last = s * t_last_curr;//求位移的线性插值
    Eigen::Vector3d point(pi->x, pi->y, pi->z);//把当前点的坐标取出
    Eigen::Vector3d un_point = q_point_last * point + t_point_last;//通过旋转和平移将 当前点转到帧起始时刻坐标系下的坐标

    //将求得的转换后的坐标赋值给输出点
    po->x = un_point.x();
    po->y = un_point.y();
    po->z = un_point.z();
    po->intensity = pi->intensity;//赋值原的intensity
}

// transform all lidar points to the start of the next frame

void TransformToEnd(PointType const *const pi, PointType *const po)
{
    // 首先先做畸变校正,都转到起始时刻
    pcl::PointXYZI un_point_tmp;//转到帧起始时刻坐标系下的点
    //先统一到起始时刻
    TransformToStart(pi, &un_point_tmp);

    //再 通过反变换的方式 将起始时刻坐标系下的点 转到 结束时刻坐标系下 
    Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);//取出起始时刻坐标系下的点
    //q_last_curr  \ t_last_curr 是结束时刻坐标系转到起始时刻坐标系 的 旋转 和 平移  
    Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);//通过反变换,求得转到 结束时刻坐标系下 的点坐标

    //将求得的转换后的坐标赋值给输出点
    po->x = point_end.x();
    po->y = point_end.y();
    po->z = point_end.z();

    //移除掉 intensity 相对时间的信息
    po->intensity = int(pi->intensity);
}

以上是 激光雷达的运动畸变以及补偿方式 的代码解析