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

fix syntax errors

release/4.3a0
John Lambert 2022-01-12 13:01:23 -05:00 committed by GitHub
parent b60ca0c107
commit d66b1d7a84
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 71 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -6,22 +6,32 @@ All Rights Reserved
See LICENSE for the license information See LICENSE for the license information
Test Triangulation Test Triangulation
Author: Frank Dellaert & Fan Jiang (Python) Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert
""" """
import unittest import unittest
from typing import Union from typing import Optional, Union
import numpy as np import numpy as np
import gtsam import gtsam
from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, from gtsam import (
CameraSetCal3Bundler, PinholeCameraCal3_S2, Cal3_S2,
PinholeCameraCal3Bundler, Point2Vector, Point3, Pose3, Cal3Bundler,
Pose3Vector, Rot3) CameraSetCal3_S2,
CameraSetCal3Bundler,
PinholeCameraCal3_S2,
PinholeCameraCal3Bundler,
Point2,
Point2Vector,
Point3,
Pose3,
Pose3Vector,
Rot3,
)
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
UPRIGHT = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2) UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
class TestTriangulationExample(GtsamTestCase): class TestTriangulationExample(GtsamTestCase):
@ -42,7 +52,13 @@ class TestTriangulationExample(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: Union[Cal3Bundler, Cal3_S2], camera_model, cal_params, camera_set=None): def generate_measurements(
self,
calibration: Union[Cal3Bundler, Cal3_S2],
camera_model,
cal_params,
camera_set: Optional[Union[CameraSetCal3Bundler, CameraSetCal3_S2]] = None,
):
""" """
Generate vector of measurements for given calibration and camera model. Generate vector of measurements for given calibration and camera model.
@ -75,15 +91,11 @@ class TestTriangulationExample(GtsamTestCase):
# Some common constants # Some common constants
sharedCal = (1500, 1200, 0, 640, 480) sharedCal = (1500, 1200, 0, 640, 480)
measurements, _ = self.generate_measurements(Cal3_S2, measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, (sharedCal, sharedCal))
PinholeCameraCal3_S2,
(sharedCal, sharedCal))
triangulated_landmark = gtsam.triangulatePoint3(self.poses, triangulated_landmark = gtsam.triangulatePoint3(
Cal3_S2(sharedCal), self.poses, Cal3_S2(sharedCal), measurements, rank_tol=1e-9, optimize=True
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)
@ -91,11 +103,9 @@ class TestTriangulationExample(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 = gtsam.triangulatePoint3(self.poses, triangulated_landmark = gtsam.triangulatePoint3(
Cal3_S2(sharedCal), self.poses, Cal3_S2(sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True
measurements_noisy, )
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
@ -105,15 +115,11 @@ class TestTriangulationExample(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, measurements, cameras = self.generate_measurements(
PinholeCameraCal3_S2, Cal3_S2, PinholeCameraCal3_S2, (K1, K2), camera_set=CameraSetCal3_S2
(K1, K2), )
camera_set=CameraSetCal3_S2)
triangulated_landmark = gtsam.triangulatePoint3(cameras, triangulated_landmark = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True)
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) -> None: def test_distinct_Ks_Bundler(self) -> None:
@ -122,15 +128,11 @@ class TestTriangulationExample(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, measurements, cameras = self.generate_measurements(
PinholeCameraCal3Bundler, Cal3Bundler, PinholeCameraCal3Bundler, (K1, K2), camera_set=CameraSetCal3Bundler
(K1, K2), )
camera_set=CameraSetCal3Bundler)
triangulated_landmark = gtsam.triangulatePoint3(cameras, triangulated_landmark = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True)
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_triangulation_robust_three_poses(self) -> None: def test_triangulation_robust_three_poses(self) -> None:
@ -142,7 +144,7 @@ class TestTriangulationExample(GtsamTestCase):
pose1 = Pose3(UPRIGHT, Point3(0, 0, 1)) pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)) pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0))
pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)) pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -0.1))
camera1 = PinholeCameraCal3_S2(pose1, sharedCal) camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
camera2 = PinholeCameraCal3_S2(pose2, sharedCal) camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
@ -152,32 +154,34 @@ class TestTriangulationExample(GtsamTestCase):
z2: Point2 = camera2.project(landmark) z2: Point2 = camera2.project(landmark)
z3: Point2 = camera3.project(landmark) z3: Point2 = camera3.project(landmark)
poses = [pose1, pose2, pose3] poses = gtsam.Pose3Vector([pose1, pose2, pose3])
measurements = Point2Vector([z1, z2, z3]) measurements = Point2Vector([z1, z2, z3])
# noise free, so should give exactly the landmark # noise free, so should give exactly the landmark
actual = gtsam.triangulatePoint3(poses, sharedCal, measurements) actual = gtsam.triangulatePoint3(poses, sharedCal, measurements, rank_tol=1e-9, optimize=False)
self.assert_equal(landmark, actual, 1e-2) self.assertTrue(np.allclose(landmark, actual, atol=1e-2))
# Add outlier # Add outlier
measurements.at(0) += Point2(100, 120) # very large pixel noise! measurements[0] += Point2(100, 120) # very large pixel noise!
# now estimate does not match landmark # now estimate does not match landmark
actual2 = gtsam.triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements) actual2 = gtsam.triangulatePoint3(poses, sharedCal, measurements, rank_tol=1e-9, optimize=False)
# DLT is surprisingly robust, but still off (actual error is around 0.26m) # DLT is surprisingly robust, but still off (actual error is around 0.26m)
self.assertTrue( (landmark - actual2).norm() >= 0.2) self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2)
self.assertTrue( (landmark - actual2).norm() <= 0.5) self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5)
# Again with nonlinear optimization # Again with nonlinear optimization
actual3 = gtsam.triangulatePoint3(poses, sharedCal, measurements, 1e-9, true) actual3 = gtsam.triangulatePoint3(poses, sharedCal, measurements, rank_tol=1e-9, optimize=True)
# result from nonlinear (but non-robust optimization) is close to DLT and still off # result from nonlinear (but non-robust optimization) is close to DLT and still off
self.assertEqual(actual2, actual3, 0.1) self.assertTrue(np.allclose(actual2, actual3, atol=0.1))
# Again with nonlinear optimization, this time with robust loss # Again with nonlinear optimization, this time with robust loss
model = noiseModel.Robust.Create(noiseModel.mEstimator.Huber.Create(1.345), noiseModel.Unit.Create(2)) model = gtsam.noiseModel.Robust.Create(
actual4 = gtsam.triangulatePoint3(poses, sharedCal, measurements, 1e-9, true, model) gtsam.noiseModel.mEstimator.Huber.Create(1.345), gtsam.noiseModel.Unit.Create(2)
)
actual4 = gtsam.triangulatePoint3(poses, sharedCal, measurements, rank_tol=1e-9, optimize=True, model=model)
# using the Huber loss we now have a quite small error!! nice! # using the Huber loss we now have a quite small error!! nice!
self.assertEqual(landmark, actual4, 0.05) self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
if __name__ == "__main__": if __name__ == "__main__":