diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index bb8ea59c8..9c7561f61 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -25,6 +25,7 @@ namespace gtsam { /** * @brief Calibration used by Bundler * @ingroup geometry + * \nosubgrouping */ class Cal3Bundler { @@ -33,31 +34,77 @@ private: public: - Cal3Bundler() ; - Cal3Bundler(const Vector &v) ; - Cal3Bundler(const double f, const double k1, const double k2) ; Matrix K() const ; Vector k() const ; Vector vector() const; + + /// @name Standard Constructors + /// @{ + + ///TODO: comment + Cal3Bundler() ; + + ///TODO: comment + Cal3Bundler(const double f, const double k1, const double k2) ; + + /// @} + /// @name Advanced Constructors + /// @{ + + ///TODO: comment + Cal3Bundler(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string void print(const std::string& s = "") const; + + /// assert equality up to a tolerance bool equals(const Cal3Bundler& K, double tol = 10e-9) const; + /// @} + /// @name Standard Interface + /// @{ + + ///TODO: comment Point2 uncalibrate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const ; + ///TODO: comment Matrix D2d_intrinsic(const Point2& p) const ; + + ///TODO: comment Matrix D2d_calibration(const Point2& p) const ; + + ///TODO: comment Matrix D2d_intrinsic_calibration(const Point2& p) const ; + /// @} + /// @name Manifold + /// @{ + + ///TODO: comment Cal3Bundler retract(const Vector& d) const ; + + ///TODO: comment Vector localCoordinates(const Cal3Bundler& T2) const ; - int dim() const { return 3 ; } - static size_t Dim() { return 3; } + ///TODO: comment + int dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + + ///TODO: comment + static size_t Dim() { return 3; } //TODO: make a final dimension variable private: + + /// @} + /// @name Advanced Interface + /// @{ + /** Serialization function */ friend class boost::serialization::access; template @@ -68,6 +115,9 @@ private: ar & BOOST_SERIALIZATION_NVP(k2_); } + + /// @} + }; } // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index dcb596d70..437a68a68 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -25,9 +25,12 @@ namespace gtsam { /** * @brief Calibration of a camera with radial distortion * @ingroup geometry + * \nosubgrouping */ class Cal3DS2 { + private: + double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double k1_, k2_ ; // radial 2nd-order and 4th-order double k3_, k4_ ; // tagential distortion @@ -39,36 +42,72 @@ private: // pi = K*pn public: - // Default Constructor with only unit focal length - Cal3DS2(); - - // Construction - Cal3DS2(double fx, double fy, double s, double u0, double v0, - double k1, double k2, double k3, double k4) ; - - Cal3DS2(const Vector &v) ; - Matrix K() const ; Vector k() const ; Vector vector() const ; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3DS2(); + + Cal3DS2(double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3, double k4) ; + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3DS2(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string void print(const std::string& s = "") const ; + + /// assert equality up to a tolerance bool equals(const Cal3DS2& K, double tol = 10e-9) const; + /// @} + /// @name Standard Interface + /// @{ + ///TODO: comment Point2 uncalibrate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const ; + ///TODO: comment Matrix D2d_intrinsic(const Point2& p) const ; + + ///TODO: comment Matrix D2d_calibration(const Point2& p) const ; + /// @} + /// @name Manifold + /// @{ + + ///TODO: comment Cal3DS2 retract(const Vector& d) const ; + + ///TODO: comment Vector localCoordinates(const Cal3DS2& T2) const ; - int dim() const { return 9 ; } - static size_t Dim() { return 9; } + ///TODO: comment + int dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + + ///TODO: comment + static size_t Dim() { return 9; } //TODO: make a final dimension variable private: + + /// @} + /// @name Advanced Interface + /// @{ + /** Serialization function */ friend class boost::serialization::access; template @@ -85,6 +124,10 @@ private: ar & BOOST_SERIALIZATION_NVP(k4_); } + + /// @} + }; + } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index e78b1d418..71adc58bc 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -28,12 +28,17 @@ namespace gtsam { /** * @brief The most common 5DOF 3D->2D calibration * @ingroup geometry + * \nosubgrouping */ class Cal3_S2 { private: double fx_, fy_, s_, u0_, v0_; + /// @name Standard Constructors + /// @{ + public: + /// Create a default calibration that leaves coordinates unchanged Cal3_S2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0) { @@ -52,6 +57,18 @@ namespace gtsam { */ Cal3_S2(double fov, int w, int h); + /// @} + /// @name Advanced Constructors + /// @{ + + /// load calibration from location (default name is calibration_info.txt) + Cal3_S2(const std::string &path); + + /// @} + /// @name Testable + /// @{ + + /// print with optional string void print(const std::string& s = "") const { gtsam::print(matrix(), s); } @@ -59,8 +76,9 @@ namespace gtsam { /// Check if equal up to specified tolerance bool equals(const Cal3_S2& K, double tol = 10e-9) const; - /// load calibration from location (default name is calibration_info.txt) - Cal3_S2(const std::string &path); + /// @} + /// @name Standard Interface + /// @{ /// focal length x inline double fx() const { return fx_;} @@ -118,6 +136,10 @@ namespace gtsam { * (v - v0_)); } + /// @} + /// @name Manifold + /// @{ + /// return DOF, dimensionality of tangent space inline size_t dim() const { return 5; @@ -138,6 +160,10 @@ namespace gtsam { return vector() - T2.vector(); } + /// @} + /// @name Advanced Interface + /// @{ + private: /// Serialization function @@ -150,8 +176,13 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(u0_); ar & BOOST_SERIALIZATION_NVP(v0_); } + + /// @} + }; - typedef boost::shared_ptr shared_ptrK; ///< shared pointer to calibration object + + + typedef boost::shared_ptr shared_ptrK; ///< shared pointer to calibration object } // gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index c727c7ddb..a8366e35f 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -24,6 +24,7 @@ namespace gtsam { /** * @brief The most common 5DOF 3D->2D calibration, stereo version * @ingroup geometry + * \nosubgrouping */ class Cal3_S2Stereo: public Cal3_S2 { private: @@ -33,32 +34,46 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; ///< shared pointer to stereo calibration object - /** - * default calibration leaves coordinates unchanged - */ + /// @name Standard Constructors + /// @ + + /// default calibration leaves coordinates unchanged Cal3_S2Stereo() : Cal3_S2(1, 1, 0, 0, 0), b_(1.0) { } - /** - * constructor from doubles - */ + /// 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) { } + /// @} + /// @name Testable + /// @{ + void print(const std::string& s = "") const { gtsam::print(matrix(), s); std::cout << "Baseline: " << b_ << std::endl; } + /// @} + /// @name Standard Interface + /// @{ + + //TODO: remove? // /** // * Check if equal up to specified tolerance // */ // bool equals(const Cal3_S2& K, double tol = 10e-9) const; + + + ///TODO: comment inline double baseline() const { return b_; } + /// @} + /// @name Advanced Interface + /// @{ private: /** Serialization function */ @@ -69,6 +84,8 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(b_); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Cal3_S2); } + /// @} + }; typedef boost::shared_ptr shared_ptrKStereo; ///< shared pointer to stereo calibration object diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 4538b3367..a53cb52df 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -33,25 +33,49 @@ namespace gtsam { * If calibration is known, it is more computationally efficient * to calibrate the measurements rather than try to predict in pixels. * @ingroup geometry + * \nosubgrouping */ class CalibratedCamera { private: Pose3 pose_; // 6DOF pose public: - CalibratedCamera() {} ///< default constructor - CalibratedCamera(const Pose3& pose); ///< construct with pose - CalibratedCamera(const Vector &v) ; ///< construct from vector - virtual ~CalibratedCamera() {} ///< destructor - /// return pose - inline const Pose3& pose() const { return pose_; } + /// @name Standard Constructors + /// @{ + + /// default constructor + CalibratedCamera() {} + + /// construct with pose + CalibratedCamera(const Pose3& pose); + + /// @} + /// @name Advanced Constructors + /// @{ + + /// construct from vector + CalibratedCamera(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ /// check equality to another camera bool equals (const CalibratedCamera &camera, double tol = 1e-9) const { return pose_.equals(camera.pose(), tol) ; } + /// @} + /// @name Standard Interface + /// @{ + + /// destructor + virtual ~CalibratedCamera() {} + + /// return pose + inline const Pose3& pose() const { return pose_; } + /// compose the poses inline const CalibratedCamera compose(const CalibratedCamera &c) const { return CalibratedCamera( pose_ * c.pose() ) ; @@ -62,6 +86,17 @@ namespace gtsam { return CalibratedCamera( pose_.inverse() ) ; } + /** + * Create a level camera at the given 2D pose and height + * @param pose2 specifies the location and viewing direction + * (theta 0 = looking in direction of positive X axis) + */ + static CalibratedCamera level(const Pose2& pose2, double height); + + /// @} + /// @name Manifold + /// @{ + /// move a cameras pose according to d CalibratedCamera retract(const Vector& d) const; @@ -74,17 +109,15 @@ namespace gtsam { /// Lie group dimensionality inline static size_t Dim() { return 6 ; } - /** - * Create a level camera at the given 2D pose and height - * @param pose2 specifies the location and viewing direction - * (theta 0 = looking in direction of positive X axis) - */ - static CalibratedCamera level(const Pose2& pose2, double height); /* ************************************************************************* */ // measurement functions and derivatives /* ************************************************************************* */ + /// @} + /// @name Transformations + /// @{ + /** * This function receives the camera pose and the landmark location and * returns the location the point is supposed to appear in the image @@ -111,12 +144,18 @@ namespace gtsam { private: + /// @} + /// @name Advanced Interface + /// @{ + /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(pose_); } + + /// @} }; } diff --git a/gtsam/geometry/CalibratedCameraT.h b/gtsam/geometry/CalibratedCameraT.h index 34e314941..6a70905a4 100644 --- a/gtsam/geometry/CalibratedCameraT.h +++ b/gtsam/geometry/CalibratedCameraT.h @@ -20,6 +20,7 @@ namespace gtsam { * AGC: Is this used or tested anywhere? * AGC: If this is a "CalibratedCamera," why is there a calibration stored internally? * @ingroup geometry + * \nosubgrouping */ template class CalibratedCameraT { @@ -28,44 +29,93 @@ namespace gtsam { Calibration k_; public: + + /// @name Standard Constructors + /// @{ + + ///TODO: comment CalibratedCameraT() {} + + ///TODO: comment CalibratedCameraT(const Pose3& pose):pose_(pose){} + + ///TODO: comment CalibratedCameraT(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + ///TODO: comment CalibratedCameraT(const Vector &v): pose_(Pose3::Expmap(v)) {} + + ///TODO: comment CalibratedCameraT(const Vector &v, const Vector &k):pose_(Pose3::Expmap(v)),k_(k){} + + /// @} + /// @name Standard Interface + /// @{ + virtual ~CalibratedCameraT() {} + ///TODO: comment inline Pose3& pose() { return pose_; } - inline const Pose3& pose() const { return pose_; } - inline Calibration& calibration() { return k_; } - inline const Calibration& calibration() const { return k_; } - bool equals (const CalibratedCameraT &camera, double tol = 1e-9) const { - return pose_.equals(camera.pose(), tol) && k_.equals(camera.calibration(), tol) ; - } + ///TODO: comment + inline const Pose3& pose() const { return pose_; } + + ///TODO: comment + inline Calibration& calibration() { return k_; } + + ///TODO: comment + inline const Calibration& calibration() const { return k_; } + + ///TODO: comment inline const CalibratedCameraT compose(const CalibratedCameraT &c) const { return CalibratedCameraT( pose_ * c.pose(), k_ ) ; } + ///TODO: comment inline const CalibratedCameraT inverse() const { return CalibratedCameraT( pose_.inverse(), k_ ) ; } - CalibratedCameraT retract(const Vector& d) const { - return CalibratedCameraT(pose().retract(d), k_) ; - } - Vector localCoordinates(const CalibratedCameraT& T2) const { - return pose().localCoordinates(T2.pose()) ; + /// @} + /// @name Testable + /// @{ + + /// assert equality up to a tolerance + bool equals (const CalibratedCameraT &camera, double tol = 1e-9) const { + return pose_.equals(camera.pose(), tol) && k_.equals(camera.calibration(), tol) ; } + /// print void print(const std::string& s = "") const { pose_.print("pose3"); k_.print("calibration"); } - inline size_t dim() const { return 6 ; } - inline static size_t Dim() { return 6 ; } + /// @} + /// @name Manifold + /// @{ + ///TODO: comment + CalibratedCameraT retract(const Vector& d) const { + return CalibratedCameraT(pose().retract(d), k_) ; + } + + ///TODO: comment + Vector localCoordinates(const CalibratedCameraT& T2) const { + return pose().localCoordinates(T2.pose()) ; + } + + ///TODO: comment + inline size_t dim() const { return 6 ; } //TODO: add final dimension variable? + + ///TODO: comment + inline static size_t Dim() { return 6 ; } //TODO: add final dimension variable? + + //TODO: remove comment and method? /** * Create a level camera at the given 2D pose and height * @param pose2 specifies the location and viewing direction @@ -85,6 +135,11 @@ namespace gtsam { * @return the intrinsic coordinates of the projected point */ + /// @} + /// @name Transformations + /// @{ + + ///TODO: comment inline Point2 project(const Point3& point, boost::optional D_intrinsic_pose = boost::none, boost::optional D_intrinsic_point = boost::none) const { @@ -92,6 +147,7 @@ namespace gtsam { return result.first ; } + ///TODO: comment std::pair projectSafe( const Point3& pw, boost::optional D_intrinsic_pose = boost::none, @@ -141,6 +197,11 @@ namespace gtsam { } private: + + /// @} + /// @name Advanced Interface + /// @{ + /** Serialization function */ friend class boost::serialization::access; template @@ -148,9 +209,13 @@ private: ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(k_); } + + /// @} }; } +//TODO: remove? + // static CalibratedCameraT Expmap(const Vector& v) { // return CalibratedCameraT(Pose3::Expmap(v), k_) ; // } diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h index 204c4fe9e..d93c56472 100644 --- a/gtsam/geometry/GeneralCameraT.h +++ b/gtsam/geometry/GeneralCameraT.h @@ -28,6 +28,7 @@ namespace gtsam { /** * General camera template * @ingroup geometry + * \nosubgrouping */ template class GeneralCameraT { @@ -37,22 +38,107 @@ private: Calibration calibration_; // Calibration public: + + /// @name Standard Constructors + /// @{ + + ///TODO: comment GeneralCameraT(){} + + ///TODO: comment GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {} + + ///TODO: comment GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {} + + ///TODO: comment GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {} + + ///TODO: comment GeneralCameraT(const Pose3& pose) : calibrated_(pose) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + ///TODO: comment GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {} - // Vector Initialization + ///TODO: comment GeneralCameraT(const Vector &v) : calibrated_(sub(v, 0, Camera::Dim())), calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {} + /// @} + /// @name Standard Interface + /// @{ + + ///TODO: comment inline const Pose3& pose() const { return calibrated_.pose(); } + + ///TODO: comment inline const Camera& calibrated() const { return calibrated_; } + + ///TODO: comment inline const Calibration& calibration() const { return calibration_; } + ///TODO: comment + inline GeneralCameraT compose(const Pose3 &p) const { + return GeneralCameraT( pose().compose(p), calibration_ ) ; + } + + ///TODO: comment + Matrix D2d_camera(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; + Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); + + const int n1 = calibrated_.dim() ; + const int n2 = calibration_.dim() ; + Matrix D(2,n1+n2) ; + + sub(D,0,2,0,n1) = D_2d_pose ; + sub(D,0,2,n1,n1+n2) = D_2d_calibration ; + return D; + } + + ///TODO: comment + Matrix D2d_3d(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + return D_2d_intrinsic * D_intrinsic_3d; + } + + ///TODO: comment + Matrix D2d_camera_3d(const Point3& point) const { + Point2 intrinsic = calibrated_.project(point); + Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); + Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); + Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; + Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); + + Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); + Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d; + + const int n1 = calibrated_.dim() ; + const int n2 = calibration_.dim() ; + + Matrix D(2,n1+n2+3) ; + + sub(D,0,2,0,n1) = D_2d_pose ; + sub(D,0,2,n1,n1+n2) = D_2d_calibration ; + sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ; + return D; + } + + /// @} + /// @name Transformations + /// @{ + + ///TODO: comment std::pair projectSafe( const Point3& P, boost::optional H1 = boost::none, @@ -64,6 +150,7 @@ public: cameraPoint.z() > 0); } + ///TODO: comment Point3 backproject(const Point2& projection, const double scale) const { Point2 intrinsic = calibration_.calibrate(projection); Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale); @@ -105,90 +192,66 @@ public: return projection; } - // dump functions for compilation for now + /// @} + /// @name Testable + /// @{ + + /// checks equality up to a tolerance bool equals (const GeneralCameraT &camera, double tol = 1e-9) const { return calibrated_.equals(camera.calibrated_, tol) && calibration_.equals(camera.calibration_, tol) ; } + /// print with optional string void print(const std::string& s = "") const { calibrated_.pose().print(s + ".camera.") ; calibration_.print(s + ".calibration.") ; } + /// @} + /// @name Manifold + /// @{ + + ///TODO: comment GeneralCameraT retract(const Vector &v) const { Vector v1 = sub(v,0,Camera::Dim()); Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim()); return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2)); } + ///TODO: comment Vector localCoordinates(const GeneralCameraT &C) const { const Vector v1(calibrated().localCoordinates(C.calibrated())), v2(calibration().localCoordinates(C.calibration())); return concatVectors(2,&v1,&v2) ; } - inline GeneralCameraT compose(const Pose3 &p) const { - return GeneralCameraT( pose().compose(p), calibration_ ) ; - } - - Matrix D2d_camera(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; - Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); - - const int n1 = calibrated_.dim() ; - const int n2 = calibration_.dim() ; - Matrix D(2,n1+n2) ; - - sub(D,0,2,0,n1) = D_2d_pose ; - sub(D,0,2,n1,n1+n2) = D_2d_calibration ; - return D; - } - - Matrix D2d_3d(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - return D_2d_intrinsic * D_intrinsic_3d; - } - - Matrix D2d_camera_3d(const Point3& point) const { - Point2 intrinsic = calibrated_.project(point); - Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point); - Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic); - Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose; - Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic); - - Matrix D_intrinsic_3d = Dproject_point(calibrated_, point); - Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d; - - const int n1 = calibrated_.dim() ; - const int n2 = calibration_.dim() ; - - Matrix D(2,n1+n2+3) ; - - sub(D,0,2,0,n1) = D_2d_pose ; - sub(D,0,2,n1,n1+n2) = D_2d_calibration ; - sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ; - return D; - } - //inline size_t dim() { return Camera::dim() + Calibration::dim() ; } + + ///TODO: comment inline size_t dim() const { return calibrated().dim() + calibration().dim() ; } + + ///TODO: comment static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; } private: + + /// @} + /// @name Advanced Interface + /// @{ + friend class boost::serialization::access; template + + /// Serialization function void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(calibrated_); ar & BOOST_SERIALIZATION_NVP(calibration_); } + /// @} + }; typedef GeneralCameraT Cal3BundlerCamera; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 12f299637..558d8bd4d 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -28,6 +28,7 @@ namespace gtsam { * Complies with the Testable Concept * Functional, so no set functions: once created, a point is constant. * @ingroup geometry + * \nosubgrouping */ class Point2 { public: @@ -37,11 +38,27 @@ private: double x_, y_; public: + + /// @name Standard Constructors + /// @{ + + ///TODO: comment Point2(): x_(0), y_(0) {} + + ///TODO: comment Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {} + + ///TODO: comment Point2(double x, double y): x_(x), y_(y) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + ///TODO: comment Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); } + /// @} /// @name Testable /// @{ @@ -72,11 +89,19 @@ public: return *this + p2; } - /** operators */ + ///TODO: comment inline Point2 operator- () const {return Point2(-x_,-y_);} + + ///TODO: comment inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} + + ///TODO: comment inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} + + ///TODO: comment inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);} + + ///TODO: comment inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);} /// @} @@ -120,12 +145,18 @@ public: return (p2 - *this).norm(); } - /** operators */ + ///TODO: comment inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} + + ///TODO: comment inline void operator *= (double s) {x_*=s;y_*=s;} + + ///TODO: comment inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;} /// @} + /// @name Standard Interface + /// @{ /** "Between", subtracts point coordinates */ inline Point2 between(const Point2& p2, @@ -136,14 +167,21 @@ public: return p2 - (*this); } - /** get functions for x, y */ + /// get x double x() const {return x_;} + + /// get y double y() const {return y_;} /** return vectorized form (column-wise) */ Vector vector() const { return Vector_(2, x_, y_); } private: + + /// @} + /// @name Advanced Interface + /// @{ + /** Serialization function */ friend class boost::serialization::access; template @@ -152,9 +190,13 @@ private: ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); } + }; /** multiply with scalar */ inline Point2 operator*(double s, const Point2& p) {return p*s;} + +/// @} + } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index d3a4fe2ed..269331490 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -31,6 +31,7 @@ namespace gtsam { /** * A 3D point * @ingroup geometry + * \nosubgrouping */ class Point3 { public: @@ -41,11 +42,27 @@ namespace gtsam { double x_, y_, z_; public: + + /// @name Standard Constructors + /// @{ + + ///TODO: comment Point3(): x_(0), y_(0), z_(0) {} + + ///TODO: comment Point3(const Point3 &p) : x_(p.x_), y_(p.y_), z_(p.z_) {} + + ///TODO: comment Point3(double x, double y, double z): x_(x), y_(y), z_(z) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + ///TODO: comment Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {} + /// @} /// @name Testable /// @{ @@ -106,11 +123,22 @@ namespace gtsam { /// @name Vector Operators /// @{ + ///TODO: comment Point3 operator - () const { return Point3(-x_,-y_,-z_);} + + ///TODO: comment bool operator ==(const Point3& q) const; + + ///TODO: comment Point3 operator + (const Point3& q) const; + + ///TODO: comment Point3 operator - (const Point3& q) const; + + ///TODO: comment Point3 operator * (double s) const; + + ///TODO: comment Point3 operator / (double s) const; /** distance between two points */ @@ -128,6 +156,8 @@ namespace gtsam { double dot(const Point3 &q) const; /// @} + /// @name Standard Interface + /// @{ /** Between using the default implementation */ inline Point3 between(const Point3& p2, @@ -144,9 +174,13 @@ namespace gtsam { return v; } - /** get functions for x, y, z */ + /// get x inline double x() const {return x_;} + + /// get y inline double y() const {return y_;} + + /// get z inline double z() const {return z_;} /** add two points, add(this,q) is same as this + q */ @@ -158,6 +192,11 @@ namespace gtsam { boost::optional H1=boost::none, boost::optional H2=boost::none) const; private: + + /// @} + /// @name Advanced Interface + /// @{ + /** Serialization function */ friend class boost::serialization::access; template @@ -172,4 +211,6 @@ namespace gtsam { /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} + /// @} + } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 74cb2453d..46eac05d7 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -30,6 +30,7 @@ namespace gtsam { /** * A 2D pose (Point2,Rot2) * @ingroup geometry + * \nosubgrouping */ class Pose2 { @@ -46,6 +47,9 @@ private: public: + /// @name Standard Constructors + /// @{ + /** default constructor = origin */ Pose2() {} // default is origin @@ -66,6 +70,8 @@ public: Pose2(double theta, const Point2& t) : r_(Rot2::fromAngle(theta)), t_(t) { } + + ///TODO: comment Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** Constructor from 3*3 matrix */ @@ -74,11 +80,16 @@ public: assert(T.rows() == 3 && T.cols() == 3); } + /// @} + /// @name Advanced Constructors + /// @{ + /** Construct from canonical coordinates (Lie algebra) */ Pose2(const Vector& v) { *this = Expmap(v); } + /// @} /// @name Testable /// @{ @@ -135,6 +146,8 @@ public: static Vector Logmap(const Pose2& p); /// @} + /// @name Standard Interface + /// @{ /** return transformation matrix */ Matrix matrix() const; @@ -224,8 +237,14 @@ public: } /** get functions for x, y, theta */ + + /// get x inline double x() const { return t_.x(); } + + /// get y inline double y() const { return t_.y(); } + + /// get theta inline double theta() const { return r_.theta(); } /** shorthand access functions */ @@ -237,6 +256,11 @@ public: inline const Rot2& rotation() const { return r_; } private: + + /// @} + /// @name Advanced Interface + /// @{ + // Serialization function friend class boost::serialization::access; template @@ -259,5 +283,7 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point2Pair; boost::optional align(const std::vector& pairs); +/// @} + } // namespace gtsam diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 1a6d91b20..67acc0bc1 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -27,6 +27,7 @@ namespace gtsam { * Rotation matrix * NOTE: the angle theta is in radians unless explicitly stated * @ingroup geometry + * \nosubgrouping */ class Rot2 { @@ -39,14 +40,18 @@ namespace gtsam { /** we store cos(theta) and sin(theta) */ double c_, s_; + + /** normalize to make sure cos and sin form unit vector */ + Rot2& normalize(); + + /// @name Standard Constructors + /// @{ + /** private constructor from cos/sin */ inline Rot2(double c, double s) : c_(c), s_(s) { } - /** normalize to make sure cos and sin form unit vector */ - Rot2& normalize(); - public: /** default constructor, zero rotation */ @@ -59,8 +64,6 @@ namespace gtsam { c_(cos(theta)), s_(sin(theta)) { } - /** "named constructors" */ - /// Named constructor from angle in radians static Rot2 fromAngle(double theta) { return Rot2(theta); @@ -88,27 +91,7 @@ namespace gtsam { /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ static Rot2 atan2(double y, double x); - /** return angle (RADIANS) */ - double theta() const { - return ::atan2(s_, c_); - } - - /** return angle (DEGREES) */ - double degrees() const { - const double degree = M_PI / 180; - return theta() / degree; - } - - /** return cos */ - inline double c() const { - return c_; - } - - /** return sin */ - inline double s() const { - return s_; - } - + /// @} /// @name Testable /// @{ @@ -197,6 +180,29 @@ namespace gtsam { } /// @} + /// @name Standard Interface + /// @{ + + /** return angle (RADIANS) */ + double theta() const { + return ::atan2(s_, c_); + } + + /** return angle (DEGREES) */ + double degrees() const { + const double degree = M_PI / 180; + return theta() / degree; + } + + /** return cos */ + inline double c() const { + return c_; + } + + /** return sin */ + inline double s() const { + return s_; + } /** Between using the default implementation */ inline Rot2 between(const Rot2& p2, boost::optional H1 = @@ -226,6 +232,9 @@ namespace gtsam { Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; + /// @} + /// @name Advanced Interface + /// @{ private: /** Serialization function */ @@ -236,6 +245,8 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(s_); } + /// @} + }; } // gtsam diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index b0f2d738c..4cf46d159 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -25,6 +25,7 @@ namespace gtsam { /** * A 2D stereo point, v will be same for rectified images * @ingroup geometry + * \nosubgrouping */ class StereoPoint2 { public: @@ -34,6 +35,9 @@ namespace gtsam { public: + /// @name Standard Constructors + /// @{ + /** default constructor */ StereoPoint2() : uL_(0), uR_(0), v_(0) { @@ -44,6 +48,7 @@ namespace gtsam { uL_(uL), uR_(uR), v_(v) { } + /// @} /// @name Testable /// @{ @@ -113,7 +118,9 @@ namespace gtsam { return p.vector(); } - /// @}} + /// @} + /// @name Standard Interface + /// @{ /** convert to vector */ Vector vector() const { @@ -130,6 +137,11 @@ namespace gtsam { } private: + + /// @} + /// @name Advanced Interface + /// @{ + /** Serialization function */ friend class boost::serialization::access; template @@ -138,6 +150,9 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(uR_); ar & BOOST_SERIALIZATION_NVP(v_); } + + /// @} + }; } diff --git a/gtsam/geometry/Tensor1.h b/gtsam/geometry/Tensor1.h index 39350e217..685166b53 100644 --- a/gtsam/geometry/Tensor1.h +++ b/gtsam/geometry/Tensor1.h @@ -24,6 +24,7 @@ namespace tensors { /** * A rank 1 tensor. Actually stores data. * @ingroup tensors + * \nosubgrouping */ template class Tensor1 { @@ -31,6 +32,9 @@ namespace tensors { public: + /// @name Standard Constructors + /// @{ + /** default constructor */ Tensor1() { } @@ -48,6 +52,10 @@ namespace tensors { T[i] = a(i); } + /// @} + /// @name Standard Interface + /// @{ + /** return data */ inline int dim() const { return N; @@ -69,6 +77,8 @@ namespace tensors { return Tensor1Expression >(*this); } + /// @} + }; // Tensor1 diff --git a/gtsam/geometry/Tensor1Expression.h b/gtsam/geometry/Tensor1Expression.h index 7416ceb40..4e43fff58 100644 --- a/gtsam/geometry/Tensor1Expression.h +++ b/gtsam/geometry/Tensor1Expression.h @@ -30,6 +30,7 @@ namespace tensors { * This class does not store any data but the result of an expression. * It is associated with an index. * @ingroup tensors + * \nosubgrouping */ template class Tensor1Expression { @@ -56,11 +57,18 @@ namespace tensors { public: + /// @name Standard Constructors + /// @{ + /** constructor */ Tensor1Expression(const A &a) : iter(a) { } + /// @} + /// @name Testable + /// @{ + /** Print */ void print(const std::string s = "") const { std::cout << s << "{"; @@ -78,6 +86,10 @@ namespace tensors { return true; } + /// @} + /// @name Standard Interface + /// @{ + /** norm */ double norm() const { double sumsqr = 0.0; @@ -122,6 +134,10 @@ namespace tensors { }; // Tensor1Expression + /// @} + /// @name Advanced Interface + /// @{ + /** Print a rank 1 expression */ template void print(const Tensor1Expression& T, const std::string s = "") { @@ -160,4 +176,6 @@ namespace tensors { return false; } + /// @} + } // namespace tensors diff --git a/gtsam/geometry/Tensor2.h b/gtsam/geometry/Tensor2.h index 5974249c4..12fd1509f 100644 --- a/gtsam/geometry/Tensor2.h +++ b/gtsam/geometry/Tensor2.h @@ -24,6 +24,7 @@ namespace tensors { /** * Rank 2 Tensor * @ingroup tensors + * \nosubgrouping */ template class Tensor2 { @@ -32,6 +33,9 @@ protected: public: + /// @name Standard Constructors + /// @{ + /** default constructor */ Tensor2() { } @@ -49,6 +53,10 @@ public: T[j] = a(j); } + /// @} + /// @name Standard Interface + /// @{ + /** dimension - TODO: is this right for anything other than 3x3? */ size_t dim() const {return N1 * N2;} @@ -67,6 +75,9 @@ public: N2, J> > operator()(Index i, Index j) const { return Tensor2Expression , Index > (*this); } + + /// @} + }; } // namespace tensors diff --git a/gtsam/geometry/Tensor2Expression.h b/gtsam/geometry/Tensor2Expression.h index 77f8b898a..6c953a5aa 100644 --- a/gtsam/geometry/Tensor2Expression.h +++ b/gtsam/geometry/Tensor2Expression.h @@ -27,6 +27,7 @@ namespace tensors { /** * Templated class to hold a rank 2 tensor expression. * @ingroup tensors + * \nosubgrouping */ template class Tensor2Expression { @@ -134,11 +135,18 @@ namespace tensors { public: + /// @name Standard Constructors + /// @{ + /** constructor */ Tensor2Expression(const A &a) : iter(a) { } + /// @} + /// @name Testable + /// @{ + /** Print */ void print(const std::string& s = "Tensor2:") const { std::cout << s << "{"; @@ -159,6 +167,10 @@ namespace tensors { return true; } + /// @} + /// @name Standard Interface + /// @{ + /** norm */ double norm() const { double sumsqr = 0.0; @@ -207,6 +219,10 @@ namespace tensors { return true; } + /// @} + /// @name Advanced Interface + /// @{ + /** c(j) = a(i,j)*b(i) */ template inline Tensor1Expression, J> operator*( @@ -289,4 +305,6 @@ namespace tensors { return false; } + /// @} + } // namespace tensors diff --git a/gtsam/geometry/Tensor3.h b/gtsam/geometry/Tensor3.h index 9be8ec909..78cdcb9f2 100644 --- a/gtsam/geometry/Tensor3.h +++ b/gtsam/geometry/Tensor3.h @@ -24,6 +24,7 @@ namespace tensors { /** * Rank 3 Tensor * @ingroup tensors + * \nosubgrouping */ template class Tensor3 { @@ -31,6 +32,9 @@ namespace tensors { public: + /// @name Standard Constructors + /// @{ + /** default constructor */ Tensor3() { } @@ -41,6 +45,10 @@ namespace tensors { T[k] = data[k]; } + /// @} + /// @name Advanced Constructors + /// @{ + /** construct from expression */ template Tensor3(const Tensor3Expression , Index , Index class Tensor3Expression { A iter; @@ -64,11 +65,18 @@ namespace tensors { public: + /// @name Standard Constructors + /// @{ + /** constructor */ Tensor3Expression(const A &a) : iter(a) { } + /// @} + /// @name Standard Interface + /// @{ + /** Print */ void print(const std::string& s = "Tensor3:") const { std::cout << s << "{"; @@ -107,6 +115,10 @@ namespace tensors { }; // Tensor3Expression + /// @} + /// @name Advanced Interface + /// @{ + /** Print */ template void print(const Tensor3Expression& T, const std::string& s = @@ -177,4 +189,6 @@ namespace tensors { return false; } + /// @} + } // namespace tensors diff --git a/gtsam/geometry/Tensor4.h b/gtsam/geometry/Tensor4.h index 53efb7d33..26a52750c 100644 --- a/gtsam/geometry/Tensor4.h +++ b/gtsam/geometry/Tensor4.h @@ -24,6 +24,7 @@ namespace tensors { /** * Rank 4 Tensor * @ingroup tensors + * \nosubgrouping */ template class Tensor4 { @@ -34,15 +35,24 @@ namespace tensors { public: + /// @name Standard Constructors + /// @{ + /** default constructor */ Tensor4() { } + /// @} + /// @name Standard Interface + /// @{ + /// element access double operator()(int i, int j, int k, int l) const { return T[l](i, j, k); } + /// @} + }; // Tensor4 } // namespace tensors diff --git a/gtsam/geometry/Tensor5.h b/gtsam/geometry/Tensor5.h index 6a72bb165..e12f7d2fb 100644 --- a/gtsam/geometry/Tensor5.h +++ b/gtsam/geometry/Tensor5.h @@ -24,6 +24,7 @@ namespace tensors { /** * Rank 5 Tensor * @ingroup tensors + * \nosubgrouping */ template class Tensor5 { @@ -34,10 +35,17 @@ namespace tensors { public: + /// @name Standard Constructors + /// @{ + /** default constructor */ Tensor5() { } + /// @} + /// @name Standard Interface + /// @{ + /** construct from expression */ template Tensor5(const Tensor5Expression , Index , Index , Index , Index , Index , Index > (*this); } + + /// @} + }; // Tensor5 } // namespace tensors diff --git a/gtsam/geometry/Tensor5Expression.h b/gtsam/geometry/Tensor5Expression.h index 0431df61d..e9f0e852a 100644 --- a/gtsam/geometry/Tensor5Expression.h +++ b/gtsam/geometry/Tensor5Expression.h @@ -26,6 +26,7 @@ namespace tensors { /** * templated class to interface to an object A as a rank 5 tensor * @ingroup tensors + * \nosubgrouping */ template class Tensor5Expression { A iter; @@ -48,11 +49,18 @@ namespace tensors { public: + /// @name Standard Constructors + /// @{ + /** constructor */ Tensor5Expression(const A &a) : iter(a) { } + /// @} + /// @name Standard Interface + /// @{ + /** Print */ void print(const std::string& s = "Tensor5:") const { std::cout << s << std::endl; @@ -85,6 +93,10 @@ namespace tensors { }; // Tensor5Expression + /// @} + /// @name Advanced Interface + /// @{ + /** Print */ template void print(const Tensor5Expression& T, @@ -118,4 +130,6 @@ namespace tensors { return Rank3Rank2_(a, b); } + /// @} + } // namespace tensors