Uncertainty definitions

I am digging into some details of GTSAM to get a better understanding, in particular, on the details of the BetweenFactor.

Hi Stefan,

I've had this confusion as well. I'll try to answer your questions below.

1. Regarding the conventions
As you said, GTSAM is built upon Lie group theory. That requires to assume one convention to apply some operations - left or right handed.
GTSAM uses right-hand notation: corrections are applied on the right hand, as well as right jacobians are chosen in general. Probability distributions of elements of the group are also defined with "right notation".

With a concrete example, let's say you have a Pose3 (i.e, a rigid-body matrix, an object from SE(3)) that represents the pose of the body in time 1 in the world frame: w_T_w_b1
Additionally, you have another estimate at time 2: w_T_w_b2
You additionally have a measurement of the relative pose between 1 and 2 (like, an odometry measurement). Let's be ambiguous for now and call it dT

With the previous information, the following expression holds.
w_T_w_b2 = w_T_w_b1 * dT

If we make an analogy with a common Kalman filter scheme, it's like a process model:
x(t+1) = x(t) + u(t)

However, the previous equations are deterministic - we're not considering noise nor any distribution. We want things to be Gaussian, so in the Kalman filter case we say that the control u(t) has some white noise n (i.e, zero mean Gaussian noise, with covariance sigma):

x(t+1) = x(t) + u(t) + n

So if you take the previous equation, an leave the noise alone, you can say that the difference distributes as a zero mean Gaussian with covariance sigma:
x(t+1) - x(t) + u(t) = n

So what you end up having on the left hand side, is basically a vector version of the between factor. You can directly use it in GTSAM using the covariance sigma as the factor covariance.


Now the thing is to make the same when you have poses. We have this equation:
w_T_w_b2 = w_T_w_b1 * dT

We say that the only source of noise is the measurement dT. So we "add" a zero mean Gaussian noise:

w_T_w_b2 = w_T_w_b1 * dT * Exp(N)

The new term, Exp(N) is the exponential map of a variable N that distributes as a zero mean Gaussian, but on the tangent space of the group. It means that it has 6 degrees of freedom (3 position + 3 orientation), and has a covariance matrix Sigma6 (6x6).

Now this new term is the only source of uncertainty. Let's isolate as  before to obtain our between factor and analyze what happens:

(dT)^-1 * (w_T_w_b1)^-1 * w_T_w_b2 = Exp(N)

The inversion (w_T_w_b1)^-1 results in b1_T_b1_w (we flip the frames)
=> (dT)^-1 * b1_T_b1_w * w_T_w_b2 = Exp(N)

The blue term basically defines the difference between frames 1 and 2, defined in frame 1
To make this consistent, (dT)^-1 should match the frames, so it should be

dT^-1 = b2_dT_b2_b1

Hence, dT must be dT = b1_dT_b1_b2
dT describes a transformation defined in b1 that "takes you" to b2 - what an odometry measurement is supposed to do.

The whole error (between factor) is finally:

(b1_dT_b1_b2)^-1 * b1_T_b1_w * w_T_w_b2 = Exp(N)

Which is defined "in b2 to b2". So it must be Exp(N) the error on the right hand side, hence its covariance Sigma. That's why we said the error is defined on the measurement frame, i.e, b2 in this example.

It is quite critical to keep track of the frames of the transformations and measurements involved when the objects are rigid-body transformations, rotation matrices or quaternions. However, this doesn't happen only in factor graphs, but Kalman filters too, because it depends how you apply the corrections and the noise.

The previous example implicitly adopted a right hand notation when defining:
w_T_w_b2 = w_T_w_b1 * dT * Exp(N)

We could have also expressed:
w_T_w_b2 = w_T_w_b1 *  Exp(N) * dT

But that would have implied "noise defined in frame b1" which could make things more involved. GTSAM always assumes that noises are applied from the right hand side.

As a last remark on this side, the expression dT * Exp(N) is also interpreted as a Gaussian with mean dT and covariance Sigma. This is also a right hand notation to express distributions on SE(3).

