Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add python unit test for triangulation with robust noise model #1031

Merged
Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
64 changes: 57 additions & 7 deletions python/gtsam/tests/test_Triangulation.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
Author: Frank Dellaert & Fan Jiang (Python)
"""
import unittest
from typing import Union

import numpy as np

Expand All @@ -20,14 +21,16 @@
from gtsam.utils.test_case import GtsamTestCase


class TestVisualISAMExample(GtsamTestCase):
UPRIGHT = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)


class TestTriangulationExample(GtsamTestCase):
""" Tests for triangulation with shared and individual calibrations """

def setUp(self):
""" Set up two camera poses """
# Looking along X-axis, 1 meter above ground plane (x-y)
upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
pose1 = Pose3(upright, Point3(0, 0, 1))
pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
dellaert marked this conversation as resolved.
Show resolved Hide resolved

# create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
Expand All @@ -39,7 +42,7 @@ def setUp(self):
# landmark ~5 meters infront of camera
self.landmark = Point3(5, 0.5, 1.2)

def generate_measurements(self, calibration, camera_model, cal_params, camera_set=None):
def generate_measurements(self, calibration: Union[Cal3Bundler, Cal3_S2], camera_model, cal_params, camera_set=None):
"""
Generate vector of measurements for given calibration and camera model.

Expand All @@ -48,6 +51,7 @@ def generate_measurements(self, calibration, camera_model, cal_params, camera_se
camera_model: Camera model e.g. PinholeCameraCal3_S2
cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2]
camera_set: Cameraset object (for individual calibrations)

Returns:
list of measurements and list/CameraSet object for cameras
"""
Expand All @@ -66,7 +70,7 @@ def generate_measurements(self, calibration, camera_model, cal_params, camera_se

return measurements, cameras

def test_TriangulationExample(self):
def test_TriangulationExample(self) -> None:
""" Tests triangulation with shared Cal3_S2 calibration"""
# Some common constants
sharedCal = (1500, 1200, 0, 640, 480)
Expand Down Expand Up @@ -95,7 +99,7 @@ def test_TriangulationExample(self):

self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)

def test_distinct_Ks(self):
def test_distinct_Ks(self) -> None:
""" Tests triangulation with individual Cal3_S2 calibrations """
# two camera parameters
K1 = (1500, 1200, 0, 640, 480)
Expand All @@ -112,7 +116,7 @@ def test_distinct_Ks(self):
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)

def test_distinct_Ks_Bundler(self):
def test_distinct_Ks_Bundler(self) -> None:
""" Tests triangulation with individual Cal3Bundler calibrations"""
# two camera parameters
K1 = (1500, 0, 0, 640, 480)
Expand All @@ -128,7 +132,53 @@ def test_distinct_Ks_Bundler(self):
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)

def test_triangulation_robust_three_poses(self) -> None:
"""Ensure triangulation with a robust model works."""
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)

# landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2)

pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0))
pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1))

camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
camera3 = PinholeCameraCal3_S2(pose3, sharedCal)

z1: Point2 = camera1.project(landmark)
z2: Point2 = camera2.project(landmark)
z3: Point2 = camera3.project(landmark)

poses = [pose1, pose2, pose3]
measurements = Point2Vector([z1, z2, z3])

# noise free, so should give exactly the landmark
actual = gtsam.triangulatePoint3(poses, sharedCal, measurements)
self.assert_equal(landmark, actual, 1e-2)

# Add outlier
measurements.at(0) += Point2(100, 120) # very large pixel noise!

# now estimate does not match landmark
actual2 = gtsam.triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements)
# DLT is surprisingly robust, but still off (actual error is around 0.26m)
self.assertTrue( (landmark - actual2).norm() >= 0.2)
self.assertTrue( (landmark - actual2).norm() <= 0.5)

# Again with nonlinear optimization
actual3 = gtsam.triangulatePoint3(poses, sharedCal, measurements, 1e-9, true)
# result from nonlinear (but non-robust optimization) is close to DLT and still off
self.assertEqual(actual2, actual3, 0.1)

# Again with nonlinear optimization, this time with robust loss
model = noiseModel.Robust.Create(noiseModel.mEstimator.Huber.Create(1.345), noiseModel.Unit.Create(2))
actual4 = gtsam.triangulatePoint3(poses, sharedCal, measurements, 1e-9, true, model)
# using the Huber loss we now have a quite small error!! nice!
self.assertEqual(landmark, actual4, 0.05)


if __name__ == "__main__":
unittest.main()