60 lines
1.5 KiB
C++
60 lines
1.5 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 "NonlinearFactor.h"
|
|
#include "GaussianFactorGraph.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A non-linear factor graph is templated on a configuration, but the factor type
|
|
* is fixed as a NonlinearFactor. The configurations 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 VectorConfig in that linearized
|
|
*/
|
|
template<class Config>
|
|
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<Config> > {
|
|
|
|
public:
|
|
|
|
typedef typename boost::shared_ptr<NonlinearFactor<Config> > sharedFactor;
|
|
|
|
/** unnormalized error */
|
|
double error(const Config& c) const;
|
|
|
|
/** all individual errors */
|
|
Vector unwhitenedError(const Config& c) const;
|
|
|
|
/** Unnormalized probability. O(n) */
|
|
double probPrime(const Config& 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)));
|
|
}
|
|
|
|
/**
|
|
* linearize a nonlinear factor graph
|
|
*/
|
|
boost::shared_ptr<GaussianFactorGraph>
|
|
linearize(const Config& config) const;
|
|
|
|
};
|
|
|
|
} // namespace
|