diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 7bbafd7d5..8282800c1 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -26,8 +26,10 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Cal3_S2Stereo: public Cal3_S2 { + class Cal3_S2Stereo { private: + + Cal3_S2 K_; double b_; public: @@ -39,12 +41,12 @@ namespace gtsam { /// default calibration leaves coordinates unchanged Cal3_S2Stereo() : - Cal3_S2(1, 1, 0, 0, 0), b_(1.0) { + K_(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) : - Cal3_S2(fx, fy, s, u0, v0), b_(b) { + K_(fx, fy, s, u0, v0), b_(b) { } /// @} @@ -52,23 +54,41 @@ namespace gtsam { /// @{ void print(const std::string& s = "") const { - gtsam::print(matrix(), s); - std::cout << "Baseline: " << b_ << std::endl; + K_.print(s+"K: "); + std::cout << s << "Baseline: " << b_ << std::endl; } - /// @} + /// Check if equal up to specified tolerance + bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const { + if (fabs(b_ - other.b_) > tol) return false; + return K_.equals(other.K_,tol); + } + + /// @} /// @name Standard Interface /// @{ - //TODO: remove? -// /** -// * Check if equal up to specified tolerance -// */ -// bool equals(const Cal3_S2& K, double tol = 10e-9) const; + /// 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();} - ///TODO: comment + /// 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(); + } + + /// return baseline inline double baseline() const { return b_; } /// @} @@ -81,10 +101,8 @@ namespace gtsam { template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3_S2Stereo", - boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(b_); - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Cal3_S2); } /// @} diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index f5e07e66c..4c278fab9 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -71,16 +71,6 @@ public: return project(point, H1, H2); } - /* - * backproject using left camera calibration, up to scale only - * i.e. does not rely on baseline - */ - Point3 backproject(const Point2& projection, const double scale) const { - Point2 intrinsic = K_->calibrate(projection); - Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);; - return pose().transform_from(cameraPoint); - } - Point3 backproject(const StereoPoint2& z) const { Vector measured = z.vector(); double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);