Reimplemented nonlinear constraints to remove the old dependency on using boost function pointers and allow for inequality and bounding constraints. Added simple equality constraints in simulated2D and new set of tests. Removed/disabled old tests until they can be worked into new structure.
Alex Cunningham
parent
7e3442286e
commit
a55860eb1d
|
|
@ -22,9 +22,8 @@ headers += NonlinearFactor.h
|
|||
sources += NonlinearOptimizer.cpp
|
||||
|
||||
# Nonlinear constraints
|
||||
headers += NonlinearConstraint.h NonlinearConstraint-inl.h
|
||||
headers += NonlinearConstraint.h
|
||||
headers += NonlinearEquality.h
|
||||
check_PROGRAMS += tests/testNonlinearConstraint
|
||||
|
||||
# SQP
|
||||
if USE_LDL
|
||||
|
|
|
|||
|
|
@ -1,166 +0,0 @@
|
|||
/*
|
||||
* @file NonlinearConstraint-inl.h
|
||||
* @brief Implementation for NonlinearConstraints
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <iostream>
|
||||
#include <boost/bind.hpp>
|
||||
#include "NonlinearConstraint.h"
|
||||
|
||||
#define INSTANTIATE_NONLINEAR_CONSTRAINT(C) \
|
||||
INSTANTIATE_FACTOR_GRAPH(NonlinearConstraint<C>); \
|
||||
template class NonlinearConstraint<C>;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Implementations of base class
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
NonlinearConstraint<Config>::NonlinearConstraint(size_t dim, double mu) :
|
||||
NonlinearFactor<Config>(noiseModel::Constrained::All(dim)),
|
||||
mu_(fabs(mu))
|
||||
{
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config>
|
||||
double NonlinearConstraint<Config>::error(const Config& c) const {
|
||||
const Vector error_vector = unwhitenedError(c);
|
||||
return mu_ * inner_prod(error_vector, error_vector);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Implementations of unary nonlinear constraints
|
||||
/* ************************************************************************* */
|
||||
|
||||
template <class Config, class Key, class X>
|
||||
NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
|
||||
Vector (*g)(const Config& config),
|
||||
const Key& key,
|
||||
Matrix (*gradG)(const Config& config),
|
||||
size_t dim, double mu) :
|
||||
NonlinearConstraint<Config>(dim, mu),
|
||||
G_(boost::bind(gradG, _1)), g_(boost::bind(g, _1)), key_(key)
|
||||
{
|
||||
this->keys_.push_back(key);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config, class Key, class X>
|
||||
NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
|
||||
boost::function<Vector(const Config& config)> g,
|
||||
const Key& key,
|
||||
boost::function<Matrix(const Config& config)> gradG,
|
||||
size_t dim, double mu) :
|
||||
NonlinearConstraint<Config>(dim, mu),
|
||||
G_(gradG), g_(g), key_(key)
|
||||
{
|
||||
this->keys_.push_back(key);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config, class Key, class X>
|
||||
void NonlinearConstraint1<Config, Key, X>::print(const std::string& s) const {
|
||||
std::cout << "NonlinearConstraint1 [" << s << "]: Dim: " << this->dim()
|
||||
<< " mu: " << this->mu_ << "\n"
|
||||
<< " Key : " << (std::string) this->key_ << "\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config, class Key, class X>
|
||||
bool NonlinearConstraint1<Config, Key, X>::equals(const Factor<Config>& f, double tol) const {
|
||||
const NonlinearConstraint1<Config, Key, X>* p = dynamic_cast<const NonlinearConstraint1<Config, Key, X>*> (&f);
|
||||
if (p == NULL) return false;
|
||||
if (!(key_ == p->key_)) return false;
|
||||
if (fabs(this->mu_ - p->mu_ ) > tol) return false;
|
||||
return this->dim() == p->dim();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config, class Key, class X>
|
||||
GaussianFactor::shared_ptr
|
||||
NonlinearConstraint1<Config, Key, X>::linearize(const Config& config) const {
|
||||
Vector g = -1.0 * g_(config);
|
||||
Matrix grad = G_(config);
|
||||
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
|
||||
GaussianFactor::shared_ptr factor(new GaussianFactor(this->key_, grad, g, model));
|
||||
return factor;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Implementations of binary nonlinear constraints
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config, class Key1, class X1, class Key2, class X2>
|
||||
NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
|
||||
Vector (*g)(const Config& config),
|
||||
const Key1& key1,
|
||||
Matrix (*G1)(const Config& config),
|
||||
const Key2& key2,
|
||||
Matrix (*G2)(const Config& config),
|
||||
size_t dim, double mu)
|
||||
: NonlinearConstraint<Config>(dim, mu),
|
||||
G1_(boost::bind(G1, _1)), G2_(boost::bind(G2, _1)), g_(boost::bind(g, _1)),
|
||||
key1_(key1), key2_(key2)
|
||||
{
|
||||
this->keys_.push_back(key1);
|
||||
this->keys_.push_back(key2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config, class Key1, class X1, class Key2, class X2>
|
||||
NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
|
||||
boost::function<Vector(const Config& config)> g,
|
||||
const Key1& key1,
|
||||
boost::function<Matrix(const Config& config)> G1,
|
||||
const Key2& key2,
|
||||
boost::function<Matrix(const Config& config)> G2,
|
||||
size_t dim, double mu)
|
||||
: NonlinearConstraint<Config>(dim, mu),
|
||||
G1_(G1), G2_(G2), g_(g),
|
||||
key1_(key1), key2_(key2)
|
||||
{
|
||||
this->keys_.push_back(key1);
|
||||
this->keys_.push_back(key2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config, class Key1, class X1, class Key2, class X2>
|
||||
void NonlinearConstraint2<Config, Key1, X1, Key2, X2>::print(const std::string& s) const {
|
||||
std::cout << "NonlinearConstraint2 [" << s << "]: Dim: " << this->dim()
|
||||
<< " mu: " << this->mu_ << "\n"
|
||||
<< " Key1 : " << (std::string) this->key1_ << "\n"
|
||||
<< " Key2 : " << (std::string) this->key2_ << "\n";
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class Config, class Key1, class X1, class Key2, class X2>
|
||||
bool NonlinearConstraint2<Config, Key1, X1, Key2, X2>::equals(const Factor<Config>& f, double tol) const {
|
||||
const NonlinearConstraint2<Config, Key1, X1, Key2, X2>* p = dynamic_cast<const NonlinearConstraint2<Config, Key1, X1, Key2, X2>*> (&f);
|
||||
if (p == NULL) return false;
|
||||
if (!(key1_ == p->key1_)) return false;
|
||||
if (!(key2_ == p->key2_)) return false;
|
||||
if (fabs(this->mu_ - p->mu_ ) > tol) return false;
|
||||
return this->dim() == p->dim();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Config, class Key1, class X1, class Key2, class X2>
|
||||
GaussianFactor::shared_ptr
|
||||
NonlinearConstraint2<Config, Key1, X1, Key2, X2>::linearize(const Config& config) const {
|
||||
Vector g = -1.0 * g_(config);
|
||||
Matrix grad1 = G1_(config);
|
||||
Matrix grad2 = G2_(config);
|
||||
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
|
||||
GaussianFactor::shared_ptr factor(new GaussianFactor(this->key1_, grad1, this->key2_, grad2, g, model));
|
||||
return factor;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -31,7 +31,8 @@ template <class Config>
|
|||
class NonlinearConstraint : public NonlinearFactor<Config> {
|
||||
|
||||
protected:
|
||||
|
||||
typedef NonlinearConstraint<Config> This;
|
||||
typedef NonlinearFactor<Config> Base;
|
||||
double mu_; // gain for quadratic merit function
|
||||
|
||||
public:
|
||||
|
|
@ -40,7 +41,8 @@ public:
|
|||
* @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);
|
||||
NonlinearConstraint(size_t dim, double mu = 1000.0):
|
||||
Base(noiseModel::Constrained::All(dim)), mu_(fabs(mu)) {}
|
||||
|
||||
/** returns the gain mu */
|
||||
double mu() const { return mu_; }
|
||||
|
|
@ -49,10 +51,29 @@ public:
|
|||
virtual void print(const std::string& s = "") const=0;
|
||||
|
||||
/** Check if two factors are equal */
|
||||
virtual bool equals(const Factor<Config>& f, double tol=1e-9) const=0;
|
||||
virtual bool equals(const Factor<Config>& 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 Config& c) const;
|
||||
virtual double error(const Config& c) const {
|
||||
const Vector error_vector = unwhitenedError(c);
|
||||
if (active(c))
|
||||
return mu_ * inner_prod(error_vector, error_vector);
|
||||
else return 0.0;
|
||||
}
|
||||
|
||||
/** Raw error vector function g(x) */
|
||||
virtual Vector unwhitenedError(const Config& c) const = 0;
|
||||
|
||||
/**
|
||||
* active set check, defines what type of constraint this is
|
||||
* By default, the constraint is always active, so it is an equality constraint
|
||||
* @return true if the constraint is active
|
||||
*/
|
||||
virtual bool active(const Config& c) const { return true; }
|
||||
|
||||
/**
|
||||
* Linearizes around a given config
|
||||
|
|
@ -64,31 +85,14 @@ public:
|
|||
|
||||
|
||||
/**
|
||||
* A unary constraint with arbitrary cost and jacobian functions
|
||||
* This is an example class designed for easy testing, but real uses should probably
|
||||
* subclass NonlinearConstraint and implement virtual functions directly
|
||||
* A unary constraint that defaults to an equality constraint
|
||||
*/
|
||||
template <class Config, class Key, class X>
|
||||
class NonlinearConstraint1 : public NonlinearConstraint<Config> {
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Calculates the jacobian of the constraint function
|
||||
* returns a pxn matrix
|
||||
* Use boost.bind to create the function object
|
||||
* @param config to use for linearization
|
||||
* @return the jacobian of the constraint in terms of key
|
||||
*/
|
||||
boost::function<Matrix(const Config& config)> G_;
|
||||
|
||||
/** calculates the constraint function of the current config
|
||||
* If the value is zero, the constraint is not active
|
||||
* Use boost.bind to create the function object
|
||||
* @param config is a configuration of all the variables
|
||||
* @return the cost for each of p constraints, arranged in a vector
|
||||
*/
|
||||
boost::function<Vector(const Config& config)> g_;
|
||||
protected:
|
||||
typedef NonlinearConstraint1<Config,Key,X> This;
|
||||
typedef NonlinearConstraint<Config> Base;
|
||||
|
||||
/** key for the constrained variable */
|
||||
Key key_;
|
||||
|
|
@ -98,44 +102,52 @@ public:
|
|||
/**
|
||||
* Basic constructor
|
||||
* @param key is the identifier for the variable constrained
|
||||
* @param G gives the jacobian of the constraint function
|
||||
* @param g is the constraint function
|
||||
* @param dim is the size of the constraint (p)
|
||||
* @param mu is the gain for the factor
|
||||
*/
|
||||
NonlinearConstraint1(
|
||||
Vector (*g)(const Config& config),
|
||||
const Key& key,
|
||||
Matrix (*G)(const Config& config),
|
||||
size_t dim,
|
||||
double mu = 1000.0);
|
||||
NonlinearConstraint1(const Key& key, size_t dim, double mu = 1000.0)
|
||||
: Base(dim, mu), key_(key) {
|
||||
this->keys_.push_back(key);
|
||||
}
|
||||
|
||||
/**
|
||||
* Basic constructor with boost function pointers
|
||||
* @param key is the identifier for the variable constrained
|
||||
* @param G gives the jacobian of the constraint function
|
||||
* @param g is the constraint function as a boost function pointer
|
||||
* @param dim is the size of the constraint (p)
|
||||
* @param mu is the gain for the factor
|
||||
*/
|
||||
NonlinearConstraint1(
|
||||
boost::function<Vector(const Config& config)> g,
|
||||
const Key& key,
|
||||
boost::function<Matrix(const Config& config)> G,
|
||||
size_t dim,
|
||||
double mu = 1000.0);
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& s = "") const;
|
||||
/** Check if two factors are equal. Note type is Factor and needs cast. */
|
||||
virtual bool equals(const Factor<Config>& 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_);
|
||||
}
|
||||
|
||||
/** Check if two factors are equal */
|
||||
bool equals(const Factor<Config>& f, double tol=1e-9) const;
|
||||
|
||||
/** Error function */
|
||||
virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); }
|
||||
/** error function g(x) */
|
||||
inline Vector unwhitenedError(const Config& x) const {
|
||||
const Key& j = key_;
|
||||
const X& xj = x[j];
|
||||
return evaluateError(xj);
|
||||
}
|
||||
|
||||
/** Linearize from config */
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
|
||||
if (!active(c)) {
|
||||
boost::shared_ptr<GaussianFactor> factor;
|
||||
return factor;
|
||||
}
|
||||
const Key& j = key_;
|
||||
const X& x = c[j];
|
||||
Matrix grad;
|
||||
Vector g = -1.0 * evaluateError(x, grad);
|
||||
SharedDiagonal model = noiseModel::Constrained::All(this->dim());
|
||||
return GaussianFactor::shared_ptr(new GaussianFactor(this->key_, grad, g, model));
|
||||
}
|
||||
|
||||
/** g(x) with optional derivative */
|
||||
virtual Vector evaluateError(const X& x, boost::optional<Matrix&> H =
|
||||
boost::none) const = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -144,25 +156,9 @@ public:
|
|||
template <class Config, class Key1, class X1, class Key2, class X2>
|
||||
class NonlinearConstraint2 : public NonlinearConstraint<Config> {
|
||||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Calculates the jacobians of the constraint function in terms of
|
||||
* the first and second variables
|
||||
* returns a pxn matrix
|
||||
* @param config to use for linearization
|
||||
* @return the jacobian of the constraint in terms of key
|
||||
*/
|
||||
boost::function<Matrix(const Config& config)> G1_;
|
||||
boost::function<Matrix(const Config& config)> G2_;
|
||||
|
||||
/** calculates the constraint function of the current config
|
||||
* If the value is zero, the constraint is not active
|
||||
* Use boost.bind to create the function object
|
||||
* @param config is a configuration of all the variables
|
||||
* @return the cost for each of p constraints, arranged in a vector
|
||||
*/
|
||||
boost::function<Vector(const Config& config)> g_;
|
||||
protected:
|
||||
typedef NonlinearConstraint2<Config,Key1,X1,Key2,X2> This;
|
||||
typedef NonlinearConstraint<Config> Base;
|
||||
|
||||
/** keys for the constrained variables */
|
||||
Key1 key1_;
|
||||
|
|
@ -172,50 +168,59 @@ public:
|
|||
|
||||
/**
|
||||
* Basic constructor
|
||||
* @param key is the identifier for the variable constrained
|
||||
* @param G gives the jacobian of the constraint function
|
||||
* @param g is the constraint function
|
||||
* @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(
|
||||
Vector (*g)(const Config& config),
|
||||
const Key1& key1,
|
||||
Matrix (*G1)(const Config& config),
|
||||
const Key2& key2,
|
||||
Matrix (*G2)(const Config& config),
|
||||
size_t dim,
|
||||
double mu = 1000.0);
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Basic constructor with direct function objects
|
||||
* Use boost.bind to construct the function objects
|
||||
* @param key is the identifier for the variable constrained
|
||||
* @param G gives the jacobian of the constraint function
|
||||
* @param g is the constraint function
|
||||
* @param dim is the size of the constraint (p)
|
||||
* @param mu is the gain for the factor
|
||||
*/
|
||||
NonlinearConstraint2(
|
||||
boost::function<Vector(const Config& config)> g,
|
||||
const Key1& key1,
|
||||
boost::function<Matrix(const Config& config)> G1,
|
||||
const Key2& key2,
|
||||
boost::function<Matrix(const Config& config)> G2,
|
||||
size_t dim,
|
||||
double mu = 1000.0);
|
||||
/* 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;
|
||||
}
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& s = "") const;
|
||||
/** Check if two factors are equal. Note type is Factor and needs cast. */
|
||||
virtual bool equals(const Factor<Config>& 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_);
|
||||
}
|
||||
|
||||
/** Check if two factors are equal */
|
||||
bool equals(const Factor<Config>& f, double tol=1e-9) const;
|
||||
|
||||
/** Error function */
|
||||
virtual inline Vector unwhitenedError(const Config& c) const { return g_(c); }
|
||||
/** error function g(x) */
|
||||
inline Vector unwhitenedError(const Config& x) const {
|
||||
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 */
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(const Config& c) const;
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Config& c) const {
|
||||
if (!active(c)) {
|
||||
boost::shared_ptr<GaussianFactor> 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());
|
||||
return GaussianFactor::shared_ptr(new GaussianFactor(j1, grad1, j2, grad2, g, model));
|
||||
}
|
||||
|
||||
/** g(x) with optional derivative2 */
|
||||
virtual Vector evaluateError(const X1& x1, const X2& x2,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const = 0;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -236,8 +236,6 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* A Gaussian nonlinear factor that takes 2 parameters
|
||||
* Note: cannot be serialized as contains function pointers
|
||||
* Specialized derived classes could do this
|
||||
*/
|
||||
template<class Config, class Key1, class X1, class Key2, class X2>
|
||||
class NonlinearFactor2: public NonlinearFactor<Config> {
|
||||
|
|
@ -353,8 +351,6 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* A Gaussian nonlinear factor that takes 3 parameters
|
||||
* Note: cannot be serialized as contains function pointers
|
||||
* Specialized derived classes could do this
|
||||
*/
|
||||
template<class Config, class Key1, class X1, class Key2, class X2, class Key3, class X3>
|
||||
class NonlinearFactor3: public NonlinearFactor<Config> {
|
||||
|
|
|
|||
|
|
@ -25,6 +25,106 @@ using namespace boost::assign;
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* *********************************************************************
|
||||
* Example from SQP testing:
|
||||
*
|
||||
* This example uses a nonlinear objective function and
|
||||
* nonlinear equality constraint. The formulation is actually
|
||||
* the Cholesky form that creates the full Hessian explicitly,
|
||||
* and isn't expecially compatible with our machinery.
|
||||
*/
|
||||
TEST (NonlinearConstraint, problem1_cholesky ) {
|
||||
bool verbose = false;
|
||||
// use a nonlinear function of f(x) = x^2+y^2
|
||||
// nonlinear equality constraint: g(x) = x^2-5-y=0
|
||||
// Lagrangian: f(x) + \lambda*g(x)
|
||||
|
||||
// Symbols
|
||||
Symbol x1("x1"), y1("y1"), L1("L1");
|
||||
|
||||
// state structure: [x y \lambda]
|
||||
VectorConfig init, state;
|
||||
init.insert(x1, Vector_(1, 1.0));
|
||||
init.insert(y1, Vector_(1, 1.0));
|
||||
init.insert(L1, Vector_(1, 1.0));
|
||||
state = init;
|
||||
|
||||
if (verbose) init.print("Initial State");
|
||||
|
||||
// loop until convergence
|
||||
int maxIt = 10;
|
||||
for (int i = 0; i<maxIt; ++i) {
|
||||
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
|
||||
|
||||
// extract the states
|
||||
double x, y, lambda;
|
||||
x = state[x1](0);
|
||||
y = state[y1](0);
|
||||
lambda = state[L1](0);
|
||||
|
||||
// calculate the components
|
||||
Matrix H1, H2, gradG;
|
||||
Vector gradL, gx;
|
||||
|
||||
// hessian of lagrangian function, in two columns:
|
||||
H1 = Matrix_(2,1,
|
||||
2.0+2.0*lambda,
|
||||
0.0);
|
||||
H2 = Matrix_(2,1,
|
||||
0.0,
|
||||
2.0);
|
||||
|
||||
// deriviative of lagrangian function
|
||||
gradL = Vector_(2,
|
||||
2.0*x*(1+lambda),
|
||||
2.0*y-lambda);
|
||||
|
||||
// constraint derivatives
|
||||
gradG = Matrix_(2,1,
|
||||
2.0*x,
|
||||
0.0);
|
||||
|
||||
// constraint value
|
||||
gx = Vector_(1,
|
||||
x*x-5-y);
|
||||
|
||||
// create a factor for the states
|
||||
GaussianFactor::shared_ptr f1(new
|
||||
GaussianFactor(x1, H1, y1, H2, L1, gradG, gradL, probModel2));
|
||||
|
||||
// create a factor for the lagrange multiplier
|
||||
GaussianFactor::shared_ptr f2(new
|
||||
GaussianFactor(x1, -sub(gradG, 0, 1, 0, 1),
|
||||
y1, -sub(gradG, 1, 2, 0, 1), -gx, constraintModel1));
|
||||
|
||||
// construct graph
|
||||
GaussianFactorGraph fg;
|
||||
fg.push_back(f1);
|
||||
fg.push_back(f2);
|
||||
if (verbose) fg.print("Graph");
|
||||
|
||||
// solve
|
||||
Ordering ord;
|
||||
ord += x1, y1, L1;
|
||||
VectorConfig delta = fg.optimize(ord).scale(-1.0);
|
||||
if (verbose) delta.print("Delta");
|
||||
|
||||
// update initial estimate
|
||||
VectorConfig newState = expmap(state, delta);
|
||||
state = newState;
|
||||
|
||||
if (verbose) state.print("Updated State");
|
||||
}
|
||||
|
||||
// verify that it converges to the nearest optimal point
|
||||
VectorConfig expected;
|
||||
expected.insert(L1, Vector_(1, -1.0));
|
||||
expected.insert(x1, Vector_(1, 2.12));
|
||||
expected.insert(y1, Vector_(1, -0.5));
|
||||
CHECK(assert_equal(expected,state, 1e-2));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Example of a single Constrained QP problem from the matlab testCQP.m file.
|
||||
TEST( matrix, CQP_example ) {
|
||||
|
|
|
|||
|
|
@ -1,373 +0,0 @@
|
|||
/**
|
||||
* @file testNonlinearConstraint.cpp
|
||||
* @brief Tests for nonlinear constraints handled via SQP
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <list>
|
||||
#include <boost/bind.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <VectorConfig.h>
|
||||
#include <NonlinearConstraint.h>
|
||||
#include <NonlinearConstraint-inl.h>
|
||||
#include <TupleConfig-inl.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace boost::assign;
|
||||
|
||||
typedef TypedSymbol<Vector, 'x'> Key;
|
||||
typedef LieConfig<Key, Vector> VecConfig;
|
||||
typedef NonlinearConstraint1<VecConfig, Key, Vector> NLC1;
|
||||
typedef NonlinearConstraint2<VecConfig, Key, Vector, Key, Vector> NLC2;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// unary functions with scalar variables
|
||||
/* ************************************************************************* */
|
||||
namespace test1 {
|
||||
|
||||
/** p = 1, g(x) = x^2-5 = 0 */
|
||||
Vector g(const VecConfig& config, const list<Key>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
return Vector_(1, x * x - 5);
|
||||
}
|
||||
|
||||
/** p = 1, jacobianG(x) = 2x */
|
||||
Matrix G(const VecConfig& config, const list<Key>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
return Matrix_(1, 1, 2 * x);
|
||||
}
|
||||
|
||||
} // \namespace test1
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_scalar_construction ) {
|
||||
// construct a constraint on x
|
||||
size_t p = 1;
|
||||
Key x1(1);
|
||||
list<Key> keys; keys += x1;
|
||||
NLC1 c1(boost::bind(test1::g, _1, keys),
|
||||
x1, boost::bind(test1::G, _1, keys),
|
||||
p, 10.0);
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VecConfig config;
|
||||
config.insert(x1, Vector_(1, 1.0));
|
||||
|
||||
// calculate the error
|
||||
Vector actualVec = c1.unwhitenedError(config);
|
||||
Vector expectedVec = Vector_(1, -4.0);
|
||||
CHECK(assert_equal(actualVec, expectedVec, 1e-5));
|
||||
|
||||
double actError = c1.error(config);
|
||||
double expError = 160.0;
|
||||
DOUBLES_EQUAL(expError, actError, 1e-5);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_scalar_linearize ) {
|
||||
size_t p = 1;
|
||||
Key x1(1);
|
||||
list<Key> keys; keys += x1;
|
||||
NLC1 c1(boost::bind(test1::g, _1, keys),
|
||||
x1, boost::bind(test1::G, _1, keys),
|
||||
p);
|
||||
|
||||
// get a configuration to use for linearization
|
||||
VecConfig realconfig;
|
||||
realconfig.insert(x1, Vector_(1, 1.0));
|
||||
|
||||
// linearize the system
|
||||
GaussianFactor::shared_ptr linfactor = c1.linearize(realconfig);
|
||||
|
||||
// verify
|
||||
SharedDiagonal model = noiseModel::Constrained::All(p);
|
||||
Matrix Ax1 = Matrix_(p,1, 2.0);
|
||||
Vector rhs = Vector_(p, 4.0);
|
||||
GaussianFactor expectedFactor(x1, Ax1, rhs, model);
|
||||
|
||||
CHECK(assert_equal(*linfactor, expectedFactor));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint1, unary_scalar_equal ) {
|
||||
Key x(0), y(1);
|
||||
list<Key> keys1, keys2; keys1 += x; keys2 += y;
|
||||
NLC1
|
||||
c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1),
|
||||
c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1),
|
||||
c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2),
|
||||
c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1);
|
||||
|
||||
CHECK(assert_equal(c1, c2));
|
||||
CHECK(assert_equal(c2, c1));
|
||||
CHECK(!c1.equals(c3));
|
||||
CHECK(!c1.equals(c4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// binary functions with scalar variables
|
||||
/* ************************************************************************* */
|
||||
namespace test2 {
|
||||
|
||||
/** p = 1, g(x) = x^2-5 -y = 0 */
|
||||
Vector g(const VecConfig& config, const list<Key>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
double y = config[keys.back()](0);
|
||||
return Vector_(1, x * x - 5.0 - y);
|
||||
}
|
||||
|
||||
/** jacobian for x, jacobianG(x,y) in x: 2x*/
|
||||
Matrix G1(const VecConfig& config, const list<Key>& keys) {
|
||||
double x = config[keys.front()](0);
|
||||
return Matrix_(1, 1, 2.0 * x);
|
||||
}
|
||||
|
||||
/** jacobian for y, jacobianG(x,y) in y: -1 */
|
||||
Matrix G2(const VecConfig& config, const list<Key>& keys) {
|
||||
return Matrix_(1, 1, -1.0);
|
||||
}
|
||||
|
||||
} // \namespace test2
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_scalar_construction ) {
|
||||
// construct a constraint on x and y
|
||||
size_t p = 1;
|
||||
Key x0(0), x1(1);
|
||||
list<Key> keys; keys += x0, x1;
|
||||
NLC2 c1(
|
||||
boost::bind(test2::g, _1, keys),
|
||||
x0, boost::bind(test2::G1, _1, keys),
|
||||
x1, boost::bind(test2::G1, _1, keys),
|
||||
p);
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VecConfig config;
|
||||
config.insert(x0, Vector_(1, 1.0));
|
||||
config.insert(x1, Vector_(1, 2.0));
|
||||
|
||||
// calculate the error
|
||||
Vector actual = c1.unwhitenedError(config);
|
||||
Vector expected = Vector_(1.0, -6.0);
|
||||
CHECK(assert_equal(actual, expected, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_scalar_linearize ) {
|
||||
// create a constraint
|
||||
size_t p = 1;
|
||||
Key x0(0), x1(1);
|
||||
list<Key> keys; keys += x0, x1;
|
||||
NLC2 c1(
|
||||
boost::bind(test2::g, _1, keys),
|
||||
x0, boost::bind(test2::G1, _1, keys),
|
||||
x1, boost::bind(test2::G2, _1, keys),
|
||||
p);
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VecConfig realconfig;
|
||||
realconfig.insert(x0, Vector_(1, 1.0));
|
||||
realconfig.insert(x1, Vector_(1, 2.0));
|
||||
|
||||
// linearize the system
|
||||
GaussianFactor::shared_ptr actualFactor = c1.linearize(realconfig);
|
||||
|
||||
// verify
|
||||
Matrix Ax0 = Matrix_(1,1, 2.0),
|
||||
Ax1 = Matrix_(1,1,-1.0);
|
||||
Vector rhs = Vector_(1, 6.0);
|
||||
SharedDiagonal expModel = noiseModel::Constrained::All(p);
|
||||
GaussianFactor expFactor(x0,Ax0, x1, Ax1,rhs, expModel);
|
||||
CHECK(assert_equal(expFactor, *actualFactor));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_scalar_equal ) {
|
||||
list<Key> keys1, keys2, keys3;
|
||||
Key x0(0), x1(1), x2(2);
|
||||
keys1 += x0, x1; keys2 += x1, x0; keys3 += x0;
|
||||
NLC2
|
||||
c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1),
|
||||
c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1),
|
||||
c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1),
|
||||
c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3);
|
||||
|
||||
CHECK(assert_equal(c1, c2));
|
||||
CHECK(assert_equal(c2, c1));
|
||||
CHECK(!c1.equals(c3));
|
||||
CHECK(!c1.equals(c4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Inequality tests - DISABLED
|
||||
/* ************************************************************************* */
|
||||
//namespace inequality1 {
|
||||
//
|
||||
// /** p = 1, g(x) x^2 - 5 > 0 */
|
||||
// Vector g(const VecConfig& config, const Key& key) {
|
||||
// double x = config[key](0);
|
||||
// double g = x * x - 5;
|
||||
// return Vector_(1, g); // return the actual cost
|
||||
// }
|
||||
//
|
||||
// /** p = 1, jacobianG(x) = 2*x */
|
||||
// Matrix G(const VecConfig& config, const Key& key) {
|
||||
// double x = config[key](0);
|
||||
// return Matrix_(1, 1, 2 * x);
|
||||
// }
|
||||
//
|
||||
//} // \namespace inequality1
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//TEST( NonlinearConstraint1, unary_inequality ) {
|
||||
// size_t p = 1;
|
||||
// Key x0(0);
|
||||
// NLC1 c1(boost::bind(inequality1::g, _1, x0),
|
||||
// x0, boost::bind(inequality1::G, _1, x0),
|
||||
// p, false); // inequality constraint
|
||||
//
|
||||
// // get configurations to use for evaluation
|
||||
// VecConfig config1, config2;
|
||||
// config1.insert(x0, Vector_(1, 10.0)); // should be inactive
|
||||
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
|
||||
//
|
||||
// // check error
|
||||
// CHECK(!c1.active(config1));
|
||||
// Vector actualError2 = c1.unwhitenedError(config2);
|
||||
// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
|
||||
// CHECK(c1.active(config2));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//TEST( NonlinearConstraint1, unary_inequality_linearize ) {
|
||||
// size_t p = 1;
|
||||
// Key x0(0);
|
||||
// NLC1 c1(boost::bind(inequality1::g, _1, x0),
|
||||
// x0, boost::bind(inequality1::G, _1, x0),
|
||||
// p, false); // inequality constraint
|
||||
//
|
||||
// // get configurations to use for linearization
|
||||
// VecConfig config1, config2;
|
||||
// config1.insert(x0, Vector_(1, 10.0)); // should have zero error
|
||||
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
|
||||
//
|
||||
// // linearize for inactive constraint
|
||||
// GaussianFactor::shared_ptr actualFactor1 = c1.linearize(config1);
|
||||
//
|
||||
// // check if the factor is active
|
||||
// CHECK(!c1.active(config1));
|
||||
//
|
||||
// // linearize for active constraint
|
||||
// GaussianFactor::shared_ptr actualFactor2 = c1.linearize(config2);
|
||||
// CHECK(c1.active(config2));
|
||||
//
|
||||
// // verify
|
||||
// Vector sigmas = Vector_(2, 1.0, 0.0);
|
||||
// SharedDiagonal model = noiseModel::Constrained::MixedSigmas(sigmas);
|
||||
// GaussianFactor expectedFactor(x0, Matrix_(2,1, 6.0, 2.0),
|
||||
// L1, Matrix_(2,1, 1.0, 0.0),
|
||||
// Vector_(2, 0.0, 4.0), model);
|
||||
//
|
||||
// CHECK(assert_equal(*actualFactor2, expectedFactor));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Binding arbitrary functions
|
||||
/* ************************************************************************* */
|
||||
//namespace binding1 {
|
||||
//
|
||||
// /** p = 1, g(x) x^2 - r > 0 */
|
||||
// Vector g(double r, const VecConfig& config, const Key& key) {
|
||||
// double x = config[key](0);
|
||||
// double g = x * x - r;
|
||||
// return Vector_(1, g); // return the actual cost
|
||||
// }
|
||||
//
|
||||
// /** p = 1, jacobianG(x) = 2*x */
|
||||
// Matrix G(double coeff, const VecConfig& config,
|
||||
// const Key& key) {
|
||||
// double x = config[key](0);
|
||||
// return Matrix_(1, 1, coeff * x);
|
||||
// }
|
||||
//
|
||||
//} // \namespace binding1
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//TEST( NonlinearConstraint1, unary_binding ) {
|
||||
// size_t p = 1;
|
||||
// double coeff = 2;
|
||||
// double radius = 5;
|
||||
// Key x0(0);
|
||||
// NLC1 c1(boost::bind(binding1::g, radius, _1, x0),
|
||||
// x0, boost::bind(binding1::G, coeff, _1, x0),
|
||||
// p, false); // inequality constraint
|
||||
//
|
||||
// // get configurations to use for evaluation
|
||||
// VecConfig config1, config2;
|
||||
// config1.insert(x0, Vector_(1, 10.0)); // should have zero error
|
||||
// config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
|
||||
//
|
||||
// // check error
|
||||
// CHECK(!c1.active(config1));
|
||||
// Vector actualError2 = c1.unwhitenedError(config2);
|
||||
// CHECK(assert_equal(actualError2, Vector_(1, -4.0, 1e-9)));
|
||||
// CHECK(c1.active(config2));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace binding2 {
|
||||
|
||||
/** p = 1, g(x) = x^2-5 -y = 0 */
|
||||
Vector g(double r, const VecConfig& config, const Key& k1, const Key& k2) {
|
||||
double x = config[k1](0);
|
||||
double y = config[k2](0);
|
||||
return Vector_(1, x * x - r - y);
|
||||
}
|
||||
|
||||
/** jacobian for x, jacobianG(x,y) in x: 2x*/
|
||||
Matrix G1(double c, const VecConfig& config, const Key& key) {
|
||||
double x = config[key](0);
|
||||
return Matrix_(1, 1, c * x);
|
||||
}
|
||||
|
||||
/** jacobian for y, jacobianG(x,y) in y: -1 */
|
||||
Matrix G2(double c, const VecConfig& config) {
|
||||
return Matrix_(1, 1, -1.0 * c);
|
||||
}
|
||||
|
||||
} // \namespace binding2
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearConstraint2, binary_binding ) {
|
||||
// construct a constraint on x and y
|
||||
size_t p = 1;
|
||||
double a = 2.0;
|
||||
double b = 1.0;
|
||||
double r = 5.0;
|
||||
Key x0(0), x1(1);
|
||||
NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1),
|
||||
x0, boost::bind(binding2::G1, a, _1, x0),
|
||||
x1, boost::bind(binding2::G2, b, _1),
|
||||
p);
|
||||
|
||||
// get a configuration to use for finding the error
|
||||
VecConfig config;
|
||||
config.insert(x0, Vector_(1, 1.0));
|
||||
config.insert(x1, Vector_(1, 2.0));
|
||||
|
||||
// calculate the error
|
||||
Vector actual = c1.unwhitenedError(config);
|
||||
Vector expected = Vector_(1.0, -6.0);
|
||||
CHECK(assert_equal(actual, expected, 1e-5));
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -56,6 +56,7 @@ namespace gtsam {
|
|||
*/
|
||||
template<class Cfg = Config, class Key = PoseKey>
|
||||
struct GenericPrior: public NonlinearFactor1<Cfg, Key, Point2> {
|
||||
typedef boost::shared_ptr<GenericPrior<Cfg, Key> > shared_ptr;
|
||||
|
||||
Point2 z_;
|
||||
|
||||
|
|
@ -76,6 +77,7 @@ namespace gtsam {
|
|||
template<class Cfg = Config, class Key = PoseKey>
|
||||
struct GenericOdometry: public NonlinearFactor2<Cfg, Key, Point2, Key,
|
||||
Point2> {
|
||||
typedef boost::shared_ptr<GenericOdometry<Cfg, Key> > shared_ptr;
|
||||
Point2 z_;
|
||||
|
||||
GenericOdometry(const Point2& z, const SharedGaussian& model,
|
||||
|
|
@ -97,7 +99,7 @@ namespace gtsam {
|
|||
class GenericMeasurement: public NonlinearFactor2<Cfg, XKey, Point2, LKey,
|
||||
Point2> {
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<GenericMeasurement<Cfg, XKey, LKey> > shared_ptr;
|
||||
Point2 z_;
|
||||
|
||||
GenericMeasurement(const Point2& z, const SharedGaussian& model,
|
||||
|
|
|
|||
|
|
@ -0,0 +1,67 @@
|
|||
/**
|
||||
* @file simulated2DConstraints.h
|
||||
* @brief measurement functions and constraint definitions for simulated 2D robot
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <NonlinearConstraint.h>
|
||||
#include <simulated2D.h>
|
||||
|
||||
// \namespace
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace simulated2D {
|
||||
|
||||
/**
|
||||
* Unary factor encoding a hard equality on a Point
|
||||
*/
|
||||
template<class Cfg = Config, class Key = PoseKey>
|
||||
struct GenericUnaryEqualityConstraint: public NonlinearConstraint1<Cfg, Key, Point2> {
|
||||
typedef NonlinearConstraint1<Cfg, Key, Point2> Base;
|
||||
typedef boost::shared_ptr<GenericUnaryEqualityConstraint<Cfg, Key> > shared_ptr;
|
||||
|
||||
Point2 z_;
|
||||
|
||||
GenericUnaryEqualityConstraint(const Point2& z, const Key& key, double mu = 1000.0) :
|
||||
Base(key, 2, mu), z_(z) {
|
||||
}
|
||||
|
||||
Vector evaluateError(const Point2& x, boost::optional<Matrix&> H =
|
||||
boost::none) const {
|
||||
return (prior(x, H) - z_).vector();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Binary factor simulating "odometry" between two Vectors
|
||||
*/
|
||||
template<class Cfg = Config, class Key = PoseKey>
|
||||
struct GenericOdoHardEqualityConstraint: public NonlinearConstraint2<Cfg, Key, Point2, Key, Point2> {
|
||||
typedef NonlinearConstraint2<Cfg, Key, Point2, Key, Point2> Base;
|
||||
typedef boost::shared_ptr<GenericOdoHardEqualityConstraint<Cfg,Key> > shared_ptr;
|
||||
Point2 z_;
|
||||
|
||||
GenericOdoHardEqualityConstraint(
|
||||
const Point2& z, const Key& i1, const Key& i2, double mu = 1000.0) :
|
||||
Base (i1, i2, 2, mu), z_(z) {
|
||||
}
|
||||
|
||||
Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
|
||||
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
||||
return (odo(x1, x2, H1, H2) - z_).vector();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** Typedefs for regular use */
|
||||
typedef GenericUnaryEqualityConstraint<Config, PoseKey> UnaryEqualityConstraint;
|
||||
typedef GenericOdoHardEqualityConstraint<Config, PoseKey> OdoEqualityConstraint;
|
||||
|
||||
} // namespace simulated2D
|
||||
} // namespace gtsam
|
||||
|
|
@ -8,8 +8,10 @@ check_PROGRAMS += testGaussianBayesNet testGaussianFactor testGaussianFactorGrap
|
|||
check_PROGRAMS += testGaussianISAM testGraph
|
||||
check_PROGRAMS += testInference testIterative testGaussianJunctionTree
|
||||
check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph
|
||||
check_PROGRAMS += testNonlinearOptimizer testNonlinearConstraint testSubgraphPreconditioner
|
||||
check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner
|
||||
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig
|
||||
# check_PROGRAMS += testNonlinearConstraint # disabled until examples rearranged
|
||||
check_PROGRAMS += testNonlinearEqualityConstraint
|
||||
|
||||
if USE_LDL
|
||||
check_PROGRAMS += testConstraintOptimizer
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,191 @@
|
|||
/**
|
||||
* @file testNonlinearEqualityConstraint.cpp
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <simulated2DConstraints.h>
|
||||
#include <NonlinearFactorGraph-inl.h>
|
||||
#include <NonlinearOptimizer-inl.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
static const double tol = 1e-9;
|
||||
|
||||
SharedDiagonal hard_model = noiseModel::Constrained::All(2);
|
||||
SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||
|
||||
typedef NonlinearFactorGraph<simulated2D::Config> Graph;
|
||||
typedef boost::shared_ptr<Graph> shared_graph;
|
||||
typedef boost::shared_ptr<simulated2D::Config> shared_config;
|
||||
typedef NonlinearOptimizer<Graph, simulated2D::Config> Optimizer;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testNonlinearEqualityConstraint, unary_basics ) {
|
||||
Point2 pt(1.0, 2.0);
|
||||
simulated2D::PoseKey key(1);
|
||||
double mu = 1000.0;
|
||||
simulated2D::UnaryEqualityConstraint constraint(pt, key, mu);
|
||||
|
||||
simulated2D::Config 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::Config 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;
|
||||
simulated2D::UnaryEqualityConstraint constraint(pt, key, mu);
|
||||
|
||||
simulated2D::Config config1;
|
||||
config1.insert(key, pt);
|
||||
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
|
||||
GaussianFactor::shared_ptr expected1(new GaussianFactor(key, eye(2,2), zero(2), hard_model));
|
||||
EXPECT(assert_equal(*expected1, *actual1, tol));
|
||||
|
||||
simulated2D::Config config2;
|
||||
Point2 ptBad(2.0, 2.0);
|
||||
config2.insert(key, ptBad);
|
||||
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
|
||||
GaussianFactor::shared_ptr expected2(new GaussianFactor(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;
|
||||
simulated2D::UnaryEqualityConstraint::shared_ptr constraint(
|
||||
new simulated2D::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_config initConfig(new simulated2D::Config());
|
||||
initConfig->insert(key, badPt);
|
||||
|
||||
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initConfig);
|
||||
simulated2D::Config 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;
|
||||
simulated2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
|
||||
|
||||
simulated2D::Config 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::Config 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;
|
||||
simulated2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
|
||||
|
||||
simulated2D::Config config1;
|
||||
config1.insert(key1, x1);
|
||||
config1.insert(key2, x2);
|
||||
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
|
||||
GaussianFactor::shared_ptr expected1(
|
||||
new GaussianFactor(key1, -eye(2,2), key2, eye(2,2), zero(2), hard_model));
|
||||
EXPECT(assert_equal(*expected1, *actual1, tol));
|
||||
|
||||
simulated2D::Config 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);
|
||||
GaussianFactor::shared_ptr expected2(
|
||||
new GaussianFactor(key1, -eye(2,2), 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
|
||||
simulated2D::UnaryEqualityConstraint::shared_ptr constraint1(
|
||||
new simulated2D::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
|
||||
simulated2D::OdoEqualityConstraint::shared_ptr constraint2(
|
||||
new simulated2D::OdoEqualityConstraint(
|
||||
gtsam::between(truth_pt1, truth_pt2), key1, key2));
|
||||
|
||||
shared_graph graph(new Graph());
|
||||
graph->push_back(constraint1);
|
||||
graph->push_back(constraint2);
|
||||
graph->push_back(factor);
|
||||
|
||||
shared_config initConfig(new simulated2D::Config());
|
||||
initConfig->insert(key1, Point2());
|
||||
initConfig->insert(key2, badPt);
|
||||
|
||||
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, initConfig);
|
||||
simulated2D::Config expected;
|
||||
expected.insert(key1, truth_pt1);
|
||||
expected.insert(key2, truth_pt2);
|
||||
CHECK(assert_equal(expected, *actual, tol));
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
Loading…
Reference in New Issue