gtsam-develop 4.3a0.dev202506061330__cp311-cp311-macosx_11_0_arm64.whl → 4.3a0.dev202507301459__cp311-cp311-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_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_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.dev202506061330.dist-info → gtsam_develop-4.3a0.dev202507301459.dist-info}/METADATA

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

{gtsam_develop-4.3a0.dev202506061330.dist-info → gtsam_develop-4.3a0.dev202507301459.dist-info}/RECORD

@@ -1,15 +1,15 @@
 gtsam/symbol_shorthand.py,sha256=HlTV-Z5aB5cXWT5AsbKOeWZIHoXQsH2W1S90ET-tidA,236
-gtsam/__init__.pyi,sha256=Fm7njpQvdznX__VgFw9tTwAZtFjvgL2rg1kkKvbw6VE,43159
+gtsam/__init__.pyi,sha256=MR5Y0yGZZ0yRjPnhAnFPWQf-Lxq3wWsIwoxP50nAkyc,43980
 gtsam/gtsfm.py,sha256=Udlkb6o5iUk69uxBkb88-W1GLfu1g8iSuZlLu-RRU0o,202
 gtsam/__init__.py,sha256=6G-WPnb_FMQEJXNDDAmrKrQPau7evtd5svhTmMqhYSI,2011
-gtsam/gtsam.cpython-311-darwin.so,sha256=FLrMON_5qRX5HgorahBFL_jd7Gp4VhzaRjGVrwg_22s,17710192
+gtsam/gtsam.cpython-311-darwin.so,sha256=kPjC8aU9oWvA1wL6Nm-eZzowrZPogLX6M9VnedpK7yA,17888224
 gtsam/noiseModel.py,sha256=ybfIHa4sLXe78_k-Dib8nTaw34BRXqEDVtS2B9dzSO0,217
 gtsam/imuBias.py,sha256=tz3bOCFl29iMycbGFoF-ud6kRsJYjA7DJ2RJoKPFRU8,209
 gtsam/gtsam/utilities.pyi,sha256=-yn7_jeJbTa_3KJGgqcr_NDD70UhDZ8tYX2M2lPLmSQ,3384
 gtsam/gtsam/gtsfm.pyi,sha256=Onoh4CRaSYIQ0FID0vBkZeV7ESIhc2fh4rJGgaPY-ug,702
-gtsam/gtsam/__init__.pyi,sha256=jXBrjnWgYNOL6XPnXscjpsnIVXibwxHBM3IptxDxXrM,719848
+gtsam/gtsam/__init__.pyi,sha256=-lp1ISDcgTTO4De0WWvwrURMRPZr43ayLVonZtrcTbU,732561
 gtsam/gtsam/so3.pyi,sha256=UoDGISMgcStVTAtc_y6CmHXCtPJrJ7ju70z3ZH1OLR4,3939
-gtsam/gtsam/imuBias.pyi,sha256=gD6w5kd9xpO4uBEpfNym_3LjW67MV7HPQRYOOoDPysQ,2478
+gtsam/gtsam/imuBias.pyi,sha256=UqK0nLVICpnyw8B8_Q1GgX-qRd6atlCXT6OZNffZ1ao,2797
 gtsam/gtsam/symbol_shorthand.pyi,sha256=iaQLf_-hch_4YTYbSWTLINKtIL9Mg7W7HKOMuCXp8Ok,991
 gtsam/gtsam/lago.pyi,sha256=-VzJaQ1ho25n7S4VjbugWmPNUtKeMsoESifX9K8l2Yo,551
 gtsam/gtsam/noiseModel/__init__.pyi,sha256=hmp6DnyP7wwhjNN_Xoh2dX0E89yJ_YMBrlbjxu-1Kr0,8787
