diff --git a/gtsam.h b/gtsam.h index ee069e0d3..a28bade6a 100644 --- a/gtsam.h +++ b/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 //************************************************************************* diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index b8cfda9ba..e3ae8859d 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/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)){} diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 58fa1cd2d..811264967 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/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 /// @{ diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 470c3496f..acaad08d7 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/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 { - return Vector_(3, uL_, uR_, v_); + Vector3 vector() const { + return Vector3(uL_, uR_, v_); } /** convenient function to get a Point2 from the left image */ diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 6c1cf8d4b..399600b7e 100644 --- a/wrap/CMakeLists.txt +++ b/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")