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

Merge pull request #1031 from borglab/add-python-unit-test-robust-noise 

Add python unit test for triangulation with robust noise model
release/4.3a0
John Lambert 2022-01-12 23:27:13 -05:00 committed by GitHub
parent c407609b8f 0ff9110f3c
commit aef2a39e94
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 132 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/geometry.i
View File

@ -923,27 +923,34 @@ class StereoCamera {
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal, gtsam::Cal3_S2* sharedCal,
const gtsam::Point2Vector& measurements, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize); double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3DS2* sharedCal, gtsam::Cal3DS2* sharedCal,
const gtsam::Point2Vector& measurements, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize); double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal, gtsam::Cal3Bundler* sharedCal,
const gtsam::Point2Vector& measurements, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize); double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
const gtsam::Point2Vector& measurements, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize); double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
const gtsam::Point2Vector& measurements, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize); double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
const gtsam::Point2Vector& measurements, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize); double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras, gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
const gtsam::Point2Vector& measurements, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize); double rank_tol, bool optimize,
const gtsam::SharedNoiseModel& model = nullptr);
gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal, gtsam::Cal3_S2* sharedCal,
const gtsam::Point2Vector& measurements, const gtsam::Point2Vector& measurements,

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

@ -6,28 +6,40 @@ 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 Iterable, List, Optional, Tuple, 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.0, -np.pi / 2)
class TestVisualISAMExample(GtsamTestCase):
""" Tests for triangulation with shared and individual calibrations """ class TestTriangulationExample(GtsamTestCase):
"""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)
upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2) pose1 = Pose3(UPRIGHT, Point3(0, 0, 1))
pose1 = Pose3(upright, Point3(0, 0, 1))
# create second camera 1 meter to the right of first camera # create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
@ -39,15 +51,24 @@ 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, cal_params, camera_set=None): def generate_measurements(
self,
calibration: Union[Cal3Bundler, Cal3_S2],
camera_model: Union[PinholeCameraCal3Bundler, PinholeCameraCal3_S2],
cal_params: Iterable[Iterable[Union[int, float]]],
camera_set: Optional[Union[CameraSetCal3Bundler,
CameraSetCal3_S2]] = None,
) -> Tuple[Point2Vector, Union[CameraSetCal3Bundler, CameraSetCal3_S2,
List[Cal3Bundler], List[Cal3_S2]]]:
""" """
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
""" """
@ -66,14 +87,15 @@ class TestVisualISAMExample(GtsamTestCase):
return measurements, cameras return measurements, cameras
def test_TriangulationExample(self): 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(
PinholeCameraCal3_S2, calibration=Cal3_S2,
(sharedCal, sharedCal)) camera_model=PinholeCameraCal3_S2,
cal_params=(sharedCal, sharedCal))
triangulated_landmark = gtsam.triangulatePoint3(self.poses, triangulated_landmark = gtsam.triangulatePoint3(self.poses,
Cal3_S2(sharedCal), Cal3_S2(sharedCal),
@ -95,16 +117,17 @@ class TestVisualISAMExample(GtsamTestCase):
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) -> 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, calibration=Cal3_S2,
(K1, K2), camera_model=PinholeCameraCal3_S2,
camera_set=CameraSetCal3_S2) cal_params=(K1, K2),
camera_set=CameraSetCal3_S2)
triangulated_landmark = gtsam.triangulatePoint3(cameras, triangulated_landmark = gtsam.triangulatePoint3(cameras,
measurements, measurements,
@ -112,16 +135,17 @@ class TestVisualISAMExample(GtsamTestCase):
optimize=True) 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) -> 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, calibration=Cal3Bundler,
(K1, K2), camera_model=PinholeCameraCal3Bundler,
camera_set=CameraSetCal3Bundler) cal_params=(K1, K2),
camera_set=CameraSetCal3Bundler)
triangulated_landmark = gtsam.triangulatePoint3(cameras, triangulated_landmark = gtsam.triangulatePoint3(cameras,
measurements, measurements,
@ -129,6 +153,71 @@ class TestVisualISAMExample(GtsamTestCase):
optimize=True) 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:
"""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, -0.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 = gtsam.Pose3Vector([pose1, pose2, pose3])
measurements = Point2Vector([z1, z2, z3])
# noise free, so should give exactly the landmark
actual = gtsam.triangulatePoint3(poses,
sharedCal,
measurements,
rank_tol=1e-9,
optimize=False)
self.assertTrue(np.allclose(landmark, actual, atol=1e-2))
# Add outlier
measurements[0] += Point2(100, 120) # very large pixel noise!
# now estimate does not match landmark
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)
self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2)
self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5)
# Again with nonlinear optimization
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
self.assertTrue(np.allclose(actual2, actual3, atol=0.1))
# Again with nonlinear optimization, this time with robust loss
model = gtsam.noiseModel.Robust.Create(
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!
self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
if __name__ == "__main__": if __name__ == "__main__":
unittest.main() unittest.main()