Loading...
Searching...
No Matches
Expression.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
#include <gtsam/nonlinear/internal/JacobianMap.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/VectorSpace.h>
#include <boost/make_shared.hpp>
#include <map>
// Forward declare tests
class ExpressionFactorShallowTest;
namespace gtsam {
// Forward declares
class Values;
template<typename T> class ExpressionFactor;
namespace internal {
template<typename T> class ExecutionTrace;
template<typename T> class ExpressionNode;
}
template<typename T>
class Expression {
public:
typedef Expression<T> type;
protected:
// Paul's trick shared pointer, polymorphic root of entire expression tree
boost::shared_ptr<internal::ExpressionNode<T> > root_;
Expression(const boost::shared_ptr<internal::ExpressionNode<T> >& root) : root_(root) {}
public:
// Expressions wrap trees of functions that can evaluate their own derivatives.
// The meta-functions below are useful to specify the type of those functions.
// Example, a function taking a camera and a 3D point and yielding a 2D point:
// Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
template<class A1>
struct UnaryFunction {
typedef std::function<
T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
};
template<class A1, class A2>
struct BinaryFunction {
typedef std::function<
T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type)> type;
};
template<class A1, class A2, class A3>
struct TernaryFunction {
typedef std::function<
T(const A1&, const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type)> type;
};
Expression(const T& value);
Expression(const Key& key);
Expression(const Symbol& symbol);
Expression(unsigned char c, std::uint64_t j);
template<typename A>
Expression(typename UnaryFunction<A>::type function,
const Expression<A>& expression);
template<typename A1, typename A2>
Expression(typename BinaryFunction<A1, A2>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2);
template<typename A1, typename A2, typename A3>
Expression(typename TernaryFunction<A1, A2, A3>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2,
const Expression<A3>& expression3);
template<typename A>
Expression(const Expression<A>& expression,
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const);
template<typename A1, typename A2>
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);
template<typename A1, typename A2, typename A3>
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);
virtual ~Expression() {
}
std::set<Key> keys() const;
void dims(std::map<Key, int>& map) const;
void print(const std::string& s) const;
T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
boost::none) const;
virtual boost::shared_ptr<Expression> clone() const {
return boost::make_shared<Expression>(*this);
}
const boost::shared_ptr<internal::ExpressionNode<T> >& root() const;
size_t traceSize() const;
Expression<T>& operator+=(const Expression<T>& e);
protected:
Expression() {}
typedef std::pair<KeyVector, FastVector<int> > KeysAndDims;
KeysAndDims keysAndDims() const;
T valueAndDerivatives(const Values& values, const KeyVector& keys,
const FastVector<int>& dims, std::vector<Matrix>& H) const;
T traceExecution(const Values& values, internal::ExecutionTrace<T>& trace,
void* traceStorage) const;
T valueAndJacobianMap(const Values& values,
internal::JacobianMap& jacobians) const;
// be very selective on who can access these private methods:
friend class ExpressionFactor<T> ;
friend class internal::ExpressionNode<T>;
// and add tests
friend class ::ExpressionFactorShallowTest;
};
template <typename T>
class ScalarMultiplyExpression : public Expression<T> {
// Check that T is a vector space
BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
public:
explicit ScalarMultiplyExpression(double s, const Expression<T>& e);
};
template <typename T>
class BinarySumExpression : public Expression<T> {
// Check that T is a vector space
BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
public:
explicit BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2);
};
template <typename T, typename A>
Expression<T> linearExpression(
const std::function<T(A)>& f, const Expression<A>& expression,
const Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension>& dTdA) {
// Use lambda to endow f with a linear Jacobian
typename Expression<T>::template UnaryFunction<A>::type g =
[=](const A& value, typename MakeOptionalJacobian<T, A>::type H) {
if (H)
*H << dTdA;
return f(value);
};
return Expression<T>(g, expression);
}
template <typename T>
ScalarMultiplyExpression<T> operator*(double s, const Expression<T>& e) {
return ScalarMultiplyExpression<T>(s, e);
}
template <typename T>
BinarySumExpression<T> operator+(const Expression<T>& e1, const Expression<T>& e2) {
return BinarySumExpression<T>(e1, e2);
}
template <typename T>
BinarySumExpression<T> operator-(const Expression<T>& e1, const Expression<T>& e2) {
// TODO(frank, abe): Implement an actual negate operator instead of multiplying by -1
return e1 + (-1.0) * e2;
}
template<typename T>
Expression<T> operator*(const Expression<T>& e1, const Expression<T>& e2);
template<typename T>
std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start = 0);
} // namespace gtsam
#include <gtsam/nonlinear/Expression-inl.h>
OptionalJacobian.h
Special class for optional Jacobian arguments.
Expression-inl.h
Internals for Expression.h, not for general consumption.
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::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+
Errors operator+(const Errors &a, const Errors &b)
Addition.
Definition Errors.cpp:60
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
gtsam::linearExpression
Expression< T > linearExpression(const std::function< T(A)> &f, const Expression< A > &expression, const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > &dTdA)
Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA TODO(frank): creat...
Definition Expression.h:241
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::operator-
Errors operator-(const Errors &a, const Errors &b)
Subtraction.
Definition Errors.cpp:75
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::IsVectorSpace
Vector Space concept.
Definition VectorSpace.h:470
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
gtsam::internal::ExecutionTrace
Definition Expression.h:40
gtsam::internal::ExpressionNode
Definition Expression.h:41
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
gtsam::Expression< NavState >::root
const boost::shared_ptr< internal::ExpressionNode< NavState > > & root() const
gtsam::Expression< NavState >::type
Expression< NavState > type
Definition Expression.h:53
gtsam::Expression::Expression
Expression()
Default constructor, for serialization.
Definition Expression.h:182
gtsam::Expression::~Expression
virtual ~Expression()
Destructor.
Definition Expression.h:141
gtsam::Expression::clone
virtual boost::shared_ptr< Expression > clone() const
Definition Expression.h:166
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::Expression
Expression(const Expression< A > &expression, T(A::*method)(typename MakeOptionalJacobian< T, A >::type) const)
Construct a nullary method expression.
Definition Expression-inl.h:90
gtsam::Expression< NavState >::KeysAndDims
std::pair< KeyVector, FastVector< int > > KeysAndDims
Definition Expression.h:185
gtsam::Expression::Expression
Expression(const Key &key)
Construct a leaf expression, with Key.
Definition Expression-inl.h:44
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::Expression
Expression(typename TernaryFunction< A1, A2, A3 >::type function, const Expression< A1 > &expression1, const Expression< A2 > &expression2, const Expression< A3 > &expression3)
Construct a ternary function expression.
Definition Expression-inl.h:79
gtsam::Expression::Expression
Expression(unsigned char c, std::uint64_t j)
Construct a leaf expression, creating Symbol.
Definition Expression-inl.h:54
gtsam::Expression::Expression
Expression(const Symbol &symbol)
Construct a leaf expression, with Symbol.
Definition Expression-inl.h:49
gtsam::Expression::print
void print(const std::string &s) const
Print.
Definition Expression-inl.h:142
gtsam::Expression::Expression
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)
Construct a unary method expression.
Definition Expression-inl.h:101
gtsam::Expression::valueAndJacobianMap
T valueAndJacobianMap(const Values &values, internal::JacobianMap &jacobians) const
brief Return value and derivatives, reverse AD version
Definition Expression-inl.h:205
gtsam::Expression::Expression
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)
Construct a binary method expression.
Definition Expression-inl.h:116
gtsam::Expression::Expression
Expression(typename UnaryFunction< A >::type function, const Expression< A > &expression)
Construct a unary function expression.
Definition Expression-inl.h:61
gtsam::Expression::valueAndDerivatives
T valueAndDerivatives(const Values &values, const KeyVector &keys, const FastVector< int > &dims, std::vector< Matrix > &H) const
private version that takes keys and dimensions, returns derivatives
Definition Expression-inl.h:174
gtsam::Expression< NavState >::value
NavState value(const Values &values, boost::optional< std::vector< Matrix > & > H=boost::none) const
gtsam::Expression::Expression
Expression(const T &value)
Construct a constant expression.
Definition Expression-inl.h:39
gtsam::Expression::operator+=
Expression< T > & operator+=(const Expression< T > &e)
Add another expression to this expression.
Definition Expression-inl.h:302
gtsam::Expression::Expression
Expression(typename BinaryFunction< A1, A2 >::type function, const Expression< A1 > &expression1, const Expression< A2 > &expression2)
Construct a binary function expression.
Definition Expression-inl.h:69
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::Expression::UnaryFunction
Definition Expression.h:70
gtsam::Expression::BinaryFunction
Definition Expression.h:76
gtsam::Expression::TernaryFunction
Definition Expression.h:83
gtsam::ScalarMultiplyExpression
A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar It optimiz...
Definition Expression.h:213
gtsam::BinarySumExpression
A BinarySumExpression is a specialization of Expression that adds two expressions together It optimiz...
Definition Expression.h:226
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
Symbol.h