diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index d60464e9e..38357b132 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -127,7 +127,9 @@ check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { */ template class IsManifold { + public: + typedef typename traits_x::structure_category structure_category_tag; static const size_t dim = traits_x::dimension; typedef typename traits_x::ManifoldType ManifoldType; @@ -141,19 +143,30 @@ public: BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); // make sure Chart methods are defined - v = traits_x::Local(p,q); - q = traits_x::Retract(p,v); + v = traits_x::Local(p, q); + q = traits_x::Retract(p, v); // and the versions with Jacobians. //v = traits_x::Local(p,q,Hp,Hq); //q = traits_x::Retract(p,v,Hp,Hv); } + private: - ManifoldType p,q; - ChartJacobian Hp,Hq,Hv; + + ManifoldType p, q; + ChartJacobian Hp, Hq, Hv; TangentVector v; bool b; }; +/// Give fixed size dimension of a type, fails at compile time if dynamic +template +struct FixedDimension { + typedef const int value_type; + static const int value = traits_x::dimension; + BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic, + "FixedDimension instantiated for dymanically-sized type."); +}; + } // \ namespace gtsam ///** diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 546951b30..a0395bb6e 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -31,16 +31,19 @@ namespace gtsam { */ template class PinholeCamera { + private: Pose3 pose_; Calibration K_; + GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + // Get dimensions of calibration type and This at compile time - static const int DimK = traits_x::dimension, // - DimC = 6 + DimK; + static const int DimK = FixedDimension::value; public: - enum { dimension = DimC }; + + enum { dimension = 6 + DimK }; /// @name Standard Constructors /// @{ @@ -169,15 +172,15 @@ public: /// Manifold dimension inline size_t dim() const { - return DimC; + return dimension; } /// Manifold dimension inline static size_t Dim() { - return DimC; + return dimension; } - typedef Eigen::Matrix VectorK6; + typedef Eigen::Matrix VectorK6; /// move a cameras according to d PinholeCamera retract(const Vector& d) const { @@ -316,7 +319,7 @@ public: */ Point2 project2( const Point3& pw, // - OptionalJacobian<2, DimC> Dcamera = boost::none, + OptionalJacobian<2, dimension> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { const Point3 pc = pose_.transform_to(pw); @@ -366,7 +369,7 @@ public: */ double range( const Point3& point, // - OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { Matrix16 Dpose_; double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); @@ -384,7 +387,7 @@ public: */ double range( const Pose3& pose, // - OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { Matrix16 Dpose_; double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); @@ -403,7 +406,7 @@ public: template double range( const PinholeCamera& camera, // - OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, dimension> Dcamera = boost::none, // OptionalJacobian<1, 6 + traits::dimension::value> Dother = boost::optional Dother = boost::none) const { @@ -431,7 +434,7 @@ public: */ double range( const CalibratedCamera& camera, // - OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { return range(camera.pose(), Dcamera, Dother); } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index b4c030b19..3b8d65275 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -35,16 +35,22 @@ namespace gtsam { * @addtogroup SLAM */ template - class GeneralSFMFactor: public NoiseModelFactor2 { + class GeneralSFMFactor: public NoiseModelFactor2 { + + GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) + GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) + + static const int DimC = FixedDimension::value; + static const int DimL = FixedDimension::value; + protected: + Point2 measured_; ///< the 2D measurement public: - typedef CAMERA Cam; ///< typedef for camera type - typedef GeneralSFMFactor This; ///< typedef for this object + typedef GeneralSFMFactor This; ///< typedef for this object typedef NoiseModelFactor2 Base; ///< typedef for the base class - typedef Point2 Measurement; ///< typedef for the measurement // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -89,7 +95,7 @@ namespace gtsam { } /** h(x)-z */ - Vector evaluateError(const Cam& camera, const Point3& point, + Vector evaluateError(const CAMERA& camera, const LANDMARK& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const { try { @@ -97,8 +103,8 @@ namespace gtsam { return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, camera.dim()); - if (H2) *H2 = zeros(2, point.dim()); + if (H1) *H1 = zeros(2, DimC); + if (H2) *H2 = zeros(2, DimL); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; return zero(2); @@ -132,15 +138,19 @@ namespace gtsam { */ template class GeneralSFMFactor2: public NoiseModelFactor3 { + + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) + static const int DimK = FixedDimension::value; + protected: - Point2 measured_; ///< the 2D measurement + + Point2 measured_; ///< the 2D measurement public: typedef GeneralSFMFactor2 This; typedef PinholeCamera Camera; ///< typedef for camera type typedef NoiseModelFactor3 Base; ///< typedef for the base class - typedef Point2 Measurement; ///< typedef for the measurement // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -194,9 +204,9 @@ namespace gtsam { return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, pose3.dim()); - if (H2) *H2 = zeros(2, point.dim()); - if (H3) *H3 = zeros(2, calib.dim()); + if (H1) *H1 = zeros(2, 6); + if (H2) *H2 = zeros(2, 3); + if (H3) *H3 = zeros(2, DimK); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; }