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: