Rot3 R_; ///< Rotation gRp, between global and pose frame |
Thanks for the response! It is helpful. I have been doing random simulations to try and understand 3D pose covariance in ROS2/RVIZ2 and GTSAM. I suspect the two systems are defining covariance in different frames. What is the proper way to rotate the full GTSAM 6x6 covariance matrix from the body frame to the world frame?
--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/b1ae9d51-0e51-473e-9bb9-fcff6303faed%40googlegroups.com.
Hi Mike, Thanks for the pointer to the header file! I think I understand the GTSAM pose concept. I have not found the document/code that explicitly defines the definitions of the columns/rows of the Pose3 covariance matrix. I think the columns are [Rx, Ry, Rz, x, y, z] where Rx is a rotation around the x axis of the pose frame.
Thanks for the pointer to the MATLAB code. I think this code is rotating only the spatial part of the covariance. I am interested in how to rotate/transform the full 6x6 covariance matrix from one 6 dimension 3D pose space to another. Another example would be a robot with a camera at one end. Say we use GTSAM to find the pose and covariance of the camera in the world frame. How should the camera pose covariance be transformed to find the covariance of the robot pose in the world frame. This must be done all the time but I am learning and haven't found the right reference yet.
--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/ce06c2b5-727f-4cbc-befc-ea0dbd2fae16%40googlegroups.com.
Hi Mike, Thanks for
the very helpful response. I will dig into your links and am very
intrigued by the "epic fail". I will look at the Adjoint Map. I agree
that this is probably what I am after.
On Dec 5, 2019, at 12:38 AM, ptrmu <pe...@peterandwendy.com> wrote:
Hi Mike, Thanks for the very helpful response. I will dig into your links and am very intrigued by the "epic fail". I will look at the Adjoint Map. I agree that this is probably what I am after.
--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/46948309-87b3-4e3b-8f22-257f8b748a99%40googlegroups.com.
The great discussion in the “Uncertainty Definition” thread has prompted me to re-open this thread to see if someone else has a better solution than I have developed through trial and error. I can relate to the “pain and misery and suffering” that Paul Furgale mentions in his “Representing Robot Pose: The good, the bad, and the ugly” article. And Paul does not even get to covariance. The original question of this thread was how to convert 3D Pose Covariances between GTSAM and ROS.
1) The easy part of the conversion is that the translation and rotation variables are interchanged.
2) According to the “Uncertainty Definition” thread, GTSAM expresses covariance in the body frame. It seems like ROS represents the translation part of the covariance in the world frame while the rotation part is in the body frame. This seems odd.
3) GTSAM’s canonical rotation components are around relative axes while ROS rotates around static axes. Reversing the order of the rotation components converts between these two representations.
So I end up with the following conversion routine:
using Pose3CovarianceMatrixGtsam = Eigen::Matrix<double,
gtsam::Pose3::dimension, gtsam::Pose3::dimension>;
using Pose3CovarianceMatrixRos = Eigen::Matrix<double, 6, 6>;
static Pose3CovarianceMatrixGtsam cov_gtsam_from_ros(const
gtsam::Pose3 &pose_gtsam,
const Pose3CovarianceMatrixRos &cov_ros)
{
Pose3CovarianceMatrixGtsam cov_gtsam_f_world;
for (int r = 0; r
< 6; r += 1) {
for (int c = 0; c
< 6; c += 1) {
static int ro[]
= {5, 4, 3, 0, 1, 2};
cov_gtsam_f_world(r, c) = cov_ros(ro[r], ro[c]);
}
}
// Rotate the
translation part of the covariance from the world frame to the body frame.
Pose3CovarianceMatrixGtsam rotate_translate_part;
rotate_translate_part << gtsam::I_3x3, gtsam::Z_3x3, gtsam::Z_3x3,
pose_gtsam.rotation().matrix();
return
rotate_translate_part.transpose() * cov_gtsam_f_world * rotate_translate_part;
}
I am not confident about my reasoning at all. I would appreciate learning about better solutions or even complete documentation on the ROS covariance definition.
Thanks!To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/a85bbf1f-dfd9-446f-8822-c3084e707c38n%40googlegroups.com.