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

Removed hard constraints from gtsam library (with the exception of NonlinearEquality) and moved them to gtsam_experimental and MastSLAM

release/4.3a0
Alex Cunningham 2010-10-17 03:56:05 +00:00
parent 257da1cefb
commit a9a066aec7
14 changed files with 0 additions and 2504 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
nonlinear/Makefile.am
View File

@ -26,7 +26,6 @@ headers += NonlinearFactor.h
sources += NonlinearOptimizer.cpp Ordering.cpp
# Nonlinear constraints
headers += NonlinearConstraint.h
headers += NonlinearEquality.h
#----------------------------------------------------------------------------------------------------

548
nonlinear/NonlinearConstraint.h
View File

@ -1,548 +0,0 @@
/* ----------------------------------------------------------------------------
* 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/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;
double mu_; /// gain for quadratic merit function
size_t dim_; /// dimension of the constraint
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)), dim_(dim) {}
virtual ~NonlinearConstraint() {}
/** returns the gain mu */
double mu() const { return mu_; }
/** Print */
virtual void print(const std::string& s = "") const=0;
/** dimension of the constraint (number of rows) */
size_t dim() const { return dim_; }
/** 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_ * inner_prod(error_vector, 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;
};
/**
* 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;
/** 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<GaussianFactor> 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 GaussianFactor(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 Factor::shared_ptr symbolic(const Ordering& ordering) const {
return Factor::shared_ptr(new Factor(ordering[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;
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; }
};
/**
* 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;
/** 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<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());
Index var1 = ordering[j1], var2 = ordering[j2];
if (var1 < var2)
GaussianFactor::shared_ptr(new GaussianFactor(var1, grad1, var2, grad2, g, model));
else
GaussianFactor::shared_ptr(new GaussianFactor(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 Factor::shared_ptr symbolic(const Ordering& ordering) const {
const Index var1 = ordering[key1_], var2 = ordering[key2_];
if(var1 < var2)
return Factor::shared_ptr(new Factor(var1, var2));
else
return Factor::shared_ptr(new Factor(var2, var1));
}
};
/**
* 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;
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; }
};
/**
* 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;
/** 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<GaussianFactor> 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 GaussianFactor(var1, A1, var2, A2, var3, A3, b, model));
else if(var2 < var1 && var1 < var3)
return GaussianFactor::shared_ptr(
new GaussianFactor(var2, A2, var1, A1, var3, A3, b, model));
else if(var1 < var3 && var3 < var2)
return GaussianFactor::shared_ptr(
new GaussianFactor(var1, A1, var3, A3, var2, A2, b, model));
else if(var2 < var3 && var3 < var1)
return GaussianFactor::shared_ptr(
new GaussianFactor(var2, A2, var3, A3, var1, A1, b, model));
else if(var3 < var1 && var1 < var2)
return GaussianFactor::shared_ptr(
new GaussianFactor(var3, A3, var1, A1, var2, A2, b, model));
else
return GaussianFactor::shared_ptr(
new GaussianFactor(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 Factor::shared_ptr symbolic(const Ordering& ordering) const {
const Index var1 = ordering[key1_], var2 = ordering[key2_], var3 = ordering[key3_];
if(var1 < var2 && var2 < var3)
return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key2_], ordering[key3_]));
else if(var2 < var1 && var1 < var3)
return Factor::shared_ptr(new Factor(ordering[key2_], ordering[key2_], ordering[key3_]));
else if(var1 < var3 && var3 < var2)
return Factor::shared_ptr(new Factor(ordering[key1_], ordering[key3_], ordering[key2_]));
else if(var2 < var3 && var3 < var1)
return Factor::shared_ptr(new Factor(ordering[key2_], ordering[key3_], ordering[key1_]));
else if(var3 < var1 && var1 < var2)
return Factor::shared_ptr(new Factor(ordering[key3_], ordering[key1_], ordering[key2_]));
else
return Factor::shared_ptr(new Factor(ordering[key3_], ordering[key2_], ordering[key1_]));
}
};
/**
* 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;
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; }
};
/**
* 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;
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);
}
};
/**
* 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;
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);
}
};
}

498
nonlinear/tests/testConstraintOptimizer.cpp
View File

@ -1,498 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testConstraintOptimizer.cpp
* @brief Tests the optimization engine for SQP and BFGS Quadratic programming techniques
* @author Alex Cunningham
*/
/** IMPORTANT NOTE: this file is only compiled when LDL is available */
#include <iostream>
#include <limits>
#include <boost/tuple/tuple.hpp>
#include <boost/optional.hpp>
#include <gtsam/CppUnitLite/TestHarness.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/ConstraintOptimizer.h>
#define GTSAM_MAGIC_KEY
#include <boost/assign/std/list.hpp> // for operator +=
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]
VectorValues 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;
VectorValues delta = fg.optimize(ord).scale(-1.0);
if (verbose) delta.print("Delta");
// update initial estimate
VectorValues newState = expmap(state, delta);
state = newState;
if (verbose) state.print("Updated State");
}
// verify that it converges to the nearest optimal point
VectorValues 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 ) {
Matrix A = Matrix_(3, 2,
-1.0, -1.0,
-2.0, 1.0,
1.0, -1.0);
Matrix At = trans(A),
B = 2.0 * eye(3,3);
Vector b = Vector_(2, 4.0, -2.0),
g = zero(3);
Matrix G = zeros(5,5);
insertSub(G, B, 0, 0);
insertSub(G, A, 0, 3);
insertSub(G, At, 3, 0);
Vector rhs = zero(5);
subInsert(rhs, -1.0*g, 0);
subInsert(rhs, -1.0*b, 3);
// solve the system with the LDL solver
Vector actualFull = solve_ldl(G, rhs);
Vector actual = sub(actualFull, 0, 3);
Vector expected = Vector_(3, 2.0/7.0, 10.0/7.0, -6.0/7.0);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( matrix, CQP_example_automatic ) {
Matrix A = Matrix_(3, 2,
-1.0, -1.0,
-2.0, 1.0,
1.0, -1.0);
Matrix At = trans(A),
B = 2.0 * eye(3,3);
Vector g = zero(3),
h = Vector_(2, 4.0, -2.0);
Vector actState, actLam;
boost::tie(actState, actLam) = solveCQP(B, A, g, h);
Vector expected = Vector_(3, 2.0/7.0, 10.0/7.0, -6.0/7.0);
CHECK(assert_equal(expected, actState));
CHECK(actLam.size() == 2);
}
/* ************************************************************************* */
/** SQP example from SQP tutorial */
namespace sqp_example1 {
/**
* objective function with gradient and hessian
* fx = (x2-2)^2 + x1^2;
*/
double objective(const Vector& x, boost::optional<Vector&> g = boost::none,
boost::optional<Matrix&> B = boost::none) {
double x1 = x(0), x2 = x(1);
if (g) *g = Vector_(2, 2.0*x1, 2.0*(x2-2.0));
if (B) *B = 2.0 * eye(2,2);
return (x2-2)*(x2-2) + x1*x1;
}
/**
* constraint function with gradient and hessian
* cx = 4*x1^2 + x2^2 - 1;
*/
Vector constraint(const Vector& x, boost::optional<Matrix&> A = boost::none,
boost::optional<Matrix&> B = boost::none) {
double x1 = x(0), x2 = x(1);
if (A) *A = Matrix_(2,1, 8.0*x1, 2.0*x2);
if (B) *B = Matrix_(2,2,
8.0, 0.0,
0.0, 2.0);
return Vector_(1, 4.0*x1*x1 + x2*x2 - 1.0);
}
/**
* evaluates a penalty function at a given point
*/
double penalty(const Vector& x) {
double constraint_gain = 1.0;
return objective(x) + constraint_gain*sum(abs(constraint(x)));
}
}
/* ************************************************************************* */
/** SQP example from SQP tutorial (real saddle point) */
namespace sqp_example2 {
/**
* objective function with gradient and hessian
* fx = (x2-2)^2 - x1^2;
*/
double objective(const Vector& x, boost::optional<Vector&> g = boost::none,
boost::optional<Matrix&> B = boost::none) {
double x1 = x(0), x2 = x(1);
if (g) *g = Vector_(2, -2.0*x1, 2.0*(x2-2.0));
if (B) *B = Matrix_(2,2, -2.0, 0.0, 0.0, 2.0);
return (x2-2)*(x2-2) - x1*x1;
}
/**
* constraint function with gradient and hessian
* cx = 4*x1^2 + x2^2 - 1;
*/
Vector constraint(const Vector& x, boost::optional<Matrix&> A = boost::none,
boost::optional<Matrix&> B = boost::none) {
double x1 = x(0), x2 = x(1);
if (A) *A = Matrix_(2,1, 8.0*x1, 2.0*x2);
if (B) *B = Matrix_(2,2,
8.0, 0.0,
0.0, 2.0);
return Vector_(1, 4.0*x1*x1 + x2*x2 - 1.0);
}
/**
* evaluates a penalty function at a given point
*/
double penalty(const Vector& x) {
double constraint_gain = 1.0;
return objective(x) + constraint_gain*sum(abs(constraint(x)));
}
}
/* ************************************************************************* */
TEST( matrix, SQP_simple_analytic ) {
using namespace sqp_example1;
// parameters
double stepsize = 0.25;
size_t maxIt = 50;
// initial conditions
Vector x0 = Vector_(2, 2.0, 4.0),
lam0 = Vector_(1, -0.5);
// current state
Vector x = x0, lam = lam0;
for (size_t i =0; i<maxIt; ++i) {
// evaluate functions
Vector dfx;
Matrix dcx, ddfx, ddcx;
objective(x, dfx, ddfx);
Vector cx = constraint(x, dcx, ddcx);
// use analytic hessian
Matrix B = ddfx - lam(0)*ddcx;
// solve subproblem
Vector delta, lambda;
boost::tie(delta, lambda) = solveCQP(B, -dcx, dfx, -cx);
// update
Vector step = stepsize * delta;
x = x + step;
lam = lambda;
}
// verify
Vector expX = Vector_(2, 0.0, 1.0),
expLambda = Vector_(1, -1.0);
CHECK(assert_equal(expX, x, 1e-4));
CHECK(assert_equal(expLambda, lam, 1e-4));
}
/* ************************************************************************* */
TEST( matrix, SQP_simple_manual_bfgs ) {
using namespace sqp_example1;
// parameters
double stepsize = 0.25;
size_t maxIt = 50;
// initial conditions
Vector x0 = Vector_(2, 2.0, 4.0),
lam0 = Vector_(1, -0.5);
// current state
Vector x = x0, lam = lam0;
Matrix Bi = eye(2,2);
Vector step, prev_dfx;
for (size_t i=0; i<maxIt; ++i) {
// evaluate functions
Vector dfx; Matrix dcx;
objective(x, dfx);
Vector cx = constraint(x, dcx);
// Just use dfx for the Hessian
if (i>0) {
Vector Bis = Bi * step,
y = dfx - prev_dfx;
Bi = Bi + outer_prod(y, y) / inner_prod(y, step)
- outer_prod(Bis, Bis) / inner_prod(step, Bis);
}
prev_dfx = dfx;
// solve subproblem
Vector delta, lambda;
boost::tie(delta, lambda) = solveCQP(Bi, -dcx, dfx, -cx);
// update
step = stepsize * delta;
x = x + step;
lam = lambda;
}
// verify
Vector expX = Vector_(2, 0.0, 1.0),
expLambda = Vector_(1, -1.0);
CHECK(assert_equal(expX, x, 1e-4));
CHECK(assert_equal(expLambda, lam, 1e-4));
}
/* ************************************************************************* */
TEST( matrix, SQP_simple_bfgs1 ) {
using namespace sqp_example1;
// parameters
size_t maxIt = 25;
// initial conditions
Vector x0 = Vector_(2, 2.0, 4.0),
lam0 = Vector_(1, -0.5);
// create a BFGSEstimator
BFGSEstimator hessian(2);
// current state
Vector x = x0, lam = lam0;
Vector step;
for (size_t i=0; i<maxIt; ++i) {
// evaluate functions
Vector dfx; Matrix dcx;
objective(x, dfx);
Vector cx = constraint(x, dcx);
// Just use dfx for the Hessian
if (i>0) {
hessian.update(dfx, step);
} else {
hessian.update(dfx);
}
// solve subproblem
Vector delta, lambda;
boost::tie(delta, lambda) = solveCQP(hessian.getB(), -dcx, dfx, -cx);
// if (i == 0) print(delta, "delta1");
// update
step = linesearch(x,delta,penalty);
// step = stepsize * delta;
x = x + step;
lam = lambda;
}
// verify
Vector expX = Vector_(2, 0.0, 1.0),
expLambda = Vector_(1, -1.0);
CHECK(assert_equal(expX, x, 1e-4));
CHECK(assert_equal(expLambda, lam, 1e-4));
}
/* ************************************************************************* */
TEST( matrix, SQP_simple_bfgs2 ) {
using namespace sqp_example2;
// parameters
double stepsize = 0.25;
size_t maxIt = 50;
// initial conditions
Vector x0 = Vector_(2, 2.0, 4.0),
lam0 = Vector_(1, -0.5);
// create a BFGSEstimator
BFGSEstimator hessian(2);
// current state
Vector x = x0, lam = lam0;
Vector step;
for (size_t i=0; i<maxIt; ++i) {
// evaluate functions
Vector dfx; Matrix dcx;
objective(x, dfx);
Vector cx = constraint(x, dcx);
// Just use dfx for the Hessian
if (i>0) {
hessian.update(dfx, step);
} else {
hessian.update(dfx);
}
// solve subproblem
Vector delta, lambda;
boost::tie(delta, lambda) = solveCQP(hessian.getB(), -dcx, dfx, -cx);
// if (i == 0) print(delta, "delta2");
// update
// step = linesearch(x,delta,penalty);
step = stepsize * delta;
x = x + step;
lam = lambda;
}
// verify
Vector expX = Vector_(2, 0.0, 1.0),
expLambda = Vector_(1, -1.0);
// should determine the actual values for this one
// CHECK(assert_equal(expX, x, 1e-4));
// CHECK(assert_equal(expLambda, lam, 1e-4));
}
/* ************************************************************************* */
TEST( matrix, line_search ) {
using namespace sqp_example2;
// initial conditions
Vector x0 = Vector_(2, 2.0, 4.0),
delta = Vector_(2, 0.85, -5.575);
Vector actual = linesearch(x0,delta,penalty);
// check that error goes down
double init_error = penalty(x0),
final_error = penalty(x0 + actual);
//double actual_stepsize = dot(actual, delta)/dot(delta, delta);
// cout << "actual_stepsize: " << actual_stepsize << endl;
CHECK(final_error <= init_error);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

58
slam/BetweenConstraint.h
View File

@ -1,58 +0,0 @@
/* ----------------------------------------------------------------------------
* 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_;
}
};
} // \namespace gtsam

120
slam/BoundingConstraint.h
View File

@ -1,120 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 Cfg, class Key>
struct BoundingConstraint1: public NonlinearConstraint1<Cfg, Key> {
typedef typename Key::Value X;
typedef NonlinearConstraint1<Cfg, Key> Base;
typedef boost::shared_ptr<BoundingConstraint1<Cfg, 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 Cfg& 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) *H = (isGreaterThan_) ? D : -1.0 * D;
return (isGreaterThan_) ? Vector_(1, error) : -1.0 * Vector_(1, error);
}
};
/**
* Binary scalar inequality constraint, with a similar value() function
* to implement for specific systems
*/
template<class Cfg, class Key1, class Key2>
struct BoundingConstraint2: public NonlinearConstraint2<Cfg, Key1, Key2> {
typedef typename Key1::Value X1;
typedef typename Key2::Value X2;
typedef NonlinearConstraint2<Cfg, Key1, Key2> Base;
typedef boost::shared_ptr<BoundingConstraint2<Cfg, 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 Cfg& 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) *H1 = (isGreaterThan_) ? D1 : -1.0 * D1;
if (H2) *H2 = (isGreaterThan_) ? D2 : -1.0 * D2;
return (isGreaterThan_) ? Vector_(1, error) : -1.0 * Vector_(1, error);
}
};
} // \namespace gtsam

