Uncertainty Data

[cjstahoviak]

Problem

We want to eventually incorporate uncertainty in the robots pose into the dynamic admittance controller. Uncertainty measurements come from processing a covariance matrix. We can get a PoseWithCovariance topic data from the EKF node's odometry/filtered topic or from SLAM's pose topic.

Solutions

We have a couple options for getting PoseWithCovariance topic data. This is what is needed to get either of these working:

1. EKF: An EKF will combine the wheel/odom and imu/data topics and estimate the pose and covariance of the robot. I don't think the IMU is contributing any meaningful information right now, while the wheels surely do. You can test by visualizing the odometry/filtered topic, pickling up the robot, and moving it around -- nothing will happen.

2. SLAM: This will use depth data to track the environment and determine where the robot is in space. It often also takes in another odemtry source such as wheel/odom or the EKF's odom/filtered data to help guess. I think the reason SLAM was failing us previously was because it only used a horizontal laser scan to map and localize. SLAM on a full 3D point cloud would be much much more accurate. We can even consider using other "SLAM" algorithms such as gmapping or RTAB-Map if those are better.

OVERALL, we need a reliable PoseWithCovariance data stream.

[Xavier-VI]

The issue with the IMU sensor on the realsense might be due to the fact it doesn't publish an orientation (source). You apparently have to use something like IMU filter madgwick to get an orientation.

I'm working on setting this up right now, but I will also look into using a 3D pointcloud with SLAM.

Later
I was able to update some of the parameters to get the robot to detect it's physically getting rotated. These changes are on the branch uncertainty_data. Don't know if this would qualify as a reliable PoseWithCovariance data stream yet.

[cjstahoviak]

I'm curious if the EKF odom is better with the IMU. One concern with an IMU is that it is very sensitive to vibrations, and we have our RealSense camera mounted on a long rod that isn't exactly "stable". So the vibrations induced during driving could actually make odometry worse.

Not sure the best way to test this. Maybe drive a meter with and without IMU data incorporated into the EKF and compare the final odom values?

SLAM
3D SLAM would most likely be the best. I've seen videos of RTAB-Map and similar algorithms doing almost perfect odometry, it's a matter of if the Intel NUC can handle the computation I think.

[Xavier-VI]

Some random notes on RTAB

• The example script they have for the RTAB seems to be working (color image example). The output is suggesting it's producing uncertainty measurements for the orientation of the robot.
• They do not have an example for the camera we use (which is the D445f). So I'm not sure what settings need to be adjusted from the example.
• The rqt graph shows the RTAB nodes take in IMU data from the camera. It also uses the imu filter madgwick node to produce proper IMU data.
• The rtab node seems to be the node which produces our desired uncertainty data. The topic this data is published to might be the /localization_pose topic.

Problems

• I tried replacing the SLAM toolbox with RTAB, and I'm having issues with the TF tree for the robot and a frame called "camera_imu_optical_frame". This frame is created by the rs_launch. In the examples, it expects a transform between a "camera_link" and this frame. When this is established, I just get the message "Dropping IMU data!" and don't know what to do to fix this. This is a weird interplay between RTAB and the IMU filter.
• I'm a bit confused on how RTAB is supposed to take in secondary odometry data. There is mentions of how to do it here and kind of here, but it is not clear at all how this is supposed to be properly set up. I have found no documentation for the parameters for RTAB nodes. (Already fixed this, the problem was madgwick was using /tf instead of /monica/tf)

[Xavier-VI]

So far, RTAB seems to be working with the external odometry (Wheel odom -> EKF and Visual odometry).

I tried driving moncia 2 meters forward at 0.1 m/s, and the covariance sigma_xx did not significantly change. All the values in covariance matrix remained near zero. I do not know if this is a good sign or a bad sign.

[cjstahoviak]

Good progress!

We should confirm, does RTAB-Map state that their documentation that it generates covariance with its pose? I might be mistaken on that as well...

If covariance is 0 that seems to indicate it isn't working or being calculated.

[Xavier-VI]

I added a notebook here to plot the covariances.

I tried looking for official documentation for the localization pose topic, but I could not find anything. It does compute covariances, as my second test ended with much larger covariances (although, they are still relatively small).

I think there is a big disconnect between where RTAB thinks the robot is on the map and where it actually is in the lab and this is causing the small covariances.

The image included shows the map and the pointcloud after driving the robot around for awhile. The red arrow is the output of the localization_pose topic. It seems to think the robot was driving back into the corner of the lab (where we usually start them), but it was actually driving out in front of the cabinets.

I wonder if this is caused by the rate at which the RGB images are published (1 Hz according to ros2 topic hz).

[Xavier-VI]

It seems we are planning on using a RealSense T265, so I'm going to put this task on hold until we get the camera's (or something else comes up).

One issue with this is the covariances are not directly published (see link below). Instead, the camera publishes a value which represents its "confidence". We might be able to fix this by modifying the source code ourselves to expose the covariance values directly.

realsenseai/realsense-ros#770