激光雷达去畸变原理

hshdeeping

已于 2022-04-16 15:47:56 修改

阅读量1k

点赞数 2

分类专栏： slam 文章标签：机器学习

于 2022-04-16 15:41:03 首次发布

本文链接：https://blog.csdn.net/Harveyhu/article/details/124214268

版权

slam 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

激光雷达去畸变原理
相比较张楫博士激光雷达里程计开山之作的loam，它的去畸变原理是直接把eigen里面的球面插值方法进行了手动推理，但是loam就直接用了eigen库，相比较而言可能会降低一点点的速度，但是影响不大，代码又不那么复杂了
这块的本质简单，就好像论文里说的：我简化下，就是雷达在运动，打出的激光也是同一时间的打出返回，这样大家想想如果雷达不动，是不是点云的状态是很好的，但是雷达一动，点云肯能倾斜了，就不像面前的那个物体了，就是下图的Pk+1这样，我们要把它还原成pk一拔这个。于是就有了必须要去畸变的说法，再加上基本是10HZ-100HZ的激光雷达帧率。

就好像这个现象：如下照片
在这里插入图片描述

以下是代码实现
在这里插入图片描述

以上是 loam 张楫博士手推球面插值公式

// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
{
    //interpolation ratio
    double s;
    if (DISTORTION)
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
    else
        s = 1.0;
    //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;
}

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

void TransformToEnd(PointType const *const pi, PointType *const po)
{
    // undistort point first
    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);
    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();

    //Remove distortion time info
    po->intensity = int(pi->intensity);
}