Handle lost visual odometry with IMU sensor fusion

Question

Hi,

I am trying to fuse data from an IMU and a visual odometry node with robot_localization in order to have a better localization for my robot. The IMU is a Tinkerforge IMU Brick 2.0; the visual odometry node is the one provided in the package rtabmap_ros.
Both nodes work well, and the extended kalman filter does what it's supposed to do.

I am using ROS kinetic on Ubuntu 16.04.
The only issue that I'm facing is when the visual odometry gets lost: in this case the node publishes a message like that:

header:
   seq: 769
    stamp:
       secs: 1522407457
       nsecs: 640873928
    frame_id: "odom"
child_frame_id: "kinect2_base_link"
pose:
    pose:
       position:
          x: 0.0
          y: 0.0
          z: 0.0
      orientation: 
          x: 0.0
          y: 0.0
          z: 0.0
          w: 0.0
    covariance: [9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0]
twist:
    twist:
       linear:
          x: 0.0
          y: 0.0
          z: 0.0
       angular: 
          x: 0.0
          y: 0.0
          z: 0.0
     covariance: [9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0]

In this case the kalman filter still continues to work, and this results in the estimante odometry having an erroneous translational velocity. How can I face with this? I was thinking about a way to "pause" the kalman filter until the visual odometry returns working but actually I can't figure it out.

Thank you in advance for all your support!

---

Originally posted by nicopal on ROS Answers with karma: 11 on 2018-03-30

Post score: 1

---

Original comments

Comment by Dronist on 2020-07-29:
I've tried to fuse the data from the visual odometry with the IMU(all from the realsense D435i) And I'm getting this drift as shown in the video: https://drive.google.com/file/d/1-_k5nmxgYUYPFH6EEnVOruqDtIrEcOlu/view?usp=sharing The second the visual odometry gets lost the odometry starts drift Even when Im setting publish_null_when_lost to false Any suggestions?

Answer 1

Hi, you can set parameter publish_null_when_lost to false of the visual odometry node. Optionally, you can also set Odom/ResetCountdown to 1 to automatically reset odom when lost. Make sure publish_tf of visual odometry node is false too if you are using robot_localization.

If you are using odometry's velocity, note that I just committed a fix to avoid publishing null velocity when publish_null_when_lost is false. In other words, visual odometry won't publish odometry until a valid velocity can be computed.

EIDT: Note that without rebuilding from source the package, you can also just republish the odometry message from visual odometry only if the values you need are valid.

cheers

---

Originally posted by matlabbe with karma: 6409 on 2018-03-31

This answer was ACCEPTED on the original site

Post score: 3

---

Original comments

Comment by nicopal on 2018-04-03:
Thank you very much matlabbe! Your help is precious!