12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

modernize main tests

release/4.3a0
Varun Agrawal 2023-06-21 16:02:18 -04:00
parent 1f951f7dfe
commit 6f94f52f41
12 changed files with 51 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
python/gtsam/tests/test_Cal3Fisheye.py
View File

@ -51,10 +51,9 @@ class TestCal3Fisheye(GtsamTestCase):
camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
cls.origin = np.array([0.0, 0.0, 0.0])
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])
cls.poses = [pose1, pose2]
cls.cameras = [camera1, camera2]
cls.measurements = [k.project(cls.origin) for k in cls.cameras]
def test_Cal3Fisheye(self):
K = gtsam.Cal3Fisheye()

9
python/gtsam/tests/test_Cal3Unified.py
View File

@ -48,10 +48,9 @@ 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 = gtsam.Pose3Vector([pose1, pose2])
cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2])
cls.measurements = gtsam.Point2Vector(
[k.project(cls.origin) for k in cls.cameras])
cls.poses = [pose1, pose2]
cls.cameras = [camera1, camera2]
cls.measurements = [k.project(cls.origin) for k in cls.cameras]
def test_Cal3Unified(self):
K = gtsam.Cal3Unified()
@ -159,7 +158,7 @@ class TestCal3Unified(GtsamTestCase):
def test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification"""
rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)])
rectified = [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)

6
python/gtsam/tests/test_DsfTrackGenerator.py
View File

@ -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 = gtsam.KeypointsVector()
keypoints_list = []
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 = gtsam.MatchIndicesMap()
matches_dict = {}
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 = gtsam.SfmMeasurementVector()
measurements = []
measurements.append((0, Point2(10, 20)))
track = SfmTrack2d(measurements=measurements)
track.measurement(0)

7
python/gtsam/tests/test_FixedLagSmootherExample.py
View File

@ -14,7 +14,6 @@ import numpy as np
from gtsam.utils.test_case import GtsamTestCase
import gtsam
import gtsam_unstable
class TestFixedLagSmootherExample(GtsamTestCase):
@ -37,7 +36,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
# that will be sent to the smoothers
new_factors = gtsam.NonlinearFactorGraph()
new_values = gtsam.Values()
new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap()
new_timestamps = {}
# Create a prior on the first pose, placing it at the origin
prior_mean = gtsam.Pose2(0, 0, 0)
@ -48,7 +47,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
gtsam.PriorFactorPose2(
X1, prior_mean, prior_noise))
new_values.insert(X1, prior_mean)
new_timestamps.insert((X1, 0.0))
new_timestamps[X1] = 0.0
delta_time = 0.25
time = 0.25
@ -78,7 +77,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
current_key = int(1000 * time)
# assign current key to the current timestamp
new_timestamps.insert((current_key, time))
new_timestamps[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] *

12
python/gtsam/tests/test_GaussianFactorGraph.py
View File

@ -41,6 +41,7 @@ def create_graph():
class TestGaussianFactorGraph(GtsamTestCase):
"""Tests for Gaussian Factor Graphs."""
def test_fg(self):
"""Test solving a linear factor graph"""
graph, X = create_graph()
@ -99,12 +100,11 @@ class TestGaussianFactorGraph(GtsamTestCase):
bn = gfg.eliminateSequential(ordering)
self.assertEqual(bn.size(), 3)
keyVector = gtsam.KeyVector()
keyVector.append(keys[2])
#TODO(Varun) Below code causes segfault in Debug config
# ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector)
# bn = gfg.eliminateSequential(ordering)
# self.assertEqual(bn.size(), 3)
keyVector = [keys[2]]
ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(
gfg, keyVector)
bn = gfg.eliminateSequential(ordering)
self.assertEqual(bn.size(), 3)
if __name__ == '__main__':

16
python/gtsam/tests/test_KarcherMeanFactor.py
View File

@ -22,7 +22,6 @@ from gtsam import Rot3
KEY = 0
MODEL = gtsam.noiseModel.Unit.Create(3)
# Rot3 version
R = Rot3.Expmap(np.array([0.1, 0, 0]))
@ -34,7 +33,7 @@ class TestKarcherMean(GtsamTestCase):
Check that optimizing for Karcher mean (which minimizes Between distance)
gets correct result.
"""
rotations = gtsam.Rot3Vector([R, R.inverse()])
rotations = [R, R.inverse()]
expected = Rot3()
actual = gtsam.FindKarcherMean(rotations)
self.gtsamAssertEquals(expected, actual)
@ -45,7 +44,7 @@ class TestKarcherMean(GtsamTestCase):
a2Rb2 = Rot3()
a3Rb3 = Rot3()
aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3])
aRb_list = [a1Rb1, a2Rb2, a3Rb3]
aRb_expected = Rot3()
aRb = gtsam.FindKarcherMean(aRb_list)
@ -61,9 +60,7 @@ class TestKarcherMean(GtsamTestCase):
graph = gtsam.NonlinearFactorGraph()
R12 = R.compose(R.compose(R))
graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
keys = gtsam.KeyVector()
keys.append(1)
keys.append(2)
keys = [1, 2]
graph.add(gtsam.KarcherMeanFactorRot3(keys))
initial = gtsam.Values()
@ -72,12 +69,9 @@ class TestKarcherMean(GtsamTestCase):
expected = Rot3()
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
actual = gtsam.FindKarcherMean(
gtsam.Rot3Vector([result.atRot3(1),
result.atRot3(2)]))
actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)])
self.gtsamAssertEquals(expected, actual)
self.gtsamAssertEquals(
R12, result.atRot3(1).between(result.atRot3(2)))
self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2)))
if __name__ == "__main__":

