Fix createPoses

examples/SFMdata.h

@@ -56,7 +56,7 @@ std::vector<Point3> createPoints() {
 
 /**
  * Create a set of ground-truth poses
- *  Default values give a circular trajectory, radius 30 at pi/4 intervals,
+ * Default values give a circular trajectory, radius 30 at pi/4 intervals,
  * always facing the circle center
  */
 std::vector<Pose3> createPoses(

python/gtsam/examples/SFMExample.py

@@ -62,7 +62,7 @@ def main():
     points = SFMdata.createPoints()
 
     # Create the set of ground-truth poses
-    poses = SFMdata.createPoses(K)
+    poses = SFMdata.createPoses()
 
     # Create a factor graph
     graph = NonlinearFactorGraph()

python/gtsam/examples/SFMdata.py

@@ -3,14 +3,14 @@ A structure-from-motion example with landmarks
  - The landmarks form a 10 meter cube
  - The robot rotates around the landmarks, always facing towards the cube
 """
+
 # pylint: disable=invalid-name, E1101
 
 from typing import List
 
 import numpy as np
 
-import gtsam
-from gtsam import Cal3_S2, Point3, Pose3
+from gtsam import Point3, Pose3, Rot3
 
 
 def createPoints() -> List[Point3]:
@@ -28,16 +28,49 @@ def createPoints() -> List[Point3]:
     return points
 
 
-def createPoses(K: Cal3_S2) -> List[Pose3]:
-    """Generate a set of ground-truth camera poses arranged in a circle about the origin."""
-    radius = 40.0
-    height = 10.0
-    angles = np.linspace(0, 2 * np.pi, 8, endpoint=False)
-    up = gtsam.Point3(0, 0, 1)
-    target = gtsam.Point3(0, 0, 0)
-    poses = []
-    for theta in angles:
-        position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), height)
-        camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K)
-        poses.append(camera.pose())
+_M_PI_2 = np.pi / 2
+_M_PI_4 = np.pi / 4
+
+
+def createPoses(
+    init: Pose3 = Pose3(Rot3.Ypr(_M_PI_2, 0, -_M_PI_2), Point3(30, 0, 0)),
+    delta: Pose3 = Pose3(
+        Rot3.Ypr(0, -_M_PI_4, 0),
+        Point3(np.sin(_M_PI_4) * 30, 0, 30 * (1 - np.sin(_M_PI_4))),
+    ),
+    steps: int = 8,
+) -> List[Pose3]:
+    """
+    Create a set of ground-truth poses
+    Default values give a circular trajectory, radius 30 at pi/4 intervals,
+    always facing the circle center
+    """
+    poses = [init]
+    for _ in range(1, steps):
+        poses.append(poses[-1].compose(delta))
     return poses
+
+
+def posesOnCircle(num_cameras=8, R=30):
+    """Create regularly spaced poses with specified radius and number of cameras."""
+    theta = 2 * np.pi / num_cameras
+
+    # Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis pointing down
+    init_rotation = Rot3.Ypr(_M_PI_2, 0, -_M_PI_2)
+    init_position = np.array([R, 0, 0])
+    init = Pose3(init_rotation, init_position)
+
+    # Delta rotation: rotate by -theta around Z-axis (counterclockwise movement)
+    delta_rotation = Rot3.Ypr(0, -theta, 0)
+
+    # Delta translation in world frame
+    delta_translation_world = np.array([R * (np.cos(theta) - 1), R * np.sin(theta), 0])
+
+    # Transform delta translation to local frame of the camera
+    delta_translation_local = init.rotation().unrotate(delta_translation_world)
+
+    # Define delta pose
+    delta = Pose3(delta_rotation, delta_translation_local)
+
+    # Generate poses
+    return createPoses(init, delta, num_cameras)

python/gtsam/examples/TranslationAveragingExample.py

@@ -31,8 +31,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]:
     that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata
     and the unit translations directions between some camera pairs are computed from their
     global translations. """
-    fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0
-    wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0))
+    wTc_list = SFMdata.createPoses()
     # Rotations of the cameras in the world frame.
     wRc_values = gtsam.Values()
     # Normalized translation directions from camera i to camera j

python/gtsam/examples/VisualISAM2Example.py

@@ -73,7 +73,7 @@ def visual_ISAM2_example():
     points = SFMdata.createPoints()
 
     # Create the set of ground-truth poses
-    poses = SFMdata.createPoses(K)
+    poses = SFMdata.createPoses()
 
     # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
     # to maintain proper linearization and efficient variable ordering, iSAM2

python/gtsam/examples/VisualISAMExample.py

@@ -36,7 +36,7 @@ def main():
     # Create the set of ground-truth landmarks
     points = SFMdata.createPoints()
     # Create the set of ground-truth poses
-    poses = SFMdata.createPoses(K)
+    poses = SFMdata.createPoses()
 
     # Create a NonlinearISAM object which will relinearize and reorder the variables
     # every "reorderInterval" updates