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

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

@ -6,29 +6,39 @@ 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):
""" Tests for triangulation with shared and individual calibrations """ """Tests for triangulation with shared and individual calibrations"""
def setUp(self): def setUp(self):
""" Set up two camera poses """ """Set up two camera poses"""
# Looking along X-axis, 1 meter above ground plane (x-y) # Looking along X-axis, 1 meter above ground plane (x-y)
pose1 = Pose3(UPRIGHT, Point3(0, 0, 1)) pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
@ -42,16 +52,22 @@ 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.
Args: Args:
calibration: Camera calibration e.g. Cal3_S2 calibration: Camera calibration e.g. Cal3_S2
camera_model: Camera model e.g. PinholeCameraCal3_S2 camera_model: Camera model e.g. PinholeCameraCal3_S2
cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2] cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2]
camera_set: Cameraset object (for individual calibrations) camera_set: Cameraset object (for individual calibrations)
Returns: Returns:
list of measurements and list/CameraSet object for cameras list of measurements and list/CameraSet object for cameras
""" """
@ -71,19 +87,15 @@ class TestTriangulationExample(GtsamTestCase):
return measurements, cameras return measurements, cameras
def test_TriangulationExample(self) -> None: def test_TriangulationExample(self) -> None:
""" 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, 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,59 +103,49 @@ 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)
def test_distinct_Ks(self) -> None: def test_distinct_Ks(self) -> None:
""" Tests triangulation with individual Cal3_S2 calibrations """ """Tests triangulation with individual Cal3_S2 calibrations"""
# two camera parameters # two camera parameters
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:
""" Tests triangulation with individual Cal3Bundler calibrations""" """Tests triangulation with individual Cal3Bundler calibrations"""
# two camera parameters # two camera parameters
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:
"""Ensure triangulation with a robust model works.""" """Ensure triangulation with a robust model works."""
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
# landmark ~5 meters infront of camera # landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2) landmark = Point3(5, 0.5, 1.2)
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)
camera3 = PinholeCameraCal3_S2(pose3, sharedCal) camera3 = PinholeCameraCal3_S2(pose3, sharedCal)
@ -151,34 +153,36 @@ class TestTriangulationExample(GtsamTestCase):
z1: Point2 = camera1.project(landmark) z1: Point2 = camera1.project(landmark)
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__":
unittest.main() unittest.main()