ROS PoseWithCovariance message <-> GTSAM Pose3 + marginals/noiseModel

[ptrmu]

Are there any examples of how to translate between these two systems? The ROS units are described here: https://www.ros.org/reps/rep-0103.html. I am particularly interested in the covariance part.

thanks!

[Frank Dellaert]

The units are up to you, so you can take them to be the same as in ROS. But the order of the covariance in ROS is different:

- # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) float64[36] covariance

In GTSAM, a pose is a (Rot3,Point3) pair, in that order. To convert from ROS [TT, TR; RT, RR] you just form [RR, RT; TR, TT], where each is a 3*3 block.

Frank

[ptrmu]

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?

[Mike Sheffler]

I'm not sure if this is the question you are asking, but look at the covarianceEllipse3d.m file in MATLAB where the frame of reference for the covariance is changed:

https://github.com/borglab/gtsam/blob/develop/matlab/%2Bgtsam/covarianceEllipse3D.m

Also, make sure that the concepts are the same; in GTSAM the pose describes a body-to-world transformation: note in Pose3.h how the comments say:

Rot3 R_; ///< Rotation gRp, between global and pose frame

Point3 t_; ///< Translation gTp, from global origin to pose frame origin

Mike Sheffler

On Tue, Dec 3, 2019 at 11:17 PM ptrmu <pe...@peterandwendy.com> wrote:

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.

[Frank Dellaert]

Note, also, when we do estimation it is always with respect to a linearization point. If you ask for a marginal covariance, it will be in the 6DOF *tangent* space of that linearization point.

On Friday, November 15, 2019 at 12:58:04 AM UTC-5, ptrmu wrote:

[ptrmu]

Hi Frank, Thanks for the Note! Your comment must be significant but I don't understand why. Could you point me to a reference that would help me understand the significance. I enjoy working with GTSAM and want to understand it better so I use it correctly in other projects.

[ptrmu]

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.

[Mike Sheffler]

ptrmu > Your comment must be significant but I don't understand why. Could you point me to a reference that would help me understand the significance

At the end of the day, we're solving non-linear optimization problems, but those are approximated by successive linear solutions. We quickly run into a problem where we notice that the idea of a 'delta' pose in a Taylor expansion doesn't really make sense (Frank calls this a 'Taylor expansion epic fail' in some slides for a talk at CVPR 2014: http://www.cs.cmu.edu/~kaess/vslam_cvpr14/media/VSLAM-Tutorial-CVPR14-A13-BundleAdjustment.pdf). The space that describes 3D poses (SE(3)) isn't a vector space, but it *is* a differentiable manifold, so there exists a 'tangent space' associated with it where we can do calculus in a familiar way. There is a map to go to the tangent space and a map to get back. Take a look at 'Lie Groups for Beginners': https://github.com/borglab/gtsam/blob/develop/doc/LieGroups.pdf and 'The New IMU Factor': https://github.com/borglab/gtsam/blob/develop/doc/ImuFactor.pdf.

His comment is significant because, when we head over to the tangent space, the route we took to get there (the linearization point) is important. It is *not* true that all routes wind up at the same tangent space that looks like R6 -- each tangent space *behaves* like R6, but they aren't necessarily the same. So, if you calculate a marginal covariance, it is valid according to the tangent space in which it was calculated, but relinearization (something we are potentially doing often -- being able to relinearize is one of the reasons we are smoothing in the first place instead of filtering) makes the old computation no longer meaningful (potentially). The other reason his comment is significant is that it is very unlikely that whatever you are comparing GTSAM answer to is being described with respect to the linearizat i on point GTSAM used to come up with its answer. It's just not an apples-to-apples comparison.

ptrmu > I think this code is rotating only the spatial part of the covariance.

That is correct.

ptrmu > 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

That's a little tricky. There is, of course, a rigid transformation that describes the pose of the camera with respect to the body (or vice versa), but that is probably not what you want. Take a look at the Adjoint Map (section 2.5 in 'Lie Groups for Beginners'). In this example, your action A might be the rigid transformation between the body and the camera, if I'm following your intent.

Ethan Eade's Lie Groups notes (http://ethaneade.com/lie.pdf) are a little more explicit than Frank's notes. Ethan writes, " In Lie groups, it is often necessary to transform a tangent vector from the tangent space around one element to the tangent space of another.The adjoint performs this transformation."

This is not easy! Or, at least, it is not easy for me. If I misspoke, someone should chime in and correct me. :)

Mike Sheffler

On Wed, Dec 4, 2019 at 12:15 PM ptrmu <pe...@peterandwendy.com> wrote:

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.

[ptrmu]

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.

[Frank Dellaert]

Yes, Mike is correct I think. See also formula (232) in http://ethaneade.com/lie.pdf

You could try this in GTSAM, if wTb is your pose and bSb your covariance (which is in the body frame), by doing:

wAb = wTb.AdjointMap()

wSw =  wAb * bSb * wAb.transpose()

Try it? And, if you have a good unit test, maybe do a PR to testPose2.cpp and/or testPose3.cpp (same recipe for both)

Frank

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.

[ptrmu]

Yes, using the AdjointMap to transform the covariance does seem to work. I will do a PR with some tests. Thanks!

[Dellaert, Frank]

Excellent! key for tests are they are simple, e.g., just diagonal covariance matrices and simple axis-aligned rotations. That will make the test super-easy to interpret and validate (as you can easily compute by hand). E.g. in SE(2) I would say

With bSb = diag(2,3,4)

wTb = (10, 10, 0) -> diag(2,3,4) ?

wTb = (0,0,90 degrees) -> diag(3,2,4) ?

wTb = (10,10,90 degrees) -> same?

Etc. I don’t have great intuition even in SE(2) for mixing of R and t, and maybe it only happens when Cov is non-diagonal, but the diagonal at least should make for understandable tests.

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of ptrmu <pe...@peterandwendy.com>
Sent: Sunday, December 8, 2019 12:50:10 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] Re: ROS PoseWithCovariance message <-> GTSAM Pose3 + marginals/noiseModel

 

--
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/82a3f17a-f056-4a8a-8477-3b604e52001c%40googlegroups.com.

[ptrmu]

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!

[Matías Mattamala]

Hi ptrmu,

To complement what we were discussing in the other thread and was previously mentioned in this, I think the Adjoints do the trick if you define explicitly where each library is applying the "noise" used to create the distribution (avoiding part of the pain we can all relate to). In fact, we had this conversation with some colleagues recently, and the covariances they were able to plot in ROS behave more reasonable:

- Let's say we have a fixed frame "world" (w) and a moving frame "base" (b)
- We agree that GTSAM defines the covariances on the right-hand side. So a pose with covariance is given by w_T_wb*Exp(b_N_b) .  N is a zero-mean Gaussian noise defined in frame b
- ROS defines its covariances in the fixed world frame, so we could interpret this as Exp(w_N_w) * w_T_wb
- In general, the previous 2 expressions are not equivalent. But "things defined on the right" are related to "things defined on the left" by the Adjoint: T * Exp(N) = Exp(Ad_T * N) * T
- Analogously: T * Exp( (Ad_T^-1) * N) = Exp(N) * T

Then, you can change from one convention to the other by applying the previous operations:

From GTSAM to ROS:
- Input: T_wb, and Sigma_b (base frame)
- "Move the covariance to the left" using the Adjoint. The transformation will be: Ad_T_wb * Sigma_b * Ad_(T_wb)^T which is the covariance in the fixed world frame
- Then, swap the orientation and position blocks accordingly to follow the "position then orientation" convention of ROS (3D poses)
- Output: covariance Sigma_w of the pose in frame b, but "observed from" the fixed world frame.

From ROS to GTSAM:
- It should be similar in the other direction: Swap the covariance block to use "orientation then position" of GTSAM, and applying the Adjoint in the other direction to move it "to the right"

Some notes:
- As Mike mentioned before in this thread, the covariances are defined locally in the frame, and they are also valid with respect to the linearization points
- If you check the implementation of the Pose3 Adjoint, it is effectively applying a rotation to both position and orientation (which makes sense with our intuition that covariances should be rotated somehow). 
- ROS plots the covariances in a decoupled way: an ellipsoid for the position, and the orientation is plotted per orientation axis, so it's strange to visualize anyways.
- If you could actually sample the distribution and plot it (which is probably a bad idea because it's gonna be super expensive to compute online), you should obtain a "banana" distribution.

I hope it makes sense. Best
Matias

[Dellaert, Frank]

If anyone wants to venture a PR to convert covariances back and forth between ROS and GTSAM, that would be great :-)

