Loading...
Searching...
No Matches
GaussianFactorGraph.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/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
// Forward declarations
class GaussianFactorGraph;
class GaussianFactor;
class GaussianConditional;
class GaussianBayesNet;
class GaussianEliminationTree;
class GaussianBayesTree;
class GaussianJunctionTree;
/* ************************************************************************* */
template<> struct EliminationTraits<GaussianFactorGraph>
{
typedef GaussianFactor FactorType;
typedef GaussianFactorGraph FactorGraphType;
typedef GaussianConditional ConditionalType;
typedef GaussianBayesNet BayesNetType;
typedef GaussianEliminationTree EliminationTreeType;
typedef GaussianBayesTree BayesTreeType;
typedef GaussianJunctionTree JunctionTreeType;
static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
return EliminatePreferCholesky(factors, keys); }
static Ordering DefaultOrderingFunc(
const FactorGraphType& graph,
boost::optional<const VariableIndex&> variableIndex) {
return Ordering::Colamd(*variableIndex);
}
};
/* ************************************************************************* */
class GTSAM_EXPORT GaussianFactorGraph :
public FactorGraph<GaussianFactor>,
public EliminateableFactorGraph<GaussianFactorGraph>
{
public:
typedef GaussianFactorGraph This;
typedef FactorGraph<GaussianFactor> Base;
typedef EliminateableFactorGraph<This> BaseEliminateable;
typedef boost::shared_ptr<This> shared_ptr;
GaussianFactorGraph() {}
GaussianFactorGraph(std::initializer_list<sharedFactor> factors) : Base(factors) {}
template<typename ITERATOR>
GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
template<class CONTAINER>
explicit GaussianFactorGraph(const CONTAINER& factors) : Base(factors) {}
template<class DERIVEDFACTOR>
GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
virtual ~GaussianFactorGraph() {}
bool equals(const This& fg, double tol = 1e-9) const;
friend bool operator==(const GaussianFactorGraph& lhs,
const GaussianFactorGraph& rhs) {
return lhs.isEqual(rhs);
}
void add(const GaussianFactor& factor) { push_back(factor.clone()); }
void add(const sharedFactor& factor) { push_back(factor); }
void add(const Vector& b) {
add(JacobianFactor(b)); }
void add(Key key1, const Matrix& A1,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
add(JacobianFactor(key1,A1,b,model)); }
void add(Key key1, const Matrix& A1,
Key key2, const Matrix& A2,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
add(JacobianFactor(key1,A1,key2,A2,b,model)); }
void add(Key key1, const Matrix& A1,
Key key2, const Matrix& A2,
Key key3, const Matrix& A3,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); }
template<class TERMS>
void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model = SharedDiagonal()) {
add(JacobianFactor(terms,b,model)); }
typedef KeySet Keys;
Keys keys() const;
/* return a map of (Key, dimension) */
std::map<Key, size_t> getKeyDimMap() const;
double error(const VectorValues& x) const;
double probPrime(const VectorValues& c) const;
virtual GaussianFactorGraph clone() const;
virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;
GaussianFactorGraph negate() const;
std::vector<std::tuple<int, int, double> > sparseJacobian(
const Ordering& ordering, size_t& nrows, size_t& ncols) const;
std::vector<std::tuple<int, int, double> > sparseJacobian() const;
Matrix sparseJacobian_() const;
Matrix augmentedJacobian(const Ordering& ordering) const;
Matrix augmentedJacobian() const;
std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
std::pair<Matrix,Vector> jacobian() const;
Matrix augmentedHessian(const Ordering& ordering) const;
Matrix augmentedHessian() const;
std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
std::pair<Matrix,Vector> hessian() const;
virtual VectorValues hessianDiagonal() const;
virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
VectorValues optimize(
const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
VectorValues optimize(const Ordering&,
const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
VectorValues optimizeDensely() const;
VectorValues gradient(const VectorValues& x0) const;
virtual VectorValues gradientAtZero() const;
VectorValues optimizeGradientSearch() const;
VectorValues transposeMultiply(const Errors& e) const;
void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
Errors gaussianErrors(const VectorValues& x) const;
Errors operator*(const VectorValues& x) const;
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const;
void multiplyInPlace(const VectorValues& x, Errors& e) const;
void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
void printErrors(
const VectorValues& x,
const std::string& str = "GaussianFactorGraph: ",
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const std::function<bool(const Factor* /*factor*/,
double /*whitenedError*/, size_t /*index*/)>&
printCondition =
[](const Factor*, double, size_t) { return true; }) const;
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
VectorValues GTSAM_DEPRECATED
optimize(boost::none_t, const Eliminate& function =
EliminationTraitsType::DefaultEliminate) const {
return optimize(function);
}
#endif
};
GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);
/****** Linear Algebra Operations ******/
//GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
//GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
template<>
struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
};
} // \ namespace gtsam
FactorGraph.h
Factor Graph Base Class.
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
VectorValues.h
Factor Graph Values.
Errors.h
vector of errors
JacobianFactor.h
gtsam::EliminatePreferCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:548
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::hasConstraints
bool hasConstraints(const GaussianFactorGraph &factors)
Evaluates whether linear factors have any constrained noise models.
Definition GaussianFactorGraph.cpp:442
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
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::Errors
FastList< Vector > Errors
Errors is a vector of errors.
Definition Errors.h:34
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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::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::FactorGraph::isEqual
bool isEqual(const FactorGraph &other) const
Check exact equality of the factor pointers. Useful for derived ==.
Definition FactorGraph.h:134
gtsam::FactorGraph< GaussianFactor >::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Definition FactorGraph.h:186
gtsam::FactorGraph< GaussianFactor >::FactorGraph
FactorGraph()
Definition FactorGraph.h:142
gtsam::FactorGraph< GaussianFactor >::sharedFactor
boost::shared_ptr< GaussianFactor > sharedFactor
Definition FactorGraph.h:101
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition EliminateableFactorGraph.h:36
gtsam::EliminateableFactorGraph
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition EliminateableFactorGraph.h:57
gtsam::EliminateableFactorGraph< GaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
Definition EliminateableFactorGraph.h:89
gtsam::Factor
Definition Factor.h:68
gtsam::Ordering
Definition Ordering.h:34
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition GaussianBayesTree.h:52
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
gtsam::GaussianEliminationTree
Definition GaussianEliminationTree.h:29
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
gtsam::GaussianFactor::clone
virtual GaussianFactor::shared_ptr clone() const =0
Clone a factor (make a deep copy).
gtsam::EliminationTraits< GaussianFactorGraph >::DefaultOrderingFunc
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
The default ordering generation function.
Definition GaussianFactorGraph.h:58
gtsam::EliminationTraits< GaussianFactorGraph >::BayesTreeType
GaussianBayesTree BayesTreeType
Type of Bayes tree.
Definition GaussianFactorGraph.h:51
gtsam::EliminationTraits< GaussianFactorGraph >::ConditionalType
GaussianConditional ConditionalType
Type of conditionals from elimination.
Definition GaussianFactorGraph.h:48
gtsam::EliminationTraits< GaussianFactorGraph >::FactorType
GaussianFactor FactorType
Type of factors in factor graph.
Definition GaussianFactorGraph.h:46
gtsam::EliminationTraits< GaussianFactorGraph >::EliminationTreeType
GaussianEliminationTree EliminationTreeType
Type of elimination tree.
Definition GaussianFactorGraph.h:50
gtsam::EliminationTraits< GaussianFactorGraph >::FactorGraphType
GaussianFactorGraph FactorGraphType
Type of the factor graph (e.g. GaussianFactorGraph).
Definition GaussianFactorGraph.h:47
gtsam::EliminationTraits< GaussianFactorGraph >::BayesNetType
GaussianBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition GaussianFactorGraph.h:49
gtsam::EliminationTraits< GaussianFactorGraph >::JunctionTreeType
GaussianJunctionTree JunctionTreeType
Type of Junction tree.
Definition GaussianFactorGraph.h:52
gtsam::EliminationTraits< GaussianFactorGraph >::DefaultEliminate
static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition GaussianFactorGraph.h:55
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
gtsam::GaussianFactorGraph::negate
GaussianFactorGraph negate() const
Returns the negation of all factors in this graph - corresponds to antifactors.
Definition GaussianFactorGraph.cpp:106
gtsam::GaussianFactorGraph::jacobian
std::pair< Matrix, Vector > jacobian(const Ordering &ordering) const
Return the dense Jacobian and right-hand-side , with the noise models baked into A and b.
Definition GaussianFactorGraph.cpp:232
gtsam::GaussianFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
Typedef to base elimination class.
Definition GaussianFactorGraph.h:80
gtsam::GaussianFactorGraph::augmentedJacobian
Matrix augmentedJacobian(const Ordering &ordering) const
Return a dense Jacobian matrix, augmented with b with the noise models baked into A and b.
Definition GaussianFactorGraph.cpp:217
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
gtsam::GaussianFactorGraph::add
void add(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add an n-ary factor.
Definition GaussianFactorGraph.h:156
gtsam::GaussianFactorGraph::sparseJacobian
std::vector< std::tuple< int, int, double > > sparseJacobian(const Ordering &ordering, size_t &nrows, size_t &ncols) const
Returns a sparse augmented Jacbian matrix as a vector of i, j, and s, where i(k) and j(k) are the bas...
Definition GaussianFactorGraph.cpp:119
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(std::initializer_list< sharedFactor > factors)
Construct from an initializer lists of GaussianFactor shared pointers.
Definition GaussianFactorGraph.h:94
gtsam::GaussianFactorGraph::optimizeGradientSearch
VectorValues optimizeGradientSearch() const
Optimize along the gradient direction, with a closed-form computation to perform the line search.
Definition GaussianFactorGraph.cpp:381
gtsam::GaussianFactorGraph::gradientAtZero
virtual VectorValues gradientAtZero() const
Compute the gradient of the energy function, , centered around zero.
Definition GaussianFactorGraph.cpp:369
gtsam::GaussianFactorGraph::hessianBlockDiagonal
virtual std::map< Key, Matrix > hessianBlockDiagonal() const
Return the block diagonal of the Hessian for this factor.
Definition GaussianFactorGraph.cpp:290
gtsam::GaussianFactorGraph::sparseJacobian_
Matrix sparseJacobian_() const
Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries such that S(i(k),...
Definition GaussianFactorGraph.cpp:199
gtsam::GaussianFactorGraph::cloneToPtr
virtual GaussianFactorGraph::shared_ptr cloneToPtr() const
CloneToPtr() performs a simple assignment to a new graph and returns it.
Definition GaussianFactorGraph.cpp:87
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph()
Default constructor.
Definition GaussianFactorGraph.h:87
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Add a factor by value - makes a copy.
Definition GaussianFactorGraph.h:127
gtsam::GaussianFactorGraph::probPrime
double probPrime(const VectorValues &c) const
Unnormalized probability.
Definition GaussianFactorGraph.cpp:81
gtsam::GaussianFactorGraph::hessian
std::pair< Matrix, Vector > hessian(const Ordering &ordering) const
Return the dense Hessian and information vector , with the noise models baked in.
Definition GaussianFactorGraph.cpp:264
gtsam::GaussianFactorGraph::optimize
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Solve the factor graph by performing multifrontal variable elimination in COLAMD order using the dens...
Definition GaussianFactorGraph.cpp:309
gtsam::GaussianFactorGraph::clone
virtual GaussianFactorGraph clone() const
Clone() performs a deep-copy of the graph, including all of the factors.
Definition GaussianFactorGraph.cpp:94
gtsam::GaussianFactorGraph::multiplyInPlace
void multiplyInPlace(const VectorValues &x, const Errors::iterator &e) const
return A*x ‍/ Errors operator(const VectorValues& x) const;
Definition GaussianFactorGraph.cpp:427
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, Key key3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a ternary factor.
Definition GaussianFactorGraph.h:148
gtsam::GaussianFactorGraph::gaussianErrors
Errors gaussianErrors(const VectorValues &x) const
return A*x-b
Definition GaussianFactorGraph.cpp:512
gtsam::GaussianFactorGraph::add
void add(const sharedFactor &factor)
Add a factor by pointer - stores pointer without copying the factor.
Definition GaussianFactorGraph.h:130
gtsam::GaussianFactorGraph::operator==
friend bool operator==(const GaussianFactorGraph &lhs, const GaussianFactorGraph &rhs)
Check exact equality.
Definition GaussianFactorGraph.h:121
gtsam::GaussianFactorGraph::gradient
VectorValues gradient(const VectorValues &x0) const
Compute the gradient of the energy function, , centered around .
Definition GaussianFactorGraph.cpp:357
gtsam::GaussianFactorGraph::This
GaussianFactorGraph This
Typedef to this class.
Definition GaussianFactorGraph.h:78
gtsam::GaussianFactorGraph::Keys
KeySet Keys
Return the set of variables involved in the factors (computes a set union).
Definition GaussianFactorGraph.h:163
gtsam::GaussianFactorGraph::hessianDiagonal
virtual VectorValues hessianDiagonal() const
Return only the diagonal of the Hessian A'*A, as a VectorValues.
Definition GaussianFactorGraph.cpp:279
gtsam::GaussianFactorGraph::error
double error(const VectorValues &x) const
unnormalized error
Definition GaussianFactorGraph.cpp:71
gtsam::GaussianFactorGraph::add
void add(const Vector &b)
Add a null factor.
Definition GaussianFactorGraph.h:133
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a unary factor.
Definition GaussianFactorGraph.h:137
gtsam::GaussianFactorGraph::~GaussianFactorGraph
virtual ~GaussianFactorGraph()
Virtual destructor.
Definition GaussianFactorGraph.h:110
gtsam::GaussianFactorGraph::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Errors &e, VectorValues &x) const
x += alpha*A'*e
Definition GaussianFactorGraph.cpp:455
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a binary factor.
Definition GaussianFactorGraph.h:142
gtsam::GaussianFactorGraph::augmentedHessian
Matrix augmentedHessian(const Ordering &ordering) const
Return a dense Hessian matrix, augmented with the information vector .
Definition GaussianFactorGraph.cpp:247
gtsam::GaussianFactorGraph::transposeMultiply
VectorValues transposeMultiply(const Errors &e) const
x = A'*e
Definition GaussianFactorGraph.cpp:493
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition GaussianFactorGraph.h:99
gtsam::GaussianFactorGraph::optimizeDensely
VectorValues optimizeDensely() const
Optimize using Eigen's dense Cholesky factorization.
Definition GaussianFactorGraph.cpp:323
gtsam::GaussianFactorGraph::Base
FactorGraph< GaussianFactor > Base
Typedef to base factor graph type.
Definition GaussianFactorGraph.h:79
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition GaussianFactorGraph.h:107
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects).
Definition GaussianFactorGraph.h:103
gtsam::GaussianJunctionTree
A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors sto...
Definition GaussianJunctionTree.h:39
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74