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?
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.