Documentation of BetweenFactor unclear.

[jola6897]

Description

Currently it is not possible for me to see how the covariance associated to BetweenFactor has to be set. I searched the references but I could not find an answer. Therefore I think it is a good idea to describe it a little more in detail as this factor is used a lot.

Steps to reproduce

1. A BetweenFactor from Poses A to Pose B shall be added. Ta and Tb are the corresponding Affine Transformation matrices in the global coordinate system.
2. The delta pose dT that is put into the BetweenFactor is dT = inverse(Ta) * Tb. I verified this by doing some tests. This means dT is given in the coordinate system of A.
3. Now the question is in what coordinate system the variances of the BetweenFactor Noise have to be given? Is it in A also or is it in the global coordinate system. From a logic standpoint I would have assumed that it is A, but I do not know how to verify this and I also did not find any documentation. All examples set variance in x = variance in y. So it cannot be infered how it is defined.

Expected behavior

Documentation should be clear in that regard as rotated covariance matrices will impact the optimization result.

Thanks a lot in advance. Hope someone can clearify this.

[dellaert]

The noise covariance is in the tangent space at dT. So, in expectation I guess it can also be viewed as being specified in B.

I guess you could verify by sampling poses with zero mean and composing them with the ground truth dT, and checking the whitenedError of the factor, which should be something chi-squared. At least, that would be story for a flat space and diagonal covariances. I’ve never done this for poses but maybe we learn something :-)

[jola6897]

Thanks for the clearification :)

[jola6897]

Hi,

I am sorry to reopen this issue, but it did some rudimentary testing and I think your answer is not quite correct. It makes sense that the noise covariance is defined in the tangent space at dT. But in this case I think the tangent space uses A (the start pose) as a reference not B (end pose) for the BetweenFactor from A to B.

In a lot of examples the dT for the BetweenFactor is computed using Pose3::between(). The result of A.between(B) matches the result of dT=inverse(Ta)*Tb. This indicates that dT is given in the coordinate system of A. Therefore I think it would be strange if the covariance noise is given in another coordinate system, namely B.

@dellaert Could you confirm this please?

[dellaert]

Yes, I think I was wrong in saying the B part. A, B, and dT are all different. Betweenfactor actually works with any type that has the right traits. For scalars, if A=6, B=8, then dT=2. You don’t have to think about where the noise is specified, as scalars are a commutative group. But, for SO(3) or SE(3), the noise is specified in dT=2, which is a different tangent space from 6 and 8.

[jlblancoc]

But, for SO(3) or SE(3), the noise is specified in dT=2,

That was my insight too...

A few weeks ago I wrote a test program precisely to debug and put at test the intuition about (non-isotropic) covariance matrices and "Between factors" in SO(3). I tried, but failed, to map YawPitchRoll angles to the elements in the diagonal of the covariance of "dT"... Will try to clean up the code and bring it here to help moving forward this interesting discussion...

[jola6897]

@jlblancoc that would be actually very helpful since I think it is much harder to do this correctly than initially anticipated due to my limited experience with GTSAM :)

[jlblancoc]

Here's the code: https://github.com/jlblancoc/gtsam-test-rotations Some comments follow...

[jlblancoc]

And here some comments:

• The problem defines two rotation values in SO(3): R0 and R1.
• R0 is fixed via a Prior to the either, the identity, or a small rotation (two configurations for the test).
• R1 is affected by two factors, which get fused (and here's the beef, of course):
  • A prior with a small (but finite) isotropic covariance, with SO(3) identity as the observed value.
  • An observed relative rotation wrt to R0.

Test matrix is:

• R0=identity, or Rot3::Ypr(deg2rad(1.0), deg2rad(2.0), deg2rad(3.0));
• For each axis i=0,1,2, set the BetweenFactor to a diagonal covariance matrix, with the active axis having a small sigma (strong observation) and the other two axis a very large covariance ("not observed").

The relative observation is always Rot3::Ypr(deg2rad(1.0), deg2rad(2.0), deg2rad(3.0));

What would be your expectations on the results? Public poll ;-)

[jlblancoc]

Actual results:

R0: identity Axis 0 is active:

Factor 0: BetweenFactor(r0,r1)
  measured:  ...
  noise model: diagonal sigmas[3.16227766e-05; 10; 10];
...
r0 (rpy):  0.00000 -0.00000  0.00000 localCoords: 0.00000 0.00000 0.00000
r1      : 0.05200 0.00000 0.00000 localCoords: 0.05200 0.00000 0.00000
r01 obs : 0.05236 0.03491 0.01745 localCoords: 0.05205 0.03535 0.01653
r01 est : 0.05200 0.00000 0.00000 localCoords: 0.05200 0.00000 0.00000
r01 est rel local coords : 0.05200 0.00000 0.00000

Great! The first coordinate (rpy) of the resulting "r01 est" (which is the relative R1 wrt R0, with these being the optimized values) matches the first coordinate of the r01 observation. Only that one because we are "trusting it" via the covariance diagonal.

The same happens for the alternative factor graphs with only the 2nd and the 3rd diagonal entries having a small variance:

