Loading...
Searching...
No Matches
NonlinearFactorGraph.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
* -------------------------------------------------------------------------- */
// \callgraph
#pragma once
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/GraphvizFormatting.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <boost/shared_ptr.hpp>
#include <functional>
namespace gtsam {
// Forward declarations
class Values;
class Ordering;
class GaussianFactorGraph;
class SymbolicFactorGraph;
template<typename T>
class Expression;
template<typename T>
class ExpressionFactor;
class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
public:
typedef FactorGraph<NonlinearFactor> Base;
typedef NonlinearFactorGraph This;
typedef boost::shared_ptr<This> shared_ptr;
NonlinearFactorGraph() {}
template<typename ITERATOR>
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
template<class CONTAINER>
explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {}
template<class DERIVEDFACTOR>
NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
virtual ~NonlinearFactorGraph() {}
void print(
const std::string& str = "NonlinearFactorGraph: ",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
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;
bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
double error(const Values& values) const;
double probPrime(const Values& values) const;
boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
Ordering orderingCOLAMD() const;
Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
typedef std::function<void(const boost::shared_ptr<HessianFactor>& hessianFactor)> Dampen;
boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
const Values& values, const Dampen& dampen = nullptr) const;
boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
Values updateCholesky(const Values& values,
const Dampen& dampen = nullptr) const;
Values updateCholesky(const Values& values, const Ordering& ordering,
const Dampen& dampen = nullptr) const;
NonlinearFactorGraph clone() const;
NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
template<typename T>
void addExpressionFactor(const SharedNoiseModel& R, const T& z,
const Expression<T>& h) {
push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
}
template<typename T>
void addPrior(Key key, const T& prior,
const SharedNoiseModel& model = nullptr) {
emplace_shared<PriorFactor<T>>(key, prior, model);
}
template<typename T>
void addPrior(Key key, const T& prior, const Matrix& covariance) {
emplace_shared<PriorFactor<T>>(key, prior, covariance);
}
using FactorGraph::dot;
using FactorGraph::saveGraph;
void dot(std::ostream& os, const Values& values,
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const GraphvizFormatting& writer = GraphvizFormatting()) const;
std::string dot(
const Values& values,
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const GraphvizFormatting& writer = GraphvizFormatting()) const;
void saveGraph(
const std::string& filename, const Values& values,
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const GraphvizFormatting& writer = GraphvizFormatting()) const;
private:
boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NonlinearFactorGraph",
boost::serialization::base_object<Base>(*this));
}
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
{return linearizeToHessianFactor(values, dampen);}
Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t,
const Dampen& dampen = nullptr) const
{return updateCholesky(values, dampen);}
void GTSAM_DEPRECATED saveGraph(
std::ostream& os, const Values& values = Values(),
const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
dot(os, values, keyFormatter, graphvizFormatting);
}
void GTSAM_DEPRECATED
saveGraph(const std::string& filename, const Values& values,
const GraphvizFormatting& graphvizFormatting,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
saveGraph(filename, values, keyFormatter, graphvizFormatting);
}
#endif
};
template<>
struct traits<NonlinearFactorGraph> : public Testable<NonlinearFactorGraph> {
};
} //\ namespace gtsam
Point2.h
2D Point
FactorGraph.h
Factor Graph Base Class.
NonlinearFactor.h
Non-linear factor base classes.
GraphvizFormatting.h
Graphviz formatter for NonlinearFactorGraph.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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::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::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:205
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::FactorGraph< NonlinearFactor >::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Definition FactorGraph.h:186
gtsam::FactorGraph::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition FactorGraph-inst.h:141
gtsam::FactorGraph< NonlinearFactor >::FactorGraph
FactorGraph()
Definition FactorGraph.h:142
gtsam::FactorGraph< NonlinearFactor >::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Definition FactorGraph.h:192
gtsam::FactorGraph::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition FactorGraph-inst.h:177
gtsam::Factor
Definition Factor.h:68
gtsam::Ordering
Definition Ordering.h:34
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
gtsam::GraphvizFormatting
Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format.
Definition GraphvizFormatting.h:32
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph()
Default constructor.
Definition NonlinearFactorGraph.h:67
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Convenience method which adds a PriorFactor to the factor graph.
Definition NonlinearFactorGraph.h:202
gtsam::NonlinearFactorGraph::rekey
NonlinearFactorGraph rekey(const std::map< Key, Key > &rekey_mapping) const
Rekey() performs a deep-copy of all of the factors, and changes keys according to a mapping.
Definition NonlinearFactorGraph.cpp:384
gtsam::NonlinearFactorGraph::clone
NonlinearFactorGraph clone() const
Clone() performs a deep-copy of the graph, including all of the factors.
Definition NonlinearFactorGraph.cpp:372
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects).
Definition NonlinearFactorGraph.h:75
gtsam::NonlinearFactorGraph::updateCholesky
Values updateCholesky(const Values &values, const Dampen &dampen=nullptr) const
Linearize and solve in one pass.
Definition NonlinearFactorGraph.cpp:353
gtsam::NonlinearFactorGraph::addExpressionFactor
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Directly add ExpressionFactor that implements |h(x)-z|^2_R.
Definition NonlinearFactorGraph.h:190
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const Matrix &covariance)
Convenience method which adds a PriorFactor to the factor graph.
Definition NonlinearFactorGraph.h:218
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition NonlinearFactorGraph.h:71
gtsam::NonlinearFactorGraph::Dampen
std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
Definition NonlinearFactorGraph.h:136
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition NonlinearFactorGraph.h:79
gtsam::NonlinearFactorGraph::~NonlinearFactorGraph
virtual ~NonlinearFactorGraph()
Destructor.
Definition NonlinearFactorGraph.h:82
gtsam::NonlinearFactorGraph::linearizeToHessianFactor
boost::shared_ptr< HessianFactor > linearizeToHessianFactor(const Values &values, const Dampen &dampen=nullptr) const
Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly into a HessianFactor.
Definition NonlinearFactorGraph.cpp:344
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::SymbolicFactorGraph
Symbolic Factor Graph.
Definition SymbolicFactorGraph.h:64
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
PriorFactor.h