Open hanzheteng opened 9 months ago
The default point type used in Velodyne LiDAR is velodyne_pointcloud::PointXYZIR
, which does not have a field called time
. This information may be needed in some LiDAR-inertial odometry algorithms (e.g., Fast-LIO). One workaround is to compute a rough timestamp for each point and save them into the intensity
field.
for (size_t i = 0; i < cloud->size(); ++i) {
cloud->points[i].intensity = float(double(i) / double(cloud->size()));
} // you can use static_cast if you want
Alternatively, if we want to create a new point type named PointXYZIRT
that includes the time
field, we can create a header file as follows and use it in the program.
#ifndef MY_PC_POINT_TYPES_H
#define MY_PC_POINT_TYPES_H
#include <pcl/point_types.h>
namespace velodyne_pointcloud
{
struct PointXYZIRT
{
PCL_ADD_POINT4D;
PCL_ADD_INTENSITY;
float time;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
}; // namespace velodyne_pointcloud
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_pointcloud::PointXYZIRT,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(float, time, time)
(uint16_t, ring, ring))
#endif // MY_PC_POINT_TYPES_H
FYI, the documentation for velodyne LiDAR driver is here:
Some user reached out for this feature request. I will document some solutions in this issue.