// axis 1 active:
r0 (rpy):  0.00000 -0.00000  0.00000 localCoords: 0.00000 0.00000 0.00000
r1      : 0.00000 0.03533 0.00000 localCoords: 0.00000 0.03533 0.00000
r01 obs : 0.05236 0.03491 0.01745 localCoords: 0.05205 0.03535 0.01653
r01 est : 0.00000 0.03533 0.00000 localCoords: 0.00000 0.03533 0.00000
r01 est rel local coords : 0.00000 0.03533 0.00000
...

// axis 2 active:
r0 (rpy):  0.00000 -0.00000  0.00000 localCoords: 0.00000 0.00000 0.00000
r1      : 0.00000 0.00000 0.01652 localCoords: 0.00000 0.00000 0.01652
r01 obs : 0.05236 0.03491 0.01745 localCoords: 0.05205 0.03535 0.01653
r01 est : 0.00000 0.00000 0.01652 localCoords: 0.00000 0.00000 0.01652
r01 est rel local coords : 0.00000 0.00000 0.01652

Now, things get harder when we use R0 != identity...

If we repeat the case of axis 0 active:

Factor 0: BetweenFactor(r0,r1)
  measured:  ...
  noise model: diagonal sigmas[0.000031623; 10.000000000; 10.000000000];
...
r0 (rpy):  0.52360 -0.00000  0.00000 localCoords: 0.52360 0.00000 0.00000
r1      : 0.57508 0.00000 0.00000 localCoords: 0.57508 0.00000 0.00000
r01 obs : **0.05236** 0.03491 0.01745 localCoords: **0.05205** 0.03535 0.01653
r01 est : **0.05148** 0.00000 0.00000 localCoords: **0.05148** 0.00000 0.00000

So far, so good: the first coordinates in both parameterizations (ypr, and in the manifold localCoordinates) coincide.

If we now move to another axis:

================= AXIS 2
NonlinearFactorGraph: size: 3

Factor 0: BetweenFactor(r0,r1)
  measured:  ...
  noise model: diagonal sigmas[10.000000000; 10.000000000; 0.000031623];
...
r0 (rpy):  0.52360 -0.00000  0.00000 localCoords: 0.52360 0.00000 0.00000
r1      : 0.00000 0.00000 0.02617 localCoords: 0.00000 0.00000 0.02617
r01 obs : 0.05236 0.03491 0.01745 localCoords: 0.05205 0.03535 0.01653
r01 est : -0.52345  0.01308  0.02267 localCoords: -0.52357  0.00685  0.02557
r01 est rel local coords : -0.52357  0.00685  0.02557

Hmm... here's is when I stop following it: different results are obtained in the estimated relative rotations by changing the reference of frame, hence the covariance of the BetweenFactor seems not to be independent of the absolute coordinates... or am I missing something?

Put explicitly, I expected these two lines to be the same ()

// Axis2 active, case R0=identity
r01 est : 0.00000 0.00000 0.01652 localCoords: 0.00000 0.00000 0.01652
...
// Axis2 active, case R0!=identity
r01 est : -0.52345  0.01308  0.02267 localCoords: -0.52357  0.00685  0.02557

This whole thing is so confusing that I might be missing something stupid, but this is how far I reached investigating this issue...

[dellaert]

Ok, this is a great conversation indeed. I think I was wrong again :-) If you look at the error that the BetweenFactor produces, it is

return traits<T>::Local(measured_, hx);

with hx the prediction, I.e. between(A,B). Now, if measured ~ hx, the above is approximately zero. Hence, let me make a third (possibly wrong) statement :-) “The noise covariance is specified in the tangent space around identity.”

I don’t know (yet) how that helps @jlblancoc ’s observation above.

[dellaert]

Looking at the test code, could it be the prior on R1 that is causing this?

[jola6897]

Looking at the test code, could it be the prior on R1 that is causing this?

I think so, too. R1 is not the identity and the assigned noise is quite small with a standard deviation (sigma) of just 1e-3 * 180/M_PI = 0.057 degree. The question would also be this 3 sigmas for the noise what is the order (roll, pitch, yaw?) and the corresponding values (do they refer to the euler angles?) they are referring to?

[jlblancoc]

Hmm... perhaps I either was wrong, or failed to transmit the idea of the test code :-)

The goal is to make "R1" to be in conflict between two contradictory values: one is the "weak" link towards SO(3) identity. With " R1 is not the identity", @jonla1 , I think you mean that the "correct" value of R1!=identity. That's correct... according to the BetweenFactor, but if it was assigned a high weight in all its 3 components. The key is precisely that one: what happens (and what should happen) if only one of the components of the BetweenFactor has a strong weight, and the other two almost ignored? In order to have a well-defined problem, the unary factor (Prior) is added so those "non-observed" components tend to be the unity rotation.

[theBlan]

Hallo everyone , this problem touches one of my understanding problems with gtsam, that I think I solved for me. But I’m not 100 % sure my solution is correct, so I look for a confirmation of the correctness. My problem addresses how to include a given covariance matrix in GTSAM (with consideration of proper variance-covariance propagation) and not to “invent” a covariance matrix of the observations like in the examples.

