gtsam 4.1.1
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
24#ifndef OPTIONALJACOBIAN_NOBOOST
25#include <boost/optional.hpp>
26#endif
27
28namespace gtsam {
29
38template<int Rows, int Cols>
40
41public:
42
45 typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
46
47private:
48
49 Eigen::Map<Jacobian> map_;
50
51 // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
52 // uses "placement new" to make map_ usurp the memory of the fixed size matrix
53 void usurp(double* data) {
54 new (&map_) Eigen::Map<Jacobian>(data);
55 }
56
57 // Private and very dangerous constructor straight from memory
58 OptionalJacobian(double* data) : map_(nullptr) {
59 if (data) usurp(data);
60 }
61
62 template<int M, int N>
63 friend class OptionalJacobian;
64
65public:
66
69 map_(nullptr) {
70 }
71
74 map_(nullptr) {
75 usurp(fixed.data());
76 }
77
80 map_(nullptr) {
81 if (fixedPtr)
82 usurp(fixedPtr->data());
83 }
84
86 OptionalJacobian(Eigen::MatrixXd& dynamic) :
87 map_(nullptr) {
88 dynamic.resize(Rows, Cols); // no malloc if correct size
89 usurp(dynamic.data());
90 }
91
93 OptionalJacobian(Eigen::MatrixXd* dynamic) :
94 map_(nullptr) {
95 dynamic->resize(Rows, Cols); // no malloc if correct size
96 usurp(dynamic->data());
97 }
98
99#ifndef OPTIONALJACOBIAN_NOBOOST
100
102 OptionalJacobian(boost::none_t /*none*/) :
103 map_(nullptr) {
104 }
105
107 OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
108 map_(nullptr) {
109 if (optional) {
110 optional->resize(Rows, Cols);
111 usurp(optional->data());
112 }
113 }
114
115#endif
116
119 // template <typename Derived, bool InnerPanel>
120 // OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
121
123 operator bool() const {
124 return map_.data() != nullptr;
125 }
126
128 Eigen::Map<Jacobian>& operator*() {
129 return map_;
130 }
131
133 Eigen::Map<Jacobian>* operator->() {
134 return &map_;
135 }
136
139 // template <int M, int N>
140 // OptionalJacobian<M, N> block(int startRow, int startCol) {
141 // if (*this)
142 // OptionalJacobian<M, N>(map_.block<M, N>(startRow, startCol));
143 // else
144 // return OptionalJacobian<M, N>();
145 // }
146
150 template <int N>
152 if (*this)
153 return OptionalJacobian<Rows, N>(&map_(0,startCol));
154 else
156 }
157
162};
163
164// The pure dynamic specialization of this is needed to support
165// variable-sized types. Note that this is designed to work like the
166// boost optional scheme from GTSAM 3.
167template<>
168class OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> {
169
170public:
171
173 typedef Eigen::MatrixXd Jacobian;
174
175private:
176
177 Jacobian* pointer_;
178
179public:
180
183 pointer_(nullptr) {
184 }
185
187 OptionalJacobian(Jacobian* pointer) : pointer_(pointer) {}
188
190 OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {}
191
192#ifndef OPTIONALJACOBIAN_NOBOOST
193
195 OptionalJacobian(boost::none_t /*none*/) :
196 pointer_(nullptr) {
197 }
198
200 OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
201 pointer_(nullptr) {
202 if (optional) pointer_ = &(*optional);
203 }
204
205#endif
206
208 operator bool() const {
209 return pointer_!=nullptr;
210 }
211
214 return *pointer_;
215 }
216
218 Jacobian* operator->(){ return pointer_; }
219};
220
221// forward declare
222template <typename T> struct traits;
223
229template <class T, class A>
231 typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
232};
233
240template<class T, class A>
244};
245
246} // namespace gtsam
247
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:39
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition: OptionalJacobian.h:107
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition: OptionalJacobian.h:102
OptionalJacobian(Eigen::MatrixXd &dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition: OptionalJacobian.h:86
Eigen::Map< Jacobian > & operator*()
De-reference, like boost optional.
Definition: OptionalJacobian.h:128
OptionalJacobian()
Default constructor acts like boost::none.
Definition: OptionalJacobian.h:68
OptionalJacobian(Jacobian &fixed)
Constructor that will usurp data of a fixed-size matrix.
Definition: OptionalJacobian.h:73
OptionalJacobian(Eigen::MatrixXd *dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition: OptionalJacobian.h:93
Eigen::Map< Jacobian > * operator->()
operator->()
Definition: OptionalJacobian.h:133
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:45
OptionalJacobian(Jacobian *fixedPtr)
Constructor that will usurp data of a fixed-size matrix, pointer version.
Definition: OptionalJacobian.h:79
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:151
OptionalJacobian()
View on constructor argument, if given.
Definition: OptionalJacobian.h:182
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition: OptionalJacobian.h:200
Eigen::MatrixXd Jacobian
Jacobian size type.
Definition: OptionalJacobian.h:173
Jacobian & operator*()
De-reference, like boost optional.
Definition: OptionalJacobian.h:213
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition: OptionalJacobian.h:195
OptionalJacobian(Jacobian *pointer)
Construct from pointer to dynamic matrix.
Definition: OptionalJacobian.h:187
OptionalJacobian(Jacobian &dynamic)
Construct from refrence to dynamic matrix.
Definition: OptionalJacobian.h:190
Jacobian * operator->()
TODO: operator->()
Definition: OptionalJacobian.h:218
: meta-function to generate Jacobian
Definition: OptionalJacobian.h:230
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition: OptionalJacobian.h:241