2. Regarding how the noise must be defined for each factor, when I'm confused I always check the Logarithm map implementation of each object.
Most of the factors that require working with Lie Groups usually have a Logarithm map that converts the matrix difference into a vector difference by mapping the difference to the tangent space.
As we said before, since the tangent space is has the same dimension of the minimum representation of the object (rotations 3, poses 6, etc), the covariance should match their dimension, and the Logarithm map resulting vector should follow the order in which the covariance matrix must be defined.

In particular, what you said is right
- Pose2 is (x, x, theta)
- Pose 3 is (alpha, beta, gamma, x, y, z)

3. Lastly, I think that handling the covariances is always tricky because we need to be careful with the conventions. 

Hi Frank,

I'd be happy to work on that. I can start writing up a draft if we are ok with it.

And thanks for the clarification on GTSAM, I'll take a look at the book.

Matias

Dear Matias,

Dear Stefan, Matías:

I think this thread is also relevant to this interesting discussion
(and in particular, to your last question on what are the components
alpha, beta, gamma):
https://github.com/borglab/gtsam/issues/504

JL

Hi Stefan, José Luis,

I'm happy to know it helped. Thanks for sharing the issue as well, I'll definitely take a look, perhaps there is something I'm missing.

Regarding Stefan's follow-up questions:

1. Yeah, I think I agree with you that according to the definitions, it would be left-invariant error, but "right convention"
if you define the state X as w_X_wb, and  w_X'_wb to represent the estimated state, then the "left invariant error" is error defined in the base frame: w_X_wb^-1 * w_X'_wb
So it makes sense to apply the correction to the estimate on the right using the Exponential map

2. Regarding the implementation
- Yes, BetweenFactor(1, 2,  b1_T_b1_b2  , sigma) should work
- While measurement  b1_T_b1_b2 is gonna be expressed in frame b1, it represents a transformation "that transforms elements expressed in frame b2 to frame b1". The tricky part here is that while the measurement itself is expressed in a frame (base 1), its covariance is expressed in a different one (base 2), because the measurement itself is changing the frames. This doesn't happen when you have vectors, because you can add vectors (noise in this case) either from the left or the right and it works. Perhaps that's the source of the confusion.