110
slam/LinearApproxFactor-inl.h
View File

@ -1,110 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 LinearApproxFactor.h
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
* @author Alex Cunningham
*/
#pragma once
#include <iostream>
#include <boost/foreach.hpp>
#include <gtsam/slam/LinearApproxFactor.h>
namespace gtsam {
/* ************************************************************************* */
template <class Values, class Key>
LinearApproxFactor<Values,Key>::LinearApproxFactor(
GaussianFactor::shared_ptr lin_factor, const Ordering& ordering, const Values& lin_points)
: Base(noiseModel::Unit::Create(lin_factor->get_model()->dim())),
b_(lin_factor->getb()), model_(lin_factor->get_model()), lin_points_(lin_points)
{
BOOST_FOREACH(const Ordering::Map::value_type& p, ordering) {
Symbol key = p.first;
Index var = p.second;
// check if actually in factor
Factor::const_iterator it = lin_factor->find(var);
if (it != lin_factor->end()) {
// store matrix
Matrix A = lin_factor->getA(it);
matrices_.insert(make_pair(key, A));
// store keys
nonlinearKeys_.push_back(Key(key.index()));
this->keys_.push_back(key);
}
}
}
/* ************************************************************************* */
template <class Values, class Key>
Vector LinearApproxFactor<Values,Key>::unwhitenedError(const Values& c) const {
// extract the points in the new config
Vector ret = - b_;
BOOST_FOREACH(const Key& key, nonlinearKeys_) {
X newPt = c[key], linPt = lin_points_[key];
Vector d = linPt.logmap(newPt);
const Matrix& A = matrices_.at(Symbol(key));
ret += prod(A, d);
}
return ret;
}
/* ************************************************************************* */
template <class Values, class Key>
boost::shared_ptr<GaussianFactor>
LinearApproxFactor<Values,Key>::linearize(const Values& c, const Ordering& ordering) const {
// sort by varid - only known at linearization time
typedef std::map<Index, Matrix> VarMatrixMap;
VarMatrixMap sorting_terms;
BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_)
sorting_terms.insert(std::make_pair(ordering[p.first], p.second));
// move into terms
std::vector<std::pair<Index, Matrix> > terms;
BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms)
terms.push_back(p);
return boost::shared_ptr<GaussianFactor>(new GaussianFactor(terms, b_, model_));
}
/* ************************************************************************* */
template <class Values, class Key>
Factor::shared_ptr
LinearApproxFactor<Values,Key>::symbolic(const Ordering& ordering) const {
std::vector<Index> key_ids(this->keys_.size());
size_t i=0;
BOOST_FOREACH(const Symbol& key, this->keys_)
key_ids[i++] = ordering[key];
std::sort(key_ids.begin(), key_ids.end());
return boost::shared_ptr<Factor>(new Factor(key_ids.begin(), key_ids.end()));
}
/* ************************************************************************* */
template <class Values, class Key>
void LinearApproxFactor<Values,Key>::print(const std::string& s) const {
LinearApproxFactor<Values,Key>::Base::print(s);
BOOST_FOREACH(const SymbolMatrixMap::value_type& p, matrices_) {
gtsam::print(p.second, (std::string) p.first);
}
gtsam::print(b_, std::string("b"));
model_->print("model");
}
} // \namespace gtsam

