Timestamp alignment issue
I'm debugging with an Orbbec camera and encountered this issue. How can I resolve it
“”“ [rgbd_odometry-2] [WARN] [1758513759.089787811] [rtabmap.rgbd_odometry]: Dropping image/scan data with stamp 1758513758.977694 (delay=0.112090). Something is wrong because the clock difference with the previous topic received (0.020811s) is much lower than the expected one (0.033184s) estimated from the topic stamps (previous stamp=1758513758.944510). If you are processing a large bag with flaky replaying delay, consider setting parameter "always_process_most_recent_frame:=false" to avoid aggressively dropping data. [rgbd_odometry-2] [WARN] [1758513759.114679726] [rtabmap.rgbd_odometry]: Dropping image/scan data with stamp 1758513759.010879 (delay=0.103798). Something is wrong because the clock difference with the previous topic received (0.024894s) is much lower than the expected one (0.033185s) estimated from the topic stamps (previous stamp=1758513758.977694). If you are processing a large bag with flaky replaying delay, consider setting parameter "always_process_most_recent_frame:=false" to avoid aggressively dropping data. ”“”
I updated the code recently to be less sensible to that warning. In your cases, the warning would not have appear. However, the frame is still dropped.
What is detected here is that images should be published at 33 Hz or 33 ms period, but we received two consecutive frames with time difference below 33 ms, which should not be possible if the sensor has a fixed capture rate. Without always_process_most_recent_frame:=false, odometry will aggressively drop frames: if odometry is currently processing an image and receives a new one, the new one is dropped. So if the time difference is under the expected one, it means maybe the camera driver is giving wrong timestamps if the camera is really capturing at 30 Hz. Thus if odometry could be computed in 30 ms, so normally not skipping any frames, but then receive a frame under 20 ms even if in reality images are captured at 30 Hz, it will skip it wrongly.
If I want to use SuperPoint for feature extraction in a ROS 2 environment, how should I proceed? Could you provide some reference examples? @matlabbe
This Dockerfile can be a good starting point: https://github.com/introlab/rtabmap_ros/blob/master/docker/noetic/superpoint/Dockerfile, by installing ROS2 Humble instead of ROS Noetic. I'll check later if I could make a dockerfile for humble.
@matlabbe I encountered the following issue during execution. How should I resolve it?
ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_args:="--delete_db_on_start"
'Mem/UseOdomFeatures': 'False',
'Vis/FeatureType': '11',
'Kp/DetectorStrategy': '11',
'Rtabmap/DetectionRate': "5.0",
'Vis/MaxFeatures': '3000',
'Vis/EstimationType': '1', #pnp
'SuperPoint/Cuda': 'true',
'SuperPoint/ModelPath': '/home/rtabmap/models/superpoint.pt',
'Vis/CorNNType': '5',
'PyMatcher/Model': 'outdoor',
'PyMatcher/Path': '/home/SuperPointPretrainedNetwork/rtabmap_superglue.py',
'PyMatcher/Threshold': '0.2',
'PyMatcher/Iterations': '20',
'PyMatcher/Cuda': 'true',
[rgbd_odometry-1] terminate called after throwing an instance of 'c10::AcceleratorError'
[rgbd_odometry-1] what(): CUDA error: no kernel image is available for execution on the device
[rgbd_odometry-1] CUDA kernel errors might be asynchronously reported at some other API call, so the stacktrace below might be incorrect.
[rgbd_odometry-1] For debugging consider passing CUDA_LAUNCH_BLOCKING=1
[rgbd_odometry-1] Compile with `TORCH_USE_CUDA_DSA` to enable device-side assertions.
[rgbd_odometry-1]
[rgbd_odometry-1] Exception raised from c10_cuda_check_implementation at /pytorch/c10/cuda/CUDAException.cpp:42 (most recent call first):
[rgbd_odometry-1] frame #0: c10::Error::Error(c10::SourceLocation, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >) + 0x9c (0x7bfc5b592f3c in /home/lifa/libtorch/lib/libc10.so)
[rgbd_odometry-1] frame #1: <unknown function> + 0x1eca6 (0x7bfbd696cca6 in /home/lifa/libtorch/lib/libc10_cuda.so)
[rgbd_odometry-1] frame #2: void at::native::gpu_kernel_impl_nocast<at::native::CUDAFunctor_add<float> >(at::TensorIteratorBase&, at::native::CUDAFunctor_add<float> const&) + 0x3ac (0x7bfbdf5dba2c in /home/lifa/libtorch/lib/libtorch_cuda.so)
[rgbd_odometry-1] frame #3: void at::native::gpu_kernel_impl<at::native::CUDAFunctor_add<float> >(at::TensorIteratorBase&, at::native::CUDAFunctor_add<float> const&) + 0x44f (0x7bfbdf5dc65f in /home/lifa/libtorch/lib/libtorch_cuda.so)
[rgbd_odometry-1] frame #4: void at::native::gpu_kernel<at::native::CUDAFunctor_add<float> >(at::TensorIteratorBase&, at::native::CUDAFunctor_add<float> const&) + 0x35b (0x7bfbdf5dce0b in /home/lifa/libtorch/lib/libtorch_cuda.so)
[rgbd_odometry-1] frame #5: <unknown function> + 0x4398536 (0x7bfbdf598536 in /home/lifa/libtorch/lib/libtorch_cuda.so)
[rgbd_odometry-1] frame #6: at::native::add_kernel(at::TensorIteratorBase&, c10::Scalar const&) + 0x34 (0x7bfbdf598664 in /home/lifa/libtorch/lib/libtorch_cuda.so)
[rgbd_odometry-1] frame #7: <unknown function> + 0x413661f (0x7bfbdf33661f in /home/lifa/libtorch/lib/libtorch_cuda.so)
[rgbd_odometry-1] frame #8: at::_ops::add__Tensor::call(at::Tensor&, at::Tensor const&, c10::Scalar const&) + 0x1ad (0x7bfc3e53502d in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #9: at::native::_convolution(at::Tensor const&, at::Tensor const&, std::optional<at::Tensor> const&, c10::ArrayRef<long>, c10::ArrayRef<long>, c10::ArrayRef<long>, bool, c10::ArrayRef<long>, long, bool, bool, bool, bool) + 0x1383 (0x7bfc3d7f7663 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #10: <unknown function> + 0x34953db (0x7bfc3ec953db in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #11: <unknown function> + 0x34a18af (0x7bfc3eca18af in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #12: at::_ops::_convolution::call(at::Tensor const&, at::Tensor const&, std::optional<at::Tensor> const&, c10::ArrayRef<c10::SymInt>, c10::ArrayRef<c10::SymInt>, c10::ArrayRef<c10::SymInt>, bool, c10::ArrayRef<c10::SymInt>, c10::SymInt, bool, bool, bool, bool) + 0x379 (0x7bfc3e1856d9 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #13: at::native::convolution(at::Tensor const&, at::Tensor const&, std::optional<at::Tensor> const&, c10::ArrayRef<long>, c10::ArrayRef<long>, c10::ArrayRef<long>, bool, c10::ArrayRef<long>, long) + 0x1e9 (0x7bfc3d7e9059 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #14: <unknown function> + 0x3494c80 (0x7bfc3ec94c80 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #15: <unknown function> + 0x34a1b5b (0x7bfc3eca1b5b in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #16: at::_ops::convolution::redispatch(c10::DispatchKeySet, at::Tensor const&, at::Tensor const&, std::optional<at::Tensor> const&, c10::ArrayRef<c10::SymInt>, c10::ArrayRef<c10::SymInt>, c10::ArrayRef<c10::SymInt>, bool, c10::ArrayRef<c10::SymInt>, c10::SymInt) + 0x198 (0x7bfc3e129268 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #17: <unknown function> + 0x558a3e5 (0x7bfc40d8a3e5 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #18: <unknown function> + 0x558b3ec (0x7bfc40d8b3ec in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #19: at::_ops::convolution::call(at::Tensor const&, at::Tensor const&, std::optional<at::Tensor> const&, c10::ArrayRef<c10::SymInt>, c10::ArrayRef<c10::SymInt>, c10::ArrayRef<c10::SymInt>, bool, c10::ArrayRef<c10::SymInt>, c10::SymInt) + 0x309 (0x7bfc3e184389 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #20: <unknown function> + 0x1d05f7e (0x7bfc3d505f7e in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #21: at::native::conv2d_symint(at::Tensor const&, at::Tensor const&, std::optional<at::Tensor> const&, c10::ArrayRef<c10::SymInt>, c10::ArrayRef<c10::SymInt>, c10::ArrayRef<c10::SymInt>, c10::SymInt) + 0x18d (0x7bfc3d7ecaad in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #22: <unknown function> + 0x3603d42 (0x7bfc3ee03d42 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #23: <unknown function> + 0x3604040 (0x7bfc3ee04040 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #24: at::_ops::conv2d::call(at::Tensor const&, at::Tensor const&, std::optional<at::Tensor> const&, c10::ArrayRef<c10::SymInt>, c10::ArrayRef<c10::SymInt>, c10::ArrayRef<c10::SymInt>, c10::SymInt) + 0x2a4 (0x7bfc3e8980d4 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #25: <unknown function> + 0x6f5b089 (0x7bfc4275b089 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #26: torch::nn::Conv2dImpl::_conv_forward(at::Tensor const&, at::Tensor const&) + 0xd4 (0x7bfc42754174 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #27: torch::nn::Conv2dImpl::forward(at::Tensor const&) + 0x29 (0x7bfc427544d9 in /home/lifa/libtorch/lib/libtorch_cpu.so)
[rgbd_odometry-1] frame #28: rtabmap::SuperPoint::forward(at::Tensor) + 0x5d (0x7bfc60949aed in /usr/local/lib/librtabmap_core.so.0.22)
[rgbd_odometry-1] frame #29: rtabmap::SPDetector::detect(cv::Mat const&, cv::Mat const&) + 0x373 (0x7bfc6094aca3 in /usr/local/lib/librtabmap_core.so.0.22)
[rgbd_odometry-1] frame #30: rtabmap::SuperPointTorch::generateKeypointsImpl(cv::Mat const&, cv::Rect_<int> const&, cv::Mat const&) + 0x29d (0x7bfc60560a4d in /usr/local/lib/librtabmap_core.so.0.22)
[rgbd_odometry-1] frame #31: rtabmap::Feature2D::generateKeypoints(cv::Mat const&, cv::Mat const&) + 0x24a (0x7bfc605652ba in /usr/local/lib/librtabmap_core.so.0.22)
[rgbd_odometry-1] frame #32: rtabmap::RegistrationVis::computeTransformationImpl(rtabmap::Signature&, rtabmap::Signature&, rtabmap::Transform, rtabmap::RegistrationInfo&) const + 0x3429 (0x7bfc6079d439 in /usr/local/lib/librtabmap_core.so.0.22)
[rgbd_odometry-1] frame #33: rtabmap::Registration::computeTransformationMod(rtabmap::Signature&, rtabmap::Signature&, rtabmap::Transform, rtabmap::RegistrationInfo*) const + 0x267 (0x7bfc60772877 in /usr/local/lib/librtabmap_core.so.0.22)
[rgbd_odometry-1] frame #34: rtabmap::OdometryF2M::computeTransform(rtabmap::SensorData&, rtabmap::Transform const&, rtabmap::OdometryInfo*) + 0x13de (0x7bfc608046fe in /usr/local/lib/librtabmap_core.so.0.22)
[rgbd_odometry-1] frame #35: rtabmap::Odometry::process(rtabmap::SensorData&, rtabmap::Transform const&, rtabmap::OdometryInfo*) + 0x92b (0x7bfc607e86db in /usr/local/lib/librtabmap_core.so.0.22)
[rgbd_odometry-1] frame #36: rtabmap_odom::OdometryROS::mainLoop() + 0xc9a (0x7bfc5fb6104a in /home/lifa/rtabmap/install/rtabmap_odom/lib/librtabmap_odom.so)
[rgbd_odometry-1] frame #37: UThread::ThreadMain() + 0x51 (0x7bfc61500c11 in /usr/local/lib/librtabmap_utilite.so.0.22)
[rgbd_odometry-1] frame #38: UThreadC<void>::ThreadMainHandler(UThreadC<void>::Instance*) + 0xa9 (0x7bfc61500f59 in /usr/local/lib/librtabmap_utilite.so.0.22)
[rgbd_odometry-1] frame #39: <unknown function> + 0x94ac3 (0x7bfc5f894ac3 in /lib/x86_64-linux-gnu/libc.so.6)
[rgbd_odometry-1] frame #40: <unknown function> + 0x126850 (0x7bfc5f926850 in /lib/x86_64-linux-gnu/libc.so.6)
[rgbd_odometry-1]
[ERROR] [rgbd_odometry-1]: process has died [pid 19838, exit code -6, cmd '/home/lifa/rtabmap/install/rtabmap_odom/lib/rtabmap_odom/rgbd_odometry --delete_db_on_start --ros-args --log-level rtabmap.rgbd_odometry:=info --log-level rgbd_odometry:=info --ros-args -r __node:=rgbd_odometry -r __ns:=/rtabmap -p use_sim_time:=False --params-file /tmp/launch_params_k8gr9sk_ -r rgb/image:=/camera/color/image_raw -r depth/image:=/camera/depth/image_raw -r rgb/camera_info:=/camera/color/camera_info -r rgbd_image:=rgbd_image_relay -r odom:=odom -r imu:=/camera/gyro_accel/sample'].
It looks like an issue with CUDA/Torch:
[rgbd_odometry-1] terminate called after throwing an instance of 'c10::AcceleratorError'
[rgbd_odometry-1] what(): CUDA error: no kernel image is available for execution on the device
Did you build pytorch manually? It seems there is an incompatibility with the cuda compute required and your graphics card capabilities.
Can you try the SuperPoint python script to test if it works (on GPU)? https://github.com/magicleap/SuperPointPretrainedNetwork
So, I created an example of a working docker image that can run rtabmap with superpoint/superglue. See https://github.com/introlab/rtabmap_ros/tree/ros2/docker/jazzy/superpoint
@matlabbe If I want to use RTAB-Map with SuperPoint and SuperGlue, do I need to install both LibTorch and PyTorch in the corresponding runtime environment?
You could only install PyTorch, which will install libtorch at the same time. When building rtabmap, just set the correct directory of libtorch so that cmake can find TorchConfig.cmake file. In the linked docker file above, we pointed it to where pytorch is installed:
cmake -DTorch_DIR=/usr/local/lib/python3.12/dist-packages/torch/share/cmake/Torch ...
If you pip installed pytorch in your --user space on your host machine, the directory may be somewhere here (minor the python version):
cmake -DTorch_DIR=~/.local/lib/python3.12/dist-packages/torch/share/cmake/Torch ...