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

format python triangulation tests

release/4.3a0
Varun Agrawal 2020-12-05 18:09:56 -05:00
parent b24f943c36
commit a7248163e8
1 changed files with 26 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -10,16 +10,16 @@ Author: Frank Dellaert & Fan Jiang (Python)
""" """
import unittest import unittest
import numpy as np
import gtsam as g import gtsam as g
import numpy as np
from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
CameraSetCal3Bundler, PinholeCameraCal3_S2,
PinholeCameraCal3Bundler, Point2Vector, Point3, Pose3,
Pose3Vector, Rot3, triangulatePoint3)
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \
PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \
Point2Vector, Pose3Vector, triangulatePoint3, \
CameraSetCal3_S2, CameraSetCal3Bundler
from numpy.core.records import array from numpy.core.records import array
class TestVisualISAMExample(GtsamTestCase): class TestVisualISAMExample(GtsamTestCase):
""" Tests for triangulation with shared and individual calibrations """ """ Tests for triangulation with shared and individual calibrations """
@ -38,7 +38,7 @@ class TestVisualISAMExample(GtsamTestCase):
# 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)
def generate_measurements(self, calibration, camera_model, camera_set, *cal_params): def generate_measurements(self, calibration, camera_model, camera_set, *cal_params):
""" Generate vector of measurements for given calibration and camera model """ Generate vector of measurements for given calibration and camera model
Args: Args:
@ -53,26 +53,27 @@ class TestVisualISAMExample(GtsamTestCase):
cameras = camera_set() cameras = camera_set()
else: else:
cameras = [] cameras = []
measurements = Point2Vector() measurements = Point2Vector()
for k, pose in zip(cal_params, self.poses): for k, pose in zip(cal_params, self.poses):
K = calibration(*k) K = calibration(*k)
camera = camera_model(pose, K) camera = camera_model(pose, K)
cameras.append(camera) cameras.append(camera)
z = camera.project(self.landmark) z = camera.project(self.landmark)
measurements.append(z) measurements.append(z)
return measurements, cameras return measurements, cameras
def test_TriangulationExample(self): def test_TriangulationExample(self):
""" Tests triangulation with shared Cal3_S2 calibration""" """ Tests triangulation with shared Cal3_S2 calibration"""
# Some common constants # Some common constants
sharedCal = (1500, 1200, 0, 640, 480) sharedCal = (1500, 1200, 0, 640, 480)
measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, None, sharedCal, sharedCal) measurements, _ = self.generate_measurements(
Cal3_S2, PinholeCameraCal3_S2, None, sharedCal, sharedCal)
triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements, rank_tol=1e-9, optimize=True) triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(
sharedCal), measurements, rank_tol=1e-9, optimize=True)
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)
@ -80,7 +81,8 @@ class TestVisualISAMExample(GtsamTestCase):
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]))
triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True) triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(
sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
def test_distinct_Ks(self): def test_distinct_Ks(self):
@ -89,9 +91,11 @@ class TestVisualISAMExample(GtsamTestCase):
K1 = (1500, 1200, 0, 640, 480) K1 = (1500, 1200, 0, 640, 480)
K2 = (1600, 1300, 0, 650, 440) K2 = (1600, 1300, 0, 650, 440)
measurements, cameras = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, CameraSetCal3_S2, K1, K2) measurements, cameras = self.generate_measurements(
Cal3_S2, PinholeCameraCal3_S2, CameraSetCal3_S2, K1, K2)
triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) triangulated_landmark = triangulatePoint3(
cameras, measurements, rank_tol=1e-9, optimize=True)
self.gtsamAssertEquals(self.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):
@ -100,10 +104,13 @@ class TestVisualISAMExample(GtsamTestCase):
K1 = (1500, 0, 0, 640, 480) K1 = (1500, 0, 0, 640, 480)
K2 = (1600, 0, 0, 650, 440) K2 = (1600, 0, 0, 650, 440)
measurements, cameras = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, CameraSetCal3Bundler, K1, K2) measurements, cameras = self.generate_measurements(
Cal3Bundler, PinholeCameraCal3Bundler, CameraSetCal3Bundler, K1, K2)
triangulated_landmark = triangulatePoint3(
cameras, measurements, rank_tol=1e-9, optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
if __name__ == "__main__": if __name__ == "__main__":
unittest.main() unittest.main()