@@ -17,7 +17,7 @@ gtsam/gtsam/noiseModel/mEstimator.pyi,sha256=DCClE_67kH2L-6wsiIEExB3xzkwi-dprZM2
 gtsam/tests/simulation.py,sha256=VAopcCosvdtXRy6xe547EDz73lXhLiYbPnFQZWjo2wU,5405
 gtsam/tests/test_OdometryExample.py,sha256=aO-ciYNbKo8ru0bxexEtlj4xN9WGiSBlLOQnRkCCAx0,2094
 gtsam/tests/test_KarcherMeanFactor.py,sha256=L3_VAOljyRDmyOjOlZqnW-5apqufsDd8UnLIZ-d-_VE,2369
-gtsam/tests/test_FrobeniusFactor.py,sha256=egW1w1CzwlfGXijNB6_eYL2s9A0fSpTqtvXmqeL_jVo,1831
+gtsam/tests/test_FrobeniusFactor.py,sha256=azS0x0EDgj9N4Z4OYAMip94hTkvYo7AcjZvuLtctGMA,4420
 gtsam/tests/ImuFactorISAM2Example.py,sha256=G9HS3tHAp3plMfulFtsxE1mR467wSXlpoND8GjbqbyM,5724
 gtsam/tests/CombinedImuFactorExample.py,sha256=RLR2_q6IU-cAHiaVYBNPeT8Xg1COVHuXKrML6l480Vk,9455
 gtsam/tests/test_Factors.py,sha256=ehOfrP2xHSQiIDgtWcoxxg59MiQsHWPB_maaf-YEs7U,1427
@@ -36,7 +36,7 @@ gtsam/tests/test_KalmanFilter.py,sha256=OinaR5eDHP9kYzn-CDvtX2FiPdOW15EkGExGIrSz
 gtsam/tests/SimpleRotation.py,sha256=3ZAReijr08HTDwA_Hl48gBgcERvd1hjl9YUSVGQTwDQ,2862
 gtsam/tests/test_pickle.py,sha256=10MzCXyC6glUuFfYOlUufvVhEOYTRyJOTaTrnYG2n1U,1526
 gtsam/tests/ViewGraphComparison.py,sha256=A2mDOHKHOKMUqUo8OR0Sv8yNjDg3wum7T6vUSeGnCvw,15837
-gtsam/tests/test_Sim2.py,sha256=7wWyW0Cvyt5r8GFWPPoWAzZ7iaepI1rPTUlxLCso1EE,6941
+gtsam/tests/test_Sim2.py,sha256=qU3nxd7fJDHS4dnTENVX-b5rukV33q649WBfQ-5q6Po,9630
 gtsam/tests/test_Robust.py,sha256=_PlWYU0geYoYTQXXKNz4jsHnjZbGyN7ZkJnymAAV9Cs,1221
 gtsam/tests/Pose3SLAMExample_g2o.py,sha256=iei2UD8L7o9XkSOLmHy8cAI3iPcVbYBPO-zraSbq_Cc,2555
 gtsam/tests/Pose3ISAM2Example.py,sha256=pTSARr97SpU9ooKTK8qgBWIdCGtcoajje25V67BiyyE,9664
@@ -55,7 +55,7 @@ gtsam/tests/Pose2SLAMExample_g2o.py,sha256=H6fQLRonBpcmwIS8SbD8DSqv4M62LC2LwlUp1
 gtsam/tests/test_PriorFactor.py,sha256=ksMSqAX5qLcyLYOuwixWetZZLYkAXPA3z9uOcY-ySyw,1692
 gtsam/tests/test_lago.py,sha256=c5QBnmxYzTiZ2-B7Uk4pyiUEU4LvQsaDrejsVxhes2I,2507
 gtsam/tests/Pose3SLAMExample_initializePose3Chordal.py,sha256=0SgX7ClrRzABbCqtNN3ZqaCR92p_uOtGoUJziwsYfOQ,1071
