gtsam  4.1.0
gtsam
Expression.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #pragma once
21 
22 #include <gtsam/nonlinear/internal/JacobianMap.h>
23 #include <gtsam/inference/Symbol.h>
25 #include <gtsam/base/VectorSpace.h>
26 
27 #include <boost/bind.hpp>
28 #include <boost/make_shared.hpp>
29 #include <map>
30 
31 // Forward declare tests
32 class ExpressionFactorShallowTest;
33 
34 namespace gtsam {
35 
36 // Forward declares
37 class Values;
38 template<typename T> class ExpressionFactor;
39 
40 namespace internal {
41 template<typename T> class ExecutionTrace;
42 template<typename T> class ExpressionNode;
43 }
44 
48 template<typename T>
49 class Expression {
50 
51 public:
52 
55 
56 protected:
57 
58  // Paul's trick shared pointer, polymorphic root of entire expression tree
59  boost::shared_ptr<internal::ExpressionNode<T> > root_;
60 
62  Expression(const boost::shared_ptr<internal::ExpressionNode<T> >& root) : root_(root) {}
63 
64 public:
65 
66  // Expressions wrap trees of functions that can evaluate their own derivatives.
67  // The meta-functions below are useful to specify the type of those functions.
68  // Example, a function taking a camera and a 3D point and yielding a 2D point:
69  // Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
70  template<class A1>
71  struct UnaryFunction {
72  typedef boost::function<
73  T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
74  };
75 
76  template<class A1, class A2>
77  struct BinaryFunction {
78  typedef boost::function<
79  T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
80  typename MakeOptionalJacobian<T, A2>::type)> type;
81  };
82 
83  template<class A1, class A2, class A3>
84  struct TernaryFunction {
85  typedef boost::function<
86  T(const A1&, const A2&, const A3&,
89  typename MakeOptionalJacobian<T, A3>::type)> type;
90  };
91 
93  Expression(const T& value);
94 
96  Expression(const Key& key);
97 
99  Expression(const Symbol& symbol);
100 
102  Expression(unsigned char c, std::uint64_t j);
103 
105  template<typename A>
106  Expression(typename UnaryFunction<A>::type function,
107  const Expression<A>& expression);
108 
110  template<typename A1, typename A2>
111  Expression(typename BinaryFunction<A1, A2>::type function,
112  const Expression<A1>& expression1, const Expression<A2>& expression2);
113 
115  template<typename A1, typename A2, typename A3>
116  Expression(typename TernaryFunction<A1, A2, A3>::type function,
117  const Expression<A1>& expression1, const Expression<A2>& expression2,
118  const Expression<A3>& expression3);
119 
121  template<typename A>
122  Expression(const Expression<A>& expression,
123  T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const);
124 
126  template<typename A1, typename A2>
127  Expression(const Expression<A1>& expression1,
128  T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
129  typename MakeOptionalJacobian<T, A2>::type) const,
130  const Expression<A2>& expression2);
131 
133  template<typename A1, typename A2, typename A3>
134  Expression(const Expression<A1>& expression1,
135  T (A1::*method)(const A2&, const A3&,
138  typename MakeOptionalJacobian<T, A3>::type) const,
139  const Expression<A2>& expression2, const Expression<A3>& expression3);
140 
142  virtual ~Expression() {
143  }
144 
146  std::set<Key> keys() const;
147 
149  void dims(std::map<Key, int>& map) const;
150 
152  void print(const std::string& s) const;
153 
159  T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
160  boost::none) const;
161 
167  virtual boost::shared_ptr<Expression> clone() const {
168  return boost::make_shared<Expression>(*this);
169  }
170 
172  const boost::shared_ptr<internal::ExpressionNode<T> >& root() const;
173 
175  size_t traceSize() const;
176 
179 
180 protected:
181 
184 
186  typedef std::pair<KeyVector, FastVector<int> > KeysAndDims;
187  KeysAndDims keysAndDims() const;
188 
190  T valueAndDerivatives(const Values& values, const KeyVector& keys,
191  const FastVector<int>& dims, std::vector<Matrix>& H) const;
192 
194  T traceExecution(const Values& values, internal::ExecutionTrace<T>& trace,
195  void* traceStorage) const;
196 
198  T valueAndJacobianMap(const Values& values,
199  internal::JacobianMap& jacobians) const;
200 
201  // be very selective on who can access these private methods:
202  friend class ExpressionFactor<T> ;
203  friend class internal::ExpressionNode<T>;
204 
205  // and add tests
206  friend class ::ExpressionFactorShallowTest;
207 };
208 
213 template <typename T>
215  // Check that T is a vector space
216  BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
217 
218  public:
219  explicit ScalarMultiplyExpression(double s, const Expression<T>& e);
220 };
221 
226 template <typename T>
227 class BinarySumExpression : public Expression<T> {
228  // Check that T is a vector space
229  BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
230 
231  public:
232  explicit BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2);
233 };
234 
235 
241 template <typename T, typename A>
243  const boost::function<T(A)>& f, const Expression<A>& expression,
244  const Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension>& dTdA) {
245  // Use lambda to endow f with a linear Jacobian
246  typename Expression<T>::template UnaryFunction<A>::type g =
247  [=](const A& value, typename MakeOptionalJacobian<T, A>::type H) {
248  if (H)
249  *H << dTdA;
250  return f(value);
251  };
252  return Expression<T>(g, expression);
253 }
254 
261 template <typename T>
263  return ScalarMultiplyExpression<T>(s, e);
264 }
265 
272 template <typename T>
274  return BinarySumExpression<T>(e1, e2);
275 }
276 
278 template <typename T>
280  // TODO(frank, abe): Implement an actual negate operator instead of multiplying by -1
281  return e1 + (-1.0) * e2;
282 }
283 
289 template<typename T>
290 Expression<T> operator*(const Expression<T>& e1, const Expression<T>& e2);
291 
297 template<typename T>
298 std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start = 0);
299 
300 } // namespace gtsam
301 
303 
gtsam::Expression::dims
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
Definition: Expression-inl.h:122
Expression-inl.h
Internals for Expression.h, not for general consumption.
gtsam::ScalarMultiplyExpression
A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar It optimiz...
Definition: Expression.h:214
gtsam::Expression::print
void print(const std::string &s) const
Print.
Definition: Expression-inl.h:127
gtsam::Expression::UnaryFunction
Definition: Expression.h:71
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::internal::ExpressionNode
Definition: Expression.h:42
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::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::Expression::TernaryFunction
Definition: Expression.h:84
gtsam::Expression::valueAndJacobianMap
T valueAndJacobianMap(const Values &values, internal::JacobianMap &jacobians) const
brief Return value and derivatives, reverse AD version
Definition: Expression-inl.h:190
gtsam::internal::ExecutionTrace
Definition: Expression.h:41
gtsam::Expression::~Expression
virtual ~Expression()
Destructor.
Definition: Expression.h:142
gtsam::Expression::operator+=
Expression< T > & operator+=(const Expression< T > &e)
Add another expression to this expression.
Definition: Expression-inl.h:274
gtsam::linearExpression
Expression< T > linearExpression(const boost::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:242
gtsam::createUnknowns
std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
Construct an array of leaves.
Definition: Expression-inl.h:256
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
OptionalJacobian.h
Special class for optional Jacobian arguments.
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:159
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:130
gtsam::Expression::Expression
Expression()
Default constructor, for serialization.
Definition: Expression.h:183
gtsam::Expression::traceSize
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
Definition: Expression-inl.h:152
gtsam::IsVectorSpace
Vector Space concept.
Definition: VectorSpace.h:470
gtsam::operator-
BinarySumExpression< T > operator-(const Expression< T > &e1, const Expression< T > &e2)
Construct an expression that subtracts one expression from another.
Definition: Expression.h:279
gtsam::BinarySumExpression
A BinarySumExpression is a specialization of Expression that adds two expressions together It optimiz...
Definition: Expression.h:227
gtsam::Expression::traceExecution
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
trace execution, very unsafe
Definition: Expression-inl.h:183
gtsam::Expression::KeysAndDims
std::pair< KeyVector, FastVector< int > > KeysAndDims
Keys and dimensions in same order.
Definition: Expression.h:186
gtsam::Expression::Expression
Expression(const boost::shared_ptr< internal::ExpressionNode< T > > &root)
Construct with a custom root.
Definition: Expression.h:62
gtsam::Expression
Expression class that supports automatic differentiation.
Definition: Expression.h:49
gtsam::Expression::keys
std::set< Key > keys() const
Return keys that play in this expression.
Definition: Expression-inl.h:117
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:132
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:45
gtsam::Expression::BinaryFunction
Definition: Expression.h:77
Symbol.h
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::Expression::root
const boost::shared_ptr< internal::ExpressionNode< T > > & root() const
Return root.
Definition: Expression-inl.h:147
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition: ExpressionFactor.h:44
gtsam::Expression::clone
virtual boost::shared_ptr< Expression > clone() const
Definition: Expression.h:167
gtsam::Expression::type
Expression< T > type
Define type so we can apply it as a meta-function.
Definition: Expression.h:54
gtsam::operator+
BinarySumExpression< T > operator+(const Expression< T > &e1, const Expression< T > &e2)
Construct an expression that sums two input expressions of the same type T The type T must be a vecto...
Definition: Expression.h:273
gtsam::Symbol
Character and index key used to refer to variables.
Definition: Symbol.h:33