ExecutionTrace.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 
19 #pragma once
20 
21 #include <gtsam/config.h> // Configuration from CMake
23 #include <gtsam/inference/Key.h>
24 #include <gtsam/base/Manifold.h>
25 
26 #include <Eigen/Core>
27 
28 #include <iostream>
29 #include <optional>
30 #include <string>
31 #include <type_traits>
32 
33 namespace gtsam {
34 namespace internal {
35 
36 template<int T> struct CallRecord;
37 
41 #ifdef _MSC_VER
42 // TODO(dellaert): this might lead to trouble if Eigen decides to use 32 on Windows.
43 static const unsigned TraceAlignment = 16; // 16 bytes max_align on Windows
44 #else
45 static const unsigned TraceAlignment = 32; // Alignment used by Eigen on some platforms.
46 #endif
47 // TODO(dellaert): we *should* be able to simplify the code using the pointer arithmetic from ExecutionTraceStorage.
49 
50 template<bool UseBlock, typename Derived>
51 struct UseBlockIf {
52  static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
53  JacobianMap& jacobians, Key key) {
54  // block makes HUGE difference
55  jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(
56  0, 0) += dTdA;
57  }
58 };
59 
61 template<typename Derived>
62 struct UseBlockIf<false, Derived> {
63  static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
64  JacobianMap& jacobians, Key key) {
65  jacobians(key) += dTdA;
66  }
67 };
68 
70 template<typename Derived>
72  JacobianMap& jacobians, Key key) {
73  UseBlockIf<
74  Derived::RowsAtCompileTime != Eigen::Dynamic
75  && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian(
76  dTdA, jacobians, key);
77 }
78 
100 template<class T>
101 class ExecutionTrace {
102  static const int Dim = traits<T>::dimension;
104  enum {
106  } kind;
107  union {
110  } content;
111 
112  public:
113 
116  kind(Constant) {
117  }
118 
120  void setLeaf(Key key) {
121  kind = Leaf;
122  content.key = key;
123  }
124 
127  kind = Function;
128  content.ptr = record;
129  }
130 
132  void print(const std::string& indent = "") const {
133  if (kind == Constant) {
134  std::cout << indent << "Constant" << std::endl;
135  } else if (kind == Leaf) {
136  std::cout << indent << "Leaf, key = " << content.key << std::endl;
137  } else if (kind == Function) {
138  content.ptr->print(indent + " ");
139  }
140  }
141 
143  template<class Record>
144  std::optional<Record*> record() {
145  if (kind != Function) {
146  return {};
147  } else {
148  Record* p = dynamic_cast<Record*>(content.ptr);
149  return p ? std::optional<Record*>(p) : std::nullopt;
150  }
151  }
152 
157  void startReverseAD1(JacobianMap& jacobians) const {
158  if (kind == Leaf) {
159  // This branch will only be called on trivial Leaf expressions, i.e. Priors
160  static const JacobianTT I = JacobianTT::Identity();
161  handleLeafCase(I, jacobians, content.key);
162  } else if (kind == Function) {
163  // This is the more typical entry point, starting the AD pipeline
164  // Inside startReverseAD2 the correctly dimensioned pipeline is chosen.
165  content.ptr->startReverseAD2(jacobians);
166  }
167  }
168 
170  // This is a crucial method in selecting the pipeline dimension: since it is templated method
171  // it will call the templated Record method reverseAD2. Hence, at this point in the pipeline
172  // there are as many reverseAD1 and reverseAD2 versions as there are different Jacobian sizes.
173  template<typename DerivedMatrix>
175  JacobianMap& jacobians) const {
176  if (kind == Leaf)
177  handleLeafCase(dTdA, jacobians, content.key);
178  else if (kind == Function)
179  content.ptr->reverseAD2(dTdA, jacobians);
180  }
181 
183  if (kind == Function) {
184  content.ptr->~CallRecord<Dim>();
185  }
186  }
187 
190 };
191 
192 } // namespace internal
193 } // namespace gtsam
gtsam::internal::ExecutionTrace::content
union gtsam::internal::ExecutionTrace::@1098 content
gtsam::internal::ExecutionTrace
Definition: Expression.h:39
gtsam::internal::ExecutionTrace::setLeaf
void setLeaf(Key key)
Change pointer to a Leaf Record.
Definition: ExecutionTrace.h:120
gtsam::internal::ExecutionTrace::record
std::optional< Record * > record()
Return record pointer, quite unsafe, used only for testing.
Definition: ExecutionTrace.h:144
gtsam.examples.DogLegOptimizerExample.type
type
Definition: DogLegOptimizerExample.py:111
gtsam::internal::ExecutionTrace::ExecutionTrace
ExecutionTrace()
Pointer always starts out as a Constant.
Definition: ExecutionTrace.h:115
gtsam::internal::UseBlockIf
Definition: ExecutionTrace.h:51
gtsam::internal::UseBlockIf< false, Derived >::addToJacobian
static void addToJacobian(const Eigen::MatrixBase< Derived > &dTdA, JacobianMap &jacobians, Key key)
Definition: ExecutionTrace.h:63
gtsam::internal::TraceAlignment
static const unsigned TraceAlignment
Definition: ExecutionTrace.h:45
gtsam::internal::ExecutionTrace::reverseAD1
void reverseAD1(const Eigen::MatrixBase< DerivedMatrix > &dTdA, JacobianMap &jacobians) const
Either add to Jacobians (Leaf) or propagate (Function)
Definition: ExecutionTrace.h:174
gtsam::internal::ExecutionTrace::Leaf
@ Leaf
Definition: ExecutionTrace.h:105
gtsam::internal::ExecutionTrace::type
ExecutionTrace< T > type
Define type so we can apply it as a meta-function.
Definition: ExecutionTrace.h:189
JacobianMap.h
JacobianMap for returning derivatives from expressions.
gtsam::internal::UseBlockIf::addToJacobian
static void addToJacobian(const Eigen::MatrixBase< Derived > &dTdA, JacobianMap &jacobians, Key key)
Definition: ExecutionTrace.h:52
gtsam::internal::ExecutionTrace::~ExecutionTrace
~ExecutionTrace()
Definition: ExecutionTrace.h:182
gtsam::internal::CallRecord< Dim >
Key.h
gtsam::internal::handleLeafCase
void handleLeafCase(const Eigen::MatrixBase< Derived > &dTdA, JacobianMap &jacobians, Key key)
Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians.
Definition: ExecutionTrace.h:71
gtsam::internal::ExecutionTrace::startReverseAD1
void startReverseAD1(JacobianMap &jacobians) const
Definition: ExecutionTrace.h:157
I
#define I
Definition: main.h:112
gtsam::internal::ExecutionTrace::kind
enum gtsam::internal::ExecutionTrace::@1097 kind
Eigen::Dynamic
const int Dynamic
Definition: Constants.h:22
gtsam::internal::ExecutionTraceStorage
std::aligned_storage< 1, TraceAlignment >::type ExecutionTraceStorage
Definition: ExecutionTrace.h:48
gtsam::internal::JacobianMap
Definition: JacobianMap.h:32
gtsam::internal::ExecutionTrace::Dim
static const int Dim
Definition: ExecutionTrace.h:102
Manifold.h
Base class and basic functions for Manifold types.
Record
Definition: testCallRecord.cpp:78
gtsam::internal::ExecutionTrace::JacobianTT
Eigen::Matrix< double, Dim, Dim > JacobianTT
Definition: ExecutionTrace.h:103
gtsam::internal::ExecutionTrace::print
void print(const std::string &indent="") const
Print.
Definition: ExecutionTrace.h:132
key
const gtsam::Symbol key('X', 0)
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::internal::ExecutionTrace::Function
@ Function
Definition: ExecutionTrace.h:105
gtsam::internal::ExecutionTrace::setFunction
void setFunction(CallRecord< Dim > *record)
Take ownership of pointer to a Function Record.
Definition: ExecutionTrace.h:126
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
internal
Definition: BandTriangularSolver.h:13
Eigen::MatrixBase
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
gtsam::internal::ExecutionTrace::key
Key key
Definition: ExecutionTrace.h:108
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::internal::ExecutionTrace::Constant
@ Constant
Definition: ExecutionTrace.h:105
gtsam::internal::ExecutionTrace::ptr
CallRecord< Dim > * ptr
Definition: ExecutionTrace.h:109


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:31