fastlio点云去畸变问题2

Son_of_the_Bronx

已于 2024-03-15 20:05:33 修改

阅读量1k

点赞数 17

分类专栏：激光slam 文章标签： c++

于 2024-03-06 16:21:39 首次发布

本文链接：https://blog.csdn.net/Son_of_the_Bronx/article/details/136508377

版权

激光slam 专栏收录该内容

5 篇文章 1 订阅

订阅专栏

主要是承接之前的问题：fastlio点云去畸变，在这个文档中说了velodyne16线雷达没有时间信息，但是在后续调试的时候发现是有时间的（之前因为没有做点云采样导致命令行显示不到前面的数据），转换格式之后同样也是XYZIRT格式的点云。

fastlio中preprocess.h中的代码块

namespace velodyne_ros {
  struct EIGEN_ALIGN16 Point {
      PCL_ADD_POINT4D;
      float intensity;
      float time;
      uint16_t ring;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
}  // namespace velodyne_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensity, intensity)
    (float, time, time)
    (uint16_t, ring, ring)
)

调试的时候发现点云中点的相对时间是负数:下图代码截图和打印截图
在这里插入图片描述

注意：这里velodyne雷达的rpm我设置成了300（5HZ），默认是600（10HZ），所以一帧雷达的相对时间为0.2秒。而且奇怪的是，相对时间是负数，与速腾雷达点云相对时间为正数是不一样的。

解决方案1

因为不想浪费时间看驱动代码，所以还是正常猜测第一个点的相对时间是0秒，所以将第一个点获得的相对时间比如-0.19902用float类型记录，到后面遍历的时候，全部点都减去这个相对时间，就能够得到每个点相对于第一个点的时间，加上ros格式的pointcloud2中的时间戳应该就是点的绝对时间（相对于工控时间的绝对时间）。

float first_point_offtime = pl_orig.points[0].time;//记录第一个点的时间
added_pt.normal_x = 0;
            added_pt.normal_y = 0;
            added_pt.normal_z = 0;
            added_pt.x = pl_orig.points[i].x;
            added_pt.y = pl_orig.points[i].y;
            added_pt.z = pl_orig.points[i].z;
            added_pt.intensity = pl_orig.points[i].intensity;
            // added_pt.curvature = pl_orig.points[i].time / 1000.0;  // curvature unit: ms
            added_pt.curvature = pl_orig.points[i].time - first_point_offtime;  // curvature unit: ms   因为first_point_offtime是负数所以减

注意：这里的时间单位还是统一成秒方便

最终效果

根据（fastlio点云去畸变）这篇文章，通过fastlio内的代码计算点云中的点的相对时间，在跑5hz点云的数据包时，效果图：

注意：配置文件velodyne.yaml中 scan_rate: 5 # only need to be set for velodyne, unit: Hz,已经改成5HZ了，用于生成雷达角速度，但是结果也很差，建图重影，定位也没回到原来位置上。
通过上面的解决方案，使用velodyne返回的相对时间，同样跑5hz点云的数据包时，效果图：

效果明显好很多，定位回到开始地方，重影虽然也是有的，但是少了很多，至少证明效果肯定比上面的要好。

解决方案2（new）

在找解决方法的时候，看到一篇博客说velodyne16线雷达，点云返回的时间是一帧点云最后一个包的时间，也就是在点云回调函数中的pointcloud2.header中的绝对时间戳是最后一个包的时间，那么就能够解释上面的问题：点云中靠前的点，驱动给出的相对时间是负数，而只有最后一小部分的点相对时间是正数的，所以解决方法就是记录最后一个包的时间，然后通过相对时间去补偿每一个点的时间

在preprocess.h这个类中加入一个public属性记录第一个点的相对时间2. 在preprocess.cpp中对上面加的属性赋值第一点的相对时间，并且需要每一个点都要减这个负的时间（简单的说就是以第一个点为基准，来计算点云中每个点相对第一个点的相对时间，这样处理IMU_Processing.hpp这个文件就跟之前的文章一样的处理方式就行）3. 点云回调中，利用点云中第一个点的相对时间，得出点云的开始扫描绝对时间。

最终效果2

跟上面的效果差不多，但我觉得解决方案2才是正解，因为这样更符合驱动传回来点云数据逻辑，这样子算才能够得到点云的开始扫描时间和结束扫描时间，通过正确时间段的imu数据来做正确的点云去畸变。

总结

虽然在雷达10hz情况下，用fastlio内的代码计算点云中的点的相对时间是可行的，但是雷达5hz情况下会出问题，可能是那个时间补偿的代码适用10hz？也可能是5hz的点云不能这样简单地不长时间？具体原因不太清楚。
点云中点的时间是非常关键的，最好是用驱动给出的相对时间。提问：为什么点云中靠前的点，驱动给出的相对时间是负数？？