Merged in feature/BAD_generic_chart_value (pull request #26)

Generic Chart Values

gtsam/base/ChartValue.h

@@ -0,0 +1,154 @@
+/* ----------------------------------------------------------------------------
+
+ * 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());
+  }
+
+  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 { };
+};
+
+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);
+ }
+
+
+} /* namespace gtsam */

gtsam/base/GenericValue.h

@@ -19,25 +19,6 @@
 #pragma once
 
 #include <gtsam/base/Value.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 {
 
@@ -49,142 +30,44 @@ template<typename T>
     return a.equals(b,tol);
   }
 
-// trait to compute the local coordinates of other with respect to origin
-template<typename T>
-Vector localCoordinates(const T& origin, const T& other) {
-  return origin.localCoordinates(other);
-}
-
-template<typename T>
-T retract(const T& origin, const Vector& delta) {
-  return origin.retract(delta);
-}
-
 template<typename T>
 void print(const T& obj, const std::string& str) {
   obj.print(str);
 }
 
-template<typename T>
-size_t getDimension(const T& obj) {
-  return obj.dim();
-}
 }
 
 template<class T>
 class GenericValue : public Value {
 public:
-  typedef T ValueType;
+  typedef T type;
-  typedef GenericValue This;
 protected:
   T value_;
 
 public:
-  GenericValue() {}
   GenericValue(const T& value) : value_(value) {}
 
-  T& value() { return value_; }
   const T& value() const { return value_; }
+  T& value() { return value_; }
 
   virtual ~GenericValue() {}
 
-  /**
-   * 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(This)>::malloc();
-    This* ptr = new(place) This(*this);
-    return ptr;
-  }
-
-  /**
-   * Destroy and deallocate this object, only if it was originally allocated using clone_().
-   */
-  virtual void deallocate_() const {
-    this->~GenericValue(); // Virtual destructor cleans up the derived object
-    boost::singleton_pool<PoolTag, sizeof(This)>::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<This>(*this);
-  }
-
   /// 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 This& genericValue2 = static_cast<const This&>(p);
+    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);
-
-  }
-
-  /// Generic 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 = traits::retract<T>(value_,delta);
-
-    // Create a Value pointer copy of the result
-    void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(This)>::malloc();
-    Value* resultAsValue = new(resultAsValuePlace) This(retractResult);
-
-    // 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 This& genericValue2 = static_cast<const This&>(value2);
-
-    // Return the result of calling localCoordinates trait on the derived class
-    return traits::localCoordinates<T>(value_,genericValue2.value_);
   }
 
   virtual void print(const std::string& str) const {
     traits::print<T>(value_,str);
   }
-  virtual size_t dim() const {
-    return traits::getDimension<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 This& derivedRhs = static_cast<const This&>(rhs);
-
-    // Do the assignment and return the result
-    this->value_ = derivedRhs.value_;
-    return *this;
-  }
-
-  /// Conversion to the derived class
-  operator const T& () const {
-    return value_;
-  }
-
-  /// Conversion to the derived class
-  operator T& () {
-    return value_;
-  }
-
 
 protected:
-  /// Assignment operator, protected because only the Value or DERIVED
+  /// Assignment operator for this class not needed since GenricValue<T> is an abstract class
-  /// 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 { };
 };
 
 // define Value::cast here since now GenericValue has been declared

gtsam/base/Manifold.h

@@ -67,12 +67,13 @@ struct is_manifold: public std::false_type {
 };
 
 // dimension, can return Eigen::Dynamic (-1) if not known at compile time
+typedef std::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> {
@@ -112,8 +113,6 @@ struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public std::true_typ
 
 // 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 {
 };
@@ -141,62 +140,105 @@ struct zero<Eigen::Matrix<double, M, N, Options> > : public std::integral_consta
   }
 };
 
+template <typename T> struct is_chart : public std::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 std::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));
+
+    /**
+     * 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_;
+};
+
+
 /**
- * Canonical<T>::value is a chart around zero<T>::value
+ * 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,12 +120,17 @@ 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/CalibratedCamera.h

@@ -220,5 +220,24 @@ private:
   }
 
   /// @}
-      };}
+};
+
+// Define GTSAM traits
+namespace traits {
+
+template<>
+struct is_group<CalibratedCamera> : public std::true_type {
+};
+
+template<>
+struct is_manifold<CalibratedCamera> : public std::true_type {
+};
+
+template<>
+struct dimension<CalibratedCamera> : public std::integral_constant<int, 6> {
+};
+
+}
+
+}
 

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/nonlinear/Values-inl.h

@@ -65,19 +65,19 @@ namespace gtsam {
   };
   // partial specialized version for ValueType == Value
   template<class CastedKeyValuePairType, class KeyValuePairType>
-  struct ValuesCastHelper<Value,CastedKeyValuePairType,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?
+      // 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> {
+  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?
+      // 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);
     }
   };
@@ -262,11 +262,11 @@ namespace gtsam {
       throw ValuesKeyDoesNotExist("retrieve", j);
 
     // Check the type and throw exception if incorrect
-    if(typeid(*item->second) != typeid(GenericValue<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 GenericValue<ValueType>&>(*item->second).value();
   }
 
   /* ************************************************************************* */
