gtsam  4.1.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 
24 #ifndef OPTIONALJACOBIAN_NOBOOST
25 #include <boost/optional.hpp>
26 #endif
27 
28 namespace gtsam {
29 
38 template<int Rows, int Cols>
40 
41 public:
42 
45  typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
46 
47 private:
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 
65 public:
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 
92 #ifndef OPTIONALJACOBIAN_NOBOOST
93 
95  OptionalJacobian(boost::none_t /*none*/) :
96  map_(nullptr) {
97  }
98 
100  OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
101  map_(nullptr) {
102  if (optional) {
103  optional->resize(Rows, Cols);
104  usurp(optional->data());
105  }
106  }
107 
108 #endif
109 
112  // template <typename Derived, bool InnerPanel>
113  // OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
114 
116  operator bool() const {
117  return map_.data() != nullptr;
118  }
119 
121  Eigen::Map<Jacobian>& operator*() {
122  return map_;
123  }
124 
126  Eigen::Map<Jacobian>* operator->() {
127  return &map_;
128  }
129 
132  // template <int M, int N>
133  // OptionalJacobian<M, N> block(int startRow, int startCol) {
134  // if (*this)
135  // OptionalJacobian<M, N>(map_.block<M, N>(startRow, startCol));
136  // else
137  // return OptionalJacobian<M, N>();
138  // }
139 
143  template <int N>
145  if (*this)
146  return OptionalJacobian<Rows, N>(&map_(0,startCol));
147  else
148  return OptionalJacobian<Rows, N>();
149  }
150 
155 };
156 
157 // The pure dynamic specialization of this is needed to support
158 // variable-sized types. Note that this is designed to work like the
159 // boost optional scheme from GTSAM 3.
160 template<>
161 class OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> {
162 
163 public:
164 
166  typedef Eigen::MatrixXd Jacobian;
167 
168 private:
169 
170  Jacobian* pointer_;
171 
172 public:
173 
176  pointer_(nullptr) {
177  }
178 
180  OptionalJacobian(Jacobian* pointer) : pointer_(pointer) {}
181 
183  OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {}
184 
185 #ifndef OPTIONALJACOBIAN_NOBOOST
186 
188  OptionalJacobian(boost::none_t /*none*/) :
189  pointer_(nullptr) {
190  }
191 
193  OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
194  pointer_(nullptr) {
195  if (optional) pointer_ = &(*optional);
196  }
197 
198 #endif
199 
201  operator bool() const {
202  return pointer_!=nullptr;
203  }
204 
207  return *pointer_;
208  }
209 
211  Jacobian* operator->(){ return pointer_; }
212 };
213 
214 // forward declare
215 template <typename T> struct traits;
216 
222 template <class T, class A>
223 struct MakeJacobian {
224  typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
225 };
226 
233 template<class T, class A>
237 };
238 
239 } // namespace gtsam
240 
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(Jacobian &dynamic)
Construct from refrence to dynamic matrix.
Definition: OptionalJacobian.h:183
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Eigen::MatrixXd &dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition: OptionalJacobian.h:86
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition: OptionalJacobian.h:95
gtsam::OptionalJacobian::operator*
Eigen::Map< Jacobian > & operator*()
De-reference, like boost optional.
Definition: OptionalJacobian.h:121
gtsam::MakeOptionalJacobian
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition: OptionalJacobian.h:234
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(Jacobian *pointer)
Construct from pointer to dynamic matrix.
Definition: OptionalJacobian.h:180
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian()
View on constructor argument, if given.
Definition: OptionalJacobian.h:175
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::operator*
Jacobian & operator*()
De-reference, like boost optional.
Definition: OptionalJacobian.h:206
gtsam::MakeJacobian
: meta-function to generate Jacobian
Definition: OptionalJacobian.h:223
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::OptionalJacobian::operator->
Eigen::Map< Jacobian > * operator->()
operator->()
Definition: OptionalJacobian.h:126
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Jacobian &fixed)
Constructor that will usurp data of a fixed-size matrix.
Definition: OptionalJacobian.h:73
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Jacobian *fixedPtr)
Constructor that will usurp data of a fixed-size matrix, pointer version.
Definition: OptionalJacobian.h:79
gtsam::OptionalJacobian::cols
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:144
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition: OptionalJacobian.h:193
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian()
Default constructor acts like boost::none.
Definition: OptionalJacobian.h:68
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition: OptionalJacobian.h:100
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::operator->
Jacobian * operator->()
TODO: operator->()
Definition: OptionalJacobian.h:211
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition: OptionalJacobian.h:188
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::Jacobian
Eigen::MatrixXd Jacobian
Jacobian size type.
Definition: OptionalJacobian.h:166
gtsam::OptionalJacobian::Jacobian
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