diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index b3d1be4b6..12635abdd 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -24,93 +24,52 @@ namespace gtsam { using namespace std; -/* ************************************************************************* */ -Cal3_S2::Cal3_S2(double fov, int w, int h) : - s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) { - double a = fov * M_PI / 360.0; // fov/2 in radians - fx_ = (double) w / (2.0 * tan(a)); // old formula: fx_ = (double) w * tan(a); - fy_ = fx_; -} - -/* ************************************************************************* */ -Cal3_S2::Cal3_S2(const std::string &path) : - fx_(320), fy_(320), s_(0), u0_(320), v0_(140) { - - char buffer[200]; - buffer[0] = 0; - sprintf(buffer, "%s/calibration_info.txt", path.c_str()); - std::ifstream infile(buffer, std::ios::in); - - if (infile) - infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; - else { - throw std::runtime_error("Cal3_S2: Unable to load the calibration"); - } - - infile.close(); -} - /* ************************************************************************* */ ostream& operator<<(ostream& os, const Cal3_S2& cal) { - os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px() - << ", py:" << cal.py() << "}"; + os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() + << ", px:" << cal.px() << ", py:" << cal.py() << "}"; return os; } /* ************************************************************************* */ void Cal3_S2::print(const std::string& s) const { - gtsam::print((Matrix)matrix(), s); + gtsam::print((Matrix)K(), s); } /* ************************************************************************* */ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { - if (std::abs(fx_ - K.fx_) > tol) - return false; - if (std::abs(fy_ - K.fy_) > tol) - return false; - if (std::abs(s_ - K.s_) > tol) - return false; - if (std::abs(u0_ - K.u0_) > tol) - return false; - if (std::abs(v0_ - K.v0_) > tol) - return false; - return true; + return Cal3::equals(K, tol); } /* ************************************************************************* */ Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, - OptionalJacobian<2, 2> Dp) const { + OptionalJacobian<2, 2> Dp) const { const double x = p.x(), y = p.y(); - if (Dcal) - *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - if (Dp) - *Dp << fx_, s_, 0.0, fy_; + if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; + if (Dp) *Dp << fx_, s_, 0.0, fy_; return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } /* ************************************************************************* */ -Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal, - OptionalJacobian<2,2> Dp) const { - const double u = p.x(), v = p.y(); - double delta_u = u - u0_, delta_v = v - v0_; - double inv_fx = 1/ fx_, inv_fy = 1/fy_; - double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy; - Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), - inv_fy_delta_v); - if(Dcal) - *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, -inv_fx * point.y(), - -inv_fx, inv_fx_s_inv_fy, - 0, -inv_fy * point.y(), 0, 0, -inv_fy; - if(Dp) - *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy; - return point; +Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) const { + const double u = p.x(), v = p.y(); + double delta_u = u - u0_, delta_v = v - v0_; + double inv_fx = 1 / fx_, inv_fy = 1 / fy_; + double inv_fy_delta_v = inv_fy * delta_v, + inv_fx_s_inv_fy = inv_fx * s_ * inv_fy; + Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v); + if (Dcal) + *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, + -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(), + 0, 0, -inv_fy; + if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy; + return point; } /* ************************************************************************* */ -Vector3 Cal3_S2::calibrate(const Vector3& p) const { - return matrix_inverse() * p; -} +Vector3 Cal3_S2::calibrate(const Vector3& p) const { return matrix_inverse() * p; } /* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index f2848d0a3..aef27323e 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -21,6 +21,7 @@ #pragma once +#include #include namespace gtsam { @@ -30,31 +31,24 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3_S2 { -private: - double fx_, fy_, s_, u0_, v0_; - -public: +class GTSAM_EXPORT Cal3_S2 : public Cal3 { + public: enum { dimension = 5 }; - typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object + typedef boost::shared_ptr + shared_ptr; ///< shared pointer to calibration object /// @name Standard Constructors /// @{ /// Create a default calibration that leaves coordinates unchanged - Cal3_S2() : - fx_(1), fy_(1), s_(0), u0_(0), v0_(0) { - } + Cal3_S2() : Cal3() {} /// constructor from doubles - Cal3_S2(double fx, double fy, double s, double u0, double v0) : - fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) { - } + Cal3_S2(double fx, double fy, double s, double u0, double v0) + : Cal3(fx, fy, s, u0, v0) {} /// constructor from vector - Cal3_S2(const Vector &d) : - fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) { - } + Cal3_S2(const Vector& d) : Cal3(d) {} /** * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect @@ -62,132 +56,59 @@ public: * @param w image width * @param h image height */ - Cal3_S2(double fov, int w, int h); + Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {} - /// @} - /// @name Advanced Constructors - /// @{ + /** + * Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves + * @param p point in intrinsic coordinates + * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; - /// load calibration from location (default name is calibration_info.txt) - Cal3_S2(const std::string &path); + /** + * Convert image coordinates uv to intrinsic coordinates xy + * @param p point in image coordinates + * @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in intrinsic coordinates + */ + Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; + + /** + * Convert homogeneous image coordinates to intrinsic coordinates + * @param p point in image coordinates + * @return point in intrinsic coordinates + */ + Vector3 calibrate(const Vector3& p) const; /// @} /// @name Testable /// @{ /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal); + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Cal3_S2& cal); /// print with optional string - void print(const std::string& s = "Cal3_S2") const; + void print(const std::string& s = "Cal3_S2") const override; /// Check if equal up to specified tolerance bool equals(const Cal3_S2& K, double tol = 10e-9) const; - /// @} - /// @name Standard Interface - /// @{ - - /// focal length x - inline double fx() const { - return fx_; - } - - /// focal length y - inline double fy() const { - return fy_; - } - - /// aspect ratio - inline double aspectRatio() const { - return fx_/fy_; - } - - /// skew - inline double skew() const { - return s_; - } - - /// image center in x - inline double px() const { - return u0_; - } - - /// image center in y - inline double py() const { - return v0_; - } - - /// return the principal point - Point2 principalPoint() const { - return Point2(u0_, v0_); - } - - /// vectorized form (column-wise) - Vector5 vector() const { - Vector5 v; - v << fx_, fy_, s_, u0_, v0_; - return v; - } - - /// return calibration matrix K - Matrix3 K() const { - Matrix3 K; - K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; - return K; - } - - /** @deprecated The following function has been deprecated, use K above */ - Matrix3 matrix() const { - return K(); - } - - /// return inverted calibration matrix inv(K) - Matrix3 matrix_inverse() const { - const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; - Matrix3 K_inverse; - K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, - 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0; - return K_inverse; - } - - /** - * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves - * @param p point in intrinsic coordinates - * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, - OptionalJacobian<2,2> Dp = boost::none) const; - - /** - * convert image coordinates uv to intrinsic coordinates xy - * @param p point in image coordinates - * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in intrinsic coordinates - */ - Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, - OptionalJacobian<2,2> Dp = boost::none) const; - - /** - * convert homogeneous image coordinates to intrinsic coordinates - * @param p point in image coordinates - * @return point in intrinsic coordinates - */ - Vector3 calibrate(const Vector3& p) const; - /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) inline Cal3_S2 between(const Cal3_S2& q, - OptionalJacobian<5,5> H1=boost::none, - OptionalJacobian<5,5> H2=boost::none) const { - if(H1) *H1 = -I_5x5; - if(H2) *H2 = I_5x5; - return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); + OptionalJacobian<5, 5> H1 = boost::none, + OptionalJacobian<5, 5> H2 = boost::none) const { + if (H1) *H1 = -I_5x5; + if (H2) *H2 = I_5x5; + return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_, + q.v0_ - v0_); } - /// @} /// @name Manifold /// @{ @@ -212,27 +133,22 @@ public: /// @name Advanced Interface /// @{ -private: - + private: /// Serialization function friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(fx_); - ar & BOOST_SERIALIZATION_NVP(fy_); - ar & BOOST_SERIALIZATION_NVP(s_); - ar & BOOST_SERIALIZATION_NVP(u0_); - ar & BOOST_SERIALIZATION_NVP(v0_); + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Cal3_S2", boost::serialization::base_object(*this)); } /// @} - }; -template<> +template <> struct traits : public internal::Manifold {}; -template<> +template <> struct traits : public internal::Manifold {}; -} // \ namespace gtsam +} // \ namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp index 9b5aea4ed..b4e70202e 100644 --- a/gtsam/geometry/Cal3_S2Stereo.cpp +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -24,16 +24,17 @@ using namespace std; /* ************************************************************************* */ void Cal3_S2Stereo::print(const std::string& s) const { - K_.print(s+"K: "); - std::cout << s << "Baseline: " << b_ << std::endl; - } + std::cout << s << (s != "" ? " " : ""); + print("K: "); + std::cout << "Baseline: " << b_ << std::endl; +} /* ************************************************************************* */ bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { - if (std::abs(b_ - other.b_) > tol) return false; - return K_.equals(other.K_,tol); + const Cal3_S2* base = dynamic_cast(&other); + return Cal3_S2::equals(*base, tol) && (std::abs(b_ - other.baseline()) < tol); } /* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index a6eb41b60..258cd0434 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -27,43 +27,40 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Cal3_S2Stereo { + class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { private: - - Cal3_S2 K_; double b_; public: enum { dimension = 6 }; - typedef boost::shared_ptr shared_ptr; ///< shared pointer to stereo calibration object + ///< shared pointer to stereo calibration object + typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @ /// default calibration leaves coordinates unchanged - Cal3_S2Stereo() : - K_(1, 1, 0, 0, 0), b_(1.0) { - } + Cal3_S2Stereo() : Cal3_S2(1, 1, 0, 0, 0), b_(1.0) {} /// constructor from doubles - Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) : - K_(fx, fy, s, u0, v0), b_(b) { - } + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, + double b) + : Cal3_S2(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)){} + Cal3_S2Stereo(const Vector& d) + : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {} /// easy constructor; field-of-view in degrees, assumes zero skew - Cal3_S2Stereo(double fov, int w, int h, double b) : - K_(fov, w, h), b_(b) { - } + Cal3_S2Stereo(double fov, int w, int h, double b) + : Cal3_S2(fov, w, h), b_(b) {} /// @} /// @name Testable /// @{ - void print(const std::string& s = "") const; + void print(const std::string& s = "") const override; /// Check if equal up to specified tolerance bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; @@ -73,28 +70,10 @@ namespace gtsam { /// @{ /// return calibration, same for left and right - const Cal3_S2& calibration() const { return K_;} + const Cal3_S2& calibration() const { return *this; } /// return calibration matrix K, same for left and right - Matrix matrix() const { return K_.matrix();} - - /// focal length x - inline double fx() const { return K_.fx();} - - /// focal length x - inline double fy() const { return K_.fy();} - - /// skew - inline double skew() const { return K_.skew();} - - /// image center in x - inline double px() const { return K_.px();} - - /// image center in y - inline double py() const { return K_.py();} - - /// return the principal point - Point2 principalPoint() const { return K_.principalPoint();} + Matrix3 K() const { return K(); } /// return baseline inline double baseline() const { return b_; } @@ -102,7 +81,7 @@ namespace gtsam { /// vectorized form (column-wise) Vector6 vector() const { Vector6 v; - v << K_.vector(), b_; + v << vector(), b_; return v; } @@ -118,7 +97,8 @@ namespace gtsam { /// Given 6-dim tangent vector, create new calibration inline Cal3_S2Stereo retract(const Vector& d) const { - return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5)); + return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3), + py() + d(4), b_ + d(5)); } /// Unretraction for the calibration @@ -137,8 +117,9 @@ namespace gtsam { template void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(K_); - ar & BOOST_SERIALIZATION_NVP(b_); + ar& boost::serialization::make_nvp( + "Cal3_S2", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(b_); } /// @} diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 55ea32e32..f7c8fb6b6 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -125,7 +125,6 @@ TEST(Cal3_S2, between) { EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2))); EXPECT(assert_equal(-I_5x5, H1)); EXPECT(assert_equal(I_5x5, H2)); - } /* ************************************************************************* */