Huge covariance values in /poseupdate

[tu-darmstadt-ros-pkg]

Huge covariance values in /poseupdate

[meyerj]

It would not be hard to add an extra parameter to select whether to output the uncertainty as a covariance (with the typical ROS convention to encode infinity by -1 on the diagonal elements) or information matrix (both have an unknown scale factor that depends on the accuracy of the input data).

I do not have time to implement this myself, but pull requests are definitely welcome (@skohlbr?).

[meyerj]

Yes. But some components can have zero columns and rows, so you have to be careful to handle this case correctly. For example, you could invert the sliced matrix that only contains the rows and columns with non-zero diagonal elements and replace the others by -1 or Nan in the inverted matrix. Should be doable with some Eigen magic. Or simply set the whole covariance matrix to -1 if the Hessian is not invertible. It really depends on how the receiving end would interpret this. I am not aware of a ROS standard on how partial covariance matrices (or partially infinite variances) should be represented in messages.

That's exactly the reason why I think the information matrix is the more natural representation of uncertainty here and it can be directly used as a weighting matrix for fusion algorithms.

[meyerj]

We are violating the specification of the geometry_msgs/PoseWithCovariance message here. What is output in the poseupdate message is not a covariance, but the Hessian calculated during the scan matching process (see ScanMatcher.h:184). It can be interpreted as the inverse of the covariance matrix up to a constant factor that depends on the variance of the distance measurement of a single laser ray and the map resolution.

The Hessian (or information matrix) has the advantage that its diagonal elements go to zero for maximum uncertainty instead of having to represent this situation with -1 like in other occurrences of variances in ROS.

[12tbuchman]

@meyerj I am trying to use Hector SLAM as a visual odometry source with an IMU, both of which will pass through an EKF to provide the odometry for AMCL localization. However because we don't have a true covariance from Hector SLAM the sensor fusion is much more difficult. Is it possible to output the real covariance instead of the Hessian?

[doisyg]

@meyerj , so you are saying that if we inverse the 6x6 poseupdate covariance message, we get the covariance up to a constant factor? As simple as that?

[doisyg]

Thanks for your answer @meyerj .
I intended to use directly the output of the /poseupdate message as an input pose for the robot_localization package. But after thinking about it, as in practice some kind of covariance tuning is always needed for Kalman filtering, your idea of using it as a weighting matrix makes more sense.

[meyerj]

Simply considering the map pose as a standard Kalman filtering measurement update is not recommended, because the pose errors in each iteration are not independent and hence violating the Kalman assumptions. hector_localization therefore uses a different approach for pose updates based on covariance intersection, but there are other options. The same is actually true for a pose provided by a GNSS receiver, which is also a filtered result and errors are not independent over time. Not sure about the implementation of robot_localization in that respect.

[doisyg]

I am aware that the map pose is violating the Kalman assumptions. However, using the yaw information in a differential way is showing very interesting result.