EssentialTransferFactor

[dellaert]

New factor does transfer using E, but also introduces calibration unknowns for the views. So, potentially O(N^2) E unknowns, but only O(N) calibration unknowns. Currently keys are derived from edges but should be possible to overwrite them.

Example output:

[100%] Built target EssentialViewGraphExample
Factor Graph:
size: 12

Factor 0:   keys = { {0, 2} {1, 2} k0 k1 k2 }

Factor 1:   keys = { {0, 1} {1, 2} k0 k2 k1 }

Factor 2:   keys = { {0, 2} {0, 1} k2 k1 k0 }

Factor 3:   keys = { {1, 3} {2, 3} k1 k2 k3 }

Factor 4:   keys = { {1, 2} {2, 3} k1 k3 k2 }

Factor 5:   keys = { {1, 3} {1, 2} k3 k2 k1 }

Factor 6:   keys = { {2, 0} {3, 0} k2 k3 k0 }

Factor 7:   keys = { {2, 3} {3, 0} k2 k0 k3 }

Factor 8:   keys = { {2, 0} {2, 3} k0 k3 k2 }

Factor 9:   keys = { {3, 1} {0, 1} k3 k0 k1 }

Factor 10:   keys = { {3, 0} {0, 1} k3 k1 k0 }

Factor 11:   keys = { {3, 1} {3, 0} k1 k0 k3 }

Initial Estimates:

Values with 12 values:
Value {0, 1}: (gtsam::EssentialMatrix)
R:
 [
        0.0099495, -0.0100495, -0.9999;
        0.0100495, 0.9999, -0.0099495;
        0.9999, -0.0099495, 0.0100495
]
d: :   0.699965
-0.00999967
   0.714107

Value {0, 2}: (gtsam::EssentialMatrix)
R:
 [
        -0.9999, 0.0099495, -0.0100495;
        0.0100495, 0.9999, -0.0099495;
        0.0099495, -0.0100495, -0.9999
]
d: :-0.00999967
 0.00999967
     0.9999

Value {1, 2}: (gtsam::EssentialMatrix)
R:
 [
        0.0099495, -0.0100495, -0.9999;
        0.0100495, 0.9999, -0.0099495;
        0.9999, -0.0099495, 0.0100495
]
d: :   0.699965
-0.00999967
   0.714107

Value {1, 3}: (gtsam::EssentialMatrix)
R:
 [
        -0.9999, 0.0099495, -0.0100495;
        0.0100495, 0.9999, -0.0099495;
        0.0099495, -0.0100495, -0.9999
]
d: :-0.00999967
 0.00999967
     0.9999

Value {2, 0}: (gtsam::EssentialMatrix)
R:
 [
        -0.9999, 0.0099495, -0.0100495;
        0.0100495, 0.9999, -0.0099495;
        0.0099495, -0.0100495, -0.9999
]
d: :-0.00999967
 0.00999967
     0.9999

Value {2, 3}: (gtsam::EssentialMatrix)
R:
 [
        0.0099495, -0.0100495, -0.9999;
        0.0100495, 0.9999, -0.0099495;
        0.9999, -0.0099495, 0.0100495
]
d: :   0.699965
-0.00999967
   0.714107

Value {3, 0}: (gtsam::EssentialMatrix)
R:
 [
        0.0099495, -0.0100495, -0.9999;
        0.0100495, 0.9999, -0.0099495;
        0.9999, -0.0099495, 0.0100495
]
d: :   0.699965
-0.00999967
   0.714107

Value {3, 1}: (gtsam::EssentialMatrix)
R:
 [
        -0.9999, 0.0099495, -0.0100495;
        0.0100495, 0.9999, -0.0099495;
        0.0099495, -0.0100495, -0.9999
]
d: :-0.00999967
 0.00999967
     0.9999

Value k0: (gtsam::Cal3_S2)
[
        50, 0, 50;
        0, 50, 50;
        0, 0, 1
]

Value k1: (gtsam::Cal3_S2)
[
        50, 0, 50;
        0, 50, 50;
        0, 0, 1
]

Value k2: (gtsam::Cal3_S2)
[
        50, 0, 50;
        0, 50, 50;
        0, 0, 1
]

Value k3: (gtsam::Cal3_S2)
[
        50, 0, 50;
        0, 50, 50;
        0, 0, 1
]

Initial Errors:
size: 12

Factor 0:   keys = { {0, 2} {1, 2} k0 k1 k2 }
error = 42.185

Factor 1:   keys = { {0, 1} {1, 2} k0 k2 k1 }
error = 55.5746

Factor 2:   keys = { {0, 2} {0, 1} k2 k1 k0 }
error = 70.0592

Factor 3:   keys = { {1, 3} {2, 3} k1 k2 k3 }
error = 42.185

Factor 4:   keys = { {1, 2} {2, 3} k1 k3 k2 }
error = 55.5746

Factor 5:   keys = { {1, 3} {1, 2} k3 k2 k1 }
error = 70.0592

Factor 6:   keys = { {2, 0} {3, 0} k2 k3 k0 }
error = 42.185

Factor 7:   keys = { {2, 3} {3, 0} k2 k0 k3 }
error = 55.5746

Factor 8:   keys = { {2, 0} {2, 3} k0 k3 k2 }
error = 70.0592

Factor 9:   keys = { {3, 1} {0, 1} k3 k0 k1 }
error = 42.185

Factor 10:   keys = { {3, 0} {0, 1} k3 k1 k0 }
error = 55.5746

Factor 11:   keys = { {3, 1} {3, 0} k1 k0 k3 }
error = 70.0592

