Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

EssentialTransferFactor #1887

Open
wants to merge 34 commits into
base: feature/wrapF
Choose a base branch
from
Open

Conversation

dellaert
Copy link
Member

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 dellaert changed the base branch from develop to feature/tripletFactor October 26, 2024 03:40
@dellaert
Copy link
Member Author

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

Base automatically changed from feature/tripletFactor to develop October 27, 2024 06:10
@dellaert dellaert changed the base branch from develop to feature/wrapF October 28, 2024 20:01
@dellaert
Copy link
Member Author

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:

image

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 = 724 (but sub-manifold!)
F: 168= 7
24
E + Ks: 132 = 524 + 12
calibrated E: 120 = 5
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, but E+K2 is...

@dellaert
Copy link
Member Author

With 100 trials it seems Calibrated is the best (although I had expected E+Ks to do better):

image

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

1 participant