I think it might fit nicely in Pose3.h/cpp

That being said, since GTSAM is traits-based, and an entirely different approach is to use a ROS pose, and just define the GTSAM traits for it. Then you can use it as unknowns in a factor graph. Lie.h has utilities (used by Pose3) that generates the traits automatically based on class methods. But even without modifying a type, you can define the traits for any type, without modifying it. To optimize, you actually only need Manifold traits, checked by 

  BOOST_CONCEPT_ASSERT((IsManifold<MyAmazingType>));

Defined in Manifold.h:

/// Manifold concept

template<typename T>

class IsManifold {

public:

typedef typename traits<T>::structure_category structure_category_tag;

static const int dim = traits<T>::dimension;

typedef typename traits<T>::ManifoldType ManifoldType;

typedef typename traits<T>::TangentVector TangentVector;

BOOST_CONCEPT_USAGE(IsManifold) {

BOOST_STATIC_ASSERT_MSG(

(boost::is_base_of<manifold_tag, structure_category_tag>::value),

"This type's structure_category trait does not assert it as a manifold (or derived)");

BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);

// make sure Chart methods are defined

v = traits<T>::Local(p, q);

q = traits<T>::Retract(p, v);

}

private:

TangentVector v;

ManifoldType p, q;

};

Of course, many other things in GTSAM are based on Pose3, but for PoseSLAM this would be all you need.

Frank

To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/a85bbf1f-dfd9-446f-8822-c3084e707c38n%40googlegroups.com.

[ptrmu]

Hi Matias,

Thanks for your response! This is helpful. I agree that is is hard to interpret how ROS displays covariances (I'm using rviz2). Is there any chance your colleagues who work with ROS would be willing to share a snippet of their code that translates covariances. It would be great to see the exact implementation. Or they could describe how their approach is different from the approach in the code I included above.

Thanks!

[ptrmu]

Wow Frank, this is eye-opening. I must admit I have been ignoring traits while reading GTSAM code trying to understand what is going on. I was saying "what ever" to myself as I traced through traits that just called methods on the object. But now I see how this level of indirection makes the library much more extensible. I could avoid figuring out what the gtsam::Pose3 canonical variables mean and just use another Pose3-like structure and define the canonical variables myself. Very neat! Thanks!

[Stefan Gächter]

Hi Matias,

My I ask a question? I started to study the paper in the other thread, about uncertainty of poses:  Characterizing the Uncertainty of Jointly Distributed Poses in the Lie Algebra

For the between factor, the measurement b1_T_b1_b2 is readily computed b1_T_b1_b2 = w_T_w_b1^-1 * w_T_w_b2.

For the measurement uncertainty, I could follow the proposed method for relative pose in the paper. But if following the paper, the resulting noise would be b1_N_b1, yes? And this is because of left-hand notation? However, for GTSAM between factor, I would need b2_N_b2. Thus, I would have derive the equations in the paper for right-hand notation.

Best regards

Stefan

[Matías Mattamala]

Hi Stefan,

Yes, you're right. That paper is really nice, but the problem is that everything is defined on the left-hand side, so I think the best way to figure this out is deriving the equations again to be consistent with your own notation/conventions.

I think that if you use the Adjoint you should be able to use their equations directly, just taking their expressions and "moving" them to the right. While we tried to do that with the formulas for the relative covariance, we got confused with the cross-covariances and we ended up deriving the equations from scratch. If you need them soon, feel free to write me, but I hope to also add this information in the blog post that Frank asked us about in the other thread.

Also note that if you have, for instance, the odometry poses with covariance from a state estimator and you want to compute the relative odometry measurements from them to put them in a SLAM system, you don't have the cross-correlation terms. So those terms are gonna vanish, but the resulting relative covariance is gonna be larger than the ones from the poses (which is the same result that you obtain in a 1d case when you subtract 2 independent Gaussians - the variances sum up). A solution for this is just assuming that some cross correlation exist, unless you have a better way in you problem to obtain such terms.

Best,
Matias

[Stefan Gächter]

Thanks a lot, Matias, for your advice. I will give it try as an exercise.