Merged in feature/Feature/FixedValues (pull request #103)

Attempt at removing fixed Vector wrapping

gtsam.h

@@ -1798,9 +1798,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);
@@ -1818,12 +1815,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

@@ -269,25 +269,114 @@ 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
-    const Value& value = *item->second;
+    // Generic version, partially specialized below for various Eigen Matrix types
-    try {
+    template<typename ValueType>
-      return dynamic_cast<const GenericValue<ValueType>&>(value).value();
+    struct handle {
-    } catch (std::bad_cast &) {
+      ValueType operator()(Key j, const gtsam::Value * const pointer) {
-      // NOTE(abe): clang warns about potential side effects if done in typeid
+        try {
-      const Value* value = item->second;
+          // value returns a const ValueType&, and the return makes a copy !!!!!
-      throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
+          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 {
+          // value returns a const Vector&, and the return makes a copy !!!!!
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, 1> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          // If a fixed vector was stored, we end up here as well.
+          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 {
+          // value returns a const Matrix&, and the return makes a copy !!!!!
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, -1, N> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          // If a fixed matrix was stored, we end up here as well.
+          throw ValuesIncorrectType(j, typeid(*pointer),
+              typeid(Eigen::Matrix<double, -1, N>));
+        }
+      }
+    };
+
+    // Request for a fixed vector
+    // TODO(jing): is this piece of code really needed ???
+    template<int M>
+    struct handle<Eigen::Matrix<double, M, 1> > {
+      Eigen::Matrix<double, M, 1> operator()(Key j,
+          const gtsam::Value * const pointer) {
+        try {
+          // value returns a const VectorM&, and the return makes a copy !!!!!
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, 1> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          // Check if a dynamic vector was stored
+          Matrix A = handle<Eigen::VectorXd>()(j, pointer); // will throw if not....
+          // Yes: check size, and throw if not a match
+          if (A.rows() != M || A.cols() != 1)
+            throw NoMatchFoundForFixed(M, 1, A.rows(), A.cols());
+          else
+            // This is not a copy because of RVO
+            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 {
+          // value returns a const MatrixMN&, and the return makes a copy !!!!!
+          return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N> >&>(*pointer).value();
+        } catch (std::bad_cast &) {
+          // Check if a dynamic matrix was stored
+          Matrix A = handle<Eigen::MatrixXd>()(j, pointer); // will throw if not....
+          // Yes: check size, and throw if not a match
+          if (A.rows() != M || A.cols() != N)
+            throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
+          else
+            // This is not a copy because of RVO
+            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);
+
+    // Check the type and throw exception if incorrect
+    return internal::handle<ValueType>()(j,item->second);
   }
 
   /* ************************************************************************* */
@@ -312,16 +401,48 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
+
+  // wrap all fixed in dynamics when insert and update
+  namespace internal {
+
+  // general type
+  template<typename ValueType>
+  struct handle_wrap {
+    inline gtsam::GenericValue<ValueType> operator()(Key j, const ValueType& val) {
+      return gtsam::GenericValue<ValueType>(val);
+    }
+  };
+
+  // when insert/update a fixed size vector: convert to dynamic size
+  template<int M>
+  struct handle_wrap<Eigen::Matrix<double, M, 1> > {
+    inline gtsam::GenericValue<Eigen::Matrix<double, -1, 1> > operator()(
+        Key j, const Eigen::Matrix<double, M, 1>& val) {
+      return gtsam::GenericValue<Eigen::Matrix<double, -1, 1> >(val);
+    }
+  };
+
+  // when insert/update a fixed size matrix: convert to dynamic size
+  template<int M, int N>
+  struct handle_wrap<Eigen::Matrix<double, M, N> > {
+    inline gtsam::GenericValue<Eigen::Matrix<double, -1, -1> > operator()(
+        Key j, const Eigen::Matrix<double, M, N>& val) {
+      return gtsam::GenericValue<Eigen::Matrix<double, -1, -1> >(val);
+    }
+  };
+
+  }
+
   // insert a templated value
   template<typename ValueType>
-   void Values::insert(Key j, const ValueType& val) {
+  void Values::insert(Key j, const ValueType& val) {
-     insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
+    insert(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val)));
-   }
+  }
 
   // update with templated value
   template <typename ValueType>
   void Values::update(Key j, const ValueType& val) {
-    update(j, static_cast<const Value&>(GenericValue<ValueType >(val)));
+    update(j, static_cast<const Value&>(internal::handle_wrap<ValueType>()(j, val)));
   }
 
 }

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) {
@@ -269,4 +234,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

@@ -168,16 +168,13 @@ namespace gtsam {
     /** Retrieve a variable by key \c j.  The type of the value associated with
      * this key is supplied as a template argument to this function.
      * @param j Retrieve the value associated with this key
-     * @tparam Value The type of the value stored with this key, this method
+     * @tparam ValueType The type of the value stored with this key, this method
-     * throws DynamicValuesIncorrectType if this requested type is not correct.
+     * Throws DynamicValuesIncorrectType if this requested type is not correct.
-     * @return A const reference to the stored value
+     * Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa.
+     * @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);}
@@ -259,10 +256,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); }
 
@@ -500,6 +493,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/expressions.h

@@ -25,7 +25,7 @@ Expression<T> compose(const Expression<T>& t1, const Expression<T>& t2) {
 }
 
 // Some typedefs
-typedef Expression<double> double_;
+typedef Expression<double> Double_;
 typedef Expression<Vector1> Vector1_;
 typedef Expression<Vector2> Vector2_;
 typedef Expression<Vector3> Vector3_;

gtsam/nonlinear/tests/testExpression.cpp

@@ -17,7 +17,7 @@
  * @brief unit tests for Block Automatic Differentiation
  */
 
-#include <gtsam/nonlinear/Expression.h>
+#include <gtsam/nonlinear/expressions.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point3.h>
@@ -32,9 +32,7 @@ using boost::assign::map_list_of;
 using namespace std;
 using namespace gtsam;
 
-typedef Expression<double> double_;
 typedef Expression<Point3> Point3_;
-typedef Expression<Vector3> Vector3_;
 typedef Expression<Pose3> Pose3_;
 typedef Expression<Rot3> Rot3_;
 
@@ -101,7 +99,7 @@ TEST(Expression, Unary1) {
 }
 TEST(Expression, Unary2) {
   using namespace unary;
-  double_ e(f2, p);
+  Double_ e(f2, p);
   EXPECT(expected == e.keys());
 }
 
@@ -156,7 +154,7 @@ Point3_ p_cam(x, &Pose3::transform_to, p);
 // Check that creating an expression to double compiles
 TEST(Expression, BinaryToDouble) {
   using namespace binary;
-  double_ p_cam(doubleF, x, p);
+  Double_ p_cam(doubleF, x, p);
 }
 
 /* ************************************************************************* */
@@ -372,8 +370,8 @@ TEST(Expression, TripleSum) {
 /* ************************************************************************* */
 TEST(Expression, SumOfUnaries) {
   const Key key(67);
-  const double_ norm_(&gtsam::norm, Point3_(key));
+  const Double_ norm_(&gtsam::norm, Point3_(key));
-  const double_ sum_ = norm_ + norm_;
+  const Double_ sum_ = norm_ + norm_;
 
   Values values;
   values.insert<Point3>(key, Point3(6, 0, 0));
@@ -391,7 +389,7 @@ TEST(Expression, SumOfUnaries) {
 TEST(Expression, UnaryOfSum) {
   const Key key1(42), key2(67);
   const Point3_ sum_ = Point3_(key1) + Point3_(key2);
-  const double_ norm_(&gtsam::norm, sum_);
+  const Double_ norm_(&gtsam::norm, sum_);
 
   map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
   norm_.dims(actual_dims);

gtsam/nonlinear/tests/testValues.cpp

@@ -477,13 +477,59 @@ TEST(Values, Destructors) {
 }
 
 /* ************************************************************************* */
-TEST(Values, FixedSize) {
+TEST(Values, VectorDynamicInsertFixedRead) {
   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)));
+  Vector3 actual = values.at<Vector3>(key1);
-  CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error);
+  CHECK(assert_equal(expected, actual));
+  CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
+}
+
+/* ************************************************************************* */
+TEST(Values, VectorDynamicInsertDynamicRead) {
+  Values values;
+  Vector v(3); v << 5.0, 6.0, 7.0;
+  values.insert(key1, v);
+  Vector expected(3); expected << 5.0, 6.0, 7.0;
+  Vector actual = values.at<Vector>(key1);
+  LONGS_EQUAL(3, actual.rows());
+  LONGS_EQUAL(1, actual.cols());
+  CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(Values, VectorFixedInsertFixedRead) {
+  Values values;
+  Vector3 v; v << 5.0, 6.0, 7.0;
+  values.insert(key1, v);
+  Vector3 expected; expected << 5.0, 6.0, 7.0;
+  Vector3 actual = values.at<Vector3>(key1);
+  CHECK(assert_equal(expected, actual));
+  CHECK_EXCEPTION(values.at<Vector7>(key1), exception);
+}
+
+/* ************************************************************************* */
+TEST(Values, VectorFixedInsertDynamicRead) {
+  Values values;
+  Vector3 v; v << 5.0, 6.0, 7.0;
+  values.insert(key1, v);
+  Vector expected(3); expected << 5.0, 6.0, 7.0;
+  Vector actual = values.at<Vector>(key1);
+  LONGS_EQUAL(3, actual.rows());
+  LONGS_EQUAL(1, actual.cols());
+  CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST(Values, MatrixDynamicInsertFixedRead) {
+  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); }

gtsam_unstable/slam/TOAFactor.h

@@ -17,7 +17,7 @@
  *  @date December 2014
  */
 
-#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
 #include <gtsam_unstable/geometry/Event.h>
 
 namespace gtsam {
@@ -25,7 +25,7 @@ namespace gtsam {
 /// A "Time of Arrival" factor - so little code seems hardly worth it :-)
 class TOAFactor: public ExpressionFactor<double> {
 
-  typedef Expression<double> double_;
+  typedef Expression<double> Double_;
 
 public:
 
@@ -40,7 +40,7 @@ public:
       const Expression<Point3>& microphone_, double toaMeasurement,
       const SharedNoiseModel& model) :
       ExpressionFactor<double>(model, toaMeasurement,
-          double_(&Event::toa, eventExpression, microphone_)) {
+          Double_(&Event::toa, eventExpression, microphone_)) {
   }
 
 };

gtsam_unstable/slam/tests/testTOAFactor.cpp

@@ -31,7 +31,6 @@ using namespace std;
 using namespace gtsam;
 
 // typedefs
-typedef Eigen::Matrix<double, 1, 1> Vector1;
 typedef Expression<Point3> Point3_;
 typedef Expression<Event> Event_;
 
@@ -52,7 +51,7 @@ TEST( TOAFactor, NewWay ) {
   Event_ eventExpression(key);
   Point3_ microphoneConstant(microphoneAt0); // constant expression
   double measurement = 7;
-  double_ expression(&Event::toa, eventExpression, microphoneConstant);
+  Double_ expression(&Event::toa, eventExpression, microphoneConstant);
   ExpressionFactor<double> factor(model, measurement, expression);
 }
 
@@ -92,7 +91,7 @@ TEST( TOAFactor, WholeEnchilada ) {
   Event_ eventExpression(key);
   for (size_t i = 0; i < K; i++) {
     Point3_ microphone_i(microphones[i]); // constant expression
-    double_ predictTOA(&Event::toa, eventExpression, microphone_i);
+    Double_ predictTOA(&Event::toa, eventExpression, microphone_i);
     graph.addExpressionFactor(predictTOA, simulatedTOA[i], model);
   }
 

matlab/+gtsam/plot3DPoints.m

@@ -7,7 +7,7 @@ function plot3DPoints(values, linespec, marginals)
 import gtsam.*
 
 if ~exist('linespec', 'var') || isempty(linespec)
-    linespec = 'g';
+    linespec = 'g*';
 end
 haveMarginals = exist('marginals', 'var');
 keys = KeyVector(values.keys);
@@ -15,19 +15,25 @@ keys = KeyVector(values.keys);
 holdstate = ishold;
 hold on
 
-% Plot points and covariance matrices
+if haveMarginals
-for i = 0:keys.size-1
+    % Plot points and covariance matrices (slow)
-    key = keys.at(i);
+    for i = 0:keys.size-1
-    try
+        key = keys.at(i);
-        p = values.atPoint3(key);
+        try
-        if haveMarginals
+            p = values.atPoint3(key)
             P = marginals.marginalCovariance(key);
             gtsam.plotPoint3(p, linespec, P);
-        else
+        catch
-            gtsam.plotPoint3(p, linespec);
+            % I guess it's not a Point3
         end
-    catch
+    end
-        % I guess it's not a Point3
+else
+    % Extract all in C++ and plot all at once (fast)
+    P = utilities.extractPoint3(values);
+    if size(linespec,2)==1
+        plot3(P(:,1),P(:,2),P(:,3),[linespec '*']);
+    else
+        plot3(P(:,1),P(:,2),P(:,3),linespec);
     end
 end
 

python/handwritten/nonlinear/Values.cpp

@@ -56,7 +56,6 @@ void exportValues(){
   .def("empty", &Values::empty)
   .def("equals", &Values::equals)
   .def("erase", &Values::erase)
-  .def("insert_fixed", &Values::insertFixed)
   .def("print", &Values::print, print_overloads(args("s")))
   .def("size", &Values::size)
   .def("swap", &Values::swap)
@@ -69,15 +68,15 @@ void exportValues(){
   .def("insert", insert_bias)
   .def("insert", insert_navstate)
   .def("insert", insert_vector3)
-  .def("atPoint2", &Values::at<Point2>, return_value_policy<copy_const_reference>())
+  .def("atPoint2", &Values::at<Point2>)
-  .def("atRot2", &Values::at<Rot2>, return_value_policy<copy_const_reference>())
+  .def("atRot2", &Values::at<Rot2>)
-  .def("atPose2", &Values::at<Pose2>, return_value_policy<copy_const_reference>())
+  .def("atPose2", &Values::at<Pose2>)
-  .def("atPoint3", &Values::at<Point3>, return_value_policy<copy_const_reference>())
+  .def("atPoint3", &Values::at<Point3>)
-  .def("atRot3", &Values::at<Rot3>, return_value_policy<copy_const_reference>())
+  .def("atRot3", &Values::at<Rot3>)
-  .def("atPose3", &Values::at<Pose3>, return_value_policy<copy_const_reference>())
+  .def("atPose3", &Values::at<Pose3>)
-  .def("atConstantBias", &Values::at<Bias>, return_value_policy<copy_const_reference>())
+  .def("atConstantBias", &Values::at<Bias>)
-  .def("atNavState", &Values::at<NavState>, return_value_policy<copy_const_reference>())
+  .def("atNavState", &Values::at<NavState>)
-  .def("atVector3", &Values::at<Vector3>, return_value_policy<copy_const_reference>())
+  .def("atVector3", &Values::at<Vector3>)
   .def("exists", exists1)
   .def("keys", &Values::keys)
   ;