-gtsam/tests/test_Sim3.py,sha256=N-QnVc_PHVp292lsnNWgiA0NJvIYVLYN94Z74WxG1gA,29949
+gtsam/tests/test_Sim3.py,sha256=6Ku0Ij_VsU5u7pUZVZCBDN4uf3-V8jTPbJM9EHNLjEM,32486
 gtsam/tests/test_Pose3SLAMExample.py,sha256=aKR4LiKfjGg2eypgreQeFr40zsq3ScCDGfpdGs6j-lc,2168
 gtsam/tests/test_custom_factor.py,sha256=J9Bu3SSGaEyyO0xHmyO7l1wWsw9olEon0lAjic7WCaU,7105
 gtsam/tests/test_NonlinearOptimizer.py,sha256=-2bnIvl2L_KrGUQICkiCvyAyOSjvKMrIG0jkrKmZiiY,7021
@@ -127,19 +127,20 @@ gtsam/utils/__init__.pyi,sha256=_LsGvjUEKTIpqYuifhzC8uGskeHctfcwq6GYKj6oR08,245
 gtsam/utils/__init__.py,sha256=_ID7pb13SDZedga5KdBqpPug4PW3eU3THdVF_3jrakQ,678
 gtsam/utils/test_case.py,sha256=3wIqAW5_smes95XUJgUjD4v3FXACYSVzQ1l6MMdwSkA,1848
 gtsam/utils/logging_optimizer.py,sha256=tRnzQKV4eT5djS0Ojy5J7OGu7oVRjZw7Jhjrx_VQVTU,4417
+gtsam/utils/generate_trajectory.py,sha256=_1TZWhpRKj8ha0FOEmqUpEzAt3KLgwqyZJ4ueFD2jmE,2663
+gtsam/.dylibs/libgtsam.4.3a0.dev202507301459.dylib,sha256=0d5gQ3xdnfzfgZupSCTOclN9Fmpi3XVSTTggGXc4xrA,5838048
 gtsam/.dylibs/libboost_thread.dylib,sha256=aT1DCt1Kvw1A2w2mRHIJhZRFrYmoDaI5xYM6wEt86L8,137200
-gtsam/.dylibs/libgtsam.4.3a0.dev202506061330.dylib,sha256=yCY8rM8vF85B49fz-RYpI4St_0zc-xHgpygrmM7K0qw,5666736
 gtsam/.dylibs/libboost_regex.dylib,sha256=XWw3H3ehJjFuFVSSDGo2lyKOIsoTQ-0Aaf1ekhsQJfY,356464
 gtsam/.dylibs/libboost_serialization.dylib,sha256=7OW78djID14u2YB_F_TuXomOIsEpt8I7RnCAGuGmwCc,418288
 gtsam/.dylibs/libboost_timer.dylib,sha256=leVXIyCdydip4vwIm-Ghrt6UGy_Q-yhL2f8ITyfRku0,81520
 gtsam/.dylibs/libboost_filesystem.dylib,sha256=WUuuO1JZADriLciFi63Ky4dmqt1KA7_V6DcNdzCANSo,197328
 gtsam/.dylibs/libcephes-gtsam.1.0.0.dylib,sha256=pNZtgCIaV7_dC0WPXobOc8sNvd_0WQo1AdYxUsqZ1D0,176352
-gtsam/.dylibs/libgtsam_unstable.4.3a0.dev202506061330.dylib,sha256=PjLTnvaKxF0VILoMxGmvK6YffPMIMQZOo6djA4rXLkI,1623104
 gtsam/.dylibs/libmetis-gtsam.dylib,sha256=Od5V_NOZKhxu5cEw1wMfGxk3l0_2LEhV-GkEwnqyx4Q,449504
 gtsam/.dylibs/libboost_atomic.dylib,sha256=AbG9FmLd7biQVvDL9F7_sLOEgmhgj5bwxX98c5jnVVc,104160
 gtsam/.dylibs/libboost_date_time.dylib,sha256=zkeDyIl7aGu1K5avCbDjtJURdIPT8llHnNZ-G7N-_Cc,35008
 gtsam/.dylibs/libboost_system.dylib,sha256=W2i7A0l2E-Kkxdzs6E4Fr0S3hXTfBJOJ0WW32M3Fqh8,34992
 gtsam/.dylibs/libboost_chrono.dylib,sha256=WTFx4Mqt77f3jd9cE11WxBAABvcmj23DbMxXwXkTz2o,79824
