JacobianMap, header discipline

gtsam/nonlinear/ExecutionTrace.h

@@ -11,30 +11,31 @@
 
 /**
  * @file ExecutionTrace.h
- * @date September 18, 2014
+ * @date May 11, 2015
  * @author Frank Dellaert
- * @brief Used in Expression.h, not for general consumption
+ * @brief Execution trace for expressions
  */
 
 #pragma once
 
-#include <Eigen/Core>
+#include <gtsam/nonlinear/JacobianMap.h>
 #include <gtsam/inference/Key.h>
-//#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Manifold.h>
-//#include <gtsam/nonlinear/ExpressionNode.h>
+
-//#include <gtsam/base/Lie.h>
+#include <boost/type_traits/aligned_storage.hpp>
-//
+
-//#include <boost/foreach.hpp>
+#include <Eigen/Core>
-//#include <boost/tuple/tuple.hpp>
-//#include <boost/bind.hpp>
-//#include <boost/type_traits/aligned_storage.hpp>
-//
-//#include <map>
 
 namespace gtsam {
 
 template<int T> struct CallRecord;
 
+/// Storage type for the execution trace.
+/// It enforces the proper alignment in a portable way.
+/// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
+static const unsigned TraceAlignment = 16;
+typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
+
 namespace internal {
 
 template<bool UseBlock, typename Derived>

gtsam/nonlinear/Expression-inl.h

@@ -19,16 +19,11 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/ExecutionTrace.h>
 #include <gtsam/nonlinear/ExpressionNode.h>
-#include <gtsam/base/Lie.h>
 
-#include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
-#include <boost/bind.hpp>
+#include <boost/range/adaptor/map.hpp>
-#include <boost/type_traits/aligned_storage.hpp>
+#include <boost/range/algorithm.hpp>
-
-#include <map>
 
 namespace gtsam {
 
@@ -116,8 +111,7 @@ Expression<T>::Expression(const Expression<A1>& expression1,
 /// Construct a ternary function expression
 template<typename T>
 template<typename A1, typename A2, typename A3>
-Expression<T>::Expression(
+Expression<T>::Expression(typename TernaryFunction<A1, A2, A3>::type function,
-    typename TernaryFunction<A1, A2, A3>::type function,
     const Expression<A1>& expression1, const Expression<A2>& expression2,
     const Expression<A3>& expression3) :
     root_(
@@ -125,7 +119,6 @@ Expression<T>::Expression(
             expression3)) {
 }
 
-
 /// Return root
 template<typename T>
 const boost::shared_ptr<ExpressionNode<T> >& Expression<T>::root() const {
@@ -156,7 +149,8 @@ void Expression<T>::dims(std::map<Key, int>& map) const {
  * The order of the Jacobians is same as keys in either keys() or dims()
  */
 template<typename T>
-T Expression<T>::value(const Values& values, boost::optional<std::vector<Matrix>&> H) const {
+T Expression<T>::value(const Values& values,
+    boost::optional<std::vector<Matrix>&> H) const {
 
   if (H) {
     // Call private version that returns derivatives in H
@@ -193,8 +187,9 @@ T Expression<T>::value(const Values& values, const FastVector<Key>& keys,
 
 template<typename T>
 T Expression<T>::traceExecution(const Values& values, ExecutionTrace<T>& trace,
-    ExecutionTraceStorage* traceStorage) const {
+    void* traceStorage) const {
-  return root_->traceExecution(values, trace, traceStorage);
+  return root_->traceExecution(values, trace,
+      static_cast<ExecutionTraceStorage*>(traceStorage));
 }
 
 template<typename T>
@@ -226,16 +221,15 @@ T Expression<T>::value(const Values& values, JacobianMap& jacobians) const {
   return value;
 }
 
-// JacobianMap:
+template<typename T>
-JacobianMap::JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
+typename Expression<T>::KeysAndDims Expression<T>::keysAndDims() const {
-    keys_(keys), Ab_(Ab) {
+  std::map<Key, int> map;
-}
+  dims(map);
-
+  size_t n = map.size();
-VerticalBlockMatrix::Block JacobianMap::operator()(Key key) {
+  KeysAndDims pair = std::make_pair(FastVector < Key > (n), FastVector<int>(n));
-  FastVector<Key>::const_iterator it = std::find(keys_.begin(), keys_.end(),
+  boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
-      key);
+  boost::copy(map | boost::adaptors::map_values, pair.second.begin());
-  DenseIndex block = it - keys_.begin();
+  return pair;
-  return Ab_(block);
 }
 
 } // namespace gtsam

gtsam/nonlinear/Expression.h

@@ -19,15 +19,12 @@
 
 #pragma once
 
+#include <gtsam/nonlinear/JacobianMap.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/base/OptionalJacobian.h>
-#include <gtsam/base/FastVector.h>
-#include <gtsam/base/VerticalBlockMatrix.h>
 
 #include <boost/bind.hpp>
-#include <boost/range/adaptor/map.hpp>
+#include <map>
-#include <boost/range/algorithm.hpp>
-#include <boost/make_shared.hpp>
 
 class ExpressionFactorShallowTest;
 
@@ -39,16 +36,6 @@ template<typename T> class ExecutionTrace;
 template<typename T> class ExpressionNode;
 template<typename T> class ExpressionFactor;
 
-// A JacobianMap is the primary mechanism by which derivatives are returned.
-// For clarity, it is forward declared here but implemented at the end of this header.
-class JacobianMap;
-
-/// Storage type for the execution trace.
-/// It enforces the proper alignment in a portable way.
-/// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
-const unsigned TraceAlignment = 16;
-typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
-
 /**
  * Expression class that supports automatic differentiation
  */
@@ -176,15 +163,7 @@ private:
 
   /// Vaguely unsafe keys and dimensions in same order
   typedef std::pair<FastVector<Key>, FastVector<int> > KeysAndDims;
-  KeysAndDims keysAndDims() const {
+  KeysAndDims keysAndDims() const;
-    std::map<Key, int> map;
-    dims(map);
-    size_t n = map.size();
-    KeysAndDims pair = std::make_pair(FastVector<Key>(n), FastVector<int>(n));
-    boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
-    boost::copy(map | boost::adaptors::map_values, pair.second.begin());
-    return pair;
-  }
 
   /// private version that takes keys and dimensions, returns derivatives
   T value(const Values& values, const FastVector<Key>& keys,
@@ -192,7 +171,7 @@ private:
 
   /// trace execution, very unsafe
   T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const;
+      void* traceStorage) const;
 
   /**
    * @brief Return value and derivatives, reverse AD version
@@ -204,23 +183,6 @@ private:
   // be very selective on who can access these private methods:
   friend class ExpressionFactor<T> ;
   friend class ::ExpressionFactorShallowTest;
-
-};
-
-// Expressions are designed to write their derivatives into an already allocated
-// Jacobian of the correct size, of type VerticalBlockMatrix.
-// The JacobianMap provides a mapping from keys to the underlying blocks.
-class JacobianMap {
-private:
-  const FastVector<Key>& keys_;
-  VerticalBlockMatrix& Ab_;
-
-public:
-  /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab
-  JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab);
-
-  /// Access blocks of via key
-  VerticalBlockMatrix::Block operator()(Key key);
 };
 
 // http://stackoverflow.com/questions/16260445/boost-bind-to-operator
@@ -252,7 +214,7 @@ std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start = 0) {
   return unknowns;
 }
 
-}
+} // namespace gtsam
 
 #include <gtsam/nonlinear/Expression-inl.h>
 

gtsam/nonlinear/ExpressionNode.h

@@ -19,6 +19,7 @@
 
 #pragma once
 
+#include <gtsam/nonlinear/ExecutionTrace.h>
 #include <gtsam/nonlinear/CallRecord.h>
 #include <gtsam/nonlinear/Values.h>
 

gtsam/nonlinear/JacobianMap.h

@@ -0,0 +1,52 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 JacobianMap.h
+ * @date May 11, 2015
+ * @author Frank Dellaert
+ * @brief JacobianMap for returning derivatives from expressions
+ */
+
+#pragma once
+
+#include <gtsam/inference/Key.h>
+#include <gtsam/base/FastVector.h>
+#include <gtsam/base/VerticalBlockMatrix.h>
+
+namespace gtsam {
+
+// A JacobianMap is the primary mechanism by which derivatives are returned.
+// Expressions are designed to write their derivatives into an already allocated
+// Jacobian of the correct size, of type VerticalBlockMatrix.
+// The JacobianMap provides a mapping from keys to the underlying blocks.
+class JacobianMap {
+private:
+  typedef FastVector<Key> Keys;
+  const FastVector<Key>& keys_;
+  VerticalBlockMatrix& Ab_;
+
+public:
+  /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab
+  JacobianMap(const Keys& keys, VerticalBlockMatrix& Ab) :
+      keys_(keys), Ab_(Ab) {
+  }
+
+  /// Access blocks of via key
+  VerticalBlockMatrix::Block operator()(Key key) {
+    Keys::const_iterator it = std::find(keys_.begin(), keys_.end(), key);
+    DenseIndex block = it - keys_.begin();
+    return Ab_(block);
+  }
+};
+
+} // namespace gtsam
+

gtsam/nonlinear/tests/testExecutionTrace.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <gtsam/nonlinear/ExecutionTrace.h>
+#include <gtsam/geometry/Point2.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -26,7 +27,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 // Constant
 TEST(ExecutionTrace, construct) {
-  ExecutionTrace trace;
+  ExecutionTrace<Point2> trace;
 }
 
 /* ************************************************************************* */