Correct way to log LiDAR pose in map or odom frame

[tutunarsl]

Hello,

I am currently benchmarking RESPLE as part of a benchmark. While I am having great time with it, ROS-2 and the way the framework is set bring a serious challenge in logging poses.

I have confirmed that RESPLE.cpp receives all the pointCloud2 data I am publishing under ROS-2 and with mcap playback however, the odometry published by the mapper (I am not sure why mapper publishes the pose) has approximately missing 30% of the messages.

I see that a pose is published only if the know of the spline increases, but this brings the question. What is the correct way to log the poses of RESPLE?

Thank you for your support.

[ATeapot649]

Hi!

Thank you for your interest in RESPLE.

For benchmarking, the recommended way is to write your own code to interpolate the estimated trajectory at the ground-truth timestamps once a sequence is done. We did not provide the code due to the big variety of ground truth format on different datasets. Below is an example:

int64_t t_ns = gt_data.time_ns;
Eigen::Quaterniond ort_itp;
Eigen::Vector3d pos_itp = spline_global.itpPosition(t_ns);
spline_global.itpQuaternion(t_ns, &ort_itp); 

Here are a bit more explanation about the /odometry topic published by the Mapping node:

We use it only for illustration of the current body frame in the world frame. As shown in lines 525–531 of Mapping.cpp, it always publishes the most recent pose from the opt_old_path trajectory, so it does not necessarily correspond to the input LiDAR messages (assuming I understood the reported issue correctly, please let me know if not). The RESPLE node is meant to only estimate the RCPs (recurrent control points). The Mapping node computes poses and grows the global map through spline interpolation.

Please let me know if I can provide further support! I can start a new branch just to demonstrate how to do benchmarking properly. You can also contact me via Email.