Added nonlinear constraints to gtsam library

release/4.3a0
Alex Cunningham 2011-06-03 15:07:11 +00:00
parent 79c09708e8
commit 9160775d2a
9 changed files with 1697 additions and 1 deletions

View File

@ -31,7 +31,7 @@ check_PROGRAMS += tests/testKey tests/testOrdering
headers += NonlinearISAM.h NonlinearISAM-inl.h
# Nonlinear constraints
headers += NonlinearEquality.h
headers += NonlinearEquality.h NonlinearConstraint.h
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed

View File

@ -0,0 +1,670 @@
/* ----------------------------------------------------------------------------
* 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 NonlinearConstraint.h
* @brief Implements nonlinear constraints that can be linearized using
* direct linearization and solving through a quadratic merit function
* @author Alex Cunningham
*/
#pragma once
#include <map>
#include <boost/function.hpp>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Base class for nonlinear constraints
* This allows for both equality and inequality constraints,
* where equality constraints are active all the time (even slightly
* nonzero constraint functions will still be active - inequality
* constraints should be sure to force to actual zero)
*
* NOTE: inequality constraints removed for now
*
* Nonlinear constraints evaluate their error as a part of a quadratic
* error function: ||h(x)-z||^2 + mu * ||c(x)|| where mu is a gain
* on the constraint function that should be made high enough to be
* significant
*/
template <class VALUES>
class NonlinearConstraint : public NonlinearFactor<VALUES> {
protected:
typedef NonlinearConstraint<VALUES> This;
typedef NonlinearFactor<VALUES> Base;
/** default constructor to allow for serialization */
NonlinearConstraint() {}
double mu_; /// gain for quadratic merit function
public:
/** Constructor - sets the cost function and the lagrange multipliers
* @param dim is the dimension of the factor
* @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)) {}
virtual ~NonlinearConstraint() {}
/** returns the gain mu */
double mu() const { return mu_; }
/** Print */
virtual void print(const std::string& s = "") const=0;
/** 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_);
}
/** 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;
}
/** Raw error vector function g(x) */
virtual Vector unwhitenedError(const VALUES& c) const = 0;
/**
* active set check, defines what type of constraint this is
*
* In an inequality/bounding constraint, this active() returns true
* when the constraint is *NOT* fulfilled.
* @return true if the constraint is active
*/
virtual bool active(const VALUES& c) 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=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<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(mu_);
}
};
/**
* A unary constraint that defaults to an equality constraint
*/
template <class VALUES, class KEY>
class NonlinearConstraint1 : public NonlinearConstraint<VALUES> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearConstraint1<VALUES,KEY> This;
typedef NonlinearConstraint<VALUES> Base;
/** default constructor to allow for serialization */
NonlinearConstraint1() {}
/** key for the constrained variable */
KEY key_;
public:
/**
* Basic constructor
* @param key is the identifier for the variable constrained
* @param dim is the size of the constraint (p)
* @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);
}
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;
}
/** 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)) {
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.
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
return IndexFactor::shared_ptr(new IndexFactor(ordering[key_]));
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key_);
}
};
/**
* Unary Equality constraint - simply forces the value of active() to true
*/
template <class VALUES, class KEY>
class NonlinearEqualityConstraint1 : public NonlinearConstraint1<VALUES, KEY> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearEqualityConstraint1<VALUES,KEY> This;
typedef NonlinearConstraint1<VALUES,KEY> Base;
/** default constructor to allow for serialization */
NonlinearEqualityConstraint1() {}
public:
NonlinearEqualityConstraint1(const KEY& key, size_t dim, double mu = 1000.0)
: Base(key, dim, mu) {}
virtual ~NonlinearEqualityConstraint1() {}
/** Always active, so fixed value for active() */
virtual bool active(const VALUES& c) const { return true; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint1",
boost::serialization::base_object<Base>(*this));
}
};
/**
* A binary constraint with arbitrary cost and jacobian functions
*/
template <class VALUES, class KEY1, class KEY2>
class NonlinearConstraint2 : public NonlinearConstraint<VALUES> {
public:
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
protected:
typedef NonlinearConstraint2<VALUES,KEY1,KEY2> This;
typedef NonlinearConstraint<VALUES> Base;
/** default constructor to allow for serialization */
NonlinearConstraint2() {}
/** keys for the constrained variables */
KEY1 key1_;
KEY2 key2_;
public:
/**
* Basic constructor
* @param key1 is the identifier for the first variable constrained
* @param key2 is the identifier for the second variable constrained
* @param dim is the size of the constraint (p)
* @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);
}
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;
}
/** 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)) {
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.
*/
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));
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
}
};
/**
* Binary Equality constraint - simply forces the value of active() to true
*/
template <class VALUES, class KEY1, class KEY2>
class NonlinearEqualityConstraint2 : public NonlinearConstraint2<VALUES, KEY1, KEY2> {
public:
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
protected:
typedef NonlinearEqualityConstraint2<VALUES,KEY1,KEY2> This;
typedef NonlinearConstraint2<VALUES,KEY1,KEY2> Base;
/** default constructor to allow for serialization */
NonlinearEqualityConstraint2() {}
public:
NonlinearEqualityConstraint2(const KEY1& key1, const KEY2& key2, size_t dim, double mu = 1000.0)
: Base(key1, key2, dim, mu) {}
virtual ~NonlinearEqualityConstraint2() {}
/** Always active, so fixed value for active() */
virtual bool active(const VALUES& c) const { return true; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint2",
boost::serialization::base_object<Base>(*this));
}
};
/**
* A ternary constraint
*/
template <class VALUES, class KEY1, class KEY2, class KEY3>
class NonlinearConstraint3 : public NonlinearConstraint<VALUES> {
public:
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
protected:
typedef NonlinearConstraint3<VALUES,KEY1,KEY2,KEY3> This;
typedef NonlinearConstraint<VALUES> Base;
/** default constructor to allow for serialization */
NonlinearConstraint3() {}
/** keys for the constrained variables */
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
public:
/**
* Basic constructor
* @param key1 is the identifier for the first variable constrained
* @param key2 is the identifier for the second variable constrained
* @param key3 is the identifier for the second variable constrained
* @param dim is the size of the constraint (p)
* @param mu is the gain for the factor
*/
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);
}
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;
}
/** 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)) {
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,
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 */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
}
};
/**
* Ternary Equality constraint - simply forces the value of active() to true
*/
template <class VALUES, class KEY1, class KEY2, class KEY3>
class NonlinearEqualityConstraint3 : public NonlinearConstraint3<VALUES, KEY1, KEY2, KEY3> {
public:
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
protected:
typedef NonlinearEqualityConstraint3<VALUES,KEY1,KEY2,KEY3> This;
typedef NonlinearConstraint3<VALUES,KEY1,KEY2,KEY3> Base;
/** default constructor to allow for serialization */
NonlinearEqualityConstraint3() {}
public:
NonlinearEqualityConstraint3(const KEY1& key1, const KEY2& key2, const KEY3& key3,
size_t dim, double mu = 1000.0)
: Base(key1, key2, key3, dim, mu) {}
virtual ~NonlinearEqualityConstraint3() {}
/** Always active, so fixed value for active() */
virtual bool active(const VALUES& c) const { return true; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint3",
boost::serialization::base_object<Base>(*this));
}
};
/**
* Simple unary equality constraint - fixes a value for a variable
*/
template<class VALUES, class KEY>
class NonlinearEquality1 : public NonlinearEqualityConstraint1<VALUES, KEY> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearEqualityConstraint1<VALUES, KEY> Base;
/** default constructor to allow for serialization */
NonlinearEquality1() {}
X value_; /// fixed value for variable
public:
typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > shared_ptr;
NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
: Base(key1, X::Dim(), mu), value_(value) {}
virtual ~NonlinearEquality1() {}
/** g(x) with optional derivative */
Vector evaluateError(const X& x1, boost::optional<Matrix&> H1 = boost::none) const {
const size_t p = X::Dim();
if (H1) *H1 = eye(p);
return value_.logmap(x1);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearEqualityConstraint1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(value_);
}
};
/**
* Simple binary equality constraint - this constraint forces two factors to
* be the same. This constraint requires the underlying type to a Lie type
*/
template<class VALUES, class KEY>
class NonlinearEquality2 : public NonlinearEqualityConstraint2<VALUES, KEY, KEY> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearEqualityConstraint2<VALUES, KEY, KEY> Base;
/** default constructor to allow for serialization */
NonlinearEquality2() {}
public:
typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > shared_ptr;
NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)
: Base(key1, key2, X::Dim(), mu) {}
virtual ~NonlinearEquality2() {}
/** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
const size_t p = X::Dim();
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
return x1.logmap(x2);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2",
boost::serialization::base_object<Base>(*this));
}
};
}

View File

@ -0,0 +1,69 @@
/* ----------------------------------------------------------------------------
* 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 BetweenConstraint.h
* @brief Implements a constraint to force a between
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/nonlinear/NonlinearConstraint.h>
namespace gtsam {
/**
* Binary between constraint - forces between to a given value
* This constraint requires the underlying type to a Lie type
*/
template<class VALUES, class KEY>
class BetweenConstraint : public NonlinearEqualityConstraint2<VALUES, KEY, KEY> {
public:
typedef typename KEY::Value X;
protected:
typedef NonlinearEqualityConstraint2<VALUES, KEY, KEY> Base;
X measured_; /// fixed between value
public:
typedef boost::shared_ptr<BetweenConstraint<VALUES, KEY> > shared_ptr;
BetweenConstraint(const X& measured, const KEY& key1, const KEY& key2, double mu = 1000.0)
: Base(key1, key2, X::Dim(), mu), measured_(measured) {}
/** g(x) with optional derivative2 */
Vector evaluateError(const X& x1, const X& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
X hx = x1.between(x2, H1, H2);
return measured_.logmap(hx);
}
inline const X measured() const {
return measured_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearEqualityConstraint2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
} // \namespace gtsam

View File

@ -0,0 +1,161 @@
/* ----------------------------------------------------------------------------
* 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 BoundingConstraint.h
* @brief Provides partially implemented constraints to implement bounds
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearConstraint.h>
namespace gtsam {
/**
* Unary inequality constraint forcing a scalar to be
* greater/less than a fixed threshold. The function
* will need to have its value function implemented to return
* a scalar for comparison.
*/
template<class VALUES, class KEY>
struct BoundingConstraint1: public NonlinearConstraint1<VALUES, KEY> {
typedef typename KEY::Value X;
typedef NonlinearConstraint1<VALUES, KEY> Base;
typedef boost::shared_ptr<BoundingConstraint1<VALUES, KEY> > shared_ptr;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint1(const KEY& key, double threshold,
bool isGreaterThan, double mu = 1000.0) :
Base(key, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {
}
inline double threshold() const { return threshold_; }
inline bool isGreaterThan() const { return isGreaterThan_; }
/**
* function producing a scalar value to compare to the threshold
* Must have optional argument for derivative with 1xN matrix, where
* N = X::dim()
*/
virtual double value(const X& x, boost::optional<Matrix&> H =
boost::none) const = 0;
/** active when constraint *NOT* met */
bool active(const VALUES& c) const {
// note: still active at equality to avoid zigzagging
double x = value(c[this->key_]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
Vector evaluateError(const X& x, boost::optional<Matrix&> H =
boost::none) const {
Matrix D;
double error = value(x, D) - threshold_;
if (H) {
if (isGreaterThan_) *H = D;
else *H = -1.0 * D;
}
if (isGreaterThan_)
return Vector_(1, error);
else
return -1.0 * Vector_(1, error);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(threshold_);
ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
}
};
/**
* Binary scalar inequality constraint, with a similar value() function
* to implement for specific systems
*/
template<class VALUES, class KEY1, class KEY2>
struct BoundingConstraint2: public NonlinearConstraint2<VALUES, KEY1, KEY2> {
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef NonlinearConstraint2<VALUES, KEY1, KEY2> Base;
typedef boost::shared_ptr<BoundingConstraint2<VALUES, KEY1, KEY2> > shared_ptr;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint2(const KEY1& key1, const KEY2& key2, double threshold,
bool isGreaterThan, double mu = 1000.0)
: Base(key1, key2, 1, mu), threshold_(threshold), isGreaterThan_(isGreaterThan) {}
inline double threshold() const { return threshold_; }
inline bool isGreaterThan() const { return isGreaterThan_; }
/**
* function producing a scalar value to compare to the threshold
* Must have optional argument for derivatives)
*/
virtual double value(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const = 0;
/** active when constraint *NOT* met */
bool active(const VALUES& c) const {
// note: still active at equality to avoid zigzagging
double x = value(c[this->key1_], c[this->key2_]);
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
}
Vector evaluateError(const X1& x1, const X2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Matrix D1, D2;
double error = value(x1, x2, D1, D2) - threshold_;
if (H1) {
if (isGreaterThan_) *H1 = D1;
else *H1 = -1.0 * D1;
}
if (H2) {
if (isGreaterThan_) *H2 = D2;
else *H2 = -1.0 * D2;
}
if (isGreaterThan_)
return Vector_(1, error);
else
return -1.0 * Vector_(1, error);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearConstraint2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(threshold_);
ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
}
};
} // \namespace gtsam

View File

@ -14,6 +14,7 @@ check_PROGRAMS =
headers += Simulated2DValues.h
headers += Simulated2DPosePrior.h Simulated2DPointPrior.h
headers += Simulated2DOdometry.h Simulated2DMeasurement.h
headers += simulated2DConstraints.h
sources += simulated2D.cpp smallExample.cpp
check_PROGRAMS += tests/testSimulated2D
@ -31,6 +32,9 @@ check_PROGRAMS += tests/testSimulated3D
# Generic SLAM headers
headers += BetweenFactor.h PriorFactor.h PartialPriorFactor.h
# Generic constraint headers
headers += BetweenConstraint.h BoundingConstraint.h
# 2D Pose SLAM
sources += pose2SLAM.cpp dataset.cpp
#sources += Pose2SLAMOptimizer.cpp

View File

@ -0,0 +1,134 @@
/* ----------------------------------------------------------------------------
* 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 simulated2DConstraints.h
* @brief measurement functions and constraint definitions for simulated 2D robot
* @author Alex Cunningham
*/
// \callgraph
#pragma once
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/NonlinearConstraint.h>
#include <gtsam/slam/BetweenConstraint.h>
#include <gtsam/slam/BoundingConstraint.h>
#include <gtsam/slam/simulated2D.h>
// \namespace
namespace gtsam {
namespace simulated2D {
namespace equality_constraints {
/** Typedefs for regular use */
typedef NonlinearEquality1<Values, PoseKey> UnaryEqualityConstraint;
typedef NonlinearEquality1<Values, PointKey> UnaryEqualityPointConstraint;
typedef BetweenConstraint<Values, PoseKey> OdoEqualityConstraint;
/** Equality between variables */
typedef NonlinearEquality2<Values, PoseKey> PoseEqualityConstraint;
typedef NonlinearEquality2<Values, PointKey> PointEqualityConstraint;
} // \namespace equality_constraints
namespace inequality_constraints {
/**
* Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c)
* Demo implementation: should be made more general using BoundingConstraint
*/
template<class Cfg, class Key, unsigned int Idx>
struct ScalarCoordConstraint1: public BoundingConstraint1<Cfg, Key> {
typedef BoundingConstraint1<Cfg, Key> Base;
typedef boost::shared_ptr<ScalarCoordConstraint1<Cfg, Key, Idx> > shared_ptr;
ScalarCoordConstraint1(const Key& key, double c,
bool isGreaterThan, double mu = 1000.0) :
Base(key, c, isGreaterThan, mu) {
}
inline unsigned int index() const { return Idx; }
/** extracts a single value from the point */
virtual double value(const Point2& x, boost::optional<Matrix&> H =
boost::none) const {
if (H) {
Matrix D = zeros(1, 2);
D(0, Idx) = 1.0;
*H = D;
}
return x.vector()(Idx);
}
};
/** typedefs for use with simulated2D systems */
typedef ScalarCoordConstraint1<Values, PoseKey, 0> PoseXInequality;
typedef ScalarCoordConstraint1<Values, PoseKey, 1> PoseYInequality;
double range(const Point2& a, const Point2& b) { return a.dist(b); }
/**
* Binary inequality constraint forcing the range between points
* to be less than or equal to a bound
*/
template<class Cfg, class Key>
struct MaxDistanceConstraint : public BoundingConstraint2<Cfg, Key, Key> {
typedef BoundingConstraint2<Cfg, Key, Key> Base;
MaxDistanceConstraint(const Key& key1, const Key& key2, double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, false, mu) {}
/** extracts a single scalar value with derivatives */
virtual double value(const Point2& x1, const Point2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5);
if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5);
return x1.dist(x2);
}
};
typedef MaxDistanceConstraint<Values, PoseKey> PoseMaxDistConstraint;
/**
* Binary inequality constraint forcing a minimum range
* NOTE: this is not a convex function! Be careful with initialization.
*/
template<class Cfg, class XKey, class PKey>
struct MinDistanceConstraint : public BoundingConstraint2<Cfg, XKey, PKey> {
typedef BoundingConstraint2<Cfg, XKey, PKey> Base;
MinDistanceConstraint(const XKey& key1, const PKey& key2, double range_bound, double mu = 1000.0)
: Base(key1, key2, range_bound, true, mu) {}
/** extracts a single scalar value with derivatives */
virtual double value(const Point2& x1, const Point2& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = numericalDerivative21(range, x1, x2, 1e-5);
if (H1) *H2 = numericalDerivative22(range, x1, x2, 1e-5);
return x1.dist(x2);
}
};
typedef MinDistanceConstraint<Values, PoseKey, PointKey> LandmarkAvoid;
} // \namespace inequality_constraints
} // \namespace simulated2D
} // \namespace gtsam

View File

@ -20,6 +20,8 @@ check_PROGRAMS += testNonlinearOptimizer
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
check_PROGRAMS += testTupleValues
check_PROGRAMS += testNonlinearISAM
check_PROGRAMS += testBoundingConstraint
check_PROGRAMS += testNonlinearEqualityConstraint
# only if serialization is available
if ENABLE_SERIALIZATION

View File

@ -0,0 +1,287 @@
/* ----------------------------------------------------------------------------
* 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 testBoundingConstraint.cpp
* @brief test of nonlinear inequality constraints on scalar bounds
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/simulated2DConstraints.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
namespace iq2D = gtsam::simulated2D::inequality_constraints;
using namespace std;
using namespace gtsam;
static const double tol = 1e-5;
SharedDiagonal soft_model2 = noiseModel::Unit::Create(2);
SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1);
SharedDiagonal hard_model1 = noiseModel::Constrained::All(1);
typedef NonlinearFactorGraph<simulated2D::Values> Graph;
typedef boost::shared_ptr<Graph> shared_graph;
typedef boost::shared_ptr<simulated2D::Values> shared_values;
typedef NonlinearOptimizer<Graph, simulated2D::Values> Optimizer;
// some simple inequality constraints
simulated2D::PoseKey key(1);
double mu = 10.0;
// greater than
iq2D::PoseXInequality constraint1(key, 1.0, true, mu);
iq2D::PoseYInequality constraint2(key, 2.0, true, mu);
// less than
iq2D::PoseXInequality constraint3(key, 1.0, false, mu);
iq2D::PoseYInequality constraint4(key, 2.0, false, mu);
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_basics_inactive1 ) {
Point2 pt1(2.0, 3.0);
simulated2D::Values config;
config.insert(key, pt1);
EXPECT(!constraint1.active(config));
EXPECT(!constraint2.active(config));
EXPECT_DOUBLES_EQUAL(1.0, constraint1.threshold(), tol);
EXPECT_DOUBLES_EQUAL(2.0, constraint2.threshold(), tol);
EXPECT(constraint1.isGreaterThan());
EXPECT(constraint2.isGreaterThan());
EXPECT(assert_equal(ones(1), constraint1.evaluateError(pt1), tol));
EXPECT(assert_equal(ones(1), constraint2.evaluateError(pt1), tol));
EXPECT(assert_equal(zero(1), constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(zero(1), constraint2.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint1.error(config), tol);
EXPECT_DOUBLES_EQUAL(0.0, constraint2.error(config), tol);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_basics_inactive2 ) {
Point2 pt2(-2.0, -3.0);
simulated2D::Values config;
config.insert(key, pt2);
EXPECT(!constraint3.active(config));
EXPECT(!constraint4.active(config));
EXPECT_DOUBLES_EQUAL(1.0, constraint3.threshold(), tol);
EXPECT_DOUBLES_EQUAL(2.0, constraint4.threshold(), tol);
EXPECT(!constraint3.isGreaterThan());
EXPECT(!constraint4.isGreaterThan());
EXPECT(assert_equal(repeat(1, 3.0), constraint3.evaluateError(pt2), tol));
EXPECT(assert_equal(repeat(1, 5.0), constraint4.evaluateError(pt2), tol));
EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol);
EXPECT_DOUBLES_EQUAL(0.0, constraint4.error(config), tol);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_basics_active1 ) {
Point2 pt2(-2.0, -3.0);
simulated2D::Values config;
config.insert(key, pt2);
EXPECT(constraint1.active(config));
EXPECT(constraint2.active(config));
EXPECT(assert_equal(repeat(1,-3.0), constraint1.evaluateError(pt2), tol));
EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol));
EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(90.0, constraint1.error(config), tol);
EXPECT_DOUBLES_EQUAL(250.0, constraint2.error(config), tol);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_basics_active2 ) {
Point2 pt1(2.0, 3.0);
simulated2D::Values config;
config.insert(key, pt1);
EXPECT(constraint3.active(config));
EXPECT(constraint4.active(config));
EXPECT(assert_equal(-1.0 * ones(1), constraint3.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint4.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(-1.0 * ones(1), constraint4.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(10.0, constraint3.error(config), tol);
EXPECT_DOUBLES_EQUAL(10.0, constraint4.error(config), tol);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_linearization_inactive) {
Point2 pt1(2.0, 3.0);
simulated2D::Values config1;
config1.insert(key, pt1);
Ordering ordering;
ordering += key;
GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1, ordering);
GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1, ordering);
EXPECT(!actual1);
EXPECT(!actual2);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_linearization_active) {
Point2 pt2(-2.0, -3.0);
simulated2D::Values config2;
config2.insert(key, pt2);
Ordering ordering;
ordering += key;
GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2, ordering);
GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2, ordering);
JacobianFactor expected1(ordering[key], Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1);
JacobianFactor expected2(ordering[key], Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1);
EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol));
EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol));
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_simple_optimization1) {
// create a single-node graph with a soft and hard constraint to
// ensure that the hard constraint overrides the soft constraint
Point2 goal_pt(1.0, 2.0);
Point2 start_pt(0.0, 1.0);
shared_graph graph(new Graph());
simulated2D::PoseKey x1(1);
graph->add(iq2D::PoseXInequality(x1, 1.0, true));
graph->add(iq2D::PoseYInequality(x1, 2.0, true));
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
shared_values initValues(new simulated2D::Values());
initValues->insert(x1, start_pt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
simulated2D::Values expected;
expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_simple_optimization2) {
// create a single-node graph with a soft and hard constraint to
// ensure that the hard constraint overrides the soft constraint
Point2 goal_pt(1.0, 2.0);
Point2 start_pt(2.0, 3.0);
shared_graph graph(new Graph());
simulated2D::PoseKey x1(1);
graph->add(iq2D::PoseXInequality(x1, 1.0, false));
graph->add(iq2D::PoseYInequality(x1, 2.0, false));
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
shared_values initValues(new simulated2D::Values());
initValues->insert(x1, start_pt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
simulated2D::Values expected;
expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_basics) {
simulated2D::PoseKey key1(1), key2(2);
Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
EXPECT(!rangeBound.isGreaterThan());
EXPECT(rangeBound.dim() == 1);
EXPECT(assert_equal(Vector_(1, 2.0), rangeBound.evaluateError(pt1, pt1)));
EXPECT(assert_equal(ones(1), rangeBound.evaluateError(pt1, pt2)));
EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3)));
EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4)));
simulated2D::Values config1;
config1.insert(key1, pt1);
config1.insert(key2, pt1);
Ordering ordering; ordering += key1, key2;
EXPECT(!rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT(!rangeBound.linearize(config1, ordering));
EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
config1.update(key2, pt2);
EXPECT(!rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT(!rangeBound.linearize(config1, ordering));
EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
config1.update(key2, pt3);
EXPECT(rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT_DOUBLES_EQUAL(0.0, rangeBound.error(config1), tol);
config1.update(key2, pt4);
EXPECT(rangeBound.active(config1));
EXPECT(assert_equal(-1.0*ones(1), rangeBound.unwhitenedError(config1)));
EXPECT_DOUBLES_EQUAL(1.0*mu, rangeBound.error(config1), tol);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
simulated2D::PoseKey x1(1), x2(2);
Graph graph;
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1));
graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2));
graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0));
simulated2D::Values initial_state;
initial_state.insert(x1, pt1);
initial_state.insert(x2, pt2_init);
simulated2D::Values expected;
expected.insert(x1, pt1);
expected.insert(x2, pt2_goal);
// FAILS: segfaults on optimization
// Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initial_state);
// EXPECT(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
TEST( testBoundingConstraint, avoid_demo) {
simulated2D::PoseKey x1(1), x2(2), x3(3);
simulated2D::PointKey l1(1);
double radius = 1.0;
Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
Point2 odo(2.0, 0.0);
Graph graph;
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1));
graph.add(simulated2D::Odometry(odo, soft_model2_alt, x1, x2));
graph.add(iq2D::LandmarkAvoid(x2, l1, radius));
graph.add(simulated2D::equality_constraints::UnaryEqualityPointConstraint(l1_pt, l1));
graph.add(simulated2D::Odometry(odo, soft_model2_alt, x2, x3));
graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3));
simulated2D::Values init, expected;
init.insert(x1, x1_pt);
init.insert(x3, x3_pt);
init.insert(l1, l1_pt);
expected = init;
init.insert(x2, x2_init);
expected.insert(x2, x2_goal);
// FAILS: segfaults on optimization
// Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init);
// EXPECT(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

View File

@ -0,0 +1,369 @@
/* ----------------------------------------------------------------------------
* 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 testNonlinearEqualityConstraint.cpp
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/simulated2DConstraints.h>
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
namespace eq2D = gtsam::simulated2D::equality_constraints;
using namespace std;
using namespace gtsam;
static const double tol = 1e-5;
SharedDiagonal hard_model = noiseModel::Constrained::All(2);
SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
typedef NonlinearFactorGraph<simulated2D::Values> Graph;
typedef boost::shared_ptr<Graph> shared_graph;
typedef boost::shared_ptr<simulated2D::Values> shared_values;
typedef NonlinearOptimizer<Graph, simulated2D::Values> Optimizer;
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_basics ) {
Point2 pt(1.0, 2.0);
simulated2D::PoseKey key(1);
double mu = 1000.0;
eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
simulated2D::Values config1;
config1.insert(key, pt);
EXPECT(constraint.active(config1));
EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol));
EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
simulated2D::Values config2;
Point2 ptBad1(2.0, 2.0);
config2.insert(key, ptBad1);
EXPECT(constraint.active(config2));
EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.evaluateError(ptBad1), tol));
EXPECT(assert_equal(Vector_(2, 1.0, 0.0), constraint.unwhitenedError(config2), tol));
EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol);
}
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_linearization ) {
Point2 pt(1.0, 2.0);
simulated2D::PoseKey key(1);
double mu = 1000.0;
Ordering ordering;
ordering += key;
eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
simulated2D::Values config1;
config1.insert(key, pt);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering);
GaussianFactor::shared_ptr expected1(new JacobianFactor(ordering[key], eye(2,2), zero(2), hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol));
simulated2D::Values config2;
Point2 ptBad(2.0, 2.0);
config2.insert(key, ptBad);
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering);
GaussianFactor::shared_ptr expected2(new JacobianFactor(ordering[key], eye(2,2), Vector_(2,-1.0,0.0), hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol));
}
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
// create a single-node graph with a soft and hard constraint to
// ensure that the hard constraint overrides the soft constraint
Point2 truth_pt(1.0, 2.0);
simulated2D::PoseKey key(1);
double mu = 1000.0;
eq2D::UnaryEqualityConstraint::shared_ptr constraint(
new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));
Point2 badPt(100.0, -200.0);
simulated2D::Prior::shared_ptr factor(
new simulated2D::Prior(badPt, soft_model, key));
shared_graph graph(new Graph());
graph->push_back(constraint);
graph->push_back(factor);
shared_values initValues(new simulated2D::Values());
initValues->insert(key, badPt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
simulated2D::Values expected;
expected.insert(key, truth_pt);
CHECK(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, odo_basics ) {
Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
simulated2D::PoseKey key1(1), key2(2);
double mu = 1000.0;
eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
simulated2D::Values config1;
config1.insert(key1, x1);
config1.insert(key2, x2);
EXPECT(constraint.active(config1));
EXPECT(assert_equal(zero(2), constraint.evaluateError(x1, x2), tol));
EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol);
simulated2D::Values config2;
Point2 x1bad(2.0, 2.0);
Point2 x2bad(2.0, 2.0);
config2.insert(key1, x1bad);
config2.insert(key2, x2bad);
EXPECT(constraint.active(config2));
EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol));
EXPECT(assert_equal(Vector_(2, -1.0, -1.0), constraint.unwhitenedError(config2), tol));
EXPECT_DOUBLES_EQUAL(2000.0, constraint.error(config2), tol);
}
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, odo_linearization ) {
Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
simulated2D::PoseKey key1(1), key2(2);
double mu = 1000.0;
Ordering ordering;
ordering += key1, key2;
eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
simulated2D::Values config1;
config1.insert(key1, x1);
config1.insert(key2, x2);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering);
GaussianFactor::shared_ptr expected1(
new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2],
eye(2,2), zero(2), hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol));
simulated2D::Values config2;
Point2 x1bad(2.0, 2.0);
Point2 x2bad(2.0, 2.0);
config2.insert(key1, x1bad);
config2.insert(key2, x2bad);
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering);
GaussianFactor::shared_ptr expected2(
new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2],
eye(2,2), Vector_(2, 1.0, 1.0), hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol));
}
/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
// create a two-node graph, connected by an odometry constraint, with
// a hard prior on one variable, and a conflicting soft prior
// on the other variable - the constraints should override the soft constraint
Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0);
simulated2D::PoseKey key1(1), key2(2);
// hard prior on x1
eq2D::UnaryEqualityConstraint::shared_ptr constraint1(
new eq2D::UnaryEqualityConstraint(truth_pt1, key1));
// soft prior on x2
Point2 badPt(100.0, -200.0);
simulated2D::Prior::shared_ptr factor(
new simulated2D::Prior(badPt, soft_model, key2));
// odometry constraint
eq2D::OdoEqualityConstraint::shared_ptr constraint2(
new eq2D::OdoEqualityConstraint(
truth_pt1.between(truth_pt2), key1, key2));
shared_graph graph(new Graph());
graph->push_back(constraint1);
graph->push_back(constraint2);
graph->push_back(factor);
shared_values initValues(new simulated2D::Values());
initValues->insert(key1, Point2());
initValues->insert(key2, badPt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
simulated2D::Values expected;
expected.insert(key1, truth_pt1);
expected.insert(key2, truth_pt2);
CHECK(assert_equal(expected, *actual, tol));
}
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, two_pose ) {
/*
* Determining a ground truth linear system
* with two poses seeing one landmark, with each pose
* constrained to a particular value
*/
shared_graph graph(new Graph());
simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1), l2(2);
Point2 pt_x1(1.0, 1.0),
pt_x2(5.0, 6.0);
graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1));
graph->add(eq2D::UnaryEqualityConstraint(pt_x2, x2));
Point2 z1(0.0, 5.0);
SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
graph->add(simulated2D::Measurement(z1, sigma, x1,l1));
Point2 z2(-4.0, 0.0);
graph->add(simulated2D::Measurement(z2, sigma, x2,l2));
graph->add(eq2D::PointEqualityConstraint(l1, l2));
shared_values initialEstimate(new simulated2D::Values());
initialEstimate->insert(x1, pt_x1);
initialEstimate->insert(x2, Point2());
initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate);
simulated2D::Values expected;
expected.insert(x1, pt_x1);
expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0));
expected.insert(x2, Point2(5.0, 6.0));
CHECK(assert_equal(expected, *actual, 1e-5));
}
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, map_warp ) {
// get a graph
shared_graph graph(new Graph());
// keys
simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1), l2(2);
// constant constraint on x1
Point2 pose1(1.0, 1.0);
graph->add(eq2D::UnaryEqualityConstraint(pose1, x1));
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1);
// measurement from x1 to l1
Point2 z1(0.0, 5.0);
graph->add(simulated2D::Measurement(z1, sigma, x1, l1));
// measurement from x2 to l2
Point2 z2(-4.0, 0.0);
graph->add(simulated2D::Measurement(z2, sigma, x2, l2));
// equality constraint between l1 and l2
graph->add(eq2D::PointEqualityConstraint(l1, l2));
// create an initial estimate
shared_values initialEstimate(new simulated2D::Values());
initialEstimate->insert(x1, Point2( 1.0, 1.0));
initialEstimate->insert(l1, Point2( 1.0, 6.0));
initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin
// optimize
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate);
simulated2D::Values expected;
expected.insert(x1, Point2(1.0, 1.0));
expected.insert(l1, Point2(1.0, 6.0));
expected.insert(l2, Point2(1.0, 6.0));
expected.insert(x2, Point2(5.0, 6.0));
CHECK(assert_equal(expected, *actual, tol));
}
// make a realistic calibration matrix
double fov = 60; // degrees
size_t w=640,h=480;
Cal3_S2 K(fov,w,h);
boost::shared_ptr<Cal3_S2> shK(new Cal3_S2(K));
// typedefs for visual SLAM example
typedef visualSLAM::Values VValues;
typedef boost::shared_ptr<VValues> shared_vconfig;
typedef visualSLAM::Graph VGraph;
typedef NonlinearOptimizer<VGraph,VValues> VOptimizer;
// factors for visual slam
typedef NonlinearEquality2<VValues, visualSLAM::PointKey> Point3Equality;
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, stereo_constrained ) {
// create initial estimates
Rot3 faceDownY(Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 0.0, 1.0,
0.0, 1.0, 0.0));
Pose3 pose1(faceDownY, Point3()); // origin, left camera
SimpleCamera camera1(K, pose1);
Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left
SimpleCamera camera2(K, pose2);
Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away
// keys
visualSLAM::PoseKey x1(1), x2(2);
visualSLAM::PointKey l1(1), l2(2);
// create graph
VGraph::shared_graph graph(new VGraph());
// create equality constraints for poses
graph->addPoseConstraint(1, camera1.pose());
graph->addPoseConstraint(2, camera2.pose());
// create factors
SharedDiagonal vmodel = noiseModel::Unit::Create(3);
graph->addMeasurement(camera1.project(landmark), vmodel, 1, 1, shK);
graph->addMeasurement(camera2.project(landmark), vmodel, 2, 2, shK);
// add equality constraint
graph->add(Point3Equality(l1, l2));
// create initial data
Point3 landmark1(0.5, 5.0, 0.0);
Point3 landmark2(1.5, 5.0, 0.0);
shared_vconfig initValues(new VValues());
initValues->insert(x1, pose1);
initValues->insert(x2, pose2);
initValues->insert(l1, landmark1);
initValues->insert(l2, landmark2);
// optimize
VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues);
// create config
VValues truthValues;
truthValues.insert(x1, camera1.pose());
truthValues.insert(x2, camera2.pose());
truthValues.insert(l1, landmark);
truthValues.insert(l2, landmark);
// check if correct
CHECK(assert_equal(truthValues, *actual, 1e-5));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */