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

Implemented N-way factor base class in NoiseModelFactor, added NonlinearFactor{3-6}, adapted NonlinearFactor1,2,3 and NonlinearConstraint1,2,3 to derive from NoiseModelFactor in a minimal way

release/4.3a0
Richard Roberts 2011-10-03 04:24:24 +00:00
parent 5bd4680100
commit af3c12a7df
8 changed files with 849 additions and 794 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/linear/NoiseModel.cpp
View File

@ -163,6 +163,11 @@ SharedDiagonal Gaussian::Cholesky(Matrix& Ab, size_t nFrontals) const {
return Unit::Create(maxrank);
}
void Gaussian::WhitenSystem(vector<Matrix>& A, Vector& b) const {
BOOST_FOREACH(Matrix& Aj, A) { WhitenInPlace(Aj); }
whitenInPlace(b);
}
void Gaussian::WhitenSystem(Matrix& A, Vector& b) const {
WhitenInPlace(A);
whitenInPlace(b);
@ -460,6 +465,24 @@ Vector Base::sqrtWeight(const Vector &error) const {
/** The following three functions reweight block matrices and a vector
* according to their weight implementation */
/** Reweight n block matrices with one error vector */
void Base::reweight(vector<Matrix> &A, Vector &error) const {
if ( reweight_ == Block ) {
const double w = sqrtWeight(error.norm());
BOOST_FOREACH(Matrix& Aj, A) {
Aj *= w;
}
error *= w;
}
else {
const Vector W = sqrtWeight(error);
BOOST_FOREACH(Matrix& Aj, A) {
vector_scale_inplace(W,Aj);
}
error = emul(W, error);
}
}
/** Reweight one block matrix with one error vector */
void Base::reweight(Matrix &A, Vector &error) const {
if ( reweight_ == Block ) {
@ -592,6 +615,11 @@ bool Robust::equals(const Base& expected, double tol) const {
return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
}
void Robust::WhitenSystem(vector<Matrix>& A, Vector& b) const {
noise_->WhitenSystem(A,b);
robust_->reweight(A,b);
}
void Robust::WhitenSystem(Matrix& A, Vector& b) const {
noise_->WhitenSystem(A,b);
robust_->reweight(A,b);

4
gtsam/linear/NoiseModel.h
View File

@ -77,6 +77,7 @@ namespace gtsam {
virtual double distance(const Vector& v) const = 0;
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
@ -185,6 +186,7 @@ namespace gtsam {
/**
* Whiten a system, in place as well
*/
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const ;
virtual void WhitenSystem(Matrix& A, Vector& b) const ;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
@ -554,6 +556,7 @@ namespace gtsam {
Vector sqrtWeight(const Vector &error) const;
/** reweight block matrices and a vector according to their weight implementation */
void reweight(std::vector<Matrix> &A, Vector &error) const;
void reweight(Matrix &A, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
@ -642,6 +645,7 @@ namespace gtsam {
// TODO: these are really robust iterated re-weighting support functions
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
virtual void WhitenSystem(Matrix& A, Vector& b) const;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;

275
gtsam/nonlinear/NonlinearConstraint.h
View File

@ -51,36 +51,41 @@ public:
/** Constructor - sets the cost function and the lagrange multipliers
* @param dim is the dimension of the factor
* @param keys is a boost::tuple containing the keys, e.g. \c make_tuple(key1,key2,key3)
* @param mu is the gain used at error evaluation (forced to be positive)
*/
NonlinearConstraint(size_t dim, double mu = 1000.0):
Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)) {}
template<class TUPLE>
NonlinearConstraint(const TUPLE& keys, size_t dim, double mu = 1000.0):
Base(noiseModel::Constrained::All(dim), keys), mu_(fabs(mu)) {}
virtual ~NonlinearConstraint() {}
/** returns the gain mu */
double mu() const { return mu_; }
/** Print */
virtual void print(const std::string& s = "") const=0;
virtual void print(const std::string& s = "") const {
std::cout << "NonlinearConstraint " << s << std::endl;
std::cout << " ";
BOOST_FOREACH(const Symbol& key, this->keys()) { std::cout << (std::string)key << " "; }
std::cout << "\n";
std::cout << "mu: " << this->mu_ << std::endl;
}
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor<VALUES>& f, double tol=1e-9) const {
const This* p = dynamic_cast<const This*> (&f);
if (p == NULL) return false;
return Base::equals(*p, tol) && (mu_ == p->mu_);
return Base::equals(*p, tol) && (fabs(mu_ - p->mu_) <= tol);
}
/** error function - returns the quadratic merit function */
virtual double error(const VALUES& c) const {
const Vector error_vector = unwhitenedError(c);
if (active(c))
return mu_ * error_vector.dot(error_vector);
else return 0.0;
return mu_ * unwhitenedError(c).squaredNorm();
else
return 0.0;
}
/** Raw error vector function g(x) */
virtual Vector unwhitenedError(const VALUES& c) const = 0;
/**
* active set check, defines what type of constraint this is
*
@ -95,7 +100,12 @@ public:
* @param config is the values structure
* @return a combined linear factor containing both the constraint and the constraint factor
*/
virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const=0;
virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
if (!active(c))
return boost::shared_ptr<JacobianFactor>();
else
return Base::linearize(c, ordering);
}
private:
@ -138,60 +148,31 @@ public:
* @param mu is the gain for the factor
*/
NonlinearConstraint1(const KEY& key, size_t dim, double mu = 1000.0)
: Base(dim, mu), key_(key) {
this->keys_.push_back(key);
}
: Base(make_tuple(key), dim, mu), key_(key) { }
virtual ~NonlinearConstraint1() {}
/* print */
void print(const std::string& s = "") const {
std::cout << "NonlinearConstraint1 " << s << std::endl;
std::cout << "key: " << (std::string) key_ << std::endl;
std::cout << "mu: " << this->mu_ << std::endl;
/** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
const X& x1 = x[key_];
if(H) {
return evaluateError(x1, (*H)[0]);
} else {
return evaluateError(x1);
}
/** Check if two factors are equal. Note type is Factor and needs cast. */
virtual bool equals(const NonlinearFactor<VALUES>& 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 g(x), switched depending on whether the constraint is active */
inline Vector unwhitenedError(const VALUES& x) const {
if (!active(x)) {
} else {
return zero(this->dim());
}
const KEY& j = key_;
const X& xj = x[j];
return evaluateError(xj);
}
/** Linearize from config */
boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
if (!active(x)) {
boost::shared_ptr<JacobianFactor> factor;
return factor;
}
const X& xj = x[key_];
Matrix A;
Vector b = - evaluateError(xj, A);
Index var = ordering[key_];
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b, model));
}
/** g(x) with optional derivative - does not depend on active */
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
* 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 IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
return IndexFactor::shared_ptr(new IndexFactor(ordering[key_]));
}
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
private:
@ -272,73 +253,33 @@ public:
* @param mu is the gain for the factor
*/
NonlinearConstraint2(const KEY1& key1, const KEY2& key2, size_t dim, double mu = 1000.0) :
Base(dim, mu), key1_(key1), key2_(key2) {
this->keys_.push_back(key1);
this->keys_.push_back(key2);
}
Base(make_tuple(key1, key2), dim, mu), key1_(key1), key2_(key2) { }
virtual ~NonlinearConstraint2() {}
/* print */
void print(const std::string& s = "") const {
std::cout << "NonlinearConstraint2 " << s << std::endl;
std::cout << "key1: " << (std::string) key1_ << std::endl;
std::cout << "key2: " << (std::string) key2_ << std::endl;
std::cout << "mu: " << this->mu_ << std::endl;
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
if(H) {
return evaluateError(x1, x2, (*H)[0], (*H)[1]);
} else {
return evaluateError(x1, x2);
}
/** Check if two factors are equal. Note type is Factor and needs cast. */
virtual bool equals(const NonlinearFactor<VALUES>& 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 g(x), switched depending on whether the constraint is active */
inline Vector unwhitenedError(const VALUES& x) const {
if (!active(x)) {
} else {
return zero(this->dim());
}
const KEY1& j1 = key1_;
const KEY2& j2 = key2_;
const X1& xj1 = x[j1];
const X2& xj2 = x[j2];
return evaluateError(xj1, xj2);
}
/** Linearize from config */
boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
if (!active(c)) {
boost::shared_ptr<JacobianFactor> factor;
return factor;
}
const KEY1& j1 = key1_; const KEY2& j2 = key2_;
const X1& x1 = c[j1]; const X2& x2 = c[j2];
Matrix grad1, grad2;
Vector g = -1.0 * evaluateError(x1, x2, grad1, grad2);
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
Index var1 = ordering[j1], var2 = ordering[j2];
if (var1 < var2)
return GaussianFactor::shared_ptr(new JacobianFactor(var1, grad1, var2, grad2, g, model));
else
return GaussianFactor::shared_ptr(new JacobianFactor(var2, grad2, var1, grad1, g, model));
}
/** g(x) with optional derivative2 - does not depend on active */
virtual Vector evaluateError(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
* 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 IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index var1 = ordering[key1_], var2 = ordering[key2_];
if(var1 < var2)
return IndexFactor::shared_ptr(new IndexFactor(var1, var2));
else
return IndexFactor::shared_ptr(new IndexFactor(var2, var1));
}
virtual Vector
evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0;
private:
@ -424,103 +365,37 @@ public:
*/
NonlinearConstraint3(const KEY1& key1, const KEY2& key2, const KEY3& key3,
size_t dim, double mu = 1000.0) :
Base(dim, mu), key1_(key1), key2_(key2), key3_(key3) {
this->keys_.push_back(key1);
this->keys_.push_back(key2);
this->keys_.push_back(key3);
}
Base(make_tuple(key1, key2, key3), dim, mu), key1_(key1), key2_(key2), key3_(key3) { }
virtual ~NonlinearConstraint3() {}
/* print */
void print(const std::string& s = "") const {
std::cout << "NonlinearConstraint3 " << 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;
std::cout << "mu: " << this->mu_ << std::endl;
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
const X3& x3 = x[key3_];
if(H) {
return evaluateError(x1, x2, x3, (*H)[0], (*H)[1], (*H)[2]);
} else {
return evaluateError(x1, x2, x3);
}
/** Check if two factors are equal. Note type is Factor and needs cast. */
virtual bool equals(const NonlinearFactor<VALUES>& 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 g(x), switched depending on whether the constraint is active */
inline Vector unwhitenedError(const VALUES& x) const {
if (!active(x)) {
} else {
return zero(this->dim());
}
const KEY1& j1 = key1_;
const KEY2& j2 = key2_;
const KEY3& j3 = key3_;
const X1& xj1 = x[j1];
const X2& xj2 = x[j2];
const X3& xj3 = x[j3];
return evaluateError(xj1, xj2, xj3);
}
/** Linearize from config */
boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
if (!active(c)) {
boost::shared_ptr<JacobianFactor> factor;
return factor;
}
const KEY1& j1 = key1_; const KEY2& j2 = key2_; const KEY3& j3 = key3_;
const X1& x1 = c[j1]; const X2& x2 = c[j2]; const X3& x3 = c[j3];
Matrix A1, A2, A3;
Vector b = -1.0 * evaluateError(x1, x2, x3, A1, A2, A3);
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
Index var1 = ordering[j1], var2 = ordering[j2], var3 = ordering[j3];
// perform sorting
if(var1 < var2 && var2 < var3)
return GaussianFactor::shared_ptr(
new JacobianFactor(var1, A1, var2, A2, var3, A3, b, model));
else if(var2 < var1 && var1 < var3)
return GaussianFactor::shared_ptr(
new JacobianFactor(var2, A2, var1, A1, var3, A3, b, model));
else if(var1 < var3 && var3 < var2)
return GaussianFactor::shared_ptr(
new JacobianFactor(var1, A1, var3, A3, var2, A2, b, model));
else if(var2 < var3 && var3 < var1)
return GaussianFactor::shared_ptr(
new JacobianFactor(var2, A2, var3, A3, var1, A1, b, model));
else if(var3 < var1 && var1 < var2)
return GaussianFactor::shared_ptr(
new JacobianFactor(var3, A3, var1, A1, var2, A2, b, model));
else
return GaussianFactor::shared_ptr(
new JacobianFactor(var3, A3, var2, A2, var1, A1, b, model));
}
/** g(x) with optional derivative3 - does not depend on active */
virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3,
/**
* 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;
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
if(var1 < var2 && var2 < var3)
return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key2_], ordering[key3_]));
else if(var2 < var1 && var1 < var3)
return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key1_], ordering[key3_]));
else if(var1 < var3 && var3 < var2)
return IndexFactor::shared_ptr(new IndexFactor(ordering[key1_], ordering[key3_], ordering[key2_]));
else if(var2 < var3 && var3 < var1)
return IndexFactor::shared_ptr(new IndexFactor(ordering[key2_], ordering[key3_], ordering[key1_]));
else if(var3 < var1 && var1 < var2)
return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key1_], ordering[key2_]));
else
return IndexFactor::shared_ptr(new IndexFactor(ordering[key3_], ordering[key2_], ordering[key1_]));
}
private:
/** Serialization function */

695
gtsam/nonlinear/NonlinearFactor.h
View File

@ -24,6 +24,7 @@
#include <limits>
#include <boost/serialization/base_object.hpp>
#include <boost/tuple/tuple.hpp>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexFactor.h>
@ -34,6 +35,9 @@
namespace gtsam {
using boost::make_tuple;
/* ************************************************************************* */
/**
* Nonlinear factor base class
*
@ -61,42 +65,11 @@ namespace gtsam {
/** Destructor */
virtual ~NonlinearFactor() {}
/**
* Constructor
* @param key1 by which to look up X value in Values
*/
NonlinearFactor(const Symbol& key1) :
Factor<Symbol>(key1) {
}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor(const Symbol& j1, const Symbol& j2) :
Factor<Symbol>(j1,j2) {
}
/**
* Constructor - arbitrary number of keys
* @param keys is the set of Symbols in the factor
*/
NonlinearFactor(const std::set<Symbol>& keys) :
Factor<Symbol>(keys) {
}
/** print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor\n";
}
/**
* Vector of errors, unwhitened
* This is the raw error i.e. (h(x)-z) in case of NoiseModelFactor derived class
*/
virtual Vector unwhitenedError(const VALUES& c) const = 0;
/**
* Calculate the error of the factor
* This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian.
@ -120,9 +93,26 @@ namespace gtsam {
}; // \class NonlinearFactor
// Helper function to fill a vector from a tuple function of any length
template<typename CONS>
inline void __fill_from_tuple(std::vector<Symbol>& vector, size_t position, const CONS& tuple) {
vector[position] = tuple.get_head();
__fill_from_tuple<typename CONS::tail_type>(vector, position+1, tuple.get_tail());
}
template<>
inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vector, size_t position, const boost::tuples::null_type& tuple) {
// Do nothing
}
/* ************************************************************************* */
/**
* Nonlinear factor which assumes a zero-mean noise model
* on a measurement predicted by a non-linear function h.
* A nonlinear sum-of-squares factor with a zero-mean noise model
* implementing the density \f$ P(z|x) \propto exp -0.5*|z-h(x)|^2_C \f$
* Templated on the parameter type X and the values structure Values
* There is no return type specified for h(x). Instead, we require
* the derived class implements \f$ \mathtt{error\_vector}(x) = h(x)-z \approx A \delta x - b \f$
* This allows a graph to have factors with measurements of mixed type.
* The noise model is typically Gaussian, but robust error models are also supported.
*/
template<class VALUES>
@ -149,47 +139,39 @@ namespace gtsam {
/**
* Constructor
* @param noiseModel shared pointer to a noise model
* @param keys A boost::tuple containing the variables involved in this factor,
* example: <tt>NoiseModelFactor(noiseModel, make_tuple(symbol1, symbol2, symbol3)</tt>
*/
NoiseModelFactor(const SharedNoiseModel& noiseModel) :
template<class U1, class U2>
NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons<U1,U2>& keys) :
noiseModel_(noiseModel) {
this->keys_.resize(boost::tuples::length<boost::tuples::cons<U1,U2> >::value);
// Use helper function to fill key vector, using 'cons' representation of tuple
__fill_from_tuple(this->keys(), 0, keys);
}
/**
* Constructor
* @param z measurement
* @param key by which to look up X value in Values
* @param keys The variables involved in this factor
*/
NoiseModelFactor(const SharedNoiseModel& noiseModel, const Symbol& key1) :
Base(key1), noiseModel_(noiseModel) {
template<class ITERATOR>
NoiseModelFactor(const SharedNoiseModel& noiseModel, ITERATOR beginKeys, ITERATOR endKeys) :
Base(noiseModel) {
this->keys_.insert(this->keys_.end(), beginKeys, endKeys);
}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NoiseModelFactor(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) :
Base(j1,j2), noiseModel_(noiseModel) {
}
/**
* Constructor - arbitrary number of keys
* @param keys is the set of Symbols in the factor
*/
NoiseModelFactor(const SharedNoiseModel& noiseModel, const std::set<Symbol>& keys) :
Base(keys), noiseModel_(noiseModel) {
}
/** print */
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NoiseModelFactor\n";
noiseModel_->print(" noise model");
std::cout << " ";
BOOST_FOREACH(const Symbol& key, this->keys()) { std::cout << (std::string)key << " "; }
std::cout << "\n";
this->noiseModel_->print(" noise model: ");
}
/** Check if two NoiseModelFactor objects are equal */
/** Check if two factors are equal */
virtual bool equals(const NoiseModelFactor<VALUES>& f, double tol = 1e-9) const {
return noiseModel_->equals(*f.noiseModel_, tol);
return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol);
}
/** get the dimension of the factor (number of rows on linearization) */
@ -202,9 +184,17 @@ namespace gtsam {
return noiseModel_;
}
/**
* Error function *without* the NoiseModel, \f$ z-h(x) \f$.
* Override this method to finish implementing an N-way 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 unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
/**
* Vector of errors, whitened
* This is the raw error, i.e., i.e. (h(x)-z)/sigma in case of a Gaussian
* This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian
*/
Vector whitenedError(const VALUES& c) const {
return noiseModel_->whiten(unwhitenedError(c));
@ -212,21 +202,66 @@ namespace gtsam {
/**
* Calculate the error of the factor.
* This is the log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian.
* In this class, we take the raw prediction error h(x)-z, ask the noise model
* to transform it to (h(x)-z)^2/sigma^2, and then multiply by 0.5.
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
virtual double error(const VALUES& c) const {
return 0.5 * noiseModel_->distance(unwhitenedError(c));
}
/**
* Linearize a non-linearFactorN to get a GaussianFactor,
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
// Create the set of terms - Jacobians for each index
Vector b;
// Call evaluate error to get Jacobians and b vector
std::vector<Matrix> A(this->size());
b = -unwhitenedError(x, A);
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if(!constrained)
this->noiseModel_->WhitenSystem(A,b);
std::vector<std::pair<Index, Matrix> > terms(this->size());
// Fill in terms
for(size_t j=0; j<this->size(); ++j) {
terms[j].first = ordering[this->keys()[j]];
terms[j].second.swap(A[j]);
}
if(constrained)
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b, constrained));
else
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b, noiseModel::Unit::Create(b.size())));
}
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
std::vector<Index> indices(this->size());
for(size_t j=0; j<this->size(); ++j)
indices[j] = ordering[this->keys()[j]];
return IndexFactor::shared_ptr(new IndexFactor(indices));
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Factor",
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(noiseModel_);
}
@ -234,14 +269,9 @@ namespace gtsam {
}; // \class NoiseModelFactor
/**
* 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 values structure Values
* 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.
*/
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY>
class NonlinearFactor1: public NoiseModelFactor<VALUES> {
@ -261,14 +291,11 @@ namespace gtsam {
public:
/** Default constructor for I/O only */
NonlinearFactor1() {
}
NonlinearFactor1() {}
virtual ~NonlinearFactor1() {}
inline const KEY& key() const {
return key_;
}
inline const KEY& key() const { return key_; }
/**
* Constructor
@ -276,56 +303,19 @@ namespace gtsam {
* @param key by which to look up X value in Values
*/
NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) :
Base(noiseModel,key1), key_(key1) {
Base(noiseModel, make_tuple(key1)), key_(key1) {
}
/* print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor1\n";
std::cout << " key: " << (std::string) key_ << std::endl;
this->noiseModel_->print(" noise model: ");
}
/** Check if two factors are equal. Note type is IndexFactor and needs cast. */
virtual bool equals(const NonlinearFactor1<VALUES,KEY>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key_ == f.key_);
}
/** error function h(x)-z, unwhitened !!! */
inline Vector unwhitenedError(const VALUES& x) const {
const X& xj = x[key_];
return evaluateError(xj);
/** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key_], (*H)[0]);
else
return evaluateError(x[key_]);
}
/**
* 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 VALUES& x, const Ordering& ordering) const {
const X& xj = x[key_];
Matrix A;
Vector b = - evaluateError(xj, A);
Index var = ordering[key_];
// 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 JacobianFactor(var, A, b, constrained));
this->noiseModel_->WhitenSystem(A,b);
return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b,
noiseModel::Unit::Create(b.size())));
}
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
return IndexFactor::shared_ptr(new IndexFactor(ordering[key_]));
}
/*
* 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.
@ -343,12 +333,12 @@ namespace gtsam {
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key_);
}
};// \class NonlinearFactor1
/**
* A Gaussian nonlinear factor that takes 2 parameters
*/
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 2
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2>
class NonlinearFactor2: public NoiseModelFactor<VALUES> {
@ -372,82 +362,32 @@ namespace gtsam {
/**
* Default Constructor for I/O
*/
NonlinearFactor2() {
}
NonlinearFactor2() {}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor2(const SharedNoiseModel& noiseModel, KEY1 j1, KEY2 j2) :
Base(noiseModel,j1,j2), key1_(j1), key2_(j2) {
}
NonlinearFactor2(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2) :
Base(noiseModel, make_tuple(j1,j2)), key1_(j1), key2_(j2) {}
virtual ~NonlinearFactor2() {}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor2\n";
std::cout << " key1: " << (std::string) key1_ << "\n";
std::cout << " key2: " << (std::string) key2_ << "\n";
this->noiseModel_->print(" noise model: ");
}
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor2<VALUES,KEY1,KEY2>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_)
&& (key2_ == f.key2_);
}
/** error function z-h(x1,x2) */
inline Vector unwhitenedError(const VALUES& 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 VALUES& c, const Ordering& ordering) const {
const X1& x1 = c[key1_];
const X2& x2 = c[key2_];
Matrix A1, A2;
Vector b = -evaluateError(x1, x2, A1, A2);
const Index var1 = ordering[key1_], var2 = ordering[key2_];
// TODO pass unwhitened + noise model to Gaussian factor
SharedDiagonal constrained =
boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2,
A2, b, constrained));
}
this->noiseModel_->WhitenSystem(A1,A2,b);
return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2,
A2, b, noiseModel::Unit::Create(b.size())));
}
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index var1 = ordering[key1_], var2 = ordering[key2_];
return IndexFactor::shared_ptr(new IndexFactor(var1, var2));
}
/** methods to retrieve both keys */
inline const KEY1& key1() const {
return key1_;
}
inline const KEY2& key2() const {
return key2_;
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key1_], x[key2_], (*H)[0], (*H)[1]);
else
return evaluateError(x[key1_], x[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).
@ -467,14 +407,11 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
}
}; // \class NonlinearFactor2
/* ************************************************************************* */
/**
* A Gaussian nonlinear factor that takes 3 parameters
*/
/** A convenient base class for creating your own NoiseModelFactor with 3
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3>
class NonlinearFactor3: public NoiseModelFactor<VALUES> {
@ -500,8 +437,7 @@ namespace gtsam {
/**
* Default Constructor for I/O
*/
NonlinearFactor3() {
}
NonlinearFactor3() {}
/**
* Constructor
@ -509,84 +445,26 @@ namespace gtsam {
* @param j2 key of the second variable
* @param j3 key of the third variable
*/
NonlinearFactor3(const SharedNoiseModel& noiseModel, KEY1 j1, KEY2 j2, KEY3 j3) :
Base(noiseModel), key1_(j1), key2_(j2), key3_(j3) {
this->keys_.reserve(3);
this->keys_.push_back(key1_);
this->keys_.push_back(key2_);
this->keys_.push_back(key3_);
}
NonlinearFactor3(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3) :
Base(noiseModel, make_tuple(j1,j2,j3)), key1_(j1), key2_(j2), key3_(j3) {}
virtual ~NonlinearFactor3() {}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor3\n";
std::cout << " key1: " << (std::string) key1_ << "\n";
std::cout << " key2: " << (std::string) key2_ << "\n";
std::cout << " key3: " << (std::string) key3_ << "\n";
this->noiseModel_->print(" noise model: ");
}
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor3<VALUES,KEY1,KEY2,KEY3>& f, double tol = 1e-9) const {
return Base::noiseModel_->equals(*f.noiseModel_, tol) && (key1_ == f.key1_)
&& (key2_ == f.key2_) && (key3_ == f.key3_);
}
/** error function z-h(x1,x2) */
inline Vector unwhitenedError(const VALUES& 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 VALUES& c, const Ordering& ordering) const {
const X1& x1 = c[key1_];
const X2& x2 = c[key2_];
const X3& x3 = c[key3_];
Matrix A1, A2, A3;
Vector b = -evaluateError(x1, x2, x3, A1, A2, A3);
const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
// 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 JacobianFactor(var1, A1, var2, A2, var3, A3, b, constrained));
}
this->noiseModel_->WhitenSystem(A1,A2,A3,b);
return GaussianFactor::shared_ptr(
new JacobianFactor(var1, A1, var2, A2, var3, A3, b, noiseModel::Unit::Create(b.size())));
}
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
return IndexFactor::shared_ptr(new IndexFactor(var1, var2, var3));
}
/** 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_;
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline const KEY3& key3() const { return key3_; }
/** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
else
return evaluateError(x[key1_], x[key2_], x[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).
@ -609,9 +487,284 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
}
}; // \class NonlinearFactor3
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 4
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4>
class NonlinearFactor4: public NoiseModelFactor<VALUES> {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef typename KEY4::Value X4;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor4<VALUES, KEY1, KEY2, KEY3, KEY4> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearFactor4() {}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
* @param j3 key of the third variable
* @param j4 key of the fourth variable
*/
NonlinearFactor4(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4) :
Base(noiseModel, make_tuple(j1,j2,j3,j4)), key1_(j1), key2_(j2), key3_(j3), key4_(j4) {}
virtual ~NonlinearFactor4() {}
/** 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_; }
inline const KEY4& key4() const { return key4_; }
/** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]);
}
/**
* Override this method to finish implementing a 4-way 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&, const X4&,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = 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("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
}
}; // \class NonlinearFactor4
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 5
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
class NonlinearFactor5: public NoiseModelFactor<VALUES> {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef typename KEY4::Value X4;
typedef typename KEY5::Value X5;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
KEY5 key5_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor5<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearFactor5() {}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
* @param j3 key of the third variable
* @param j4 key of the fourth variable
* @param j5 key of the fifth variable
*/
NonlinearFactor5(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5) :
Base(noiseModel, make_tuple(j1,j2,j3,j4,j5)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5) {}
virtual ~NonlinearFactor5() {}
/** 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_; }
inline const KEY4& key4() const { return key4_; }
inline const KEY5& key5() const { return key5_; }
/** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]);
}
/**
* Override this method to finish implementing a 5-way 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&, const X4&, const X5&,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = 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("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
ar & BOOST_SERIALIZATION_NVP(key5_);
}
}; // \class NonlinearFactor5
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 6
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
class NonlinearFactor6: public NoiseModelFactor<VALUES> {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef typename KEY4::Value X4;
typedef typename KEY5::Value X5;
typedef typename KEY6::Value X6;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
KEY5 key5_;
KEY6 key6_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor6<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearFactor6() {}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
* @param j3 key of the third variable
* @param j4 key of the fourth variable
* @param j5 key of the fifth variable
* @param j6 key of the fifth variable
*/
NonlinearFactor6(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5, const KEY6& j6) :
Base(noiseModel, make_tuple(j1,j2,j3,j4,j5,j6)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5), key6_(j6) {}
virtual ~NonlinearFactor6() {}
/** 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_; }
inline const KEY4& key4() const { return key4_; }
inline const KEY5& key5() const { return key5_; }
inline const KEY6& key6() const { return key6_; }
/** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]);
}
/**
* Override this method to finish implementing a 6-way 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&, const X4&, const X5&, const X6&,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = 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("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
ar & BOOST_SERIALIZATION_NVP(key5_);
ar & BOOST_SERIALIZATION_NVP(key6_);
}
}; // \class NonlinearFactor6
/* ************************************************************************* */
} // \namespace gtsam

