Merged in feature/BAD_generic_value_traits (pull request #24)

GenericValue to replace DerivedValue

gtsam/base/ChartValue.h

@@ -0,0 +1,190 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * @file ChartValue.h
+ * @date Jan 26, 2012
+ * @author Michael Bosse, Abel Gawel, Renaud Dube
+ * based on DrivedValue.h by Duy Nguyen Ta
+ */
+
+#pragma once
+
+#include <gtsam/base/GenericValue.h>
+#include <gtsam/base/Manifold.h>
+#include <boost/make_shared.hpp>
+
+//////////////////
+// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR
+#include <boost/pool/singleton_pool.hpp>
+
+#ifdef min
+#undef min
+#endif
+
+#ifdef max
+#undef max
+#endif
+
+#ifdef ERROR
+#undef ERROR
+#endif
+//////////////////
+
+
+namespace gtsam {
+
+// ChartValue is derived from GenericValue<T> and Chart so that Chart can be zero sized (as in DefaultChart<T>)
+// if the Chart is a member variable then it won't ever be zero sized.
+template<class T, class Chart_=DefaultChart<T> >
+class ChartValue : public GenericValue<T>, public Chart_ {
+  BOOST_CONCEPT_ASSERT((ChartConcept<Chart_>));
+ public:
+  typedef T type;
+  typedef Chart_ Chart;
+
+ public:
+  ChartValue() : GenericValue<T>(T()) {}
+  ChartValue(const T& value) : GenericValue<T>(value) {}
+  template<typename C>
+  ChartValue(const T& value, C chart_initializer) : GenericValue<T>(value), Chart(chart_initializer) {}
+
+  virtual ~ChartValue() {}
+
+  /**
+   * Create a duplicate object returned as a pointer to the generic Value interface.
+   * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
+   * The result must be deleted with Value::deallocate_, not with the 'delete' operator.
+   */
+  virtual Value* clone_() const {
+    void *place = boost::singleton_pool<PoolTag, sizeof(ChartValue)>::malloc();
+    ChartValue* ptr = new(place) ChartValue(*this); // calls copy constructor to fill in
+    return ptr;
+  }
+
+  /**
+   * Destroy and deallocate this object, only if it was originally allocated using clone_().
+   */
+  virtual void deallocate_() const {
+    this->~ChartValue(); // Virtual destructor cleans up the derived object
+    boost::singleton_pool<PoolTag, sizeof(ChartValue)>::free((void*)this); // Release memory from pool
+  }
+
+  /**
+   * Clone this value (normal clone on the heap, delete with 'delete' operator)
+   */
+  virtual boost::shared_ptr<Value> clone() const {
+    return boost::make_shared<ChartValue>(*this);
+  }
+
+  /// just use the equals_ defined in GenericValue
+  // virtual bool equals_(const Value& p, double tol = 1e-9) const {
+  //  }
+
+  /// Chart Value interface version of retract
+  virtual Value* retract_(const Vector& delta) const {
+    // Call retract on the derived class using the retract trait function
+    const T retractResult = Chart::retract(GenericValue<T>::value(), delta);
+
+    // Create a Value pointer copy of the result
+    void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(ChartValue)>::malloc();
+    Value* resultAsValue = new(resultAsValuePlace) ChartValue(retractResult, static_cast<const Chart&>(*this));
+
+    // Return the pointer to the Value base class
+    return resultAsValue;
+  }
+
+  /// Generic Value interface version of localCoordinates
+  virtual Vector localCoordinates_(const Value& value2) const {
+    // Cast the base class Value pointer to a templated generic class pointer
+    const GenericValue<T>& genericValue2 = static_cast<const GenericValue<T>&>(value2);
+
+    // Return the result of calling localCoordinates trait on the derived class
+    return Chart::local(GenericValue<T>::value(), genericValue2.value());
+  }
+
+  /// Non-virtual version of retract
+  ChartValue retract(const Vector& delta) const {
+    return ChartValue(Chart::retract(GenericValue<T>::value(), delta),static_cast<const Chart&>(*this));
+  }
+
+  /// Non-virtual version of localCoordinates
+  Vector localCoordinates(const ChartValue& value2) const {
+    return localCoordinates_(value2);
+  }
+
+  virtual size_t dim() const {
+    return Chart::getDimension(GenericValue<T>::value()); // need functional form here since the dimension may be dynamic
+  }
+
+  /// Assignment operator
+  virtual Value& operator=(const Value& rhs) {
+    // Cast the base class Value pointer to a derived class pointer
+    const ChartValue& derivedRhs = static_cast<const ChartValue&>(rhs);
+
+    // Do the assignment and return the result
+    *this = ChartValue(derivedRhs); // calls copy constructor
+    return *this;
+  }
+
+protected:
+  // implicit assignment operator for (const ChartValue& rhs) works fine here
+  /// Assignment operator, protected because only the Value or DERIVED
+  /// assignment operators should be used.
+  //  DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
+  //    // Nothing to do, do not call base class assignment operator
+  //    return *this;
+  //  }
+
+private:
+  /// Fake Tag struct for singleton pool allocator. In fact, it is never used!
+  struct PoolTag { };
+
+private:
+
+   /// @}
+   /// @name Advanced Interface
+   /// @{
+
+   /** Serialization function */
+   friend class boost::serialization::access;
+   template<class ARCHIVE>
+   void serialize(ARCHIVE & ar, const unsigned int version) {
+     // ar & boost::serialization::make_nvp("value",);
+     // todo: implement a serialization for charts
+     ar & boost::serialization::make_nvp("GenericValue", boost::serialization::base_object< GenericValue<T> >(*this));
+   }
+
+   /// @}
+
+};
+
+namespace traits {
+template <typename T, typename Chart>
+struct dimension<ChartValue<T, Chart> > : public dimension<Chart> {};
+}
+
+template<typename Chart>
+const Chart& Value::getChart() const {
+// define Value::cast here since now ChartValue has been declared
+   return dynamic_cast<const Chart&>(*this);
+ }
+
+/// Convenience function that can be used to make an expression to convert a value to a chart
+template <typename T>
+ChartValue<T> convertToChartValue(const T& value, boost::optional<Eigen::Matrix<double, traits::dimension<T>::value, traits::dimension<T>::value >& > H=boost::none) {
+  if (H) {
+    *H = Eigen::Matrix<double, traits::dimension<T>::value, traits::dimension<T>::value >::Identity();
+  }
+  return ChartValue<T>(value);
+}
+
+} /* namespace gtsam */

gtsam/base/GenericValue.h

@@ -0,0 +1,89 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * @file GenericValue.h
+ * @date Jan 26, 2012
+ * @author Michael Bosse, Abel Gawel, Renaud Dube
+ * based on DrivedValue.h by Duy Nguyen Ta
+ */
+
+#pragma once
+
+#include <gtsam/base/Value.h>
+
+namespace gtsam {
+
+namespace traits {
+
+// trait to wrap the default equals of types
+template<typename T>
+  bool equals(const T& a, const T& b, double tol) {
+    return a.equals(b,tol);
+  }
+
+template<typename T>
+void print(const T& obj, const std::string& str) {
+  obj.print(str);
+}
+
+}
+
+template<class T>
+class GenericValue : public Value {
+public:
+  typedef T type;
+protected:
+  T value_;
+
+public:
+  GenericValue(const T& value) : value_(value) {}
+
+  const T& value() const { return value_; }
+  T& value() { return value_; }
+
+  virtual ~GenericValue() {}
+
+  /// equals implementing generic Value interface
+  virtual bool equals_(const Value& p, double tol = 1e-9) const {
+    // Cast the base class Value pointer to a templated generic class pointer
+    const GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
+    // Return the result of using the equals traits for the derived class
+    return traits::equals<T>(this->value_, genericValue2.value_, tol);
+  }
+
+  // non virtual equals function
+  bool equals(const GenericValue &other, double tol = 1e-9) const {
+    return traits::equals<T>(this->value(),other.value(),tol);
+  }
+
+  virtual void print(const std::string& str) const {
+    traits::print<T>(value_,str);
+  }
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
+    ar & BOOST_SERIALIZATION_NVP(value_);
+  }
+protected:
+  /// Assignment operator for this class not needed since GenricValue<T> is an abstract class
+
+};
+
+// define Value::cast here since now GenericValue has been declared
+template<typename ValueType>
+ const ValueType& Value::cast() const {
+   return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
+ }
+
+
+} /* namespace gtsam */

gtsam/base/LieMatrix.h

@@ -29,7 +29,7 @@ namespace gtsam {
 /**
  * LieVector is a wrapper around vector to allow it to be a Lie type
  */
-struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
+struct LieMatrix : public Matrix {
 
   /// @name Constructors
   /// @{
@@ -166,8 +166,6 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar & boost::serialization::make_nvp("LieMatrix",
-       boost::serialization::base_object<Value>(*this));
     ar & boost::serialization::make_nvp("Matrix",
        boost::serialization::base_object<Matrix>(*this));
 
@@ -179,7 +177,7 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<LieMatrix> : public std::true_type {
+struct is_manifold<LieMatrix> : public boost::true_type {
 };
 
 template<>

gtsam/base/LieScalar.h

@@ -26,7 +26,7 @@ namespace gtsam {
   /**
    * LieScalar is a wrapper around double to allow it to be a Lie type
    */
-  struct GTSAM_EXPORT LieScalar : public DerivedValue<LieScalar> {
+  struct GTSAM_EXPORT LieScalar {
 
     /** default constructor */
     LieScalar() : d_(0.0) {}
@@ -116,15 +116,15 @@ namespace gtsam {
   namespace traits {
 
   template<>
-  struct is_group<LieScalar> : public std::true_type {
+  struct is_group<LieScalar> : public boost::true_type {
   };
 
   template<>
-  struct is_manifold<LieScalar> : public std::true_type {
+  struct is_manifold<LieScalar> : public boost::true_type {
   };
 
   template<>
-  struct dimension<LieScalar> : public std::integral_constant<int, 1> {
+  struct dimension<LieScalar> : public boost::integral_constant<int, 1> {
   };
 
   }

gtsam/base/LieVector.h

@@ -26,7 +26,7 @@ namespace gtsam {
 /**
  * LieVector is a wrapper around vector to allow it to be a Lie type
  */
-struct LieVector : public Vector, public DerivedValue<LieVector> {
+struct LieVector : public Vector {
 
   /** default constructor - should be unnecessary */
   LieVector() {}
@@ -123,8 +123,6 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar & boost::serialization::make_nvp("LieVector",
-       boost::serialization::base_object<Value>(*this));
     ar & boost::serialization::make_nvp("Vector",
        boost::serialization::base_object<Vector>(*this));
   }
@@ -134,7 +132,7 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<LieVector> : public std::true_type {
+struct is_manifold<LieVector> : public boost::true_type {
 };
 
 template<>

gtsam/base/Manifold.h

@@ -20,7 +20,7 @@
 
 #include <gtsam/base/Matrix.h>
 #include <boost/static_assert.hpp>
-#include <type_traits>
+#include <boost/type_traits.hpp>
 #include <string>
 
 namespace gtsam {
@@ -50,7 +50,7 @@ namespace traits {
 
 // is group, by default this is false
 template<typename T>
-struct is_group: public std::false_type {
+struct is_group: public boost::false_type {
 };
 
 // identity, no default provided, by default given by default constructor
@@ -63,16 +63,17 @@ struct identity {
 
 // is manifold, by default this is false
 template<typename T>
-struct is_manifold: public std::false_type {
+struct is_manifold: public boost::false_type {
 };
 
 // dimension, can return Eigen::Dynamic (-1) if not known at compile time
+typedef boost::integral_constant<int, Eigen::Dynamic> Dynamic;
 template<typename T>
-struct dimension;
+struct dimension : public Dynamic {}; //default to dynamic
 
 /**
  * zero<T>::value is intended to be the origin of a canonical coordinate system
- * with canonical(t) == DefaultChart<T>(zero<T>::value).apply(t)
+ * with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
  * Below we provide the group identity as zero *in case* it is a group
  */
 template<typename T> struct zero: public identity<T> {
@@ -82,15 +83,15 @@ template<typename T> struct zero: public identity<T> {
 // double
 
 template<>
-struct is_group<double> : public std::true_type {
+struct is_group<double> : public boost::true_type {
 };
 
 template<>
-struct is_manifold<double> : public std::true_type {
+struct is_manifold<double> : public boost::true_type {
 };
 
 template<>
-struct dimension<double> : public std::integral_constant<int, 1> {
+struct dimension<double> : public boost::integral_constant<int, 1> {
 };
 
 template<>
@@ -103,17 +104,15 @@ struct zero<double> {
 // Fixed size Eigen::Matrix type
 
 template<int M, int N, int Options>
-struct is_group<Eigen::Matrix<double, M, N, Options> > : public std::true_type {
+struct is_group<Eigen::Matrix<double, M, N, Options> > : public boost::true_type {
 };
 
 template<int M, int N, int Options>
-struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public std::true_type {
+struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public boost::true_type {
 };
 
 // TODO: Could be more sophisticated using Eigen traits and SFINAE?
 
-typedef std::integral_constant<int, Eigen::Dynamic> Dynamic;
-
 template<int Options>
 struct dimension<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Options> > : public Dynamic {
 };
@@ -127,12 +126,12 @@ struct dimension<Eigen::Matrix<double, Eigen::Dynamic, N, Options> > : public Dy
 };
 
 template<int M, int N, int Options>
-struct dimension<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant<
+struct dimension<Eigen::Matrix<double, M, N, Options> > : public boost::integral_constant<
     int, M * N> {
 };
 
 template<int M, int N, int Options>
-struct zero<Eigen::Matrix<double, M, N, Options> > : public std::integral_constant<
+struct zero<Eigen::Matrix<double, M, N, Options> > : public boost::integral_constant<
     int, M * N> {
   BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
       "traits::zero is only supported for fixed-size matrices");
@@ -141,62 +140,105 @@ struct zero<Eigen::Matrix<double, M, N, Options> > : public std::integral_consta
   }
 };
 
+template <typename T> struct is_chart : public boost::false_type {};
+
 } // \ namespace traits
 
 // Chart is a map from T -> vector, retract is its inverse
 template<typename T>
 struct DefaultChart {
-  BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
+  //BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
+  typedef T type;
   typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
-  T t_;
+
-  DefaultChart(const T& t) :
+  static vector local(const T& origin, const T& other) {
-      t_(t) {
+    return origin.localCoordinates(other);
   }
-  vector apply(const T& other) {
+  static T retract(const T& origin, const vector& d) {
-    return t_.localCoordinates(other);
+    return origin.retract(d);
   }
-  T retract(const vector& d) {
+  static int getDimension(const T& origin) {
-    return t_.retract(d);
+    return origin.dim();
   }
 };
 
+namespace traits {
+// populate default traits
+template <typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {};
+template <typename T> struct dimension<DefaultChart<T> > : public dimension<T> {};
+}
+
+template<class C>
+struct ChartConcept {
+ public:
+  typedef typename C::type type;
+  typedef typename C::vector vector;
+
+  BOOST_CONCEPT_USAGE(ChartConcept) {
+    // is_chart trait should be true
+    BOOST_STATIC_ASSERT((traits::is_chart<C>::value));
+
     /**
- * Canonical<T>::value is a chart around zero<T>::value
+     * Returns Retraction update of val_
+     */
+    type retract_ret = C::retract(val_, vec_);
+
+    /**
+     * Returns local coordinates of another object
+     */
+    vec_ = C::local(val_, retract_ret);
+
+    // a way to get the dimension that is compatible with dynamically sized types
+    dim_ = C::getDimension(val_);
+  }
+
+ private:
+  type val_;
+  vector vec_;
+  int dim_;
+};
+
+
+/**
+ * CanonicalChart<Chart<T> > is a chart around zero<T>::value
+ * Canonical<T> is CanonicalChart<DefaultChart<T> >
  * An example is Canonical<Rot3>
  */
-template<typename T> struct Canonical {
+template<typename C> struct CanonicalChart {
-  typedef T type;
+ BOOST_CONCEPT_ASSERT((ChartConcept<C>));
-  typedef typename DefaultChart<T>::vector vector;
+
-  DefaultChart<T> chart;
+  typedef C Chart;
-  Canonical() :
+  typedef typename Chart::type type;
-      chart(traits::zero<T>::value()) {
+  typedef typename Chart::vector vector;
-  }
+
   // Convert t of type T into canonical coordinates
-  vector apply(const T& t) {
+  vector local(const type& t) {
-    return chart.apply(t);
+    return Chart::local(traits::zero<type>::value(), t);
   }
   // Convert back from canonical coordinates to T
-  T retract(const vector& v) {
+  type retract(const vector& v) {
-    return chart.retract(v);
+    return Chart::retract(traits::zero<type>::value(), v);
   }
 };
+template <typename T> struct Canonical : public CanonicalChart<DefaultChart<T> > {};
 
 // double
 
 template<>
 struct DefaultChart<double> {
+  typedef double type;
   typedef Eigen::Matrix<double, 1, 1> vector;
-  double t_;
+
-  DefaultChart(double t) :
+  static vector local(double origin, double other) {
-      t_(t) {
-  }
-  vector apply(double other) {
     vector d;
-    d << other - t_;
+    d << other - origin;
     return d;
   }
-  double retract(const vector& d) {
+  static double retract(double origin, const vector& d) {
-    return t_ + d[0];
+    return origin + d[0];
+  }
+  static const int getDimension(double) {
+    return 1;
   }
 };
 
@@ -204,22 +246,23 @@ struct DefaultChart<double> {
 
 template<int M, int N, int Options>
 struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
-  typedef Eigen::Matrix<double, M, N, Options> T;
+  typedef Eigen::Matrix<double, M, N, Options> type;
+  typedef type T;
   typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
   BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
       "DefaultChart has not been implemented yet for dynamically sized matrices");
-  T t_;
+  static vector local(const T& origin, const T& other) {
-  DefaultChart(const T& t) :
+    T diff = other - origin;
-      t_(t) {
-  }
-  vector apply(const T& other) {
-    T diff = other - t_;
     Eigen::Map<vector> map(diff.data());
     return vector(map);
+    // Why is this function not : return other - origin; ?? what is the Eigen::Map used for?
   }
-  T retract(const vector& d) {
+  static T retract(const T& origin, const vector& d) {
     Eigen::Map<const T> map(d.data());
-    return t_ + map;
+    return origin + map;
+  }
+  static int getDimension(const T&origin) {
+    return origin.rows()*origin.cols();
   }
 };
 
@@ -227,16 +270,16 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
 template<>
 struct DefaultChart<Vector> {
   typedef Vector T;
+  typedef T type;
   typedef T vector;
-  T t_;
+  static vector local(const T& origin, const T& other) {
-  DefaultChart(const T& t) :
+    return other - origin;
-      t_(t) {
   }
-  vector apply(const T& other) {
+  static T retract(const T& origin, const vector& d) {
-    return other - t_;
+    return origin + d;
   }
-  T retract(const vector& d) {
+  static int getDimension(const T& origin) {
-    return t_ + d;
+    return origin.size();
   }
 };
 

gtsam/base/Value.h

@@ -120,7 +120,16 @@ namespace gtsam {
     virtual Vector localCoordinates_(const Value& value) const = 0;
 
     /** Assignment operator */
-    virtual Value& operator=(const Value& rhs) = 0;
+    virtual Value& operator=(const Value& rhs) {
+      //needs a empty definition so recursion in implicit derived assignment operators work
+     return *this;
+    }
+    /** Cast to known ValueType */
+    template<typename ValueType>
+    const ValueType& cast() const;
+
+    template<typename Chart>
+    const Chart& getChart() const;
 
     /** Virutal destructor */
     virtual ~Value() {}

gtsam/base/numericalDerivative.h

@@ -73,7 +73,7 @@ Vector numericalGradient(boost::function<double(const X&)> h, const X& x,
   typedef typename ChartX::vector TangentX;
 
   // get chart at x
-  ChartX chartX(x);
+  ChartX chartX;
 
   // Prepare a tangent vector to perturb x with, only works for fixed size
   TangentX d;
@@ -82,9 +82,9 @@ Vector numericalGradient(boost::function<double(const X&)> h, const X& x,
   Vector g = zero(N); // Can be fixed size
   for (int j = 0; j < N; j++) {
     d(j) = delta;
-    double hxplus = h(chartX.retract(d));
+    double hxplus = h(chartX.retract(x, d));
     d(j) = -delta;
-    double hxmin = h(chartX.retract(d));
+    double hxmin = h(chartX.retract(x, d));
     d(j) = 0;
     g(j) = (hxplus - hxmin) * factor;
   }
@@ -121,14 +121,14 @@ Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
 
   // get value at x, and corresponding chart
   Y hx = h(x);
-  ChartY chartY(hx);
+  ChartY chartY;
 
   // Bit of a hack for now to find number of rows
-  TangentY zeroY = chartY.apply(hx);
+  TangentY zeroY = chartY.local(hx, hx);
   size_t m = zeroY.size();
 
   // get chart at x
-  ChartX chartX(x);
+  ChartX chartX;
 
   // Prepare a tangent vector to perturb x with, only works for fixed size
   TangentX dx;
@@ -139,9 +139,9 @@ Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
   double factor = 1.0 / (2.0 * delta);
   for (int j = 0; j < N; j++) {
     dx(j) = delta;
-    TangentY dy1 = chartY.apply(h(chartX.retract(dx)));
+    TangentY dy1 = chartY.local(hx, h(chartX.retract(x, dx)));
     dx(j) = -delta;
-    TangentY dy2 = chartY.apply(h(chartX.retract(dx)));
+    TangentY dy2 = chartY.local(hx, h(chartX.retract(x, dx)));
     dx(j) = 0;
     H.col(j) << (dy1 - dy2) * factor;
   }

gtsam/geometry/Cal3Bundler.h

@@ -28,7 +28,7 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3Bundler: public DerivedValue<Cal3Bundler> {
+class GTSAM_EXPORT Cal3Bundler {
 
 private:
   double f_; ///< focal length
@@ -172,9 +172,6 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar
-        & boost::serialization::make_nvp("Cal3Bundler",
-            boost::serialization::base_object<Value>(*this));
     ar & BOOST_SERIALIZATION_NVP(f_);
     ar & BOOST_SERIALIZATION_NVP(k1_);
     ar & BOOST_SERIALIZATION_NVP(k2_);
@@ -190,11 +187,11 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<Cal3Bundler> : public std::true_type {
+struct is_manifold<Cal3Bundler> : public boost::true_type {
 };
 
 template<>
-struct dimension<Cal3Bundler> : public std::integral_constant<int, 3> {
+struct dimension<Cal3Bundler> : public boost::integral_constant<int, 3> {
 };
 
 template<>

gtsam/geometry/Cal3DS2.h

@@ -37,7 +37,7 @@ namespace gtsam {
  *                      k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y  ]
  * pi = K*pn
  */
-class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> {
+class GTSAM_EXPORT Cal3DS2 {
 
 protected:
 
@@ -153,8 +153,6 @@ private:
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version)
   {
-    ar & boost::serialization::make_nvp("Cal3DS2",
-        boost::serialization::base_object<Value>(*this));
     ar & BOOST_SERIALIZATION_NVP(fx_);
     ar & BOOST_SERIALIZATION_NVP(fy_);
     ar & BOOST_SERIALIZATION_NVP(s_);
@@ -172,11 +170,11 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<Cal3DS2> : public std::true_type {
+struct is_manifold<Cal3DS2> : public boost::true_type {
 };
 
 template<>
-struct dimension<Cal3DS2> : public std::integral_constant<int, 9> {
+struct dimension<Cal3DS2> : public boost::integral_constant<int, 9> {
 };
 
 }

gtsam/geometry/Cal3Unified.h

@@ -131,8 +131,6 @@ private:
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version)
   {
-    ar & boost::serialization::make_nvp("Cal3Unified",
-        boost::serialization::base_object<Cal3DS2>(*this));
     ar & BOOST_SERIALIZATION_NVP(xi_);
   }
 
@@ -142,11 +140,11 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<Cal3Unified> : public std::true_type {
+struct is_manifold<Cal3Unified> : public boost::true_type {
 };
 
 template<>
-struct dimension<Cal3Unified> : public std::integral_constant<int, 10> {
+struct dimension<Cal3Unified> : public boost::integral_constant<int, 10> {
 };
 
 template<>

gtsam/geometry/Cal3_S2.h

@@ -31,7 +31,7 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Cal3_S2: public DerivedValue<Cal3_S2> {
+class GTSAM_EXPORT Cal3_S2 {
 private:
   double fx_, fy_, s_, u0_, v0_;
 
@@ -226,9 +226,6 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar
-        & boost::serialization::make_nvp("Cal3_S2",
-            boost::serialization::base_object<Value>(*this));
     ar & BOOST_SERIALIZATION_NVP(fx_);
     ar & BOOST_SERIALIZATION_NVP(fy_);
     ar & BOOST_SERIALIZATION_NVP(s_);
@@ -244,11 +241,11 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<Cal3_S2> : public std::true_type {
+struct is_manifold<Cal3_S2> : public boost::true_type {
 };
 
 template<>
-struct dimension<Cal3_S2> : public std::integral_constant<int, 5> {
+struct dimension<Cal3_S2> : public boost::integral_constant<int, 5> {
 };
 
 template<>

gtsam/geometry/CalibratedCamera.h

@@ -39,7 +39,7 @@ public:
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT CalibratedCamera: public DerivedValue<CalibratedCamera> {
+class GTSAM_EXPORT CalibratedCamera {
 private:
   Pose3 pose_; // 6DOF pose
 
@@ -214,12 +214,28 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar
-        & boost::serialization::make_nvp("CalibratedCamera",
-            boost::serialization::base_object<Value>(*this));
     ar & BOOST_SERIALIZATION_NVP(pose_);
   }
 
   /// @}
-      };}
+};
+
+// Define GTSAM traits
+namespace traits {
+
+template<>
+struct is_group<CalibratedCamera> : public boost::true_type {
+};
+
+template<>
+struct is_manifold<CalibratedCamera> : public boost::true_type {
+};
+
+template<>
+struct dimension<CalibratedCamera> : public boost::integral_constant<int, 6> {
+};
+
+}
+
+}
 

gtsam/geometry/EssentialMatrix.h

@@ -20,7 +20,7 @@ namespace gtsam {
  * but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
  * We can then non-linearly optimize immediately on this 5-dimensional manifold.
  */
-class GTSAM_EXPORT EssentialMatrix: public DerivedValue<EssentialMatrix> {
+class GTSAM_EXPORT EssentialMatrix {
 
 private:
 
@@ -176,8 +176,6 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("EssentialMatrix",
-          boost::serialization::base_object<Value>(*this));
       ar & BOOST_SERIALIZATION_NVP(aRb_);
       ar & BOOST_SERIALIZATION_NVP(aTb_);
 
@@ -200,11 +198,11 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<EssentialMatrix> : public std::true_type {
+struct is_manifold<EssentialMatrix> : public boost::true_type {
 };
 
 template<>
-struct dimension<EssentialMatrix> : public std::integral_constant<int, 5> {
+struct dimension<EssentialMatrix> : public boost::integral_constant<int, 5> {
 };
 
 template<>

gtsam/geometry/PinholeCamera.h

@@ -37,7 +37,7 @@ namespace gtsam {
  * \nosubgrouping
  */
 template<typename Calibration>
-class PinholeCamera: public DerivedValue<PinholeCamera<Calibration> > {
+class PinholeCamera {
 private:
   Pose3 pose_;
   Calibration K_;
@@ -653,7 +653,6 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
     ar & BOOST_SERIALIZATION_NVP(pose_);
     ar & BOOST_SERIALIZATION_NVP(K_);
   }
@@ -664,11 +663,11 @@ private:
 namespace traits {
 
 template<typename Calibration>
-struct is_manifold<PinholeCamera<Calibration> > : public std::true_type {
+struct is_manifold<PinholeCamera<Calibration> > : public boost::true_type {
 };
 
 template<typename Calibration>
-struct dimension<PinholeCamera<Calibration> > : public std::integral_constant<
+struct dimension<PinholeCamera<Calibration> > : public boost::integral_constant<
     int, dimension<Pose3>::value + dimension<Calibration>::value> {
 };
 

gtsam/geometry/Point2.h

@@ -32,7 +32,7 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Point2 : public DerivedValue<Point2> {
+class GTSAM_EXPORT Point2 {
 
 private:
 
@@ -237,8 +237,6 @@ private:
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int version)
   {
-    ar & boost::serialization::make_nvp("Point2",
-        boost::serialization::base_object<Value>(*this));
     ar & BOOST_SERIALIZATION_NVP(x_);
     ar & BOOST_SERIALIZATION_NVP(y_);
   }
@@ -254,15 +252,15 @@ inline Point2 operator*(double s, const Point2& p) {return p*s;}
 namespace traits {
 
 template<>
-struct is_group<Point2> : public std::true_type {
+struct is_group<Point2> : public boost::true_type {
 };
 
 template<>
-struct is_manifold<Point2> : public std::true_type {
+struct is_manifold<Point2> : public boost::true_type {
 };
 
 template<>
-struct dimension<Point2> : public std::integral_constant<int, 2> {
+struct dimension<Point2> : public boost::integral_constant<int, 2> {
 };
 
 }

gtsam/geometry/Point3.h

@@ -36,7 +36,7 @@ namespace gtsam {
    * @addtogroup geometry
    * \nosubgrouping
    */
-  class GTSAM_EXPORT Point3 : public DerivedValue<Point3> {
+  class GTSAM_EXPORT Point3 {
 
   private:
 
@@ -228,8 +228,6 @@ namespace gtsam {
     template<class ARCHIVE>
       void serialize(ARCHIVE & ar, const unsigned int version)
     {
-      ar & boost::serialization::make_nvp("Point3",
-          boost::serialization::base_object<Value>(*this));
       ar & BOOST_SERIALIZATION_NVP(x_);
       ar & BOOST_SERIALIZATION_NVP(y_);
       ar & BOOST_SERIALIZATION_NVP(z_);
@@ -246,15 +244,15 @@ namespace gtsam {
   namespace traits {
 
   template<>
-  struct is_group<Point3> : public std::true_type {
+  struct is_group<Point3> : public boost::true_type {
   };
 
   template<>
-  struct is_manifold<Point3> : public std::true_type {
+  struct is_manifold<Point3> : public boost::true_type {
   };
 
   template<>
-  struct dimension<Point3> : public std::integral_constant<int, 3> {
+  struct dimension<Point3> : public boost::integral_constant<int, 3> {
   };
 
   }

gtsam/geometry/Pose2.h

@@ -33,7 +33,7 @@ namespace gtsam {
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Pose2 : public DerivedValue<Pose2> {
+class GTSAM_EXPORT Pose2 {
 
 public:
 
@@ -301,8 +301,6 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar & boost::serialization::make_nvp("Pose2",
-        boost::serialization::base_object<Value>(*this));
     ar & BOOST_SERIALIZATION_NVP(t_);
     ar & BOOST_SERIALIZATION_NVP(r_);
   }
@@ -325,11 +323,11 @@ GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
 namespace traits {
 
 template<>
-struct is_manifold<Pose2> : public std::true_type {
+struct is_manifold<Pose2> : public boost::true_type {
 };
 
 template<>
-struct dimension<Pose2> : public std::integral_constant<int, 3> {
+struct dimension<Pose2> : public boost::integral_constant<int, 3> {
 };
 
 }

gtsam/geometry/Pose3.h

@@ -39,7 +39,7 @@ class Pose2;
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT Pose3: public DerivedValue<Pose3> {
+class GTSAM_EXPORT Pose3{
 public:
 
   /** Pose Concept requirements */
@@ -326,8 +326,6 @@ public:
     friend class boost::serialization::access;
     template<class Archive>
     void serialize(Archive & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("Pose3",
-              boost::serialization::base_object<Value>(*this));
       ar & BOOST_SERIALIZATION_NVP(R_);
       ar & BOOST_SERIALIZATION_NVP(t_);
     }
@@ -358,15 +356,15 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
 namespace traits {
 
 template<>
-struct is_group<Pose3> : public std::true_type {
+struct is_group<Pose3> : public boost::true_type {
 };
 
 template<>
-struct is_manifold<Pose3> : public std::true_type {
+struct is_manifold<Pose3> : public boost::true_type {
 };
 
 template<>
-struct dimension<Pose3> : public std::integral_constant<int, 6> {
+struct dimension<Pose3> : public boost::integral_constant<int, 6> {
 };
 
 }

gtsam/geometry/Rot2.h

@@ -31,7 +31,7 @@ namespace gtsam {
    * @addtogroup geometry
    * \nosubgrouping
    */
-  class GTSAM_EXPORT Rot2 : public DerivedValue<Rot2> {
+  class GTSAM_EXPORT Rot2 {
 
   public:
     /** get the dimension by the type */
@@ -235,8 +235,6 @@ namespace gtsam {
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("Rot2",
-          boost::serialization::base_object<Value>(*this));
       ar & BOOST_SERIALIZATION_NVP(c_);
       ar & BOOST_SERIALIZATION_NVP(s_);
     }
@@ -247,15 +245,15 @@ namespace gtsam {
   namespace traits {
 
   template<>
-  struct is_group<Rot2> : public std::true_type {
+  struct is_group<Rot2> : public boost::true_type {
   };
 
   template<>
-  struct is_manifold<Rot2> : public std::true_type {
+  struct is_manifold<Rot2> : public boost::true_type {
   };
 
   template<>
-  struct dimension<Rot2> : public std::integral_constant<int, 1> {
+  struct dimension<Rot2> : public boost::integral_constant<int, 1> {
   };
 
   }

gtsam/geometry/Rot3.h

@@ -58,7 +58,7 @@ namespace gtsam {
    * @addtogroup geometry
    * \nosubgrouping
    */
-  class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> {
+  class GTSAM_EXPORT Rot3 {
 
   private:
 
@@ -456,8 +456,6 @@ namespace gtsam {
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version)
     {
-       ar & boost::serialization::make_nvp("Rot3",
-           boost::serialization::base_object<Value>(*this));
 #ifndef GTSAM_USE_QUATERNIONS
        ar & boost::serialization::make_nvp("rot11", rot_(0,0));
        ar & boost::serialization::make_nvp("rot12", rot_(0,1));
@@ -494,15 +492,15 @@ namespace gtsam {
   namespace traits {
 
   template<>
-  struct is_group<Rot3> : public std::true_type {
+  struct is_group<Rot3> : public boost::true_type {
   };
 
   template<>
-  struct is_manifold<Rot3> : public std::true_type {
+  struct is_manifold<Rot3> : public boost::true_type {
   };
 
   template<>
-  struct dimension<Rot3> : public std::integral_constant<int, 3> {
+  struct dimension<Rot3> : public boost::integral_constant<int, 3> {
   };
 
   }

gtsam/geometry/StereoCamera.h

@@ -36,7 +36,7 @@ public:
  * A stereo camera class, parameterize by left camera pose and stereo calibration
  * @addtogroup geometry
  */
-class GTSAM_EXPORT StereoCamera  : public DerivedValue<StereoCamera> {
+class GTSAM_EXPORT StereoCamera {
 
 private:
   Pose3 leftCamPose_;
@@ -147,8 +147,6 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar & boost::serialization::make_nvp("StereoCamera",
-       boost::serialization::base_object<Value>(*this));
     ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
     ar & BOOST_SERIALIZATION_NVP(K_);
   }
@@ -159,11 +157,11 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<StereoCamera> : public std::true_type {
+struct is_manifold<StereoCamera> : public boost::true_type {
 };
 
 template<>
-struct dimension<StereoCamera> : public std::integral_constant<int, 6> {
+struct dimension<StereoCamera> : public boost::integral_constant<int, 6> {
 };
 
 template<>

gtsam/geometry/StereoPoint2.h

@@ -28,7 +28,7 @@ namespace gtsam {
    * @addtogroup geometry
    * \nosubgrouping
    */
-  class GTSAM_EXPORT StereoPoint2 : public DerivedValue<StereoPoint2> {
+  class GTSAM_EXPORT StereoPoint2 {
   public:
     static const size_t dimension = 3;
   private:
@@ -162,8 +162,6 @@ namespace gtsam {
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("StereoPoint2",
-          boost::serialization::base_object<Value>(*this));
       ar & BOOST_SERIALIZATION_NVP(uL_);
       ar & BOOST_SERIALIZATION_NVP(uR_);
       ar & BOOST_SERIALIZATION_NVP(v_);
@@ -177,15 +175,15 @@ namespace gtsam {
   namespace traits {
 
   template<>
-  struct is_group<StereoPoint2> : public std::true_type {
+  struct is_group<StereoPoint2> : public boost::true_type {
   };
 
   template<>
-  struct is_manifold<StereoPoint2> : public std::true_type {
+  struct is_manifold<StereoPoint2> : public boost::true_type {
   };
 
   template<>
-  struct dimension<StereoPoint2> : public std::integral_constant<int, 3> {
+  struct dimension<StereoPoint2> : public boost::integral_constant<int, 3> {
   };
 
   }

gtsam/geometry/Unit3.h

@@ -27,7 +27,7 @@
 namespace gtsam {
 
 /// Represents a 3D point on a unit sphere.
-class GTSAM_EXPORT Unit3: public DerivedValue<Unit3> {
+class GTSAM_EXPORT Unit3{
 
 private:
 
@@ -146,8 +146,6 @@ private:
   friend class boost::serialization::access;
   template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("Unit3",
-          boost::serialization::base_object<Value>(*this));
       ar & BOOST_SERIALIZATION_NVP(p_);
       ar & BOOST_SERIALIZATION_NVP(B_);
     }
@@ -160,11 +158,11 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<Unit3> : public std::true_type {
+struct is_manifold<Unit3> : public boost::true_type {
 };
 
 template<>
-struct dimension<Unit3> : public std::integral_constant<int, 2> {
+struct dimension<Unit3> : public boost::integral_constant<int, 2> {
 };
 
 template<>

gtsam/geometry/tests/testRot3.cpp

@@ -142,7 +142,7 @@ TEST( Rot3, retract)
 
   // test Canonical coordinates
   Canonical<Rot3> chart;
-  Vector v2 = chart.apply(R);
+  Vector v2 = chart.local(R);
   CHECK(assert_equal(R, chart.retract(v2)));
 }
 

gtsam/navigation/ImuBias.h

@@ -39,7 +39,7 @@ namespace gtsam {
 /// All bias models live in the imuBias namespace
 namespace imuBias {
 
-  class ConstantBias : public DerivedValue<ConstantBias> {
+  class ConstantBias {
   private:
     Vector3 biasAcc_;
     Vector3 biasGyro_;
@@ -205,8 +205,7 @@ namespace imuBias {
     template<class ARCHIVE>
       void serialize(ARCHIVE & ar, const unsigned int version)
     {
-      ar & boost::serialization::make_nvp("imuBias::ConstantBias",
+      ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this);
-          boost::serialization::base_object<Value>(*this));
       ar & BOOST_SERIALIZATION_NVP(biasAcc_);
       ar & BOOST_SERIALIZATION_NVP(biasGyro_);
     }
@@ -222,15 +221,15 @@ namespace imuBias {
 namespace traits {
 
 template<>
-struct is_group<imuBias::ConstantBias> : public std::true_type {
+struct is_group<imuBias::ConstantBias> : public boost::true_type {
 };
 
 template<>
-struct is_manifold<imuBias::ConstantBias> : public std::true_type {
+struct is_manifold<imuBias::ConstantBias> : public boost::true_type {
 };
 
 template<>
-struct dimension<imuBias::ConstantBias> : public std::integral_constant<int, 6> {
+struct dimension<imuBias::ConstantBias> : public boost::integral_constant<int, 6> {
 };
 
 }

gtsam/nonlinear/Values-inl.h

@@ -33,6 +33,7 @@
 
 namespace gtsam {
 
+
   /* ************************************************************************* */
   template<class ValueType>
   struct _ValuesKeyValuePair {
@@ -52,6 +53,36 @@ namespace gtsam {
     _ValuesConstKeyValuePair(const _ValuesKeyValuePair<ValueType>& rhs) : key(rhs.key), value(rhs.value) {}
   };
 
+  /* ************************************************************************* */
+
+  // Cast helpers for making _Values[Const]KeyValuePair's from Values::[Const]KeyValuePair
+  // need to use a struct here for later partial specialization
+  template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
+  struct ValuesCastHelper {
+    static CastedKeyValuePairType cast(KeyValuePairType key_value) {
+      // Static cast because we already checked the type during filtering
+      return CastedKeyValuePairType(key_value.key, const_cast<GenericValue<ValueType>&>(static_cast<const GenericValue<ValueType>&>(key_value.value)).value());
+    }
+  };
+  // partial specialized version for ValueType == Value
+  template<class CastedKeyValuePairType, class KeyValuePairType>
+  struct ValuesCastHelper<Value, CastedKeyValuePairType, KeyValuePairType> {
+    static CastedKeyValuePairType cast(KeyValuePairType key_value) {
+      // Static cast because we already checked the type during filtering
+      // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value?
+      return CastedKeyValuePairType(key_value.key, key_value.value);
+    }
+  };
+  // partial specialized version for ValueType == Value
+  template<class CastedKeyValuePairType, class KeyValuePairType>
+  struct ValuesCastHelper<const Value, CastedKeyValuePairType, KeyValuePairType> {
+    static CastedKeyValuePairType cast(KeyValuePairType key_value) {
+      // Static cast because we already checked the type during filtering
+      // in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value?
+      return CastedKeyValuePairType(key_value.key, key_value.value);
+    }
+  };
+
   /* ************************************************************************* */
   template<class ValueType>
   class Values::Filtered {
@@ -99,19 +130,19 @@ namespace gtsam {
       begin_(boost::make_transform_iterator(
       boost::make_filter_iterator(
       filter, values.begin(), values.end()),
-      &castHelper<ValueType, KeyValuePair, Values::KeyValuePair>)),
+      &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)),
       end_(boost::make_transform_iterator(
       boost::make_filter_iterator(
       filter, values.end(), values.end()),
-      &castHelper<ValueType, KeyValuePair, Values::KeyValuePair>)),
+      &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)),
       constBegin_(boost::make_transform_iterator(
       boost::make_filter_iterator(
       filter, ((const Values&)values).begin(), ((const Values&)values).end()),
-      &castHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>)),
+      &ValuesCastHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>::cast)),
       constEnd_(boost::make_transform_iterator(
       boost::make_filter_iterator(
       filter, ((const Values&)values).end(), ((const Values&)values).end()),
-      &castHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>)) {}
+      &ValuesCastHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>::cast)) {}
 
     friend class Values;
     iterator begin_;
@@ -175,7 +206,7 @@ namespace gtsam {
   Values::Values(const Values::Filtered<ValueType>& view) {
     BOOST_FOREACH(const typename Filtered<ValueType>::KeyValuePair& key_value, view) {
       Key key = key_value.key;
-      insert(key, key_value.value);
+      insert(key, static_cast<const ValueType&>(key_value.value));
     }
   }
 
@@ -184,7 +215,7 @@ namespace gtsam {
   Values::Values(const Values::ConstFiltered<ValueType>& view) {
     BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) {
       Key key = key_value.key;
-      insert(key, key_value.value);
+      insert<ValueType>(key, key_value.value);
     }
   }
 
@@ -214,6 +245,13 @@ namespace gtsam {
     return ConstFiltered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this);
   }
 
+  /* ************************************************************************* */
+   template<>
+   inline bool Values::filterHelper<Value>(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
+     // Filter and check the type
+     return filter(key_value.key);
+   }
+
   /* ************************************************************************* */
   template<typename ValueType>
   const ValueType& Values::at(Key j) const {
@@ -225,11 +263,11 @@ namespace gtsam {
       throw ValuesKeyDoesNotExist("retrieve", j);
 
     // Check the type and throw exception if incorrect
-    if(typeid(*item->second) != typeid(ValueType))
+    try {
+      return dynamic_cast<const GenericValue<ValueType>&>(*item->second).value();
+    } catch (std::bad_cast &) {
       throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
-
+    }
-    // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
-    return static_cast<const ValueType&>(*item->second);
   }
 
   /* ************************************************************************* */
@@ -239,15 +277,49 @@ namespace gtsam {
     KeyValueMap::const_iterator item = values_.find(j);
 
     if(item != values_.end()) {
-      // Check the type and throw exception if incorrect
+      // dynamic cast the type and throw exception if incorrect
-      if(typeid(*item->second) != typeid(ValueType))
+      try {
+        return dynamic_cast<const GenericValue<ValueType>&>(*item->second).value();
+      } catch (std::bad_cast &) {
         throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
-
+      }
-      // We have already checked the type, so do a "blind" static_cast, not dynamic_cast
-      return static_cast<const ValueType&>(*item->second);
      } else {
       return boost::none;
     }
   }
 
+  /* ************************************************************************* */
+  // insert a plain value using the default chart
+  template<typename ValueType>
+   void Values::insert(Key j, const ValueType& val) {
+     insert(j, static_cast<const Value&>(ChartValue<ValueType, DefaultChart<ValueType> >(val)));
+   }
+
+  // insert with custom chart type
+  template<typename ValueType, typename Chart>
+   void Values::insert(Key j, const ValueType& val) {
+     insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val)));
+   }
+  // overloaded insert with chart initializer
+  template<typename ValueType, typename Chart, typename ChartInit>
+  void Values::insert(Key j, const ValueType& val, ChartInit chart) {
+    insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
+  }
+
+  // update with default chart
+  template <typename ValueType>
+  void Values::update(Key j, const ValueType& val) {
+    update(j, static_cast<const Value&>(ChartValue<ValueType, DefaultChart<ValueType> >(val)));
+  }
+  // update with custom chart
+  template <typename ValueType, typename Chart>
+  void Values::update(Key j, const ValueType& val) {
+    update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val)));
+  }
+  // update with chart initializer, /todo: perhaps there is a way to init chart from old value...
+  template<typename ValueType, typename Chart, typename ChartInit>
+  void Values::update(Key j, const ValueType& val, ChartInit chart) {
+    update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
+  }
+
 }

gtsam/nonlinear/Values.h

@@ -44,7 +44,7 @@
 #include <string>
 #include <utility>
 
-#include <gtsam/base/Value.h>
+#include <gtsam/base/ChartValue.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/inference/Key.h>
 
@@ -251,6 +251,19 @@ namespace gtsam {
     /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
     void insert(const Values& values);
 
+    /** Templated verion to add a variable with the given j,
+     * throws KeyAlreadyExists<J> if j is already present
+     * if no chart is specified, the DefaultChart<ValueType> is used
+     */
+    template <typename ValueType>
+    void insert(Key j, const ValueType& val);
+    template <typename ValueType, typename Chart>
+    void insert(Key j, const ValueType& val);
+     // overloaded insert version that also specifies a chart initializer
+    template <typename ValueType, typename Chart, typename ChartInit>
+    void insert(Key j, const ValueType& val, ChartInit chart);
+
+
     /** insert that mimics the STL map insert - if the value already exists, the map is not modified
      *  and an iterator to the existing value is returned, along with 'false'.  If the value did not
      *  exist, it is inserted and an iterator pointing to the new element, along with 'true', is
@@ -260,6 +273,17 @@ namespace gtsam {
     /** single element change of existing element */
     void update(Key j, const Value& val);
 
+    /** Templated verion to update a variable with the given j,
+      * throws KeyAlreadyExists<J> if j is already present
+      * if no chart is specified, the DefaultChart<ValueType> is used
+      */
+    template <typename T>
+    void update(Key j, const T& val);
+    template <typename T, typename Chart>
+    void update(Key j, const T& val);
+    template <typename T, typename Chart, typename ChartInit>
+    void update(Key j, const T& val, ChartInit chart);
+
     /** update the current available values without adding new ones */
     void update(const Values& values);
 
@@ -369,15 +393,9 @@ namespace gtsam {
     // supplied \c filter function.
     template<class ValueType>
     static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
+      BOOST_STATIC_ASSERT((!boost::is_same<ValueType, Value>::value));
       // Filter and check the type
-      return filter(key_value.key) && (typeid(ValueType) == typeid(key_value.value) || typeid(ValueType) == typeid(Value));
+      return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
-    }
-
-    // Cast to the derived ValueType
-    template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
-    static CastedKeyValuePairType castHelper(KeyValuePairType key_value) {
-      // Static cast because we already checked the type during filtering
-      return CastedKeyValuePairType(key_value.key, static_cast<ValueType&>(key_value.value));
     }
 
     /** Serialization function */

gtsam/nonlinear/tests/testSerializationNonlinear.cpp

@@ -32,20 +32,37 @@ using namespace gtsam::serializationTestHelpers;
 
 /* ************************************************************************* */
 // Export all classes derived from Value
-BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
+BOOST_CLASS_EXPORT(gtsam::Cal3_S2);
-BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
+BOOST_CLASS_EXPORT(gtsam::Cal3Bundler);
-BOOST_CLASS_EXPORT(gtsam::Point3)
+BOOST_CLASS_EXPORT(gtsam::Point3);
-BOOST_CLASS_EXPORT(gtsam::Pose3)
+BOOST_CLASS_EXPORT(gtsam::Pose3);
-BOOST_CLASS_EXPORT(gtsam::Rot3)
+BOOST_CLASS_EXPORT(gtsam::Rot3);
-BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>);
-BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>);
-BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
+BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>);
+
+namespace detail {
+template<class T> struct pack {
+ typedef T type;
+};
+}
+#define CHART_VALUE_EXPORT(UNIQUE_NAME, TYPE) \
+  typedef gtsam::ChartValue<TYPE,gtsam::DefaultChart<TYPE> > UNIQUE_NAME; \
+  BOOST_CLASS_EXPORT( UNIQUE_NAME );
+
 
 /* ************************************************************************* */
 typedef PinholeCamera<Cal3_S2>        PinholeCal3S2;
 typedef PinholeCamera<Cal3DS2>        PinholeCal3DS2;
 typedef PinholeCamera<Cal3Bundler>    PinholeCal3Bundler;
 
+CHART_VALUE_EXPORT(gtsamPoint3Chart, gtsam::Point3);
+CHART_VALUE_EXPORT(Cal3S2Chart, PinholeCal3S2);
+CHART_VALUE_EXPORT(Cal3DS2Chart, PinholeCal3DS2);
+CHART_VALUE_EXPORT(Cal3BundlerChart, PinholeCal3Bundler);
+
+
+
 /* ************************************************************************* */
 static Point3 pt3(1.0, 2.0, 3.0);
 static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
@@ -56,12 +73,27 @@ static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
 static Cal3Bundler cal3(1.0, 2.0, 3.0);
 
 TEST (Serialization, TemplatedValues) {
+  std::cout << __LINE__ << std::endl;
+  EXPECT(equalsObj(pt3));
+  std::cout << __LINE__ << std::endl;
+  ChartValue<Point3> chv1(pt3);
+  std::cout << __LINE__ << std::endl;
+  EXPECT(equalsObj(chv1));
+  std::cout << __LINE__ << std::endl;
+  PinholeCal3S2 pc(pose3,cal1);
+  EXPECT(equalsObj(pc));
+  std::cout << __LINE__ << std::endl;
   Values values;
+  values.insert(1,pt3);
+  std::cout << __LINE__ << std::endl;
+  EXPECT(equalsObj(values));
+  std::cout << __LINE__ << std::endl;
   values.insert(Symbol('a',0),  PinholeCal3S2(pose3, cal1));
   values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2));
   values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3));
   values.insert(Symbol('a',5),  PinholeCal3S2(pose3, cal1));
   EXPECT(equalsObj(values));
+  std::cout << __LINE__ << std::endl;
   EXPECT(equalsXML(values));
   EXPECT(equalsBinary(values));
 }

gtsam/nonlinear/tests/testValues.cpp

@@ -51,16 +51,25 @@ public:
 };
 int TestValueData::ConstructorCount = 0;
 int TestValueData::DestructorCount = 0;
-class TestValue : public DerivedValue<TestValue> {
+class TestValue {
   TestValueData data_;
 public:
-  virtual void print(const std::string& str = "") const {}
+  void print(const std::string& str = "") const {}
   bool equals(const TestValue& other, double tol = 1e-9) const { return true; }
-  virtual size_t dim() const { return 0; }
+  size_t dim() const { return 0; }
   TestValue retract(const Vector&) const { return TestValue(); }
   Vector localCoordinates(const TestValue&) const { return Vector(); }
 };
 
+namespace gtsam {
+namespace traits {
+template <>
+struct is_manifold<TestValue> : public boost::true_type {};
+template <>
+struct dimension<TestValue> : public boost::integral_constant<int, 0> {};
+}
+}
+
 /* ************************************************************************* */
 TEST( Values, equals1 )
 {
@@ -353,12 +362,14 @@ TEST(Values, filter) {
   BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) {
     if(i == 0) {
       LONGS_EQUAL(2, (long)key_value.key);
-      EXPECT(typeid(Pose2) == typeid(key_value.value));
+      try {key_value.value.cast<Pose2>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");}
-      EXPECT(assert_equal(pose2, dynamic_cast<const Pose2&>(key_value.value)));
+      THROWS_EXCEPTION(key_value.value.cast<Pose3>());
+      EXPECT(assert_equal(pose2, key_value.value.cast<Pose2>()));
     } else if(i == 1) {
       LONGS_EQUAL(3, (long)key_value.key);
-      EXPECT(typeid(Pose3) == typeid(key_value.value));
+      try {key_value.value.cast<Pose3>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose3");}
-      EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.value)));
+      THROWS_EXCEPTION(key_value.value.cast<Pose2>());
+      EXPECT(assert_equal(pose3, key_value.value.cast<Pose3>()));
     } else {
       EXPECT(false);
     }
@@ -416,10 +427,10 @@ TEST(Values, Symbol_filter) {
   BOOST_FOREACH(const Values::Filtered<Value>::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) {
     if(i == 0) {
       LONGS_EQUAL(Symbol('y', 1), (long)key_value.key);
-      EXPECT(assert_equal(pose1, dynamic_cast<const Pose3&>(key_value.value)));
+      EXPECT(assert_equal(pose1, key_value.value.cast<Pose3>()));
     } else if(i == 1) {
       LONGS_EQUAL(Symbol('y', 3), (long)key_value.key);
-      EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.value)));
+      EXPECT(assert_equal(pose3, key_value.value.cast<Pose3>()));
     } else {
       EXPECT(false);
     }
@@ -441,11 +452,15 @@ TEST(Values, Destructors) {
       values.insert(0, value1);
       values.insert(1, value2);
     }
-    LONGS_EQUAL(4, (long)TestValueData::ConstructorCount);
+    // additional 2 con/destructor counts for the temporary
-    LONGS_EQUAL(2, (long)TestValueData::DestructorCount);
+    // GenericValue<TestValue> in insert()
+    // but I'm sure some advanced programmer can figure out
+    // a way to avoid the temporary, or optimize it out
+    LONGS_EQUAL(4+2, (long)TestValueData::ConstructorCount);
+    LONGS_EQUAL(2+2, (long)TestValueData::DestructorCount);
   }
-  LONGS_EQUAL(4, (long)TestValueData::ConstructorCount);
+  LONGS_EQUAL(4+2, (long)TestValueData::ConstructorCount);
-  LONGS_EQUAL(4, (long)TestValueData::DestructorCount);
+  LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount);
 }
 
 /* ************************************************************************* */

gtsam/slam/dataset.cpp

@@ -1,5 +1,4 @@
 /* ----------------------------------------------------------------------------
-
  * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
  * Atlanta, Georgia 30332-0415
  * All Rights Reserved
@@ -330,8 +329,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
   fstream stream(filename.c_str(), fstream::out);
 
   // save poses
+
   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
-    const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
+    const Pose2& pose = key_value.value.cast<Pose2>();
     stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
         << " " << pose.theta() << endl;
   }
@@ -373,26 +373,23 @@ GraphAndValues readG2o(const string& g2oFile, const bool is3D,
 /* ************************************************************************* */
 void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
     const string& filename) {
-
   fstream stream(filename.c_str(), fstream::out);
 
   // save 2D & 3D poses
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
+  Values::ConstFiltered<Pose2> viewPose2 = estimate.filter<Pose2>();
-
+  BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, viewPose2) {
-    const Pose2* pose2D = dynamic_cast<const Pose2*>(&key_value.value);
+    stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
-    if(pose2D){
+        << key_value.value.y() << " " << key_value.value.theta() << endl;
-    stream << "VERTEX_SE2 " << key_value.key << " " << pose2D->x() << " "
-        << pose2D->y() << " " << pose2D->theta() << endl;
   }
-    const Pose3* pose3D = dynamic_cast<const Pose3*>(&key_value.value);
+
-    if(pose3D){
+  Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>();
-      Point3 p = pose3D->translation();
+  BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, viewPose3) {
-      Rot3 R = pose3D->rotation();
+      Point3 p = key_value.value.translation();
+      Rot3 R = key_value.value.rotation();
       stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " "  << p.y() << " " << p.z()
         << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
         << " " << R.toQuaternion().w() << endl;
   }
-  }
 
   // save edges (2D or 3D)
   BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {

gtsam_unstable/dynamics/PoseRTV.h

@@ -19,7 +19,7 @@ typedef Point3 Velocity3;
  * Robot state for use with IMU measurements
  * - contains translation, translational velocity and rotation
  */
-class GTSAM_UNSTABLE_EXPORT PoseRTV : public DerivedValue<PoseRTV> {
+class GTSAM_UNSTABLE_EXPORT PoseRTV {
 protected:
 
   Pose3 Rt_;
@@ -187,11 +187,11 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<PoseRTV> : public std::true_type {
+struct is_manifold<PoseRTV> : public boost::true_type {
 };
 
 template<>
-struct dimension<PoseRTV> : public std::integral_constant<int, 9> {
+struct dimension<PoseRTV> : public boost::integral_constant<int, 9> {
 };
 
 template<>

gtsam_unstable/geometry/BearingS2.h

@@ -15,7 +15,7 @@
 
 namespace gtsam {
 
-class GTSAM_UNSTABLE_EXPORT BearingS2 : public DerivedValue<BearingS2> {
+class GTSAM_UNSTABLE_EXPORT BearingS2 {
 protected:
   Rot2 azimuth_, elevation_;
 

gtsam_unstable/geometry/Pose3Upright.h

@@ -22,7 +22,7 @@ namespace gtsam {
  * @ingroup geometry
  * \nosubgrouping
  */
-class GTSAM_UNSTABLE_EXPORT Pose3Upright : public DerivedValue<Pose3Upright> {
+class GTSAM_UNSTABLE_EXPORT Pose3Upright {
 public:
   static const size_t dimension = 4;
 
@@ -144,11 +144,11 @@ private:
 namespace traits {
 
 template<>
-struct is_manifold<Pose3Upright> : public std::true_type {
+struct is_manifold<Pose3Upright> : public boost::true_type {
 };
 
 template<>
-struct dimension<Pose3Upright> : public std::integral_constant<int, 4> {
+struct dimension<Pose3Upright> : public boost::integral_constant<int, 4> {
 };
 
 }

gtsam_unstable/nonlinear/AdaptAutoDiff.h

@@ -59,8 +59,8 @@ public:
     using ceres::internal::AutoDiff;
 
     // Make arguments
-    Vector1 v1 = chart1.apply(a1);
+    Vector1 v1 = chart1.local(a1);
-    Vector2 v2 = chart2.apply(a2);
+    Vector2 v2 = chart2.local(a2);
 
     bool success;
     VectorT result;

gtsam_unstable/nonlinear/Expression-inl.h

@@ -311,8 +311,9 @@ public:
 
 //-----------------------------------------------------------------------------
 /// Leaf Expression
-template<class T>
+template<class T, class Chart=DefaultChart<T> >
 class LeafExpression: public ExpressionNode<T> {
+  typedef ChartValue<T,Chart> value_type; // perhaps this can be something else like a std::pair<T,Chart> ??
 
   /// The key into values
   Key key_;
@@ -321,6 +322,53 @@ class LeafExpression: public ExpressionNode<T> {
   LeafExpression(Key key) :
       key_(key) {
   }
+  // todo: do we need a virtual destructor here too?
+
+  friend class Expression<value_type> ;
+
+public:
+
+  /// Return keys that play in this expression
+  virtual std::set<Key> keys() const {
+    std::set<Key> keys;
+    keys.insert(key_);
+    return keys;
+  }
+
+  /// Return dimensions for each argument
+  virtual void dims(std::map<Key, size_t>& map) const {
+    // get dimension from the chart; only works for fixed dimension charts
+    map[key_] = traits::dimension<Chart>::value;
+  }
+
+  /// Return value
+  virtual const value_type& value(const Values& values) const {
+    return dynamic_cast<const value_type&>(values.at(key_));
+  }
+
+  /// Construct an execution trace for reverse AD
+  virtual const value_type& traceExecution(const Values& values, ExecutionTrace<value_type>& trace,
+      char* raw) const {
+    trace.setLeaf(key_);
+    return dynamic_cast<const value_type&>(values.at(key_));
+  }
+
+};
+
+//-----------------------------------------------------------------------------
+/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value
+template<typename T>
+class LeafExpression<T, DefaultChart<T> >: public ExpressionNode<T> {
+  typedef T value_type;
+
+  /// The key into values
+  Key key_;
+
+  /// Constructor with a single key
+  LeafExpression(Key key) :
+      key_(key) {
+  }
+  // todo: do we need a virtual destructor here too?
 
   friend class Expression<T> ;
 
@@ -392,7 +440,7 @@ struct Jacobian {
 
 /// meta-function to generate JacobianTA optional reference
 template<class T, class A>
-struct Optional {
+struct OptionalJacobian {
   typedef Eigen::Matrix<double, traits::dimension<T>::value,
       traits::dimension<A>::value> Jacobian;
   typedef boost::optional<Jacobian&> type;
@@ -522,7 +570,7 @@ struct FunctionalNode {
     // Argument types and derived, note these are base 0 !
     typedef TYPES Arguments;
     typedef typename boost::mpl::transform<TYPES, Jacobian<T, MPL::_1> >::type Jacobians;
-    typedef typename boost::mpl::transform<TYPES, Optional<T, MPL::_1> >::type Optionals;
+    typedef typename boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
 
     /// Reset expression shared pointer
     template<class A, size_t N>
@@ -577,7 +625,7 @@ class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
 
 public:
 
-  typedef boost::function<T(const A1&, typename Optional<T, A1>::type)> Function;
+  typedef boost::function<T(const A1&, typename OptionalJacobian<T, A1>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
   typedef typename Base::Record Record;
 
@@ -622,8 +670,8 @@ class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::t
 public:
 
   typedef boost::function<
-      T(const A1&, const A2&, typename Optional<T, A1>::type,
+      T(const A1&, const A2&, typename OptionalJacobian<T, A1>::type,
-          typename Optional<T, A2>::type)> Function;
+          typename OptionalJacobian<T, A2>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
   typedef typename Base::Record Record;
 
@@ -676,8 +724,8 @@ class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3>
 public:
 
   typedef boost::function<
-      T(const A1&, const A2&, const A3&, typename Optional<T, A1>::type,
+      T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type,
-          typename Optional<T, A2>::type, typename Optional<T, A3>::type)> Function;
+          typename OptionalJacobian<T, A2>::type, typename OptionalJacobian<T, A3>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
   typedef typename Base::Record Record;
 

gtsam_unstable/nonlinear/Expression.h

@@ -61,7 +61,7 @@ public:
   /// Construct a nullary method expression
   template<typename A>
   Expression(const Expression<A>& expression,
-      T (A::*method)(typename Optional<T, A>::type) const) :
+      T (A::*method)(typename OptionalJacobian<T, A>::type) const) :
       root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
   }
 
@@ -75,8 +75,8 @@ public:
   /// Construct a unary method expression
   template<typename A1, typename A2>
   Expression(const Expression<A1>& expression1,
-      T (A1::*method)(const A2&, typename Optional<T, A1>::type,
+      T (A1::*method)(const A2&, typename OptionalJacobian<T, A1>::type,
-          typename Optional<T, A2>::type) const,
+          typename OptionalJacobian<T, A2>::type) const,
       const Expression<A2>& expression2) :
       root_(
           new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),

gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp

@@ -202,7 +202,7 @@ TEST(Expression, Snavely) {
 #ifdef GTSAM_USE_QUATERNIONS
   EXPECT_LONGS_EQUAL(480,expression.traceSize()); // Todo, should be zero
 #else
-  EXPECT_LONGS_EQUAL(448,expression.traceSize()); // Todo, should be zero
+  EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero
 #endif
   set<Key> expected = list_of(1)(2);
   EXPECT(expected == expression.keys());

gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp

@@ -147,19 +147,19 @@ TEST(ExpressionFactor, Binary) {
 
   // traceRaw will fill raw with [Trace<Point2> | Binary::Record]
   EXPECT_LONGS_EQUAL(8, sizeof(double));
-  EXPECT_LONGS_EQUAL(24, sizeof(Point2));
+  EXPECT_LONGS_EQUAL(16, sizeof(Point2));
-  EXPECT_LONGS_EQUAL(48, sizeof(Cal3_S2));
+  EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2));
   EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Point2>));
   EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Cal3_S2>));
   EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian<Point2,Cal3_S2>::type));
   EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian<Point2,Point2>::type));
-  size_t expectedRecordSize = 24 + 24 + 48 + 2 * 16 + 80 + 32;
+  size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32;
-  EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record));
+  EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record));
 
   // Check size
   size_t size = binary.traceSize();
   CHECK(size);
-  EXPECT_LONGS_EQUAL(expectedRecordSize, size);
+  EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size);
   // Use Variable Length Array, allocated on stack by gcc
   // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla
   char raw[size];
@@ -211,8 +211,8 @@ TEST(ExpressionFactor, Shallow) {
   EXPECT_LONGS_EQUAL(464, sizeof(Binary::Record));
   LONGS_EQUAL(112+464, expectedTraceSize);
 #else
-  EXPECT_LONGS_EQUAL(432, sizeof(Binary::Record));
+  EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record));
-  LONGS_EQUAL(112+432, expectedTraceSize);
+  LONGS_EQUAL(112+400, expectedTraceSize);
 #endif
   size_t size = expression.traceSize();
   CHECK(size);

gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp

@@ -201,7 +201,7 @@ Pose3  pose;
   // Now, let's create the optional Jacobian arguments
   typedef Point3 T;
   typedef boost::mpl::vector<Pose3, Point3> TYPES;
-  typedef boost::mpl::transform<TYPES, Optional<T, MPL::_1> >::type Optionals;
+  typedef boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
 
   // Unfortunately this is moot: we need a pointer to a function with the
   // optional derivatives; I don't see a way of calling a function that we

tests/testManifold.cpp

@@ -17,6 +17,7 @@
  * @brief unit tests for Block Automatic Differentiation
  */
 
+#include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
@@ -65,39 +66,39 @@ TEST(Manifold, _dimension) {
 // charts
 TEST(Manifold, DefaultChart) {
 
-  DefaultChart<Point2> chart1(Point2(0, 0));
+  DefaultChart<Point2> chart1;
-  EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0));
+  EXPECT(chart1.local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0));
-  EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0));
+  EXPECT(chart1.retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0));
 
   Vector v2(2);
   v2 << 1, 0;
-  DefaultChart<Vector2> chart2(Vector2(0, 0));
+  DefaultChart<Vector2> chart2;
-  EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0))));
+  EXPECT(assert_equal(v2, chart2.local(Vector2(0, 0), Vector2(1, 0))));
-  EXPECT(chart2.retract(v2)==Vector2(1,0));
+  EXPECT(chart2.retract(Vector2(0, 0), v2) == Vector2(1, 0));
 
-  DefaultChart<double> chart3(0);
+  DefaultChart<double> chart3;
   Vector v1(1);
   v1 << 1;
-  EXPECT(assert_equal(v1,chart3.apply(1)));
+  EXPECT(assert_equal(v1, chart3.local(0, 1)));
-  EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9);
+  EXPECT_DOUBLES_EQUAL(chart3.retract(0, v1), 1, 1e-9);
 
   // Dynamic does not work yet !
   Vector z = zero(2), v(2);
   v << 1, 0;
-  DefaultChart<Vector> chart4(z);
+  DefaultChart<Vector> chart4;
-  EXPECT(assert_equal(chart4.apply(v),v));
+  EXPECT(assert_equal(chart4.local(z, v), v));
-  EXPECT(assert_equal(chart4.retract(v),v));
+  EXPECT(assert_equal(chart4.retract(z, v), v));
 
   Vector v3(3);
   v3 << 1, 1, 1;
   Rot3 I = Rot3::identity();
   Rot3 R = I.retract(v3);
-  DefaultChart<Rot3> chart5(I);
+  DefaultChart<Rot3> chart5;
-  EXPECT(assert_equal(v3,chart5.apply(R)));
+  EXPECT(assert_equal(v3, chart5.local(I, R)));
-  EXPECT(assert_equal(chart5.retract(v3),R));
+  EXPECT(assert_equal(chart5.retract(I, v3), R));
   // Check zero vector
-  DefaultChart<Rot3> chart6(R);
+  DefaultChart<Rot3> chart6;
-  EXPECT(assert_equal(zero(3),chart6.apply(R)));
+  EXPECT(assert_equal(zero(3), chart6.local(R, R)));
 }
 
 /* ************************************************************************* */
@@ -114,47 +115,47 @@ TEST(Manifold, _zero) {
 TEST(Manifold, Canonical) {
 
   Canonical<Point2> chart1;
-  EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0));
+  EXPECT(chart1.local(Point2(1, 0))==Vector2(1, 0));
   EXPECT(chart1.retract(Vector2(1, 0))==Point2(1, 0));
 
   Vector v2(2);
   v2 << 1, 0;
   Canonical<Vector2> chart2;
-  EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0))));
+  EXPECT(assert_equal(v2, chart2.local(Vector2(1, 0))));
   EXPECT(chart2.retract(v2)==Vector2(1, 0));
 
   Canonical<double> chart3;
   Eigen::Matrix<double, 1, 1> v1;
   v1 << 1;
-  EXPECT(chart3.apply(1)==v1);
+  EXPECT(chart3.local(1)==v1);
   EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9);
 
   Canonical<Point3> chart4;
   Point3 point(1, 2, 3);
   Vector v3(3);
   v3 << 1, 2, 3;
-  EXPECT(assert_equal(v3,chart4.apply(point)));
+  EXPECT(assert_equal(v3, chart4.local(point)));
   EXPECT(assert_equal(chart4.retract(v3), point));
 
   Canonical<Pose3> chart5;
   Pose3 pose(Rot3::identity(), point);
   Vector v6(6);
   v6 << 0, 0, 0, 1, 2, 3;
-  EXPECT(assert_equal(v6,chart5.apply(pose)));
+  EXPECT(assert_equal(v6, chart5.local(pose)));
   EXPECT(assert_equal(chart5.retract(v6), pose));
 
   Canonical<Camera> chart6;
   Cal3Bundler cal0(0, 0, 0);
   Camera camera(Pose3(), cal0);
   Vector z9 = Vector9::Zero();
-  EXPECT(assert_equal(z9,chart6.apply(camera)));
+  EXPECT(assert_equal(z9, chart6.local(camera)));
   EXPECT(assert_equal(chart6.retract(z9), camera));
 
   Cal3Bundler cal; // Note !! Cal3Bundler() != zero<Cal3Bundler>::value()
   Camera camera2(pose, cal);
   Vector v9(9);
   v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0;
-  EXPECT(assert_equal(v9,chart6.apply(camera2)));
+  EXPECT(assert_equal(v9, chart6.local(camera2)));
   EXPECT(assert_equal(chart6.retract(v9), camera2));
 }