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

Created implementation file for NonlinearFactor and moved non-templated code there

release/4.3a0
dellaert 2014-10-01 13:14:55 +02:00
parent ce53346a9e
commit c01c756d69
2 changed files with 160 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

141
gtsam/nonlinear/NonlinearFactor.cpp Normal file
View File

@ -0,0 +1,141 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* @file NonlinearFactor.cpp
* @brief Nonlinear Factor base classes
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/make_shared.hpp>
namespace gtsam {
/* ************************************************************************* */
void NonlinearFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::cout << s << " keys = { ";
BOOST_FOREACH(Key key, this->keys()) {
std::cout << keyFormatter(key) << " ";
}
std::cout << "}" << std::endl;
}
bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const {
return Base::equals(f);
}
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
const std::map<Key, Key>& rekey_mapping) const {
shared_ptr new_factor = clone();
for (size_t i = 0; i < new_factor->size(); ++i) {
Key& cur_key = new_factor->keys()[i];
std::map<Key, Key>::const_iterator mapping = rekey_mapping.find(cur_key);
if (mapping != rekey_mapping.end())
cur_key = mapping->second;
}
return new_factor;
}
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
const std::vector<Key>& new_keys) const {
assert(new_keys.size() == this->keys().size());
shared_ptr new_factor = clone();
new_factor->keys() = new_keys;
return new_factor;
}
/* ************************************************************************* */
void NoiseModelFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
Base::print(s, keyFormatter);
this->noiseModel_->print(" noise model: ");
}
bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
return e && Base::equals(f, tol)
&& ((!noiseModel_ && !e->noiseModel_)
|| (noiseModel_ && e->noiseModel_
&& noiseModel_->equals(*e->noiseModel_, tol)));
}
Vector NoiseModelFactor::whitenedError(const Values& c) const {
const Vector unwhitenedErrorVec = unwhitenedError(c);
if ((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
throw std::invalid_argument(
"This factor was created with a NoiseModel of incorrect dimension.");
return noiseModel_->whiten(unwhitenedErrorVec);
}
double NoiseModelFactor::error(const Values& c) const {
if (this->active(c)) {
const Vector unwhitenedErrorVec = unwhitenedError(c);
if ((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
throw std::invalid_argument(
"This factor was created with a NoiseModel of incorrect dimension.");
return 0.5 * noiseModel_->distance(unwhitenedErrorVec);
} else {
return 0.0;
}
}
boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
const Values& x) const {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();
// Call evaluate error to get Jacobians and RHS vector b
std::vector<Matrix> A(this->size());
Vector b = -unwhitenedError(x, A);
// If a noiseModel is given, whiten the corresponding system now
if (noiseModel_) {
if ((size_t) b.size() != noiseModel_->dim())
throw std::invalid_argument(
"This factor was created with a NoiseModel of incorrect dimension.");
this->noiseModel_->WhitenSystem(A, b);
}
// Fill in terms, needed to create JacobianFactor below
std::vector<std::pair<Key, Matrix> > terms(this->size());
for (size_t j = 0; j < this->size(); ++j) {
terms[j].first = this->keys()[j];
terms[j].second.swap(A[j]);
}
// TODO pass unwhitened + noise model to Gaussian factor
// For now, only linearized constrained factors have noise model at linear level!!!
if (noiseModel_) {
noiseModel::Constrained::shared_ptr constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained) {
// Create a factor of reduced row dimension d_
size_t d_ = terms[0].second.rows() - constrained->dim();
Vector zero_ = zero(d_);
Vector b_ = concatVectors(2, &b, &zero_);
Constrained::shared_ptr model = constrained->unit(d_);
return boost::make_shared<JacobianFactor>(terms, b_, model);
} else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
} else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}
/* ************************************************************************* */
} // \namespace gtsam

132
gtsam/nonlinear/NonlinearFactor.h
View File

@ -43,12 +43,10 @@ namespace gtsam {
using boost::assign::cref_list_of; using boost::assign::cref_list_of;
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Nonlinear factor base class * Nonlinear factor base class
* *
* Templated on a values structure type. The values structures are typically
* more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups).
* \nosubgrouping * \nosubgrouping
*/ */
class NonlinearFactor: public Factor { class NonlinearFactor: public Factor {
@ -82,18 +80,10 @@ public:
/** print */ /** print */
virtual void print(const std::string& s = "", virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
{
std::cout << s << " keys = { ";
BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
std::cout << "}" << std::endl;
}
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
return Base::equals(f);
}
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
@ -128,17 +118,6 @@ public:
virtual boost::shared_ptr<GaussianFactor> virtual boost::shared_ptr<GaussianFactor>
linearize(const Values& c) const = 0; linearize(const Values& c) const = 0;
/**
* 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));
//}
/** /**
* Creates a shared_ptr clone of the factor - needs to be specialized to allow * Creates a shared_ptr clone of the factor - needs to be specialized to allow
* for subclasses * for subclasses
@ -155,28 +134,13 @@ public:
* Creates a shared_ptr clone of the factor with different keys using * Creates a shared_ptr clone of the factor with different keys using
* a map from old->new keys * a map from old->new keys
*/ */
shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const { shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
shared_ptr new_factor = clone();
for (size_t i=0; i<new_factor->size(); ++i) {
Key& cur_key = new_factor->keys()[i];
std::map<Key,Key>::const_iterator mapping = rekey_mapping.find(cur_key);
if (mapping != rekey_mapping.end())
cur_key = mapping->second;
}
return new_factor;
}
/** /**
* Clones a factor and fully replaces its keys * Clones a factor and fully replaces its keys
* @param new_keys is the full replacement set of keys * @param new_keys is the full replacement set of keys
*/ */
shared_ptr rekey(const std::vector<Key>& new_keys) const { shared_ptr rekey(const std::vector<Key>& new_keys) const;
assert(new_keys.size() == this->keys().size());
shared_ptr new_factor = clone();
new_factor->keys() = new_keys;
return new_factor;
}
}; // \class NonlinearFactor }; // \class NonlinearFactor
@ -229,19 +193,10 @@ public:
/** Print */ /** Print */
virtual void print(const std::string& s = "", virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
{
Base::print(s, keyFormatter);
this->noiseModel_->print(" noise model: ");
}
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
return e && Base::equals(f, tol) &&
((!noiseModel_ && !e->noiseModel_) ||
(noiseModel_ && e->noiseModel_ && noiseModel_->equals(*e->noiseModel_, tol)));
}
/** get the dimension of the factor (number of rows on linearization) */ /** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const { virtual size_t dim() const {
@ -265,12 +220,7 @@ public:
* Vector of errors, whitened * Vector of errors, whitened
* This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ 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 { Vector whitenedError(const Values& c) const;
const Vector unwhitenedErrorVec = unwhitenedError(c);
if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
return noiseModel_->whiten(unwhitenedErrorVec);
}
/** /**
* Calculate the error of the factor. * Calculate the error of the factor.
@ -278,65 +228,14 @@ public:
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * 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. * 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 { virtual double error(const Values& c) const;
if (this->active(c)) {
const Vector unwhitenedErrorVec = unwhitenedError(c);
if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
return 0.5 * noiseModel_->distance(unwhitenedErrorVec);
} else {
return 0.0;
}
}
/** /**
* Linearize a non-linearFactorN to get a GaussianFactor, * Linearize a non-linearFactorN to get a GaussianFactor,
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * \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$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { boost::shared_ptr<GaussianFactor> linearize(const Values& x) const;
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();
// 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);
if(noiseModel_)
{
if((size_t) b.size() != noiseModel_->dim())
throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
this->noiseModel_->WhitenSystem(A,b);
}
std::vector<std::pair<Key, Matrix> > terms(this->size());
// Fill in terms
for(size_t j=0; j<this->size(); ++j) {
terms[j].first = this->keys()[j];
terms[j].second.swap(A[j]);
}
// TODO pass unwhitened + noise model to Gaussian factor
// For now, only linearized constrained factors have noise model at linear level!!!
if(noiseModel_)
{
noiseModel::Constrained::shared_ptr constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if(constrained) {
size_t augmentedDim = terms[0].second.rows() - constrained->dim();
Vector augmentedZero = zero(augmentedDim);
Vector augmentedb = concatVectors(2, &b, &augmentedZero);
return GaussianFactor::shared_ptr(new JacobianFactor(terms, augmentedb, constrained->unit(augmentedDim)));
}
else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}
else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}
private: private:
@ -353,8 +252,15 @@ private:
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError(). */ /**
* A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError().
*
* Templated on a values structure type. The values structures are typically
* more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups).
*/
template<class VALUE> template<class VALUE>
class NoiseModelFactor1: public NoiseModelFactor { class NoiseModelFactor1: public NoiseModelFactor {