diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 006095420..88ffbcdbd 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -28,10 +28,11 @@ namespace gtsam { class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error { public: - StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {} + StereoCheiralityException() : + std::runtime_error("Stereo Cheirality Exception") { + } }; - /** * A stereo camera class, parameterize by left camera pose and stereo calibration * @addtogroup geometry @@ -52,16 +53,22 @@ private: public: - enum { dimension = 6 }; + enum { + dimension = 6 + }; /// @name Standard Constructors /// @{ - StereoCamera() { + /// Default constructor allocates a calibration! + StereoCamera() : + K_(new Cal3_S2Stereo()) { } + /// Construct from pose and shared calibration StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); + /// Return shared pointer to calibration const Cal3_S2Stereo::shared_ptr calibration() const { return K_; } @@ -70,26 +77,28 @@ public: /// @name Testable /// @{ + /// print void print(const std::string& s = "") const { leftCamPose_.print(s + ".camera."); K_->print(s + ".calibration."); } + /// equals bool equals(const StereoCamera &camera, double tol = 1e-9) const { - return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals( - *camera.K_, tol); + return leftCamPose_.equals(camera.leftCamPose_, tol) + && K_->equals(*camera.K_, tol); } /// @} /// @name Manifold /// @{ - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space inline size_t dim() const { return 6; } - /** Dimensionality of the tangent space */ + /// Dimensionality of the tangent space static inline size_t Dim() { return 6; } @@ -108,10 +117,12 @@ public: /// @name Standard Interface /// @{ + /// pose const Pose3& pose() const { return leftCamPose_; } + /// baseline double baseline() const { return K_->baseline(); } @@ -122,19 +133,16 @@ public: * @param H2 derivative with respect to point * @param H3 IGNORED (for calibration) */ - StereoPoint2 project(const Point3& point, - OptionalJacobian<3, 6> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none, + StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 = + boost::none, OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = boost::none) const; - /** - * - */ + /// back-project a measurement Point3 backproject(const StereoPoint2& z) const { Vector measured = z.vector(); - double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); - double X = Z *(measured[0]- K_->px()) / K_->fx(); - double Y = Z *(measured[2]- K_->py()) / K_->fy(); + double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); + double X = Z * (measured[0] - K_->px()) / K_->fx(); + double Y = Z * (measured[2] - K_->py()) / K_->fy(); Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); return world_point; } @@ -142,7 +150,8 @@ public: /// @} private: - /** utility functions */ + + /// utility function Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; friend class boost::serialization::access; @@ -155,8 +164,10 @@ private: }; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; template<> -struct traits : public internal::Manifold {}; +struct traits : public internal::Manifold { +}; }