gtsam 4.2.0
gtsam
OptionalJacobian.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#include <gtsam/config.h> // Configuration from CMake
22#include <Eigen/Dense>
23#include <stdexcept>
24#include <string>
25
26#ifndef OPTIONALJACOBIAN_NOBOOST
27#include <boost/optional.hpp>
28#endif
29
30namespace gtsam {
31
40template<int Rows, int Cols>
42
43public:
44
47 typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
48
49private:
50
51 Eigen::Map<Jacobian> map_;
52
53 // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
54 // uses "placement new" to make map_ usurp the memory of the fixed size matrix
55 void usurp(double* data) {
56 new (&map_) Eigen::Map<Jacobian>(data);
57 }
58
59 // Private and very dangerous constructor straight from memory
60 OptionalJacobian(double* data) : map_(nullptr) {
61 if (data) usurp(data);
62 }
63
64 template<int M, int N>
65 friend class OptionalJacobian;
66
67public:
68
71 map_(nullptr) {
72 }
73
76 map_(nullptr) {
77 usurp(fixed.data());
78 }
79
82 map_(nullptr) {
83 if (fixedPtr)
84 usurp(fixedPtr->data());
85 }
86
88 OptionalJacobian(Eigen::MatrixXd& dynamic) :
89 map_(nullptr) {
90 dynamic.resize(Rows, Cols); // no malloc if correct size
91 usurp(dynamic.data());
92 }
93
95 OptionalJacobian(Eigen::MatrixXd* dynamic) :
96 map_(nullptr) {
97 dynamic->resize(Rows, Cols); // no malloc if correct size
98 usurp(dynamic->data());
99 }
100
105 template<class MATRIX>
106 OptionalJacobian(Eigen::Ref<MATRIX> dynamic_ref) :
107 map_(nullptr) {
108 if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
109 usurp(dynamic_ref.data());
110 } else {
111 throw std::invalid_argument(
112 std::string("OptionalJacobian called with wrong dimensions or "
113 "storage order.\n"
114 "Expected: ") +
115 "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
116 }
117 }
118
119#ifndef OPTIONALJACOBIAN_NOBOOST
120
122 OptionalJacobian(boost::none_t /*none*/) :
123 map_(nullptr) {
124 }
125
127 OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
128 map_(nullptr) {
129 if (optional) {
130 optional->resize(Rows, Cols);
131 usurp(optional->data());
132 }
133 }
134
135#endif
136
139 // template <typename Derived, bool InnerPanel>
140 // OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
141
143 operator bool() const {
144 return map_.data() != nullptr;
145 }
146
148 Eigen::Map<Jacobian>& operator*() {
149 return map_;
150 }
151
153 Eigen::Map<Jacobian>* operator->() {
154 return &map_;
155 }
156
159 // template <int M, int N>
160 // OptionalJacobian<M, N> block(int startRow, int startCol) {
161 // if (*this)
162 // OptionalJacobian<M, N>(map_.block<M, N>(startRow, startCol));
163 // else
164 // return OptionalJacobian<M, N>();
165 // }
166
170 template <int N>
172 if (*this)
173 return OptionalJacobian<Rows, N>(&map_(0,startCol));
174 else
176 }
177
182};
183
184// The pure dynamic specialization of this is needed to support
185// variable-sized types. Note that this is designed to work like the
186// boost optional scheme from GTSAM 3.
187template<>
188class OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> {
189
190public:
191
193 typedef Eigen::MatrixXd Jacobian;
194
195private:
196
197 Jacobian* pointer_;
198
199public:
200
203 pointer_(nullptr) {
204 }
205
207 OptionalJacobian(Jacobian* pointer) : pointer_(pointer) {}
208
210 OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {}
211
212#ifndef OPTIONALJACOBIAN_NOBOOST
213
215 OptionalJacobian(boost::none_t /*none*/) :
216 pointer_(nullptr) {
217 }
218
220 OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
221 pointer_(nullptr) {
222 if (optional) pointer_ = &(*optional);
223 }
224
225#endif
226
228 operator bool() const {
229 return pointer_!=nullptr;
230 }
231
234 return *pointer_;
235 }
236
238 Jacobian* operator->(){ return pointer_; }
239};
240
241// forward declare
242template <typename T> struct traits;
243
249template <class T, class A>
251 typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
252};
253
260template<class T, class A>
264};
265
266} // namespace gtsam
267
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition: OptionalJacobian.h:127
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition: OptionalJacobian.h:122
OptionalJacobian(Eigen::MatrixXd &dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition: OptionalJacobian.h:88
Eigen::Map< Jacobian > & operator*()
De-reference, like boost optional.
Definition: OptionalJacobian.h:148
OptionalJacobian()
Default constructor acts like boost::none.
Definition: OptionalJacobian.h:70
OptionalJacobian(Jacobian &fixed)
Constructor that will usurp data of a fixed-size matrix.
Definition: OptionalJacobian.h:75
OptionalJacobian(Eigen::Ref< MATRIX > dynamic_ref)
Constructor from an Eigen::Ref value.
Definition: OptionalJacobian.h:106
OptionalJacobian(Eigen::MatrixXd *dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition: OptionalJacobian.h:95
Eigen::Map< Jacobian > * operator->()
operator->()
Definition: OptionalJacobian.h:153
Eigen::Matrix< double, Rows, Cols > Jacobian
Jacobian size type TODO(frank): how to enforce RowMajor? Or better, make it work with any storage ord...
Definition: OptionalJacobian.h:47
OptionalJacobian(Jacobian *fixedPtr)
Constructor that will usurp data of a fixed-size matrix, pointer version.
Definition: OptionalJacobian.h:81
OptionalJacobian< Rows, N > cols(int startCol)
Access M*N sub-block if we are allocated, otherwise none TODO(frank): this could work as is below if ...
Definition: OptionalJacobian.h:171
OptionalJacobian()
View on constructor argument, if given.
Definition: OptionalJacobian.h:202
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition: OptionalJacobian.h:220
Eigen::MatrixXd Jacobian
Jacobian size type.
Definition: OptionalJacobian.h:193
Jacobian & operator*()
De-reference, like boost optional.
Definition: OptionalJacobian.h:233
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition: OptionalJacobian.h:215
OptionalJacobian(Jacobian *pointer)
Construct from pointer to dynamic matrix.
Definition: OptionalJacobian.h:207
OptionalJacobian(Jacobian &dynamic)
Construct from refrence to dynamic matrix.
Definition: OptionalJacobian.h:210
Jacobian * operator->()
TODO: operator->()
Definition: OptionalJacobian.h:238
: meta-function to generate Jacobian
Definition: OptionalJacobian.h:250
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition: OptionalJacobian.h:261