diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index 246ec19ee..3e4c58ccb 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -11,11 +11,12 @@ Refactored: Roderick Koehle """ import unittest -import gtsam import numpy as np from gtsam.symbol_shorthand import K, L, P from gtsam.utils.test_case import GtsamTestCase +import gtsam + def ulp(ftype=np.float64): """ @@ -26,7 +27,7 @@ def ulp(ftype=np.float64): class TestCal3Fisheye(GtsamTestCase): - + @classmethod def setUpClass(cls): """ @@ -50,10 +51,11 @@ class TestCal3Fisheye(GtsamTestCase): camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) cls.origin = np.array([0.0, 0.0, 0.0]) - cls.poses = [pose1, pose2] - cls.cameras = [camera1, camera2] - cls.measurements = [k.project(cls.origin) for k in cls.cameras] - + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) + cls.measurements = gtsam.Point2Vector( + [k.project(cls.origin) for k in cls.cameras]) + def test_Cal3Fisheye(self): K = gtsam.Cal3Fisheye() self.assertEqual(K.fx(), 1.) @@ -62,7 +64,7 @@ class TestCal3Fisheye(GtsamTestCase): def test_distortion(self): """Fisheye distortion and rectification""" equidistant = gtsam.Cal3Fisheye() - perspective_pt = self.obj_point[0:2]/self.obj_point[2] + perspective_pt = self.obj_point[0:2] / self.obj_point[2] distorted_pt = equidistant.uncalibrate(perspective_pt) rectified_pt = equidistant.calibrate(distorted_pt) self.gtsamAssertEquals(distorted_pt, self.img_point) @@ -166,7 +168,7 @@ class TestCal3Fisheye(GtsamTestCase): pose = gtsam.Pose3() camera = gtsam.Cal3Fisheye() state = gtsam.Values() - camera_key, pose_key, landmark_key = K(0), P(0), L(0) + pose_key, landmark_key = P(0), L(0) state.insert_point3(landmark_key, obj_point) state.insert_pose3(pose_key, pose) g = gtsam.NonlinearFactorGraph() diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index 32e7cea9d..d0cb1a683 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -10,11 +10,12 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python) """ import unittest -import gtsam import numpy as np from gtsam.symbol_shorthand import K, L, P from gtsam.utils.test_case import GtsamTestCase +import gtsam + class TestCal3Unified(GtsamTestCase): @@ -47,9 +48,10 @@ class TestCal3Unified(GtsamTestCase): camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic) camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) cls.origin = np.array([0.0, 0.0, 0.0]) - cls.poses = [pose1, pose2] - cls.cameras = [camera1, camera2] - cls.measurements = [k.project(cls.origin) for k in cls.cameras] + cls.poses = gtsam.Pose3Vector([pose1, pose2]) + cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) + cls.measurements = gtsam.Point2Vector( + [k.project(cls.origin) for k in cls.cameras]) def test_Cal3Unified(self): K = gtsam.Cal3Unified() @@ -106,7 +108,7 @@ class TestCal3Unified(GtsamTestCase): state = gtsam.Values() measured = self.img_point noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) - camera_key, pose_key, landmark_key = K(0), P(0), L(0) + 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()) @@ -141,7 +143,7 @@ class TestCal3Unified(GtsamTestCase): Dcal = np.zeros((2, 10), order='F') Dp = np.zeros((2, 2), order='F') camera.calibrate(img_point, Dcal, Dp) - + self.gtsamAssertEquals(Dcal, np.array( [[ 0., 0., 0., -1., 0., 0., 0., 0., 0., 0.], [ 0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]])) @@ -157,7 +159,7 @@ class TestCal3Unified(GtsamTestCase): def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - rectified = [k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)] + rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(triangulated, self.origin) diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index be6aa0796..681b39c6d 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -23,14 +23,14 @@ class TestDsfTrackGenerator(GtsamTestCase): kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]])) kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]])) - keypoints_list = [] + keypoints_list = gtsam.KeypointsVector() keypoints_list.append(kps_i0) keypoints_list.append(kps_i1) keypoints_list.append(kps_i2) # For each image pair (i1,i2), we provide a (K,2) matrix # of corresponding image indices (k1,k2). - matches_dict = {} + matches_dict = gtsam.MatchIndicesMap() matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) @@ -86,7 +86,7 @@ class TestSfmTrack2d(GtsamTestCase): def test_sfm_track_2d_constructor(self) -> None: """Test construction of 2D SfM track.""" - measurements = [] + measurements = gtsam.SfmMeasurementVector() measurements.append((0, Point2(10, 20))) track = SfmTrack2d(measurements=measurements) track.measurement(0) diff --git a/python/gtsam/tests/test_FixedLagSmootherExample.py b/python/gtsam/tests/test_FixedLagSmootherExample.py index 412abe987..05256f358 100644 --- a/python/gtsam/tests/test_FixedLagSmootherExample.py +++ b/python/gtsam/tests/test_FixedLagSmootherExample.py @@ -37,7 +37,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = {} + new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) @@ -48,7 +48,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): gtsam.PriorFactorPose2( X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) - new_timestamps[X1] = 0.0 + new_timestamps.insert((X1, 0.0)) delta_time = 0.25 time = 0.25 @@ -78,7 +78,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): current_key = int(1000 * time) # assign current key to the current timestamp - new_timestamps[current_key] = time + new_timestamps.insert((current_key, time)) # Add a guess for this pose to the new values # Assume that the robot moves at 2 m/s. Position is time[s] * diff --git a/python/gtsam/tests/test_GaussianFactorGraph.py b/python/gtsam/tests/test_GaussianFactorGraph.py index d96f28747..4d88b4214 100644 --- a/python/gtsam/tests/test_GaussianFactorGraph.py +++ b/python/gtsam/tests/test_GaussianFactorGraph.py @@ -14,11 +14,12 @@ from __future__ import print_function import unittest -import gtsam import numpy as np from gtsam.symbol_shorthand import X from gtsam.utils.test_case import GtsamTestCase +import gtsam + def create_graph(): """Create a basic linear factor graph for testing""" @@ -98,7 +99,7 @@ class TestGaussianFactorGraph(GtsamTestCase): bn = gfg.eliminateSequential(ordering) self.assertEqual(bn.size(), 3) - keyVector = [] + keyVector = gtsam.KeyVector() keyVector.append(keys[2]) #TODO(Varun) Below code causes segfault in Debug config # ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector) diff --git a/python/gtsam/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py index 9ec33ad8a..4c6f8db78 100644 --- a/python/gtsam/tests/test_KarcherMeanFactor.py +++ b/python/gtsam/tests/test_KarcherMeanFactor.py @@ -13,11 +13,12 @@ Author: Frank Dellaert import unittest -import gtsam import numpy as np -from gtsam import Rot3 from gtsam.utils.test_case import GtsamTestCase +import gtsam +from gtsam import Rot3 + KEY = 0 MODEL = gtsam.noiseModel.Unit.Create(3) @@ -29,9 +30,11 @@ R = Rot3.Expmap(np.array([0.1, 0, 0])) class TestKarcherMean(GtsamTestCase): def test_find(self): - # Check that optimizing for Karcher mean (which minimizes Between distance) - # gets correct result. - rotations = [R, R.inverse()] + """ + Check that optimizing for Karcher mean (which minimizes Between distance) + gets correct result. + """ + rotations = gtsam.Rot3Vector([R, R.inverse()]) expected = Rot3() actual = gtsam.FindKarcherMean(rotations) self.gtsamAssertEquals(expected, actual) @@ -42,7 +45,7 @@ class TestKarcherMean(GtsamTestCase): a2Rb2 = Rot3() a3Rb3 = Rot3() - aRb_list = [a1Rb1, a2Rb2, a3Rb3] + aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3]) aRb_expected = Rot3() aRb = gtsam.FindKarcherMean(aRb_list) @@ -58,7 +61,9 @@ class TestKarcherMean(GtsamTestCase): graph = gtsam.NonlinearFactorGraph() R12 = R.compose(R.compose(R)) graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL)) - keys = [1, 2] + keys = gtsam.KeyVector() + keys.append(1) + keys.append(2) graph.add(gtsam.KarcherMeanFactorRot3(keys)) initial = gtsam.Values() @@ -67,7 +72,9 @@ class TestKarcherMean(GtsamTestCase): expected = Rot3() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)]) + actual = gtsam.FindKarcherMean( + gtsam.Rot3Vector([result.atRot3(1), + result.atRot3(2)])) self.gtsamAssertEquals(expected, actual) self.gtsamAssertEquals( R12, result.atRot3(1).between(result.atRot3(2))) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index f364f55e3..69eb02b62 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -12,9 +12,10 @@ import math import unittest import numpy as np -from gtsam import Point2, Pose2 from gtsam.utils.test_case import GtsamTestCase +from gtsam import Point2, Point2Pairs, Pose2 + class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" @@ -82,7 +83,7 @@ class TestPose2(GtsamTestCase): ] # fmt: on - ab_pairs = list(zip(pts_a, pts_b)) + ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) aTb = Pose2.Align(ab_pairs) self.assertIsNotNone(aTb) diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index 40bf9a87f..5d1761b08 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -12,11 +12,12 @@ Author: Frank Dellaert, Duy Nguyen Ta import math import unittest -import gtsam import numpy as np -from gtsam import Point3, Pose3, Rot3 from gtsam.utils.test_case import GtsamTestCase +import gtsam +from gtsam import Point3, Pose3, Rot3 + def numerical_derivative_pose(pose, method, delta=1e-5): jacobian = np.zeros((6, 6)) @@ -222,7 +223,7 @@ class TestPose3(GtsamTestCase): sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0)) transformed = sTt.transformTo(square) - st_pairs = [] + st_pairs = gtsam.Point3Pairs() for j in range(4): st_pairs.append((square[:,j], transformed[:,j])) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 036ea401c..c9709ecf9 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -12,12 +12,13 @@ Author: Frank Dellaert import unittest -import gtsam import numpy as np +from gtsam.utils.test_case import GtsamTestCase + +import gtsam from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2, ShonanAveraging2, ShonanAveraging3, ShonanAveragingParameters2, ShonanAveragingParameters3) -from gtsam.utils.test_case import GtsamTestCase DEFAULT_PARAMS = ShonanAveragingParameters3( gtsam.LevenbergMarquardtParams.CeresDefaults() @@ -176,7 +177,7 @@ class TestShonanAveraging(GtsamTestCase): shonan_params.setCertifyOptimality(True) noise_model = gtsam.noiseModel.Unit.Create(3) - between_factors = [] + between_factors = gtsam.BetweenFactorPose2s() for (i1, i2), i2Ri1 in i2Ri1_dict.items(): i2Ti1 = Pose2(i2Ri1, np.zeros(2)) between_factors.append( diff --git a/python/gtsam/tests/test_Sim2.py b/python/gtsam/tests/test_Sim2.py index 96ba6eb1e..50a8cf156 100644 --- a/python/gtsam/tests/test_Sim2.py +++ b/python/gtsam/tests/test_Sim2.py @@ -12,9 +12,10 @@ Author: John Lambert import unittest import numpy as np -from gtsam import Pose2, Rot2, Similarity2 from gtsam.utils.test_case import GtsamTestCase +from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2 + class TestSim2(GtsamTestCase): """Test selected Sim2 methods.""" @@ -46,7 +47,7 @@ class TestSim2(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - we_pairs = list(zip(wToi_list, eToi_list)) + we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity2.Align(we_pairs) @@ -81,7 +82,7 @@ class TestSim2(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - we_pairs = list(zip(wToi_list, eToi_list)) + we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity2.Align(we_pairs) @@ -119,7 +120,7 @@ class TestSim2(GtsamTestCase): bTi_list = [bTi0, bTi1, bTi2, bTi3] - ab_pairs = list(zip(aTi_list, bTi_list)) + ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list))) # Recover the transformation wSe (i.e. world_S_egovehicle) aSb = Similarity2.Align(ab_pairs) diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index c3fd9e909..b5ae25221 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -12,11 +12,12 @@ Author: John Lambert import unittest from typing import List, Optional -import gtsam import numpy as np -from gtsam import Point3, Pose3, Rot3, Similarity3 from gtsam.utils.test_case import GtsamTestCase +import gtsam +from gtsam import Point3, Pose3, Rot3, Similarity3 + class TestSim3(GtsamTestCase): """Test selected Sim3 methods.""" @@ -48,7 +49,7 @@ class TestSim3(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - we_pairs = list(zip(wToi_list, eToi_list)) + we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list))) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity3.Align(we_pairs) @@ -83,7 +84,7 @@ class TestSim3(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - we_pairs = list(zip(wToi_list, eToi_list)) + we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list))) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity3.Align(we_pairs) @@ -121,7 +122,7 @@ class TestSim3(GtsamTestCase): bTi_list = [bTi0, bTi1, bTi2, bTi3] - ab_pairs = list(zip(aTi_list, bTi_list)) + ab_pairs = gtsam.Pose3Pairs(list(zip(aTi_list, bTi_list))) # Recover the transformation wSe (i.e. world_S_egovehicle) aSb = Similarity3.Align(ab_pairs) @@ -688,7 +689,7 @@ def align_poses_sim3(aTi_list: List[Pose3], bTi_list: List[Pose3]) -> Similarity assert len(aTi_list) == len(bTi_list) assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames" - ab_pairs = list(zip(aTi_list, bTi_list)) + ab_pairs = gtsam.Pose3Pairs(list(zip(aTi_list, bTi_list))) aSb = Similarity3.Align(ab_pairs) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 25d293e6f..703392d67 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -12,13 +12,14 @@ Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert import unittest from typing import Iterable, List, Optional, Tuple, Union -import gtsam import numpy as np +from gtsam.utils.test_case import GtsamTestCase + +import gtsam from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, CameraSetCal3Bundler, PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3, TriangulationParameters, TriangulationResult) -from gtsam.utils.test_case import GtsamTestCase UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2) @@ -34,7 +35,9 @@ class TestTriangulationExample(GtsamTestCase): # create second camera 1 meter to the right of first camera pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) # twoPoses - self.poses = [pose1, pose2] + self.poses = gtsam.Pose3Vector() + self.poses.append(pose1) + self.poses.append(pose2) # landmark ~5 meters infront of camera self.landmark = Point3(5, 0.5, 1.2) @@ -64,7 +67,7 @@ class TestTriangulationExample(GtsamTestCase): cameras = camera_set() else: cameras = [] - measurements = [] + measurements = gtsam.Point2Vector() for k, pose in zip(cal_params, self.poses): K = calibration(*k) @@ -93,7 +96,7 @@ class TestTriangulationExample(GtsamTestCase): self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) - measurements_noisy = [] + measurements_noisy = gtsam.Point2Vector() measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) @@ -160,8 +163,8 @@ class TestTriangulationExample(GtsamTestCase): z2: Point2 = camera2.project(landmark) z3: Point2 = camera3.project(landmark) - poses = [pose1, pose2, pose3] - measurements = [z1, z2, z3] + poses = gtsam.Pose3Vector([pose1, pose2, pose3]) + measurements = gtsam.Point2Vector([z1, z2, z3]) # noise free, so should give exactly the landmark actual = gtsam.triangulatePoint3(poses, @@ -226,7 +229,7 @@ class TestTriangulationExample(GtsamTestCase): cameras.append(camera1) cameras.append(camera2) - measurements = [] + measurements = gtsam.Point2Vector() measurements.append(z1) measurements.append(z2)