FixedDimension

gtsam/base/Manifold.h

@@ -127,7 +127,9 @@ check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
  */
 template<typename M>
 class IsManifold {
+
 public:
+
   typedef typename traits_x<M>::structure_category structure_category_tag;
   static const size_t dim = traits_x<M>::dimension;
   typedef typename traits_x<M>::ManifoldType ManifoldType;
@@ -141,19 +143,30 @@ public:
     BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
 
     // make sure Chart methods are defined
-    v = traits_x<M>::Local(p,q);
+    v = traits_x<M>::Local(p, q);
-    q = traits_x<M>::Retract(p,v);
+    q = traits_x<M>::Retract(p, v);
     // and the versions with Jacobians.
     //v = traits_x<M>::Local(p,q,Hp,Hq);
     //q = traits_x<M>::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<typename M>
+struct FixedDimension {
+  typedef const int value_type;
+  static const int value = traits_x<M>::dimension;
+  BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
+      "FixedDimension instantiated for dymanically-sized type.");
+};
+
 } // \ namespace gtsam
 
 ///**

gtsam/geometry/PinholeCamera.h

@@ -31,16 +31,19 @@ namespace gtsam {
  */
 template<typename Calibration>
 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<Calibration>::dimension, //
+  static const int DimK = FixedDimension<Calibration>::value;
-      DimC = 6 + DimK;
 
 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<double, DimC, 1> VectorK6;
+  typedef Eigen::Matrix<double, dimension, 1> 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<class CalibrationB>
   double range(
       const PinholeCamera<CalibrationB>& camera, //
-      OptionalJacobian<1, DimC> Dcamera = boost::none,
+      OptionalJacobian<1, dimension> Dcamera = boost::none,
 //      OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
        boost::optional<Matrix&> 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);
   }

gtsam/slam/GeneralSFMFactor.h

@@ -36,15 +36,21 @@ namespace gtsam {
    */
   template <class CAMERA, class LANDMARK>
   class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
+
+    GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
+    GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
+
+    static const int DimC = FixedDimension<CAMERA>::value;
+    static const int DimL = FixedDimension<LANDMARK>::value;
+
   protected:
+
     Point2 measured_;      ///< the 2D measurement
 
   public:
 
-    typedef CAMERA Cam;                        ///< typedef for camera type
     typedef GeneralSFMFactor<CAMERA, LANDMARK> This;   ///< typedef for this object
     typedef NoiseModelFactor2<CAMERA, LANDMARK> 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<This> 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<Matrix&> H1=boost::none, boost::optional<Matrix&> 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 (H1) *H1 = zeros(2, DimC);
-        if (H2) *H2 = zeros(2, point.dim());
+        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,7 +138,12 @@ namespace gtsam {
    */
   template <class CALIBRATION>
   class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
+
+    GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
+    static const int DimK = FixedDimension<CALIBRATION>::value;
+
   protected:
+
     Point2 measured_; ///< the 2D measurement
 
   public:
@@ -140,7 +151,6 @@ namespace gtsam {
     typedef GeneralSFMFactor2<CALIBRATION> This;
     typedef PinholeCamera<CALIBRATION> Camera;                  ///< typedef for camera type
     typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> 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<This> shared_ptr;
@@ -194,9 +204,9 @@ namespace gtsam {
         return reprojError.vector();
       }
       catch( CheiralityException& e) {
-        if (H1) *H1 = zeros(2, pose3.dim());
+        if (H1) *H1 = zeros(2, 6);
-        if (H2) *H2 = zeros(2, point.dim());
+        if (H2) *H2 = zeros(2, 3);
-        if (H3) *H3 = zeros(2, calib.dim());
+        if (H3) *H3 = zeros(2, DimK);
         std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
                               << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
       }