30 class ExpressionFactorBinaryTest;
41 BOOST_STATIC_ASSERT(
sizeof(
T) ==
sizeof(
size_t));
42 size_t & uiValue =
reinterpret_cast<size_t &
>(
value);
43 size_t misAlignment = uiValue % requiredAlignment;
45 uiValue += requiredAlignment - misAlignment;
51 return upAlign(value, requiredAlignment);
82 virtual void print(
const std::string& indent =
"")
const = 0;
94 virtual std::set<Key>
keys()
const {
100 virtual void dims(std::map<Key, int>& map)
const {
138 void print(
const std::string& indent =
"")
const override {
139 std::cout << indent <<
"Constant" << std::endl;
179 void print(
const std::string& indent =
"")
const override {
184 std::set<Key>
keys()
const override {
191 void dims(std::map<Key, int>& map)
const override {
197 return values.
at<
T>(key_);
204 return values.
at<
T>(key_);
211 template<
class T,
class A>
217 template <
class T,
class A>
221 static const Eigen::IOFormat kMatlabFormat(0, 1,
" ",
"; ",
"",
"",
"[",
"]");
223 <<
") = " << dTdA.format(kMatlabFormat) << std::endl;
229 template<
class T,
class A1>
238 expression1_(e1.
root()), function_(f) {
251 void print(
const std::string& indent =
"")
const override {
252 std::cout << indent <<
"UnaryExpression" << std::endl;
253 expression1_->print(indent +
" ");
258 return function_(expression1_->value(values), boost::none);
262 std::set<Key>
keys()
const override {
263 return expression1_->keys();
267 void dims(std::map<Key, int>& map)
const override {
268 expression1_->dims(map);
283 void print(
const std::string& indent)
const {
284 std::cout << indent <<
"UnaryExpression::Record {" << std::endl;
285 PrintJacobianAndTrace<T,A1>(indent, dTdA1, trace1);
286 std::cout << indent <<
"}" << std::endl;
303 template<
typename MatrixType>
334 template<
class T,
class A1,
class A2>
345 expression1_(e1.
root()), expression2_(e2.
root()), function_(f) {
351 friend class ::ExpressionFactorBinaryTest;
360 void print(
const std::string& indent =
"")
const override {
361 std::cout << indent <<
"BinaryExpression" << std::endl;
362 expression1_->print(indent +
" ");
363 expression2_->print(indent +
" ");
369 return function_(expression1_->value(values), expression2_->value(values),
374 std::set<Key>
keys()
const override {
375 std::set<Key>
keys = expression1_->keys();
376 std::set<Key> myKeys = expression2_->keys();
377 keys.insert(myKeys.begin(), myKeys.end());
382 void dims(std::map<Key, int>& map)
const override {
383 expression1_->dims(map);
384 expression2_->dims(map);
407 void print(
const std::string& indent)
const {
408 std::cout << indent <<
"BinaryExpression::Record {" << std::endl;
409 PrintJacobianAndTrace<T,A1>(indent, dTdA1, trace1);
410 PrintJacobianAndTrace<T,A2>(indent, dTdA2, trace2);
411 std::cout << indent <<
"}" << std::endl;
421 template<
typename MatrixType>
432 Record* record =
new (
ptr)
Record(values, *expression1_, *expression2_, ptr);
440 template<
class T,
class A1,
class A2,
class A3>
452 expression1_(e1.
root()), expression2_(e2.
root()), expression3_(e3.
root()),
467 void print(
const std::string& indent =
"")
const override {
468 std::cout << indent <<
"TernaryExpression" << std::endl;
469 expression1_->print(indent +
" ");
470 expression2_->print(indent +
" ");
471 expression3_->print(indent +
" ");
477 return function_(expression1_->value(values), expression2_->value(values),
478 expression3_->value(values),
none,
none, none);
482 std::set<Key>
keys()
const override {
483 std::set<Key>
keys = expression1_->keys();
484 std::set<Key> myKeys = expression2_->keys();
485 keys.insert(myKeys.begin(), myKeys.end());
486 myKeys = expression3_->keys();
487 keys.insert(myKeys.begin(), myKeys.end());
492 void dims(std::map<Key, int>& map)
const override {
493 expression1_->dims(map);
494 expression2_->dims(map);
495 expression3_->dims(map);
522 void print(
const std::string& indent)
const {
523 std::cout << indent <<
"TernaryExpression::Record {" << std::endl;
524 PrintJacobianAndTrace<T,A1>(indent, dTdA1, trace1);
525 PrintJacobianAndTrace<T,A2>(indent, dTdA2, trace2);
526 PrintJacobianAndTrace<T,A3>(indent, dTdA3, trace3);
527 std::cout << indent <<
"}" << std::endl;
538 template<
typename MatrixType>
550 Record* record =
new (
ptr)
Record(values, *expression1_, *expression2_, *expression3_, ptr);
577 void print(
const std::string& indent =
"")
const override {
578 std::cout << indent <<
"ScalarMultiplyNode" << std::endl;
579 expression_->print(indent +
" ");
584 return scalar_ * expression_->
value(values);
588 std::set<Key>
keys()
const override {
589 return expression_->keys();
593 void dims(std::map<Key, int>& map)
const override {
594 expression_->dims(map);
606 void print(
const std::string& indent)
const {
607 std::cout << indent <<
"ScalarMultiplyNode::Record {" << std::endl;
609 <<
") = " << scalar_dTdA << std::endl;
611 std::cout << indent <<
"}" << std::endl;
616 trace.
reverseAD1(scalar_dTdA * JacobianTT::Identity(), jacobians);
620 template <
typename MatrixType>
622 trace.
reverseAD1(dFdT * scalar_dTdA, jacobians);
632 T value = expression_->traceExecution(values, record->
trace, ptr);
633 ptr += expression_->traceSize();
636 return scalar_ *
value;
656 : expression1_(e1.
root()), expression2_(e2.
root()) {
665 void print(
const std::string& indent =
"")
const override {
666 std::cout << indent <<
"BinarySumNode" << std::endl;
667 expression1_->print(indent +
" ");
668 expression2_->print(indent +
" ");
673 return expression1_->
value(values) + expression2_->value(values);
677 std::set<Key>
keys()
const override {
678 std::set<Key>
keys = expression1_->keys();
679 std::set<Key> myKeys = expression2_->keys();
680 keys.insert(myKeys.begin(), myKeys.end());
685 void dims(std::map<Key, int>& map)
const override {
686 expression1_->dims(map);
687 expression2_->dims(map);
696 void print(
const std::string& indent)
const {
697 std::cout << indent <<
"BinarySumNode::Record {" << std::endl;
698 trace1.
print(indent);
699 trace2.
print(indent);
700 std::cout << indent <<
"}" << std::endl;
711 template <
typename MatrixType>
728 return expression1_->traceExecution(values, record->
trace1, ptr1) +
729 expression2_->traceExecution(values, record->
trace2, ptr2);
ExpressionNode(size_t traceSize=0)
Constructor, traceSize is size of the execution trace of expression rooted here.
void print(const std::string &indent="") const override
Print.
void print(const std::string &indent="") const
Print.
std::set< Key > keys() const override
Return keys that play in this expression.
void reverseAD1(const Eigen::MatrixBase< DerivedMatrix > &dTdA, JacobianMap &jacobians) const
Either add to Jacobians (Leaf) or propagate (Function)
void dims(std::map< Key, int > &map) const override
Return dimensions for each argument.
Expression< T >::template BinaryFunction< A1, A2 >::type Function
ExecutionTrace< T > trace
T value(const Values &values) const override
Return value.
meta-function to generate fixed-size JacobianTA type
T value(const Values &values) const override
Return value.
void print(const std::string &indent="") const override
Print.
void dims(std::map< Key, int > &map) const override
Return dimensions for each argument.
boost::shared_ptr< ExpressionNode< A2 > > expression2_
A non-templated config holding any types of Manifold-group elements.
void print(const std::string &indent) const
Print to std::cout.
T traceExecution(const Values &values, ExecutionTrace< T > &trace, ExecutionTraceStorage *traceStorage) const override
Construct an execution trace for reverse AD.
Jacobian< T, A1 >::type dTdA1
ScalarMultiplyNode(double s, const Expression< T > &e)
Constructor with a unary function f, and input argument e1.
T traceExecution(const Values &values, ExecutionTrace< T > &trace, ExecutionTraceStorage *ptr) const override
Construct an execution trace for reverse AD.
BinarySumNode(const Expression< T > &e1, const Expression< T > &e2)
Constructor with a binary function f, and two input arguments.
void startReverseAD4(JacobianMap &jacobians) const
Start the reverse AD process.
void print(const std::string &indent="") const override
Print.
boost::shared_ptr< ExpressionNode< T > > expression1_
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
BinaryExpression(Function f, const Expression< A1 > &e1, const Expression< A2 > &e2)
Constructor with a binary function f, and two input arguments.
ConstantExpression(const T &value)
Constructor with a value, yielding a constant.
ExecutionTrace< A1 > trace1
~UnaryExpression() override
Destructor.
void print(const std::string &indent) const
Print to std::cout.
ExecutionTrace< A1 > trace1
void print(const std::string &indent) const
Print to std::cout.
void startReverseAD4(JacobianMap &jacobians) const
Start the reverse AD process.
void print(const std::string &indent="") const override
Print.
T value(const Values &values) const override
Return value.
ExecutionTrace< A2 > trace2
Expression< T >::template UnaryFunction< A1 >::type Function
virtual ~ExpressionNode()
Destructor.
const mpreal root(const mpreal &x, unsigned long int k, mp_rnd_t r=mpreal::get_default_rnd())
boost::shared_ptr< ExpressionNode< T > > expression2_
void reverseAD4(const MatrixType &dFdT, JacobianMap &jacobians) const
Given df/dT, multiply in dT/dA and continue reverse AD process.
void print(const std::string &indent="") const override
Print.
ExpressionNode< T > NodeT
static const KeyFormatter DefaultKeyFormatter
boost::aligned_storage< 1, TraceAlignment >::type ExecutionTraceStorage
T upAligned(T value, unsigned requiredAlignment=TraceAlignment)
static void PrintJacobianAndTrace(const std::string &indent, const typename Jacobian< T, A >::type &dTdA, const ExecutionTrace< A > trace)
TernaryExpression(Function f, const Expression< A1 > &e1, const Expression< A2 > &e2, const Expression< A3 > &e3)
Constructor with a ternary function f, and two input arguments.
Jacobian< T, A1 >::type dTdA1
~ConstantExpression() override
Destructor.
Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > type
void reverseAD4(const MatrixType &dFdT, JacobianMap &jacobians) const
Given df/dT, multiply in dT/dA and continue reverse AD process.
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const ExpressionNode &node)
Streaming.
boost::shared_ptr< ExpressionNode< T > > expression_
T traceExecution(const Values &values, ExecutionTrace< T > &trace, ExecutionTraceStorage *ptr) const override
Construct an execution trace for reverse AD, see UnaryExpression for explanation. ...
std::set< Key > keys() const override
Return keys that play in this expression.
Expression for scalar multiplication.
void print(const std::string &indent) const
Print to std::cout.
T traceExecution(const Values &values, ExecutionTrace< T > &trace, ExecutionTraceStorage *ptr) const override
Construct an execution trace for reverse AD, see UnaryExpression for explanation. ...
Key key_
The key into values.
Expression< T >::template TernaryFunction< A1, A2, A3 >::type Function
const ValueType at(Key j) const
T traceExecution(const Values &values, ExecutionTrace< T > &trace, ExecutionTraceStorage *ptr) const override
Construct an execution trace for reverse AD.
boost::shared_ptr< ExpressionNode< A3 > > expression3_
LeafExpression(Key key)
Constructor with a single key.
void startReverseAD4(JacobianMap &jacobians) const
If the BinarySumExpression is the root, we just start as many pipelines as there are terms...
ExecutionTrace< A2 > trace2
ExecutionTrace< T > trace2
~BinaryExpression() override
Destructor.
void reverseAD4(const MatrixType &dFdT, JacobianMap &jacobians) const
If we are not the root, we simply pass on the adjoint matrix dFdT to all terms.
std::string demangle(const char *name)
Pretty print Value type name.
virtual void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
T traceExecution(const Values &values, ExecutionTrace< T > &trace, ExecutionTraceStorage *ptr) const override
Construct an execution trace for reverse AD.
T & upAlign(T &value, unsigned requiredAlignment=TraceAlignment)
Unary Function Expression.
Internals for Expression.h, not for general consumption.
void dims(std::map< Key, int > &map) const override
Return dimensions for each argument.
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
T value(const Values &values) const override
Return value.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Jacobian< T, A3 >::type dTdA3
virtual std::set< Key > keys() const
Return keys that play in this expression as a set.
T value(const Values &values) const override
Return value.
Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value...
boost::shared_ptr< ExpressionNode< A1 > > expression1_
void startReverseAD4(JacobianMap &jacobians) const
Start the reverse AD process, see comments in UnaryExpression.
ExecutionTrace< T > trace1
Record(const Values &values, const ExpressionNode< A1 > &expression1, const ExpressionNode< A2 > &expression2, const ExpressionNode< A3 > &expression3, ExecutionTraceStorage *ptr)
Construct record by calling 3 argument expressions.
void reverseAD4(const MatrixType &dFdT, JacobianMap &jacobians) const
Given df/dT, multiply in dT/dA and continue reverse AD process.
void dims(std::map< Key, int > &map) const override
Return dimensions for each argument.
virtual T traceExecution(const Values &values, ExecutionTrace< T > &trace, ExecutionTraceStorage *traceStorage) const =0
Construct an execution trace for reverse AD.
T traceExecution(const Values &values, ExecutionTrace< T > &trace, ExecutionTraceStorage *traceStorage) const override
Construct an execution trace for reverse AD.
virtual void print(const std::string &indent="") const =0
Print.
boost::shared_ptr< ExpressionNode< A1 > > expression1_
void dims(std::map< Key, int > &map) const override
Return dimensions for each argument.
void print(const std::string &indent="") const override
Print.
const Scalar & value() const
void setFunction(CallRecord< Dim > *record)
Take ownership of pointer to a Function Record.
Jacobian< T, A2 >::type dTdA2
void startReverseAD1(JacobianMap &jacobians) const
std::set< Key > keys() const override
Return keys that play in this expression.
ofstream os("timeSchurFactors.csv")
~BinarySumNode() override
Destructor.
~LeafExpression() override
Destructor.
UnaryExpression(Function f, const Expression< A1 > &e1)
Constructor with a unary function f, and input argument e1.
~TernaryExpression() override
Destructor.
Eigen::Matrix< double, Dim, Dim > JacobianTT
T constant_
The constant value.
void reverseAD4(const MatrixType &dFdT, JacobianMap &jacobians) const
Given df/dT, multiply in dT/dA and continue reverse AD process.
ExecutionTrace< A3 > trace3
boost::shared_ptr< ExpressionNode< A1 > > expression1_
std::set< Key > keys() const override
Return keys that play in this expression.
virtual T value(const Values &values) const =0
Return value.
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
T value(const Values &values) const override
Return value.
Record(const Values &values, const ExpressionNode< A1 > &expression1, ExecutionTraceStorage *ptr)
Construct record by calling argument expression.
Record(const Values &values, const ExpressionNode< A1 > &expression1, const ExpressionNode< A2 > &expression2, ExecutionTraceStorage *ptr)
Construct record by calling argument expressions.
void dims(std::map< Key, int > &map) const override
Return dimensions for each argument.
Jacobian< T, A1 >::type dTdA1
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Execution trace for expressions.
static const unsigned TraceAlignment
~ScalarMultiplyNode() override
Destructor.
boost::shared_ptr< ExpressionNode< A2 > > expression2_
The matrix class, also used for vectors and row-vectors.
void startReverseAD4(JacobianMap &jacobians) const
Start the reverse AD process, see comments in Base.
Jacobian< T, A2 >::type dTdA2
std::set< Key > keys() const override
Return keys that play in this expression.
void setLeaf(Key key)
Change pointer to a Leaf Record.
std::set< Key > keys() const override
Return keys that play in this expression.
std::uint64_t Key
Integer nonlinear key type.
ExecutionTrace< A1 > trace1
void print(const std::string &indent="") const override
Print.
T value(const Values &values) const override
Return value.
void print(const std::string &indent) const
Print to std::cout.