search

koide3 / glim

GLIM: versatile and extensible range-based 3D localization and mapping framework
MIT License
441 stars 55 forks source link

The timestamps in traj_lidar.txt does not match LiDAR timestamps #52

Closed changh95 closed 2 weeks ago

changh95 commented 1 month ago

Describe the bug

When glim_rosbag is over, the LiDAR trajectory is automatically saved in /tmp/dump/traj_lidar.txt. The data is saved in tum format (stamp, tx, ty, tz, rx, ry, rz, rw). The stamp should mean timestamp, and it should match the LiDAR timestamp in the rosbag or db3 depending on ROS1/2 usages. They don't seem to match, and the stamp value does not seem to match the IMU values either.

Where do these stamp values come from?

Also, it seems like the first 1 second seems to be removed. Ideally, the number of entries in the traj_lidar.txt should match the number of LiDAR frames used in the glim_rosbag.

To Reproduce Steps to reproduce the behavior:

  1. Run glim_rosbag
  2. Close the iridiscene viewer. LiDAR trajectory data is automatically saved in /tmp/dump/traj_lidar.txt.
  3. Compare column 1 of the txt file to rosbag/db3 file.

Expected behavior

The timestamp of rosbag/db3 should be exactly the same as traj_lidar.txt file. If some values are missing due to initialization stage, the remaining values should still match (it does not at the moment).

Screenshots (Running on my own custom data at the moment)

Environment (please complete the following information):

koide3 commented 1 month ago

Where do these stamp values come from?

GLIM stores timestamps as double precision floats internally. While that makes the timestamps not exactly the same as those in ros messages, the differences should be very small (should be micro to nanosecond-order). I suppose the timestamps you printed out are "date arrival system times" rather than "date observation times (msg.header.stamp)". Could you re-check data observation timestamps with ros2 topic echo /points --field header?

Also, it seems like the first 1 second seems to be removed. Ideally, the number of entries in the traj_lidar.txt should match the number of LiDAR frames used in the glim_rosbag.

GLIM performs initial state estimation at the very beginning of estimation, and during this stage, the sensor trajectory is not available. I recommend using the pose of the very first frame to fill the dropped timestamps.

changh95 commented 1 month ago

@koide3 The second screenshot (The actual timestamp from db3), is the timestamp values when I make the db3 file (i.e. msg.header.stamp). This was verified using ros2 topic echo /points --field header (see below screenshot).

image

koide3 commented 1 month ago

Hmm, did you see any warning related to timestamps, like "use first point timestamp ..." or "point timestamp is too apart ..."?

changh95 commented 1 month ago

Yes, I got something similar to those:

Here's the full log of my glim_rosbag on custom ROS2 dataset. The output pose looks okay qualitatively.

[2024-08-01 06:23:03.382] [glim] [info] config_path: /root/ros2_ws/install/glim/share/glim/config
[2024-08-01 06:23:03.382] [glim] [info] load libodometry_estimation_gpu.so
[2024-08-01 06:23:03.443] [glim] [info] load libsub_mapping.so
[2024-08-01 06:23:03.444] [glim] [info] load libglobal_mapping.so
[2024-08-01 06:23:03.445] [glim] [warning] Extension modules are enabled!!
[2024-08-01 06:23:03.445] [glim] [warning] You must carefully check and follow the licenses of ext modules
[2024-08-01 06:23:03.445] [glim] [warning] glim_ext package path was not found!!
[2024-08-01 06:23:03.445] [glim] [info] load libmemory_monitor.so
[2024-08-01 06:23:03.445] [glim] [info] load libstandard_viewer.so
[2024-08-01 06:23:03.448] [glim] [info] load librviz_viewer.so
[2024-08-01 06:23:03.454] [glim] [info] topics:
[2024-08-01 06:23:03.454] [glim] [info] - /surf/oxts/imu
[2024-08-01 06:23:03.454] [glim] [info] - /surf/hesai_lidar
[2024-08-01 06:23:03.454] [glim] [info] - /image
[2024-08-01 06:23:03.454] [glim] [info] bag_filenames:
[2024-08-01 06:23:03.454] [glim] [info] - /data/KOR_TM9470_M_20240417_185311/KOR_TM9470_M_20240417_185311.db3
[2024-08-01 06:23:03.454] [glim] [warning] param glim_ros/playback_speed not found
[2024-08-01 06:23:03.454] [glim] [warning] use default_value=100
[2024-08-01 06:23:03.454] [glim] [info] opening /data/KOR_TM9470_M_20240417_185311/KOR_TM9470_M_20240417_185311.db3

