Comments, formatting, some TODO questions

gtsam/base/ChartValue.h

@@ -11,9 +11,10 @@
 
 /*
  * @file ChartValue.h
- * @date Jan 26, 2012
+ * @brief
+ * @date October, 2014
  * @author Michael Bosse, Abel Gawel, Renaud Dube
- * based on DrivedValue.h by Duy Nguyen Ta
+ * based on DerivedValue.h by Duy Nguyen Ta
  */
 
 #pragma once
@@ -39,25 +40,44 @@
 #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.
+ * ChartValue is derived from GenericValue<T> and Chart so that
-template<class T, class Chart_=DefaultChart<T> >
+ * Chart can be zero sized (as in DefaultChart<T>)
-class ChartValue : public GenericValue<T>, public Chart_ {
+ * 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:
+
+public:
+
   typedef T type;
   typedef Chart_ Chart;
 
- public:
+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() {}
+  /// Default Constructor. TODO might not make sense for some types
+  ChartValue() :
+      GenericValue<T>(T()) {
+  }
+
+  /// Construct froma  value
+  ChartValue(const T& value) :
+      GenericValue<T>(value) {
+  }
+
+  /// Construct from a value and initialize the chart
+  template<typename C>
+  ChartValue(const T& value, C chart_initializer) :
+      GenericValue<T>(value), Chart(chart_initializer) {
+  }
+
+  /// Destructor
+  virtual ~ChartValue() {
+  }
 
   /**
    * Create a duplicate object returned as a pointer to the generic Value interface.
@@ -66,7 +86,7 @@ class ChartValue : public GenericValue<T>, public Chart_ {
    */
   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
+    ChartValue* ptr = new (place) ChartValue(*this); // calls copy constructor to fill in
     return ptr;
   }
 
@@ -75,7 +95,7 @@ class ChartValue : public GenericValue<T>, public Chart_ {
    */
   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
+    boost::singleton_pool<PoolTag, sizeof(ChartValue)>::free((void*) this); // Release memory from pool
   }
 
   /**
@@ -85,18 +105,16 @@ class ChartValue : public GenericValue<T>, public Chart_ {
     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();
+    void* resultAsValuePlace =
-    Value* resultAsValue = new(resultAsValuePlace) ChartValue(retractResult, static_cast<const Chart&>(*this));
+        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;
@@ -105,7 +123,8 @@ class ChartValue : public GenericValue<T>, public Chart_ {
   /// 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);
+    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());
@@ -113,7 +132,8 @@ class ChartValue : public GenericValue<T>, public Chart_ {
 
   /// Non-virtual version of retract
   ChartValue retract(const Vector& delta) const {
-    return ChartValue(Chart::retract(GenericValue<T>::value(), delta),static_cast<const Chart&>(*this));
+    return ChartValue(Chart::retract(GenericValue<T>::value(), delta),
+        static_cast<const Chart&>(*this));
   }
 
   /// Non-virtual version of localCoordinates
@@ -121,8 +141,10 @@ class ChartValue : public GenericValue<T>, public Chart_ {
     return localCoordinates_(value2);
   }
 
+  /// Return run-time dimensionality
   virtual size_t dim() const {
-    return Chart::getDimension(GenericValue<T>::value()); // need functional form here since the dimension may be dynamic
+    // need functional form here since the dimension may be dynamic
+    return Chart::getDimension(GenericValue<T>::value());
   }
 
   /// Assignment operator
@@ -136,6 +158,7 @@ class ChartValue : public GenericValue<T>, public Chart_ {
   }
 
 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.
@@ -145,44 +168,52 @@ protected:
   //  }
 
 private:
+
   /// Fake Tag struct for singleton pool allocator. In fact, it is never used!
-  struct PoolTag { };
+  struct PoolTag {
+  };
 
 private:
 
-   /// @}
+  /** Serialization function */
-   /// @name Advanced Interface
+  friend class boost::serialization::access;
-   /// @{
+  template<class ARCHIVE>
-
+  void serialize(ARCHIVE & ar, const unsigned int version) {
-   /** Serialization function */
+    // ar & boost::serialization::make_nvp("value",);
-   friend class boost::serialization::access;
+    // todo: implement a serialization for charts
-   template<class ARCHIVE>
+    ar
-   void serialize(ARCHIVE & ar, const unsigned int version) {
+        & boost::serialization::make_nvp("GenericValue",
-     // ar & boost::serialization::make_nvp("value",);
+            boost::serialization::base_object<GenericValue<T> >(*this));
-     // todo: implement a serialization for charts
+  }
-     ar & boost::serialization::make_nvp("GenericValue", boost::serialization::base_object< GenericValue<T> >(*this));
-   }
-
-   /// @}
 
 };
 
