Open Hypothesis-Z opened 1 year ago
The following code is how this repository rotate covariance: https://github.com/ros2/geometry2/blob/d1dc38b8a0e706fbd67a800a241fee950fce39f4/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py#L101-L104
This is how mrpt rotate covariance: https://github.com/MRPT/mrpt/blob/4af6ea558bf75af9bc9798abfc3745fe7ce39c66/libs/poses/src/CPose3DPDF.cpp#L134-L158
Obviously different. It seems euler angles cannot be rorated via left-multipling a rotation matrix. Therefore, transforming a covariance matrix of euler angles is not the same as transforming translation's:
$$ Cov(RX)=R\cdot Cov(X)\cdot R^T $$
The following code is how this repository rotate covariance: https://github.com/ros2/geometry2/blob/d1dc38b8a0e706fbd67a800a241fee950fce39f4/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py#L101-L104
This is how mrpt rotate covariance: https://github.com/MRPT/mrpt/blob/4af6ea558bf75af9bc9798abfc3745fe7ce39c66/libs/poses/src/CPose3DPDF.cpp#L134-L158
Obviously different. It seems euler angles cannot be rorated via left-multipling a rotation matrix. Therefore, transforming a covariance matrix of euler angles is not the same as transforming translation's:
$$ Cov(RX)=R\cdot Cov(X)\cdot R^T $$