Covariance is not transformed in do_transform_pose_with_covariance_stamped

[aprotyas]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• Installation type:
  • From source
• Version or commit hash:
  • a4f152f
• DDS implementation:
  • N/A
• Client library (if applicable):
  • rclpy

Steps to reproduce issue

Expected behavior

do_transform_pose_with_covariance_stamped should also transform covariance.

Actual behavior

do_transform_pose_with_covariance_stamped copies the covariance without transforming it.

Additional information

The error is in this line

geometry2/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py

Line 106 in a4f152f

res.pose.covariance = pose.pose.covariance

This issue is quite similar to #372. For reference, this same functionality has been implemented in C++ in #430.

[vineet131]

Hi guys!
I am relatively new to ROS and wanted to try and contribute. I can see that this change has been made for the C++ API so I wanted to try and help for the Python API as well as it seems to not have been done.

I wrote up the following code below. It's nothing but a copy paste of #430 but I translated the relevant parts to Python code. I couldn't find the equivalent of tf::Matrix3x3 in Python so I just used np.array instead. I also couldn't find a transform.getBasis() method so I just used this link instead

`
def transformCovariance(cov_in, transform):
import numpy as np
q0 = transform.transform.rotation.w
q1 = transform.transform.rotation.x
q2 = transform.transform.rotation.y
q3 = transform.transform.rotation.z

# First row of the rotation matrix
r00 = 2 * (q0 * q0 + q1 * q1) - 1
r01 = 2 * (q1 * q2 - q0 * q3)
r02 = 2 * (q1 * q3 + q0 * q2)
 
# Second row of the rotation matrix
r10 = 2 * (q1 * q2 + q0 * q3)
r11 = 2 * (q0 * q0 + q2 * q2) - 1
r12 = 2 * (q2 * q3 - q0 * q1)
 
# Third row of the rotation matrix
r20 = 2 * (q1 * q3 - q0 * q2)
r21 = 2 * (q2 * q3 + q0 * q1)
r22 = 2 * (q0 * q0 + q3 * q3) - 1

R =  np.array([[r00, r01, r02],
               [r10, r11, r12],
               [r20, r21, r22]])

R_transpose = np.transpose(R)

cov_11 = np.array([cov_in[:3], cov_in[6:9], cov_in[12:15]])
cov_12 = np.array([cov_in[3:6], cov_in[9:12], cov_in[15:18]])
cov_21 = np.array([cov_in[18:21], cov_in[24:27], cov_in[30:33]])
cov_22 = np.array([cov_in[21:24], cov_in[27:30], cov_in[33:]])

result_11 = R * cov_11 * R_transpose
result_12 = R * cov_12 * R_transpose
result_21 = R * cov_21 * R_transpose
result_22 = R * cov_22 * R_transpose

cov_out = PoseWithCovariance()

cov_out.covariance[0] = result_11[0][0]
cov_out.covariance[1] = result_11[0][1]
cov_out.covariance[2] = result_11[0][2]
cov_out.covariance[6] = result_11[1][0]
cov_out.covariance[7] = result_11[1][1]
cov_out.covariance[8] = result_11[1][2]
cov_out.covariance[12] = result_11[2][0]
cov_out.covariance[13] = result_11[2][1]
cov_out.covariance[14] = result_11[2][2]

cov_out.covariance[3] = result_12[0][0]
cov_out.covariance[4] = result_12[0][1]
cov_out.covariance[5] = result_12[0][2]
cov_out.covariance[9] = result_12[1][0]
cov_out.covariance[10] = result_12[1][1]
cov_out.covariance[11] = result_12[1][2]
cov_out.covariance[15] = result_12[2][0]
cov_out.covariance[16] = result_12[2][1]
cov_out.covariance[17] = result_12[2][2]

cov_out.covariance[18] = result_21[0][0]
cov_out.covariance[19] = result_21[0][1]
cov_out.covariance[20] = result_21[0][2]
cov_out.covariance[24] = result_21[1][0]
cov_out.covariance[25] = result_21[1][1]
cov_out.covariance[26] = result_21[1][2]
cov_out.covariance[30] = result_21[2][0]
cov_out.covariance[31] = result_21[2][1]
cov_out.covariance[32] = result_21[2][2]

cov_out.covariance[21] = result_22[0][0]
cov_out.covariance[22] = result_22[0][1]
cov_out.covariance[23] = result_22[0][2]
cov_out.covariance[27] = result_22[1][0]
cov_out.covariance[28] = result_22[1][1]
cov_out.covariance[29] = result_22[1][2]
cov_out.covariance[33] = result_22[2][0]
cov_out.covariance[34] = result_22[2][1]
cov_out.covariance[35] = result_22[2][2]

return cov_out`

And this just gets added to do_transform_pose_with_covariance_stamped(pose, transform) as res.pose.covariance = transformCovariance(pose.pose.covariance, transform)

[aprotyas]

Welcome! Thanks for the write up. What you have looks mostly fine - and is the path that I would've taken to fix this issue too.

However, I believe I didn't pursue this issue earlier because the Python API actually does not get installed (see snippet from CMakeLists.txt file), since there are some pending TODOs for #110 (specifically #90 (comment)).

geometry2/tf2_geometry_msgs/CMakeLists.txt

Lines 21 to 28 in 8b12844

	# TODO(ros2/geometry2#110) Port python once PyKDL becomes usable in ROS 2
	# ament_python_install_package(${PROJECT_NAME}
	# PACKAGE_DIR src/${PROJECT_NAME})
	# TODO(ros2/geometry2#110) Port python once PyKDL becomes usable in ROS 2
	# install(PROGRAMS scripts/test.py
	# DESTINATION lib/${PROJECT_NAME}
	# )

I will look into if the Python API can be installed now. Regardless, if you don't mind opening a draft PR with what you suggested, I can give you a proper review there. Please remember to update the tests as necessary for the changes you introduce.

[vineet131]

Thanks for the feedback! I have the PR ready, I need to confirm the tests. I will try and submit a PR before the day's end

[clalancette]

I will look into if the Python API can be installed now

Unfortunately not. We still need to finish ros2/orocos_kinematics_dynamics#19 before we can re-enable Python for this package.

[aprotyas]

I think this issue is actually solved since #453 was merged? If so, feel free to close it please!

[clalancette]

I'm going to close this out per the last comment. Thanks for the PR.