3. Minimal representation
I tend to think that the minimal representation of orientation (tangent space) is Axis-Angle, since the exponential map is usually defined as the Rodrigues rotation formula. And I try to avoid thinking in Euler angles because all their conventions confuse me :(

Best,
Matias

Thanks, Jose Luis, for pointing to the discussion. I have yet to study your example.

Hi again,

I was about to share some slides with figures, since we were having the same discussion with my group about uncertainties this morning (such a coincidence).

From your figure, I understand (let me know if I'm right):
- i is the frame at the left
- j is the frame at the center
- the frame at the right is the pose of the robot/vehicle
- j_r_jj and j_C_j are the odometry measurements that move the robot away from frame j, relative to the same frame.

If I got all of that ok, yes, the covariance/uncertainty will be defined as you did. This is locally valid in j, if you want to express it in i you need to transform it with the Adjoint (the slides above suggest some procedures)

Regarding the last question, yes, the covariance should be axis-angle. We're not using Euler angles at all in the tangent spaces (but probably there are some conditions/conventions under which they are the same as Euler angles)

Best,
Matias 

Yes, thanks for the slides. It helped me to solidify my understanding.
Ok, understood that with the tangent space now.

Thank you Frank and Stefan, I'm glad it helped.

Yeah, taking a deeper look at the figure, it would be correct. Ideally dC_jj and dr_jj should be zero (residual zero) and exactly match the pose of frame j. But in reality there could still exist a difference -that is explicit in your diagram-, which is handled by assuming that the measurement follows some distribution (with the green covariance).

Hello Frank,

Thanks, Mike, for your comment. Meanwhile familiar with manifolds and groups, I agree that it is wise to look at the tangent space first.

Thanks  a lot , Matias, and all who have contributed, for the three part tutorial. You put everything nicely together and it consolidated my understanding. It was a treat to read it.

One thing, which I do not yet fully understand, though, is, how the distribution looks like. Equation (32) states the following

Hi Stefan,

I'm glad to know the posts were helpful :)

Yes, I understand the question. The distribution on the tangent space is indeed Gaussian in the "normal sense", like an ellipse, or ellipsoid when you have higher dimensions, because the tangent space is Euclidean.

However, when you apply the exponential map, we still say it's a "Gaussian on the manifold" but if you actually try to plot it in an Euclidean space (as in the RSS paper, using sampling for instance), you will see that the shape is not an ellipsoid but a "banana distribution" as you pointed out. This fact -plotting a manifold, with specific constraints, into Euclidean axes-  is what causes this mismatch with the intuition.

I hope this helps to solve the question.

Best,
Matias

Thanks, Matias, I finally got it.
I am taking a closer look at adjoints and I got stuck with the notation. May I ask for some clarification in part 3?

Isn't the combination of equation (1) and (2) resulting equivalence in the following way?

Hm, maybe my previous question does not make much sense. The normal distribution is not a transformation and, thus, my notation does make much sense.

Hi Stefan,

I'm not sure I got the previous question fully (the one where you shared the equations) but let me know if you want to discuss it further. I think that's one of the weakest parts of the posts, so I'm happy to discuss it further to make it clearer.


Regarding Equation (15) and (17) you're absolutely right, there is a notation problem that makes things confusing. Thanks for pointing this out. Someone else also made me notice and I'll fix it. 


The last question about Sola, if you meant equation (32) in the paper I think that both are right. I'll use the same notation as the blog post to show it:

Sola uses X⊕t = (Ad_X * t) ⊕ X. Using the notation of the posts, this is equivalent to:
 T_w_b * Exp(b_xi) = Exp(Ad_T_w_b * b_xi) T_w_b

So he wants to move an increment defined on the right, to the left hand side. In this case, we use the adjoint of T_w_b (no inversion), because that transforms b_xi into Ad_T_w_b * b_xi (in world frame), which matches the frame on the left of T_w_b. We can use the same subindex cancelation trick to confirm the frames are consistent (the only frame that remains is w on the left)

On the other hand, I used the opposite. I wanted to move an increment defined on the left w_xi, to the right hand side:

-> Exp(w_xi)  * T_w_b = T_w_b * Exp(Ad_(T_w_b)^{-1} * w_xi)

-> Exp(w_xi)  * T_w_b = T_w_b * Exp(Ad_T_b_w * w_xi) 

The inversion swaps the indices and then Ad_T_b_w * w_xi represents an increment defined in the base, which matches the frame on the right of T_w_b

You can also confirm it if you concatenate both operations starting from Sola's, moving the increment to the left, and then back to the right using the one in the post:

 T_w_b * Exp(b_xi) = Exp(Ad_T_w_b * b_xi) T_w_b                                  // Sola
                                 = Exp(Ad_T_w_b * b_xi) T_w_b                                   // Increment now in world frame
                                 = T_w_b * Exp(Ad_T_w_b^{-1} * Ad_T_w_b * b_xi)  // Blog post's to move the increment to the right
                                 = T_w_b * Exp(Ad_T_b_w * Ad_T_w_b * b_xi)          // Inverting and swapping indices
                                 = T_w_b * Exp(Ad_T_b_w * Ad_T_w_b * b_xi)          // Now everything is in base frame and the adjoints cancel each other
                                 = T_w_b * Exp(b_xi) 

I hope it helps, and thanks again for noticing the typos :)

Thanks, Matias, for your derivations.
I had a look again at my derivations and, I beg your pardon, I made a mistake. Firstly, your derivations correspond perfectly with Sola's one, as you showed.
Secondly, equation (15) and (17) are correct, but could be improved for readability by adding an apostrophe.

Here, my derivations, which should match yours, maybe helpful for others:

Hi Stefan,

No worries at all, I'm happy to know we conclude the same. Thanks again for the feedback in those equations, it's confusing indeed.

I also agree that the increments/corrections are not that straightforward to interpret. While it's tempting to define them as a from-to-transformation, it doesn't make sense in the end as you mentioned and I haven't figure out a better way to explain it either. In the blog post I decided to use only the frame in which the increment is defined, but the interpretation is up to the reader.