0
点赞
收藏
分享

微信扫一扫

激光雷达去畸变原理

夏沐沐 2022-04-16 阅读 94
机器学习

激光雷达去畸变原理
相比较张楫博士激光雷达里程计开山之作的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);
}

以上是aloam 秦通博士的代码

代码的核心思路:
在这里插入图片描述

举报

相关推荐

0 条评论