Initial error: 671.275, values: 12
iter      cost      cost_change    lambda  success iter_time
   0      1.32323      6.7e+02      1e+03      1       0.01
   1      0.00039          1.3      1e+02      1          0
   2      3.3e-07      0.00039         10      1          0
   3      6.3e-08      2.6e-07          1      1       0.01
Initial error = 6.7e+02
Final error = 6.3e-08
Final Results:

Values with 12 values:
Value {0, 1}: (gtsam::EssentialMatrix)
R:
 [
        2e-06, -1.9e-05, -1;
        1.6e-05, 1, -1.9e-05;
        1, -1.6e-05, 2e-06
]
d: :   0.71
1.4e-05
   0.71

Value {0, 2}: (gtsam::EssentialMatrix)
R:
 [
        -1, 3.5e-06, 2e-10;
        3.5e-06, 1, -3.4e-05;
        -3.2e-10, -3.4e-05, -1
]
d: :-1.3e-06
 1.7e-05
       1

Value {1, 2}: (gtsam::EssentialMatrix)
R:
 [
        2e-06, -1.9e-05, -1;
        1.6e-05, 1, -1.9e-05;
        1, -1.6e-05, 2e-06
]
d: :   0.71
1.4e-05
   0.71

Value {1, 3}: (gtsam::EssentialMatrix)
R:
 [
        -1, 3.5e-06, 2e-10;
        3.5e-06, 1, -3.4e-05;
        -3.2e-10, -3.4e-05, -1
]
d: :-1.3e-06
 1.7e-05
       1

Value {2, 0}: (gtsam::EssentialMatrix)
R:
 [
        -1, 3.5e-06, 2e-10;
        3.5e-06, 1, -3.4e-05;
        -3.2e-10, -3.4e-05, -1
]
d: :-1.3e-06
 1.7e-05
       1

Value {2, 3}: (gtsam::EssentialMatrix)
R:
 [
        2e-06, -1.9e-05, -1;
        1.6e-05, 1, -1.9e-05;
        1, -1.6e-05, 2e-06
]
d: :   0.71
1.4e-05
   0.71

Value {3, 0}: (gtsam::EssentialMatrix)
R:
 [
        2e-06, -1.9e-05, -1;
        1.6e-05, 1, -1.9e-05;
        1, -1.6e-05, 2e-06
]
d: :   0.71
1.4e-05
   0.71

Value {3, 1}: (gtsam::EssentialMatrix)
R:
 [
        -1, 3.5e-06, 2e-10;
        3.5e-06, 1, -3.4e-05;
        -3.2e-10, -3.4e-05, -1
]
d: :-1.3e-06
 1.7e-05
       1

Value k0: (gtsam::Cal3_S2)
[
        50, -0.00017, 50;
        0, 50, 50;
        0, 0, 1
]

Value k1: (gtsam::Cal3_S2)
[
        50, -0.00017, 50;
        0, 50, 50;
        0, 0, 1
]

Value k2: (gtsam::Cal3_S2)
[
        50, -0.00017, 50;
        0, 50, 50;
        0, 0, 1
]

Value k3: (gtsam::Cal3_S2)
[
        50, -0.00017, 50;
        0, 50, 50;
        0, 0, 1
]

Ground Truth E1:
 4.3e-17    -0.71  2.7e-33
   -0.71        0    -0.71
-2.7e-33     0.71  4.3e-17
Ground Truth E2:
 7.5e-33       -1 -6.1e-17
      -1        0 -1.2e-16
 6.1e-17   -2e-18  7.5e-33

[dellaert]

@travisdriver was able to run Python version of EssentialViewGraphExample, see new commits :-)

[dellaert]

OK, I added 2 more cases: SimpleF, and Calibrated. Global F will be another PR. I experimented with storing a scaled s_ in F but it does not work uniformly well, so I removed it again. I now initialize F from SimpleF, which is why the number of iterations for F is small or 0.

Some findings and figure from one experiment:

The figure above shows comparison with 12 cameras, 20 points, and 10 trials. Every camera plays in 2 triplets, so total edges = 24. Hence, the dimensionality of each method is:
SimpleF: 168 = 7 x 24 (but sub-manifold!)
F: 168= 7 x 24
E + Ks: 132 = 5 x 24 + 12
calibrated E: 120 = 5 x 24
This experiment behaves as I would expect (smaller error for more DOF) except that I had expected Calibrated to be best in terms of ground truth...

[dellaert]

With 100 trials, however, the results match expectations exactly:

[travisdriver]

I think optimization of the calibrations should be reserved for the transfer factors that leverage the fundamental matrix, and this factor should only optimize values involved with the essential matrix, i.e., the relative orientation and direction.

[dellaert]

You can use the TransferFactor above, templated on EssentialMatrix, to optimize for R,t only. This factor does E and Ks simultaneously. It works very well as the data bears out.

[dellaert]

I'll think of better name

[travisdriver]

With 100 trials, however, the results match expectations exactly:

Is this saying that the median error is higher but the median geodesic distance between the ground truth and estimated fundamental matrices is lower for methods with less DOF? This seems counterintuitive but maybe I'm misunderstanding something.

[dellaert]

With 100 trials, however, the results match expectations exactly:

...

Is this saying that the median error is higher but the median geodesic distance between the ground truth and estimated fundamental matrices is lower for methods with less DOF? This seems counterintuitive but maybe I'm misunderstanding something.

In maximum-likelihood settings the more DOF, the less the error will be, but you will overfit on the noise. The graph shows that this is exactly what happens: the error is lowest for models with most parameters, but if we measure the geodesic distance to the ground truth, the models with the (correct) inductive biases are better, on average.

[travisdriver]

LGTM modulo adding something to the name that indicates calibrations are being optimized