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

NonlinearConstraint now derives from NonlinearFactor rather than NoiseModelFactor

release/4.3a0
Alex Cunningham 2011-10-07 20:51:47 +00:00
parent fcd7bc2a21
commit c14d69326a
2 changed files with 101 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
gtsam/nonlinear/NonlinearConstraint.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <boost/function.hpp>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -36,16 +35,18 @@ namespace gtsam {
* significant
*/
template <class VALUES>
class NonlinearConstraint : public NoiseModelFactor<VALUES> {
class NonlinearConstraint : public NonlinearFactor<VALUES> {
protected:
typedef NonlinearConstraint<VALUES> This;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor<VALUES> Base;
typedef Factor<Symbol> BaseFactor;
/** default constructor to allow for serialization */
NonlinearConstraint() {}
double mu_; /// gain for quadratic merit function
size_t dim_; /// dimension of the constraint
public:
@ -54,14 +55,30 @@ public:
* @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)
*/
template<class TUPLE>
NonlinearConstraint(const TUPLE& keys, size_t dim, double mu = 1000.0):
Base(noiseModel::Constrained::All(dim), keys), mu_(fabs(mu)) {}
template<class U1, class U2>
NonlinearConstraint(const boost::tuples::cons<U1,U2>& keys, size_t dim, double mu = 1000.0)
: Base(keys), mu_(fabs(mu)), dim_(dim) {
}
/**
* Constructor
* @param keys The variables involved in this factor
*/
template<class ITERATOR>
NonlinearConstraint(ITERATOR beginKeys, ITERATOR endKeys, size_t dim, double mu = 1000.0)
: Base(beginKeys, endKeys), mu_(fabs(mu)), dim_(dim) {
}
virtual ~NonlinearConstraint() {}
/** returns the gain mu */
double mu() const { return mu_; }
/** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const {
return dim_;
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << "NonlinearConstraint " << s << std::endl;
@ -75,7 +92,7 @@ public:
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) && (fabs(mu_ - p->mu_) <= tol);
return BaseFactor::equals(*p, tol) && (fabs(mu_ - p->mu_) <= tol) && dim_ == p->dim_;
}
/** error function - returns the quadratic merit function */
@ -95,16 +112,38 @@ public:
*/
virtual bool active(const VALUES& c) const=0;
/**
* Error function \f$ z-c(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;
/**
* Linearizes around a given config
* @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 {
if (!active(c))
return boost::shared_ptr<JacobianFactor>();
else
return Base::linearize(c, ordering);
if (!active(c))
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(c, A);
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]);
}
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b, noiseModel::Constrained::All(dim_)));
}
private:
@ -116,10 +155,11 @@ private:
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(mu_);
ar & BOOST_SERIALIZATION_NVP(dim_);
}
};
/* ************************************************************************* */
/**
* A unary constraint that defaults to an equality constraint
*/
@ -186,6 +226,7 @@ private:
}
};
/* ************************************************************************* */
/**
* Unary Equality constraint - simply forces the value of active() to true
*/
@ -222,6 +263,7 @@ private:
};
/* ************************************************************************* */
/**
* A binary constraint with arbitrary cost and jacobian functions
*/
@ -294,6 +336,7 @@ private:
}
};
/* ************************************************************************* */
/**
* Binary Equality constraint - simply forces the value of active() to true
*/
@ -330,6 +373,7 @@ private:
}
};
/* ************************************************************************* */
/**
* A ternary constraint
*/
@ -410,6 +454,7 @@ private:
}
};
/* ************************************************************************* */
/**
* Ternary Equality constraint - simply forces the value of active() to true
*/
@ -448,7 +493,7 @@ private:
}
};
/* ************************************************************************* */
/**
* Simple unary equality constraint - fixes a value for a variable
*/
@ -493,7 +538,7 @@ private:
}
};
/* ************************************************************************* */
/**
* Simple binary equality constraint - this constraint forces two factors to
* be the same. This constraint requires the underlying type to a Lie type

74
gtsam/nonlinear/NonlinearFactor.h
View File

@ -37,6 +37,17 @@ namespace gtsam {
using boost::make_tuple;
// 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 base class
@ -62,6 +73,27 @@ public:
NonlinearFactor() {
}
/**
* Constructor
* @param keys A boost::tuple containing the variables involved in this factor,
* example: <tt>NonlinearFactor(make_tuple(symbol1, symbol2, symbol3))</tt>
*/
template<class U1, class U2>
NonlinearFactor(const boost::tuples::cons<U1,U2>& keys) {
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 keys The variables involved in this factor
*/
template<class ITERATOR>
NonlinearFactor(ITERATOR beginKeys, ITERATOR endKeys) {
this->keys_.insert(this->keys_.end(), beginKeys, endKeys);
}
/** Destructor */
virtual ~NonlinearFactor() {}
@ -88,22 +120,15 @@ public:
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const = 0;
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));
}
}; // \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
}
/* ************************************************************************* */
/**
* A nonlinear sum-of-squares factor with a zero-mean noise model
@ -143,11 +168,8 @@ public:
* example: <tt>NoiseModelFactor(noiseModel, make_tuple(symbol1, symbol2, symbol3)</tt>
*/
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);
NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons<U1,U2>& keys)
: Base(keys), noiseModel_(noiseModel) {
}
/**
@ -155,9 +177,8 @@ public:
* @param keys The variables involved in this factor
*/
template<class ITERATOR>
NoiseModelFactor(const SharedNoiseModel& noiseModel, ITERATOR beginKeys, ITERATOR endKeys) :
Base(noiseModel) {
this->keys_.insert(this->keys_.end(), beginKeys, endKeys);
NoiseModelFactor(const SharedNoiseModel& noiseModel, ITERATOR beginKeys, ITERATOR endKeys)
: Base(beginKeys, endKeys), noiseModel_(noiseModel) {
}
protected:
@ -253,17 +274,6 @@ public:
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 */