# Looking for a reference use of PoseWithCovariance: any pkg using it?

Hi all,

PoseWithCovariance has a 6x6 covariance matrix which, according to REP 103, corresponds to these variables in this order: (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis). With rotation around fixed axes, not like in the "common" yaw/pitch/roll various conventions.

I'm working in a pkg for covariance transformations (pose_cov_ops) so it would be great if I could compare my numerical results with any other existing ROS packages that output PoseWithCovariance at present. Checks would go in the package unit tests.

More explicitly: I'm looking for some "stable" ROS package which computes covariances in 6D, so I can peek its source code and make sure I'm following the right convention.

Any recommendation?

If there're none, I'll assume the 3x3 rotation matrix is as follows:

R = R_x(roll) * R_y(pitch) * R_z(yaw)

R = R_z(yaw) * R_y(pitch) * R_x(roll)

where matrices are in the inverse same order than the non-global axis, yaw-pitch-roll convention.

PS: wouldn't it have made more sense to keep a 7x7 covariance for the unit quaternion form? In general, equations for uncertainty propagation in this form are far simpler than for Euler angles. Is there room for a PoseWith7x7CovarianceStamped yet?

Originally posted by Jose Luis Blanco on ROS Answers with karma: 288 on 2012-03-30

Post score: 6

Comment by Jose Luis Blanco on 2012-04-11:
Just for the records: the numerical values of yaw/pitch/roll in the dynamic-axes (rotating) convention are exactly the same than those in the roll/pitch/yaw fixed-axes convention. So a conversion of covariance matrices between both formats becomes just a permutation. Hope it may help someone else.

AFAIK there is no ROS package which make the transform between two poses with uncertainty (perhaps http://ros.org/wiki/bfl but I don't think so). Because of this the package pose_cov_ops seems unique. I belive that the MRPT ([[mrpt-ros-pkg]]) in which the pose_cov_ops package is based looks very powerful handling poses with uncertainty and ROS should take advantage of this fact.

I think that this package should be integrated in the generic http://ros.org/wiki/tf2 package as a plugin in a similar way that the kdl package was integrated (see https://kforge.ros.org/geometry/experimental/file/40be50217595/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h). Perhaps the tf2 package should be refactored to support more explicitly the inverse tranformation or at least the unary inverse transform.

Edit: Definitely I think you can find what you are looking for in the source code of the package http://ros.org/wiki/robot_pose_ekf

Regards.

Originally posted by Pablo Iñigo Blasco with karma: 2982 on 2012-06-25

This answer was ACCEPTED on the original site

Post score: 1

There is tf_uncertainty that might do what you are looking for.

Originally posted by dornhege with karma: 31395 on 2012-06-25

This answer was NOT ACCEPTED on the original site

Post score: 1

Comment by Pablo Iñigo Blasco on 2012-06-26:
I don't locate in the package source code the gaussian error propagation (using Jacobians) produced by the nonlinear tranforms. The resulting uncertainty after a transform is represented by a pose cloud(particles) and not as a PoseWithCovariance. In such case it is not what José Luis is looking for.

uncertain_tf is an extension to tf that allows to maintain uncertainty in the translation and rotation of coordinate frames.

if you want to add uncertainity to your odometry data in 6D you should check out robot_pose_ekf

turtlebot and pr2 use uncertainity for their odom datas check out this example of turtlebot

https://kforge.ros.org/turtlebot/turtlebot/file/e47d5a1dca1c/turtlebot_node/nodes/turtlebot_node.py

here is another example from cwru robotics' package

https://github.com/cwru-robotics/cwru-ros-pkg/blob/master/cwru_semi_stable/cwru_base/src/odom_translator.py

Originally posted by cagatay with karma: 1850 on 2012-07-03

This answer was NOT ACCEPTED on the original site

Post score: 1