Cannot display on custom sensor: Warning: Odometry could not get transform, Error in process()

[TamirEll]

Hello!
I would like to start with some kudos about this awesome project!
 
I am trying to integrate a new sensor. To which I already implemented a ROS layer which publishes Depth, RGB and IMU messages.
When trying to start rtab, I am getting some warnings and something is clearly not working.
The RGB image is registered to Depth(meaning if there is no depth data for a pixel, its value is empty or black).
Maybe this is the issue? should it be the other way around(depth registered to RGB, so the RGB is intact)?

This is the command I use to start:

roslaunch rtabmap_ros rtabmap.launch \
    rtabmap_args:="--delete_db_on_start" \
    depth_topic:=/camera/aligned_depth_to_color/image_raw \
    rgb_topic:=/camera/color/image_raw \
    camera_info_topic:=/camera/color/camera_info \
    frame_id:=camera_link \
    approx_sync:=true

and these are the Error & warnings I get:

[ INFO] [1606409721.685517570]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.061583s
[ WARN] (2020-11-26 18:55:21.743) OdometryF2M.cpp:1256::computeTransform() 20 visual features required to initialize the odometry (only 1000 extracted).
[ INFO] [1606409721.743502554]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.056421s
[ERROR] (2020-11-26 18:55:21.748) Rtabmap.cpp:1095::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 182 is ignored!
[ INFO] [1606409721.748633005]: rtabmap (0): Rate=1.00s, Limit=0.000s, RTAB-Map=0.0001s, Maps update=0.0000s pub=0.0000s (local map=0, WM=0)
[ WARN] [1606409721.827163777]: Could not get transform from odom to camera_link after 0.200000 seconds (for stamp=1606409721.212329)! Error="canTransform: target_frame odom does not exist. canTransform: source_frame camera_link does not exist.. canTransform returned after 0.200893 timeout was 0.2.".



Your help is  greatly appreciated!

[matlabbe]

Hi,

Can you record a rosbag of your image topics? In general, it is preferred to register depth to RGB, but the inverse should still work. I feel that the depth image may be empty or not a format correctly understood by rtabmap. Maybe the /camera/color/camera_info is not correct.

cheers,
Mathieu

[TamirEll]

Hi Mathieu,
Thanks for the quick reply.
Per your recommendation, I am now registering depth to RGB. Unfortunately, I am still getting the same messages.Depth_Example.bag
See the following camera_info message and the attached bag file.

---
header:
  seq: 8
  stamp:
    secs: 1606727602
    nsecs: 297354960
  frame_id: "webcam"
height: 960
width: 1280
distortion_model: "plumb_bob"
D: [0.19992762804031372, -0.8087725043296814, -0.0011053060879930854, -0.00013425601355265826, 0.3449177145957947]
K: [1474.19189453125, 0.0, 629.7936401367188, 0.0, 1474.19189453125, 491.1912841796875, 0.0, 0.0, 1.0]
R: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
P: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
binning_x: 0
binning_y: 0
roi:
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

Thanks!

[TamirEll]

See Log1.Log1
attached log with -debug enabled.

[TamirEll]

Any help is greatly appreciated. I am stuck on this point for about a week now :(
Has anyone encountered a similar issue and could point me in the right direction?

Thanks in advance!

[matlabbe]

Your P in the calibration is not right, at least it should be [K [0;0;0]]. For the bag it would be easier to reproduce if rgb image and camera_info are also included.

EDIT: This warning:

[ WARN] (2020-12-06 11:33:01.779) OdometryF2M.cpp:1256::computeTransform() 20 visual features required to initialize the odometry (only 769 extracted).

means that this condition is not true, so there is indeed a problem with the projection of the depth in 3D. With a P=null, this may automatically cause invalid 3D points.

[TamirEll]

Hi Mathieu,
Thanks for your help.
I changed the P to [K [0;0;0]] as you suggested. Also changed the R to be the identity matrix.
Now I am getting a different error:

[FATAL] (2020-12-13 15:43:42.281) OdometryF2M.cpp:1192::computeTransform() no valid camera model!

I recorded a new bag which includes messages of all 3 types:
RGB
RGB camera info
Depth(Registered)
You can download it from:
https://drive.google.com/file/d/1Svrh_oxIXysiz9MX9HRm1lrdWlLQI-qH/view?usp=sharing

Thanks for you support so far!

[matlabbe]

Hi,

It was not super clear my equation, the last zeros is a column vector, we
should read P (3x4) as:
P: [1474.19189453125, 0.0, 629.7936401367188, 0.0, 0.0, 1474.19189453125,
491.1912841796875, 0.0, 0.0, 0.0, 1.0, 0.0]
instead of:
P: [1474.19189453125, 0.0, 629.7936401367188, 0.0, 1474.19189453125,
491.1912841796875, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]

cheers,
Mathieu

[TamirEll]

Hi Mathieu,
Thanks for the clarification! we are back on getting the image, and I can even see two new windows opening.
However, the tracking still doesn't start. Looking at the log, it looks like a registration problem..
Perhaps I am still setting something wrong in camera info?
Maybe the timestamp gap between the depth and RGB images is too big?
I recorded a new set, you can download here.

The log shows mostly these 3 warn/error:
[ WARN] [1607927085.493817396]: Could not get transform from odom to camera_link after 0.800000 seconds (for stamp=1607927083.957527)! Error="Lookup would require extrapolation at time 1607927083.957527213, but only time 1607927052.294570491 is in the buffer, when looking up transform from frame [camera_link] to frame [odom]. canTransform returned after 0.802296 timeout was 0.8.".
[ WARN] (2020-12-14 08:24:45.511) OdometryF2M.cpp:533::computeTransform() Registration failed: "Not enough inliers 0/20 (matches=10) between -1 and 158"
[ INFO] [1607927085.512414793]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.125817s
[ERROR] (2020-12-14 08:24:45.520) Rtabmap.cpp:1095::process() RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. Image 190 is ignored!


regards,
Tamir.

[TamirEll]

Hi Mathieu,
Quick update! I am finally able to run the application!
I made sure the depth and RGB frames have the same time stamp and seq.
Tracking seems to be working ok but VERY slowly. Furthermore, it looses tracking easily.
I tried running it with approx_sync:=true but it keeps telling me no data recieved since 5 seconds!

Any ideas on how to speed it up? or fix the approx_sync issue?

Best regards & thanks for your support so far!
Tamir.