From 13dcc977f20bbedea2ca564ed5ee4065e0f2ea41 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 3 May 2015 17:31:08 -0700 Subject: [PATCH 1/7] Moved Point2Vector wrapper from gtsam_unstable.h to gtsam.h --- gtsam.h | 26 +++++++++++++++++++++++++ gtsam/geometry/Point2.h | 4 +++- gtsam_unstable/geometry/SimPolygon2D.h | 1 - gtsam_unstable/gtsam_unstable.h | 27 +------------------------- 4 files changed, 30 insertions(+), 28 deletions(-) diff --git a/gtsam.h b/gtsam.h index 1aee996dc..f6a2ed631 100644 --- a/gtsam.h +++ b/gtsam.h @@ -288,6 +288,32 @@ class Point2 { void serialize() const; }; +// std::vector +class Point2Vector +{ + // Constructors + Point2Vector(); + Point2Vector(const gtsam::Point2Vector& v); + + //Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + //Element access + gtsam::Point2 at(size_t n) const; + gtsam::Point2 front() const; + gtsam::Point2 back() const; + + //Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; + class StereoPoint2 { // Standard Constructors StereoPoint2(); diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index b6f1eca4f..5e8b0695c 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -192,9 +192,11 @@ private: } /// @} - }; +// For MATLAB wrapper +typedef std::vector Point2Vector; + /// multiply with scalar inline Point2 operator*(double s, const Point2& p) {return p*s;} diff --git a/gtsam_unstable/geometry/SimPolygon2D.h b/gtsam_unstable/geometry/SimPolygon2D.h index 02892c519..835bb4083 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.h +++ b/gtsam_unstable/geometry/SimPolygon2D.h @@ -128,7 +128,6 @@ public: }; typedef std::vector SimPolygon2DVector; -typedef std::vector Point2Vector; } //\namespace gtsam diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 9aa32e1c3..6b57bfcd0 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -8,6 +8,7 @@ class gtsam::Vector6; class gtsam::LieScalar; class gtsam::LieVector; class gtsam::Point2; +class gtsam::Point2Vector; class gtsam::Rot2; class gtsam::Pose2; class gtsam::Point3; @@ -159,32 +160,6 @@ class BearingS2 { void serializable() const; // enabling serialization functionality }; -// std::vector -class Point2Vector -{ - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); - - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; - - //Modifiers - void assign(size_t n, const gtsam::Point2& u); - void push_back(const gtsam::Point2& x); - void pop_back(); -}; - #include class SimWall2D { SimWall2D(); From 30a36595d452da0c6f71d0f65b3999accc1eec89 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 3 May 2015 17:31:46 -0700 Subject: [PATCH 2/7] Formatting only --- gtsam/geometry/Point3.h | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7465b0582..883e5fb62 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -189,15 +189,14 @@ namespace gtsam { } /// @} - }; - /// Syntactic sugar for multiplying coordinates by a scalar s*p - inline Point3 operator*(double s, const Point3& p) { return p*s;} +/// Syntactic sugar for multiplying coordinates by a scalar s*p +inline Point3 operator*(double s, const Point3& p) { return p*s;} - template<> - struct traits : public internal::VectorSpace {}; +template<> +struct traits : public internal::VectorSpace {}; - template<> - struct traits : public internal::VectorSpace {}; +template<> +struct traits : public internal::VectorSpace {}; } From 39deb923146bf61ed686b5dbdc592e7eb69e2f34 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 3 May 2015 17:32:00 -0700 Subject: [PATCH 3/7] Fixed Spelling mistake --- gtsam/geometry/OrientedPlane3.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 28b67cb4e..dad283760 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -62,7 +62,7 @@ public: d_ = d; } - /// The print fuction + /// The print function void print(const std::string& s = std::string()) const; /// The equals function with tolerance From cd077c336df1e7050082c2ebf09604ff2b9cf316 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 3 May 2015 17:32:13 -0700 Subject: [PATCH 4/7] Added Pose3Vector --- gtsam.h | 10 ++++++++++ gtsam/geometry/Pose3.h | 3 +++ 2 files changed, 13 insertions(+) diff --git a/gtsam.h b/gtsam.h index f6a2ed631..282a55a7d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -576,6 +576,16 @@ class Pose3 { void serialize() const; }; +// std::vector +class Pose3Vector +{ + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& x); +}; + #include class Unit3 { // Standard Constructors diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 4e529ea98..1ea8e8d5c 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -322,6 +322,9 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); +// For MATLAB wrapper +typedef std::vector Pose3Vector; + template<> struct traits : public internal::LieGroupTraits {}; From 3299127e6a24ed9a0fc2bbe0ee65cd2a3e6de2c5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 3 May 2015 18:21:11 -0700 Subject: [PATCH 5/7] Optional point --- gtsam.h | 5 +++++ gtsam/geometry/Point3.h | 3 +++ 2 files changed, 8 insertions(+) diff --git a/gtsam.h b/gtsam.h index 282a55a7d..ebc308ff8 100644 --- a/gtsam.h +++ b/gtsam.h @@ -383,6 +383,11 @@ class Point3 { void serialize() const; }; +class OptionalPoint3 { + bool is_initialized() const; + gtsam::Point3 value(); +}; + class Rot2 { // Standard Constructors and Named Constructors Rot2(); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 883e5fb62..db6fa4b6e 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -194,6 +194,9 @@ namespace gtsam { /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} +// For MATLAB wrapper +typedef boost::optional OptionalPoint3; + template<> struct traits : public internal::VectorSpace {}; From 67cf13ad74b91c6ff9068545a57fddca875245bc Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 3 May 2015 18:32:28 -0700 Subject: [PATCH 6/7] Fixed errors in PinholeCamera wrapping and removed SimpleCamera (made it a simple typedef) --- gtsam.h | 60 ++++++++++++--------------------------------------------- 1 file changed, 12 insertions(+), 48 deletions(-) diff --git a/gtsam.h b/gtsam.h index ebc308ff8..c56071c8f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -829,56 +829,16 @@ class CalibratedCamera { void serialize() const; }; -class SimpleCamera { - // Standard Constructors and Named Constructors - SimpleCamera(); - SimpleCamera(const gtsam::Pose3& pose); - SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, - const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, - const gtsam::Point3& target, const gtsam::Point3& upVector, - const gtsam::Cal3_S2& K); - - // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - gtsam::Cal3_S2 calibration(); - - // Manifold - gtsam::SimpleCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::SimpleCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); - - // enabling serialization functionality - void serialize() const; -}; - -template +template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); PinholeCamera(const gtsam::Pose3& pose); - PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K); - static This Level(const gtsam::Cal3DS2& K, - const gtsam::Pose2& pose, double height); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); static This Level(const gtsam::Pose2& pose, double height); - static This Lookat(const gtsam::Point3& eye, - const gtsam::Point3& target, const gtsam::Point3& upVector, - const gtsam::Cal3DS2& K); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION& K); // Testable void print(string s) const; @@ -906,6 +866,13 @@ class PinholeCamera { void serialize() const; }; +// Do typedefs here so we can also define SimpleCamera +typedef gtsam::PinholeCamera SimpleCamera; +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; + class StereoCamera { // Standard Constructors and Named Constructors StereoCamera(); @@ -2217,9 +2184,6 @@ class NonlinearISAM { //************************************************************************* // Nonlinear factor types //************************************************************************* -#include -#include -#include #include #include #include From cd77ec8fd4e1c03fc207b7ef1fe092dbcb69804f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 3 May 2015 18:54:17 -0700 Subject: [PATCH 7/7] Added triangulation wrapping, tested and works in MATLAB ! --- gtsam.h | 10 ++++ matlab/gtsam_tests/testTriangulation.m | 70 ++++++++++++++++++++++++++ 2 files changed, 80 insertions(+) create mode 100644 matlab/gtsam_tests/testTriangulation.m diff --git a/gtsam.h b/gtsam.h index c56071c8f..6acc493fd 100644 --- a/gtsam.h +++ b/gtsam.h @@ -901,6 +901,16 @@ class StereoCamera { void serialize() const; }; +#include + +// Templates appear not yet supported for free functions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); + //************************************************************************* // Symbolic //************************************************************************* diff --git a/matlab/gtsam_tests/testTriangulation.m b/matlab/gtsam_tests/testTriangulation.m new file mode 100644 index 000000000..d46493328 --- /dev/null +++ b/matlab/gtsam_tests/testTriangulation.m @@ -0,0 +1,70 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Test triangulation +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Some common constants +sharedCal = Cal3_S2(1500, 1200, 0, 640, 480); + +%% Looking along X-axis, 1 meter above ground plane (x-y) +upright = Rot3.Ypr(-pi / 2, 0., -pi / 2); +pose1 = Pose3(upright, Point3(0, 0, 1)); +camera1 = SimpleCamera(pose1, sharedCal); + +%% create second camera 1 meter to the right of first camera +pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))); +camera2 = SimpleCamera(pose2, sharedCal); + +%% 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); + +%% twoPoses +poses = Pose3Vector; +measurements = Point2Vector; + +poses.push_back(pose1); +poses.push_back(pose2); +measurements.push_back(z1); +measurements.push_back(z2); + +optimize = true; +rank_tol = 1e-9; + +triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9)); + +%% 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) +measurements = Point2Vector; +measurements.push_back(z1.retract([0.1;0.5])); +measurements.push_back(z2.retract([-0.2;0.3])); + +triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(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.push_back(z1); +measurements.push_back(z2); + +triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize); +CHECK('triangulated_landmark',landmark.equals(triangulated_landmark,1e-9));