Odometry is reset (identity pose detected) on latest update

[andreacelani]

Hi,

I really enjoyed the RTABMAP in the last 3 months. Everything was very good!
Currently I'm working with intel R200 for outdoor purpose and after some changes made on some launch files I had the chance to use the R200 and it worked very well.
Two days ago I started an update from the update manager of Ubuntu 14.04. The update was fine and I recovered every changes I made but I'm stucked with the following warning:
"CoreWrapper.cpp:731::commonOdomTFUpdate() Odometry is reset (identity pose detected). Increment map id!"
Since I read that was a startup issue fixed a while ago I'm wondering if it appears again back due to some changes in the code.
Actually I cannot use my robot because RTABmap reset odometry every 0.5 sec and it doesn't generate a stable map.
(I'm using ROS-indigo with rgbd-odometry)
Any Idea about that?
Thank you

[matlabbe]

Hi,

The binaries Indigo version (0.11.8) did not get an update since July 17 2016. Can you show the output of rgbd_odometry?

$ rostopic echo /rtabmap/odom

and TF?

$ rosrun tf tf_monitor
$ rosrun tf view_frames

Odometry is reset because Identity transform is published on odometry TF for some reasons. Either odometry auto-reset is activated (if so, the odometry gets lost easily because of a lack of visual features and reset) or you could have multiple nodes published the same TF (/odom).

cheers,
Mathieu

[andreacelani]

Thank you for your fast reply!

I see about the latest RTABMAP update. I supposed the RTABMAP update due to global update but I was wrong.

This is part of the "$ rostopic echo /rtabmap/odom" output (I was not able to snapshot an identity transform on this output):
header:
  seq: 2002
  stamp:
    secs: 1484079846
    nsecs:  88979658
  frame_id: odom
child_frame_id: base_footprint
pose:
  pose:
    position:
      x: -0.00632222974673
      y: 0.0316520929337
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.00268146727392
      w: 0.9999962449
  covariance: [0.0013475469313561916, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0013475469313561916, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0013475469313561916, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0013475469313561916, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0013475469313561916, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0013475469313561916]
twist:
  twist:
    linear:
      x: -0.171575888991
      y: -0.0583064742386
      z: 0.0
    angular:
      x: 0.0
      y: -0.0
      z: 0.0148986969143
  covariance: [0.0006737734656780958, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0006737734656780958, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0006737734656780958, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0006737734656780958, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0006737734656780958, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0006737734656780958]

This is part of the rtabmap bash output :

[ INFO] [1484079848.061276944]: Odom: quality=100, std dev=0.034177m, update time=0.039983s
[ INFO] [1484079848.102225988]: Odom: quality=108, std dev=0.031353m, update time=0.039233s
[ WARN] (2017-01-10 21:24:08.127) CoreWrapper.cpp:731::commonOdomTFUpdate() Odometry is reset (identity pose detected). Increment map id!
[ INFO] [1484079848.145665291]: Odom: quality=129, std dev=0.029959m, update time=0.042017s
[ INFO] [1484079848.194348926]: Odom: quality=128, std dev=0.031350m, update time=0.041992s
^C[ INFO] [1484079848.237107515]: Odom: quality=107, std dev=0.031690m, update time=0.040930s

This is part of the "$ rosrun tf tf_monitor" output :

Frames:
Frame: /base_footprint published by unknown_publisher Average Delay: -0.0996337 Max Delay: 0
Frame: /odom published by unknown_publisher Average Delay: -0.0996216 Max Delay: 0
Frame: base_footprint published by unknown_publisher Average Delay: 0.562104 Max Delay: 0.602214
Frame: base_link published by unknown_publisher Average Delay: -0.499478 Max Delay: 0
Frame: bracket published by unknown_publisher Average Delay: -0.499489 Max Delay: 0
Frame: bracket_end_left published by unknown_publisher Average Delay: -0.499485 Max Delay: 0
Frame: bracket_end_right published by unknown_publisher Average Delay: -0.499487 Max Delay: 0
Frame: camera_depth_frame published by unknown_publisher Average Delay: -0.49949 Max Delay: 0
Frame: camera_depth_optical_frame published by unknown_publisher Average Delay: -0.499492 Max Delay: 0
Frame: camera_link published by unknown_publisher Average Delay: -0.499493 Max Delay: 0
Frame: camera_rgb_frame published by unknown_publisher Average Delay: -0.499493 Max Delay: 0
Frame: camera_rgb_optical_frame published by unknown_publisher Average Delay: -0.499493 Max Delay: 0
Frame: caster_back_link published by unknown_publisher Average Delay: -0.499504 Max Delay: 0
Frame: caster_front_link published by unknown_publisher Average Delay: -0.499505 Max Delay: 0
Frame: cliff_sensor_front_link published by unknown_publisher Average Delay: -0.499505 Max Delay: 0
Frame: cliff_sensor_left_link published by unknown_publisher Average Delay: -0.499505 Max Delay: 0
Frame: cliff_sensor_right_link published by unknown_publisher Average Delay: -0.499506 Max Delay: 0
Frame: gyro_link published by unknown_publisher Average Delay: -0.499507 Max Delay: 0
Frame: odom published by unknown_publisher Average Delay: -0.0994961 Max Delay: 0
Frame: plate_bottom_link published by unknown_publisher Average Delay: -0.499507 Max Delay: 0
Frame: plate_middle_link published by unknown_publisher Average Delay: -0.499507 Max Delay: 0
Frame: plate_top_link published by unknown_publisher Average Delay: -0.49951 Max Delay: 0
Frame: pole_bottom_0_link published by unknown_publisher Average Delay: -0.49951 Max Delay: 0
Frame: pole_bottom_1_link published by unknown_publisher Average Delay: -0.49951 Max Delay: 0
Frame: pole_bottom_2_link published by unknown_publisher Average Delay: -0.499511 Max Delay: 0
Frame: pole_bottom_3_link published by unknown_publisher Average Delay: -0.499511 Max Delay: 0
Frame: pole_bottom_4_link published by unknown_publisher Average Delay: -0.499511 Max Delay: 0
Frame: pole_bottom_5_link published by unknown_publisher Average Delay: -0.499512 Max Delay: 0
Frame: pole_kinect_0_link published by unknown_publisher Average Delay: -0.49951 Max Delay: 0
Frame: pole_kinect_1_link published by unknown_publisher Average Delay: -0.49951 Max Delay: 0
Frame: pole_middle_0_link published by unknown_publisher Average Delay: -0.499511 Max Delay: 0
Frame: pole_middle_1_link published by unknown_publisher Average Delay: -0.499511 Max Delay: 0
Frame: pole_middle_2_link published by unknown_publisher Average Delay: -0.499511 Max Delay: 0
Frame: pole_middle_3_link published by unknown_publisher Average Delay: -0.499512 Max Delay: 0
Frame: pole_top_0_link published by unknown_publisher Average Delay: -0.499512 Max Delay: 0
Frame: pole_top_1_link published by unknown_publisher Average Delay: -0.499512 Max Delay: 0
Frame: pole_top_2_link published by unknown_publisher Average Delay: -0.499513 Max Delay: 0
Frame: pole_top_3_link published by unknown_publisher Average Delay: -0.499517 Max Delay: 0
Frame: wheel_left_link published by unknown_publisher Average Delay: -0.499517 Max Delay: 0
Frame: wheel_right_link published by unknown_publisher Average Delay: -0.499517 Max Delay: 0

All Broadcasters:
Node: unknown_publisher 66.4555 Hz, Average Delay: 0.0827141 Max Delay: 0.584455

This is part of the "$ rosrun tf view_frames" output :

frames.pdf


I didn't activate the odometry auto-reset but from tf_monitor output seems that your second hypothesis, multiple nodes publish the same TF (/odom) could be the issue.
I know this problem should be out of this great forum but could you suggest me some advice on how to debug it?
From the tf tree everything seems fine. Where is the additional node?

Thank you
Andrea

[matlabbe]

Hi,

Thx sending the TF tree, I can see the problem. You have two nodes named /map_to_odom and  /odom_to_base_footprint that should not be there, as /rtabmap and /rgbd_odometry nodes already publishes these frames respectively. Remove /odom_to_base_footprint and /map_to_odom nodes. For the bug above, /odom_to_base_footprint may always send Identity transform. mixing with transform sent by visual odometry.

cheers,
Mathieu

[andreacelani]

It worked! Thank you!
After the change the local costmap and the global costmap don't send map anymore.
I fixed changing the transform tolerance of global costmap params from 0.5 to 0.8 .

Since everything's back to work like one week ago I would like to ask you a more RTABMAP related question that could be useful for others.
I'm working with Intel R200 capable of 60 hz on stereo camera (depth) and rgb camera.
I already changed the camera parameters in order to have 60 fps @ 320X240 on depth and 60fps  @ 640X480 on rgb.
Which RTABMAP parameters should I change in order to speed up the odometry at the full speed of the camera and obtain a odometry output rate of 60 hz?
RTABMAP has lot of parameters and sometimes is very difficult which should I modify to obtain a given result.

Andrea

[andreacelani]

Maybe I understand in the wrong way how RTABMAP works.
RTABMAP processes out all odometry data at max speed. The processor or the graphic card is the bottleneck.
Now with my laptop (Core i7-4500UI) achieve around 16-21 hz. If I set up RTABMAP on a different and more powerful pc I could achieve 60 hz odom output.
Could you please confirm that?
May I move the rgb_odometry process partially on the graphic card?
If I compile the OpenCV with CUDA support could I save processing power on the CPU?

[matlabbe]

Hi,

Yes, odometry tries to process as many images as it can. The processing is done on CPU, there is no port yet on GPU (OpenCL or CUDA).

cheers,
Mathieu