closing.

closing.
[INFO] [1722493383.457388807] [rosbag2_storage]: Opened database '/data/KOR_TM9470_M_20240417_185311/KOR_TM9470_M_20240417_185311.db3' for READ_ONLY.
MESA: error: Failed to query drm device.
libGL error: glx: failed to create dri3 screen
libGL error: failed to load driver: iris
[2024-08-01 06:23:04.137] [glim] [warning] large point timestamp (1713347591.499737 > 1.0) found!!
[2024-08-01 06:23:04.137] [glim] [warning] assume that point times are absolute and convert them to relative
[2024-08-01 06:23:04.137] [glim] [warning] replace_frame_stamp=true wrt_first_frame_timestamp=true
[2024-08-01 06:23:04.137] [glim] [warning] point timestamp is too apart from frame timestamp!!
[2024-08-01 06:23:04.137] [glim] [warning] use time offset w.r.t. the first frame timestamp
[2024-08-01 06:23:04.137] [glim] [warning] frame=0.000000 point=1713347591.399809 diff=-1713347591.399809
[2024-08-01 06:23:04.461] [odom] [info] estimate initial IMU state
Initial error: 176.894, values: 36
iter      cost      cost_change    lambda  success iter_time
   0  5.267343e-02    1.77e+02    1.00e-05     1    2.77e-04
   1  5.226291e-02    4.11e-04    1.00e-06     1    2.81e-04
   2  5.226291e-02    5.60e-13    1.00e-07     1    3.16e-04
[2024-08-01 06:23:04.465] [odom] [info] initial IMU state estimation result
[2024-08-01 06:23:04.465] [odom] [info] T_world_imu=se3(-0.001532,0.002066,0.000238,0.000697,-0.001061,-0.000014,0.999999)
[2024-08-01 06:23:04.465] [odom] [info] v_world_imu=vec(-0.014364,-0.003310,-0.005929)
[2024-08-01 06:23:04.465] [odom] [info] imu_bias=vec(0.000038,0.000023,-0.000265,0.000133,-0.000085,0.000008)
warning: an exception was caught in fixed-lag smoother update!!
       : Requested variable 'v320' is not in this VectorValues.
falling back!!
smoother_lag:5
current_stamp:38.104542
- symbol=x min_stamp=33.200712 fixation=34.200712
- symbol=v min_stamp=33.200712 fixation=34.200712
- symbol=b min_stamp=33.200712 fixation=34.200712
factors:1502 fixed:40 values:150 stamps:150
[2024-08-01 06:23:08.285] [odom] [warning] odometry estimation smoother fallback happened (time=38.10454249382019)
[2024-08-01 06:23:13.461] [glim] [info] playback speed: 8.919x
warning: an exception was caught in fixed-lag smoother update!!
       : Requested variable 'x1098' is not in this VectorValues.
falling back!!
smoother_lag:5
current_stamp:116.003516
- symbol=x min_stamp=111.005246 fixation=112.005246
- symbol=v min_stamp=111.005246 fixation=112.005246
- symbol=b min_stamp=111.005246 fixation=112.005246
factors:1408 fixed:40 values:153 stamps:153
[2024-08-01 06:23:17.749] [odom] [warning] odometry estimation smoother fallback happened (time=116.00351572036743)
[2024-08-01 06:23:18.463] [glim] [info] playback speed: 7.776x
falling back!!
smoother_lag:5
current_stamp:127.400489
- symbol=x min_stamp=122.402237 fixation=123.402237
- symbol=v min_stamp=122.402237 fixation=123.402237
- symbol=b min_stamp=122.402237 fixation=123.402237
factors:1452 fixed:44 values:153 stamps:153
warning: an exception was caught in fixed-lag smoother update!!
       : Requested variable 'x1212' is not in this VectorValues.
