diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 1afbd1417..34edae2e1 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -249,12 +249,12 @@ public: /// Return canonical coordinate Vector localCoordinates(const CalibratedCamera& T2) const; - /// Lie group dimensionality + /// @deprecated inline size_t dim() const { return 6; } - /// Lie group dimensionality + /// @deprecated inline static size_t Dim() { return 6; } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index e3dd98be0..7bcc22172 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -22,7 +22,6 @@ #include #include #include -#include namespace gtsam { @@ -34,7 +33,9 @@ namespace gtsam { template class GTSAM_EXPORT PinholeBaseK: public PinholeBase { -public: + GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + +public : /// @name Standard Constructors /// @{ @@ -45,7 +46,7 @@ public: /** constructor with pose */ explicit PinholeBaseK(const Pose3& pose) : - PinholeBase(pose) { + PinholeBase(pose) { } /// @} @@ -53,7 +54,7 @@ public: /// @{ explicit PinholeBaseK(const Vector &v) : - PinholeBase(v) { + PinholeBase(v) { } /// @} @@ -92,10 +93,8 @@ public: Dpose || Dpoint ? &Dpi_pn : 0); // If needed, apply chain rule - if (Dpose) - *Dpose = Dpi_pn * *Dpose; - if (Dpoint) - *Dpoint = Dpi_pn * *Dpoint; + if (Dpose) *Dpose = Dpi_pn * (*Dpose); + if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint); return pi; } @@ -246,10 +245,12 @@ public: /// @name Advanced Constructors /// @{ + /// Init from 6D vector explicit PinholePose(const Vector &v) : Base(v), K_(new Calibration()) { } + /// Init from Vector and calibration PinholePose(const Vector &v, const Vector &K) : Base(v), K_(new Calibration(K)) { } @@ -286,25 +287,23 @@ public: /// @name Manifold /// @{ - /// Manifold 6 + /// @deprecated size_t dim() const { return 6; } - /// Manifold 6 + /// @deprecated static size_t Dim() { return 6; } - typedef Eigen::Matrix VectorK6; - /// move a cameras according to d PinholePose retract(const Vector6& d) const { return PinholePose(Base::pose().retract(d), K_); } /// return canonical coordinate - VectorK6 localCoordinates(const PinholePose& p) const { + Vector6 localCoordinates(const PinholePose& p) const { return Base::pose().localCoordinates(p.Base::pose()); }