Loading...
Searching...
No Matches
Expression-inl.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
// The MSVC compiler workaround for the unsupported variable length array
// utilizes the std::unique_ptr<> custom deleter.
// See Expression<T>::valueAndJacobianMap() below.
#ifdef _MSC_VER
#include <memory>
#endif
#include <gtsam/nonlinear/internal/ExpressionNode.h>
#include <boost/bind/bind.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm.hpp>
namespace gtsam {
template<typename T>
Expression<T>::Expression(const T& value) :
root_(new internal::ConstantExpression<T>(value)) {
}
template<typename T>
Expression<T>::Expression(const Key& key) :
root_(new internal::LeafExpression<T>(key)) {
}
template<typename T>
Expression<T>::Expression(const Symbol& symbol) :
root_(new internal::LeafExpression<T>(symbol)) {
}
template<typename T>
Expression<T>::Expression(unsigned char c, std::uint64_t j) :
root_(new internal::LeafExpression<T>(Symbol(c, j))) {
}
template<typename T>
template<typename A>
Expression<T>::Expression(typename UnaryFunction<A>::type function,
const Expression<A>& expression) :
root_(new internal::UnaryExpression<T, A>(function, expression)) {
}
template<typename T>
template<typename A1, typename A2>
Expression<T>::Expression(typename BinaryFunction<A1, A2>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2) :
root_(
new internal::BinaryExpression<T, A1, A2>(function, expression1,
expression2)) {
}
template<typename T>
template<typename A1, typename A2, typename A3>
Expression<T>::Expression(typename TernaryFunction<A1, A2, A3>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2,
const Expression<A3>& expression3) :
root_(
new internal::TernaryExpression<T, A1, A2, A3>(function, expression1,
expression2, expression3)) {
}
template<typename T>
template<typename A>
Expression<T>::Expression(const Expression<A>& expression,
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
root_(
new internal::UnaryExpression<T, A>(std::bind(method,
std::placeholders::_1, std::placeholders::_2),
expression)) {
}
template<typename T>
template<typename A1, typename A2>
Expression<T>::Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type) const,
const Expression<A2>& expression2) :
root_(
new internal::BinaryExpression<T, A1, A2>(
std::bind(method, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4),
expression1, expression2)) {
}
template<typename T>
template<typename A1, typename A2, typename A3>
Expression<T>::Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type) const,
const Expression<A2>& expression2, const Expression<A3>& expression3) :
root_(
new internal::TernaryExpression<T, A1, A2, A3>(
std::bind(method, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4, std::placeholders::_5,
std::placeholders::_6),
expression1, expression2, expression3)) {
}
template<typename T>
std::set<Key> Expression<T>::keys() const {
return root_->keys();
}
template<typename T>
void Expression<T>::dims(std::map<Key, int>& map) const {
root_->dims(map);
}
template<typename T>
void Expression<T>::print(const std::string& s) const {
root_->print(s);
}
template<typename T>
T Expression<T>::value(const Values& values,
boost::optional<std::vector<Matrix>&> H) const {
if (H) {
// Call private version that returns derivatives in H
KeyVector keys;
FastVector<int> dims;
boost::tie(keys, dims) = keysAndDims();
return valueAndDerivatives(values, keys, dims, *H);
} else
// no derivatives needed, just return value
return root_->value(values);
}
template<typename T>
const boost::shared_ptr<internal::ExpressionNode<T> >& Expression<T>::root() const {
return root_;
}
template<typename T>
size_t Expression<T>::traceSize() const {
return root_->traceSize();
}
// Private methods:
template<typename T>
T Expression<T>::valueAndDerivatives(const Values& values,
const KeyVector& keys, const FastVector<int>& dims,
std::vector<Matrix>& H) const {
// H should be pre-allocated
assert(H.size()==keys.size());
// Pre-allocate and zero VerticalBlockMatrix
static const int Dim = traits<T>::dimension;
VerticalBlockMatrix Ab(dims, Dim);
Ab.matrix().setZero();
internal::JacobianMap jacobianMap(keys, Ab);
// Call unsafe version
T result = valueAndJacobianMap(values, jacobianMap);
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
H[i] = Ab(i);
return result;
}
template<typename T>
T Expression<T>::traceExecution(const Values& values,
internal::ExecutionTrace<T>& trace, void* traceStorage) const {
return root_->traceExecution(values, trace,
static_cast<internal::ExecutionTraceStorage*>(traceStorage));
}
template<typename T>
T Expression<T>::valueAndJacobianMap(const Values& values,
internal::JacobianMap& jacobians) const {
// The following piece of code is absolutely crucial for performance.
// We allocate a block of memory on the stack, which can be done at runtime
// with modern C++ compilers. The traceExecution then fills this memory
// with an execution trace, made up entirely of "Record" structs, see
// the FunctionalNode class in expression-inl.h
size_t size = traceSize();
// Windows does not support variable length arrays, so memory must be dynamically
// allocated on Visual Studio. For more information see the issue below
// https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
#ifdef _MSC_VER
std::unique_ptr<void, void(*)(void*)> traceStorageDeleter(
_aligned_malloc(size, internal::TraceAlignment),
[](void *ptr){ _aligned_free(ptr); });
auto traceStorage = static_cast<internal::ExecutionTraceStorage*>(traceStorageDeleter.get());
#else
internal::ExecutionTraceStorage traceStorage[size];
#endif
internal::ExecutionTrace<T> trace;
T value(this->traceExecution(values, trace, traceStorage));
trace.startReverseAD1(jacobians);
return value;
}
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(KeyVector(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 internal {
// http://stackoverflow.com/questions/16260445/boost-bind-to-operator
template<class T>
struct apply_compose {
typedef T result_type;
static const int Dim = traits<T>::dimension;
T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
return x.compose(y, H1, H2);
}
};
template <>
struct apply_compose<double> {
double operator()(const double& x, const double& y,
OptionalJacobian<1, 1> H1 = boost::none,
OptionalJacobian<1, 1> H2 = boost::none) const {
if (H1) H1->setConstant(y);
if (H2) H2->setConstant(x);
return x * y;
}
};
}
// Global methods:
template<typename T>
Expression<T> operator*(const Expression<T>& expression1,
const Expression<T>& expression2) {
return Expression<T>(
std::bind(internal::apply_compose<T>(), std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3,
std::placeholders::_4),
expression1, expression2);
}
template<typename T>
std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start) {
std::vector<Expression<T> > unknowns;
unknowns.reserve(n);
for (size_t i = start; i < start + n; i++)
unknowns.push_back(Expression<T>(c, i));
return unknowns;
}
template <typename T>
ScalarMultiplyExpression<T>::ScalarMultiplyExpression(double s, const Expression<T>& e)
: Expression<T>(boost::make_shared<internal::ScalarMultiplyNode<T>>(s, e)) {}
template <typename T>
BinarySumExpression<T>::BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2)
: Expression<T>(boost::make_shared<internal::BinarySumNode<T>>(e1, e2)) {}
template <typename T>
Expression<T>& Expression<T>::operator+=(const Expression<T>& e) {
root_ = boost::make_shared<internal::BinarySumNode<T>>(*this, e);
return *this;
}
} // namespace gtsam
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:33
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Create a symbol key from a character and index, i.e.
Definition Symbol.h:135
gtsam::createUnknowns
std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
Construct an array of leaves.
Definition Expression-inl.h:284
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
gtsam::internal::apply_compose
Definition Expression-inl.h:247
gtsam::internal::ExecutionTrace
Definition Expression.h:40
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
gtsam::Expression::root
const boost::shared_ptr< internal::ExpressionNode< T > > & root() const
Return root.
Definition Expression-inl.h:162
gtsam::Expression::Expression
Expression()
Default constructor, for serialization.
Definition Expression.h:182
gtsam::Expression::Expression
Expression(const boost::shared_ptr< internal::ExpressionNode< T > > &root)
Construct with a custom root.
Definition Expression.h:61
gtsam::Expression::keys
std::set< Key > keys() const
Return keys that play in this expression.
Definition Expression-inl.h:132
gtsam::Expression::KeysAndDims
std::pair< KeyVector, FastVector< int > > KeysAndDims
Keys and dimensions in same order.
Definition Expression.h:185
gtsam::Expression::dims
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
Definition Expression-inl.h:137
gtsam::Expression::print
void print(const std::string &s) const
Print.
Definition Expression-inl.h:142
gtsam::Expression< typename Bearing< A1, A2 >::result_type >::valueAndJacobianMap
typename Bearing< A1, A2 >::result_type valueAndJacobianMap(const Values &values, internal::JacobianMap &jacobians) const
Definition Expression-inl.h:205
gtsam::Expression< typename Bearing< A1, A2 >::result_type >::valueAndDerivatives
typename Bearing< A1, A2 >::result_type valueAndDerivatives(const Values &values, const KeyVector &keys, const FastVector< int > &dims, std::vector< Matrix > &H) const
Definition Expression-inl.h:174
gtsam::Expression::value
T value(const Values &values, boost::optional< std::vector< Matrix > & > H=boost::none) const
Return value and optional derivatives, reverse AD version Notes: this is not terribly efficient,...
Definition Expression-inl.h:147
gtsam::Expression::operator+=
Expression< T > & operator+=(const Expression< T > &e)
Add another expression to this expression.
Definition Expression-inl.h:302
gtsam::Expression::traceSize
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
Definition Expression-inl.h:167
gtsam::Expression::traceExecution
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
trace execution, very unsafe
Definition Expression-inl.h:198
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65