Conditional control of generating odometry output

[SahanGura]

Feature request

Selecting a primary source of measurement, and using it for conditionally controlling odometry output

Feature description

When using robot localization package to fuse visual odometry and IMU data for getting the visual inertial odometry, I have met with the following issue.

This is the method I used to get the filtered odometry

• RGBD data ---> rgbd_node of rtabmap ---> visual odometry (/vo)
• IMU raw data ---> Madgwick filter ---> orientation estimation in ENU frame (/imu/data)
• (/vo) + (/imu/data) ---> ekf_node ---> filtered_odometry (/odometry/filtered)
• (/odometry/filtered) ---> rtabmap ---> map_data

When the rgbd_odometry node loses the odometry, EKF cannot understand it and it tries to predict odometry using the state transition model without visual odometry. Therefore the generated map starts to distort, from the moment the first odometry loss happened.

In this case, primary source of measurement should be visual odometry. Because, if visual odometry is lost, there is no point of continue predicting odometry using IMU data, as it results generating inaccurate odometry output which leads to generating a distorted map. There should be a way to communicate this odometry losing moment to the EKF, so it can stop predicting inaccurate odometry values. That is what happens when we feed visual odometry generated by rgbd_odometry node to the rtabmap node directly. rtabmap node identifies when odometry is lost, and it stops mapping until odometry is properly given.

[Timple]

For short term, the odometry probably is still quite accurate because of the imu. But this will get worse in time. The covariance in de odometry topic reflects this (it will increase).

Perhaps rtabmap can use that information (or threshold it?) to determine if the given odometry is good enough to use?

[ayrton04]

I presume your robot is still moving at the point that the odometry data is lost. How do you plan to cope with this? If the EKF just stops producing output, then when it resumes, the state estimate will not account for any of that "outage" period.

The issue here is that you are not measuring state variables that need to be measured, so your covariance is exploding.

Your feature request notwithstanding, you have two options within the current framework:

1. You can have some node detect the loss of vo data and either make a service call to pause the EKF, or to output dummy messages. Then you could feed those to the EKF as another input.
2. Some node in your system must be issuing control commands. If you can make that node (or an interim node) convert the commands to a TwistWithCovarianceStamped message, you could feed those measurements (with large covariance values) as an additional input to the EKF. When the vo data is available, the filter will trust it more. When it's not available, the filter will still have your commanded velocities to keep it from diverging.

[SahanGura]

@ayrton04 Thank you for your suggestion. I would try the first option you have suggested to deal with this issue.

I presume your robot is still moving at the point that the odometry data is lost. How do you plan to cope with this?

Since I have to use the visual odometry as the primary source of measurement, I should make sure to move the camera in a way it would not lose the odometry. In case of an odometry loss, I should operate the robot in a way it can get back to the track so it can continue providing the visual odometry again. This is the basic method we follow when using rtabmap and rgbd_odometry node in remote mapping.