gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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// The MSVC compiler workaround for the unsupported variable length array
23// utilizes the std::unique_ptr<> custom deleter.
24// See Expression<T>::valueAndJacobianMap() below.
25#ifdef _MSC_VER
26#include <memory>
27#endif
28
29#include <gtsam/nonlinear/internal/ExpressionNode.h>
30
31#include <boost/bind/bind.hpp>
32#include <boost/tuple/tuple.hpp>
33#include <boost/range/adaptor/map.hpp>
34#include <boost/range/algorithm.hpp>
35
36namespace gtsam {
37
38template<typename T>
40 root_(new internal::ConstantExpression<T>(value)) {
41}
42
43template<typename T>
45 root_(new internal::LeafExpression<T>(key)) {
46}
47
48template<typename T>
50 root_(new internal::LeafExpression<T>(symbol)) {
51}
52
53template<typename T>
54Expression<T>::Expression(unsigned char c, std::uint64_t j) :
55 root_(new internal::LeafExpression<T>(Symbol(c, j))) {
56}
57
59template<typename T>
60template<typename A>
61Expression<T>::Expression(typename UnaryFunction<A>::type function,
62 const Expression<A>& expression) :
63 root_(new internal::UnaryExpression<T, A>(function, expression)) {
64}
65
67template<typename T>
68template<typename A1, typename A2>
69Expression<T>::Expression(typename BinaryFunction<A1, A2>::type function,
70 const Expression<A1>& expression1, const Expression<A2>& expression2) :
71 root_(
72 new internal::BinaryExpression<T, A1, A2>(function, expression1,
73 expression2)) {
74}
75
77template<typename T>
78template<typename A1, typename A2, typename A3>
79Expression<T>::Expression(typename TernaryFunction<A1, A2, A3>::type function,
80 const Expression<A1>& expression1, const Expression<A2>& expression2,
81 const Expression<A3>& expression3) :
82 root_(
83 new internal::TernaryExpression<T, A1, A2, A3>(function, expression1,
84 expression2, expression3)) {
85}
86
88template<typename T>
89template<typename A>
91 T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
92 root_(
93 new internal::UnaryExpression<T, A>(std::bind(method,
94 std::placeholders::_1, std::placeholders::_2),
95 expression)) {
96}
97
99template<typename T>
100template<typename A1, typename A2>
102 T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
103 typename MakeOptionalJacobian<T, A2>::type) const,
104 const Expression<A2>& expression2) :
105 root_(
106 new internal::BinaryExpression<T, A1, A2>(
107 std::bind(method, std::placeholders::_1,
108 std::placeholders::_2, std::placeholders::_3,
109 std::placeholders::_4),
110 expression1, expression2)) {
111}
112
114template<typename T>
115template<typename A1, typename A2, typename A3>
117 T (A1::*method)(const A2&, const A3&,
118 typename MakeOptionalJacobian<T, A1>::type,
119 typename MakeOptionalJacobian<T, A2>::type,
120 typename MakeOptionalJacobian<T, A3>::type) const,
121 const Expression<A2>& expression2, const Expression<A3>& expression3) :
122 root_(
123 new internal::TernaryExpression<T, A1, A2, A3>(
124 std::bind(method, std::placeholders::_1,
125 std::placeholders::_2, std::placeholders::_3,
126 std::placeholders::_4, std::placeholders::_5,
127 std::placeholders::_6),
128 expression1, expression2, expression3)) {
129}
130
131template<typename T>
132std::set<Key> Expression<T>::keys() const {
133 return root_->keys();
134}
135
136template<typename T>
137void Expression<T>::dims(std::map<Key, int>& map) const {
138 root_->dims(map);
139}
140
141template<typename T>
142void Expression<T>::print(const std::string& s) const {
143 root_->print(s);
144}
146template<typename T>
148 boost::optional<std::vector<Matrix>&> H) const {
149
150 if (H) {
151 // Call private version that returns derivatives in H
152 KeyVector keys;
153 FastVector<int> dims;
154 boost::tie(keys, dims) = keysAndDims();
155 return valueAndDerivatives(values, keys, dims, *H);
156 } else
157 // no derivatives needed, just return value
158 return root_->value(values);
159}
160
161template<typename T>
162const boost::shared_ptr<internal::ExpressionNode<T> >& Expression<T>::root() const {
163 return root_;
164}
165
166template<typename T>
168 return root_->traceSize();
169}
170
171// Private methods:
172
173template<typename T>
175 const KeyVector& keys, const FastVector<int>& dims,
176 std::vector<Matrix>& H) const {
178 // H should be pre-allocated
179 assert(H.size()==keys.size());
180
181 // Pre-allocate and zero VerticalBlockMatrix
182 static const int Dim = traits<T>::dimension;
183 VerticalBlockMatrix Ab(dims, Dim);
184 Ab.matrix().setZero();
185 internal::JacobianMap jacobianMap(keys, Ab);
186
187 // Call unsafe version
188 T result = valueAndJacobianMap(values, jacobianMap);
190 // Copy blocks into the vector of jacobians passed in
191 for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
192 H[i] = Ab(i);
194 return result;
195}
196
197template<typename T>
199 internal::ExecutionTrace<T>& trace, void* traceStorage) const {
200 return root_->traceExecution(values, trace,
201 static_cast<internal::ExecutionTraceStorage*>(traceStorage));
202}
203
204template<typename T>
206 internal::JacobianMap& jacobians) const {
207 // The following piece of code is absolutely crucial for performance.
208 // We allocate a block of memory on the stack, which can be done at runtime
209 // with modern C++ compilers. The traceExecution then fills this memory
210 // with an execution trace, made up entirely of "Record" structs, see
211 // the FunctionalNode class in expression-inl.h
212 size_t size = traceSize();
213
214 // Windows does not support variable length arrays, so memory must be dynamically
215 // allocated on Visual Studio. For more information see the issue below
216 // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
217#ifdef _MSC_VER
218 std::unique_ptr<void, void(*)(void*)> traceStorageDeleter(
219 _aligned_malloc(size, internal::TraceAlignment),
220 [](void *ptr){ _aligned_free(ptr); });
221 auto traceStorage = static_cast<internal::ExecutionTraceStorage*>(traceStorageDeleter.get());
222#else
223 internal::ExecutionTraceStorage traceStorage[size];
224#endif
225
227 T value(this->traceExecution(values, trace, traceStorage));
228 trace.startReverseAD1(jacobians);
229
230 return value;
231}
232
233template<typename T>
235 std::map<Key, int> map;
236 dims(map);
237 size_t n = map.size();
238 KeysAndDims pair = std::make_pair(KeyVector(n), FastVector<int>(n));
239 boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
240 boost::copy(map | boost::adaptors::map_values, pair.second.begin());
241 return pair;
242}
243
244namespace internal {
245// http://stackoverflow.com/questions/16260445/boost-bind-to-operator
246template<class T>
248 typedef T result_type;
249 static const int Dim = traits<T>::dimension;
250 T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
251 boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
252 return x.compose(y, H1, H2);
253 }
254};
255
256template <>
257struct apply_compose<double> {
258 double operator()(const double& x, const double& y,
259 OptionalJacobian<1, 1> H1 = boost::none,
260 OptionalJacobian<1, 1> H2 = boost::none) const {
261 if (H1) H1->setConstant(y);
262 if (H2) H2->setConstant(x);
263 return x * y;
264 }
265};
266
267}
268
269// Global methods:
270
272template<typename T>
274 const Expression<T>& expression2) {
275 return Expression<T>(
276 std::bind(internal::apply_compose<T>(), std::placeholders::_1,
277 std::placeholders::_2, std::placeholders::_3,
278 std::placeholders::_4),
279 expression1, expression2);
280}
281
283template<typename T>
284std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start) {
285 std::vector<Expression<T> > unknowns;
286 unknowns.reserve(n);
287 for (size_t i = start; i < start + n; i++)
288 unknowns.push_back(Expression<T>(c, i));
289 return unknowns;
290}
291
292template <typename T>
293ScalarMultiplyExpression<T>::ScalarMultiplyExpression(double s, const Expression<T>& e)
294 : Expression<T>(boost::make_shared<internal::ScalarMultiplyNode<T>>(s, e)) {}
295
296
297template <typename T>
298BinarySumExpression<T>::BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2)
299 : Expression<T>(boost::make_shared<internal::BinarySumNode<T>>(e1, e2)) {}
300
301template <typename T>
303 root_ = boost::make_shared<internal::BinarySumNode<T>>(*this, e);
304 return *this;
305}
306
307} // namespace gtsam
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:34
Global functions in a separate testing namespace.
Definition chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
Key symbol(unsigned char c, std::uint64_t j)
Create a symbol key from a character and index, i.e.
Definition Symbol.h:135
std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
Construct an array of leaves.
Definition Expression-inl.h:284
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
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
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition OptionalJacobian.h:261
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
Character and index key used to refer to variables.
Definition Symbol.h:35
Definition Expression-inl.h:247
Definition Expression.h:40
Expression class that supports automatic differentiation.
Definition Expression.h:48
Expression()
Default constructor, for serialization.
Definition Expression.h:182
std::set< Key > keys() const
Return keys that play in this expression.
Definition Expression-inl.h:132
std::pair< KeyVector, FastVector< int > > KeysAndDims
Keys and dimensions in same order.
Definition Expression.h:185
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
Definition Expression-inl.h:137
void print(const std::string &s) const
Print.
Definition Expression-inl.h:142
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
Definition Expression-inl.h:167
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
trace execution, very unsafe
Definition Expression-inl.h:198
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65