gtsam  4.1.0
gtsam
Expression-inl.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/ExpressionNode.h>
23 
24 #include <boost/tuple/tuple.hpp>
25 #include <boost/range/adaptor/map.hpp>
26 #include <boost/range/algorithm.hpp>
27 
28 namespace gtsam {
29 
30 template<typename T>
31 Expression<T>::Expression(const T& value) :
32  root_(new internal::ConstantExpression<T>(value)) {
33 }
34 
35 template<typename T>
37  root_(new internal::LeafExpression<T>(key)) {
38 }
39 
40 template<typename T>
42  root_(new internal::LeafExpression<T>(symbol)) {
43 }
44 
45 template<typename T>
46 Expression<T>::Expression(unsigned char c, std::uint64_t j) :
47  root_(new internal::LeafExpression<T>(Symbol(c, j))) {
48 }
49 
51 template<typename T>
52 template<typename A>
53 Expression<T>::Expression(typename UnaryFunction<A>::type function,
54  const Expression<A>& expression) :
55  root_(new internal::UnaryExpression<T, A>(function, expression)) {
56 }
57 
59 template<typename T>
60 template<typename A1, typename A2>
61 Expression<T>::Expression(typename BinaryFunction<A1, A2>::type function,
62  const Expression<A1>& expression1, const Expression<A2>& expression2) :
63  root_(
64  new internal::BinaryExpression<T, A1, A2>(function, expression1,
65  expression2)) {
66 }
67 
69 template<typename T>
70 template<typename A1, typename A2, typename A3>
71 Expression<T>::Expression(typename TernaryFunction<A1, A2, A3>::type function,
72  const Expression<A1>& expression1, const Expression<A2>& expression2,
73  const Expression<A3>& expression3) :
74  root_(
75  new internal::TernaryExpression<T, A1, A2, A3>(function, expression1,
76  expression2, expression3)) {
77 }
78 
80 template<typename T>
81 template<typename A>
83  T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
84  root_(
85  new internal::UnaryExpression<T, A>(boost::bind(method, _1, _2),
86  expression)) {
87 }
88 
90 template<typename T>
91 template<typename A1, typename A2>
93  T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
94  typename MakeOptionalJacobian<T, A2>::type) const,
95  const Expression<A2>& expression2) :
96  root_(
97  new internal::BinaryExpression<T, A1, A2>(
98  boost::bind(method, _1, _2, _3, _4), expression1, expression2)) {
99 }
100 
102 template<typename T>
103 template<typename A1, typename A2, typename A3>
105  T (A1::*method)(const A2&, const A3&,
108  typename MakeOptionalJacobian<T, A3>::type) const,
109  const Expression<A2>& expression2, const Expression<A3>& expression3) :
110  root_(
111  new internal::TernaryExpression<T, A1, A2, A3>(
112  boost::bind(method, _1, _2, _3, _4, _5, _6), expression1,
113  expression2, expression3)) {
114 }
115 
116 template<typename T>
117 std::set<Key> Expression<T>::keys() const {
118  return root_->keys();
119 }
120 
121 template<typename T>
122 void Expression<T>::dims(std::map<Key, int>& map) const {
123  root_->dims(map);
124 }
125 
126 template<typename T>
127 void Expression<T>::print(const std::string& s) const {
128  root_->print(s);
129 }
130 
131 template<typename T>
132 T Expression<T>::value(const Values& values,
133  boost::optional<std::vector<Matrix>&> H) const {
134 
135  if (H) {
136  // Call private version that returns derivatives in H
137  KeyVector keys;
138  FastVector<int> dims;
139  boost::tie(keys, dims) = keysAndDims();
140  return valueAndDerivatives(values, keys, dims, *H);
141  } else
142  // no derivatives needed, just return value
143  return root_->value(values);
144 }
145 
146 template<typename T>
147 const boost::shared_ptr<internal::ExpressionNode<T> >& Expression<T>::root() const {
148  return root_;
149 }
150 
151 template<typename T>
152 size_t Expression<T>::traceSize() const {
153  return root_->traceSize();
154 }
155 
156 // Private methods:
157 
158 template<typename T>
160  const KeyVector& keys, const FastVector<int>& dims,
161  std::vector<Matrix>& H) const {
162 
163  // H should be pre-allocated
164  assert(H.size()==keys.size());
165 
166  // Pre-allocate and zero VerticalBlockMatrix
167  static const int Dim = traits<T>::dimension;
168  VerticalBlockMatrix Ab(dims, Dim);
169  Ab.matrix().setZero();
170  internal::JacobianMap jacobianMap(keys, Ab);
171 
172  // Call unsafe version
173  T result = valueAndJacobianMap(values, jacobianMap);
174 
175  // Copy blocks into the vector of jacobians passed in
176  for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
177  H[i] = Ab(i);
178 
179  return result;
180 }
181 
182 template<typename T>
184  internal::ExecutionTrace<T>& trace, void* traceStorage) const {
185  return root_->traceExecution(values, trace,
186  static_cast<internal::ExecutionTraceStorage*>(traceStorage));
187 }
188 
189 template<typename T>
191  internal::JacobianMap& jacobians) const {
192  // The following piece of code is absolutely crucial for performance.
193  // We allocate a block of memory on the stack, which can be done at runtime
194  // with modern C++ compilers. The traceExecution then fills this memory
195  // with an execution trace, made up entirely of "Record" structs, see
196  // the FunctionalNode class in expression-inl.h
197  size_t size = traceSize();
198 
199  // Windows does not support variable length arrays, so memory must be dynamically
200  // allocated on Visual Studio. For more information see the issue below
201  // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
202 #ifdef _MSC_VER
203  auto traceStorage = static_cast<internal::ExecutionTraceStorage*>(_aligned_malloc(size, internal::TraceAlignment));
204 #else
205  internal::ExecutionTraceStorage traceStorage[size];
206 #endif
207 
209  T value(this->traceExecution(values, trace, traceStorage));
210  trace.startReverseAD1(jacobians);
211 
212 #ifdef _MSC_VER
213  _aligned_free(traceStorage);
214 #endif
215 
216  return value;
217 }
218 
219 template<typename T>
221  std::map<Key, int> map;
222  dims(map);
223  size_t n = map.size();
224  KeysAndDims pair = std::make_pair(KeyVector(n), FastVector<int>(n));
225  boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
226  boost::copy(map | boost::adaptors::map_values, pair.second.begin());
227  return pair;
228 }
229 
230 namespace internal {
231 // http://stackoverflow.com/questions/16260445/boost-bind-to-operator
232 template<class T>
234  typedef T result_type;
235  static const int Dim = traits<T>::dimension;
236  T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
237  boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
238  return x.compose(y, H1, H2);
239  }
240 };
241 }
242 
243 // Global methods:
244 
246 template<typename T>
248  const Expression<T>& expression2) {
249  return Expression<T>(
250  boost::bind(internal::apply_compose<T>(), _1, _2, _3, _4), expression1,
251  expression2);
252 }
253 
255 template<typename T>
256 std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start) {
257  std::vector<Expression<T> > unknowns;
258  unknowns.reserve(n);
259  for (size_t i = start; i < start + n; i++)
260  unknowns.push_back(Expression<T>(c, i));
261  return unknowns;
262 }
263 
264 template <typename T>
265 ScalarMultiplyExpression<T>::ScalarMultiplyExpression(double s, const Expression<T>& e)
266  : Expression<T>(boost::make_shared<internal::ScalarMultiplyNode<T>>(s, e)) {}
267 
268 
269 template <typename T>
270 BinarySumExpression<T>::BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2)
271  : Expression<T>(boost::make_shared<internal::BinarySumNode<T>>(e1, e2)) {}
272 
273 template <typename T>
275  root_ = boost::make_shared<internal::BinarySumNode<T>>(*this, e);
276  return *this;
277 }
278 
279 } // namespace gtsam
gtsam::Expression::dims
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
Definition: Expression-inl.h:122
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::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::internal::ExecutionTrace
Definition: Expression.h:41
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
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
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::internal::apply_compose
Definition: Expression-inl.h:233
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::Expression::traceExecution
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
trace execution, very unsafe
Definition: Expression-inl.h:183
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition: VerticalBlockMatrix.h:187
gtsam::Expression::KeysAndDims
std::pair< KeyVector, FastVector< int > > KeysAndDims
Keys and dimensions in same order.
Definition: Expression.h:186
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::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::operator*
Expression< T > operator*(const Expression< T > &expression1, const Expression< T > &expression2)
Construct a product expression, assumes T::compose(T) -> T.
Definition: Expression-inl.h:247
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
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::Symbol
Character and index key used to refer to variables.
Definition: Symbol.h:33