+// Define
 namespace traits {
-template <typename T, typename Chart>
-struct dimension<ChartValue<T, Chart> > : public dimension<Chart> {};
-}
 
+/// The dimension of a ChartValue is the dimension of the chart
+template<typename T, typename Chart>
+struct dimension<ChartValue<T, Chart> > : public dimension<Chart> {
+  // TODO Frank thinks dimension is a property of type, chart should conform
+};
+
+} // \ traits
+
+/// Get the chart from a Value
 template<typename Chart>
 const Chart& Value::getChart() const {
-// define Value::cast here since now ChartValue has been declared
+  return dynamic_cast<const Chart&>(*this);
-   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>
+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) {
+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();
+    *H = Eigen::Matrix<double, traits::dimension<T>::value,
+        traits::dimension<T>::value>::Identity();
   }
   return ChartValue<T>(value);
 }

gtsam/base/GenericValue.h

@@ -11,9 +11,10 @@
 
 /*
  * @file GenericValue.h
- * @date Jan 26, 2012
+ * @brief Wraps any type T so it can play as a Value
+ * @date October, 2014
  * @author Michael Bosse, Abel Gawel, Renaud Dube
- * based on DrivedValue.h by Duy Nguyen Ta
+ * based on DerivedValue.h by Duy Nguyen Ta
  */
 
 #pragma once
