Merged from branch 'trunk'

gtsam.h

@@ -317,6 +317,9 @@ virtual class StereoPoint2 : gtsam::Value {
 
   // Standard Interface
   Vector vector() const;
+  double uL() const;
+  double uR() const;
+  double v() const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -637,10 +640,11 @@ class Cal3_S2Stereo {
   // Standard Constructors
   Cal3_S2Stereo();
   Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
+  Cal3_S2Stereo(Vector v);
 
   // Testable
   void print(string s) const;
-  bool equals(const gtsam::Cal3_S2Stereo& pose, double tol) const;
+  bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
 
   // Standard Interface
   double fx() const;
@@ -762,6 +766,34 @@ virtual class PinholeCamera : gtsam::Value {
   void serialize() const;
 };
 
+virtual class StereoCamera : gtsam::Value {
+  // Standard Constructors and Named Constructors
+  StereoCamera();
+  StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
+
+  // Testable
+  void print(string s) const;
+  bool equals(const gtsam::StereoCamera& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  double baseline() const;
+  gtsam::Cal3_S2Stereo* calibration() const;
+
+  // Manifold
+  gtsam::StereoCamera retract(const Vector& d) const;
+  Vector localCoordinates(const gtsam::StereoCamera& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  gtsam::StereoPoint2 project(const gtsam::Point3& point);
+  gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
 //*************************************************************************
 // inference
 //*************************************************************************

gtsam/geometry/Cal3_S2.h

@@ -52,6 +52,7 @@ namespace gtsam {
       fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
     }
 
+    /// constructor from vector
     Cal3_S2(const Vector &d): fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)){}
 
 

gtsam/geometry/Cal3_S2Stereo.h

@@ -49,6 +49,9 @@ namespace gtsam {
       K_(fx, fy, s, u0, v0), b_(b) {
     }
 
+    /// constructor from vector
+    Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
+
     /// @}
     /// @name Testable
     /// @{

gtsam/geometry/StereoPoint2.h

@@ -128,9 +128,18 @@ namespace gtsam {
     /// @name Standard Interface
     /// @{
 
+    /// get uL
+    inline double uL() const {return uL_;}
+
+    /// get uR
+    inline double uR() const {return uR_;}
+
+    /// get v
+    inline double v() const {return v_;}
+
     /** convert to vector */
-    Vector vector() const {
+    Vector3 vector() const {
-      return Vector_(3, uL_, uR_, v_);
+      return Vector3(uL_, uR_, v_);
     }
 
     /** convenient function to get a Point2 from the left image */

wrap/CMakeLists.txt

@@ -1,6 +1,6 @@
 # Build/install Wrap
 
-set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY})
+set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_REGEX_LIBRARY})
 
 # Build the executable itself
 file(GLOB wrap_srcs "*.cpp")