From 2caa56c0d51b5c52af4f6c57776c88d61428a43e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 7 Sep 2011 05:02:36 +0000 Subject: [PATCH] Documentation and creation of Doxygen module "geometry" --- gtsam/geometry/Cal3Bundler.h | 14 +++-- gtsam/geometry/Cal3DS2.h | 16 ++--- gtsam/geometry/Cal3_S2.h | 93 ++++++++++++----------------- gtsam/geometry/Cal3_S2Stereo.h | 5 +- gtsam/geometry/CalibratedCamera.cpp | 12 ++-- gtsam/geometry/CalibratedCamera.h | 45 ++++++++------ gtsam/geometry/CalibratedCameraT.h | 14 ++--- gtsam/geometry/GeneralCameraT.h | 14 +++-- gtsam/geometry/Point2.h | 1 + gtsam/geometry/Point3.h | 5 +- gtsam/geometry/Pose2.h | 1 + gtsam/geometry/Pose3.h | 5 +- gtsam/geometry/Rot2.h | 14 +++-- gtsam/geometry/Rot3.h | 7 ++- gtsam/geometry/SimpleCamera.h | 10 ++-- gtsam/geometry/StereoCamera.h | 3 +- gtsam/geometry/StereoPoint2.h | 9 +-- 17 files changed, 145 insertions(+), 123 deletions(-) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 1883d28a5..cb508af15 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -9,11 +9,11 @@ * -------------------------------------------------------------------------- */ -/* - * Cal3Bundler.h - * - * Created on: Sep 25, 2010 - * Author: ydjian +/** + * @file Cal3Bundler.h + * @brief Calibration used by Bundler + * @date Sep 25, 2010 + * @author ydjian */ #pragma once @@ -22,6 +22,10 @@ namespace gtsam { +/** + * @brief Calibration used by Bundler + * @ingroup geometry + */ class Cal3Bundler: public Testable { private: diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index a834bd3bf..90ed76aca 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -9,21 +9,23 @@ * -------------------------------------------------------------------------- */ -/* - * Cal3DS2.h - * - * Created on: Feb 28, 2010 - * Author: ydjian +/** + * @file Cal3DS2.h + * @brief Calibration of a camera with radial distortion + * @date Feb 28, 2010 + * @author ydjian */ - #pragma once #include namespace gtsam { - /** This class a camera with radial distortion */ + /** + * @brief Calibration of a camera with radial distortion + * @ingroup geometry + */ class Cal3DS2 : Testable { private: double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 43a88d88b..b59cf9216 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -15,28 +15,31 @@ * @author Frank Dellaert */ +/** + * @defgroup geometry + */ + #pragma once #include namespace gtsam { - /** The most common 5DOF 3D->2D calibration */ + /** + * @brief The most common 5DOF 3D->2D calibration + * @ingroup geometry + */ class Cal3_S2: Testable { private: double fx_, fy_, s_, u0_, v0_; public: - /** - * default calibration leaves coordinates unchanged - */ + /// Create a default calibration that leaves coordinates unchanged Cal3_S2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0) { } - /** - * constructor from doubles - */ + /// 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) { } @@ -53,42 +56,33 @@ namespace gtsam { gtsam::print(matrix(), s); } - /** - * Check if equal up to specified tolerance - */ + /// 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) - */ + /// load calibration from location (default name is calibration_info.txt) Cal3_S2(const std::string &path); - inline double fx() const { - return fx_; - } - inline double fy() const { - return fy_; - } - inline double skew() const { - return s_; - } - inline double px() const { - return u0_; - } - inline double py() const { - return v0_; - } + /// focal length x + inline double fx() const { return fx_;} - /** - * return the principal point - */ + /// focal length x + inline double fy() const { return 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_); } - /** - * return vectorized form (column-wise) - */ + /// vectorized form (column-wise) Vector vector() const { double r[] = { fx_, fy_, s_, u0_, v0_ }; Vector v(5); @@ -96,17 +90,12 @@ namespace gtsam { return v; } - /** - * return calibration matrix K - */ + /// return calibration matrix K Matrix matrix() const { return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); } - - /** - * return calibration matrix inv(K) - */ + /// return inverted calibration matrix inv(K) Matrix matrix_inverse() const { const double fxy = fx_*fy_, sv0 = s_*v0_, fyu0 = fy_*u0_; return Matrix_(3, 3, @@ -115,7 +104,6 @@ namespace gtsam { 0.0, 0.0, 1.0); } - /** * convert intrinsic coordinates xy to image coordinates uv * with optional derivatives @@ -123,41 +111,36 @@ namespace gtsam { Point2 uncalibrate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; - /** - * convert image coordinates uv to intrinsic coordinates xy - */ + /// convert image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p) const { const double u = p.x(), v = p.y(); return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), (1 / fy_) * (v - v0_)); } - /** - * return DOF, dimensionality of tangent space - */ + /// return DOF, dimensionality of tangent space inline size_t dim() const { return 5; } + + /// return DOF, dimensionality of tangent space static size_t Dim() { return 5; } - /** - * Given 5-dim tangent vector, create new calibration - */ + /// Given 5-dim tangent vector, create new calibration inline Cal3_S2 expmap(const Vector& d) const { return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); } - /** - * logmap for the calibration - */ + /// logmap for the calibration Vector logmap(const Cal3_S2& T2) const { return vector() - T2.vector(); } private: - /** Serialization function */ + + /// Serialization function friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 1b69468e7..12585bf83 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -21,7 +21,10 @@ namespace gtsam { - /** The most common 5DOF 3D->2D calibration */ + /** + * @brief The most common 5DOF 3D->2D calibration, stereo version + * @ingroup geometry + */ class Cal3_S2Stereo: public Cal3_S2 { private: double b_; diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 873e07a25..6b528b51a 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -9,14 +9,14 @@ * -------------------------------------------------------------------------- */ -/* - * CalibratedCamera.cpp - * - * Created on: Aug 17, 2009 - * Author: dellaert +/** + * @file CalibratedCamera.cpp + * @brief Calibrated camera for which only pose is unknown + * @date Aug 17, 2009 + * @author Frank Dellaert */ -#include +#include #include namespace gtsam { diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index b45933a44..a7d11d13e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -9,58 +9,70 @@ * -------------------------------------------------------------------------- */ -/* - * CalibratedCamera.h - * - * Created on: Aug 17, 2009 - * Author: dellaert +/** + * @file CalibratedCamera.h + * @brief Calibrated camera for which only pose is unknown + * @date Aug 17, 2009 + * @author Frank Dellaert */ -#ifndef CalibratedCAMERA_H_ -#define CalibratedCAMERA_H_ +#pragma once #include #include namespace gtsam { - class Point2; - /** * A Calibrated camera class [R|-R't], calibration K=I. * If calibration is known, it is more computationally efficient * to calibrate the measurements rather than try to predict in pixels. + * @ingroup geometry */ class CalibratedCamera { private: Pose3 pose_; // 6DOF pose public: - CalibratedCamera(); - CalibratedCamera(const Pose3& pose); - CalibratedCamera(const Vector &v) ; - virtual ~CalibratedCamera(); + 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_; } + + /// check equality to another camera bool equals (const CalibratedCamera &camera, double tol = 1e-9) const { return pose_.equals(camera.pose(), tol) ; } + /// compose the poses inline const CalibratedCamera compose(const CalibratedCamera &c) const { return CalibratedCamera( pose_ * c.pose() ) ; } + /// invert the camera's pose inline const CalibratedCamera inverse() const { return CalibratedCamera( pose_.inverse() ) ; } + /// move a cameras pose according to d CalibratedCamera expmap(const Vector& d) const; - Vector logmap(const CalibratedCamera& T2) const; + /// Return canonical coordinate + Vector logmap(const CalibratedCamera& T2) const; + + /// move a cameras pose according to d static CalibratedCamera Expmap(const Vector& v); - static Vector Logmap(const CalibratedCamera& p); + /// Return canonical coordinate + static Vector Logmap(const CalibratedCamera& p); + + /// Lie group dimensionality inline size_t dim() const { return 6 ; } + + /// Lie group dimensionality inline static size_t Dim() { return 6 ; } /** @@ -99,14 +111,13 @@ namespace gtsam { static Point3 backproject_from_camera(const Point2& p, const double scale); private: + /** Serialization function */ friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(pose_); } - }; } -#endif /* CalibratedCAMERA_H_ */ diff --git a/gtsam/geometry/CalibratedCameraT.h b/gtsam/geometry/CalibratedCameraT.h index 3ecdfc01a..c432364e6 100644 --- a/gtsam/geometry/CalibratedCameraT.h +++ b/gtsam/geometry/CalibratedCameraT.h @@ -1,8 +1,8 @@ -/* - * CalibratedCameraT.h - * - * Created on: Mar 5, 2011 - * Author: Yong-Dian Jian +/** + * @file CalibratedCameraT.h + * @date Mar 5, 2011 + * @author Yong-Dian Jian + * @brief calibrated camera template */ #pragma once @@ -12,13 +12,13 @@ #include namespace gtsam { - class Point2; + /** * A Calibrated camera class [R|-R't], calibration K. * If calibration is known, it is more computationally efficient * to calibrate the measurements rather than try to predict in pixels. + * @ingroup geometry */ - template class CalibratedCameraT { private: diff --git a/gtsam/geometry/GeneralCameraT.h b/gtsam/geometry/GeneralCameraT.h index ae2235e5b..6ec87e0d8 100644 --- a/gtsam/geometry/GeneralCameraT.h +++ b/gtsam/geometry/GeneralCameraT.h @@ -9,11 +9,11 @@ * -------------------------------------------------------------------------- */ -/* - * DistortedCameraT.h - * - * Created on: Mar 1, 2010 - * Author: ydjian +/** + * @file GeneralCameraT.h + * @brief General camera template + * @date Mar 1, 2010 + * @author Yong-Dian Jian */ #pragma once @@ -25,6 +25,10 @@ namespace gtsam { +/** + * General camera template + * @ingroup geometry + */ template class GeneralCameraT { diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 447fe864a..8a1a42ea2 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -28,6 +28,7 @@ namespace gtsam { * A 2D point * Derived from testable so has standard print and equals, and assert_equals works * Functional, so no set functions: once created, a point is constant. + * @ingroup geometry */ class Point2: Testable, public Lie { public: diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 59a23bdd3..f2cc509a5 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -29,7 +29,10 @@ namespace gtsam { - /** A 3D point */ + /** + * A 3D point + * @ingroup geometry + */ class Point3: Testable, public Lie { public: /// dimension of the variable - used to autodetect sizes diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 043d74452..6e53ac4f3 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -29,6 +29,7 @@ namespace gtsam { /** * A 2D pose (Point2,Rot2) + * @ingroup geometry */ class Pose2: Testable, public Lie { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index f4b7a6caa..997d9ed36 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -23,7 +23,10 @@ namespace gtsam { - /** A 3D pose (R,t) : (Rot3,Point3) */ + /** + * A 3D pose (R,t) : (Rot3,Point3) + * @ingroup geometry + */ class Pose3 : Testable, public Lie { public: static const size_t dimension = 6; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index ff159364f..916bdf6d1 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -9,11 +9,11 @@ * -------------------------------------------------------------------------- */ -/* - * Rot2.h - * - * Created on: Dec 9, 2009 - * Author: Frank Dellaert +/** + * @file Rot2.h + * @brief 2D rotation + * @date Dec 9, 2009 + * @author Frank Dellaert */ #pragma once @@ -23,8 +23,10 @@ namespace gtsam { - /** Rotation matrix + /** + * Rotation matrix * NOTE: the angle theta is in radians unless explicitly stated + * @ingroup geometry */ class Rot2: Testable, public Lie { diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 9f955829b..b2b6ada3f 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -11,7 +11,7 @@ /** * @file Rot3.h - * @brief Rotation + * @brief 3D Rotation represented as 3*3 matrix * @author Alireza Fathi * @author Christian Potthast * @author Frank Dellaert @@ -25,7 +25,10 @@ namespace gtsam { - /* 3D Rotations in GTSAM are represented as rotation matrices */ + /** + * @brief 3D Rotations represented as rotation matrices + * @ingroup geometry + */ class Rot3: Testable, public Lie { public: static const size_t dimension = 3; diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 252af900f..22bb0fda1 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -9,11 +9,11 @@ * -------------------------------------------------------------------------- */ -/* - * SimpleCamera.h - * - * Created on: Aug 16, 2009 - * Author: dellaert +/** + * @file SimpleCamera.h + * @brief A simple camera class with a Cal3_S2 calibration + * @date Aug 16, 2009 + * @author Frank Dellaert */ #ifndef SIMPLECAMERA_H_ diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index f06e45cfb..6427cefbd 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -25,7 +25,8 @@ namespace gtsam { /** - * A stereo camera class + * A stereo camera class, parameterize by left camera pose and stereo calibration + * @ingroup geometry */ class StereoCamera { diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index a4ed3eb9a..551b6ecdc 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -10,10 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * StereoPoint2.h - * - * Created on: Jan 26, 2010 - * Author: dellaert + * @file StereoPoint2.h + * @brief A 2D stereo point (uL,uR,v) + * @date Jan 26, 2010 + * @author Frank Dellaert */ #pragma once @@ -24,6 +24,7 @@ namespace gtsam { /** * A 2D stereo point, v will be same for rectified images + * @ingroup geometry */ class StereoPoint2: Testable, Lie { public: