Example with SimpleF

examples/SFMdata.h

@@ -78,12 +78,27 @@ std::vector<Pose3> createPoses(
 /**
  * Create regularly spaced poses with specified radius and number of cameras
  */
-std::vector<Pose3> posesOnCircle(int num_cameras = 8, double radius = 30) {
-  const Pose3 init(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {radius, 0, 0});
-  const double theta = M_PI / num_cameras;
-  const Pose3 delta(
-      Rot3::Ypr(0, -2 * theta, 0),
-      {sin(2 * theta) * radius, 0, radius * (1 - sin(2 * theta))});
+std::vector<Pose3> posesOnCircle(int num_cameras = 8, double R = 30) {
+  const double theta = 2 * M_PI / num_cameras;
+
+  // Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis
+  // pointing down
+  const Pose3 init(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {R, 0, 0});
+
+  // Delta rotation: rotate by -theta around Z-axis (counterclockwise movement)
+  Rot3 delta_rotation = Rot3::Ypr(0, -theta, 0);
+
+  // Delta translation in world frame
+  Vector3 delta_translation_world(R * (cos(theta) - 1), R * sin(theta), 0);
+
+  // Transform delta translation to local frame of the camera
+  Vector3 delta_translation_local =
+      init.rotation().inverse() * delta_translation_world;
+
+  // Define delta pose
+  const Pose3 delta(delta_rotation, delta_translation_local);
+
+  // Generate poses using createPoses
   return createPoses(init, delta, num_cameras);
 }
 }  // namespace gtsam

examples/ViewGraphExample.cpp

@@ -0,0 +1,113 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    ViewGraphExample.cpp
+ * @brief   View-graph calibration on a simulated dataset, a la Sweeney 2015
+ * @author  Frank Dellaert
+ * @author  October 2024
+ */
+
+#include <gtsam/geometry/Cal3_S2.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/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/sfm/TransferFactor.h>
+
+#include <vector>
+
+#include "SFMdata.h"
+#include "gtsam/geometry/EssentialMatrix.h"
+#include "gtsam/inference/Key.h"
+
+using namespace std;
+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));
+
+  // 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);
+
+  // 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);
+    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
+  NonlinearFactorGraph graph;
+
+  using Factor = TransferFactor<SimpleFundamentalMatrix>;
+  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) {
+      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],
+                                   p[c][j], p[b][j]);
+      graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), p[c][j],
+                                   p[b][j], p[a][j]);
+    }
+  }
+
+  auto formatter = [](Key key) {
+    EdgeKey edge(key);
+    return (std::string)edge;
+  };
+
+  graph.print("Factor Graph:\n", formatter);
+
+  // 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.print("Initial Estimates:\n", formatter);
+  //   graph.printErrors(initialEstimate, "errors: ", formatter);
+
+  /* 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();
+  result.print("Final results:\n", formatter);
+  cout << "initial error = " << graph.error(initialEstimate) << endl;
+  cout << "final error = " << graph.error(result) << endl;
+
+  return 0;
+}
+/* ************************************************************************* */