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();
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
gtsam::internal::apply_compose::result_type
T result_type
Definition: Expression-inl.h:247
H
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
Definition: gnuplot_common_settings.hh:74
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
gtsam::internal::ExecutionTrace
Definition: Expression.h:39
gtsam::internal::apply_compose
Definition: Expression-inl.h:246
A3
static const double A3[]
Definition: expn.h:8
gtsam.examples.DogLegOptimizerExample.type
type
Definition: DogLegOptimizerExample.py:111
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::BinarySumExpression::BinarySumExpression
BinarySumExpression(const Expression< T > &e1, const Expression< T > &e2)
Definition: Expression-inl.h:297
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
gtsam::internal::TraceAlignment
static const unsigned TraceAlignment
Definition: ExecutionTrace.h:45
x
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
Definition: gnuplot_common_settings.hh:12
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
gtsam::Expression
Definition: Expression.h:47
n
int n
Definition: BiCGSTAB_simple.cpp:1
A
Definition: test_numpy_dtypes.cpp:298
gtsam::internal::ExecutionTrace::startReverseAD1
void startReverseAD1(JacobianMap &jacobians) const
Definition: ExecutionTrace.h:157
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
gtsam::Expression::KeysAndDims
std::pair< KeyVector, FastVector< int > > KeysAndDims
Keys and dimensions in same order.
Definition: Expression.h:191
gtsam::ScalarMultiplyExpression
Definition: Expression.h:219
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::allocAligned
std::unique_ptr< internal::ExecutionTraceStorage[]> allocAligned(size_t size)
Definition: Expression-inl.h:197
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > make_shared(Args &&... args)
Definition: make_shared.h:56
gtsam::internal::ExecutionTraceStorage
std::aligned_storage< 1, TraceAlignment >::type ExecutionTraceStorage
Definition: ExecutionTrace.h:48
gtsam::internal::JacobianMap
Definition: JacobianMap.h:32
ExpressionNode.h
ExpressionNode class.
A2
static const double A2[]
Definition: expn.h:7
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Definition: inference/Symbol.h:139
gtsam::createUnknowns
std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
Construct an array of leaves.
Definition: Expression-inl.h:283
gtsam::Expression::Expression
Expression()
Default constructor, for serialization.
Definition: Expression.h:188
Eigen::Triplet< double >
gtsam::internal::apply_compose< double >::operator()
double operator()(const double &x, const double &y, OptionalJacobian< 1, 1 > H1={}, OptionalJacobian< 1, 1 > H2={}) const
Definition: Expression-inl.h:257
gtsam::internal::apply_compose::operator()
T operator()(const T &x, const T &y, OptionalJacobian< Dim, Dim > H1={}, OptionalJacobian< Dim, Dim > H2={}) const
Definition: Expression-inl.h:249
anyset::size
size_t size() const
Definition: pytypes.h:2220
gtsam::Expression::dims
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
Definition: Expression-inl.h:132
y
Scalar * y
Definition: level1_cplx_impl.h:124
key
const gtsam::Symbol key('X', 0)
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Values
Definition: Values.h:65
gtsam::Expression::traceSize
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
Definition: Expression-inl.h:160
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
std
Definition: BFloat16.h:88
A1
static const double A1[]
Definition: expn.h:6
exampleQR::Ab
Matrix Ab
Definition: testNoiseModel.cpp:207
function
Definition: pytypes.h:2252
gtsam::Expression::traceExecution
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, char *traceStorage) const
trace execution, very unsafe
Definition: Expression-inl.h:191
uint64_t
unsigned __int64 uint64_t
Definition: ms_stdint.h:95
internal
Definition: BandTriangularSolver.h:13
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::Expression::operator+=
Expression< T > & operator+=(const Expression< T > &e)
Add another expression to this expression.
Definition: Expression-inl.h:301
test_callbacks.value
value
Definition: test_callbacks.py:160
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Eigen::UniformScaling::operator*
operator*(const MatrixBase< Derived > &matrix, const UniformScaling< Scalar > &s)
Definition: Eigen/src/Geometry/Scaling.h:135
gtsam::Symbol
Definition: inference/Symbol.h:37


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:30