[2024-08-01 06:23:18.909] [odom] [warning] odometry estimation smoother fallback happened (time=127.40048885345459)
[2024-08-01 06:23:23.470] [glim] [info] playback speed: 9.927x
[2024-08-01 06:23:28.476] [glim] [info] playback speed: 9.568x
[2024-08-01 06:23:33.483] [glim] [info] playback speed: 9.087x
[2024-08-01 06:23:38.483] [glim] [info] playback speed: 4.758x
[INFO] [1722493435.389029104] [rclcpp]: signal_handler(signum=15)
[2024-08-01 06:23:55.389] [glim] [info] waiting for odometry estimation
[2024-08-01 06:23:55.390] [glim] [info] waiting for local mapping
[2024-08-01 06:23:55.575] [glim] [info] waiting for global mapping
[2024-08-01 06:23:55.724] [global] [info] saving to /tmp/dump...
[2024-08-01 06:23:55.725] [global] [info] serializing factor graph to /tmp/dump/graph.bin
[2024-08-01 06:24:25.977] [global] [info] saved
koide3 commented 1 month ago

I got it. The issue stems from the inconsistency between the point cloud timestamp and the per-point timestamp. The following log shows that the point cloud timestamp (points_msg.header.stamp) is 0.0 while the per-point timestamp (points_msg.points[i].time (pseudo code)) is 1713347591.499737, which may be the system absolute timestamp. GLIM tried to automatically resolve this inconsistency and overwrote the point cloud timestamp with time-difference-compensated per-point timestamp.

[2024-08-01 06:23:04.137] [glim] [warning] large point timestamp (1713347591.499737 > 1.0) found!!
[2024-08-01 06:23:04.137] [glim] [warning] assume that point times are absolute and convert them to relative
[2024-08-01 06:23:04.137] [glim] [warning] replace_frame_stamp=true wrt_first_frame_timestamp=true
[2024-08-01 06:23:04.137] [glim] [warning] point timestamp is too apart from frame timestamp!!
[2024-08-01 06:23:04.137] [glim] [warning] use time offset w.r.t. the first frame timestamp
[2024-08-01 06:23:04.137] [glim] [warning] frame=0.000000 point=1713347591.399809 diff=-1713347591.399809

I saw a similar issue with a Hesai LiDAR. In that case, the issue was resolved by configuring the Hesai driver so that it used a consistent time system for both point cloud and per-point timestamps.

Meanwhile, I'm thinking of making this per-point time management manually configurable. While I tried to cover most of the per-point timestamp representation conventions, each LiDAR driver stores per-point timestamps in a very different way, and the automatic time management does bad sometimes.

changh95 commented 1 month ago

That sounds right - I am using Hesai LiDAR.

Since point_time_offset is only determined at the very first frame, I can see how the timestamps of later frames can be affected. I think I can change the code to fix the problem for my case, but I doubt the code will work for other LiDAR cases. Once done, I'll share the code here.

On the side note, it seems like other types of errors can arise from the code here.

