From 02e94730a634a56d411ccbaf75bbbcf267bdb9cf Mon Sep 17 00:00:00 2001 From: Sushmita Date: Fri, 27 Nov 2020 00:14:52 -0500 Subject: [PATCH] vector of cameras and triangulation function wrapped --- gtsam/geometry/triangulation.h | 5 ++ gtsam/gtsam.i | 34 +++++++++- python/CMakeLists.txt | 2 + python/gtsam/preamble.h | 2 + python/gtsam/specializations.h | 2 + python/gtsam/tests/test_Triangulation.py | 83 ++++++++++++++++++++---- 6 files changed, 116 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 6f6c645b8..01daab361 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -24,6 +24,8 @@ #include #include #include +#include +#include namespace gtsam { @@ -494,5 +496,8 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, } } +typedef CameraSet> CameraSetCal3Bundler; +typedef CameraSet> CameraSetCal3_S2; + } // \namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 880b1d4c7..47a09648a 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1108,6 +1108,32 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +class CameraSetCal3Bundler { + CameraSetCal3Bundler(); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::PinholeCameraCal3Bundler at(size_t i) const; + void push_back(gtsam::PinholeCameraCal3Bundler& cam) const; +}; + +class CameraSetCal3_S2 { + CameraSetCal3_S2(); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::PinholeCameraCal3_S2 at(size_t i) const; + void push_back(gtsam::PinholeCameraCal3_S2& cam) const; +}; + #include class StereoCamera { // Standard Constructors and Named Constructors @@ -1149,7 +1175,13 @@ 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..a318a483b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -38,6 +38,8 @@ set(ignore gtsam::Pose3Vector gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 + gtsam::CameraSetCal3Bundler + gtsam::CameraSetCal3_S2 gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_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..c358152ae 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -15,21 +15,24 @@ import numpy as np import gtsam as g from gtsam.utils.test_case import GtsamTestCase from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ - PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3 + PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3, CameraSetCal3_S2, CameraSetCal3Bundler, PinholeCameraCal3Bundler class TestVisualISAMExample(GtsamTestCase): - def test_TriangulationExample(self): - # Some common constants - sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) - + 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) + self.pose1 = Pose3(upright, Point3(0, 0, 1)) # 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) + self.pose2 = self.pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) + + + def test_TriangulationExample(self): + # Some common constants + sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) + camera1 = PinholeCameraCal3_S2(self.pose1, sharedCal) + camera2 = PinholeCameraCal3_S2(self.pose2, sharedCal) # landmark ~5 meters infront of camera landmark = Point3(5, 0.5, 1.2) @@ -42,8 +45,8 @@ class TestVisualISAMExample(GtsamTestCase): poses = Pose3Vector() measurements = Point2Vector() - poses.append(pose1) - poses.append(pose2) + poses.append(self.pose1) + poses.append(self.pose2) measurements.append(z1) measurements.append(z2) @@ -76,5 +79,63 @@ class TestVisualISAMExample(GtsamTestCase): # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize) # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) + def test_distinct_Ks(self): + K1 = Cal3_S2(1500, 1200, 0, 640, 480) + camera1 = PinholeCameraCal3_S2(self.pose1, K1) + + K2 = Cal3_S2(1600, 1300, 0, 650, 440) + camera2 = PinholeCameraCal3_S2(self.pose2, K2) + + # landmark ~5 meters infront of camera + landmark = Point3(5, 0.5, 1.2) + + # 1. Project two landmarks into two cameras and triangulate + z1 = camera1.project(landmark) + z2 = camera2.project(landmark) + # two cameras + measurements = Point2Vector() + cameras = CameraSetCal3_S2() + + measurements.append(z1) + measurements.append(z2) + cameras.append(camera1) + cameras.append(camera2) + + optimize = True + rank_tol = 1e-9 + + triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) + self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-2) + + def test_distinct_Ks_Bundler(self): + K1 = Cal3Bundler(1500, 0, 0, 640, 480) + camera1 = PinholeCameraCal3Bundler(self.pose1, K1) + + K2 = Cal3Bundler(1500, 0, 0, 640, 480) + camera2 = PinholeCameraCal3Bundler(self.pose2, K2) + + # landmark ~5 meters infront of camera + landmark = Point3(5, 0.5, 1.2) + + # 1. Project two landmarks into two cameras and triangulate + z1 = camera1.project(landmark) + z2 = camera2.project(landmark) + # two cameras + measurements = Point2Vector() + cameras = CameraSetCal3Bundler() + + measurements.append(z1) + measurements.append(z2) + cameras.append(camera1) + cameras.append(camera2) + + optimize = True + rank_tol = 1e-9 + + triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) + self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-2) + + + if __name__ == "__main__": unittest.main()