From 8d8f270b608874fc20a44219ee62a0deb06ada47 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 20:33:15 -0700 Subject: [PATCH] JacobianMap, header discipline --- gtsam/nonlinear/ExecutionTrace.h | 27 +++++----- gtsam/nonlinear/Expression-inl.h | 40 +++++++-------- gtsam/nonlinear/Expression.h | 48 ++---------------- gtsam/nonlinear/ExpressionNode.h | 1 + gtsam/nonlinear/JacobianMap.h | 52 ++++++++++++++++++++ gtsam/nonlinear/tests/testExecutionTrace.cpp | 3 +- 6 files changed, 91 insertions(+), 80 deletions(-) create mode 100644 gtsam/nonlinear/JacobianMap.h diff --git a/gtsam/nonlinear/ExecutionTrace.h b/gtsam/nonlinear/ExecutionTrace.h index c0f1657be..292ad7719 100644 --- a/gtsam/nonlinear/ExecutionTrace.h +++ b/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 +#include #include -//#include -//#include -//#include -// -//#include -//#include -//#include -//#include -// -//#include +#include + +#include + +#include namespace gtsam { template 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 diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 7db3fd191..9c69cf0f7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,16 +19,11 @@ #pragma once -#include #include -#include -#include #include -#include -#include - -#include +#include +#include namespace gtsam { @@ -116,8 +111,7 @@ Expression::Expression(const Expression& expression1, /// Construct a ternary function expression template template -Expression::Expression( - typename TernaryFunction::type function, +Expression::Expression(typename TernaryFunction::type function, const Expression& expression1, const Expression& expression2, const Expression& expression3) : root_( @@ -125,7 +119,6 @@ Expression::Expression( expression3)) { } - /// Return root template const boost::shared_ptr >& Expression::root() const { @@ -156,7 +149,8 @@ void Expression::dims(std::map& map) const { * The order of the Jacobians is same as keys in either keys() or dims() */ template -T Expression::value(const Values& values, boost::optional&> H) const { +T Expression::value(const Values& values, + boost::optional&> H) const { if (H) { // Call private version that returns derivatives in H @@ -193,8 +187,9 @@ T Expression::value(const Values& values, const FastVector& keys, template T Expression::traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return root_->traceExecution(values, trace, traceStorage); + void* traceStorage) const { + return root_->traceExecution(values, trace, + static_cast(traceStorage)); } template @@ -226,16 +221,15 @@ T Expression::value(const Values& values, JacobianMap& jacobians) const { return value; } -// JacobianMap: -JacobianMap::JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : - keys_(keys), Ab_(Ab) { -} - -VerticalBlockMatrix::Block JacobianMap::operator()(Key key) { - FastVector::const_iterator it = std::find(keys_.begin(), keys_.end(), - key); - DenseIndex block = it - keys_.begin(); - return Ab_(block); +template +typename Expression::KeysAndDims Expression::keysAndDims() const { + std::map map; + dims(map); + size_t n = map.size(); + KeysAndDims pair = std::make_pair(FastVector < Key > (n), FastVector(n)); + boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); + boost::copy(map | boost::adaptors::map_values, pair.second.begin()); + return pair; } } // namespace gtsam diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index df575dbbf..1ea9e7f49 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -19,15 +19,12 @@ #pragma once +#include #include #include -#include -#include #include -#include -#include -#include +#include class ExpressionFactorShallowTest; @@ -39,16 +36,6 @@ template class ExecutionTrace; template class ExpressionNode; template 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 > KeysAndDims; - KeysAndDims keysAndDims() const { - std::map map; - dims(map); - size_t n = map.size(); - KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); - boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); - boost::copy(map | boost::adaptors::map_values, pair.second.begin()); - return pair; - } + KeysAndDims keysAndDims() const; /// private version that takes keys and dimensions, returns derivatives T value(const Values& values, const FastVector& keys, @@ -192,7 +171,7 @@ private: /// trace execution, very unsafe T traceExecution(const Values& values, ExecutionTrace& 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 ; 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& keys_; - VerticalBlockMatrix& Ab_; - -public: - /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab - JacobianMap(const FastVector& 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 > createUnknowns(size_t n, char c, size_t start = 0) { return unknowns; } -} +} // namespace gtsam #include diff --git a/gtsam/nonlinear/ExpressionNode.h b/gtsam/nonlinear/ExpressionNode.h index 259d3ab5d..26585ce4d 100644 --- a/gtsam/nonlinear/ExpressionNode.h +++ b/gtsam/nonlinear/ExpressionNode.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include diff --git a/gtsam/nonlinear/JacobianMap.h b/gtsam/nonlinear/JacobianMap.h new file mode 100644 index 000000000..43e22fc16 --- /dev/null +++ b/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 +#include +#include + +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 Keys; + const FastVector& 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 + diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp index b1b0038bd..cb4dfd2e7 100644 --- a/gtsam/nonlinear/tests/testExecutionTrace.cpp +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -17,6 +17,7 @@ */ #include +#include #include @@ -26,7 +27,7 @@ using namespace gtsam; /* ************************************************************************* */ // Constant TEST(ExecutionTrace, construct) { - ExecutionTrace trace; + ExecutionTrace trace; } /* ************************************************************************* */