12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

JacobianMap, header discipline

release/4.3a0
dellaert 2015-05-11 20:33:15 -07:00
parent 0e764fadb3
commit 8d8f270b60
6 changed files with 91 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/nonlinear/ExecutionTrace.h
View File

@ -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/nonlinear/ExpressionNode.h>
//#include <gtsam/base/Lie.h>
//
//#include <boost/foreach.hpp>
//#include <boost/tuple/tuple.hpp>
//#include <boost/bind.hpp>
//#include <boost/type_traits/aligned_storage.hpp>
//
//#include <map>
#include <gtsam/base/Manifold.h>
#include <boost/type_traits/aligned_storage.hpp>
#include <Eigen/Core>
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>

40
gtsam/nonlinear/Expression-inl.h
View File

@ -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/type_traits/aligned_storage.hpp>
#include <map>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm.hpp>
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(
typename TernaryFunction<A1, A2, A3>::type function,
Expression<T>::Expression(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 {
return root_->traceExecution(values, trace, traceStorage);
void* traceStorage) const {
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:
JacobianMap::JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
keys_(keys), Ab_(Ab) {
}
VerticalBlockMatrix::Block JacobianMap::operator()(Key key) {
FastVector<Key>::const_iterator it = std::find(keys_.begin(), keys_.end(),
key);
DenseIndex block = it - keys_.begin();
return Ab_(block);
template<typename T>
typename Expression<T>::KeysAndDims Expression<T>::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;
}
} // namespace gtsam

48
gtsam/nonlinear/Expression.h
View File

@ -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 <boost/range/algorithm.hpp>
#include <boost/make_shared.hpp>
#include <map>
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 {
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;
}
KeysAndDims keysAndDims() const;
/// 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>

1
gtsam/nonlinear/ExpressionNode.h
View File

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

52
gtsam/nonlinear/JacobianMap.h Normal file
View File

@ -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

3
gtsam/nonlinear/tests/testExecutionTrace.cpp
View File

@ -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;
}
/* ************************************************************************* */