+gtsam/.dylibs/libgtsam_unstable.4.3a0.dev202507301459.dylib,sha256=WaeY3IZj8b-iyzURRumUHNwaS7qr8UJRXwU5lPv6TKk,1623248
 gtsam/examples/simulation.py,sha256=VAopcCosvdtXRy6xe547EDz73lXhLiYbPnFQZWjo2wU,5405
 gtsam/examples/ImuFactorISAM2Example.py,sha256=G9HS3tHAp3plMfulFtsxE1mR467wSXlpoND8GjbqbyM,5724
 gtsam/examples/CombinedImuFactorExample.py,sha256=RLR2_q6IU-cAHiaVYBNPeT8Xg1COVHuXKrML6l480Vk,9455
@@ -249,11 +250,11 @@ gtsam/Data/Balbianello/BalbianelloMedium-4.key.gz,sha256=P6tu3JmA0NKlcFYdgYczTOH
 gtsam/Data/Balbianello/BalbianelloMedium-1.key.gz,sha256=RgT7tVXXOwvDug20TW-9xtoeO3dSSPMF1n9DaKxdrIw,332878
 gtsam/Data/Balbianello/BalbianelloMedium-3.key.gz,sha256=yNMcyqwZCOj9FG-6qXQ9xhJjpM135cTBJYlOUBG0rnQ,296236
 gtsam/Data/Balbianello/BalbianelloMedium-5.key.gz,sha256=4veDrxRdLH8k1DIhgj1984MitZ7nAWoXDWTWyXuu7Lg,241361
-gtsam_develop-4.3a0.dev202506061330.dist-info/RECORD,,
-gtsam_develop-4.3a0.dev202506061330.dist-info/WHEEL,sha256=sunMa2yiYbrNLGeMVDqEA0ayyJbHlex7SCn1TZrEq60,136
-gtsam_develop-4.3a0.dev202506061330.dist-info/top_level.txt,sha256=DOnqfd8DN2HpG5-V5t32TjFOB_vcYuyOWyRsgeoANEo,30
-gtsam_develop-4.3a0.dev202506061330.dist-info/METADATA,sha256=CZiVg2nPAQ4dfY0_DPsYTDXG0RzJJQO5kt8GQzJCfF4,7767
-gtsam_unstable/gtsam_unstable.cpython-311-darwin.so,sha256=ZSCSJX8PZLT4JTmCENFQNYD-e0lGC2IzFAHgqmm-_YI,2033424
+gtsam_develop-4.3a0.dev202507301459.dist-info/RECORD,,
+gtsam_develop-4.3a0.dev202507301459.dist-info/WHEEL,sha256=sunMa2yiYbrNLGeMVDqEA0ayyJbHlex7SCn1TZrEq60,136
+gtsam_develop-4.3a0.dev202507301459.dist-info/top_level.txt,sha256=DOnqfd8DN2HpG5-V5t32TjFOB_vcYuyOWyRsgeoANEo,30
+gtsam_develop-4.3a0.dev202507301459.dist-info/METADATA,sha256=19eJ7Qll-3d6D6ZKYngPIl2VdkUEVPhwMvoHvbcIPnQ,7767
+gtsam_unstable/gtsam_unstable.cpython-311-darwin.so,sha256=emI30psGx1-ShJXRiP-Jvdka-JLJ-Ye_an5BzHu2McY,2033440
 gtsam_unstable/__init__.py,sha256=FPc_oO5PFQZbrfpgugzQuI6LJfP1fzq82UQf_nuyGtk,30
 gtsam_unstable/tests/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
 gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py,sha256=t2l62uWoXfjrM8oH6ogV7M20WjTYKZ4CSferdurMIY0,2156