106
slam/LinearApproxFactor.h
View File

@ -1,106 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 LinearApproxFactor.h
* @brief A dummy factor that allows a linear factor to act as a nonlinear factor
* @author Alex Cunningham
*/
#pragma once
#include <vector>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* A dummy factor that takes a linearized factor and inserts it into
* a nonlinear graph. This version uses exactly one type of variable.
*/
template <class Values, class Key>
class LinearApproxFactor : public NonlinearFactor<Values> {
public:
/** type for the variable */
typedef typename Key::Value X;
/** base type */
typedef NonlinearFactor<Values> Base;
/** shared pointer for convenience */
typedef boost::shared_ptr<LinearApproxFactor<Values,Key> > shared_ptr;
/** typedefs for key vectors */
typedef std::vector<Key> KeyVector;
protected:
/** hold onto the factor itself */
// GaussianFactor::shared_ptr lin_factor_;
// store components of a linear factor that can be reordered
typedef std::map<Symbol, Matrix> SymbolMatrixMap;
SymbolMatrixMap matrices_;
Vector b_;
SharedDiagonal model_;
/** linearization points for error calculation */
Values lin_points_;
/** keep keys for the factor */
KeyVector nonlinearKeys_;
/**
* use this for derived classes with keys that don't copy easily
*/
LinearApproxFactor(size_t dim, const Values& lin_points)
: Base(noiseModel::Unit::Create(dim)), lin_points_(lin_points) {}
public:
/**
* use this constructor when starting with nonlinear keys
*
* Note that you need to have the ordering used to construct the factor
* initially in order to find the actual keys
*/
LinearApproxFactor(GaussianFactor::shared_ptr lin_factor,
const Ordering& ordering, const Values& lin_points);
virtual ~LinearApproxFactor() {}
/** Vector of errors, unwhitened ! */
virtual Vector unwhitenedError(const Values& c) const;
/**
* linearize to a GaussianFactor
* Reconstructs the linear factor from components to ensure that
* the ordering is correct
*/
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& c, const Ordering& ordering) const;
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
Factor::shared_ptr symbolic(const Ordering& ordering) const;
/** get access to nonlinear keys */
KeyVector nonlinearKeys() const { return nonlinearKeys_; }
/** override print function */
virtual void print(const std::string& s="") const;
/** access to b vector of gaussian */
Vector get_b() const { return b_; }
};
} // \namespace gtsam

