Loading...
Searching...
No Matches
JacobianFactor.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/linear/GaussianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/global_includes.h>
#include <gtsam/inference/VariableSlots.h>
#include <boost/make_shared.hpp>
#include <boost/serialization/version.hpp>
#include <boost/serialization/split_member.hpp>
namespace gtsam {
// Forward declarations
class HessianFactor;
class VariableSlots;
class GaussianFactorGraph;
class GaussianConditional;
class HessianFactor;
class VectorValues;
class Ordering;
class JacobianFactor;
GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
class GTSAM_EXPORT JacobianFactor : public GaussianFactor
{
public:
typedef JacobianFactor This;
typedef GaussianFactor Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef VerticalBlockMatrix::Block ABlock;
typedef VerticalBlockMatrix::constBlock constABlock;
typedef ABlock::ColXpr BVector;
typedef constABlock::ConstColXpr constBVector;
protected:
VerticalBlockMatrix Ab_; // the block view of the full matrix
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
public:
explicit JacobianFactor(const GaussianFactor& gf);
JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {}
explicit JacobianFactor(const HessianFactor& hf);
JacobianFactor();
explicit JacobianFactor(const Vector& b_in);
JacobianFactor(Key i1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model = SharedDiagonal());
JacobianFactor(Key i1, const Matrix& A1,
Key i2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model = SharedDiagonal());
JacobianFactor(Key i1, const Matrix& A1, Key i2,
const Matrix& A2, Key i3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model = SharedDiagonal());
template<typename TERMS>
JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal());
template<typename KEYS>
JacobianFactor(
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal());
explicit JacobianFactor(
const GaussianFactorGraph& graph);
explicit JacobianFactor(
const GaussianFactorGraph& graph,
const VariableSlots& p_variableSlots);
explicit JacobianFactor(
const GaussianFactorGraph& graph,
const Ordering& ordering);
explicit JacobianFactor(
const GaussianFactorGraph& graph,
const Ordering& ordering,
const VariableSlots& p_variableSlots);
~JacobianFactor() override {}
GaussianFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<GaussianFactor>(
boost::make_shared<JacobianFactor>(*this));
}
// Implementing Testable interface
void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
Vector unweighted_error(const VectorValues& c) const;
Vector error_vector(const VectorValues& c) const;
using GaussianFactor::error;
double error(const VectorValues& c) const override;
Matrix augmentedInformation() const override;
Matrix information() const override;
using Base::hessianDiagonal;
void hessianDiagonalAdd(VectorValues& d) const override;
void hessianDiagonal(double* d) const override;
std::map<Key,Matrix> hessianBlockDiagonal() const override;
std::pair<Matrix, Vector> jacobian() const override;
std::pair<Matrix, Vector> jacobianUnweighted() const;
Matrix augmentedJacobian() const override;
Matrix augmentedJacobianUnweighted() const;
const VerticalBlockMatrix& matrixObject() const { return Ab_; }
VerticalBlockMatrix& matrixObject() { return Ab_; }
GaussianFactor::shared_ptr negate() const override;
bool isConstrained() const {
return model_ && model_->isConstrained();
}
DenseIndex getDim(const_iterator variable) const override {
return Ab_(variable - begin()).cols();
}
size_t rows() const { return Ab_.rows(); }
size_t cols() const { return Ab_.cols(); }
const SharedDiagonal& get_model() const { return model_; }
SharedDiagonal& get_model() { return model_; }
const constBVector getb() const { return Ab_(size()).col(0); }
constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }
constABlock getA() const { return Ab_.range(0, size()); }
BVector getb() { return Ab_(size()).col(0); }
ABlock getA(iterator variable) { return Ab_(variable - begin()); }
ABlock getA() { return Ab_.range(0, size()); }
void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
Vector operator*(const VectorValues& x) const;
void transposeMultiplyAdd(double alpha, const Vector& e,
VectorValues& x) const;
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const override;
void multiplyHessianAdd(double alpha, const double* x, double* y,
const std::vector<size_t>& accumulatedDims) const;
VectorValues gradientAtZero() const override;
void gradientAtZero(double* d) const override;
Vector gradient(Key key, const VectorValues& x) const override;
JacobianFactor whiten() const;
std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
eliminate(const Ordering& keys);
void setModel(bool anyConstrained, const Vector& sigmas);
friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
protected:
template<typename TERMS>
void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
private:
void JacobianFactorHelper(
const GaussianFactorGraph& graph,
const FastVector<VariableSlots::const_iterator>& orderedSlots);
template<class KEYS, class DIMENSIONS>
JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
const SharedDiagonal& model = SharedDiagonal()) :
Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
}
// be very selective on who can access these private methods:
template<typename T> friend class ExpressionFactor;
friend class boost::serialization::access;
template<class ARCHIVE>
void save(ARCHIVE & ar, const unsigned int version) const {
// TODO(fan): This is a hack for Boost < 1.66
// We really need to introduce proper versioning in the archives
// As otherwise this will not read objects serialized by older
// versions of GTSAM
ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar << BOOST_SERIALIZATION_NVP(Ab_);
bool model_null = false;
if(model_.get() == nullptr) {
model_null = true;
ar << boost::serialization::make_nvp("model_null", model_null);
} else {
ar << boost::serialization::make_nvp("model_null", model_null);
ar << BOOST_SERIALIZATION_NVP(model_);
}
}
template<class ARCHIVE>
void load(ARCHIVE & ar, const unsigned int version) {
// invoke serialization of the base class
ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar >> BOOST_SERIALIZATION_NVP(Ab_);
if (version < 1) {
ar >> BOOST_SERIALIZATION_NVP(model_);
} else {
bool model_null;
ar >> BOOST_SERIALIZATION_NVP(model_null);
if (!model_null) {
ar >> BOOST_SERIALIZATION_NVP(model_);
}
}
}
BOOST_SERIALIZATION_SPLIT_MEMBER()
}; // JacobianFactor
template<>
struct traits<JacobianFactor> : public Testable<JacobianFactor> {
};
} // \ namespace gtsam
BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
#include <gtsam/linear/JacobianFactor-inl.h>
VerticalBlockMatrix.h
A matrix with column blocks of pre-defined sizes.
VariableSlots.h
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
NoiseModel.h
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
global_includes.h
Included from all GTSAM files.
gtsam::FastVector
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:33
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition LossFunctions.cpp:27
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::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
gtsam::Factor::begin
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition Factor.h:143
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
gtsam::Factor::iterator
KeyVector::iterator iterator
Iterator over keys.
Definition Factor.h:77
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
gtsam::Ordering
Definition Ordering.h:34
gtsam::VariableSlots
A combined factor is assembled as one block of rows for each component factor.
Definition VariableSlots.h:52
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
gtsam::GaussianFactor::GaussianFactor
GaussianFactor()
Default constructor creates empty factor.
Definition GaussianFactor.h:46
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form).
Definition HessianFactor.h:101
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
gtsam::JacobianFactor::getb
BVector getb()
Get a view of the r.h.s.
Definition JacobianFactor.h:306
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
get a copy of model
Definition JacobianFactor.h:291
gtsam::JacobianFactor::JacobianFactor
JacobianFactor(const GaussianFactor &gf)
Convert from other GaussianFactor.
Definition JacobianFactor.cpp:60
gtsam::JacobianFactor::JacobianFactor
JacobianFactor(const JacobianFactor &jf)
Copy constructor.
Definition JacobianFactor.h:114
gtsam::JacobianFactor::This
JacobianFactor This
Typedef to this class.
Definition JacobianFactor.h:94
gtsam::JacobianFactor::isConstrained
bool isConstrained() const
is noise model constrained ?
Definition JacobianFactor.h:269
gtsam::JacobianFactor::getA
constABlock getA() const
Get a view of the A matrix, not weighted by noise.
Definition JacobianFactor.h:303
gtsam::JacobianFactor::clone
GaussianFactor::shared_ptr clone() const override
Clone this JacobianFactor.
Definition JacobianFactor.h:189
gtsam::JacobianFactor::getA
ABlock getA(iterator variable)
Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version).
Definition JacobianFactor.h:309
gtsam::JacobianFactor::get_model
SharedDiagonal & get_model()
get a copy of model (non-const version)
Definition JacobianFactor.h:294
gtsam::JacobianFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Return the dimension of the variable pointed to by the given key iterator todo: Remove this in favor ...
Definition JacobianFactor.h:276
gtsam::JacobianFactor::matrixObject
VerticalBlockMatrix & matrixObject()
Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition JacobianFactor.h:259
gtsam::JacobianFactor::getA
ABlock getA()
Get a view of the A matrix.
Definition JacobianFactor.h:312
gtsam::JacobianFactor::Base
GaussianFactor Base
Typedef to base class.
Definition JacobianFactor.h:95
gtsam::JacobianFactor::~JacobianFactor
~JacobianFactor() override
Virtual destructor.
Definition JacobianFactor.h:186
gtsam::JacobianFactor::matrixObject
const VerticalBlockMatrix & matrixObject() const
Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition JacobianFactor.h:256
gtsam::JacobianFactor::rows
size_t rows() const
return the number of rows in the corresponding linear system
Definition JacobianFactor.h:283
gtsam::JacobianFactor::cols
size_t cols() const
return the number of columns in the corresponding linear system
Definition JacobianFactor.h:288
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
gtsam::JacobianFactor::getA
constABlock getA(const_iterator variable) const
Get a view of the A matrix for the variable pointed to by the given key iterator.
Definition JacobianFactor.h:300
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
VectorValues
The Factor::error simply extracts the.