tf::Transform a covariance Matrix

edit

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]);