From 00fc2ecc2ba88a1aaf65a234c7761ba018ffc77d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 24 Oct 2024 11:43:40 -0700
Subject: [PATCH] Slight modernization

---
 examples/SFMdata.h | 82 ++++++++++++++++++++++++++++------------------
 1 file changed, 50 insertions(+), 32 deletions(-)

diff --git a/examples/SFMdata.h b/examples/SFMdata.h
index 3031828f1..112bd927c 100644
--- a/examples/SFMdata.h
+++ b/examples/SFMdata.h
@@ -16,56 +16,74 @@
  */
 
 /**
- * A structure-from-motion example with landmarks, default function arguments give
+ * A structure-from-motion example with landmarks, default arguments give:
  *  - The landmarks form a 10 meter cube
  *  - The robot rotates around the landmarks, always facing towards the cube
- * Passing function argument allows to specificy an initial position, a pose increment and step count.
+ * Passing function argument allows to specify an initial position, a pose
+ * increment and step count.
  */
 
 #pragma once
 
-// As this is a full 3D problem, we will use Pose3 variables to represent the camera
-// positions and Point3 variables (x, y, z) to represent the landmark coordinates.
-// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
-// We will also need a camera object to hold calibration information and perform projections.
-#include <gtsam/geometry/Pose3.h>
+// As this is a full 3D problem, we will use Pose3 variables to represent the
+// camera positions and Point3 variables (x, y, z) to represent the landmark
+// coordinates. Camera observations of landmarks (i.e. pixel coordinates) will
+// be stored as Point2 (x, y).
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
 
-// We will also need a camera object to hold calibration information and perform projections.
-#include <gtsam/geometry/PinholeCamera.h>
+// We will also need a camera object to hold calibration information and perform
+// projections.
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/PinholeCamera.h>
 
-/* ************************************************************************* */
-std::vector<gtsam::Point3> createPoints() {
+namespace gtsam {
 
-  // Create the set of ground-truth landmarks
-  std::vector<gtsam::Point3> points;
-  points.push_back(gtsam::Point3(10.0,10.0,10.0));
-  points.push_back(gtsam::Point3(-10.0,10.0,10.0));
-  points.push_back(gtsam::Point3(-10.0,-10.0,10.0));
-  points.push_back(gtsam::Point3(10.0,-10.0,10.0));
-  points.push_back(gtsam::Point3(10.0,10.0,-10.0));
-  points.push_back(gtsam::Point3(-10.0,10.0,-10.0));
-  points.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
-  points.push_back(gtsam::Point3(10.0,-10.0,-10.0));
+/// Create a set of ground-truth landmarks
+std::vector<Point3> createPoints() {
+  std::vector<Point3> points;
+  points.push_back(Point3(10.0, 10.0, 10.0));
+  points.push_back(Point3(-10.0, 10.0, 10.0));
+  points.push_back(Point3(-10.0, -10.0, 10.0));
+  points.push_back(Point3(10.0, -10.0, 10.0));
+  points.push_back(Point3(10.0, 10.0, -10.0));
+  points.push_back(Point3(-10.0, 10.0, -10.0));
+  points.push_back(Point3(-10.0, -10.0, -10.0));
+  points.push_back(Point3(10.0, -10.0, -10.0));
 
   return points;
 }
 
-/* ************************************************************************* */
-std::vector<gtsam::Pose3> createPoses(
-            const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)),
-            const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))),
-            int steps = 8) {
+/**
+ * Create a set of ground-truth poses
+ *  Default values give a circular trajectory, radius 30 at pi/4 intervals,
+ * always facing the circle center
+ */
+std::vector<Pose3> createPoses(
+    const Pose3& init = Pose3(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {30, 0, 0}),
+    const Pose3& delta = Pose3(Rot3::Ypr(0, -M_PI_4, 0),
+                               {sin(M_PI_4) * 30, 0, 30 * (1 - sin(M_PI_4))}),
+    int steps = 8) {
+  std::vector<Pose3> poses;
+  poses.reserve(steps);
 
-  // Create the set of ground-truth poses
-  // Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center
-  std::vector<gtsam::Pose3> poses;
-  int i = 1;
   poses.push_back(init);
-  for(; i < steps; ++i) {
-    poses.push_back(poses[i-1].compose(delta));
+  for (int i = 1; i < steps; ++i) {
+    poses.push_back(poses[i - 1].compose(delta));
   }
 
   return poses;
 }
+
+/**
+ * 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))});
+  return createPoses(init, delta, num_cameras);
+}
+}  // namespace gtsam
\ No newline at end of file