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 
23 
24 #include <map>
25 #include <memory>
26 #include <set>
27 #include <string>
28 #include <vector>
29 
30 
31 namespace gtsam {
32 
33 template<typename T>
35  root_(new internal::ConstantExpression<T>(value)) {
36 }
37 
38 template<typename T>
40  root_(new internal::LeafExpression<T>(key)) {
41 }
42 
43 template<typename T>
45  root_(new internal::LeafExpression<T>(symbol)) {
46 }
47 
48 template<typename T>
50  root_(new internal::LeafExpression<T>(Symbol(c, j))) {
51 }
52 
54 template<typename T>
55 template<typename A>
57  const Expression<A>& expression) :
58  root_(new internal::UnaryExpression<T, A>(function, expression)) {
59 }
60 
62 template<typename T>
63 template<typename A1, typename A2>
65  const Expression<A1>& expression1, const Expression<A2>& expression2) :
66  root_(
67  new internal::BinaryExpression<T, A1, A2>(function, expression1,
68  expression2)) {
69 }
70 
72 template<typename T>
73 template<typename A1, typename A2, typename A3>
75  const Expression<A1>& expression1, const Expression<A2>& expression2,
76  const Expression<A3>& expression3) :
77  root_(
78  new internal::TernaryExpression<T, A1, A2, A3>(function, expression1,
79  expression2, expression3)) {
80 }
81 
83 template<typename T>
84 template<typename A>
86  T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
87  root_(
88  new internal::UnaryExpression<T, A>(std::bind(method,
89  std::placeholders::_1, std::placeholders::_2),
90  expression)) {
91 }
92 
94 template<typename T>
95 template<typename A1, typename A2>
97  T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
98  typename MakeOptionalJacobian<T, A2>::type) const,
99  const Expression<A2>& expression2) :
100  root_(
101  new internal::BinaryExpression<T, A1, A2>(
102  std::bind(method, std::placeholders::_1,
103  std::placeholders::_2, std::placeholders::_3,
104  std::placeholders::_4),
105  expression1, expression2)) {
106 }
107 
109 template<typename T>
110 template<typename A1, typename A2, typename A3>
112  T (A1::*method)(const A2&, const A3&,
115  typename MakeOptionalJacobian<T, A3>::type) const,
116  const Expression<A2>& expression2, const Expression<A3>& expression3) :
117  root_(
118  new internal::TernaryExpression<T, A1, A2, A3>(
119  std::bind(method, std::placeholders::_1,
120  std::placeholders::_2, std::placeholders::_3,
121  std::placeholders::_4, std::placeholders::_5,
122  std::placeholders::_6),
123  expression1, expression2, expression3)) {
124 }
125 
126 template<typename T>
127 std::set<Key> Expression<T>::keys() const {
128  return root_->keys();
129 }
130 
131 template<typename T>
132 void Expression<T>::dims(std::map<Key, int>& map) const {
133  root_->dims(map);
134 }
135 
136 template<typename T>
137 void Expression<T>::print(const std::string& s) const {
138  root_->print(s);
139 }
140 
141 template<typename T>
143  std::vector<Matrix>* H) const {
144  if (H) {
145  // Call private version that returns derivatives in H
146  const auto [keys, dims] = keysAndDims();
147  return valueAndDerivatives(values, keys, dims, *H);
148  } else {
149  // no derivatives needed, just return value
150  return root_->value(values);
151  }
152 }
153 
154 template<typename T>
155 const std::shared_ptr<internal::ExpressionNode<T> >& Expression<T>::root() const {
156  return root_;
157 }
158 
159 template<typename T>
160 size_t Expression<T>::traceSize() const {
161  return root_->traceSize();
162 }
163 
164 // Private methods:
165 
166 template<typename T>
168  const KeyVector& keys, const FastVector<int>& dims,
169  std::vector<Matrix>& H) const {
170 
171  // H should be pre-allocated
172  assert(H.size()==keys.size());
173 
174  // Pre-allocate and zero VerticalBlockMatrix
175  static const int Dim = traits<T>::dimension;
176  VerticalBlockMatrix Ab(dims, Dim);
177  Ab.matrix().setZero();
178  internal::JacobianMap jacobianMap(keys, Ab);
179 
180  // Call unsafe version
181  T result = valueAndJacobianMap(values, jacobianMap);
182 
183  // Copy blocks into the vector of jacobians passed in
184  for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
185  H[i] = Ab(i);
186 
187  return result;
188 }
189 
190 template<typename T>
192  internal::ExecutionTrace<T>& trace, char* traceStorage) const {
193  return root_->traceExecution(values, trace, traceStorage);
194 }
195 
196 // Allocate a single block of aligned memory using a unique_ptr.
197 inline std::unique_ptr<internal::ExecutionTraceStorage[]> allocAligned(size_t size) {
198  const size_t alignedSize = (size + internal::TraceAlignment - 1) / internal::TraceAlignment;
199  return std::unique_ptr<internal::ExecutionTraceStorage[]>(
200  new internal::ExecutionTraceStorage[alignedSize]);
201 }
202 
203 template<typename T>
205  internal::JacobianMap& jacobians) const {
206  try {
207  // We allocate a single block of aligned memory using a unique_ptr.
208  const size_t size = traceSize();
209  auto traceStorage = allocAligned(size);
210 
211  // The traceExecution call then fills this memory
212  // with an execution trace, made up entirely of "Record" structs, see
213  // the FunctionalNode class in expression-inl.h
215  T value(this->traceExecution(values, trace, reinterpret_cast<char *>(traceStorage.get())));
216 
217  // We then calculate the Jacobians using reverse automatic differentiation (AD).
218  trace.startReverseAD1(jacobians);
219  return value;
220  } catch (const std::bad_alloc &e) {
221  std::cerr << "valueAndJacobianMap exception: " << e.what() << '\n';
222  throw e;
223  }
224  // Here traceStorage will be de-allocated properly.
225 }
226 
227 template<typename T>
229  std::map<Key, int> map;
230  dims(map);
231  size_t n = map.size();
232  KeysAndDims pair = {KeyVector(n), FastVector<int>(n)};
233  // Copy map into pair of vectors
234  auto key_it = pair.first.begin();
235  auto dim_it = pair.second.begin();
236  for (const auto& [key, value] : map) {
237  *key_it++ = key;
238  *dim_it++ = value;
239  }
240  return pair;
241 }
242 
243 namespace internal {
244 // http://stackoverflow.com/questions/16260445/boost-bind-to-operator
245 template<class T>
247  typedef T result_type;
248  static const int Dim = traits<T>::dimension;
249  T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
250  {}, OptionalJacobian<Dim, Dim> H2 = {}) const {
251  return x.compose(y, H1, H2);
252  }
253 };
254 
255 template <>
256 struct apply_compose<double> {
257  double operator()(const double& x, const double& y,
258  OptionalJacobian<1, 1> H1 = {},
259  OptionalJacobian<1, 1> H2 = {}) const {
260  if (H1) H1->setConstant(y);
261  if (H2) H2->setConstant(x);
262  return x * y;
263  }
264 };
265 
266 } // namespace internal
267 
268 // Global methods:
269 
271 template<typename T>
273  const Expression<T>& expression2) {
274  return Expression<T>(
275  std::bind(internal::apply_compose<T>(), std::placeholders::_1,
276  std::placeholders::_2, std::placeholders::_3,
277  std::placeholders::_4),
278  expression1, expression2);
279 }
280 
282 template<typename T>
283 std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start) {
284  std::vector<Expression<T> > unknowns;
285  unknowns.reserve(n);
286  for (size_t i = start; i < start + n; i++)
287  unknowns.push_back(Expression<T>(c, i));
288  return unknowns;
289 }
290 
291 template <typename T>
293  : Expression<T>(std::make_shared<internal::ScalarMultiplyNode<T>>(s, e)) {}
294 
295 
296 template <typename T>
298  : Expression<T>(std::make_shared<internal::BinarySumNode<T>>(e1, e2)) {}
299 
300 template <typename T>
302  root_ = std::make_shared<internal::BinarySumNode<T>>(*this, e);
303  return *this;
304 }
305 
306 } // namespace gtsam
const gtsam::Symbol key('X', 0)
std::pair< KeyVector, FastVector< int > > KeysAndDims
Keys and dimensions in same order.
Definition: Expression.h:191
Vector3_ operator*(const Double_ &s, const Vector3_ &v)
static Matrix A1
Scalar * y
std::shared_ptr< internal::ExpressionNode< T > > root_
Definition: Expression.h:57
Expression< T > & operator+=(const Expression< T > &e)
Add another expression to this expression.
T operator()(const T &x, const T &y, OptionalJacobian< Dim, Dim > H1={}, OptionalJacobian< Dim, Dim > H2={}) const
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
int n
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
leaf::MyValues values
const Matrix & matrix() const
Definition: BFloat16.h:88
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
std::unique_ptr< internal::ExecutionTraceStorage[]> allocAligned(size_t size)
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
Construct an array of leaves.
Values result
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > make_shared(Args &&... args)
Definition: make_shared.h:56
void startReverseAD1(JacobianMap &jacobians) const
double operator()(const double &x, const double &y, OptionalJacobian< 1, 1 > H1={}, OptionalJacobian< 1, 1 > H2={}) const
unsigned __int64 uint64_t
Definition: ms_stdint.h:95
ExpressionNode class.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Key symbol(unsigned char c, std::uint64_t j)
std::aligned_storage< 1, TraceAlignment >::type ExecutionTraceStorage
traits
Definition: chartTesting.h:28
BinarySumExpression(const Expression< T > &e1, const Expression< T > &e2)
Expression()
Default constructor, for serialization.
Definition: Expression.h:188
static const unsigned TraceAlignment
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::ptrdiff_t j
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, char *traceStorage) const
trace execution, very unsafe
size_t traceSize() const
Return size needed for memory buffer in traceExecution.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:13