ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A
Ask Your Question

tf::Transform a covariance Matrix

asked 2018-05-16 01:57:02 -0500

Markus gravatar image

updated 2018-05-16 02:08:58 -0500

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[0], result_cov[1], result_cov[2]);

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[0], cov_in[1], cov_in[2]);
  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[0], cov_out[1], cov_out[2]);
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2018-05-16 07:46:20 -0500

VictorLamoine gravatar image

updated 2018-05-16 07:50:19 -0500

You can find the documentation of the setRPY method here:

The implementation can be found here: (note this is the melodic branch but it is the same since indigo)

This method modifies the quaternions and given the Roll Pitch Yaw convention, which is the aeronautic convention for defining angles:

getRPY is the exact opposite, given a matrix you get the angles corresponding to the convention, the implementation can be found here:

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2018-05-16 01:57:02 -0500

Seen: 1,195 times

Last updated: May 16 '18