Switch to general F

examples/SFMExample.cpp

@@ -39,7 +39,7 @@
 // Finally, once all of the factors have been added to our factor graph, we will want to
 // solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
 // GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
-// trust-region method known as Powell's Degleg
+// trust-region method known as Powell's Dogleg
 #include <gtsam/nonlinear/DoglegOptimizer.h>
 
 // The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
@@ -57,7 +57,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
   // Define the camera calibration parameters
-  Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
+  auto K = std::make_shared<Cal3_S2>(50.0, 50.0, 0.0, 50.0, 50.0);
 
   // Define the camera observation noise model
   auto measurementNoise =

examples/ViewGraphExample.cpp

@@ -30,7 +30,6 @@
 #include <vector>
 
 #include "SFMdata.h"
-#include "gtsam/geometry/EssentialMatrix.h"
 #include "gtsam/inference/Key.h"
 
 using namespace std;
@@ -39,33 +38,44 @@ using namespace gtsam;
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
   // Define the camera calibration parameters
-  Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
+  Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
 
   // Create the set of 8 ground-truth landmarks
   vector<Point3> points = createPoints();
 
   // Create the set of 4 ground-truth poses
   vector<Pose3> poses = posesOnCircle(4, 30);
 
+  // Calculate ground truth fundamental matrices, 1 and 2 poses apart
+  auto F1 = GeneralFundamentalMatrix(K, poses[0].between(poses[1]), K);
+  auto F2 = GeneralFundamentalMatrix(K, poses[0].between(poses[2]), K);
+
   // Simulate measurements from each camera pose
   std::array<std::array<Point2, 8>, 4> p;
   for (size_t i = 0; i < 4; ++i) {
-    GTSAM_PRINT(poses[i]);
-    PinholeCamera<Cal3_S2> camera(poses[i], *K);
+    PinholeCamera<Cal3_S2> camera(poses[i], K);
     for (size_t j = 0; j < 8; ++j) {
-      cout << "Camera index: " << i << ", Landmark index: " << j << endl;
       p[i][j] = camera.project(points[j]);
     }
   }
 
-  // Create a factor graph
+  // This section of the code is inspired by the work of Sweeney et al.
+  // [link](sites.cs.ucsb.edu/~holl/pubs/Sweeney-2015-ICCV.pdf) on view-graph
+  // calibration. The graph is made up of transfer factors that enforce the
+  // epipolar constraint between corresponding points across three views, as
+  // described in the paper. Rather than adding one ternary error term per point
+  // in a triplet, we add three binary factors for sparsity during optimization.
+  // In this version, we only include triplets between 3 successive cameras.
   NonlinearFactorGraph graph;
-
-  using Factor = TransferFactor<SimpleFundamentalMatrix>;
+  using Factor = TransferFactor<GeneralFundamentalMatrix>;
   for (size_t a = 0; a < 4; ++a) {
     size_t b = (a + 1) % 4;  // Next camera
     size_t c = (a + 2) % 4;  // Camera after next
     for (size_t j = 0; j < 4; ++j) {
+      // Add transfer factors between views a, b, and c. Note that the EdgeKeys
+      // are crucial in performing the transfer in the right direction. We use
+      // exactly 8 unique EdgeKeys, corresponding to 8 unknown fundamental
+      // matrices we will optimize for.
       graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), p[a][j],
                                    p[b][j], p[c][j]);
       graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), p[a][j],
@@ -82,18 +92,19 @@ int main(int argc, char* argv[]) {
 
   graph.print("Factor Graph:\n", formatter);
 
+  // Create a delta vector to perturb the ground truth
+  // We can't really go far before convergence becomes problematic :-(
+  Vector7 delta;
+  delta << 1, 2, 3, 4, 5, 6, 7;
+  delta *= 5e-5;
+
   // Create the data structure to hold the initial estimate to the solution
   Values initialEstimate;
-  const Point2 center(50, 50);
-  auto E1 = EssentialMatrix::FromPose3(poses[0].between(poses[1]));
-  auto E2 = EssentialMatrix::FromPose3(poses[0].between(poses[2]));
   for (size_t a = 0; a < 4; ++a) {
     size_t b = (a + 1) % 4;  // Next camera
     size_t c = (a + 2) % 4;  // Camera after next
-    initialEstimate.insert(EdgeKey(a, b),
-                           SimpleFundamentalMatrix(E1, 50, 50, center, center));
-    initialEstimate.insert(EdgeKey(a, c),
-                           SimpleFundamentalMatrix(E2, 50, 50, center, center));
+    initialEstimate.insert(EdgeKey(a, b), F1.retract(delta));
+    initialEstimate.insert(EdgeKey(a, c), F2.retract(delta));
   }
   initialEstimate.print("Initial Estimates:\n", formatter);
   //   graph.printErrors(initialEstimate, "errors: ", formatter);
@@ -104,10 +115,15 @@ int main(int argc, char* argv[]) {
   params.setVerbosityLM("SUMMARY");
   Values result =
       LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize();
-  result.print("Final results:\n", formatter);
+
   cout << "initial error = " << graph.error(initialEstimate) << endl;
   cout << "final error = " << graph.error(result) << endl;
 
+  result.print("Final results:\n", formatter);
+
+  cout << "Ground Truth F1:\n" << F1.matrix() << endl;
+  cout << "Ground Truth F2:\n" << F2.matrix() << endl;
+
   return 0;
 }
 /* ************************************************************************* */