Merged in feature/wrap_triangulation (pull request #134)

Triangulation now available in MATLAB

gtsam.h

@@ -288,6 +288,32 @@ class Point2 {
   void serialize() const;
 };
 
+// std::vector<gtsam::Point2>
+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();
@@ -357,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();
@@ -550,6 +581,16 @@ class Pose3 {
   void serialize() const;
 };
 
+// std::vector<gtsam::Pose3>
+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 <gtsam/geometry/Unit3.h>
 class Unit3 {
   // Standard Constructors
@@ -788,56 +829,16 @@ class CalibratedCamera {
   void serialize() const;
 };
 
-class SimpleCamera {
+template<CALIBRATION>
-  // 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<gtsam::Point2,bool> 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<CALIBRATION = {gtsam::Cal3DS2}>
 class PinholeCamera {
   // Standard Constructors and Named Constructors
   PinholeCamera();
   PinholeCamera(const gtsam::Pose3& pose);
-  PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K);
+  PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
-  static This Level(const gtsam::Cal3DS2& K,
+  static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height);
-    const gtsam::Pose2& pose, double height);
   static This Level(const gtsam::Pose2& pose, double height);
-  static This Lookat(const gtsam::Point3& eye,
+  static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
-    const gtsam::Point3& target, const gtsam::Point3& upVector,
+      const gtsam::Point3& upVector, const CALIBRATION& K);
-    const gtsam::Cal3DS2& K);
 
   // Testable
   void print(string s) const;
@@ -865,6 +866,13 @@ class PinholeCamera {
   void serialize() const;
 };
 
+// Do typedefs here so we can also define SimpleCamera
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> SimpleCamera;
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+
 class StereoCamera {
   // Standard Constructors and Named Constructors
   StereoCamera();
@@ -893,6 +901,16 @@ class StereoCamera {
   void serialize() const;
 };
 
+#include <gtsam/geometry/triangulation.h>
+
+// 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
 //*************************************************************************
@@ -2176,9 +2194,6 @@ class NonlinearISAM {
 //*************************************************************************
 // Nonlinear factor types
 //*************************************************************************
-#include <gtsam/geometry/Cal3_S2.h>
-#include <gtsam/geometry/Cal3DS2.h>
-#include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/StereoPoint2.h>

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

gtsam/geometry/Point2.h

@@ -192,9 +192,11 @@ private:
   }
 
   /// @}
-
 };
 
+// For MATLAB wrapper
+typedef std::vector<Point2> Point2Vector;
+
 /// multiply with scalar
 inline Point2 operator*(double s, const Point2& p) {return p*s;}
 

gtsam/geometry/Point3.h

@@ -189,15 +189,17 @@ namespace gtsam {
     }
 
     /// @}
-
   };
 
-  /// Syntactic sugar for multiplying coordinates by a scalar s*p
+/// Syntactic sugar for multiplying coordinates by a scalar s*p
-  inline Point3 operator*(double s, const Point3& p) { return p*s;}
+inline Point3 operator*(double s, const Point3& p) { return p*s;}
 
-  template<>
+// For MATLAB wrapper
-  struct traits<Point3> : public internal::VectorSpace<Point3> {};
+typedef boost::optional<Point3> OptionalPoint3;
 
- template<>
+template<>
-  struct traits<const Point3> : public internal::VectorSpace<Point3> {};
+struct traits<Point3> : public internal::VectorSpace<Point3> {};
+
+template<>
+struct traits<const Point3> : public internal::VectorSpace<Point3> {};
 }

gtsam/geometry/Pose3.h

@@ -322,6 +322,9 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
 typedef std::pair<Point3, Point3> Point3Pair;
 GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
 
+// For MATLAB wrapper
+typedef std::vector<Pose3> Pose3Vector;
+
 template<>
 struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {};
 

gtsam_unstable/geometry/SimPolygon2D.h

@@ -128,7 +128,6 @@ public:
 };
 
 typedef std::vector<SimPolygon2D> SimPolygon2DVector;
-typedef std::vector<Point2> Point2Vector;
 
 } //\namespace gtsam
 

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<gtsam::Point2>
-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 <gtsam_unstable/geometry/SimWall2D.h>
 class SimWall2D {
   SimWall2D();

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));