New FixedRef type

gtsam/base/Matrix.h

@@ -534,6 +534,75 @@ Eigen::Matrix<double, N, N> CayleyFixed(const Eigen::Matrix<double, N, N>& A) {
 
 std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
 
+/**
+ * Eigen::Ref like class that cane take either a fixed size or dynamic
+ * Eigen matrix. In the latter case, the dynamic matrix will be resized.
+ * Finally, the default constructor acts like boost::none.
+ */
+template<int Rows, int Cols>
+class FixedRef {
+
+public:
+
+  /// Fixed size type
+  typedef Eigen::Matrix<double, Rows, Cols> Fixed;
+
+private:
+
+  bool empty_;
+  Eigen::Map<Fixed> map_;
+
+  // Trick on http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
+  // to make map_ usurp the memory of the fixed size matrix
+  void usurp(double* data) {
+    new (&map_) Eigen::Map<Fixed>(data);
+  }
+
+public:
+
+  /// Defdault constructo acts like boost::none
+  FixedRef() :
+      empty_(true), map_(NULL) {
+  }
+
+  /// Defdault constructo acts like boost::none
+  FixedRef(boost::none_t none) :
+      empty_(true), map_(NULL) {
+  }
+
+  /// Constructor that will usurp data of a fixed size matrix
+  FixedRef(Fixed& fixed) :
+      empty_(false), map_(NULL) {
+    usurp(fixed.data());
+  }
+
+  /// Constructor that will resize a dynamic matrix
+  FixedRef(Matrix& dynamic) :
+      empty_(false), map_(NULL) {
+    dynamic.resize(Rows, Cols);
+    usurp(dynamic.data());
+  }
+
+  /// Constructor compatible with old-style
+  FixedRef(const boost::optional<Matrix&> optional) :
+      empty_(!optional), map_(NULL) {
+    if (optional) {
+      optional->resize(Rows, Cols);
+      usurp(optional->data());
+    }
+  }
+
+  /// Return true is allocated, false if default constructor was used
+  operator bool() const {
+    return !empty_;
+  }
+
+  /// De-reference, like boost optional
+  Eigen::Map<Fixed>& operator* () {
+    return map_;
+  }
+};
+
 } // namespace gtsam
 
 #include <boost/serialization/nvp.hpp>