29#include <gtsam/nonlinear/internal/ExpressionNode.h>
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>
40 root_(new internal::ConstantExpression<T>(value)) {
45 root_(new internal::LeafExpression<T>(key)) {
50 root_(new internal::LeafExpression<T>(
symbol)) {
55 root_(new internal::LeafExpression<T>(
Symbol(c, j))) {
63 root_(new internal::UnaryExpression<T, A>(function, expression)) {
68template<
typename A1,
typename A2>
72 new internal::BinaryExpression<T, A1, A2>(function, expression1,
78template<
typename A1,
typename A2,
typename A3>
83 new internal::TernaryExpression<T, A1, A2, A3>(function, expression1,
84 expression2, expression3)) {
93 new internal::UnaryExpression<T, A>(std::bind(method,
94 std::placeholders::_1, std::placeholders::_2),
100template<
typename A1,
typename A2>
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)) {
115template<
typename A1,
typename A2,
typename A3>
117 T (A1::*method)(const A2&, const A3&,
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)) {
148 boost::optional<std::vector<Matrix>&> H)
const {
154 boost::tie(keys, dims) = keysAndDims();
155 return valueAndDerivatives(values, keys, dims, *H);
158 return root_->value(values);
176 std::vector<Matrix>& H)
const {
179 assert(H.size()==keys.size());
184 Ab.matrix().setZero();
185 internal::JacobianMap jacobianMap(keys, Ab);
188 T result = valueAndJacobianMap(values, jacobianMap);
191 for (
DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
201 static_cast<internal::ExecutionTraceStorage*
>(traceStorage));
206 internal::JacobianMap& jacobians)
const {
212 size_t size = traceSize();
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());
223 internal::ExecutionTraceStorage traceStorage[size];
227 T value(this->traceExecution(values, trace, traceStorage));
228 trace.startReverseAD1(jacobians);
235 std::map<Key, int> map;
237 size_t n = map.size();
239 boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
240 boost::copy(map | boost::adaptors::map_values, pair.second.begin());
248 typedef T result_type;
252 return x.compose(y, H1, H2);
258 double operator()(
const double& x,
const double& y,
261 if (H1) H1->setConstant(y);
262 if (H2) H2->setConstant(x);
277 std::placeholders::_2, std::placeholders::_3,
278 std::placeholders::_4),
279 expression1, expression2);
285 std::vector<Expression<T> > unknowns;
287 for (
size_t i = start; i < start + n; i++)
293ScalarMultiplyExpression<T>::ScalarMultiplyExpression(
double s,
const Expression<T>& e)
294 : Expression<T>(boost::make_shared<internal::ScalarMultiplyNode<T>>(s, e)) {}
298BinarySumExpression<T>::BinarySumExpression(
const Expression<T>& e1,
const Expression<T>& e2)
299 : Expression<T>(boost::
make_shared<internal::BinarySumNode<T>>(e1, e2)) {}
303 root_ = boost::make_shared<internal::BinarySumNode<T>>(*
this, e);
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