Realsense-ros: T265 publish covariance

[ahmadfarisfs]

Hi, I see here https://github.com/IntelRealSense/realsense-ros/pull/734
It stated that t265 will not publish relevant covariance matrices for its estimate. Why this happen ? I think its better to spit out covariance matrices as well, so its easier if user intented to fuse t265 pose estimate with other source.

Thx in advance

[western-co-at]

Hello. I would also be interested in getting the covariance of the pose estimation of the t265.

[doisyg]

Hello,
Jumping on the issue, and as discussed with @doronhi at ICR, it would be highly valuable to have some kind of confidence metric (covariance would be ideal) about the quality of the motion estimation. To be clear, in order to be usable by a fusion algorithm, we don't need the pose information and the loop closure (which currently generate non continuous trajectory when closing a loop), but the motion estimate (Odom twist).
Even something as naive as 2 states GOOD/BAD would be beneficial. Like BAD when in the dark or in a featureless environment, and GOOD for the rest.

[doronhi]

@doisyg,
The motion estimation is available in the "twist" part of the Odometry message. The "Covariance" given in the field "twist.covariance" is not really a covariance, true enough, but derived from the field "tracker_confidence" in the original librealsense output. As it has 4 levels, it can give the information of VERY_GOOD/GOOD/MEDIUM/BAD in values configurable by the realsense2_camera's users.
Would that answer?

[BriceRenaudeau]

Hello,
Is the twist covariance changing according to the field "tracker_confidence" you are mentioning?
In my experiments, the twist covariance starts at 0.1 for 10sec then goes to 0.01 for the rest of the mission (more than 500sec).

How can we access the "tracker_confidence" using ROS wrapper?

[doronhi]

In librealsense, the pose class has a "tracker_confidence" field with 4 levels (Failed, Low, Medium and High). Indeed, there is a PR #734 suggesting to replace the covariance with tracker_confidence.
However, the pose is published in the format of nav_msgs/Odometry which contains covariance fields.
In order to avoid redundancy and to follow what seems like the ROS way of passing this information, the realsense2_camera ROS wrapper creates a "covariance" matrix, based on the following calculations:
for pose: _linear_accel_cov * pow(10, 3-(int)pose.tracker_confidence)
for twist: _angular_velocity_cov * pow(10, 1-(int)pose.tracker_confidence)

"_linear_accel_cov" and "_angular_velocity_cov" can be set in the launch file using linear_accel_cov and angular_velocity_cov respectably.
By default _linear_accel_cov=_angular_velocity_cov=0.01.
You could manipulated the values in the launch file to match your experience with the device and use the output covariance with other ROS packages.
You can also access the original "tracker_confidence" values using the following table:

cov value    | confidence
-------------------------
0.01         | High
0.1          | Medium
1            | Low
10           | Fail

You could also notice that at this point, the confidence level only describes the current, most recent, situation of the device. This is a known issue and will be addressed in future versions. If you block the T265's vision for a while, the confidence drops. When the device gains its vision again its confidence may rise to describe the fact that it is now tracking enough features, although, it may not in the correct global position.
If you use the velocities, both linear and angular, it's fine but for the position it could be problematic.
That behavior, combined with the fact that it only has 4 levels of confidence, could explain why you barely see the covariance change.

[doisyg]

Thanks @doronhi !

Few remarks and questions:

for pose: _linear_accel_cov * pow(10, 3-(int)pose.tracker_confidence)
for twist: _angular_velocity_cov * pow(10, 1-(int)pose.tracker_confidence)

That's actually not the case from my understanding. There seems to be a confusion on how the covarience is set, if we look at the code of base_realsense_node:
https://github.com/IntelRealSense/realsense-ros/blob/31fa75ec835650d319f2d5abd3f7d0ab66fd37f4/realsense2_camera/src/base_realsense_node.cpp#L1300-L1301
and then:
https://github.com/IntelRealSense/realsense-ros/blob/31fa75ec835650d319f2d5abd3f7d0ab66fd37f4/realsense2_camera/src/base_realsense_node.cpp#L1330-L1343
The linear part (X,Y,Z) of both the pose covariance and the twist covariance seems to be set to _linear_accel_cov * pow(10, 3-(int)pose.tracker_confidence)
And the angular angular part (rotation X, rotation Y, rotation Z) of both the pose covariance and the twist covariance seems to be set to_angular_velocity_cov * pow(10, 1-(int)pose.tracker_confidence). Which is okay. It is just very unfortunate and misleading to have named, in the code, cov_pose the linear part of the covariance and cov_twist the linear part.

You could also notice that at this point, the confidence level only describes the current, most recent, situation of the device. This is a known issue and will be addressed in future versions. If you block the T265's vision for a while, the confidence drops. When the device gains its vision again its confidence may rise to describe the fact that it is now tracking enough features, although, it may not in the correct global position.
If you use the velocities, both linear and angular, it's fine but for the position it could be problematic.

I understand the pose covariance can be complex to compute, specially with localization against a map and loop closure. But we are interested in the covariance of the low level motion estimation (twist), which should be independent from any map or loop closure, and I believe, easier to compute.
These 4 levels of confidence can get us started, however, in the future it would really help if we could get a more granular confidence variable, and in particular if the angular and linear part could be separated (and even better the 3 dimensions X, Y and Z).

Another point of improvement would be to output a twist based on the IMU with the appropriate covariance when the tracking is failing. For now the publication of the odom topic is stopped when the tracker is in the lowest level of confidence.

[doronhi]

Regarding the confusing variable names, I agree. It should be changed.

The low level covariance sounds like an interesting option to me. @radfordi ? @schmidtp1 ?

[radfordi]

We should not be mapping our "confidence" enum to a float covariance. It's arbitrary and confusing. We do have the covariances you seek, internally. They are local by nature, but we could expose them.

The T265 should continue to output orientation when the confidence drops to 1, but that seems to not be the case. I'll look into fixing that.

[doisyg]

. They are local by nature, but we could expose them.

The T265 should continue to output orientation when the confidence drops to 1, but that seems to not be the case. I'll look into fixing that.

That would be great and very useful to us!

[crheckman]

Has there been any update to this? We could really use it for our application as well. Thank you!

[theikkila]

This issue affects us as well. We would like to use the internal covariances as well. Do you have any timeline for that? @radfordi @schmidtp1

[RealSenseSupport]

Thank you for highlighting the need for published internal covariance. We have moved our focus to our next generation of products and consequently, we will not be addressing this in the T265.