# tf::Transform a covariance Matrix

Hey there,

I simply would like to transform a covariance Matrix from one coordinate frame to another one.

Now what I have is the covariance matrix of a position which simply consists out of 3 diagonal values so basically a vector for the covariance in x, y, and z direction.

                [a, 0, 0
Cov_mat =        0, b, 0
0, 0, c]


Moreover I have the rotation matrix between the two coordinate frames I would like to transform (named R)

Now I would like to transform like this:

new_Cov = RCov_matR^T

My problem the new covariance matrix is 3x3 and can I also now simply extract the diagonal values of this matrix?

Cause as a result I also would like to just have 3 covariance values for the major axis ....

One Idea I saw was doing this:

new_Cov.getRPY(result_cov, result_cov, result_cov);

But i do not understand in detail what this method getRPY of a matrix does ....

My full code looks like this now:

  tf::Quaternion cov_pos_quat;
cov_pos_quat.setRPY(cov_in, cov_in, cov_in);
tf::Matrix3x3 rotation_m = tf::Matrix3x3(transform.getRotation());
tf::Matrix3x3 cov_pos = rotation_m*tf::Matrix3x3(cov_pos_quat)*rotation_m.transpose();
cov_pos.getRPY(cov_out, cov_out, cov_out);

edit retag close merge delete

Sort by » oldest newest most voted You can find the documentation of the setRPY method here: http://docs.ros.org/kinetic/api/tf/ht...

The implementation can be found here: (note this is the melodic branch but it is the same since indigo) https://github.com/ros/geometry/blob/...

This method modifies the quaternions and given the Roll Pitch Yaw convention, which is the aeronautic convention for defining angles: https://en.wikipedia.org/wiki/Aircraf...

getRPY is the exact opposite, given a matrix you get the angles corresponding to the convention, the implementation can be found here: https://github.com/ros/geometry/blob/...

more