6
slam/Makefile.am
View File

@ -31,12 +31,6 @@ check_PROGRAMS += tests/testSimulated3D
# Pose SLAM headers
headers += BetweenFactor.h PriorFactor.h
# General constraints
headers += BetweenConstraint.h BoundingConstraint.h TransformConstraint.h
# Utility factors
headers += LinearApproxFactor.h LinearApproxFactor-inl.h
# 2D Pose SLAM
sources += pose2SLAM.cpp dataset.cpp
#sources += Pose2SLAMOptimizer.cpp

71
slam/TransformConstraint.h
View File

@ -1,71 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 TransformConstraint.h
* @brief A constraint for combining graphs by common landmarks and a transform node
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/nonlinear/NonlinearConstraint.h>
namespace gtsam {
/**
* A constraint between two landmarks in separate maps
* Templated on:
* Values : The overall config
* PKey : Key of landmark being constrained
* Point : Type of landmark
* TKey : Key of transform used
* Transform : Transform variable class
*
* The Point and Transform concepts must be Lie types, and the transform
* relationship "Point = transform_from(Transform, Point)" must exist.
*
* This base class should be specialized to implement the cost function for
* specific classes of landmarks
*/
template<class Values, class PKey, class TKey>
class TransformConstraint : public NonlinearEqualityConstraint3<Values, PKey, TKey, PKey> {
public:
typedef typename PKey::Value Point;
typedef typename TKey::Value Transform;
typedef NonlinearEqualityConstraint3<Values, PKey, TKey, PKey> Base;
typedef TransformConstraint<Values, PKey, TKey> This;
/**
* General constructor with all of the keys to variables in the map
* Extracts everything necessary from the key types
*/
TransformConstraint(const PKey& foreignKey, const TKey& transKey, const PKey& localKey, double mu = 1000.0)
: Base(foreignKey, transKey, localKey, Point().dim(), mu) {}
virtual ~TransformConstraint(){}
/** Combined cost and derivative function using boost::optional */
virtual Vector evaluateError(const Point& foreign, const Transform& trans, const Point& local,
boost::optional<Matrix&> Dforeign = boost::none,
boost::optional<Matrix&> Dtrans = boost::none,
boost::optional<Matrix&> Dlocal = boost::none) const {
Point newlocal = trans.transform_from(foreign, Dtrans, Dforeign);
if (Dlocal) {
Point dummy;
*Dlocal = -1* eye(dummy.dim());
}
return local.logmap(newlocal);
}
};
} // \namespace gtsam

