88 lines
2.7 KiB
C++
88 lines
2.7 KiB
C++
/**
|
|
* @file NonlinearFactorGraph.h
|
|
* @brief Factor Graph Constsiting of non-linear factors
|
|
* @author Frank Dellaert
|
|
* @author Carlos Nieto
|
|
* @author Christian Potthast
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include <math.h>
|
|
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
|
#include <gtsam/nonlinear/Ordering.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A non-linear factor graph is templated on a values structure, but the factor type
|
|
* is fixed as a NonlinearFactor. The values structures are typically (in SAM) more general
|
|
* than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
|
|
* Linearizing the non-linear factor graph creates a linear factor graph on the
|
|
* tangent vector space at the linearization point. Because the tangent space is a true
|
|
* vector space, the config type will be an VectorValues in that linearized factor graph.
|
|
*/
|
|
template<class Values>
|
|
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Values> > {
|
|
|
|
public:
|
|
|
|
typedef FactorGraph<NonlinearFactor<Values> > Base;
|
|
typedef typename boost::shared_ptr<NonlinearFactor<Values> > sharedFactor;
|
|
|
|
/** print just calls base class */
|
|
void print(const std::string& str = "NonlinearFactorGraph: ") const;
|
|
|
|
/** unnormalized error */
|
|
double error(const Values& c) const;
|
|
|
|
/** all individual errors */
|
|
Vector unwhitenedError(const Values& c) const;
|
|
|
|
/** Unnormalized probability. O(n) */
|
|
double probPrime(const Values& c) const {
|
|
return exp(-0.5 * error(c));
|
|
}
|
|
|
|
template<class F>
|
|
void add(const F& factor) {
|
|
push_back(boost::shared_ptr<F>(new F(factor)));
|
|
}
|
|
|
|
/**
|
|
* Create a symbolic factor graph using an existing ordering
|
|
*/
|
|
SymbolicFactorGraph::shared_ptr symbolic(const Values& config, const Ordering& ordering) const;
|
|
|
|
/**
|
|
* Create a symbolic factor graph and initial variable ordering that can
|
|
* be used for graph operations like determining a fill-reducing ordering.
|
|
* The graph and ordering should be permuted after such a fill-reducing
|
|
* ordering is found.
|
|
*/
|
|
std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
|
|
symbolic(const Values& config) const;
|
|
|
|
/**
|
|
* Compute a fill-reducing ordering using COLAMD. This returns the
|
|
* ordering and a VariableIndex, which can later be re-used to save
|
|
* computation.
|
|
*/
|
|
std::pair<Ordering::shared_ptr, GaussianVariableIndex<>::shared_ptr>
|
|
orderingCOLAMD(const Values& config) const;
|
|
|
|
/**
|
|
* linearize a nonlinear factor graph
|
|
*/
|
|
boost::shared_ptr<GaussianFactorGraph>
|
|
linearize(const Values& config, const Ordering& ordering) const;
|
|
|
|
};
|
|
|
|
} // namespace
|