11
gtsam/nonlinear/NonlinearFactorGraph-inl.h
View File

@ -45,17 +45,6 @@ namespace gtsam {
Base::print(str);
}
/* ************************************************************************* */
template<class VALUES>
Vector NonlinearFactorGraph<VALUES>::unwhitenedError(const VALUES& c) const {
list<Vector> errors;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
errors.push_back(factor->unwhitenedError(c));
}
return concatVectors(errors);
}
/* ************************************************************************* */
template<class VALUES>
double NonlinearFactorGraph<VALUES>::error(const VALUES& c) const {

3
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -53,9 +53,6 @@ namespace gtsam {
/** 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;

13
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -160,8 +160,17 @@ TEST( planarSLAM, constructor )
double z2(sqrt(2) - 0.22); // h(x) - z = 0.22
G.addRange(2, 3, z2, sigma);
Vector expected = Vector_(8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, 0.22);
EXPECT(assert_equal(expected,G.unwhitenedError(c)));
Vector expected0 = Vector_(3, 0.0, 0.0, 0.0);
Vector expected1 = Vector_(3, 0.0, 0.0, 0.0);
Vector expected2 = Vector_(1, -0.1);
Vector expected3 = Vector_(1, 0.22);
// Get NoiseModelFactors
FactorGraph<NoiseModelFactor<planarSLAM::Values> > GNM =
*G.dynamicCastFactors<FactorGraph<NoiseModelFactor<planarSLAM::Values> > >();
EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c)));
EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c)));
EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c)));
EXPECT(assert_equal(expected3, GNM[3]->unwhitenedError(c)));
}
/* ************************************************************************* */

2
tests/testNonlinearFactor.cpp
View File

@ -87,7 +87,7 @@ TEST( NonlinearFactor, NonlinearFactor )
// calculate the error_vector from the factor "f1"
// error_vector = [0.1 0.1]
Vector actual_e = factor->unwhitenedError(cfg);
Vector actual_e = boost::dynamic_pointer_cast<NoiseModelFactor<Values> >(factor)->unwhitenedError(cfg);
CHECK(assert_equal(0.1*ones(2),actual_e));
// error = 0.5 * [1 1] * [1;1] = 1