diff --git a/examples/SFMdata.h b/examples/SFMdata.h index f2561b490..2a86aa593 100644 --- a/examples/SFMdata.h +++ b/examples/SFMdata.h @@ -56,7 +56,7 @@ std::vector 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 createPoses( diff --git a/python/gtsam/examples/SFMExample.py b/python/gtsam/examples/SFMExample.py index 87bb3cb87..c8d1f1271 100644 --- a/python/gtsam/examples/SFMExample.py +++ b/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() diff --git a/python/gtsam/examples/SFMdata.py b/python/gtsam/examples/SFMdata.py index ad414a256..1ce7430f6 100644 --- a/python/gtsam/examples/SFMdata.py +++ b/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) diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index ea1cab88d..e3380ce94 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/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 diff --git a/python/gtsam/examples/VisualISAM2Example.py b/python/gtsam/examples/VisualISAM2Example.py index 60d4fb376..03dd079f0 100644 --- a/python/gtsam/examples/VisualISAM2Example.py +++ b/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 diff --git a/python/gtsam/examples/VisualISAMExample.py b/python/gtsam/examples/VisualISAMExample.py index 9691b3c46..48e803919 100644 --- a/python/gtsam/examples/VisualISAMExample.py +++ b/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