Loading...
Searching...
No Matches
OptionalJacobian.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/config.h> // Configuration from CMake
#include <Eigen/Dense>
#include <stdexcept>
#include <string>
#ifndef OPTIONALJACOBIAN_NOBOOST
#include <boost/optional.hpp>
#endif
namespace gtsam {
template<int Rows, int Cols>
class OptionalJacobian {
public:
typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
private:
Eigen::Map<Jacobian> map_;
// Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
// uses "placement new" to make map_ usurp the memory of the fixed size matrix
void usurp(double* data) {
new (&map_) Eigen::Map<Jacobian>(data);
}
// Private and very dangerous constructor straight from memory
OptionalJacobian(double* data) : map_(nullptr) {
if (data) usurp(data);
}
template<int M, int N>
friend class OptionalJacobian;
public:
OptionalJacobian() :
map_(nullptr) {
}
OptionalJacobian(Jacobian& fixed) :
map_(nullptr) {
usurp(fixed.data());
}
OptionalJacobian(Jacobian* fixedPtr) :
map_(nullptr) {
if (fixedPtr)
usurp(fixedPtr->data());
}
OptionalJacobian(Eigen::MatrixXd& dynamic) :
map_(nullptr) {
dynamic.resize(Rows, Cols); // no malloc if correct size
usurp(dynamic.data());
}
OptionalJacobian(Eigen::MatrixXd* dynamic) :
map_(nullptr) {
dynamic->resize(Rows, Cols); // no malloc if correct size
usurp(dynamic->data());
}
template<class MATRIX>
OptionalJacobian(Eigen::Ref<MATRIX> dynamic_ref) :
map_(nullptr) {
if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
usurp(dynamic_ref.data());
} else {
throw std::invalid_argument(
std::string("OptionalJacobian called with wrong dimensions or "
"storage order.\n"
"Expected: ") +
"(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
}
}
#ifndef OPTIONALJACOBIAN_NOBOOST
OptionalJacobian(boost::none_t /*none*/) :
map_(nullptr) {
}
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
map_(nullptr) {
if (optional) {
optional->resize(Rows, Cols);
usurp(optional->data());
}
}
#endif
// template <typename Derived, bool InnerPanel>
// OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
operator bool() const {
return map_.data() != nullptr;
}
Eigen::Map<Jacobian>& operator*() {
return map_;
}
Eigen::Map<Jacobian>* operator->() {
return &map_;
}
// template <int M, int N>
// OptionalJacobian<M, N> block(int startRow, int startCol) {
// if (*this)
// OptionalJacobian<M, N>(map_.block<M, N>(startRow, startCol));
// else
// return OptionalJacobian<M, N>();
// }
template <int N>
OptionalJacobian<Rows, N> cols(int startCol) {
if (*this)
return OptionalJacobian<Rows, N>(&map_(0,startCol));
else
return OptionalJacobian<Rows, N>();
}
};
// The pure dynamic specialization of this is needed to support
// variable-sized types. Note that this is designed to work like the
// boost optional scheme from GTSAM 3.
template<>
class OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> {
public:
typedef Eigen::MatrixXd Jacobian;
private:
Jacobian* pointer_;
public:
OptionalJacobian() :
pointer_(nullptr) {
}
OptionalJacobian(Jacobian* pointer) : pointer_(pointer) {}
OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {}
#ifndef OPTIONALJACOBIAN_NOBOOST
OptionalJacobian(boost::none_t /*none*/) :
pointer_(nullptr) {
}
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
pointer_(nullptr) {
if (optional) pointer_ = &(*optional);
}
#endif
operator bool() const {
return pointer_!=nullptr;
}
Jacobian& operator*() {
return *pointer_;
}
Jacobian* operator->(){ return pointer_; }
};
// forward declare
template <typename T> struct traits;
template <class T, class A>
struct MakeJacobian {
typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
};
template<class T, class A>
struct MakeOptionalJacobian {
typedef OptionalJacobian<traits<T>::dimension,
traits<A>::dimension> type;
};
} // namespace gtsam
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition OptionalJacobian.h:127
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition OptionalJacobian.h:122
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Eigen::MatrixXd &dynamic)
Constructor that will resize a dynamic matrix (unless already correct).
Definition OptionalJacobian.h:88
gtsam::OptionalJacobian::operator*
Eigen::Map< Jacobian > & operator*()
De-reference, like boost optional.
Definition OptionalJacobian.h:148
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian()
Default constructor acts like boost::none.
Definition OptionalJacobian.h:70
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Jacobian &fixed)
Constructor that will usurp data of a fixed-size matrix.
Definition OptionalJacobian.h:75
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Eigen::Ref< MATRIX > dynamic_ref)
Constructor from an Eigen::Ref value.
Definition OptionalJacobian.h:106
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Eigen::MatrixXd *dynamic)
Constructor that will resize a dynamic matrix (unless already correct).
Definition OptionalJacobian.h:95
gtsam::OptionalJacobian::operator->
Eigen::Map< Jacobian > * operator->()
operator->()
Definition OptionalJacobian.h:153
gtsam::OptionalJacobian< N, N >::Jacobian
Eigen::Matrix< double, Rows, Cols > Jacobian
Definition OptionalJacobian.h:47
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Jacobian *fixedPtr)
Constructor that will usurp data of a fixed-size matrix, pointer version.
Definition OptionalJacobian.h:81
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:171
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian()
View on constructor argument, if given.
Definition OptionalJacobian.h:202
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition OptionalJacobian.h:220
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::Jacobian
Eigen::MatrixXd Jacobian
Jacobian size type.
Definition OptionalJacobian.h:193
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::operator*
Jacobian & operator*()
De-reference, like boost optional.
Definition OptionalJacobian.h:233
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition OptionalJacobian.h:215
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(Jacobian *pointer)
Construct from pointer to dynamic matrix.
Definition OptionalJacobian.h:207
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(Jacobian &dynamic)
Construct from refrence to dynamic matrix.
Definition OptionalJacobian.h:210
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::operator->
Jacobian * operator->()
TODO: operator->().
Definition OptionalJacobian.h:238
gtsam::MakeJacobian
: meta-function to generate Jacobian
Definition OptionalJacobian.h:250
gtsam::MakeOptionalJacobian
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition OptionalJacobian.h:261