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) camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
cls.origin = np.array([0.0, 0.0, 0.0]) cls.origin = np.array([0.0, 0.0, 0.0])
cls.poses = gtsam.Pose3Vector([pose1, pose2]) cls.poses = [pose1, pose2]
cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) cls.cameras = [camera1, camera2]
cls.measurements = gtsam.Point2Vector( cls.measurements = [k.project(cls.origin) for k in cls.cameras]
[k.project(cls.origin) for k in cls.cameras])
def test_Cal3Fisheye(self): def test_Cal3Fisheye(self):
K = gtsam.Cal3Fisheye() 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) camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic)
camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic)
cls.origin = np.array([0.0, 0.0, 0.0]) cls.origin = np.array([0.0, 0.0, 0.0])
cls.poses = gtsam.Pose3Vector([pose1, pose2]) cls.poses = [pose1, pose2]
cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) cls.cameras = [camera1, camera2]
cls.measurements = gtsam.Point2Vector( cls.measurements = [k.project(cls.origin) for k in cls.cameras]
[k.project(cls.origin) for k in cls.cameras])
def test_Cal3Unified(self): def test_Cal3Unified(self):
K = gtsam.Cal3Unified() K = gtsam.Cal3Unified()
@ -159,7 +158,7 @@ class TestCal3Unified(GtsamTestCase):
def test_triangulation_rectify(self): def test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification""" """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() shared_cal = gtsam.Cal3_S2()
triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
self.gtsamAssertEquals(triangulated, self.origin) 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_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]]))
kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]])) 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_i0)
keypoints_list.append(kps_i1) keypoints_list.append(kps_i1)
keypoints_list.append(kps_i2) keypoints_list.append(kps_i2)
# For each image pair (i1,i2), we provide a (K,2) matrix # For each image pair (i1,i2), we provide a (K,2) matrix
# of corresponding image indices (k1,k2). # 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(0, 1)] = np.array([[0, 0], [1, 1]])
matches_dict[IndexPair(1, 2)] = np.array([[2, 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: def test_sfm_track_2d_constructor(self) -> None:
"""Test construction of 2D SfM track.""" """Test construction of 2D SfM track."""
measurements = gtsam.SfmMeasurementVector() measurements = []
measurements.append((0, Point2(10, 20))) measurements.append((0, Point2(10, 20)))
track = SfmTrack2d(measurements=measurements) track = SfmTrack2d(measurements=measurements)
track.measurement(0) 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 from gtsam.utils.test_case import GtsamTestCase
import gtsam import gtsam
import gtsam_unstable
class TestFixedLagSmootherExample(GtsamTestCase): class TestFixedLagSmootherExample(GtsamTestCase):
@ -37,7 +36,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
# that will be sent to the smoothers # that will be sent to the smoothers
new_factors = gtsam.NonlinearFactorGraph() new_factors = gtsam.NonlinearFactorGraph()
new_values = gtsam.Values() new_values = gtsam.Values()
new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() new_timestamps = {}
# Create a prior on the first pose, placing it at the origin # Create a prior on the first pose, placing it at the origin
prior_mean = gtsam.Pose2(0, 0, 0) prior_mean = gtsam.Pose2(0, 0, 0)
@ -48,7 +47,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
gtsam.PriorFactorPose2( gtsam.PriorFactorPose2(
X1, prior_mean, prior_noise)) X1, prior_mean, prior_noise))
new_values.insert(X1, prior_mean) new_values.insert(X1, prior_mean)
new_timestamps.insert((X1, 0.0)) new_timestamps[X1] = 0.0
delta_time = 0.25 delta_time = 0.25
time = 0.25 time = 0.25
@ -78,7 +77,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
current_key = int(1000 * time) current_key = int(1000 * time)
# assign current key to the current timestamp # 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 # Add a guess for this pose to the new values
# Assume that the robot moves at 2 m/s. Position is time[s] * # 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): class TestGaussianFactorGraph(GtsamTestCase):
"""Tests for Gaussian Factor Graphs.""" """Tests for Gaussian Factor Graphs."""
def test_fg(self): def test_fg(self):
"""Test solving a linear factor graph""" """Test solving a linear factor graph"""
graph, X = create_graph() graph, X = create_graph()
@ -99,12 +100,11 @@ class TestGaussianFactorGraph(GtsamTestCase):
bn = gfg.eliminateSequential(ordering) bn = gfg.eliminateSequential(ordering)
self.assertEqual(bn.size(), 3) self.assertEqual(bn.size(), 3)
keyVector = gtsam.KeyVector() keyVector = [keys[2]]
keyVector.append(keys[2]) ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(
#TODO(Varun) Below code causes segfault in Debug config gfg, keyVector)
# ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector) bn = gfg.eliminateSequential(ordering)
# bn = gfg.eliminateSequential(ordering) self.assertEqual(bn.size(), 3)
# self.assertEqual(bn.size(), 3)
if __name__ == '__main__': if __name__ == '__main__':

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

@ -22,7 +22,6 @@ from gtsam import Rot3
KEY = 0 KEY = 0
MODEL = gtsam.noiseModel.Unit.Create(3) MODEL = gtsam.noiseModel.Unit.Create(3)
# Rot3 version # Rot3 version
R = Rot3.Expmap(np.array([0.1, 0, 0])) 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) Check that optimizing for Karcher mean (which minimizes Between distance)
gets correct result. gets correct result.
""" """
rotations = gtsam.Rot3Vector([R, R.inverse()]) rotations = [R, R.inverse()]
expected = Rot3() expected = Rot3()
actual = gtsam.FindKarcherMean(rotations) actual = gtsam.FindKarcherMean(rotations)
self.gtsamAssertEquals(expected, actual) self.gtsamAssertEquals(expected, actual)
@ -45,7 +44,7 @@ class TestKarcherMean(GtsamTestCase):
a2Rb2 = Rot3() a2Rb2 = Rot3()
a3Rb3 = Rot3() a3Rb3 = Rot3()
aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3]) aRb_list = [a1Rb1, a2Rb2, a3Rb3]
aRb_expected = Rot3() aRb_expected = Rot3()
aRb = gtsam.FindKarcherMean(aRb_list) aRb = gtsam.FindKarcherMean(aRb_list)
@ -61,9 +60,7 @@ class TestKarcherMean(GtsamTestCase):
graph = gtsam.NonlinearFactorGraph() graph = gtsam.NonlinearFactorGraph()
R12 = R.compose(R.compose(R)) R12 = R.compose(R.compose(R))
graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL)) graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
keys = gtsam.KeyVector() keys = [1, 2]
keys.append(1)
keys.append(2)
graph.add(gtsam.KarcherMeanFactorRot3(keys)) graph.add(gtsam.KarcherMeanFactorRot3(keys))
initial = gtsam.Values() initial = gtsam.Values()
@ -72,12 +69,9 @@ class TestKarcherMean(GtsamTestCase):
expected = Rot3() expected = Rot3()
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
actual = gtsam.FindKarcherMean( actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)])
gtsam.Rot3Vector([result.atRot3(1),
result.atRot3(2)]))
self.gtsamAssertEquals(expected, actual) self.gtsamAssertEquals(expected, actual)
self.gtsamAssertEquals( self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2)))
R12, result.atRot3(1).between(result.atRot3(2)))
if __name__ == "__main__": 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): class TestPose2(GtsamTestCase):
"""Test selected Pose2 methods.""" """Test selected Pose2 methods."""
def test_adjoint(self) -> None: def test_adjoint(self) -> None:
"""Test adjoint method.""" """Test adjoint method."""
xi = np.array([1, 2, 3]) xi = np.array([1, 2, 3])
@ -28,7 +29,7 @@ class TestPose2(GtsamTestCase):
def test_transformTo(self): def test_transformTo(self):
"""Test transformTo method.""" """Test transformTo method."""
pose = Pose2(2, 4, -math.pi/2) pose = Pose2(2, 4, -math.pi / 2)
actual = pose.transformTo(Point2(3, 2)) actual = pose.transformTo(Point2(3, 2))
expected = Point2(2, 1) expected = Point2(2, 1)
self.gtsamAssertEquals(actual, expected, 1e-6) self.gtsamAssertEquals(actual, expected, 1e-6)
@ -42,7 +43,7 @@ class TestPose2(GtsamTestCase):
def test_transformFrom(self): def test_transformFrom(self):
"""Test transformFrom method.""" """Test transformFrom method."""
pose = Pose2(2, 4, -math.pi/2) pose = Pose2(2, 4, -math.pi / 2)
actual = pose.transformFrom(Point2(2, 1)) actual = pose.transformFrom(Point2(2, 1))
expected = Point2(3, 2) expected = Point2(3, 2)
self.gtsamAssertEquals(actual, expected, 1e-6) self.gtsamAssertEquals(actual, expected, 1e-6)
@ -83,7 +84,7 @@ class TestPose2(GtsamTestCase):
] ]
# fmt: on # fmt: on
ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) ab_pairs = list(zip(pts_a, pts_b))
aTb = Pose2.Align(ab_pairs) aTb = Pose2.Align(ab_pairs)
self.assertIsNotNone(aTb) 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)) sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0))
transformed = sTt.transformTo(square) transformed = sTt.transformTo(square)
st_pairs = gtsam.Point3Pairs() st_pairs = []
for j in range(4): for j in range(4):
st_pairs.append((square[:,j], transformed[:,j])) 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) self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3)
result, _lambdaMin = shonan.run(initial, 3, 3) result, _lambdaMin = shonan.run(initial, 3, 3)
self.assertAlmostEqual(0.0015, shonan.cost(result), places=3) self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
def test_constructorBetweenFactorPose2s(self) -> None: def test_constructorBetweenFactorPose2s(self) -> None:
"""Check if ShonanAveraging2 constructor works when not initialized from g2o file. """Check if ShonanAveraging2 constructor works when not initialized from g2o file.
@ -177,7 +176,7 @@ class TestShonanAveraging(GtsamTestCase):
shonan_params.setCertifyOptimality(True) shonan_params.setCertifyOptimality(True)
noise_model = gtsam.noiseModel.Unit.Create(3) noise_model = gtsam.noiseModel.Unit.Create(3)
between_factors = gtsam.BetweenFactorPose2s() between_factors = []
for (i1, i2), i2Ri1 in i2Ri1_dict.items(): for (i1, i2), i2Ri1 in i2Ri1_dict.items():
i2Ti1 = Pose2(i2Ri1, np.zeros(2)) i2Ti1 = Pose2(i2Ri1, np.zeros(2))
between_factors.append( between_factors.append(
@ -190,11 +189,11 @@ class TestShonanAveraging(GtsamTestCase):
wRi_list = [result_values.atRot2(i) for i in range(num_images)] wRi_list = [result_values.atRot2(i) for i in range(num_images)]
thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) thetas_deg = np.array([wRi.degrees() for wRi in wRi_list])
# map all angles to [0,360) # map all angles to [0,360)
thetas_deg = thetas_deg % 360 thetas_deg = thetas_deg % 360
thetas_deg -= thetas_deg[0] thetas_deg -= thetas_deg[0]
expected_thetas_deg = np.array([0.0, 90.0, 0.0]) expected_thetas_deg = np.array([0.0, 90.0, 0.0])
np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) 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 import numpy as np
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2 from gtsam import Pose2, Rot2, Similarity2
class TestSim2(GtsamTestCase): class TestSim2(GtsamTestCase):
@ -47,7 +47,7 @@ class TestSim2(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2] 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) # Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity2.Align(we_pairs) wSe = Similarity2.Align(we_pairs)
@ -56,7 +56,7 @@ class TestSim2(GtsamTestCase):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_along_straight_line_gauge(self): 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: Scenario:
3 object poses 3 object poses
@ -82,7 +82,7 @@ class TestSim2(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2] 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) # Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity2.Align(we_pairs) wSe = Similarity2.Align(we_pairs)
@ -91,7 +91,7 @@ class TestSim2(GtsamTestCase):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_scaled_squares(self): 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. 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). 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] 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) # Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity2.Align(ab_pairs) 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.""" """Test selected Sim3 methods."""
def test_align_poses_along_straight_line(self): def test_align_poses_along_straight_line(self):
"""Test Align Pose3Pairs method. """Test Pose3 Align method.
Scenario: Scenario:
3 object poses 3 object poses
@ -49,7 +49,7 @@ class TestSim3(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2] 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) # Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs) wSe = Similarity3.Align(we_pairs)
@ -58,7 +58,7 @@ class TestSim3(GtsamTestCase):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_along_straight_line_gauge(self): 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: Scenario:
3 object poses 3 object poses
@ -84,7 +84,7 @@ class TestSim3(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2] 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) # Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs) wSe = Similarity3.Align(we_pairs)
@ -93,7 +93,7 @@ class TestSim3(GtsamTestCase):
self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi)) self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def test_align_poses_scaled_squares(self): 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. 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). 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] 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) # Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity3.Align(ab_pairs) 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 len(aTi_list) == len(bTi_list)
assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames" 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) 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 # create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
# twoPoses # twoPoses
self.poses = gtsam.Pose3Vector() self.poses = [pose1, pose2]
self.poses.append(pose1)
self.poses.append(pose2)
# landmark ~5 meters infront of camera # landmark ~5 meters infront of camera
self.landmark = Point3(5, 0.5, 1.2) self.landmark = Point3(5, 0.5, 1.2)
@ -67,7 +65,7 @@ class TestTriangulationExample(GtsamTestCase):
cameras = camera_set() cameras = camera_set()
else: else:
cameras = [] cameras = []
measurements = gtsam.Point2Vector() measurements = []
for k, pose in zip(cal_params, self.poses): for k, pose in zip(cal_params, self.poses):
K = calibration(*k) K = calibration(*k)
@ -96,7 +94,7 @@ class TestTriangulationExample(GtsamTestCase):
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
# Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) # 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[0] - np.array([0.1, 0.5]))
measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
@ -163,8 +161,8 @@ class TestTriangulationExample(GtsamTestCase):
z2: Point2 = camera2.project(landmark) z2: Point2 = camera2.project(landmark)
z3: Point2 = camera3.project(landmark) z3: Point2 = camera3.project(landmark)
poses = gtsam.Pose3Vector([pose1, pose2, pose3]) poses = [pose1, pose2, pose3]
measurements = gtsam.Point2Vector([z1, z2, z3]) measurements = [z1, z2, z3]
# noise free, so should give exactly the landmark # noise free, so should give exactly the landmark
actual = gtsam.triangulatePoint3(poses, actual = gtsam.triangulatePoint3(poses,
@ -229,7 +227,7 @@ class TestTriangulationExample(GtsamTestCase):
cameras.append(camera1) cameras.append(camera1)
cameras.append(camera2) cameras.append(camera2)
measurements = gtsam.Point2Vector() measurements = []
measurements.append(z1) measurements.append(z1)
measurements.append(z2) measurements.append(z2)