7
python/gtsam/tests/test_Pose2.py
View File

@ -19,6 +19,7 @@ from gtsam import Point2, Point2Pairs, Pose2
class TestPose2(GtsamTestCase):
"""Test selected Pose2 methods."""
def test_adjoint(self) -> None:
"""Test adjoint method."""
xi = np.array([1, 2, 3])
@ -28,7 +29,7 @@ class TestPose2(GtsamTestCase):
def test_transformTo(self):
"""Test transformTo method."""
pose = Pose2(2, 4, -math.pi/2)
pose = Pose2(2, 4, -math.pi / 2)
actual = pose.transformTo(Point2(3, 2))
expected = Point2(2, 1)
self.gtsamAssertEquals(actual, expected, 1e-6)
@ -42,7 +43,7 @@ class TestPose2(GtsamTestCase):
def test_transformFrom(self):
"""Test transformFrom method."""
pose = Pose2(2, 4, -math.pi/2)
pose = Pose2(2, 4, -math.pi / 2)
actual = pose.transformFrom(Point2(2, 1))
expected = Point2(3, 2)
self.gtsamAssertEquals(actual, expected, 1e-6)
@ -83,7 +84,7 @@ class TestPose2(GtsamTestCase):
]
# fmt: on
ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
ab_pairs = list(zip(pts_a, pts_b))
aTb = Pose2.Align(ab_pairs)
self.assertIsNotNone(aTb)

2
python/gtsam/tests/test_Pose3.py
View File

@ -223,7 +223,7 @@ class TestPose3(GtsamTestCase):
sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0))
transformed = sTt.transformTo(square)
st_pairs = gtsam.Point3Pairs()
st_pairs = []
for j in range(4):
st_pairs.append((square[:,j], transformed[:,j]))

7
python/gtsam/tests/test_ShonanAveraging.py
View File

@ -140,7 +140,6 @@ class TestShonanAveraging(GtsamTestCase):
self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3)
result, _lambdaMin = shonan.run(initial, 3, 3)
self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
def test_constructorBetweenFactorPose2s(self) -> None:
"""Check if ShonanAveraging2 constructor works when not initialized from g2o file.
@ -177,7 +176,7 @@ class TestShonanAveraging(GtsamTestCase):
shonan_params.setCertifyOptimality(True)
noise_model = gtsam.noiseModel.Unit.Create(3)
between_factors = gtsam.BetweenFactorPose2s()
between_factors = []
for (i1, i2), i2Ri1 in i2Ri1_dict.items():
i2Ti1 = Pose2(i2Ri1, np.zeros(2))
between_factors.append(
@ -190,11 +189,11 @@ class TestShonanAveraging(GtsamTestCase):
wRi_list = [result_values.atRot2(i) for i in range(num_images)]
thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
# map all angles to [0,360)
thetas_deg = thetas_deg % 360
thetas_deg -= thetas_deg[0]
expected_thetas_deg = np.array([0.0, 90.0, 0.0])
np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)

12
python/gtsam/tests/test_Sim2.py
View File

@ -14,7 +14,7 @@ import unittest
import numpy as np
from gtsam.utils.test_case import GtsamTestCase
from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2
from gtsam import Pose2, Rot2, Similarity2
class TestSim2(GtsamTestCase):
@ -47,7 +47,7 @@ class TestSim2(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
we_pairs = list(zip(wToi_list, eToi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity2.Align(we_pairs)
@ -56,7 +56,7 @@ class TestSim2(GtsamTestCase):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_along_straight_line_gauge(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
"""Test if Pose2 Align method can account for gauge ambiguity.
Scenario:
3 object poses
@ -82,7 +82,7 @@ class TestSim2(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
we_pairs = list(zip(wToi_list, eToi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity2.Align(we_pairs)
@ -91,7 +91,7 @@ class TestSim2(GtsamTestCase):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_scaled_squares(self):
"""Test if Align Pose2Pairs method can account for gauge ambiguity.
"""Test if Align method can account for gauge ambiguity.
Make sure a big and small square can be aligned.
The u's represent a big square (10x10), and v's represents a small square (4x4).
@ -120,7 +120,7 @@ class TestSim2(GtsamTestCase):
bTi_list = [bTi0, bTi1, bTi2, bTi3]
ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list)))
ab_pairs = list(zip(aTi_list, bTi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity2.Align(ab_pairs)

14
python/gtsam/tests/test_Sim3.py
View File

@ -23,7 +23,7 @@ class TestSim3(GtsamTestCase):
"""Test selected Sim3 methods."""
def test_align_poses_along_straight_line(self):
"""Test Align Pose3Pairs method.
"""Test Pose3 Align method.
Scenario:
3 object poses
@ -49,7 +49,7 @@ class TestSim3(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list)))
we_pairs = list(zip(wToi_list, eToi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs)
@ -58,7 +58,7 @@ class TestSim3(GtsamTestCase):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_along_straight_line_gauge(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
"""Test if Pose3 Align method can account for gauge ambiguity.
Scenario:
3 object poses
@ -84,7 +84,7 @@ class TestSim3(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2]
we_pairs = gtsam.Pose3Pairs(list(zip(wToi_list, eToi_list)))
we_pairs = list(zip(wToi_list, eToi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs)
@ -93,7 +93,7 @@ class TestSim3(GtsamTestCase):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_scaled_squares(self):
"""Test if Align Pose3Pairs method can account for gauge ambiguity.
"""Test if Pose3 Align method can account for gauge ambiguity.
Make sure a big and small square can be aligned.
The u's represent a big square (10x10), and v's represents a small square (4x4).
@ -122,7 +122,7 @@ class TestSim3(GtsamTestCase):
bTi_list = [bTi0, bTi1, bTi2, bTi3]
ab_pairs = gtsam.Pose3Pairs(list(zip(aTi_list, bTi_list)))
ab_pairs = list(zip(aTi_list, bTi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity3.Align(ab_pairs)
@ -689,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 = gtsam.Pose3Pairs(list(zip(aTi_list, bTi_list)))
ab_pairs = list(zip(aTi_list, bTi_list))
aSb = Similarity3.Align(ab_pairs)

14
python/gtsam/tests/test_Triangulation.py
View File

@ -35,9 +35,7 @@ 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 = gtsam.Pose3Vector()
self.poses.append(pose1)
self.poses.append(pose2)
self.poses = [pose1, pose2]
# landmark ~5 meters infront of camera
self.landmark = Point3(5, 0.5, 1.2)
@ -67,7 +65,7 @@ class TestTriangulationExample(GtsamTestCase):
cameras = camera_set()
else:
cameras = []
measurements = gtsam.Point2Vector()
measurements = []
for k, pose in zip(cal_params, self.poses):
K = calibration(*k)
@ -96,7 +94,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 = gtsam.Point2Vector()
measurements_noisy = []
measurements_noisy.append(measurements[0] - np.array([0.1, 0.5]))
measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
@ -163,8 +161,8 @@ class TestTriangulationExample(GtsamTestCase):
z2: Point2 = camera2.project(landmark)
z3: Point2 = camera3.project(landmark)
poses = gtsam.Pose3Vector([pose1, pose2, pose3])
measurements = gtsam.Point2Vector([z1, z2, z3])
poses = [pose1, pose2, pose3]
measurements = [z1, z2, z3]
# noise free, so should give exactly the landmark
actual = gtsam.triangulatePoint3(poses,
@ -229,7 +227,7 @@ class TestTriangulationExample(GtsamTestCase):
cameras.append(camera1)
cameras.append(camera2)
measurements = gtsam.Point2Vector()
measurements = []
measurements.append(z1)
measurements.append(z2)