@@ -276,25 +276,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(GenericValue<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 GenericValue<ValueType>&>(*item->second).value();
      } 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&>(GenericValue<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&>(GenericValue<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,8 +44,7 @@
 #include <string>
 #include <utility>
 
-#include <gtsam/base/Value.h>
+#include <gtsam/base/ChartValue.h>
-#include <gtsam/base/GenericValue.h>
 #include <gtsam/base/FastMap.h>
 #include <gtsam/inference/Key.h>
 
@@ -249,15 +248,22 @@ namespace gtsam {
     /** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
     void insert(Key j, const Value& val);
 
-    /** Templated verion to add a variable with the given j,
-     * throws KeyAlreadyExists<J> if j is already present
-     * will wrap the val into a GenericValue<ValueType> to insert*/
-    template <typename ValueType> void insert(Key j, const ValueType& val);
-
-
     /** 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
@@ -266,7 +272,17 @@ namespace gtsam {
 
     /** single element change of existing element */
     void update(Key j, const Value& val);
-    template <typename T> void update(Key j, const T& 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);
@@ -377,9 +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((!std::is_same<ValueType,Value>::value));
+      BOOST_STATIC_ASSERT((!std::is_same<ValueType, Value>::value));
       // Filter and check the type
-      return filter(key_value.key) && (typeid(GenericValue<ValueType>) == typeid(key_value.value) );
+      return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
     }
 
     /** Serialization function */

gtsam/nonlinear/tests/testValues.cpp

@@ -38,7 +38,7 @@ static double inf = std::numeric_limits<double>::infinity();
 using symbol_shorthand::X;
 using symbol_shorthand::L;
 
-const Symbol key1('v',1), key2('v',2), key3('v',3), key4('v',4);
+const Symbol key1('v', 1), key2('v', 2), key3('v', 3), key4('v', 4);
 
 
 class TestValueData {
@@ -61,16 +61,25 @@ public:
   Vector localCoordinates(const TestValue&) const { return Vector(); }
 };
 
+namespace gtsam {
+namespace traits {
+template <>
+struct is_manifold<TestValue> : public std::true_type {};
+template <>
+struct dimension<TestValue> : public std::integral_constant<int, 0> {};
+}
+}
+
 /* ************************************************************************* */
 TEST( Values, equals1 )
 {
   Values expected;
   LieVector v((Vector(3) << 5.0, 6.0, 7.0));
 
-  expected.insert(key1,v);
+  expected.insert(key1, v);
   Values actual;
-  actual.insert(key1,v);
+  actual.insert(key1, v);
-  CHECK(assert_equal(expected,actual));
+  CHECK(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */
@@ -260,7 +269,7 @@ TEST(Values, expmap_d)
   CHECK(config0.equals(config0));
 
   Values poseconfig;
-  poseconfig.insert(key1, Pose2(1,2,3));
+  poseconfig.insert(key1, Pose2(1, 2, 3));
   poseconfig.insert(key2, Pose2(0.3, 0.4, 0.5));
 
   CHECK(equal(config0, config0));
@@ -330,7 +339,7 @@ TEST(Values, update)
   Values expected;
   expected.insert(key1, LieVector((Vector(1) << -1.)));
   expected.insert(key2, LieVector((Vector(1) << -2.)));
-  CHECK(assert_equal(expected,config0));
+  CHECK(assert_equal(expected, config0));
 }
 
 /* ************************************************************************* */
@@ -410,9 +419,9 @@ TEST(Values, Symbol_filter) {
 
   Values values;
   values.insert(X(0), pose0);
-  values.insert(Symbol('y',1), pose1);
+  values.insert(Symbol('y', 1), pose1);
   values.insert(X(2), pose2);
-  values.insert(Symbol('y',3), pose3);
+  values.insert(Symbol('y', 3), pose3);
 
   int i = 0;
   BOOST_FOREACH(const Values::Filtered<Value>::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) {

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

@@ -293,8 +293,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_;
@@ -303,6 +304,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> ;
 
@@ -374,7 +422,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;
@@ -504,7 +552,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>
@@ -559,7 +607,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;
 
@@ -604,8 +652,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;
 
@@ -658,8 +706,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/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,48 +66,48 @@ 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)));
 }
 
 /* ************************************************************************* */
 // zero
 TEST(Manifold, _zero) {
-  EXPECT(assert_equal(Pose3(),traits::zero<Pose3>::value()));
+  EXPECT(assert_equal(Pose3(), traits::zero<Pose3>::value()));
   Cal3Bundler cal(0, 0, 0);
-  EXPECT(assert_equal(cal,traits::zero<Cal3Bundler>::value()));
+  EXPECT(assert_equal(cal, traits::zero<Cal3Bundler>::value()));
-  EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero<Camera>::value()));
+  EXPECT(assert_equal(Camera(Pose3(), cal), traits::zero<Camera>::value()));
 }
 
 /* ************************************************************************* */
@@ -114,48 +115,48 @@ 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));
+  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));
+  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));
+  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));
+  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));
+  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));
+  EXPECT(assert_equal(chart6.retract(v9), camera2));
 }
 
 /* ************************************************************************* */