Ask Your Question
0

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
0

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: 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/...

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

2 followers

Stats

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

Seen: 387 times

Last updated: May 16 '18