Posted by
matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/TIps-on-increasing-the-accuracy-and-robustness-of-icp-odometry-tp10685p10715.html
I am currently seeing that error:
terminate called after throwing an instance of 'std::runtime_error'
what(): Time is out of dual 32-bit range
and icp_odometry dies. Here the gdb backtrace:
#0 __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1 0x00007ffff6937859 in __GI_abort () at abort.c:79
#2 0x00007ffff6bc08d1 in () at /lib/x86_64-linux-gnu/libstdc++.so.6
#3 0x00007ffff6bcc37c in () at /lib/x86_64-linux-gnu/libstdc++.so.6
#4 0x00007ffff6bcc3e7 in () at /lib/x86_64-linux-gnu/libstdc++.so.6
#5 0x00007ffff6bcc699 in () at /lib/x86_64-linux-gnu/libstdc++.so.6
#6 0x00007fffe9416cac in ros::TimeBase<ros::Time, ros::Duration>::fromSec(double) () at /opt/ros/noetic/lib/librostime.so
#7 0x00007fffaec78a9a in ros::Time::Time(double) () at /catkin_ws/devel/lib//librtabmap_odom_plugins.so
#8 0x00007fffd2357324 in rtabmap_conversions::deskew_impl(sensor_msgs::PointCloud2_<std::allocator<void> > const&, sensor_msgs::PointCloud2_<std::allocator<void> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, tf::TransformListener*, double, bool, rtabmap::Transform const&, double) () at /catkin_ws/devel/lib/librtabmap_conversions.so
#9 0x00007fffd2359d5b in rtabmap_conversions::deskew(sensor_msgs::PointCloud2_<std::allocator<void> > const&, sensor_msgs::PointCloud2_<std::allocator<void> >&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, tf::TransformListener&, double, bool) ()
at /catkin_ws/devel/lib/librtabmap_conversions.so
#10 0x00007fffaee5e788 in rtabmap_odom::ICPOdometry::callbackCloud(boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&) ()
at /catkin_ws/devel/lib//librtabmap_odom_plugins.so
#11 0x00007fffaee7e5db in boost::_mfi::mf1<void, rtabmap_odom::ICPOdometry, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&>::operator()(rtabmap_odom::ICPOdometry*, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&) const ()
at /catkin_ws/devel/lib//librtabmap_odom_plugins.so
#12 0x00007fffaee7c6df in void boost::_bi::list2<boost::_bi::value<rtabmap_odom::ICPOdometry*>, boost::arg<1> >::operator()<boost::_mfi::mf1<void, rtabmap_odom::ICPOdometry, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&>, boost::_bi::rrlist1<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&> >(boost::_bi::type<void>, boost::_mfi::mf1<void, rtabmap_odom::ICPOdometry, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&>&, boost::_bi::rrlist1<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&>&, int) () at /catkin_ws/devel/lib//librtabmap_odom_plugins.so
The lidar data's timestamp field is data type 8 (double):
fields:
-
name: "timestamp"
offset: 18
datatype: 8
count: 1
The timestamp values look like this:
First: 1732526855256824064.000000
Last: 1732526855357080320.000000
Topic header stamp is:
header:
seq: 5653
stamp:
secs: 1732526855
nsecs: 256824017
frame_id: "livox_frame"
The stamps are in nano seconds (10^-9), and are causing issue with this
conversion (as we assume the double value is in seconds). ros::Time will assert if the number of seconds is over max integer (32 bits)
here.
I think when I
added the original code to support FLOAT64 type, the point cloud I tested was coming from a Hesai lidar and the timestamps were in seconds, not nanoseconds. The bag is too short to see if it really worked, but I pushed a fix here:
https://github.com/introlab/rtabmap_ros/commit/e5667b280a516e1b610438d7985db67678016b33 (EDIT: also need this
commit)
cheers,
Mathieu