Indeterminant linear system detected: EssentialMatrixConstraint + BetweenFactor

[Thomas Eberhard]

Hi everybody  I have the following Pose Graph:
```
NonlinearFactorGraph: size: 3

Factor 0: PriorFactor on x0
  prior mean:  R: [
        -0.201547519, -0.977235902, 0.0662464351;
        -0.917424872, 0.164655624, -0.36224319;
        0.343089203, -0.133785344, -0.929726455
]
t: 0 0 0
  noise model: diagonal sigmas[10; 10; 10; 100; 100; 100];

Factor 1: BetweenFactor(x0,x1)
  measured:  R: [
        0.999317692, -0.0257647751, 0.0264636783;
        0.0257493748, 0.999668004, 0.000922604244;
        -0.0264786631, -0.000240551574, 0.99964935
]
t: 0 0 0
  noise model: diagonal sigmas[1; 1; 1; 10; 10; 10];

Factor 2: EssentialMatrixConstraint(x0,x1)
  measured: R:
 [
        0.994285511, 0.0718329152, -0.078970591;
        -0.0694154532, 0.997043655, 0.0329460994;
        0.081103741, -0.0272760499, 0.996332374
]
d: :-0.688679287
 0.614011921
-0.385629614
isotropic dim=5 sigma=1e-05

Values with 2 values:
Value x0: (gtsam::Pose3)
R: [
        -0.201547519, -0.977235902, 0.0662464351;
        -0.917424872, 0.164655624, -0.36224319;
        0.343089203, -0.133785344, -0.929726455
]
t: 0 0 0

Value x1: (gtsam::Pose3)
R: [
        -0.228327332, -0.971734573, 0.0599879151;
        -0.902967412, 0.188325342, -0.386242695;
        0.364028135, -0.142356896, -0.920444475
]
t: 0 0 0
```
But if I try to optimize I get:
```
Indeterminant linear system detected while working near variable
8646911284551352321 (Symbol: x1).

Thrown when a linear system is ill-posed.  The most common cause for this
error is having underconstrained variables.  Mathematically, the system is
underdetermined.  See the GTSAM Doxygen documentation at
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
more information.
```
I had assumed that the system was properly constrained by the added factors,
have I made a mistake here?
I have tested setting the covariance of the EssentialMatrixConstraint to 0.5 and used Dogleg over LevenbergMarquardt but with the same results.
What should I take care of when using the EssentialMatrixConstraint?

[Till Kroeger]

Have you tried without adding any EssentialMatrix constraint? It may be worth seeing whether you can get simple one to work. (It should be no issue as long as you don't select your in-between covariance too large.

If that already results in the indeterminate system, you've little chance in recovering that with adding the essential matrix constraint (which only has 5 degrees of freedom).

Alternatively, you could try adding a second prior on X1, and remove the in-between factor, and see whether that works.

Also, note that both cameras share a center of projection at the origin, which may make any operation of essential matrix  geometry somewhat unstable.

[Dellaert, Frank]

Yusuf and I looked at this and indeed, the Jacobian of the linearized graph contain NaNs when the initial estimate is at the same location. This makes sense, as there is no epipolar geometry for co-located cameras. Just make sure that initial estimate satisfies this constraint whenever you add an essentialMatrix constraint between two cameras.

 

Curious on how to avoid this in the future: any ideas on how could this have been reported better by GTSAM? What would you have expected? Trying to make GTSAM better 😊

 

Frank

--
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/ed2db042-eeca-4a2b-98ba-c86054821a4an%40googlegroups.com.

[Till Kroeger]

How to deal with ill-conditioned geometry is very good question. This not only applies to the essentialMatrix constraint, but also to others, e.g. the projection factor, where a point with zero depth in the camera frame would result in a divide-by-zero. (I am sure there are more factor types that could have such ill-conditioned geometry.)

Moreover, these conditions can be present 

- (1) on construction, as is the case here with the trivially co-located cameras,

- (2) may also happen during optimization even if it initially was valid, e.g. in the case of a camera projection factor: If a landmark with positive depth in the camera frame, crosses over the border of z < 0 and swaps behind the camera.

For (1) it would be great if factors that can have geometric ill-conditioning would provide a sort of factory, with appropriate checking on construction. However, that requires a set of gtsam::Values to be available on construction.

For (2) the case is a bit harder, since the factor will generally be part of some graph class, and the ill-condition is only verifiable together with a set of state values, which not part of the graph. So, the reasonable place to catch such ill-conditioning during optimization would be as part of the optimizer, or the wrapper class (e.g. the isam2 / fixedlagsmoother class) which could return some info struct on every update call to alert the user. This would allow the user to find out what happened at which factor.

Even better would be some way to configure the behavior for the solver ahead of time if such conditions arise on some factors. (E.g. “hard crash”, “clamp to previous valid value”.) 

Currently, some factors use the “CheiralityException” to catch projection errors, which, however, very destructively takes down the entire solver without possibility for mitigation, soft recovery, or even identification which factor is to blame. For deployed robotic systems that may be a bit tough.

Till