Added nonlinear constraints to gtsam library
Alex Cunningham
parent
79c09708e8
commit
9160775d2a
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
Loading…
Reference in New Issue