gtsam-develop 4.3a0.dev202506050756__cp313-cp313-macosx_11_0_arm64.whl → 4.3a0.dev202507301459__cp313-cp313-macosx_11_0_arm64.whl

gtsam/gtsam/imuBias.pyi

@@ -52,10 +52,16 @@ class ConstantBias:
         """
         get gyroscope bias
         """
+    def localCoordinates(self, b: ConstantBias) -> numpy.ndarray[tuple[typing.Literal[6], typing.Literal[1]], numpy.dtype[numpy.float64]]:
+        ...
     def print(self, s: str = '') -> None:
         """
         print with optional string
         """
+    def retract(self, v: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> ConstantBias:
+        """
+        The retract function.
+        """
     def serialize(self) -> str:
         ...
     def vector(self) -> numpy.ndarray[tuple[typing.Literal[6], typing.Literal[1]], numpy.dtype[numpy.float64]]:

gtsam/tests/test_Cal3Fisheye.py

@@ -91,8 +91,8 @@ class TestCal3Fisheye(GtsamTestCase):
         noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
         pose_key, point_key = P(0), L(0)
         k = gtsam.Cal3Fisheye()
-        state.insert_pose3(pose_key, gtsam.Pose3())
-        state.insert_point3(point_key, self.obj_point)
+        state.insert(pose_key, gtsam.Pose3())
+        state.insertPoint3(point_key, self.obj_point)
         factor = gtsam.GenericProjectionFactorCal3Fisheye(measured, noise_model, pose_key, point_key, k)
         graph.add(factor)
         score = graph.error(state)
@@ -106,9 +106,9 @@ class TestCal3Fisheye(GtsamTestCase):
         noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
         camera_key, pose_key, landmark_key = K(0), P(0), L(0)
         k = gtsam.Cal3Fisheye()
-        state.insert_cal3fisheye(camera_key, k)
-        state.insert_pose3(pose_key, gtsam.Pose3())
-        state.insert_point3(landmark_key, gtsam.Point3(self.obj_point))
+        state.insert(camera_key, k)
+        state.insert(pose_key, gtsam.Pose3())
+        state.insertPoint3(landmark_key, gtsam.Point3(self.obj_point))
         factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model, pose_key, landmark_key, camera_key)
         graph.add(factor)
         score = graph.error(state)
@@ -168,8 +168,8 @@ class TestCal3Fisheye(GtsamTestCase):
         camera = gtsam.Cal3Fisheye()
         state = gtsam.Values()
         pose_key, landmark_key = P(0), L(0)
-        state.insert_point3(landmark_key, obj_point)
-        state.insert_pose3(pose_key, pose)
+        state.insertPoint3(landmark_key, obj_point)
+        state.insert(pose_key, pose)
         g = gtsam.NonlinearFactorGraph()
         noise_model = gtsam.noiseModel.Unit.Create(2)
         factor = gtsam.GenericProjectionFactorCal3Fisheye(img_point, noise_model, pose_key, landmark_key, camera)

gtsam/tests/test_Cal3Unified.py

@@ -93,8 +93,8 @@ class TestCal3Unified(GtsamTestCase):
         noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
         pose_key, point_key = P(0), L(0)
         k = self.stereographic
-        state.insert_pose3(pose_key, gtsam.Pose3())
-        state.insert_point3(point_key, self.obj_point)
+        state.insert(pose_key, gtsam.Pose3())
+        state.insertPoint3(point_key, self.obj_point)
         factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k)
         graph.add(factor)
         score = graph.error(state)
@@ -109,9 +109,9 @@ class TestCal3Unified(GtsamTestCase):
         noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
         camera_key, pose_key, landmark_key = K(0), P(0), L(0)
         k = self.stereographic
-        state.insert_cal3unified(camera_key, k)
-        state.insert_pose3(pose_key, gtsam.Pose3())
-        state.insert_point3(landmark_key, self.obj_point)
+        state.insert(camera_key, k)
+        state.insert(pose_key, gtsam.Pose3())
+        state.insertPoint3(landmark_key, self.obj_point)
         factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key)
         graph.add(factor)
         score = graph.error(state)
@@ -125,9 +125,9 @@ class TestCal3Unified(GtsamTestCase):
         camera = gtsam.Cal3Unified()
         state = gtsam.Values()
         camera_key, pose_key, landmark_key = K(0), P(0), L(0)
-        state.insert_cal3unified(camera_key, camera)
-        state.insert_point3(landmark_key, obj_point_on_axis)
-        state.insert_pose3(pose_key, pose)
+        state.insert(camera_key, camera)
+        state.insertPoint3(landmark_key, obj_point_on_axis)
+        state.insert(pose_key, pose)
         g = gtsam.NonlinearFactorGraph()
         noise_model = gtsam.noiseModel.Unit.Create(2)
         factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model, pose_key, landmark_key, camera_key)

gtsam/tests/test_FrobeniusFactor.py

@@ -12,8 +12,24 @@ Author: Frank Dellaert
 import unittest
 
 import numpy as np
-from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
-                   ShonanFactor3, SOn)
+from gtsam import (
+    Rot3,
+    SO3,
+    SO4,
+    FrobeniusBetweenFactorSO4,
+    FrobeniusFactorSO4,
+    ShonanFactor3,
+    SOn,
+    Similarity2,
+    Similarity3,
+    FrobeniusFactorSimilarity2,
+    FrobeniusBetweenFactorSimilarity2,
+    FrobeniusFactorSimilarity3,
+    FrobeniusBetweenFactorSimilarity3,
+    Gal3,
+    FrobeniusFactorGal3,
+    FrobeniusBetweenFactorGal3,
+)
 
 id = SO4()
 v1 = np.array([0, 0, 0, 0.1, 0, 0])
@@ -21,6 +37,15 @@ Q1 = SO4.Expmap(v1)
 v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
 Q2 = SO4.Expmap(v2)
 
+P1_sim2 = Similarity2.Expmap(np.array([0.1, 0.2, 0.3, 0.4]))
+P2_sim2 = Similarity2.Expmap(np.array([0.2, 0.3, 0.4, 0.5]))
+
+P1_sim3 = Similarity3.Expmap(np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7]))
+P2_sim3 = Similarity3.Expmap(np.array([0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8]))
+
+G1_gal3 = Gal3(Rot3.Rz(0.1), np.array([0.2, 0.3, 0.4]), np.array([0.5, 0.6, 0.7]), 0.8)
+G2_gal3 = Gal3(Rot3.Rz(0.2), np.array([0.3, 0.4, 0.5]), np.array([0.6, 0.7, 0.8]), 0.9)
+
 
 class TestFrobeniusFactorSO4(unittest.TestCase):
     """Test FrobeniusFactor factors."""
@@ -29,7 +54,7 @@ class TestFrobeniusFactorSO4(unittest.TestCase):
         """Test creation of a factor that calculates the Frobenius norm."""
         factor = FrobeniusFactorSO4(1, 2)
         actual = factor.evaluateError(Q1, Q2)
-        expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,))
+        expected = (Q2.matrix() - Q1.matrix()).transpose().reshape((16,))
         np.testing.assert_array_equal(actual, expected)
 
     def test_frobenius_between_factor(self):
@@ -52,5 +77,47 @@ class TestFrobeniusFactorSO4(unittest.TestCase):
         np.testing.assert_array_almost_equal(actual, expected, decimal=4)
 
 
+class TestFrobeniusFactorSimilarity2(unittest.TestCase):
+    def test_frobenius_factor(self):
+        factor = FrobeniusFactorSimilarity2(1, 2)
+        actual = factor.evaluateError(P1_sim2, P2_sim2)
+        expected = (P2_sim2.matrix() - P1_sim2.matrix()).transpose().reshape((9,))
+        np.testing.assert_allclose(actual, expected, atol=1e-9)
+
+    def test_frobenius_between_factor(self):
+        factor = FrobeniusBetweenFactorSimilarity2(1, 2, P1_sim2.between(P2_sim2))
+        actual = factor.evaluateError(P1_sim2, P2_sim2)
+        expected = np.zeros((9,))
+        np.testing.assert_allclose(actual, expected, atol=1e-9)
+
+
+class TestFrobeniusFactorSimilarity3(unittest.TestCase):
+    def test_frobenius_factor(self):
+        factor = FrobeniusFactorSimilarity3(1, 2)
+        actual = factor.evaluateError(P1_sim3, P2_sim3)
+        expected = (P2_sim3.matrix() - P1_sim3.matrix()).transpose().reshape((16,))
+        np.testing.assert_allclose(actual, expected, atol=1e-9)
+
+    def test_frobenius_between_factor(self):
+        factor = FrobeniusBetweenFactorSimilarity3(1, 2, P1_sim3.between(P2_sim3))
+        actual = factor.evaluateError(P1_sim3, P2_sim3)
+        expected = np.zeros((16,))
+        np.testing.assert_allclose(actual, expected, atol=1e-9)
+
+
+class TestFrobeniusFactorGal3(unittest.TestCase):
+    def test_frobenius_factor(self):
+        factor = FrobeniusFactorGal3(1, 2)
+        actual = factor.evaluateError(G1_gal3, G2_gal3)
+        expected = (G2_gal3.matrix() - G1_gal3.matrix()).transpose().reshape((25,))
+        np.testing.assert_allclose(actual, expected, atol=1e-9)
+
+    def test_frobenius_between_factor(self):
+        factor = FrobeniusBetweenFactorGal3(1, 2, G1_gal3.between(G2_gal3))
+        actual = factor.evaluateError(G1_gal3, G2_gal3)
+        expected = np.zeros((25,))
+        np.testing.assert_allclose(actual, expected, atol=1e-9)
+
+
 if __name__ == "__main__":
     unittest.main()

gtsam/tests/test_GraphvizFormatting.py

@@ -29,9 +29,9 @@ class TestGraphvizFormatting(GtsamTestCase):
         self.graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
 
         self.values = gtsam.Values()
-        self.values.insert_pose2(0, gtsam.Pose2(0., 0., 0.))
-        self.values.insert_pose2(1, gtsam.Pose2(2., 0., 0.))
-        self.values.insert_pose2(2, gtsam.Pose2(4., 0., 0.))
+        self.values.insert(0, gtsam.Pose2(0., 0., 0.))
+        self.values.insert(1, gtsam.Pose2(2., 0., 0.))
+        self.values.insert(2, gtsam.Pose2(4., 0., 0.))
 
     def test_default(self):
         """Test with default GraphvizFormatting"""

gtsam/tests/test_HybridNonlinearFactorGraph.py

@@ -40,8 +40,8 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
         nlfg.push_back(gtsam.HybridNonlinearFactor(mode, factors))
         nlfg.push_back(gtsam.DecisionTreeFactor(mode, "1 3"))
         values = gtsam.Values()
-        values.insert_point3(1, Point3(0, 0, 0))
-        values.insert_point3(2, Point3(2, 3, 1))
+        values.insertPoint3(1, Point3(0, 0, 0))
+        values.insertPoint3(2, Point3(2, 3, 1))
         hfg = nlfg.linearize(values)
         hbn = hfg.eliminateSequential()
         hbv = hbn.optimize()

gtsam/tests/test_Sim2.py

@@ -8,6 +8,7 @@ See LICENSE for the license information
 Sim3 unit tests.
 Author: John Lambert
 """
+
 # pylint: disable=no-name-in-module
 import unittest
 
@@ -27,12 +28,12 @@ class TestSim2(GtsamTestCase):
            3 object poses
            same scale (no gauge ambiguity)
            world frame has poses rotated about 180 degrees.
-           world and egovehicle frame translated by 15 meters w.r.t. each other
+           world and ego-vehicle frame translated by 15 meters w.r.t. each other
         """
         R180 = Rot2.fromDegrees(180)
 
-        # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
-        # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+        # Create source poses (three objects o1, o2, o3 living in the ego-vehicle "e" frame)
+        # Suppose they are 3d cuboids detected by an onboard sensor in the ego-vehicle frame
         eTo0 = Pose2(Rot2(), np.array([5, 0]))
         eTo1 = Pose2(Rot2(), np.array([10, 0]))
         eTo2 = Pose2(Rot2(), np.array([15, 0]))
@@ -49,7 +50,7 @@ class TestSim2(GtsamTestCase):
 
         we_pairs = list(zip(wToi_list, eToi_list))
 
-        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        # Recover the transformation wSe (i.e. world_S_ego-vehicle)
         wSe = Similarity2.Align(we_pairs)
 
         for wToi, eToi in zip(wToi_list, eToi_list):
@@ -62,12 +63,12 @@ class TestSim2(GtsamTestCase):
            3 object poses
            with gauge ambiguity (2x scale)
            world frame has poses rotated by 90 degrees.
-           world and egovehicle frame translated by 11 meters w.r.t. each other
+           world and ego-vehicle frame translated by 11 meters w.r.t. each other
         """
         R90 = Rot2.fromDegrees(90)
 
-        # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
-        # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+        # Create source poses (three objects o1, o2, o3 living in the ego-vehicle "e" frame)
+        # Suppose they are 3d cuboids detected by an onboard sensor in the ego-vehicle frame
         eTo0 = Pose2(Rot2(), np.array([1, 0]))
         eTo1 = Pose2(Rot2(), np.array([2, 0]))
         eTo2 = Pose2(Rot2(), np.array([4, 0]))
@@ -84,7 +85,7 @@ class TestSim2(GtsamTestCase):
 
         we_pairs = list(zip(wToi_list, eToi_list))
 
-        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        # Recover the transformation wSe (i.e. world_S_ego-vehicle)
         wSe = Similarity2.Align(we_pairs)
 
         for wToi, eToi in zip(wToi_list, eToi_list):
@@ -122,7 +123,7 @@ class TestSim2(GtsamTestCase):
 
         ab_pairs = list(zip(aTi_list, bTi_list))
 
-        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        # Recover the transformation wSe (i.e. world_S_ego-vehicle)
         aSb = Similarity2.Align(ab_pairs)
 
         for aTi, bTi in zip(aTi_list, bTi_list):
@@ -190,6 +191,89 @@ class TestSim2(GtsamTestCase):
         bSa = Similarity2(R=bRa, t=bta, s=bsa)
         self.assertEqual(bSa.scale(), 3.0)
 
+    def test_compose(self) -> None:
+        """Test group operation: compose two Similarity2 elements."""
+        R1 = Rot2.fromDegrees(30)
+        t1 = np.array([1, 2])
+        s1 = 2.0
+        S1 = Similarity2(R=R1, t=t1, s=s1)
+
+        R2 = Rot2.fromDegrees(45)
+        t2 = np.array([-1, 1])
+        s2 = 4.0
+        S2 = Similarity2(R=R2, t=t2, s=s2)
+
+        S3 = S1.compose(S2)
+
+        # Compose manually
+        expected_R = R1.compose(R2)
+        expected_s = s1 * s2
+        expected_t = t1 / s2 + R1.matrix() @ t2
+        expected_S3 = Similarity2(R=expected_R, t=expected_t, s=expected_s)
+
+        self.gtsamAssertEquals(S3, expected_S3)
+        self.gtsamAssertEquals(S1 * S2, expected_S3)
+        self.gtsamAssertEquals(S1.matrix() @ S2.matrix(), S3.matrix())
+
+    def test_inverse(self) -> None:
+        """Test group operation: inverse of a Similarity2 element."""
+        R = Rot2.fromDegrees(60)
+        t = np.array([3, -2])
+        s = 4.0
+        S = Similarity2(R=R, t=t, s=s)
+        S_inv = S.inverse()
+
+        # Check that S * S_inv is identity
+        I_sim = S.compose(S_inv)
+        expected_I = Similarity2()
+        self.gtsamAssertEquals(I_sim, expected_I)
+
+    def test_identity(self) -> None:
+        """Test that the identity Similarity2 acts as expected."""
+        S_id = Similarity2()
+        R = Rot2.fromDegrees(10)
+        t = np.array([5, 7])
+        s = 2.5
+        S = Similarity2(R=R, t=t, s=s)
+
+        # Compose with identity
+        self.gtsamAssertEquals(S.compose(S_id), S)
+        self.gtsamAssertEquals(S_id.compose(S), S)
+
+    def test_transform_from_point2(self):
+        """Test Similarity2.transformFrom with a Point2."""
+        R = Rot2.fromAngle(np.pi / 4)  # 45 degrees
+        t = np.array([1.0, 2.0])
+        s = 3.0
+        sim = Similarity2(R, t, s)
+
+        p = np.array([2.0, 0.0])
+
+        # Expected: s * (R * p + t)
+        expected = s * (R.matrix() @ p + t)
+        actual = sim.transformFrom(p)
+
+        np.testing.assert_allclose(expected, actual, atol=1e-9)
+
+    def test_transform_from_pose2(self):
+        """Test Similarity2.transformFrom with a Pose2."""
+        R_sim = Rot2.fromAngle(np.pi / 6)  # 30 degrees
+        t_sim = np.array([1.0, -1.0])
+        s_sim = 2.0
+        sim = Similarity2(R_sim, t_sim, s_sim)
+
+        R_pose = Rot2.fromAngle(-np.pi / 4)  # -45 degrees
+        t_pose = np.array([3.0, 4.0])
+        pose = Pose2(R_pose, t_pose)
+
+        expected_R = R_sim.compose(R_pose)
+        expected_t = s_sim * (R_sim.matrix() @ t_pose + t_sim)
+        expected = Pose2(expected_R, expected_t)
+
+        actual = sim.transformFrom(pose)
+
+        self.gtsamAssertEquals(actual, expected)
+
 
 if __name__ == "__main__":
     unittest.main()

gtsam/tests/test_Sim3.py

@@ -8,6 +8,7 @@ See LICENSE for the license information
 Sim3 unit tests.
 Author: John Lambert
 """
+
 # pylint: disable=no-name-in-module
 import unittest
 from typing import List, Optional
@@ -16,12 +17,88 @@ import numpy as np
 from gtsam.utils.test_case import GtsamTestCase
 
 import gtsam
-from gtsam import Point3, Pose3, Rot3, Similarity3, BetweenFactorSimilarity3, NonlinearFactorGraph, Values, LevenbergMarquardtOptimizer, LevenbergMarquardtParams
+from gtsam import (
+    BetweenFactorSimilarity3,
+    LevenbergMarquardtOptimizer,
+    LevenbergMarquardtParams,
+    NonlinearFactorGraph,
+    Point3,
+    Pose3,
+    Rot3,
+    Similarity3,
+    Values,
+)
 
 
 class TestSim3(GtsamTestCase):
     """Test selected Sim3 methods."""
 
+    # Class constants for reuse
+    R1 = Rot3.RzRyRx(0.1, 0.2, 0.3)
+    t1 = Point3(1, 2, 3)
+    s1 = 2.0
+    S1 = Similarity3(R1, t1, s1)
+
+    R2 = Rot3.RzRyRx(0.4, 0.5, 0.6)
+    t2 = Point3(-1, 1, 0)
+    s2 = 4.0
+    S2 = Similarity3(R2, t2, s2)
+
+    def test_compose(self) -> None:
+        """Test group operation: compose two Similarity3 elements."""
+        S3 = self.S1.compose(self.S2)
+
+        # Compose manually
+        expected_R = self.R1.compose(self.R2)
+        expected_s = self.s1 * self.s2
+        expected_t = self.t1 / self.s2 + self.R1.matrix() @ self.t2
+        expected_S3 = Similarity3(expected_R, expected_t, expected_s)
+
+        self.gtsamAssertEquals(S3, expected_S3)
+        self.gtsamAssertEquals(self.S1 * self.S2, expected_S3)
+        self.gtsamAssertEquals(self.S1.matrix() @ self.S2.matrix(), S3.matrix())
+
+    def test_inverse(self) -> None:
+        """Test group operation: inverse of a Similarity3 element."""
+        S_inv = self.S1.inverse()
+
+        # Check that S1 * S1_inv is identity
+        I_sim = self.S1.compose(S_inv)
+        expected_I = Similarity3()
+        self.gtsamAssertEquals(I_sim, expected_I)
+
+    def test_identity(self) -> None:
+        """Test that the identity Similarity3 acts as expected."""
+        S_id = Similarity3()
+
+        # Compose with identity
+        self.gtsamAssertEquals(self.S1.compose(S_id), self.S1)
+        self.gtsamAssertEquals(S_id.compose(self.S1), self.S1)
+
+    def test_transform_from_point3(self):
+        """Test Similarity3.transformFrom with a Point3."""
+        p = Point3(2.0, 0.0, 1.0)
+
+        # Expected: s * (R * p + t)
+        expected = self.s1 * (self.R1.matrix() @ p + self.t1)
+        actual = self.S1.transformFrom(p)
+
+        np.testing.assert_allclose(expected, actual, atol=1e-9)
+
+    def test_transform_from_pose3(self):
+        """Test Similarity3.transformFrom with a Pose3."""
+        R_pose = Rot3.RzRyRx(0.2, -0.1, 0.3)
+        t_pose = Point3(3.0, 4.0, 5.0)
+        pose = Pose3(R_pose, t_pose)
+
+        expected_R = self.R1.compose(R_pose)
+        expected_t = self.s1 * (self.R1.matrix() @ t_pose + self.t1)
+        expected = Pose3(expected_R, expected_t)
+
+        actual = self.S1.transformFrom(pose)
+
+        self.gtsamAssertEquals(actual, expected)
+
     def test_align_poses_along_straight_line(self):
         """Test Pose3 Align method.
 
@@ -130,7 +207,7 @@ class TestSim3(GtsamTestCase):
         for aTi, bTi in zip(aTi_list, bTi_list):
             self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
 
-    def test_sim3_optimization(self)->None:
+    def test_sim3_optimization(self) -> None:
         # Create a PriorFactor with a Sim3 prior
         prior = Similarity3(Rot3.Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4)
         model = gtsam.noiseModel.Isotropic.Sigma(7, 1)
@@ -154,14 +231,22 @@ class TestSim3(GtsamTestCase):
     def test_sim3_optimization2(self) -> None:
         prior = Similarity3()
         m1 = Similarity3(Rot3.Ypr(np.pi / 4.0, 0, 0), Point3(2.0, 0, 0), 1.0)
-        m2 = Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(np.sqrt(8) * 0.9, 0, 0), 1.0)
-        m3 = Similarity3(Rot3.Ypr(3 * np.pi / 4.0, 0, 0), Point3(np.sqrt(32) * 0.8, 0, 0), 1.0)
+        m2 = Similarity3(
+            Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(np.sqrt(8) * 0.9, 0, 0), 1.0
+        )
+        m3 = Similarity3(
+            Rot3.Ypr(3 * np.pi / 4.0, 0, 0), Point3(np.sqrt(32) * 0.8, 0, 0), 1.0
+        )
         m4 = Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(6 * 0.7, 0, 0), 1.0)
         loop = Similarity3(1.42)
 
         priorNoise = gtsam.noiseModel.Isotropic.Sigma(7, 0.01)
-        betweenNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10]))
-        betweenNoise2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([ 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0]))
+        betweenNoise = gtsam.noiseModel.Diagonal.Sigmas(
+            np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10])
+        )
+        betweenNoise2 = gtsam.noiseModel.Diagonal.Sigmas(
+            np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0])
+        )
         b1 = BetweenFactorSimilarity3(1, 2, m1, betweenNoise)
         b2 = BetweenFactorSimilarity3(2, 3, m2, betweenNoise)
         b3 = BetweenFactorSimilarity3(3, 4, m3, betweenNoise)
@@ -178,13 +263,20 @@ class TestSim3(GtsamTestCase):
         graph.add(lc)
 
         # graph.print("Full Graph\n");
-        initial=Values()
+        initial = Values()
         initial.insert(1, prior)
-        initial.insert(2, Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(1, 0, 0), 1.1))
-        initial.insert(3, Similarity3(Rot3.Ypr(2.0 * np.pi / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2))
-        initial.insert(4, Similarity3(Rot3.Ypr(3.0 * np.pi / 2.0, 0, 0), Point3(0, 1, 0), 1.3))
-        initial.insert(5, Similarity3(Rot3.Ypr(4.0 * np.pi / 2.0, 0, 0), Point3(0, 0, 0), 1.0))
-
+        initial.insert(
+            2, Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(1, 0, 0), 1.1)
+        )
+        initial.insert(
+            3, Similarity3(Rot3.Ypr(2.0 * np.pi / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2)
+        )
+        initial.insert(
+            4, Similarity3(Rot3.Ypr(3.0 * np.pi / 2.0, 0, 0), Point3(0, 1, 0), 1.3)
+        )
+        initial.insert(
+            5, Similarity3(Rot3.Ypr(4.0 * np.pi / 2.0, 0, 0), Point3(0, 0, 0), 1.0)
+        )
 
         result = LevenbergMarquardtOptimizer(graph, initial).optimizeSafely()
         self.gtsamAssertEquals(Similarity3(0.7), result.atSimilarity3(5), 0.4)
@@ -695,11 +787,15 @@ class TestSim3(GtsamTestCase):
             wTi = unaligned_pose_dict.get(i, None)
             unaligned_pose_list.append(wTi)
         # GT poses are the reference/target
-        rSe = align_poses_sim3_ignore_missing(aTi_list=poses_gt, bTi_list=unaligned_pose_list)
+        rSe = align_poses_sim3_ignore_missing(
+            aTi_list=poses_gt, bTi_list=unaligned_pose_list
+        )
         assert rSe.scale() >= 0
 
 
-def align_poses_sim3_ignore_missing(aTi_list: List[Optional[Pose3]], bTi_list: List[Optional[Pose3]]) -> Similarity3:
+def align_poses_sim3_ignore_missing(
+    aTi_list: List[Optional[Pose3]], bTi_list: List[Optional[Pose3]]
+) -> Similarity3:
     """Align by similarity transformation, but allow missing estimated poses in the input.
 
     Note: this is a wrapper for align_poses_sim3() that allows for missing poses/dropped cameras.
@@ -763,7 +859,9 @@ def align_poses_sim3(aTi_list: List[Pose3], bTi_list: List[Pose3]) -> Similarity
 
         # fit a single translation motion to the centroid
         aTi_centroid = np.array([aTi.translation() for aTi in aTi_list]).mean(axis=0)
-        aTi_rot_aligned_centroid = np.array([aTi.translation() for aTi in aTi_list_rot_aligned]).mean(axis=0)
+        aTi_rot_aligned_centroid = np.array(
+            [aTi.translation() for aTi in aTi_list_rot_aligned]
+        ).mean(axis=0)
 
         # construct the final SIM3 transform
         aSb = Similarity3(aSb.rotation(), aTi_centroid - aTi_rot_aligned_centroid, 1.0)

gtsam/utils/generate_trajectory.py

@@ -0,0 +1,100 @@
+"""
+Generates a CSV file with ground-truth trajectory data for testing a
+DiscreteScenario in GTSAM.
+
+The trajectory is a simple horizontal circle at a constant height and speed.
+
+CSV Format:
+timestamp,px,py,pz,qw,qx,qy,qz,vx,vy,vz,omegax,omegay,omegaz,ax,ay,az
+"""
+import numpy as np
+import csv
+from gtsam import Rot3
+
+# --- Trajectory Parameters ---
+RADIUS = 5.0  # meters
+HEIGHT = 1.0  # meters
+OMEGA_Z = 0.2  # rad/s (angular velocity around Z-axis)
+DURATION = 20.0  # seconds
+FREQUENCY = 100  # Hz
+FILENAME = "trajectory.csv"
+
+
+def generate_data():
+    """Generates and saves the trajectory data."""
+
+    # Time vector
+    t = np.arange(0.0, DURATION, 1.0 / FREQUENCY)
+
+    # Angle at each time step
+    angle = OMEGA_Z * t
+
+    # Position (p_n) in navigation frame
+    px = RADIUS * np.cos(angle)
+    py = RADIUS * np.sin(angle)
+    pz = np.full_like(t, HEIGHT)
+
+    # Velocity (v_n) in navigation frame (derivative of position)
+    speed = RADIUS * OMEGA_Z
+    vx = -speed * np.sin(angle)
+    vy = speed * np.cos(angle)
+    vz = np.zeros_like(t)
+
+    # Acceleration (a_n) in navigation frame (derivative of velocity)
+    # This is the centripetal acceleration.
+    ax = -speed * OMEGA_Z * np.cos(angle)
+    ay = -speed * OMEGA_Z * np.sin(angle)
+    az = np.zeros_like(t)
+
+    # Angular velocity (omega_b) in the body frame
+    # The body is only rotating around its z-axis to face forward.
+    omegax = np.zeros_like(t)
+    omegay = np.zeros_like(t)
+    omegaz = np.full_like(t, OMEGA_Z)
+
+    # Orientation (quaternion q_n_b)
+    # We use gtsam.Rot3 to easily get the quaternion
+    quaternions = [Rot3.Yaw(a).toQuaternion() for a in angle]
+
+    # --- Write to CSV ---
+    header = [
+        'timestamp',
+        'px',
+        'py',
+        'pz',  # Position
+        'qw',
+        'qx',
+        'qy',
+        'qz',  # Quaternion (orientation)
+        'vx',
+        'vy',
+        'vz',  # Velocity (nav frame)
+        'omegax',
+        'omegay',
+        'omegaz',  # Angular velocity (body frame)
+        'ax',
+        'ay',
+        'az'  # Acceleration (nav frame)
+    ]
+
+    with open(FILENAME, 'w', newline='') as f:
+        writer = csv.writer(f)
+        writer.writerow(header)
+
+        for i in range(len(t)):
+            q = quaternions[i]
+            row = [
+                t[i], px[i], py[i], pz[i],
+                q.w(),
+                q.x(),
+                q.y(),
+                q.z(), vx[i], vy[i], vz[i], omegax[i], omegay[i], omegaz[i],
+                ax[i], ay[i], az[i]
+            ]
+            writer.writerow(row)
+
+    print(f"Successfully generated trajectory data at '{FILENAME}'")
+
+
+if __name__ == "__main__":
+    generate_data()

{gtsam_develop-4.3a0.dev202506050756.dist-info → gtsam_develop-4.3a0.dev202507301459.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: gtsam-develop
-Version: 4.3a0.dev202506050756
+Version: 4.3a0.dev202507301459
 Summary: Georgia Tech Smoothing And Mapping library
 Home-page: https://gtsam.org/
 Author: Frank Dellaert et. al.