I answer the question “how the covariance associated to BetweenFactor has to be set”, that the covariance matrix has to be stated in the tangentspace of the odometry observation.

As Explanation of this answer I consider a S03 example.

GTSAM defines the following Noise-Modell Robs=Rtrue x Rnoise. For the tangenspace description GTSAM uses a rotation vector to model the rotation. We could also describe Robs by Exp(w+dw),where w is a rotation vector (at the identity) and dw is its noise. To be perfectly clear here, the rotation vector w would express in a between factor the orientation difference of the both constrained orientations. Is this noise dw now the used noise in GTSAM ? The answer is no because that would indicate that the following equation Exp(w+dw)=Exp(w) x Exp(dw) holds, but in contrast to the scalar case we need to consider here the right jacobian Jr to approximate the equation Exp(w+dw) ~Exp(w) x Exp(Jr x dw) By comparing the two equations we obtain that Rnoise equals Exp(Jr x dw) and now using the GTSAM observation equation Log(Robs.T x Rtrue)=Log(Rnoise.T x Rtrue.T x Rtrue)=Log(Rnoise.T) with the plugging of the true rotation difference Rtrue instead of the estimated rotation difference and acknowledge of Log(Rnoise.T) =-Log(Rnoise), we obtain v=-Jr x dw, which is completely in agreement of the GTSAM error definition. (The definition of Jr can be looked up in the literature.. So instead of a global noise definition at the identity the covariance matrix is defined locally in the tangentspace of w)

So if we know the covariance matrix Qww of the orientation difference as rotation vector, we would still need to transform the covariance matrix with Jr x Qww x Jr.T. Obviously the measurement noise is independent of the estimated entity and its associated frame. This is in contrast to the estimated error that depends on the linearization point of the estimated entities. (Or in another words this is not a pseudo-gauß-markov-model-observation that results of a gauß-helmert-model-observation-transformation.)

From my understanding GTSAM uses internally in no form euler angles, so if the covariance matrix of the odometry observation is referenced in euler angles by another algorithmus or odometry sensor, the covariance matrix of the euler angles needs to be transformed, before it can be used in GTSAM. Another approach would be to write our own euler angle observation model, but in that cases we must make ourselves aware of the fact, that the estimated enitities by GTSAM are still not euler angles. This fact does also include global orientation observations. So if our absolute orientation is given in euler angles, we have to appropriately address this observation and its covariance matrix in GTSAM.

(Another similar case is a Unit3 observation m, because GTSAM uses the minimal representation for a direction vector, the (degenerated ) covariance matrix Qnn ( 3x3) has to be transformed in the tangentspace (2D) of m. ( Only two elements of the three observation elements are independent because of the norm constrain). So let be B a orthogonal Basis of the tangenspace of m, then we can obtain (B.T x )m=(B.T x )B x d, and under the knowledge that B.T x B is the identity matrix, we get the used observation d in GTSAM and its covariance matrix Qdd= B.T x Qnn x B. )

Please, if I’m mistaken, correct me.

[jlblancoc]

If your Jr is the adjoint, I think your conclusion is correct.

I'll try to add a PR with an example/unit test, following this issue and yesterday's discussion on gtsam mailing list...

[theBlan]

Okay, i really meant the right jacobian Jr that can defined like this

wn=norm(w) Jr(w)= Identity(3,3)-(1-cos(wn))/wn^2 x Skew(w) + (wn-sin(wn))/wn^3 x Skew(wn) x Skew(wn)

In the case of the between factor w would be the measured rotation vector, that describes the orientation difference.

The adjoint of S03, when I’m not mistaken, is simple R (only 50 % sure), so I don’t think Jr equals the adjoint.

For further Information I have to look in the literature, where I found the expression of Jr, but I’m pretty sure it’s even GTSAM related.

[dellaert]

R is indeed the adjoint for rotations, see math.pdf.

Also, this PR might be (part) of what you are looking for: PR #499 from the description:

The title sums it up. I guess the motivation is pretty straightforward as well : These derivatives are useful for when you want to propagate uncertainties related to Euler angles.

[theBlan]

So I can conclude, that your second answer was right and your third was wrong. So only thing that is left to confirm is the mapping with the right jacobian Jr. Based on your paper "IMU Preintegration on Manifold for EfficientVisual-Inertial Maximum-a-Posteriori Estimation" (http://www.roboticsproceedings.org/rss11/p06.pdf) p.2 i strongly assume that my opinion of using Jr instead of the adjoint is also valid. (In (8) is the definition of Jr and in (7) is the given exp-relationship. The name dphi in (9) could be a little bit misleading for our case because the variable dphi is actually a substitute for Jr*dphireal.) But it would be lovely, if you can confirm that.

[dellaert]

Assuming "you" above means "me" :-) Sorry, this is a long thread and I have little bandwidth to get into this now. Did you already read the blog posts by Mattias on gtsam.org and does that help this discussion?