Removed all fixed hacks - replaced it with possibly something worse (no reference returned from at)

gtsam.h

@@ -1722,9 +1722,6 @@ class Values {
   void insert(size_t j, Vector t);
   void insert(size_t j, Matrix t);
 
-  // Fixed size version
-  void insertFixed(size_t j, Vector t, size_t n);
-
   void update(size_t j, const gtsam::Point2& t);
   void update(size_t j, const gtsam::Point3& t);
   void update(size_t j, const gtsam::Rot2& t);
@@ -1742,12 +1739,6 @@ class Values {
   template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
   T at(size_t j);
 
-  /// Fixed size versions, for n in 1..9
-  void insertFixed(size_t j, Vector t, size_t n);
-
-  /// Fixed size versions, for n in 1..9
-  Vector atFixed(size_t j, size_t n);
-  
   /// version for double
   void insertDouble(size_t j, double c);
   double atDouble(size_t j) const;

gtsam/nonlinear/Values-inl.h

@@ -252,23 +252,97 @@ namespace gtsam {
      return filter(key_value.key);
    }
 
-  /* ************************************************************************* */
+   /* ************************************************************************* */
-  template<typename ValueType>
-  const ValueType& Values::at(Key j) const {
-    // Find the item
-    KeyValueMap::const_iterator item = values_.find(j);
 
-    // Throw exception if it does not exist
+    namespace internal {
-    if(item == values_.end())
-      throw ValuesKeyDoesNotExist("retrieve", j);
 
     // Check the type and throw exception if incorrect
-    try {
+    template<typename ValueType>
-      return dynamic_cast<const GenericValue<ValueType>&>(*item->second).value();
+    struct handle {
-    } catch (std::bad_cast &) {
+      ValueType operator()(Key j, const gtsam::Value * const pointer) {
-      throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
+        try {
-    }
+          return dynamic_cast<const GenericValue<ValueType>&>(*pointer).value();
-  }
+        } catch (std::bad_cast &) {
+          throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
+        }
+      }
+    };
+
+    // Handle dynamic vectors
+    template<>
+    struct handle<Eigen::Matrix<double, -1, 1> > {
+       Eigen::Matrix<double, -1, 1> operator()(Key j,
+          const gtsam::Value * const pointer) {
+        try {
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          throw ValuesIncorrectType(j, typeid(*pointer),
+              typeid(Eigen::Matrix<double, -1, 1>));
+        }
+      }
+    };
+
+    // Handle dynamic matrices
+    template<int N>
+    struct handle<Eigen::Matrix<double, -1, N> > {
+       Eigen::Matrix<double, -1, N> operator()(Key j,
+          const gtsam::Value * const pointer) {
+        try {
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, N> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          throw ValuesIncorrectType(j, typeid(*pointer),
+              typeid(Eigen::Matrix<double, -1, N>));
+        }
+      }
+    };
+
+    // Request for a fixed vector
+    template<int M>
+    struct handle<Eigen::Matrix<double, M, 1> > {
+      Eigen::Matrix<double, M, 1> operator()(Key j,
+          const gtsam::Value * const pointer) {
+        try {
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, 1> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          Matrix A = handle<Eigen::VectorXd>()(j, pointer);
+          if (A.rows() != M || A.cols() != 1)
+            throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols());
+          else
+            return A;
+        }
+      }
+    };
+
+    // Request for a fixed matrix
+    template<int M, int N>
+    struct handle<Eigen::Matrix<double, M, N> > {
+      Eigen::Matrix<double, M, N> operator()(Key j,
+          const gtsam::Value * const pointer) {
+        try {
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          Matrix A = handle<Eigen::MatrixXd>()(j, pointer);
+          if (A.rows() != M || A.cols() != N)
+            throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
+          else
+            return A;
+        }
+      }
+    };
+    } // internal
+
+   /* ************************************************************************* */
+   template<typename ValueType>
+   ValueType Values::at(Key j) const {
+     // Find the item
+     KeyValueMap::const_iterator item = values_.find(j);
+
+     // Throw exception if it does not exist
+     if(item == values_.end())
+       throw ValuesKeyDoesNotExist("at", j);
+
+     return internal::handle<ValueType>()(j,item->second);
+   }
 
   /* ************************************************************************* */
   template<typename ValueType>

gtsam/nonlinear/Values.cpp

@@ -25,8 +25,6 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/linear/VectorValues.h>
 
-#include <list>
-
 #include <boost/foreach.hpp>
 #ifdef __GNUC__
 #pragma GCC diagnostic push
@@ -38,6 +36,9 @@
 #endif
 #include <boost/iterator/transform_iterator.hpp>
 
+#include <list>
+#include <sstream>
+
 using namespace std;
 
 namespace gtsam {
@@ -112,24 +113,6 @@ namespace gtsam {
     return result;
   }
 
-  /* ************************************************************************* */
-  Vector Values::atFixed(Key j,  size_t n) {
-    switch (n) {
-    case 1: return at<Vector1>(j);
-    case 2: return at<Vector2>(j);
-    case 3: return at<Vector3>(j);
-    case 4: return at<Vector4>(j);
-    case 5: return at<Vector5>(j);
-    case 6: return at<Vector6>(j);
-    case 7: return at<Vector7>(j);
-    case 8: return at<Vector8>(j);
-    case 9: return at<Vector9>(j);
-    default:
-      throw runtime_error(
-          "Values::at fixed size can only handle n in 1..9");
-    }
-  }
-
   /* ************************************************************************* */
   const Value& Values::at(Key j) const {
     // Find the item
@@ -148,24 +131,6 @@ namespace gtsam {
       throw ValuesKeyAlreadyExists(j);
   }
 
-  /* ************************************************************************* */
-  void Values::insertFixed(Key j, const Vector& v, size_t n) {
-    switch (n) {
-    case 1: insert<Vector1>(j,v); break;
-    case 2: insert<Vector2>(j,v); break;
-    case 3: insert<Vector3>(j,v); break;
-    case 4: insert<Vector4>(j,v); break;
-    case 5: insert<Vector5>(j,v); break;
-    case 6: insert<Vector6>(j,v); break;
-    case 7: insert<Vector7>(j,v); break;
-    case 8: insert<Vector8>(j,v); break;
-    case 9: insert<Vector9>(j,v); break;
-    default:
-      throw runtime_error(
-          "Values::insert fixed size can only handle n in 1..9");
-    }
-  }
-
   /* ************************************************************************* */
   void Values::insert(const Values& values) {
     for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
@@ -267,4 +232,18 @@ namespace gtsam {
     return message_.c_str();
   }
 
+  /* ************************************************************************* */
+  const char* NoMatchFoundForFixed::what() const throw() {
+    if(message_.empty()) {
+      ostringstream oss;
+    oss
+        << "Attempting to retrieve fixed-size matrix with dimensions " //
+        << M1_ << "x" << N1_
+        << ", but found dynamic Matrix with mismatched dimensions " //
+        << M2_ << "x" << N2_;
+      message_ = oss.str();
+    }
+    return message_.c_str();
+  }
+
 }

gtsam/nonlinear/Values.h

@@ -175,14 +175,10 @@ namespace gtsam {
      * @param j Retrieve the value associated with this key
      * @tparam Value The type of the value stored with this key, this method
      * throws DynamicValuesIncorrectType if this requested type is not correct.
-     * @return A const reference to the stored value
+     * @return The stored value
      */
     template<typename ValueType>
-    const ValueType& at(Key j) const;
+    ValueType at(Key j) const;
-
-    /// Special version for small fixed size vectors, for matlab/python
-    /// throws truntime error if n<1 || n>9
-    Vector atFixed(Key j, size_t n);
 
     /// version for double
     double atDouble(size_t key) const { return at<double>(key);}
@@ -264,10 +260,6 @@ namespace gtsam {
     template <typename ValueType>
     void insert(Key j, const ValueType& val);
 
-    /// Special version for small fixed size vectors, for matlab/python
-    /// throws truntime error if n<1 || n>9
-    void insertFixed(Key j, const Vector& v, size_t n);
-
     /// version for double
     void insertDouble(Key j, double c) { insert<double>(j,c); }
 
@@ -505,6 +497,28 @@ namespace gtsam {
     }
   };
 
+  /* ************************************************************************* */
+  class GTSAM_EXPORT NoMatchFoundForFixed: public std::exception {
+
+  protected:
+    const size_t M1_, N1_;
+    const size_t M2_, N2_;
+
+  private:
+    mutable std::string message_;
+
+  public:
+    NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () :
+        M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
+    }
+
+    virtual ~NoMatchFoundForFixed() throw () {
+    }
+
+    virtual const char* what() const throw ();
+  };
+
+  /* ************************************************************************* */
   /// traits
   template<>
   struct traits<Values> : public Testable<Values> {

gtsam/nonlinear/tests/testValues.cpp

@@ -476,13 +476,22 @@ TEST(Values, Destructors) {
 }
 
 /* ************************************************************************* */
-TEST(Values, FixedSize) {
+TEST(Values, FixedVector) {
   Values values;
   Vector v(3); v << 5.0, 6.0, 7.0;
-  values.insertFixed(key1, v, 3);
+  values.insert(key1, v);
   Vector3 expected(5.0, 6.0, 7.0);
   CHECK(assert_equal((Vector)expected, values.at<Vector3>(key1)));
-  CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error);
+  CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
+}
+/* ************************************************************************* */
+TEST(Values, FixedMatrix) {
+  Values values;
+  Matrix v(1,3); v << 5.0, 6.0, 7.0;
+  values.insert(key1, v);
+  Vector3 expected(5.0, 6.0, 7.0);
+  CHECK(assert_equal((Vector)expected, values.at<Matrix13>(key1)));
+  CHECK_EXCEPTION(values.at<Matrix23>(key1), exception);
 }
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }