diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 6f6c645b8..1df9efd22 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,8 +18,10 @@ #pragma once -#include +#include +#include #include +#include #include #include #include @@ -494,5 +496,9 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, } } +// Vector of Cameras - used by the Python/MATLAB wrapper +using CameraSetCal3Bundler = CameraSet>; +using CameraSetCal3_S2 = CameraSet>; + } // \namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 493c1d7db..64e57a1d5 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1067,6 +1067,15 @@ typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +template +class CameraSet { + CameraSet(); + + // structure specific methods + T at(size_t i) const; + void push_back(const T& cam); +}; + #include class StereoCamera { // Standard Constructors and Named Constructors @@ -1098,7 +1107,7 @@ class StereoCamera { #include -// Templates appear not yet supported for free functions +// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); @@ -1108,6 +1117,12 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); //************************************************************************* // Symbolic diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 00b537340..bfe08a76a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -82,6 +82,8 @@ set(ignore gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::BinaryMeasurementsUnit3 + gtsam::CameraSetCal3_S2 + gtsam::CameraSetCal3Bundler gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_unstable_py # target diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 6166f615e..b56766c72 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,3 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index cacad874c..431697aac 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,3 +13,5 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); +py::bind_vector > >(m_, "CameraSetCal3_S2"); +py::bind_vector > >(m_, "CameraSetCal3Bundler"); diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index b43ad9b57..399cf019d 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -12,69 +12,123 @@ import unittest import numpy as np -import gtsam as g +import gtsam +from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, + CameraSetCal3Bundler, PinholeCameraCal3_S2, + PinholeCameraCal3Bundler, Point2Vector, Point3, Pose3, + Pose3Vector, Rot3) from gtsam.utils.test_case import GtsamTestCase -from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ - PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3 + class TestVisualISAMExample(GtsamTestCase): - def test_TriangulationExample(self): - # Some common constants - sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) + """ 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)) - camera1 = PinholeCameraCal3_S2(pose1, sharedCal) # create second camera 1 meter to the right of first camera pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) - camera2 = PinholeCameraCal3_S2(pose2, sharedCal) + # twoPoses + self.poses = Pose3Vector() + self.poses.append(pose1) + self.poses.append(pose2) # landmark ~5 meters infront of camera - landmark = Point3(5, 0.5, 1.2) + self.landmark = Point3(5, 0.5, 1.2) - # 1. Project two landmarks into two cameras and triangulate - z1 = camera1.project(landmark) - z2 = camera2.project(landmark) + def generate_measurements(self, calibration, camera_model, cal_params, camera_set=None): + """ + Generate vector of measurements for given calibration and camera model. - # twoPoses - poses = Pose3Vector() + Args: + calibration: Camera calibration e.g. Cal3_S2 + 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 + """ + if camera_set is not None: + cameras = camera_set() + else: + cameras = [] measurements = Point2Vector() - poses.append(pose1) - poses.append(pose2) - measurements.append(z1) - measurements.append(z2) + for k, pose in zip(cal_params, self.poses): + K = calibration(*k) + camera = camera_model(pose, K) + cameras.append(camera) + z = camera.project(self.landmark) + measurements.append(z) - optimize = True - rank_tol = 1e-9 + return measurements, cameras - triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) - self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) + def test_TriangulationExample(self): + """ Tests triangulation with shared Cal3_S2 calibration""" + # Some common constants + sharedCal = (1500, 1200, 0, 640, 480) - # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) - measurements = Point2Vector() - measurements.append(z1 - np.array([0.1, 0.5])) - measurements.append(z2 - np.array([-0.2, 0.3])) + measurements, _ = self.generate_measurements(Cal3_S2, + PinholeCameraCal3_S2, + (sharedCal, sharedCal)) + + triangulated_landmark = gtsam.triangulatePoint3(self.poses, + Cal3_S2(sharedCal), + measurements, + rank_tol=1e-9, + optimize=True) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) + + # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + measurements_noisy = Point2Vector() + measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) + measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) + + triangulated_landmark = gtsam.triangulatePoint3(self.poses, + Cal3_S2(sharedCal), + measurements_noisy, + rank_tol=1e-9, + optimize=True) + + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2) + + def test_distinct_Ks(self): + """ Tests triangulation with individual Cal3_S2 calibrations """ + # two camera parameters + K1 = (1500, 1200, 0, 640, 480) + K2 = (1600, 1300, 0, 650, 440) + + measurements, cameras = self.generate_measurements(Cal3_S2, + PinholeCameraCal3_S2, + (K1, K2), + camera_set=CameraSetCal3_S2) + + triangulated_landmark = gtsam.triangulatePoint3(cameras, + measurements, + rank_tol=1e-9, + optimize=True) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) + + def test_distinct_Ks_Bundler(self): + """ Tests triangulation with individual Cal3Bundler calibrations""" + # two camera parameters + K1 = (1500, 0, 0, 640, 480) + K2 = (1600, 0, 0, 650, 440) + + measurements, cameras = self.generate_measurements(Cal3Bundler, + PinholeCameraCal3Bundler, + (K1, K2), + camera_set=CameraSetCal3Bundler) + + triangulated_landmark = gtsam.triangulatePoint3(cameras, + measurements, + rank_tol=1e-9, + optimize=True) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) - triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) - self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2) - # - # # two Poses with Bundler Calibration - # bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480) - # camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal) - # camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal) - # - # z1 = camera1.project(landmark) - # z2 = camera2.project(landmark) - # - # measurements = Point2Vector() - # measurements.append(z1) - # measurements.append(z2) - # - # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize) - # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) if __name__ == "__main__": unittest.main()