4
tests/Makefile.am
View File

@ -19,10 +19,6 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG
check_PROGRAMS += testNonlinearOptimizer
check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph
check_PROGRAMS += testTupleValues
#check_PROGRAMS += testNonlinearEqualityConstraint
#check_PROGRAMS += testBoundingConstraint
check_PROGRAMS += testTransformConstraint
check_PROGRAMS += testLinearApproxFactor
# only if serialization is available
if ENABLE_SERIALIZATION

282
tests/testBoundingConstraint.cpp
View File

@ -1,282 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 <gtsam/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);
GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1);
GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1);
EXPECT(!actual1);
EXPECT(!actual2);
}
/* ************************************************************************* */
TEST( testBoundingConstraint, unary_linearization_active) {
Point2 pt2(-2.0, -3.0);
simulated2D::Values config2;
config2.insert(key, pt2);
GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2);
GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2);
GaussianFactor expected1(key, Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1);
GaussianFactor expected2(key, Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1);
EXPECT(assert_equal(expected1, *actual1, tol));
EXPECT(assert_equal(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, Optimizer::SILENT);
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, Optimizer::SILENT);
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);
EXPECT(!rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT(!rangeBound.linearize(config1));
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));
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);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initial_state);
simulated2D::Values expected;
expected.insert(x1, pt1);
expected.insert(x2, pt2_goal);
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);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init);
EXPECT(assert_equal(expected, *actual, tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

61
tests/testLinearApproxFactor.cpp
View File

@ -1,61 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testLinearApproxFactor.cpp
* @brief tests for dummy factor that contains a linear factor
* @author Alex Cunningham
*/
#include <iostream>
#include <gtsam/CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/LinearApproxFactor-inl.h>
using namespace std;
using namespace gtsam;
typedef LinearApproxFactor<planarSLAM::Values,planarSLAM::PointKey> ApproxFactor;
/* ************************************************************************* */
TEST ( LinearApproxFactor, basic ) {
Symbol key1('l', 1);
Matrix A1 = eye(2);
Vector b = repeat(2, 1.2);
SharedDiagonal model = noiseModel::Unit::Create(2);
GaussianFactor::shared_ptr lin_factor(new GaussianFactor(0, A1, b, model));
Ordering ordering;
ordering.push_back(key1);
planarSLAM::Values lin_points;
planarSLAM::PointKey PKey(1);
Point2 point(1.0, 2.0);
lin_points.insert(PKey, point);
ApproxFactor f1(lin_factor, ordering, lin_points);
EXPECT(f1.size() == 1);
EXPECT(assert_equal(key1, f1.keys().front()));
EXPECT(assert_equal(b, f1.get_b()));
planarSLAM::Values config;
config.insert(PKey, Point2(2.0, 3.0));
GaussianFactor::shared_ptr actual = f1.linearize(config, ordering);
EXPECT(assert_equal(Vector_(2, -0.2, -0.2), f1.unwhitenedError(config)));
// Check the linearization
EXPECT(assert_equal(*lin_factor, *actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

363
tests/testNonlinearEqualityConstraint.cpp
View File

@ -1,363 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 <gtsam/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;
eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
simulated2D::Values 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::Values 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;
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;
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);
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::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);
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
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); }
/* ************************************************************************* */

276
tests/testTransformConstraint.cpp
View File

@ -1,276 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testTransformConstraint.cpp
* @author Alex Cunningham
*/
#include <iostream>
#include <boost/bind.hpp>
#include <gtsam/CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/slam/TransformConstraint.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
// implementations
#include <gtsam/nonlinear/LieValues-inl.h>
#include <gtsam/nonlinear/TupleValues-inl.h>
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
using namespace std;
using namespace gtsam;
using namespace boost;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'T'> TransformKey;
typedef LieValues<PoseKey> PoseValues;
typedef LieValues<PointKey> PointValues;
typedef LieValues<TransformKey> TransformValues;
typedef TupleValues3< PoseValues, PointValues, TransformValues > DDFValues;
typedef NonlinearFactorGraph<DDFValues> DDFGraph;
typedef NonlinearOptimizer<DDFGraph, DDFValues> Optimizer;
typedef NonlinearEquality<DDFValues, PoseKey> PoseConstraint;
typedef NonlinearEquality<DDFValues, PointKey> PointConstraint;
typedef NonlinearEquality<DDFValues, TransformKey> TransformPriorConstraint;
typedef TransformConstraint<DDFValues, PointKey, TransformKey> PointTransformConstraint;
PointKey lA1(1), lA2(2), lB1(3);
TransformKey t1(1);
/* ************************************************************************* */
TEST( TransformConstraint, equals ) {
PointTransformConstraint c1(lB1, t1, lA1),
c2(lB1, t1, lA1),
c3(lB1, t1, lA2);
CHECK(assert_equal(c1, c1));
CHECK(assert_equal(c1, c2));
CHECK(!c1.equals(c3));
}
/* ************************************************************************* */
LieVector evaluateError_(const PointTransformConstraint& c,
const Point2& global, const Pose2& trans, const Point2& local) {
return LieVector(c.evaluateError(global, trans, local));
}
TEST( TransformConstraint, jacobians ) {
// from examples below
Point2 local(2.0, 3.0), global(-1.0, 2.0);
Pose2 trans(1.5, 2.5, 0.3);
PointTransformConstraint tc(lA1, t1, lB1);
Matrix actualDT, actualDL, actualDF;
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
CHECK(assert_equal(numericalDF, actualDF));
CHECK(assert_equal(numericalDL, actualDL));
CHECK(assert_equal(numericalDT, actualDT));
}
/* ************************************************************************* */
TEST( TransformConstraint, jacobians_zero ) {
// get values that are ideal
Pose2 trans(2.0, 3.0, 0.0);
Point2 global(5.0, 6.0);
Point2 local = trans.transform_from(global);
PointTransformConstraint tc(lA1, t1, lB1);
Vector actCost = tc.evaluateError(global, trans, local),
expCost = zero(2);
CHECK(assert_equal(expCost, actCost, 1e-5));
Matrix actualDT, actualDL, actualDF;
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
CHECK(assert_equal(numericalDF, actualDF));
CHECK(assert_equal(numericalDL, actualDL));
CHECK(assert_equal(numericalDT, actualDT));
}
/* ************************************************************************* */
TEST( TransformConstraint, converge_trans ) {
// initial points
Point2 local1(2.0, 2.0), local2(4.0, 5.0),
global1(-1.0, 5.0), global2(2.0, 3.0);
Pose2 transIdeal(7.0, 3.0, M_PI/2);
// verify direction
CHECK(assert_equal(local1, transIdeal.transform_from(global1)));
CHECK(assert_equal(local2, transIdeal.transform_from(global2)));
// choose transform
// Pose2 trans = transIdeal; // ideal - works
// Pose2 trans = transIdeal * Pose2(0.1, 1.0, 0.00); // translation - works
// Pose2 trans = transIdeal * Pose2(10.1, 1.0, 0.00); // large translation - works
// Pose2 trans = transIdeal * Pose2(0.0, 0.0, 0.1); // small rotation - works
Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 1.3); // combined - works
// Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails
// keys
PointKey localK1(1), localK2(2),
globalK1(3), globalK2(4);
TransformKey transK(1);
DDFGraph graph;
graph.add(PointTransformConstraint(globalK1, transK, localK1));
graph.add(PointTransformConstraint(globalK2, transK, localK2));
// hard constraints on points
double error_gain = 1000.0;
graph.add(PointConstraint(localK1, local1, error_gain));
graph.add(PointConstraint(localK2, local2, error_gain));
graph.add(PointConstraint(globalK1, global1, error_gain));
graph.add(PointConstraint(globalK2, global2, error_gain));
// create initial estimate
DDFValues init;
init.insert(localK1, local1);
init.insert(localK2, local2);
init.insert(globalK1, global1);
init.insert(globalK2, global2);
init.insert(transK, trans);
// optimize
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init);
DDFValues expected;
expected.insert(localK1, local1);
expected.insert(localK2, local2);
expected.insert(globalK1, global1);
expected.insert(globalK2, global2);
expected.insert(transK, transIdeal);
CHECK(assert_equal(expected, *actual, 1e-4));
}
/* ************************************************************************* */
TEST( TransformConstraint, converge_local ) {
// initial points
Point2 global(-1.0, 2.0);
// Pose2 trans(1.5, 2.5, 0.3); // original
// Pose2 trans(1.5, 2.5, 1.0); // larger rotation
Pose2 trans(1.5, 2.5, 3.1); // significant rotation
Point2 idealLocal = trans.transform_from(global);
// perturb the initial estimate
// Point2 local = idealLocal; // Ideal case - works
// Point2 local = idealLocal + Point2(1.0, 0.0); // works
Point2 local = idealLocal + Point2(-10.0, 10.0); // works
// keys
PointKey localK(1), globalK(2);
TransformKey transK(1);
DDFGraph graph;
double error_gain = 1000.0;
graph.add(PointTransformConstraint(globalK, transK, localK));
graph.add(PointConstraint(globalK, global, error_gain));
graph.add(TransformPriorConstraint(transK, trans, error_gain));
// create initial estimate
DDFValues init;
init.insert(localK, local);
init.insert(globalK, global);
init.insert(transK, trans);
// optimize
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init);
CHECK(assert_equal(idealLocal, actual->at(localK), 1e-5));
}
/* ************************************************************************* */
TEST( TransformConstraint, converge_global ) {
// initial points
Point2 local(2.0, 3.0);
// Pose2 trans(1.5, 2.5, 0.3); // original
// Pose2 trans(1.5, 2.5, 1.0); // larger rotation
Pose2 trans(1.5, 2.5, 3.1); // significant rotation
Point2 idealForeign = trans.inverse().transform_from(local);
// perturb the initial estimate
// Point2 global = idealForeign; // Ideal - works
// Point2 global = idealForeign + Point2(1.0, 0.0); // simple - works
Point2 global = idealForeign + Point2(10.0, -10.0); // larger - works
// keys
PointKey localK(1), globalK(2);
TransformKey transK(1);
DDFGraph graph;
double error_gain = 1000.0;
graph.add(PointTransformConstraint(globalK, transK, localK));
graph.add(PointConstraint(localK, local, error_gain));
graph.add(TransformPriorConstraint(transK, trans, error_gain));
// create initial estimate
DDFValues init;
init.insert(localK, local);
init.insert(globalK, global);
init.insert(transK, trans);
// optimize
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, init);
// verify
CHECK(assert_equal(idealForeign, actual->at(globalK), 1e-5));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */