TransferFactor with numerical derivatives

[dellaert]

Overview

• TransferFactor, a binary factor a la Sweeney15
• new Key-like class, EdgeKey, which is used to do transfer correctly
• some misc changes in adjacent files
• an example that shows everything put together, ViewGraphExample.cpp

Update

I realized that by batching all the points for a particular transfer config, the numerical derivatives will be much faster: while we have to twiddle up and down 14 times, now we get results for all the 2D points for that transfer.

Testing

Many unit tests, and example to show it optimizes to zero. Result of latter:

errors: size: 12

Factor 0:   keys = { {0, 2} {1, 2} }
error = 619.74

Factor 1:   keys = { {0, 1} {1, 2} }
error = 772.809

Factor 2:   keys = { {0, 2} {0, 1} }
error = 246.764

Factor 3:   keys = { {1, 3} {2, 3} }
error = 619.74

Factor 4:   keys = { {1, 2} {2, 3} }
error = 772.809

Factor 5:   keys = { {1, 3} {1, 2} }
error = 246.764

Factor 6:   keys = { {2, 0} {3, 0} }
error = 619.74

Factor 7:   keys = { {2, 3} {3, 0} }
error = 772.809

Factor 8:   keys = { {2, 0} {2, 3} }
error = 246.764

Factor 9:   keys = { {3, 1} {0, 1} }
error = 619.74

Factor 10:   keys = { {3, 0} {0, 1} }
error = 772.809

Factor 11:   keys = { {3, 1} {3, 0} }
error = 246.764

Initial error: 6557.25, values: 8
iter      cost      cost_change    lambda  success iter_time
   0      86.3644      6.5e+03      1e+03      1          0
   1           11           76      1e+02      1          0
   2        0.066           11         10      1       0.01
   3      2.4e-06        0.066          1      1          0
   4      3.9e-10      2.4e-06        0.1      1          0
initial error = 6.6e+03
final error = 3.9e-10
Final results:

Values with 8 values:
Value {0, 1}: (gtsam::FundamentalMatrix)
 5.5e-12  -0.0002     0.01
 -0.0002 -1.9e-11    9e-10
    0.01     0.02       -1

Value {0, 2}: (gtsam::FundamentalMatrix)
0.00017 3.5e-05   -0.01
3.5e-05 0.00017   -0.01
  -0.01   -0.01       1

Value {1, 2}: (gtsam::FundamentalMatrix)
 5.5e-12  -0.0002     0.01
 -0.0002 -1.9e-11    9e-10
    0.01     0.02       -1

Value {1, 3}: (gtsam::FundamentalMatrix)
0.00017 3.5e-05   -0.01
3.5e-05 0.00017   -0.01
  -0.01   -0.01       1

Value {2, 0}: (gtsam::FundamentalMatrix)
0.00017 3.5e-05   -0.01
3.5e-05 0.00017   -0.01
  -0.01   -0.01       1

Value {2, 3}: (gtsam::FundamentalMatrix)
 5.5e-12  -0.0002     0.01
 -0.0002 -1.9e-11    9e-10
    0.01     0.02       -1

Value {3, 0}: (gtsam::FundamentalMatrix)
 5.5e-12  -0.0002     0.01
 -0.0002 -1.9e-11    9e-10
    0.01     0.02       -1

Value {3, 1}: (gtsam::FundamentalMatrix)
0.00017 3.5e-05   -0.01
3.5e-05 0.00017   -0.01
  -0.01   -0.01       1

Ground Truth F1:
-5.2e-20  -0.0002     0.01
 -0.0002    4e-21  8.3e-19
    0.01     0.02       -1
Ground Truth F2:
 0.0002 7.6e-20   -0.01
7.6e-20  0.0002   -0.01
  -0.01   -0.01       1```

[dellaert]

PS I used git push --force-with-lease

[akshay-krishnan]

Looks great!