New example now uses EssentialTransferFactor

examples/EssentialViewGraphExample.cpp

@@ -17,57 +17,53 @@
  */
 
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/EdgeKey.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/sfm/TransferFactor.h>
+#include <gtsam/sfm/TransferFactor.h>  // Contains EssentialTransferFactor
 
 #include <vector>
 
-#include "SFMdata.h"
-#include "gtsam/inference/Key.h"
+#include "SFMdata.h"  // For createPoints() and posesOnCircle()
 
 using namespace std;
 using namespace gtsam;
+using namespace symbol_shorthand;  // For K(symbol)
 
-/* ************************************************************************* */
+// Main function
 int main(int argc, char* argv[]) {
   // Define the camera calibration parameters
-  Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
+  Cal3_S2 K_initial(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 = FundamentalMatrix(K, poses[0].between(poses[1]), K);
-  auto F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K);
+  // Calculate ground truth essential matrices, 1 and 2 poses apart
+  auto E1 = EssentialMatrix::FromPose3(poses[0].between(poses[1]));
+  auto E2 = EssentialMatrix::FromPose3(poses[0].between(poses[2]));
 
   // Simulate measurements from each camera pose
   std::array<std::array<Point2, 8>, 4> p;
   for (size_t i = 0; i < 4; ++i) {
-    PinholeCamera<Cal3_S2> camera(poses[i], K);
+    PinholeCamera<Cal3_S2> camera(poses[i], K_initial);
     for (size_t j = 0; j < 8; ++j) {
       p[i][j] = camera.project(points[j]);
     }
   }
 
-  // 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.
+  // Create the factor graph
   NonlinearFactorGraph graph;
-  using Factor = TransferFactor<FundamentalMatrix>;
+  using Factor = EssentialTransferFactor<Cal3_S2>;
 
   for (size_t a = 0; a < 4; ++a) {
     size_t b = (a + 1) % 4;  // Next camera
@@ -83,54 +79,60 @@ int main(int argc, char* argv[]) {
       tuples3.emplace_back(p[c][j], p[b][j], p[a][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.
+    // Add transfer factors between views a, b, and c.
     graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), tuples1);
     graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), tuples2);
     graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), tuples3);
   }
 
+  // Formatter for printing keys
   auto formatter = [](Key key) {
-    EdgeKey edge(key);
-    return (std::string)edge;
+    if (Symbol(key).chr() == 'k') {
+      return (string)Symbol(key);
+    } else {
+      EdgeKey edge(key);
+      return (std::string)edge;
+    }
   };
 
   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 *= 1e-5;
+  // Create a delta vector to perturb the ground truth (small perturbation)
+  Vector5 delta;
+  delta << 1, 1, 1, 1, 1;
+  delta *= 1e-2;
 
-  // Create the data structure to hold the initial estimate to the solution
+  // Create the initial estimate for essential matrices
   Values initialEstimate;
   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), F1.retract(delta));
-    initialEstimate.insert(EdgeKey(a, c), F2.retract(delta));
+    initialEstimate.insert(EdgeKey(a, b), E1.retract(delta));
+    initialEstimate.insert(EdgeKey(a, c), E2.retract(delta));
   }
+
+  // Insert initial calibrations (using K symbol)
+  for (size_t i = 0; i < 4; ++i) {
+    initialEstimate.insert(K(i), K_initial);
+  }
+
   initialEstimate.print("Initial Estimates:\n", formatter);
-    graph.printErrors(initialEstimate, "errors: ", formatter);
+  graph.printErrors(initialEstimate, "Initial Errors:\n", formatter);
 
-  /* Optimize the graph and print results */
+  // Optimize the graph and print results
   LevenbergMarquardtParams params;
   params.setlambdaInitial(1000.0);  // Initialize lambda to a high value
   params.setVerbosityLM("SUMMARY");
   Values result =
       LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize();
 
-  cout << "initial error = " << graph.error(initialEstimate) << endl;
-  cout << "final error = " << graph.error(result) << endl;
+  cout << "Initial error = " << graph.error(initialEstimate) << endl;
+  cout << "Final error = " << graph.error(result) << endl;
 
-  result.print("Final results:\n", formatter);
+  result.print("Final Results:\n", formatter);
 
-  cout << "Ground Truth F1:\n" << F1.matrix() << endl;
-  cout << "Ground Truth F2:\n" << F2.matrix() << endl;
+  cout << "Ground Truth E1:\n" << E1.matrix() << endl;
+  cout << "Ground Truth E2:\n" << E2.matrix() << endl;
 
   return 0;
-}
-/* ************************************************************************* */
+}