Covariance for poses

[NikolausDemmel]

Is there a way in the ARNL library to access the current localization covariance? There is a ToDo in the ros node (

ros-arnl/rosarnl_node.cpp

Line 178 in 19702d6

// TODO add covariance to position

) and I assume that internally ARNL does compute a covariance, but is there an API to access it?

[reed-adept]

There is a function in the ArBaseLocalizationTask interface called findLocalizationMeanVar. I haven't looked deeply into it to see how to properly transform that to what might be expected in ROS. I will also look into whether there is a good way to estimate that for rosaria as well, if I have time. I don't know if anything useful is available from the robot controller (which is integrating odometry and calculating the first stage of position estimate) but maybe it could be as simple as a coefficients you could manually set in rosparam which would produce covariance as a function of speed or something like that? Not sure.

[NikolausDemmel]

Thanks, that looks like the right thing. Two questions about findLocalizaitonMeanVar:

1. What is the corresponding timestamp to the Pose and variance returned by findLocalizationMeanVar? Is it always "now"?
2. What are the units of the covariance matrix? I assume it would be a 3x3 matrix for the components x, y, th. Is that [m] [m] [rad], or [mm] [mm] [deg], or ...?

[reed-adept]

The mean pose is certainly mm/deg. I think the variance matrix is also in those units.

Also calling findLocalizationMeanVar() causes a bit of extra computation so should be a bit careful on how frequently its called. I think right now ros-arnl publishes localization pose on the ARIA cycle which is 100ms. But maybe if we add this I might also put in a flag somewhere to disable it if desire, or cache it somehow (since ARNL does relocalization asynchronously from other tasks such as robot communication and path planning/navigation, and then only if the robot is moving.)

[reed-adept]

Yes, The Vars should be
(mmmm, mmmm, mmdeg)
(mmmm, mmmm, mmdeg)
(mmdeg, mmdeg, deg*deg)

With mm from x and y, deg from Theta

By the way, the non diagonal values may not be accurate.... can you use only the diagonal elements?

[NikolausDemmel]

Thanks for the response.

Diagonal elements are fine.

I'm more concerned about the timestamps. It seems they are not that accurate. If you simply take the current system time when sending out the pose to ROS, there might be quite some offset to the actual time that the pose corresponds to (since pose computation takes some time).

However we are right now accurate pose time is not our highest priority, so I won't bug you further on this for the time being.

[reed-adept]

Timestamps for laser data (and robot pose (perhaps interpolated between robot pose updates if necesary) at that time) are available in ARIA, haven't looked into how to expose that to ROS yet. Use issue #2 to track that issue.

[NikolausDemmel]

Makes sense. I'll close this. Thank you!

[reed-adept]

I don't want to close this until the covariance publishing is actually implemented, so let's leave it open until that change is pushed/merged.

[NikolausDemmel]

Sure!

[reed-adept]

I've now added this to ros-arnl, let me know if it looks correct or if there are any apparent problems with it. Values look reasonable to me "by eye" but I don't have any tests for how it might affect other software that's consuming this data.