Hilti-Research / hilti-slam-challenge-2022

51 stars 1 forks source link

Timestamps of the LIDAR point cloud messages #18

Closed YoshuaNava closed 1 year ago

YoshuaNava commented 2 years ago

Hi there, I noticed that there is no mention of whether the point cloud timestamps are captured at the start, half-way, or at the end of a LIDAR sweep.

Could this be clarified?

Thanks in advance.

bedaberner commented 2 years ago

Hi Yoshua

The time stamp is the start of the scan. However there is actually time information for every single point available. In this short Python script you can see that you can extract timestamps for every point and verify that the "scan timestamp" is actually at the beginning of the scan:

import rosbag
import sensor_msgs.point_cloud2 as pc2

import numpy as np

if __name__ == '__main__':
    input_bag = "/home/beda/Datasets/construction_ground_level_easy_2022-03-03-14-11-49.bag"
    pointcloud_topic = "/hesai/pandar"

    in_bag = rosbag.Bag(input_bag)
    for topic, msg, t in in_bag.read_messages():

        if topic == pointcloud_topic:
            gen = pc2.read_points(msg, skip_nans=True)
            data = list(gen)
            array = np.array(data)
            timestamps = array[:, 4]

            print("scan timestamp: {}, lowest point timestamp: {}, highest point timestamp: {}".format(msg.header.stamp,
                                                                                                       min(timestamps),
                                                                                                       max(timestamps)))