12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
gtsam/nonlinear/NonlinearFactor.h

483 lines
14 KiB
C++
Raw Blame History

/**
* @file NonlinearFactor.h
* @brief Non-linear factor class
* @author Frank Dellaert
* @author Richard Roberts
*/
// \callgraph
#pragma once
#include <list>
#include <limits>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/base_object.hpp>
#include "Factor.h"
#include "Vector.h"
#include "Matrix.h"
#include "SharedGaussian.h"
#include "GaussianFactor.h"
#define INSTANTIATE_NONLINEAR_FACTOR1(C,J,X) \
template class gtsam::NonlinearFactor1<C,J,X>;
#define INSTANTIATE_NONLINEAR_FACTOR2(C,J1,X1,J2,X2) \
template class gtsam::NonlinearFactor2<C,J1,X1,J2,X2>;
namespace gtsam {
/**
* Nonlinear factor which assumes zero-mean Gaussian noise on the
* on a measurement predicted by a non-linear function h.
*
* Templated on a configuration type. The configurations are typically
* more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups).
*/
template<class Config>
class NonlinearFactor: public Factor<Config> {
protected:
typedef NonlinearFactor<Config> This;
SharedGaussian noiseModel_; /** Noise model */
std::list<Symbol> keys_; /** cached keys */
public:
/** Default constructor for I/O only */
NonlinearFactor() {
}
/**
* Constructor
* @param noiseModel shared pointer to a noise model
*/
NonlinearFactor(const SharedGaussian& noiseModel) :
noiseModel_(noiseModel) {
}
/** print */
void print(const std::string& s = "") const {
std::cout << "NonlinearFactor " << s << std::endl;
noiseModel_->print("noise model");
}
/** Check if two NonlinearFactor objects are equal */
bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const NonlinearFactor<Config>*> (&f);
if (p == NULL) return false;
return noiseModel_->equals(*p->noiseModel_, tol);
}
/**
* calculate the error of the factor
* Override for systems with unusual noise models
*/
virtual double error(const Config& c) const {
return 0.5 * noiseModel_->Mahalanobis(unwhitenedError(c));
}
/** return keys */
std::list<Symbol> keys() const {
return keys_;
}
/** get the dimension of the factor (number of rows on linearization) */
size_t dim() const {
return noiseModel_->dim();
}
/* return the begin iterator of keys */
std::list<Symbol>::const_iterator begin() const { return keys_.begin(); }
/* return the end iterator of keys */
std::list<Symbol>::const_iterator end() const { return keys_.end(); }
/** access to the noise model */
SharedGaussian get_noiseModel() const {
return noiseModel_;
}
/** get the size of the factor */
std::size_t size() const {
return keys_.size();
}
/** Vector of errors, unwhitened ! */
virtual Vector unwhitenedError(const Config& c) const = 0;
/** Vector of errors, whitened ! */
Vector whitenedError(const Config& c) const {
return noiseModel_->whiten(unwhitenedError(c));
}
/** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor>
linearize(const Config& c) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
// TODO NoiseModel
}
}; // NonlinearFactor
/**
* A Gaussian nonlinear factor that takes 1 parameter
* implementing the density P(z|x) \propto exp -0.5*|z-h(x)|^2_C
* Templated on the parameter type X and the configuration Config
* There is no return type specified for h(x). Instead, we require
* the derived class implements error_vector(c) = h(x)-z \approx Ax-b
* This allows a graph to have factors with measurements of mixed type.
*/
template<class Config, class Key, class X>
class NonlinearFactor1: public NonlinearFactor<Config> {
protected:
// The value of the key. Not const to allow serialization
Key key_;
typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor1<Config, Key, X> This;
public:
/** Default constructor for I/O only */
NonlinearFactor1() {
}
inline const Key& key() const {
return key_;
}
/**
* Constructor
* @param z measurement
* @param key by which to look up X value in Config
*/
NonlinearFactor1(const SharedGaussian& noiseModel,
const Key& key1) :
Base(noiseModel), key_(key1) {
this->keys_.push_back(key_);
}
/* print */
void print(const std::string& s = "") const {
std::cout << "NonlinearFactor1 " << s << std::endl;
std::cout << "key: " << (std::string) key_ << std::endl;
Base::print("parent");
}
/** Check if two factors are equal. Note type is Factor and needs cast. */
bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false;
return Base::equals(*p, tol) && (key_ == p->key_);
}
/** error function h(x)-z, unwhitened !!! */
inline Vector unwhitenedError(const Config& x) const {
const Key& j = key_;
const X& xj = x[j];
return evaluateError(xj);
}
/**
* Linearize a non-linearFactor1 to get a GaussianFactor
* Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z
* Hence b = z - h(x0) = - error_vector(x)
*/
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& x) const {
const X& xj = x[key_];
Matrix A;
Vector b = - evaluateError(xj, A);
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b, constrained));
}
this->noiseModel_->WhitenInPlace(A);
this->noiseModel_->whitenInPlace(b);
return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b,
noiseModel::Unit::Create(b.size())));
}
/*
* Override this method to finish implementing a unary factor.
* If the optional Matrix reference argument is specified, it should compute
* both the function evaluation and its derivative in X.
*/
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key_);
}
};
/**
* A Gaussian nonlinear factor that takes 2 parameters
* Note: cannot be serialized as contains function pointers
* Specialized derived classes could do this
*/
template<class Config, class Key1, class X1, class Key2, class X2>
class NonlinearFactor2: public NonlinearFactor<Config> {
protected:
// The values of the keys. Not const to allow serialization
Key1 key1_;
Key2 key2_;
typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor2<Config, Key1, X1, Key2, X2> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearFactor2() {
}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor2(const SharedGaussian& noiseModel, Key1 j1,
Key2 j2) :
Base(noiseModel), key1_(j1), key2_(j2) {
this->keys_.push_back(key1_);
this->keys_.push_back(key2_);
}
/** Print */
void print(const std::string& s = "") const {
std::cout << "NonlinearFactor2 " << s << std::endl;
std::cout << "key1: " << (std::string) key1_ << std::endl;
std::cout << "key2: " << (std::string) key2_ << std::endl;
Base::print("parent");
}
/** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false;
return Base::equals(*p, tol) && (key1_ == p->key1_)
&& (key2_ == p->key2_);
}
/** error function z-h(x1,x2) */
inline Vector unwhitenedError(const Config& x) const {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
return evaluateError(x1, x2);
}
/**
* Linearize a non-linearFactor2 to get a GaussianFactor
* Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
* Hence b = z - h(x1,x2) = - error_vector(x)
*/
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
const X1& x1 = c[key1_];
const X2& x2 = c[key2_];
Matrix A1, A2;
Vector b = -evaluateError(x1, x2, A1, A2);
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_,
A2, b, constrained));
}
this->noiseModel_->WhitenInPlace(A1);
this->noiseModel_->WhitenInPlace(A2);
this->noiseModel_->whitenInPlace(b);
return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_,
A2, b, noiseModel::Unit::Create(b.size())));
}
/** methods to retrieve both keys */
inline const Key1& key1() const {
return key1_;
}
inline const Key2& key2() const {
return key2_;
}
/*
* Override this method to finish implementing a binary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2).
*/
virtual Vector
evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
}
};
/* ************************************************************************* */
/**
* A Gaussian nonlinear factor that takes 3 parameters
* Note: cannot be serialized as contains function pointers
* Specialized derived classes could do this
*/
template<class Config, class Key1, class X1, class Key2, class X2, class Key3, class X3>
class NonlinearFactor3: public NonlinearFactor<Config> {
protected:
// The values of the keys. Not const to allow serialization
Key1 key1_;
Key2 key2_;
Key3 key3_;
typedef NonlinearFactor<Config> Base;
typedef NonlinearFactor3<Config, Key1, X1, Key2, X2, Key3, X3> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearFactor3() {
}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
* @param j3 key of the third variable
*/
NonlinearFactor3(const SharedGaussian& noiseModel, Key1 j1, Key2 j2, Key3 j3) :
Base(noiseModel), key1_(j1), key2_(j2), key3_(j3) {
this->keys_.push_back(key1_);
this->keys_.push_back(key2_);
this->keys_.push_back(key3_);
}
/** Print */
void print(const std::string& s = "") const {
std::cout << "NonlinearFactor2 " << s << std::endl;
std::cout << "key1: " << (std::string) key1_ << std::endl;
std::cout << "key2: " << (std::string) key2_ << std::endl;
std::cout << "key3: " << (std::string) key3_ << std::endl;
Base::print("parent");
}
/** Check if two factors are equal */
bool equals(const Factor<Config>& f, double tol = 1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false;
return Base::equals(*p, tol) && (key1_ == p->key1_)
&& (key2_ == p->key2_) && (key3_ == p->key3_);
}
/** error function z-h(x1,x2) */
inline Vector unwhitenedError(const Config& x) const {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
const X3& x3 = x[key3_];
return evaluateError(x1, x2, x3);
}
/**
* Linearize a non-linearFactor2 to get a GaussianFactor
* Ax-b \approx h(x1+dx1,x2+dx2,x3+dx3)-z = h(x1,x2,x3) + A2*dx1 + A2*dx2 + A3*dx3 - z
* Hence b = z - h(x1,x2,x3) = - error_vector(x)
*/
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
const X1& x1 = c[key1_];
const X2& x2 = c[key2_];
const X2& x3 = c[key3_];
Matrix A1, A2, A3;
Vector b = -evaluateError(x1, x2, x3, A1, A2, A3);
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
return GaussianFactor::shared_ptr(
new GaussianFactor(key1_, A1, key2_, A2, key3_, A3, b, constrained));
}
this->noiseModel_->WhitenInPlace(A1);
this->noiseModel_->WhitenInPlace(A2);
this->noiseModel_->WhitenInPlace(A3);
this->noiseModel_->whitenInPlace(b);
return GaussianFactor::shared_ptr(
new GaussianFactor(key1_, A1, key2_, A2, key3_, A3, b, noiseModel::Unit::Create(b.size())));
}
/** methods to retrieve keys */
inline const Key1& key1() const {
return key1_;
}
inline const Key2& key2() const {
return key2_;
}
inline const Key3& key3() const {
return key3_;
}
/*
* Override this method to finish implementing a trinary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3).
*/
virtual Vector
evaluateError(const X1&, const X2&, const X3&,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<NonlinearFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
}
};
/* ************************************************************************* */
}
Reference in New Issue View Git Blame Copy Permalink