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

moved landmark variable to setup

release/4.3a0
Sushmita 2020-11-28 17:34:04 -05:00
parent a524b17cde
commit 362afce864
1 changed files with 18 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -29,6 +29,9 @@ class TestVisualISAMExample(GtsamTestCase):
# create second camera 1 meter to the right of first camera # create second camera 1 meter to the right of first camera
self.pose2 = self.pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) self.pose2 = self.pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
# landmark ~5 meters infront of camera
self.landmark = Point3(5, 0.5, 1.2)
def test_TriangulationExample(self): def test_TriangulationExample(self):
# Some common constants # Some common constants
@ -36,12 +39,9 @@ class TestVisualISAMExample(GtsamTestCase):
camera1 = PinholeCameraCal3_S2(self.pose1, sharedCal) camera1 = PinholeCameraCal3_S2(self.pose1, sharedCal)
camera2 = PinholeCameraCal3_S2(self.pose2, sharedCal) camera2 = PinholeCameraCal3_S2(self.pose2, sharedCal)
# landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2)
# 1. Project two landmarks into two cameras and triangulate # 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(landmark) z1 = camera1.project(self.landmark)
z2 = camera2.project(landmark) z2 = camera2.project(self.landmark)
# twoPoses # twoPoses
poses = Pose3Vector() poses = Pose3Vector()
@ -56,7 +56,7 @@ class TestVisualISAMExample(GtsamTestCase):
rank_tol = 1e-9 rank_tol = 1e-9
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-9)
# 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements = Point2Vector() measurements = Point2Vector()
@ -64,22 +64,22 @@ class TestVisualISAMExample(GtsamTestCase):
measurements.append(z2 - np.array([-0.2, 0.3])) measurements.append(z2 - np.array([-0.2, 0.3]))
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2) self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-2)
# #
# # two Poses with Bundler Calibration # # two Poses with Bundler Calibration
# bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480) # bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480)
# camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal) # camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal)
# camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal) # camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal)
# #
# z1 = camera1.project(landmark) # z1 = camera1.project(self.landmark)
# z2 = camera2.project(landmark) # z2 = camera2.project(self.landmark)
# #
# measurements = Point2Vector() # measurements = Point2Vector()
# measurements.append(z1) # measurements.append(z1)
# measurements.append(z2) # measurements.append(z2)
# #
# triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize) # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize)
# self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) # self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-9)
def test_distinct_Ks(self): def test_distinct_Ks(self):
# two cameras # two cameras
@ -93,12 +93,9 @@ class TestVisualISAMExample(GtsamTestCase):
cameras.append(camera1) cameras.append(camera1)
cameras.append(camera2) cameras.append(camera2)
# landmark ~5 meters infront of camera # Project two landmarks into two cameras and triangulate
landmark = Point3(5, 0.5, 1.2) z1 = camera1.project(self.landmark)
z2 = camera2.project(self.landmark)
# 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(landmark)
z2 = camera2.project(landmark)
measurements = Point2Vector() measurements = Point2Vector()
measurements.append(z1) measurements.append(z1)
@ -108,7 +105,7 @@ class TestVisualISAMExample(GtsamTestCase):
rank_tol = 1e-9 rank_tol = 1e-9
triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-9) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
def test_distinct_Ks_Bundler(self): def test_distinct_Ks_Bundler(self):
# two cameras # two cameras
@ -122,12 +119,9 @@ class TestVisualISAMExample(GtsamTestCase):
cameras.append(camera1) cameras.append(camera1)
cameras.append(camera2) cameras.append(camera2)
# landmark ~5 meters infront of camera # Project two landmarks into two cameras and triangulate
landmark = Point3(5, 0.5, 1.2) z1 = camera1.project(self.landmark)
z2 = camera2.project(self.landmark)
# 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(landmark)
z2 = camera2.project(landmark)
measurements = Point2Vector() measurements = Point2Vector()
measurements.append(z1) measurements.append(z1)
@ -137,7 +131,7 @@ class TestVisualISAMExample(GtsamTestCase):
rank_tol = 1e-9 rank_tol = 1e-9
triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-9) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
if __name__ == "__main__": if __name__ == "__main__":
unittest.main() unittest.main()