Considering how time offset is calculated by point_time_offset = points->stamp - points->times.front();, it would make incorrect time offset values if points->times are not sorted in ascending order. Consequently, points->stamp = points->times.front() + point_time_offset; calculation will be bad as well because the LiDAR timestamp should be the very start of the scanning sequence (unless, it's set to be 50% of the scanning sequence. I guess this makes everything complicated).

In my other project, I deal with this problem by normalizing all the timestamps between 0 <= t <= 1.

        if np.max(timestamps) == 0:
            return timestamps # No point-timestamp values
        return (timestamps - np.min(timestamps)) / (np.max(timestamps) - np.min(timestamps))
koide3 commented 1 month ago

Yeah, that code assumed the points are sorted by time, it would be invalid for some LiDARs. I think I will reshape the per-point time management to support a wider range of sensors.

A headache problem is that there is no standard about the per-point time representation, and each LiDAR driver uses their own rule. I saw LiDAR drivers that use absolute / relative times, ascending / descending / not ordered, sec / usec / nsec, ... There was also a driver that assigns negative times for points (frame timestamp was the time of the last point). I'm not very sure if we can support all existing per-point time conventions, but at least, I would like to let the problem output some warnings even in special cases so that the user can notice where is the problem.

changh95 commented 1 month ago

I may be wrong, but I thought it'd be simple if we sort and normalize all the timestamps between 0 <= t <= 1 ...? This might slow down the preprocess stage, so it might not be favorable since you need to publish the paper with the processing time information...

I can try writing the code for this, and maybe try to make it fast. Then maybe you can also advertise glim as 'LIO that just works' as it supports all types of lidar easily :rofl:

koide3 commented 1 month ago

Actually, there is a time sort in the preprocessing step, and I would like to avoid a duplicated sort in this time management step. I think we can instead use min/max because we are only interested in the range of the timestamps here. Normalization should work well for most of the situations, but it might do bad when the point times do not well cover the scan interval (e.g., when a part or all of points in a point cloud frame are dropped)...

I can try writing the code for this, and maybe try to make it fast.

Thanks, but it's not needed for now because I want to reshape the entire of this part for a better handling of point timestamps and providing more informative warnings. Instead, I would like to ask you to test the updated once it is done.

Then maybe you can also advertise glim as 'LIO that just works' as it supports all types of lidar easily 🤣

Yes, it is definitely what we want to do😂 Thanks for your help!

koide3 commented 1 month ago

I re-wrote the per-point time management code. The new one should better support corner cases. Could you see if it works well? https://github.com/koide3/glim/pull/61

changh95 commented 1 month ago

Hi @koide3 , Sorry for a late update. I had some busy issues and it got my testing schedule late.

I tested the new code, and it seems there's no more warnings appearing from per-point timestamp handling. :+1:

But it seems a new issue arises. There's new warning about mismatch between LiDAR per-point timestamps and IMU timestamps. Due to this error, the entire LIO system does not work.

[2024-08-13 05:33:10.815] [glim] [warning] points=1713347598.300033 imu=6.905845 diff=-1713347591.394188
[2024-08-13 05:33:10.815] [glim] [warning] large time difference between points and imu!!

This one is on me. My LiDAR timestamp has absolute timestamp (e.g. 1713347591.499737), whereas my IMU starts from 0.0 . I don't think you should care about this case, and just a note on 'LiDAR and IMU must be synced' as requirement should just do fine.

I'll fix my dataset and test it again. Once done, I'll share the results :) Thanks always.

UPDATE:

I synced the timestamp between LiDAR and IMU. It seems like there's some code to normalize IMU timestamps? The IMU timestamp starts from 0, whereas autoconf LiDAR per-point timestamp is an absolute value.

image

koide3 commented 3 weeks ago

Thanks for your feedback. The autoconf prioritizes per-point timestamp (if it is absolute) by default. But, if you want to leave the frame timestamp as is, you can set perpoint_prefer_frame_time to true. Then, the frame timestamp will not be replaced.

changh95 commented 3 weeks ago

Thank you! I'll give a try on Monday, and will update how it goes.

koide3 commented 3 weeks ago

Thanks. BTW, I just thought the parameter name was a bit confusing. Maybe I'll rename it to autoconf_prefer_frame_time or something else.

changh95 commented 2 weeks ago

Sorry for a late update! Had issues with my PC and finally got the dev environment back.

For Hesai LiDAR, the below settings worked nicely. Confirming the new code works! :+1: :+1: :+1: :+1: Thank you for all the help!

// LiDAR per-point time settings
"autoconf_perpoint_times": true,
"autoconf_prefer_frame_time": false,
"perpoint_relative_time": false,
"perpoint_time_scale": 1.0,