@@ -22,14 +23,16 @@
 
 namespace gtsam {
 
+// To play as a GenericValue, we need the following traits
 namespace traits {
 
 // trait to wrap the default equals of types
 template<typename T>
-  bool equals(const T& a, const T& b, double tol) {
+bool equals(const T& a, const T& b, double tol) {
-    return a.equals(b,tol);
+  return a.equals(b, tol);
-  }
+}
 
+// trait to wrap the default print of types
 template<typename T>
 void print(const T& obj, const std::string& str) {
   obj.print(str);
@@ -37,20 +40,40 @@ void print(const T& obj, const std::string& str) {
 
 }
 
+/**
+ * Wraps any type T so it can play as a Value
+ */
 template<class T>
-class GenericValue : public Value {
+class GenericValue: public Value {
+
 public:
+
   typedef T type;
+
 protected:
-  T value_;
+
+  T value_; ///< The wrapped value
 
 public:
-  GenericValue(const T& value) : value_(value) {}
 
-  const T& value() const { return value_; }
+  /// Construct from value
-  T& value() { return value_; }
+  GenericValue(const T& value) :
+      value_(value) {
+  }
 
-  virtual ~GenericValue() {}
+  /// Return a constant value
+  const T& value() const {
+    return value_;
+  }
+
+  /// Return the value
+  T& value() {
+    return value_;
+  }
+
+  /// Destructor
+  virtual ~GenericValue() {
+  }
 
   /// equals implementing generic Value interface
   virtual bool equals_(const Value& p, double tol = 1e-9) const {
@@ -60,30 +83,35 @@ public:
     return traits::equals<T>(this->value_, genericValue2.value_, tol);
   }
 
-  // non virtual equals function
+  /// non virtual equals function, uses traits
   bool equals(const GenericValue &other, double tol = 1e-9) const {
-    return traits::equals<T>(this->value(),other.value(),tol);
+    return traits::equals<T>(this->value(), other.value(), tol);
   }
 
+  /// Virtual print function, uses traits
   virtual void print(const std::string& str) const {
-    traits::print<T>(value_,str);
+    traits::print<T>(value_, str);
   }
+
+  // Serialization below:
+
   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
+
+  // Assignment operator for this class not needed since GenericValue<T> is an abstract class
 
 };
 
 // define Value::cast here since now GenericValue has been declared
 template<typename ValueType>
- const ValueType& Value::cast() const {
+const ValueType& Value::cast() const {
-   return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
+  return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
- }
+}
-
 
 } /* namespace gtsam */

gtsam/base/Manifold.h

@@ -67,9 +67,12 @@ struct is_manifold: public boost::false_type {
 };
 
 // dimension, can return Eigen::Dynamic (-1) if not known at compile time
+// defaults to dynamic, TODO makes sense ?
 typedef boost::integral_constant<int, Eigen::Dynamic> Dynamic;
 template<typename T>
-struct dimension : public Dynamic {}; //default to dynamic
+struct dimension: public Dynamic {
+};
+
 
 /**
  * zero<T>::value is intended to be the origin of a canonical coordinate system
@@ -140,7 +143,8 @@ struct zero<Eigen::Matrix<double, M, N, Options> > : public boost::integral_cons
   }
 };
 
-template <typename T> struct is_chart : public boost::false_type {};
+template<typename T> struct is_chart: public boost::false_type {
+};
 
 } // \ namespace traits
 
@@ -164,13 +168,15 @@ struct DefaultChart {
 
 namespace traits {
 // populate default traits
-template <typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {};
+template<typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {
-template <typename T> struct dimension<DefaultChart<T> > : public dimension<T> {};
+};
+template<typename T> struct dimension<DefaultChart<T> > : public dimension<T> {
+};
 }
 
 template<class C>
 struct ChartConcept {
- public:
+public:
   typedef typename C::type type;
   typedef typename C::vector vector;
 
@@ -192,20 +198,19 @@ struct ChartConcept {
     dim_ = C::getDimension(val_);
   }
 
- private:
+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 C> struct CanonicalChart {
- BOOST_CONCEPT_ASSERT((ChartConcept<C>));
+  BOOST_CONCEPT_ASSERT((ChartConcept<C>));
 
   typedef C Chart;
   typedef typename Chart::type type;
@@ -220,7 +225,8 @@ template<typename C> struct CanonicalChart {
     return Chart::retract(traits::zero<type>::value(), v);
   }
 };
-template <typename T> struct Canonical : public CanonicalChart<DefaultChart<T> > {};
+template<typename T> struct Canonical: public CanonicalChart<DefaultChart<T> > {
+};
 
 // double
 
@@ -248,8 +254,7 @@ template<int M, int N, int Options>
 struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
   typedef Eigen::Matrix<double, M, N, Options> type;
   typedef type T;
-  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
+  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
-  BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
       "DefaultChart has not been implemented yet for dynamically sized matrices");
   static vector local(const T& origin, const T& other) {
     T diff = other - origin;
@@ -262,7 +267,7 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
     return origin + map;
   }
   static int getDimension(const T&origin) {
-    return origin.rows()*origin.cols();
+    return origin.rows() * origin.cols();
   }
 };
 

gtsam/base/Value.h

@@ -124,6 +124,7 @@ namespace gtsam {
       //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;

gtsam/nonlinear/Values-inl.h

@@ -300,6 +300,7 @@ namespace gtsam {
    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) {
@@ -311,11 +312,13 @@ namespace gtsam {
   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) {

gtsam/nonlinear/Values.h

@@ -251,15 +251,18 @@ 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,
+    /** Templated version 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);
+
+    /// overloaded insert version that also specifies a chart
     template <typename ValueType, typename Chart>
     void insert(Key j, const ValueType& val);
-     // overloaded insert version that also specifies a chart initializer
+
+    /// 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);
 
@@ -273,14 +276,18 @@ 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,
+    /** Templated version 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);
+
+    /// overloaded insert version that also specifies a chart
     template <typename T, typename Chart>
     void update(Key j, const T& val);
+
+    /// overloaded insert version that also specifies a chart initializer
     template <typename T, typename Chart, typename ChartInit>
     void update(Key j, const T& val, ChartInit chart);