Handle lost visual odometry with IMU sensor fusion
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!
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-_k5... The second the visual odometry gets lost the odometry starts drift Even when Im setting publish_null_when_lost to false Any suggestions?