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

SQP tests and implementation now use the new Key system

release/4.3a0
Alex Cunningham 2010-01-19 05:33:44 +00:00
parent 2a57a04ba6
commit 88e465910a
30 changed files with 871 additions and 888 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.cproject
View File

@ -769,6 +769,13 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimulated2D.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

5
cpp/GaussianFactor.h
View File

@ -95,7 +95,7 @@ public:
As_.insert(terms[i]);
}
/** Construct an n-ary factor with multiple sigmas*/
/** Construct an n-ary factor with a multiple sigmas*/
GaussianFactor(const std::vector<std::pair<Symbol, Matrix> > &terms,
const Vector &b, const Vector& sigmas) :
b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) {
@ -139,9 +139,6 @@ public:
/** get a copy of sigmas */
const Vector& get_sigmas() const { return model_->sigmas(); }
/** get a copy of sigmas */
const sharedDiagonal& get_model() const { return model_; }
/**
* get a copy of the A matrix from a specific node
* O(log n)

3
cpp/GaussianISAM2.h
View File

@ -11,11 +11,12 @@
#include "ISAM2.h"
#include "GaussianConditional.h"
#include "GaussianFactor.h"
#include "simulated2D.h"
#include "planarSLAM.h"
namespace gtsam {
typedef ISAM2<GaussianConditional, VectorConfig> GaussianISAM2;
typedef ISAM2<GaussianConditional, simulated2D::Config> GaussianISAM2;
// recursively optimize this conditional and all subtrees
void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result);

9
cpp/Makefile.am
View File

@ -141,15 +141,18 @@ testKey_SOURCES = testKey.cpp
testKey_LDADD = libgtsam.la
# Nonlinear constraints
headers += SQPOptimizer.h SQPOptimizer-inl.h
headers += NonlinearConstraint.h NonlinearConstraint-inl.h
headers += NonlinearEquality.h
check_PROGRAMS += testNonlinearConstraint testNonlinearEquality testSQP testSQPOptimizer
check_PROGRAMS += testNonlinearConstraint testNonlinearEquality
testNonlinearConstraint_SOURCES = testNonlinearConstraint.cpp
testNonlinearConstraint_LDADD = libgtsam.la
testNonlinearEquality_SOURCES = testNonlinearEquality.cpp
testNonlinearEquality_LDADD = libgtsam.la
testSQP_SOURCES = testSQP.cpp
# SQP
headers += SQPOptimizer.h SQPOptimizer-inl.h
check_PROGRAMS += testSQP testSQPOptimizer
testSQP_SOURCES = $(example) testSQP.cpp
testSQP_LDADD = libgtsam.la
testSQPOptimizer_SOURCES = testSQPOptimizer.cpp
testSQPOptimizer_LDADD = libgtsam.la

120
cpp/NonlinearConstraint-inl.h
View File

@ -22,7 +22,7 @@ NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key
size_t dim_lagrange,
Vector (*g)(const Config& config),
bool isEquality)
: NonlinearFactor<Config>(1.0),z_(zero(dim_lagrange)),
: NonlinearFactor<Config>(1.0),
lagrange_key_(lagrange_key), p_(dim_lagrange),
isEquality_(isEquality), g_(boost::bind(g, _1)) {}
@ -33,7 +33,7 @@ NonlinearConstraint<Config>::NonlinearConstraint(const std::string& lagrange_key
boost::function<Vector(const Config& config)> g,
bool isEquality)
: NonlinearFactor<Config>(noiseModel::Constrained::All(dim_lagrange)),
lagrange_key_(lagrange_key), p_(dim_lagrange),z_(zero(dim_lagrange)),
lagrange_key_(lagrange_key), p_(dim_lagrange),
g_(g), isEquality_(isEquality) {}
/* ************************************************************************* */
@ -46,10 +46,10 @@ bool NonlinearConstraint<Config>::active(const Config& config) const {
// Implementations of unary nonlinear constraints
/* ************************************************************************* */
template <class Config>
NonlinearConstraint1<Config>::NonlinearConstraint1(
template <class Config, class Key, class X>
NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
Vector (*g)(const Config& config),
const std::string& key,
const Key& key,
Matrix (*gradG)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key,
@ -59,16 +59,16 @@ NonlinearConstraint1<Config>::NonlinearConstraint1(
{
// set a good lagrange key here
// TODO:should do something smart to find a unique one
if (lagrange_key == "")
this->lagrange_key_ = "L_" + key;
this->keys_.push_front(key);
// if (lagrange_key == "")
// this->lagrange_key_ = "L0"
// this->keys_.push_front(key);
}
/* ************************************************************************* */
template <class Config>
NonlinearConstraint1<Config>::NonlinearConstraint1(
template <class Config, class Key, class X>
NonlinearConstraint1<Config, Key, X>::NonlinearConstraint1(
boost::function<Vector(const Config& config)> g,
const std::string& key,
const Key& key,
boost::function<Matrix(const Config& config)> gradG,
size_t dim_constraint,
const std::string& lagrange_key,
@ -78,39 +78,39 @@ NonlinearConstraint1<Config>::NonlinearConstraint1(
{
// set a good lagrange key here
// TODO:should do something smart to find a unique one
if (lagrange_key == "")
this->lagrange_key_ = "L_" + key;
this->keys_.push_front(key);
// if (lagrange_key == "")
// this->lagrange_key_ = "L_" + key;
// this->keys_.push_front(key);
}
/* ************************************************************************* */
template <class Config>
void NonlinearConstraint1<Config>::print(const std::string& s) const {
std::cout << "NonlinearConstraint1 [" << s << "]:\n"
<< " key: " << key_ << "\n"
<< " p: " << this->p_ << "\n"
<< " lambda key: " << this->lagrange_key_ << "\n";
if (this->isEquality_)
std::cout << " Equality Factor" << std::endl;
else
std::cout << " Inequality Factor" << std::endl;
template <class Config, class Key, class X>
void NonlinearConstraint1<Config, Key, X>::print(const std::string& s) const {
std::cout << "NonlinearConstraint1 [" << s << "]:\n";
// << " key: " << key_ << "\n"
// << " p: " << this->p_ << "\n"
// << " lambda key: " << this->lagrange_key_ << "\n";
// if (this->isEquality_)
// std::cout << " Equality Factor" << std::endl;
// else
// std::cout << " Inequality Factor" << std::endl;
}
/* ************************************************************************* */
template <class Config>
bool NonlinearConstraint1<Config>::equals(const Factor<Config>& f, double tol) const {
const NonlinearConstraint1<Config>* p = dynamic_cast<const NonlinearConstraint1<Config>*> (&f);
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 (!(key_ == p->key_)) return false;
if (this->lagrange_key_ != p->lagrange_key_) return false;
if (this->isEquality_ != p->isEquality_) return false;
return this->p_ == p->p_;
}
/* ************************************************************************* */
template <class Config>
template <class Config, class Key, class X>
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr>
NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig& lagrange) const {
NonlinearConstraint1<Config, Key, X>::linearize(const Config& config, const VectorConfig& lagrange) const {
// extract lagrange multiplier
Vector lambda = lagrange[this->lagrange_key_];
@ -136,12 +136,12 @@ NonlinearConstraint1<Config>::linearize(const Config& config, const VectorConfig
/* ************************************************************************* */
/* ************************************************************************* */
template <class Config>
NonlinearConstraint2<Config>::NonlinearConstraint2(
template <class Config, class Key1, class X1, class Key2, class X2>
NonlinearConstraint2<Config, Key1, X1, Key2, X2>::NonlinearConstraint2(
Vector (*g)(const Config& config),
const std::string& key1,
const Key1& key1,
Matrix (*G1)(const Config& config),
const std::string& key2,
const Key2& key2,
Matrix (*G2)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key,
@ -152,19 +152,19 @@ NonlinearConstraint2<Config>::NonlinearConstraint2(
{
// set a good lagrange key here
// TODO:should do something smart to find a unique one
if (lagrange_key == "")
this->lagrange_key_ = "L_" + key1 + key2;
this->keys_.push_front(key1);
this->keys_.push_back(key2);
// if (lagrange_key == "")
// this->lagrange_key_ = "L_" + key1 + key2;
// this->keys_.push_front(key1);
// this->keys_.push_back(key2);
}
/* ************************************************************************* */
template <class Config>
NonlinearConstraint2<Config>::NonlinearConstraint2(
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 std::string& key1,
const Key1& key1,
boost::function<Matrix(const Config& config)> G1,
const std::string& key2,
const Key2& key2,
boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint,
const std::string& lagrange_key,
@ -175,38 +175,38 @@ NonlinearConstraint2<Config>::NonlinearConstraint2(
{
// set a good lagrange key here
// TODO:should do something smart to find a unique one
if (lagrange_key == "")
this->lagrange_key_ = "L_" + key1 + key2;
this->keys_.push_front(key1);
this->keys_.push_back(key2);
// if (lagrange_key == "")
// this->lagrange_key_ = "L_" + key1 + key2;
// this->keys_.push_front(key1);
// this->keys_.push_back(key2);
}
/* ************************************************************************* */
template <class Config>
void NonlinearConstraint2<Config>::print(const std::string& s) const {
std::cout << "NonlinearConstraint2 [" << s << "]:\n"
<< " key1: " << key1_ << "\n"
<< " key2: " << key2_ << "\n"
<< " p: " << this->p_ << "\n"
<< " lambda key: " << this->lagrange_key_ << std::endl;
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 << "]:\n";
// << " key1: " << key1_ << "\n"
// << " key2: " << key2_ << "\n"
// << " p: " << this->p_ << "\n"
// << " lambda key: " << this->lagrange_key_ << std::endl;
}
/* ************************************************************************* */
template <class Config>
bool NonlinearConstraint2<Config>::equals(const Factor<Config>& f, double tol) const {
const NonlinearConstraint2<Config>* p = dynamic_cast<const NonlinearConstraint2<Config>*> (&f);
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 (!(key1_ == p->key1_)) return false;
if (!(key2_ == p->key2_)) return false;
if (this->lagrange_key_ != p->lagrange_key_) return false;
if (this->isEquality_ != p->isEquality_) return false;
return this->p_ == p->p_;
}
/* ************************************************************************* */
template<class Config>
template<class Config, class Key1, class X1, class Key2, class X2>
std::pair<GaussianFactor::shared_ptr, GaussianFactor::shared_ptr> NonlinearConstraint2<
Config>::linearize(const Config& config, const VectorConfig& lagrange) const {
Config, Key1, X1, Key2, X2>::linearize(const Config& config, const VectorConfig& lagrange) const {
// extract lagrange multiplier
Vector lambda = lagrange[this->lagrange_key_];

25
cpp/NonlinearConstraint.h
View File

@ -28,9 +28,6 @@ class NonlinearConstraint : public NonlinearFactor<Config> {
protected:
/** Lagrange multipliers? */
Vector z_;
/** key for the lagrange multipliers */
std::string lagrange_key_;
@ -121,7 +118,7 @@ public:
/**
* A unary constraint with arbitrary cost and jacobian functions
*/
template <class Config>
template <class Config, class Key, class X>
class NonlinearConstraint1 : public NonlinearConstraint<Config> {
private:
@ -136,7 +133,7 @@ private:
boost::function<Matrix(const Config& config)> G_;
/** key for the constrained variable */
std::string key_;
Key key_;
public:
@ -151,7 +148,7 @@ public:
*/
NonlinearConstraint1(
Vector (*g)(const Config& config),
const std::string& key,
const Key& key,
Matrix (*G)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key="",
@ -168,7 +165,7 @@ public:
*/
NonlinearConstraint1(
boost::function<Vector(const Config& config)> g,
const std::string& key,
const Key& key,
boost::function<Matrix(const Config& config)> G,
size_t dim_constraint,
const std::string& lagrange_key="",
@ -194,7 +191,7 @@ public:
/**
* A binary constraint with arbitrary cost and jacobian functions
*/
template <class Config>
template <class Config, class Key1, class X1, class Key2, class X2>
class NonlinearConstraint2 : public NonlinearConstraint<Config> {
private:
@ -210,8 +207,8 @@ private:
boost::function<Matrix(const Config& config)> G2_;
/** keys for the constrained variables */
std::string key1_;
std::string key2_;
Key1 key1_;
Key2 key2_;
public:
@ -226,9 +223,9 @@ public:
*/
NonlinearConstraint2(
Vector (*g)(const Config& config),
const std::string& key1,
const Key1& key1,
Matrix (*G1)(const Config& config),
const std::string& key2,
const Key2& key2,
Matrix (*G2)(const Config& config),
size_t dim_constraint,
const std::string& lagrange_key="",
@ -246,9 +243,9 @@ public:
*/
NonlinearConstraint2(
boost::function<Vector(const Config& config)> g,
const std::string& key1,
const Key1& key1,
boost::function<Matrix(const Config& config)> G1,
const std::string& key2,
const Key2& key2,
boost::function<Matrix(const Config& config)> G2,
size_t dim_constraint,
const std::string& lagrange_key="",

14
cpp/simulated2D.cpp
View File

@ -5,20 +5,28 @@
*/
#include "simulated2D.h"
#include "TupleConfig-inl.h"
namespace gtsam {
using namespace simulated2D;
// INSTANTIATE_LIE_CONFIG(PointKey, Point2)
INSTANTIATE_PAIR_CONFIG(PoseKey, Point2, PointKey, Point2)
// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
namespace simulated2D {
static Matrix I = gtsam::eye(2);
/* ************************************************************************* */
Vector prior(const Vector& x, boost::optional<Matrix&> H) {
Point2 prior(const Point2& x, boost::optional<Matrix&> H) {
if (H) *H = I;
return x;
}
/* ************************************************************************* */
Vector odo(const Vector& x1, const Vector& x2, boost::optional<Matrix&> H1,
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
if (H1) *H1 = -I;
if (H2) *H2 = I;
@ -26,7 +34,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Vector mea(const Vector& x, const Vector& l, boost::optional<Matrix&> H1,
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) {
if (H1) *H1 = -I;
if (H2) *H2 = I;

78
cpp/simulated2D.h
View File

@ -8,9 +8,9 @@
#pragma once
#include "VectorConfig.h"
#include "Point2.h"
#include "TupleConfig.h"
#include "NonlinearFactor.h"
#include "Key.h"
// \namespace
@ -18,47 +18,51 @@ namespace gtsam {
namespace simulated2D {
typedef gtsam::VectorConfig VectorConfig;
typedef gtsam::Symbol PoseKey;
typedef gtsam::Symbol PointKey;
// Simulated2D robots have no orientation, just a position
typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef PairConfig<PoseKey, Point2, PointKey, Point2> Config;
/**
* Prior on a single pose, and optional derivative version
*/
inline Vector prior(const Vector& x) {return x;}
Vector prior(const Vector& x, boost::optional<Matrix&> H = boost::none);
inline Point2 prior(const Point2& x) {
return x;
}
Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none);
/**
* odometry between two poses, and optional derivative version
*/
inline Vector odo(const Vector& x1, const Vector& x2) {return x2-x1;}
Vector odo(const Vector& x1, const Vector& x2, boost::optional<Matrix&> H1 =
inline Point2 odo(const Point2& x1, const Point2& x2) {
return x2 - x1;
}
Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
/**
* measurement between landmark and pose, and optional derivative version
*/
inline Vector mea(const Vector& x, const Vector& l) {return l-x;}
Vector mea(const Vector& x, const Vector& l, boost::optional<Matrix&> H1 =
inline Point2 mea(const Point2& x, const Point2& l) {
return l - x;
}
Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none);
/**
* Unary factor encoding a soft prior on a vector
*/
struct Prior: public NonlinearFactor1<VectorConfig, PoseKey,
Vector> {
struct Prior: public NonlinearFactor1<Config, PoseKey, Point2> {
Vector z_;
Point2 z_;
Prior(const Vector& z, const sharedGaussian& model,
const PoseKey& key) :
NonlinearFactor1<VectorConfig, PoseKey, Vector>(model, key),
z_(z) {
Prior(const Point2& z, const sharedGaussian& model, const PoseKey& key) :
NonlinearFactor1<Config, PoseKey, Point2> (model, key), z_(z) {
}
Vector evaluateError(const Vector& x, boost::optional<Matrix&> H =
Vector evaluateError(const Point2& x, boost::optional<Matrix&> H =
boost::none) const {
return prior(x, H) - z_;
return (prior(x, H) - z_).vector();
}
};
@ -66,19 +70,19 @@ namespace gtsam {
/**
* Binary factor simulating "odometry" between two Vectors
*/
struct Odometry: public NonlinearFactor2<VectorConfig, PoseKey,
Vector, PointKey, Vector> {
Vector z_;
struct Odometry: public NonlinearFactor2<Config, PoseKey, Point2, PoseKey,
Point2> {
Point2 z_;
Odometry(const Vector& z, const sharedGaussian& model,
const PoseKey& j1, const PoseKey& j2) :
z_(z), NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
Vector>(model, j1, j2) {
Odometry(const Point2& z, const sharedGaussian& model, const PoseKey& j1,
const PoseKey& j2) :
z_(z), NonlinearFactor2<Config, PoseKey, Point2, PoseKey, Point2> (
model, j1, j2) {
}
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
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_;
return (odo(x1, x2, H1, H2) - z_).vector();
}
};
@ -86,20 +90,20 @@ namespace gtsam {
/**
* Binary factor simulating "measurement" between two Vectors
*/
struct Measurement: public NonlinearFactor2<VectorConfig, PoseKey,
Vector, PointKey, Vector> {
struct Measurement: public NonlinearFactor2<Config, PoseKey, Point2,
PointKey, Point2> {
Vector z_;
Point2 z_;
Measurement(const Vector& z, const sharedGaussian& model,
Measurement(const Point2& z, const sharedGaussian& model,
const PoseKey& j1, const PointKey& j2) :
z_(z), NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
Vector>(model, j1, j2) {
z_(z), NonlinearFactor2<Config, PoseKey, Point2, PointKey, Point2> (
model, j1, j2) {
}
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional<
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
return mea(x1, x2, H1, H2) - z_;
return (mea(x1, x2, H1, H2) - z_).vector();
}
};

153
cpp/smallExample.cpp
View File

@ -20,62 +20,65 @@ using namespace std;
#include "Matrix.h"
#include "NonlinearFactor.h"
#include "smallExample.h"
#include "simulated2D.h"
// template definitions
#include "FactorGraph-inl.h"
#include "NonlinearFactorGraph-inl.h"
namespace gtsam {
namespace example {
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
typedef boost::shared_ptr<NonlinearFactor<Config> > shared;
/* ************************************************************************* */
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedNonlinearFactorGraph() {
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph() {
// Create
boost::shared_ptr<ExampleNonlinearFactorGraph> nlfg(
new ExampleNonlinearFactorGraph);
boost::shared_ptr<Graph> nlfg(
new Graph);
// prior on x1
double sigma1 = 0.1;
Vector mu = zero(2);
shared f1(new simulated2D::Prior(mu, sigma1, "x1"));
Point2 mu;
shared f1(new simulated2D::Prior(mu, sigma1, 1));
nlfg->push_back(f1);
// odometry between x1 and x2
double sigma2 = 0.1;
Vector z2(2);
z2(0) = 1.5;
z2(1) = 0;
shared f2(new simulated2D::Odometry(z2, sigma2, "x1", "x2"));
Point2 z2(1.5, 0);
shared f2(new simulated2D::Odometry(z2, sigma2, 1, 2));
nlfg->push_back(f2);
// measurement between x1 and l1
double sigma3 = 0.2;
Vector z3(2);
z3(0) = 0.;
z3(1) = -1.;
shared f3(new simulated2D::Measurement(z3, sigma3, "x1", "l1"));
Point2 z3(0, -1);
shared f3(new simulated2D::Measurement(z3, sigma3, 1, 1));
nlfg->push_back(f3);
// measurement between x2 and l1
double sigma4 = 0.2;
Vector z4(2);
z4(0) = -1.5;
z4(1) = -1.;
shared f4(new simulated2D::Measurement(z4, sigma4, "x2", "l1"));
Point2 z4(-1.5, -1.);
shared f4(new simulated2D::Measurement(z4, sigma4, 2, 1));
nlfg->push_back(f4);
return nlfg;
}
/* ************************************************************************* */
ExampleNonlinearFactorGraph createNonlinearFactorGraph() {
Graph createNonlinearFactorGraph() {
return *sharedNonlinearFactorGraph();
}
/* ************************************************************************* */
VectorConfig createConfig() {
Config createConfig() {
Config c;
c.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0));
c.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0));
c.insert(simulated2D::PointKey(1), Point2(0.0, -1.0));
return c;
}
/* ************************************************************************* */
VectorConfig createVectorConfig() {
VectorConfig c;
c.insert("x1", Vector_(2, 0.0, 0.0));
c.insert("x2", Vector_(2, 1.5, 0.0));
@ -84,16 +87,16 @@ namespace gtsam {
}
/* ************************************************************************* */
boost::shared_ptr<const VectorConfig> sharedNoisyConfig() {
boost::shared_ptr<VectorConfig> c(new VectorConfig);
c->insert("x1", Vector_(2, 0.1, 0.1));
c->insert("x2", Vector_(2, 1.4, 0.2));
c->insert("l1", Vector_(2, 0.1, -1.1));
boost::shared_ptr<const Config> sharedNoisyConfig() {
boost::shared_ptr<Config> c(new Config);
c->insert(simulated2D::PoseKey(1), Point2(0.1, 0.1));
c->insert(simulated2D::PoseKey(2), Point2(1.4, 0.2));
c->insert(simulated2D::PointKey(1), Point2(0.1, -1.1));
return c;
}
/* ************************************************************************* */
VectorConfig createNoisyConfig() {
Config createNoisyConfig() {
return *sharedNoisyConfig();
}
@ -118,14 +121,13 @@ namespace gtsam {
/* ************************************************************************* */
GaussianFactorGraph createGaussianFactorGraph() {
Matrix I = eye(2);
VectorConfig c = createNoisyConfig();
// Create empty graph
GaussianFactorGraph fg;
// linearized prior on x1: c["x1"]+x1=0 i.e. x1=-c["x1"]
double sigma1 = 0.1;
Vector b1 = -c["x1"];
Vector b1 = -Vector_(2,0.1, 0.1);
fg.add("x1", I, b1, sigma1);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
@ -176,30 +178,31 @@ namespace gtsam {
/* ************************************************************************* */
namespace smallOptimize {
Vector h(const Vector& v) {
double x = v(0);
return Vector_(2, cos(x), sin(x));
Point2 h(const Point2& v) {
return Point2(cos(v.x()), sin(v.y()));
}
Matrix H(const Vector& v) {
double x = v(0);
return Matrix_(2, 1, -sin(x), cos(x));
Matrix H(const Point2& v) {
return Matrix_(2, 2,
-sin(v.x()), 0.0,
0.0, cos(v.y()));
}
struct UnaryFactor: public gtsam::NonlinearFactor1<VectorConfig,
std::string, Vector> {
struct UnaryFactor: public gtsam::NonlinearFactor1<Config,
simulated2D::PoseKey, Point2> {
Vector z_;
Point2 z_;
UnaryFactor(const Vector& z, double sigma, const std::string& key) :
gtsam::NonlinearFactor1<VectorConfig, std::string, Vector>(sigma, key),
z_(z) {
UnaryFactor(const Point2& z, const sharedGaussian& model,
const simulated2D::PoseKey& key) :
gtsam::NonlinearFactor1<Config, simulated2D::PoseKey,
Point2>(model, key), z_(z) {
}
Vector evaluateError(const Vector& x, boost::optional<Matrix&> A =
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A =
boost::none) const {
if (A) *A = H(x);
return h(x) - z_;
return (h(x) - z_).vector();
}
};
@ -207,53 +210,50 @@ namespace gtsam {
}
/* ************************************************************************* */
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph() {
boost::shared_ptr<ExampleNonlinearFactorGraph> fg(
new ExampleNonlinearFactorGraph);
boost::shared_ptr<const Graph> sharedReallyNonlinearFactorGraph() {
boost::shared_ptr<Graph> fg(
new Graph);
Vector z = Vector_(2, 1.0, 0.0);
double sigma = 0.1;
boost::shared_ptr<smallOptimize::UnaryFactor> factor(new smallOptimize::UnaryFactor(
z, sigma, "x"));
boost::shared_ptr<smallOptimize::UnaryFactor> factor(
new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), 1));
fg->push_back(factor);
return fg;
}
ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() {
Graph createReallyNonlinearFactorGraph() {
return *sharedReallyNonlinearFactorGraph();
}
/* ************************************************************************* */
pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T) {
pair<Graph, Config> createNonlinearSmoother(int T) {
// noise on measurements and odometry, respectively
double sigma1 = 1, sigma2 = 1;
// Create
ExampleNonlinearFactorGraph nlfg;
VectorConfig poses;
Graph nlfg;
Config poses;
// prior on x1
Vector x1 = Vector_(2, 1.0, 0.0);
string key1 = symbol('x', 1);
shared prior(new simulated2D::Prior(x1, sigma1, key1));
Point2 x1(1.0, 0.0);
shared prior(new simulated2D::Prior(x1, sigma1, 1));
nlfg.push_back(prior);
poses.insert(key1, x1);
poses.insert(simulated2D::PoseKey(1), x1);
for (int t = 2; t <= T; t++) {
// odometry between x_t and x_{t-1}
Vector odo = Vector_(2, 1.0, 0.0);
string key = symbol('x', t);
shared odometry(new simulated2D::Odometry(odo, sigma2, symbol('x', t - 1),
key));
Point2 odo(1.0, 0.0);
shared odometry(new simulated2D::Odometry(odo, sigma2, t - 1, t));
nlfg.push_back(odometry);
// measurement on x_t is like perfect GPS
Vector xt = Vector_(2, (double) t, 0.0);
shared measurement(new simulated2D::Prior(xt, sigma1, key));
Point2 xt(t, 0);
shared measurement(new simulated2D::Prior(xt, sigma1, t));
nlfg.push_back(measurement);
// initial estimate
poses.insert(key, xt);
poses.insert(simulated2D::PoseKey(t), xt);
}
return make_pair(nlfg, poses);
@ -261,8 +261,8 @@ namespace gtsam {
/* ************************************************************************* */
GaussianFactorGraph createSmoother(int T) {
ExampleNonlinearFactorGraph nlfg;
VectorConfig poses;
Graph nlfg;
Config poses;
boost::tie(nlfg, poses) = createNonlinearSmoother(T);
GaussianFactorGraph lfg = nlfg.linearize(poses);
@ -516,27 +516,25 @@ namespace gtsam {
/* ************************************************************************* */
// Create key for simulated planar graph
string key(int x, int y) {
stringstream ss;
ss << "x" << x << y;
return ss.str();
simulated2D::PoseKey key(int x, int y) {
return simulated2D::PoseKey(1000*x+y);
}
/* ************************************************************************* */
pair<GaussianFactorGraph, VectorConfig> planarGraph(size_t N) {
// create empty graph
NonlinearFactorGraph<VectorConfig> nlfg;
NonlinearFactorGraph<Config> nlfg;
// Create almost hard constraint on x11, sigma=0 will work for PCG not for normal
double sigma0 = 1e-3;
shared constraint(new simulated2D::Prior(Vector_(2, 1.0, 1.0), sigma0, "x11"));
shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sigma0, key(1,1)));
nlfg.push_back(constraint);
double sigma = 0.01;
// Create horizontal constraints, 1...N*(N-1)
Vector z1 = Vector_(2, 1.0, 0.0); // move right
Point2 z1(1.0, 0.0); // move right
for (size_t x = 1; x < N; x++)
for (size_t y = 1; y <= N; y++) {
shared f(new simulated2D::Odometry(z1, sigma, key(x, y), key(x + 1, y)));
@ -544,7 +542,7 @@ namespace gtsam {
}
// Create vertical constraints, N*(N-1)+1..2*N*(N-1)
Vector z2 = Vector_(2, 0.0, 1.0); // move up
Point2 z2(0.0, 1.0); // move up
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y < N; y++) {
shared f(new simulated2D::Odometry(z2, sigma, key(x, y), key(x, y + 1)));
@ -552,11 +550,13 @@ namespace gtsam {
}
// Create linearization and ground xtrue config
VectorConfig zeros, xtrue;
Config zeros;
VectorConfig xtrue;
Point2 zero;
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++) {
zeros.add(key(x, y), zero(2));
xtrue.add(key(x, y), Vector_(2, (double) x, double(y)));
zeros.insert(key(x, y), zero);
xtrue.add((Symbol)key(x, y), Point2(x,y).vector());
}
// linearize around zero
@ -601,4 +601,5 @@ namespace gtsam {
/* ************************************************************************* */
} // example
} // namespace gtsam

31
cpp/smallExample.h
View File

@ -12,28 +12,34 @@
#include <boost/shared_ptr.hpp>
#include "NonlinearFactorGraph.h"
#include "simulated2D.h"
namespace gtsam {
namespace example {
typedef NonlinearFactorGraph<VectorConfig> ExampleNonlinearFactorGraph;
typedef simulated2D::Config Config;
typedef NonlinearFactorGraph<Config> Graph;
/**
* Create small example for non-linear factor graph
*/
boost::shared_ptr<const ExampleNonlinearFactorGraph > sharedNonlinearFactorGraph();
ExampleNonlinearFactorGraph createNonlinearFactorGraph();
boost::shared_ptr<const Graph> sharedNonlinearFactorGraph();
Graph createNonlinearFactorGraph();
/**
* Create configuration to go with it
* The ground truth configuration for the example above
*/
VectorConfig createConfig();
Config createConfig();
/** Vector Config equivalent */
VectorConfig createVectorConfig();
/**
* create a noisy configuration for a nonlinear factor graph
*/
boost::shared_ptr<const VectorConfig> sharedNoisyConfig();
VectorConfig createNoisyConfig();
boost::shared_ptr<const Config> sharedNoisyConfig();
Config createNoisyConfig();
/**
* Zero delta config
@ -59,14 +65,15 @@ namespace gtsam {
/**
* Create really non-linear factor graph (cos/sin)
*/
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph();
ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph();
boost::shared_ptr<const Graph>
sharedReallyNonlinearFactorGraph();
Graph createReallyNonlinearFactorGraph();
/**
* Create a full nonlinear smoother
* @param T number of time-steps
*/
std::pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T);
std::pair<Graph, Config> createNonlinearSmoother(int T);
/**
* Create a Kalman smoother by linearizing a non-linear factor graph
@ -74,7 +81,6 @@ namespace gtsam {
*/
GaussianFactorGraph createSmoother(int T);
/* ******************************************************* */
// Constrained Examples
/* ******************************************************* */
@ -161,7 +167,8 @@ namespace gtsam {
* |
* -x11-x21-x31
*/
std::pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(size_t N,
const GaussianFactorGraph& original);
std::pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(
size_t N, const GaussianFactorGraph& original);
} // example
} // gtsam

3
cpp/testBayesNetPreconditioner.cpp
View File

@ -17,6 +17,7 @@
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
TEST( BayesNetPreconditioner, operators )
@ -88,7 +89,7 @@ TEST( BayesNetPreconditioner, conjugateGradients )
BOOST_FOREACH(const Symbol& j, ordering) y0.insert(j,z2);
VectorConfig y1 = y0;
y1.getReference("x23") = Vector_(2, 1.0, -1.0);
y1.getReference("x2003") = Vector_(2, 1.0, -1.0);
VectorConfig x1 = system.x(y1);
// Solve using PCG

1
cpp/testGaussianBayesNet.cpp
View File

@ -28,6 +28,7 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
TEST( GaussianBayesNet, constructor )

1
cpp/testGaussianFactor.cpp
View File

@ -24,6 +24,7 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
TEST( GaussianFactor, linearFactor )

11
cpp/testGaussianFactorGraph.cpp
View File

@ -26,6 +26,7 @@ using namespace boost::assign;
#include "inference-inl.h" // needed for eliminate and marginals
using namespace gtsam;
using namespace example;
double tol=1e-4;
@ -581,13 +582,13 @@ TEST( GaussianFactorGraph, gradient )
TEST( GaussianFactorGraph, multiplication )
{
GaussianFactorGraph A = createGaussianFactorGraph();
VectorConfig x = createConfig();
VectorConfig x = createCorrectDelta();
Errors actual = A * x;
Errors expected;
expected += Vector_(2, 0.0, 0.0);
expected += Vector_(2,15.0, 0.0);
expected += Vector_(2, 0.0,-5.0);
expected += Vector_(2,-7.5,-5.0);
expected += Vector_(2,-1.0,-1.0);
expected += Vector_(2, 2.0,-1.0);
expected += Vector_(2, 0.0, 1.0);
expected += Vector_(2,-1.0, 1.5);
CHECK(assert_equal(expected,actual));
}

1
cpp/testGaussianISAM.cpp
View File

@ -20,6 +20,7 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests

25
cpp/testGaussianISAM2.cpp
View File

@ -20,12 +20,13 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
TEST( ISAM2, solving )
{
ExampleNonlinearFactorGraph nlfg = createNonlinearFactorGraph();
VectorConfig noisy = createNoisyConfig();
Graph nlfg = createNonlinearFactorGraph();
Config noisy = createNoisyConfig();
Ordering ordering;
ordering += symbol('x', 1);
ordering += symbol('x', 2);
@ -34,8 +35,8 @@ TEST( ISAM2, solving )
VectorConfig actualDelta = optimize2(btree);
VectorConfig delta = createCorrectDelta();
CHECK(assert_equal(delta, actualDelta));
VectorConfig actualSolution = noisy+actualDelta;
VectorConfig solution = createConfig();
Config actualSolution = noisy.expmap(actualDelta);
Config solution = createConfig();
CHECK(assert_equal(solution, actualSolution));
}
@ -43,14 +44,14 @@ TEST( ISAM2, solving )
TEST( ISAM2, ISAM2_smoother )
{
// Create smoother with 7 nodes
ExampleNonlinearFactorGraph smoother;
VectorConfig poses;
Graph smoother;
Config poses;
boost::tie(smoother, poses) = createNonlinearSmoother(7);
// run ISAM2 for every factor
GaussianISAM2 actual;
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<VectorConfig> > factor, smoother) {
ExampleNonlinearFactorGraph factorGraph;
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<Config> > factor, smoother) {
Graph factorGraph;
factorGraph.push_back(factor);
actual.update(factorGraph, poses);
}
@ -76,18 +77,18 @@ TEST( ISAM2, ISAM2_smoother )
TEST( ISAM2, ISAM2_smoother2 )
{
// Create smoother with 7 nodes
ExampleNonlinearFactorGraph smoother;
VectorConfig poses;
Graph smoother;
Config poses;
boost::tie(smoother, poses) = createNonlinearSmoother(7);
// Create initial tree from first 4 timestamps in reverse order !
Ordering ord; ord += "x4","x3","x2","x1";
ExampleNonlinearFactorGraph factors1;
Graph factors1;
for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
GaussianISAM2 actual(factors1, ord, poses);
// run ISAM2 with remaining factors
ExampleNonlinearFactorGraph factors2;
Graph factors2;
for (int i=7;i<13;i++) factors2.push_back(smoother[i]);
actual.update(factors2, poses);

1
cpp/testInference.cpp
View File

@ -14,6 +14,7 @@
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
// The tests below test the *generic* inference algorithms. Some of these have

2
cpp/testIterative.cpp
View File

@ -22,9 +22,9 @@ using namespace boost::assign;
#include "NonlinearFactorGraph-inl.h"
#include "iterative-inl.h"
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
TEST( Iterative, steepestDescent )

166
cpp/testNonlinearConstraint.cpp
View File

@ -19,6 +19,11 @@ using namespace std;
using namespace gtsam;
using namespace boost::assign;
typedef TypedSymbol<Vector, 'x'> Key;
typedef NonlinearConstraint1<VectorConfig, Key, Vector> NLC1;
typedef NonlinearConstraint2<VectorConfig, Key, Vector, Key, Vector> NLC2;
/* ************************************************************************* */
// unary functions with scalar variables
/* ************************************************************************* */
@ -44,14 +49,15 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
// the lagrange multipliers will be expected on L_x1
// and there is only one multiplier
size_t p = 1;
list<Symbol> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(boost::bind(test1::g, _1, keys),
"x", boost::bind(test1::G, _1, keys),
list<Symbol> keys; keys += "x1";
Key x1(1);
NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys),
p, "L1");
// get a configuration to use for finding the error
VectorConfig config;
config.insert("x", Vector_(1, 1.0));
config.insert("x1", Vector_(1, 1.0));
// calculate the error
Vector actual = c1.unwhitenedError(config);
@ -62,14 +68,15 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_linearize ) {
size_t p = 1;
list<Symbol> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(boost::bind(test1::g, _1, keys),
"x", boost::bind(test1::G, _1, keys),
list<Symbol> keys; keys += "x1";
Key x1(1);
NLC1 c1(boost::bind(test1::g, _1, keys),
x1, boost::bind(test1::G, _1, keys),
p, "L1");
// get a configuration to use for linearization
VectorConfig realconfig;
realconfig.insert("x", Vector_(1, 1.0));
realconfig.insert(x1, Vector_(1, 1.0));
// get a configuration of Lagrange multipliers
VectorConfig lagrangeConfig;
@ -80,20 +87,21 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) {
boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
// verify
GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0);
GaussianFactor expectedFactor(x1, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint(x1, Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0);
CHECK(assert_equal(*actualFactor, expectedFactor));
CHECK(assert_equal(*actualConstraint, expectedConstraint));
}
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_scalar_equal ) {
list<Symbol> keys1, keys2; keys1 += "x"; keys2 += "y";
NonlinearConstraint1<VectorConfig>
c1(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1", true),
c2(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1"),
c3(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 2, "L_x1"),
c4(boost::bind(test1::g, _1, keys2), "y", boost::bind(test1::G, _1, keys2), 1, "L_x1");
list<Symbol> keys1, keys2; keys1 += "x0"; keys2 += "x1";
Key x(0), y(1);
NLC1
c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, "L_x1", true),
c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, "L_x1"),
c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2, "L_x1"),
c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1, "L_x1");
CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1));
@ -133,17 +141,18 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
// the lagrange multipliers will be expected on L_xy
// and there is only one multiplier
size_t p = 1;
list<Symbol> keys; keys += "x", "y";
NonlinearConstraint2<VectorConfig> c1(
list<Symbol> keys; keys += "x0", "x1";
Key x0(0), x1(1);
NLC2 c1(
boost::bind(test2::g, _1, keys),
"x", boost::bind(test2::G1, _1, keys),
"y", boost::bind(test2::G1, _1, keys),
x0, boost::bind(test2::G1, _1, keys),
x1, boost::bind(test2::G1, _1, keys),
p, "L12");
// get a configuration to use for finding the error
VectorConfig config;
config.insert("x", Vector_(1, 1.0));
config.insert("y", Vector_(1, 2.0));
config.insert("x0", Vector_(1, 1.0));
config.insert("x1", Vector_(1, 2.0));
// calculate the error
Vector actual = c1.unwhitenedError(config);
@ -155,17 +164,18 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) {
TEST( NonlinearConstraint2, binary_scalar_linearize ) {
// create a constraint
size_t p = 1;
list<Symbol> keys; keys += "x", "y";
NonlinearConstraint2<VectorConfig> c1(
list<Symbol> keys; keys += "x0", "x1";
Key x0(0), x1(1);
NLC2 c1(
boost::bind(test2::g, _1, keys),
"x", boost::bind(test2::G1, _1, keys),
"y", boost::bind(test2::G2, _1, keys),
x0, boost::bind(test2::G1, _1, keys),
x1, boost::bind(test2::G2, _1, keys),
p, "L12");
// get a configuration to use for finding the error
VectorConfig realconfig;
realconfig.insert("x", Vector_(1, 1.0));
realconfig.insert("y", Vector_(1, 2.0));
realconfig.insert(x0, Vector_(1, 1.0));
realconfig.insert(x1, Vector_(1, 2.0));
// get a configuration of Lagrange multipliers
VectorConfig lagrangeConfig;
@ -176,25 +186,26 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) {
boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig);
// verify
GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0),
"y", Matrix_(1,1, -3.0),
GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0),
x1, Matrix_(1,1, -3.0),
"L12", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0),
"y", Matrix_(1,1, -1.0),
GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0),
x1, Matrix_(1,1, -1.0),
Vector_(1, 6.0), 0.0);
CHECK(assert_equal(*actualFactor, expectedFactor));
CHECK(assert_equal(*actualConstraint, expectedConstraint));
CHECK(assert_equal(*actualConstraint, expectedConstraint)); //FAILS - wrong b value
}
/* ************************************************************************* */
TEST( NonlinearConstraint2, binary_scalar_equal ) {
list<Symbol> keys1, keys2, keys3;
keys1 += "x", "y"; keys2 += "y", "x"; keys3 += "x", "z";
NonlinearConstraint2<VectorConfig>
c1(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 1, "L_xy"),
c2(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 1, "L_xy"),
c3(boost::bind(test2::g, _1, keys2), "y", boost::bind(test2::G1, _1, keys2), "x", boost::bind(test2::G2, _1, keys2), 1, "L_xy"),
c4(boost::bind(test2::g, _1, keys3), "x", boost::bind(test2::G1, _1, keys3), "z", boost::bind(test2::G2, _1, keys3), 3, "L_xy");
keys1 += "x0", "x1"; keys2 += "x1", "x0"; keys3 += "x0", "z";
Key x0(0), x1(1), x2(2);
NLC2
c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, "L_xy"),
c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, "L_xy"),
c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1, "L_xy"),
c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3, "L_xy");
CHECK(assert_equal(c1, c2));
CHECK(assert_equal(c2, c1));
@ -208,15 +219,15 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) {
namespace inequality1 {
/** p = 1, g(x) x^2 - 5 > 0 */
Vector g(const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0);
Vector g(const VectorConfig& 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 VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0);
Matrix G(const VectorConfig& config, const Key& key) {
double x = config[key](0);
return Matrix_(1, 1, 2 * x);
}
@ -225,16 +236,16 @@ namespace inequality1 {
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality ) {
size_t p = 1;
list<Symbol> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(boost::bind(inequality1::g, _1, keys),
"x", boost::bind(inequality1::G, _1, keys),
Key x0(0);
NLC1 c1(boost::bind(inequality1::g, _1, x0),
x0, boost::bind(inequality1::G, _1, x0),
p, "L1",
false); // inequality constraint
// get configurations to use for evaluation
VectorConfig config1, config2;
config1.insert("x", Vector_(1, 10.0)); // should be inactive
config2.insert("x", Vector_(1, 1.0)); // should have nonzero error
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));
@ -246,16 +257,16 @@ TEST( NonlinearConstraint1, unary_inequality ) {
/* ************************************************************************* */
TEST( NonlinearConstraint1, unary_inequality_linearize ) {
size_t p = 1;
list<Symbol> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(boost::bind(inequality1::g, _1, keys),
"x", boost::bind(inequality1::G, _1, keys),
Key x0(0);
NLC1 c1(boost::bind(inequality1::g, _1, x0),
x0, boost::bind(inequality1::G, _1, x0),
p, "L1",
false); // inequality constraint
// get configurations to use for linearization
VectorConfig config1, config2;
config1.insert("x", Vector_(1, 10.0)); // should have zero error
config2.insert("x", Vector_(1, 1.0)); // should have nonzero error
config1.insert(x0, Vector_(1, 10.0)); // should have zero error
config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error
// get a configuration of Lagrange multipliers
VectorConfig lagrangeConfig;
@ -274,8 +285,8 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
CHECK(c1.active(config2));
// verify
GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0);
GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0);
GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0);
CHECK(assert_equal(*actualFactor2, expectedFactor));
CHECK(assert_equal(*actualConstraint2, expectedConstraint));
}
@ -286,16 +297,16 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) {
namespace binding1 {
/** p = 1, g(x) x^2 - r > 0 */
Vector g(double r, const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0);
Vector g(double r, const VectorConfig& 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 VectorConfig& config,
const list<Symbol>& keys) {
double x = config[keys.front()](0);
const Key& key) {
double x = config[key](0);
return Matrix_(1, 1, coeff * x);
}
@ -306,17 +317,16 @@ TEST( NonlinearConstraint1, unary_binding ) {
size_t p = 1;
double coeff = 2;
double radius = 5;
list<Symbol> keys; keys += "x";
NonlinearConstraint1<VectorConfig> c1(
boost::bind(binding1::g, radius, _1, keys),
"x", boost::bind(binding1::G, coeff, _1, keys),
Key x0(0);
NLC1 c1(boost::bind(binding1::g, radius, _1, x0),
x0, boost::bind(binding1::G, coeff, _1, x0),
p, "L1",
false); // inequality constraint
// get configurations to use for evaluation
VectorConfig config1, config2;
config1.insert("x", Vector_(1, 10.0)); // should have zero error
config2.insert("x", Vector_(1, 1.0)); // should have nonzero error
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));
@ -329,21 +339,20 @@ TEST( NonlinearConstraint1, unary_binding ) {
namespace binding2 {
/** p = 1, g(x) = x^2-5 -y = 0 */
Vector g(double r, const VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0);
double y = config[keys.back()](0);
Vector g(double r, const VectorConfig& 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 VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.front()](0);
Matrix G1(double c, const VectorConfig& 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 VectorConfig& config, const list<Symbol>& keys) {
double x = config[keys.back()](0);
Matrix G2(double c, const VectorConfig& config) {
return Matrix_(1, 1, -1.0 * c);
}
@ -358,17 +367,16 @@ TEST( NonlinearConstraint2, binary_binding ) {
double a = 2.0;
double b = 1.0;
double r = 5.0;
list<Symbol> keys; keys += "x", "y";
NonlinearConstraint2<VectorConfig> c1(
boost::bind(binding2::g, r, _1, keys),
"x", boost::bind(binding2::G1, a, _1, keys),
"y", boost::bind(binding2::G2, b, _1, keys),
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, "L1");
// get a configuration to use for finding the error
VectorConfig config;
config.insert("x", Vector_(1, 1.0));
config.insert("y", Vector_(1, 2.0));
config.insert(x0, Vector_(1, 1.0));
config.insert(x1, Vector_(1, 2.0));
// calculate the error
Vector actual = c1.unwhitenedError(config);

38
cpp/testNonlinearFactor.cpp
View File

@ -22,21 +22,22 @@
using namespace std;
using namespace gtsam;
using namespace example;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared_nlf;
/* ************************************************************************* */
TEST( NonlinearFactor, equals )
{
double sigma = 1.0;
sharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0));
// create two nonlinear2 factors
Vector z3 = Vector_(2,0.,-1.);
simulated2D::Measurement f0(z3, sigma, "x1", "l1");
Point2 z3(0.,-1.);
simulated2D::Measurement f0(z3, sigma, 1,1);
// measurement between x2 and l1
Vector z4 = Vector_(2,-1.5, -1.);
simulated2D::Measurement f1(z4, sigma, "x2", "l1");
Point2 z4(-1.5, -1.);
simulated2D::Measurement f1(z4, sigma, 2,1);
CHECK(assert_equal(f0,f0));
CHECK(f0.equals(f0));
@ -48,10 +49,10 @@ TEST( NonlinearFactor, equals )
TEST( NonlinearFactor, equals2 )
{
// create a non linear factor graph
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
Graph fg = createNonlinearFactorGraph();
// get two factors
shared_nlf f0 = fg[0], f1 = fg[1];
Graph::sharedFactor f0 = fg[0], f1 = fg[1];
CHECK(f0->equals(*f0));
CHECK(!f0->equals(*f1));
@ -62,13 +63,13 @@ TEST( NonlinearFactor, equals2 )
TEST( NonlinearFactor, NonlinearFactor )
{
// create a non linear factor graph
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
Graph fg = createNonlinearFactorGraph();
// create a configuration for the non linear factor graph
VectorConfig cfg = createNoisyConfig();
Config cfg = createNoisyConfig();
// get the factor "f1" from the factor graph
shared_nlf factor = fg[0];
Graph::sharedFactor factor = fg[0];
// calculate the error_vector from the factor "f1"
// the expected value for the whitened error from the factor
@ -88,7 +89,7 @@ TEST( NonlinearFactor, NonlinearFactor )
TEST( NonlinearFactor, linearize_f1 )
{
// Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
Graph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf =
boost::static_pointer_cast<NonlinearFactor1>(nfg[0]);
@ -110,7 +111,7 @@ TEST( NonlinearFactor, linearize_f1 )
TEST( NonlinearFactor, linearize_f2 )
{
// Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
Graph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf =
boost::static_pointer_cast<NonlinearFactor1>(nfg[1]);
@ -128,7 +129,7 @@ TEST( NonlinearFactor, linearize_f2 )
TEST( NonlinearFactor, linearize_f3 )
{
// Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
Graph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf =
boost::static_pointer_cast<NonlinearFactor1>(nfg[2]);
@ -146,7 +147,7 @@ TEST( NonlinearFactor, linearize_f3 )
TEST( NonlinearFactor, linearize_f4 )
{
// Grab a non-linear factor
ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
Graph nfg = createNonlinearFactorGraph();
boost::shared_ptr<NonlinearFactor1> nlf =
boost::static_pointer_cast<NonlinearFactor1>(nfg[3]);
@ -164,15 +165,14 @@ TEST( NonlinearFactor, linearize_f4 )
TEST( NonlinearFactor, size )
{
// create a non linear factor graph
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
Graph fg = createNonlinearFactorGraph();
// create a configuration for the non linear factor graph
VectorConfig cfg = createNoisyConfig();
Config cfg = createNoisyConfig();
// get some factors from the graph
shared_nlf factor1 = fg[0];
shared_nlf factor2 = fg[1];
shared_nlf factor3 = fg[2];
Graph::sharedFactor factor1 = fg[0], factor2 = fg[1],
factor3 = fg[2];
CHECK(factor1->size() == 1);
CHECK(factor2->size() == 2);

31
cpp/testNonlinearFactorGraph.cpp
View File

@ -23,43 +23,44 @@ using namespace boost::assign;
#include "NonlinearFactorGraph-inl.h"
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
TEST( ExampleNonlinearFactorGraph, equals )
TEST( Graph, equals )
{
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
ExampleNonlinearFactorGraph fg2 = createNonlinearFactorGraph();
Graph fg = createNonlinearFactorGraph();
Graph fg2 = createNonlinearFactorGraph();
CHECK( fg.equals(fg2) );
}
/* ************************************************************************* */
TEST( ExampleNonlinearFactorGraph, error )
TEST( Graph, error )
{
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
VectorConfig c1 = createConfig();
Graph fg = createNonlinearFactorGraph();
Config c1 = createConfig();
double actual1 = fg.error(c1);
DOUBLES_EQUAL( 0.0, actual1, 1e-9 );
VectorConfig c2 = createNoisyConfig();
Config c2 = createNoisyConfig();
double actual2 = fg.error(c2);
DOUBLES_EQUAL( 5.625, actual2, 1e-9 );
}
/* ************************************************************************* */
TEST( ExampleNonlinearFactorGraph, GET_ORDERING)
TEST( Graph, GET_ORDERING)
{
Ordering expected;
expected += "l1","x1","x2";
ExampleNonlinearFactorGraph nlfg = createNonlinearFactorGraph();
Graph nlfg = createNonlinearFactorGraph();
Ordering actual = nlfg.getOrdering();
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( ExampleNonlinearFactorGraph, probPrime )
TEST( Graph, probPrime )
{
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
VectorConfig cfg = createConfig();
Graph fg = createNonlinearFactorGraph();
Config cfg = createConfig();
// evaluate the probability of the factor graph
double actual = fg.probPrime(cfg);
@ -69,10 +70,10 @@ TEST( ExampleNonlinearFactorGraph, probPrime )
/* ************************************************************************* *
// TODO: Commented out until noise model is passed to Gaussian factor graph
TEST( ExampleNonlinearFactorGraph, linearize )
TEST( Graph, linearize )
{
ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
VectorConfig initial = createNoisyConfig();
Graph fg = createNonlinearFactorGraph();
Config initial = createNoisyConfig();
GaussianFactorGraph linearized = fg.linearize(initial);
GaussianFactorGraph expected = createGaussianFactorGraph();
CHECK(assert_equal(expected,linearized));

41
cpp/testNonlinearOptimizer.cpp
View File

@ -30,13 +30,15 @@ using namespace boost;
#include "SubgraphPreconditioner-inl.h"
using namespace gtsam;
using namespace example;
typedef NonlinearOptimizer<ExampleNonlinearFactorGraph,VectorConfig> Optimizer;
typedef NonlinearOptimizer<Graph,Config> Optimizer;
/* ************************************************************************* */
TEST( NonlinearOptimizer, delta )
{
shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph(createNonlinearFactorGraph()));
shared_ptr<Graph> fg(new Graph(
createNonlinearFactorGraph()));
Optimizer::shared_config initial = sharedNoisyConfig();
// Expected configuration is the difference between the noisy config
@ -81,19 +83,21 @@ TEST( NonlinearOptimizer, delta )
TEST( NonlinearOptimizer, iterateLM )
{
// really non-linear factor graph
shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph(createReallyNonlinearFactorGraph()));
shared_ptr<Graph> fg(new Graph(
createReallyNonlinearFactorGraph()));
// config far from minimum
Vector x0 = Vector_(1, 3.0);
boost::shared_ptr<VectorConfig> config(new VectorConfig);
config->insert("x", x0);
Point2 x0(3,0);
boost::shared_ptr<Config> config(new Config);
config->insert(simulated2D::PoseKey(1), x0);
// ordering
shared_ptr<Ordering> ord(new Ordering());
ord->push_back("x");
ord->push_back("x1");
// create initial optimization state, with lambda=0
Optimizer::shared_solver solver(new Factorization<ExampleNonlinearFactorGraph, VectorConfig>);
Optimizer::shared_solver solver(new Factorization<
Graph, Config> );
Optimizer optimizer(fg, ord, config, solver, 0.);
// normal iterate
@ -117,23 +121,24 @@ TEST( NonlinearOptimizer, iterateLM )
/* ************************************************************************* */
TEST( NonlinearOptimizer, optimize )
{
shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph(createReallyNonlinearFactorGraph()));
shared_ptr<Graph> fg(new Graph(
createReallyNonlinearFactorGraph()));
// test error at minimum
Vector xstar = Vector_(1, 0.0);
VectorConfig cstar;
cstar.insert("x", xstar);
Point2 xstar(0,0);
Config cstar;
cstar.insert(simulated2D::PoseKey(1), xstar);
DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Vector x0 = Vector_(1, 3.0);
boost::shared_ptr<VectorConfig> c0(new VectorConfig);
c0->insert("x", x0);
Point2 x0(3,3);
boost::shared_ptr<Config> c0(new Config);
c0->insert(simulated2D::PoseKey(1), x0);
DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
// optimize parameters
shared_ptr<Ordering> ord(new Ordering());
ord->push_back("x");
ord->push_back("x1");
double relativeThreshold = 1e-5;
double absoluteThreshold = 1e-5;
@ -143,12 +148,12 @@ TEST( NonlinearOptimizer, optimize )
// Gauss-Newton
Optimizer actual1 = optimizer.gaussNewton(relativeThreshold,
absoluteThreshold);
CHECK(assert_equal(*(actual1.config()),cstar));
DOUBLES_EQUAL(0,fg->error(*(actual1.config())),1e-3);
// Levenberg-Marquardt
Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold,
absoluteThreshold, Optimizer::SILENT);
CHECK(assert_equal(*(actual2.config()),cstar));
DOUBLES_EQUAL(0,fg->error(*(actual2.config())),1e-3);
}
/* ************************************************************************* */

268
cpp/testSQP.cpp
View File

@ -16,6 +16,7 @@
#define GTSAM_MAGIC_GAUSSIAN 2
#define GTSAM_MAGIC_KEY
#include <Pose3.h>
#include <GaussianFactorGraph.h>
#include <NonlinearOptimizer.h>
#include <SQPOptimizer.h>
@ -239,62 +240,58 @@ TEST (SQP, problem1_sqp ) {
}
/* ********************************************************************* */
// components for nonlinear factor graphs
bool vector_compare(const Vector& a, const Vector& b) {
return equal_with_abs_tol(a, b, 1e-5);
}
typedef NonlinearFactorGraph<VectorConfig> NLGraph;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
typedef boost::shared_ptr<NonlinearConstraint<VectorConfig> > shared_c;
typedef simulated2D::Config Config2D;
typedef NonlinearFactorGraph<Config2D> NLGraph;
typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> NLE;
typedef boost::shared_ptr<simulated2D::Measurement > shared;
typedef NonlinearOptimizer<NLGraph, Config2D> Optimizer;
typedef NonlinearEquality<VectorConfig,Symbol,Vector> NLE;
typedef boost::shared_ptr<NLE> shared_eq;
typedef boost::shared_ptr<VectorConfig> shared_cfg;
typedef NonlinearOptimizer<NLGraph,VectorConfig> Optimizer;
typedef TypedSymbol<Vector, 'L'> LamKey;
/* *********************************************************************
/*
* Determining a ground truth linear system
* with two poses seeing one landmark, with each pose
* constrained to a particular value
*/
TEST (SQP, two_pose_truth ) {
bool verbose = false;
// create a graph
shared_ptr<NLGraph> graph(new NLGraph);
// add the constraints on the ends
// position (1, 1) constraint for x1
// position (5, 6) constraint for x2
VectorConfig feas;
Vector feas1 = Vector_(2, 1.0, 1.0);
Vector feas2 = Vector_(2, 5.0, 6.0);
feas.insert("x1", feas1);
feas.insert("x2", feas2);
// constant constraint on x1
shared_eq ef1(new NLE("x1", feas1, vector_compare));
// constant constraint on x2
shared_eq ef2(new NLE("x2", feas2, vector_compare));
// measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0);
double sigma1 = 0.1;
shared f1(new simulated2D::Measurement(z1, sigma1, "x1", "l1"));
// measurement from x2 to l1
Vector z2 = Vector_(2, -4.0, 0.0);
double sigma2 = 0.1;
shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l1"));
// construct the graph
shared_ptr<NLGraph> graph(new NLGraph());
simulated2D::PoseKey x1(1), x2(2);
simulated2D::PointKey l1(1);
Point2 pt_x1(1.0, 1.0),
pt_x2(5.0, 6.0);
shared_ptr<NLE> ef1(new NLE(x1, pt_x1)),
ef2(new NLE(x2, pt_x2));
graph->push_back(ef1);
graph->push_back(ef2);
// measurement from x1 to l1
Point2 z1(0.0, 5.0);
sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
shared f1(new simulated2D::Measurement(z1, sigma, x1,l1));
graph->push_back(f1);
// measurement from x2 to l1
Point2 z2(-4.0, 0.0);
shared f2(new simulated2D::Measurement(z2, sigma, x2,l1));
graph->push_back(f2);
// create an initial estimate
boost::shared_ptr<VectorConfig> initialEstimate(new VectorConfig(feas)); // must start with feasible set
initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth
//initialEstimate->insert("l1", Vector_(2, 1.2, 5.6)); // with small error
Point2 pt_l1(
1.0, 6.0 // ground truth
//1.2, 5.6 // small error
);
shared_ptr<Config2D> initialEstimate(new Config2D);
initialEstimate->insert(l1, pt_l1);
initialEstimate->insert(x1, pt_x1);
initialEstimate->insert(x2, pt_x2);
// optimize the graph
shared_ptr<Ordering> ordering(new Ordering());
@ -305,12 +302,14 @@ TEST (SQP, two_pose_truth ) {
double relativeThreshold = 1e-5;
double absoluteThreshold = 1e-5;
Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold);
boost::shared_ptr<const VectorConfig> actual = act_opt.config();
boost::shared_ptr<const Config2D> actual = act_opt.config();
if (verbose) actual->print("Configuration after optimization");
// verify
VectorConfig expected(feas);
expected.insert("l1", Vector_(2, 1.0, 6.0));
Config2D expected;
expected.insert(x1, pt_x1);
expected.insert(x2, pt_x2);
expected.insert(l1, Point2(1.0, 6.0));
CHECK(assert_equal(expected, *actual, 1e-5));
}
@ -319,36 +318,43 @@ namespace sqp_test1 {
// binary constraint between landmarks
/** g(x) = x-y = 0 */
Vector g(const VectorConfig& config, const list<Symbol>& keys) {
return config[keys.front()] - config[keys.back()];
Vector g(const Config2D& config, const list<Symbol>& keys) {
Point2 pt1, pt2;
pt1 = config[simulated2D::PointKey(keys.front().index())];
pt2 = config[simulated2D::PointKey(keys.back().index())];
return Vector_(2, pt1.x() - pt2.x(), pt1.y() - pt2.y());
}
/** jacobian at l1 */
Matrix G1(const VectorConfig& config, const list<Symbol>& keys) {
Matrix G1(const Config2D& config, const list<Symbol>& keys) {
return eye(2);
}
/** jacobian at l2 */
Matrix G2(const VectorConfig& config, const list<Symbol>& keys) {
Matrix G2(const Config2D& config, const list<Symbol>& keys) {
return -1 * eye(2);
}
} // \namespace sqp_test1
namespace sqp_test2 {
//namespace sqp_test2 {
//
// // Unary Constraint on x1
// /** g(x) = x -[1;1] = 0 */
// Vector g(const Config2D& config, const list<Symbol>& keys) {
// return config[keys.front()] - Vector_(2, 1.0, 1.0);
// }
//
// /** jacobian at x1 */
// Matrix G(const Config2D& config, const list<Symbol>& keys) {
// return eye(2);
// }
//
//} // \namespace sqp_test2
// Unary Constraint on x1
/** g(x) = x -[1;1] = 0 */
Vector g(const VectorConfig& config, const list<Symbol>& keys) {
return config[keys.front()] - Vector_(2, 1.0, 1.0);
}
/** jacobian at x1 */
Matrix G(const VectorConfig& config, const list<Symbol>& keys) {
return eye(2);
}
} // \namespace sqp_test2
typedef NonlinearConstraint2<
Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2;
/* *********************************************************************
* Version that actually uses nonlinear equality constraints
@ -359,68 +365,65 @@ namespace sqp_test2 {
*/
TEST (SQP, two_pose ) {
bool verbose = false;
// position (1, 1) constraint for x1
VectorConfig feas;
feas.insert("x1", Vector_(2, 1.0, 1.0));
// constant constraint on x1
list<Symbol> keys1; keys1 += "x1";
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>(
boost::bind(sqp_test2::g, _1, keys1),
"x1", boost::bind(sqp_test2::G, _1, keys1),
2, "L1"));
// create the graph
shared_ptr<NLGraph> graph(new NLGraph);
// add the constraints on the ends
// position (1, 1) constraint for x1
// position (5, 6) constraint for x2
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);
shared_ptr<NLE> ef1(new NLE(x1, pt_x1));
graph->push_back(ef1);
// measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0);
double sigma1 = 0.1;
shared f1(new simulated2D::Measurement(z1, sigma1, "x1", "l1"));
Point2 z1(0.0, 5.0);
sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1));
shared f1(new simulated2D::Measurement(z1, sigma, x1,l1));
graph->push_back(f1);
// measurement from x2 to l2
Vector z2 = Vector_(2, -4.0, 0.0);
double sigma2 = 0.1;
shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l2"));
Point2 z2(-4.0, 0.0);
shared f2(new simulated2D::Measurement(z2, sigma, x2,l2));
graph->push_back(f2);
// equality constraint between l1 and l2
list<Symbol> keys2; keys2 += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>(
boost::shared_ptr<NLC2 > c2(new NLC2(
boost::bind(sqp_test1::g, _1, keys2),
"l1", boost::bind(sqp_test1::G1, _1, keys2),
"l2", boost::bind(sqp_test1::G2, _1, keys2),
2, "L12"));
// construct the graph
NLGraph graph;
graph.push_back(c1);
graph.push_back(c2);
graph.push_back(f1);
graph.push_back(f2);
l1, boost::bind(sqp_test1::G1, _1, keys2),
l2, boost::bind(sqp_test1::G2, _1, keys2),
2, "L1"));
graph->push_back(c2);
// create an initial estimate
shared_cfg initialEstimate(new VectorConfig(feas)); // must start with feasible set
initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth
initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin
shared_ptr<Config2D> initialEstimate(new Config2D);
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
// create an initial estimate for the lagrange multiplier
shared_cfg initLagrange(new VectorConfig);
initLagrange->insert("L12", Vector_(2, 1.0, 1.0));
initLagrange->insert("L1", Vector_(2, 1.0, 1.0));
shared_ptr<VectorConfig> initLagrange(new VectorConfig);
initLagrange->insert(LamKey(1), Vector_(2, 1.0, 1.0)); // connect the landmarks
// create state config variables and initialize them
VectorConfig state(*initialEstimate), state_lambda(*initLagrange);
Config2D state(*initialEstimate);
VectorConfig state_lambda(*initLagrange);
// optimization loop
int maxIt = 1;
for (int i = 0; i<maxIt; ++i) {
// linearize the graph
GaussianFactorGraph fg;
typedef FactorGraph<NonlinearFactor<VectorConfig> >::const_iterator const_iterator;
typedef NonlinearConstraint<VectorConfig> NLConstraint;
typedef FactorGraph<NonlinearFactor<Config2D> >::const_iterator const_iterator;
// iterate over all factors
for (const_iterator factor = graph.begin(); factor < graph.end(); factor++) {
const shared_c constraint = boost::shared_dynamic_cast<NLConstraint >(*factor);
for (const_iterator factor = graph->begin(); factor < graph->end(); factor++) {
const shared_ptr<NLC2> constraint = boost::shared_dynamic_cast<NLC2>(*factor);
if (constraint == NULL) {
// if a regular factor, linearize using the default linearization
GaussianFactor::shared_ptr f = (*factor)->linearize(state);
@ -433,11 +436,12 @@ TEST (SQP, two_pose ) {
fg.push_back(c);
}
}
if (verbose) fg.print("Linearized graph");
// create an ordering
Ordering ordering;
ordering += "x1", "x2", "l1", "l2", "L12", "L1";
ordering += "x1", "x2", "l1", "l2", "L1";
// optimize linear graph to get full delta config
VectorConfig delta = fg.optimize(ordering);
@ -451,10 +455,11 @@ TEST (SQP, two_pose ) {
}
// verify
VectorConfig expected(feas);
expected.insert("l1", Vector_(2, 1.0, 6.0));
expected.insert("l2", Vector_(2, 1.0, 6.0));
expected.insert("x2", Vector_(2, 5.0, 6.0));
Config2D 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, state, 1e-5));
}
@ -471,9 +476,14 @@ using namespace gtsam::visualSLAM;
using namespace boost;
// typedefs for visual SLAM example
typedef visualSLAM::Config VConfig;
typedef visualSLAM::Graph VGraph;
typedef boost::shared_ptr<ProjectionFactor> shared_vf;
typedef NonlinearOptimizer<Graph,Config> VOptimizer;
typedef SQPOptimizer<Graph, Config> SOptimizer;
typedef NonlinearOptimizer<VGraph,VConfig> VOptimizer;
typedef SQPOptimizer<VGraph, VConfig> VSOptimizer;
typedef NonlinearConstraint2<
VConfig, visualSLAM::PointKey, Pose3, visualSLAM::PointKey, Pose3> VNLC2;
/**
* Ground truth for a visual SLAM example with stereo vision
@ -494,13 +504,13 @@ TEST (SQP, stereo_truth ) {
Point3 landmarkNoisy(1.0, 6.0, 0.0);
// create truth config
boost::shared_ptr<Config> truthConfig(new Config);
boost::shared_ptr<VConfig> truthConfig(new Config);
truthConfig->insert(1, camera1.pose());
truthConfig->insert(2, camera2.pose());
truthConfig->insert(1, landmark);
// create graph
shared_ptr<Graph> graph(new Graph());
shared_ptr<VGraph> graph(new VGraph());
// create equality constraints for poses
graph->push_back(shared_ptr<PoseConstraint>(new PoseConstraint(1, camera1.pose())));
@ -619,12 +629,6 @@ TEST (SQP, stereo_truth_noisy ) {
CHECK(assert_equal(*truthConfig,*(optimizer.config())));
}
/* ********************************************************************* */
// Utility function to strip out a landmark number from a string key
//int getNum(const string& key) {
// return atoi(key.substr(1, key.size()-1).c_str());
//}
/* ********************************************************************* */
namespace sqp_stereo {
@ -648,7 +652,7 @@ namespace sqp_stereo {
} // \namespace sqp_stereo
/* ********************************************************************* */
Graph stereoExampleGraph() {
VGraph stereoExampleGraph() {
// create initial estimates
Rot3 faceDownY(Matrix_(3,3,
1.0, 0.0, 0.0,
@ -662,7 +666,7 @@ Graph stereoExampleGraph() {
Point3 landmark2(1.0, 5.0, 0.0);
// create graph
Graph graph;
VGraph graph;
// create equality constraints for poses
graph.push_back(shared_ptr<PoseConstraint>(new PoseConstraint(1, camera1.pose())));
@ -680,11 +684,11 @@ Graph stereoExampleGraph() {
// NOTE: this is really just a linear constraint that is exactly the same
// as the previous examples
list<Symbol> keys; keys += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<Config> > c2(
new NonlinearConstraint2<Config>(
boost::bind(sqp_stereo::g, _1, keys),
"l1", boost::bind(sqp_stereo::G1, _1, keys),
"l2", boost::bind(sqp_stereo::G2, _1, keys),
visualSLAM::PointKey l1(1), l2(2);
shared_ptr<VNLC2> c2(
new VNLC2(boost::bind(sqp_stereo::g, _1, keys),
l1, boost::bind(sqp_stereo::G1, _1, keys),
l2, boost::bind(sqp_stereo::G2, _1, keys),
3, "L12"));
graph.push_back(c2);
@ -692,7 +696,7 @@ Graph stereoExampleGraph() {
}
/* ********************************************************************* */
boost::shared_ptr<Config> stereoExampleTruthConfig() {
boost::shared_ptr<VConfig> stereoExampleTruthConfig() {
// create initial estimates
Rot3 faceDownY(Matrix_(3,3,
1.0, 0.0, 0.0,
@ -723,21 +727,21 @@ TEST (SQP, stereo_sqp ) {
bool verbose = false;
// get a graph
Graph graph = stereoExampleGraph();
VGraph graph = stereoExampleGraph();
if (verbose) graph.print("Graph after construction");
// get the truth config
boost::shared_ptr<Config> truthConfig = stereoExampleTruthConfig();
boost::shared_ptr<VConfig> truthConfig = stereoExampleTruthConfig();
// create ordering
Ordering ord;
ord += "x1", "x2", "l1", "l2";
// create optimizer
SOptimizer optimizer(graph, ord, truthConfig);
VSOptimizer optimizer(graph, ord, truthConfig);
// optimize
SOptimizer afterOneIteration = optimizer.iterate();
VSOptimizer afterOneIteration = optimizer.iterate();
// check if correct
CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
@ -775,7 +779,7 @@ TEST (SQP, stereo_sqp_noisy ) {
ord += "x1", "x2", "l1", "l2";
// create optimizer
SOptimizer optimizer(graph, ord, initConfig);
VSOptimizer optimizer(graph, ord, initConfig);
// optimize
double start_error = optimizer.error();
@ -786,9 +790,9 @@ TEST (SQP, stereo_sqp_noisy ) {
//if (verbose) optimizer.graph()->print();
if (verbose) optimizer.config()->print();
if (verbose)
optimizer = optimizer.iterate(SOptimizer::FULL);
optimizer = optimizer.iterate(VSOptimizer::FULL);
else
optimizer = optimizer.iterate(SOptimizer::SILENT);
optimizer = optimizer.iterate(VSOptimizer::SILENT);
}
if (verbose) cout << "Initial Error: " << start_error << "\n"
@ -840,11 +844,11 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
ord += "x1", "x2", "l1", "l2", "L12";
// create lagrange multipliers
SOptimizer::shared_vconfig initLagrangeConfig(new VectorConfig);
VSOptimizer::shared_vconfig initLagrangeConfig(new VectorConfig);
initLagrangeConfig->insert("L12", Vector_(3, 0.0, 0.0, 0.0));
// create optimizer
SOptimizer optimizer(graph, ord, initConfig, initLagrangeConfig);
VSOptimizer optimizer(graph, ord, initConfig, initLagrangeConfig);
// optimize
double start_error = optimizer.error();
@ -855,10 +859,10 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
<< " Iteration: " << i << endl;
optimizer.config()->print("Config Before Iteration");
optimizer.configLagrange()->print("Lagrange Before Iteration");
optimizer = optimizer.iterate(SOptimizer::FULL);
optimizer = optimizer.iterate(VSOptimizer::FULL);
}
else
optimizer = optimizer.iterate(SOptimizer::SILENT);
optimizer = optimizer.iterate(VSOptimizer::SILENT);
}
if (verbose) cout << "Initial Error: " << start_error << "\n"

333
cpp/testSQPOptimizer.cpp
View File

@ -27,23 +27,34 @@
using namespace std;
using namespace gtsam;
using namespace boost;
using namespace boost::assign;
using namespace simulated2D;
static sharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1));
// typedefs
typedef boost::shared_ptr<VectorConfig> shared_config;
typedef NonlinearFactorGraph<VectorConfig> NLGraph;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
typedef boost::shared_ptr<NonlinearConstraint<VectorConfig> > shared_c;
typedef simulated2D::Config Config2D;
typedef boost::shared_ptr<Config2D> shared_config;
typedef NonlinearFactorGraph<Config2D> NLGraph;
typedef boost::shared_ptr<NonlinearFactor<Config2D> > shared;
namespace map_warp_example {
typedef NonlinearConstraint1<
Config2D, simulated2D::PoseKey, Point2> NLC1;
typedef NonlinearConstraint2<
Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2;
} // \namespace map_warp_example
/* ********************************************************************* */
TEST ( SQPOptimizer, basic ) {
// create a basic optimizer
NLGraph graph;
Ordering ordering;
shared_config config(new VectorConfig);
shared_config config(new Config2D);
SQPOptimizer<NLGraph, VectorConfig> optimizer(graph, ordering, config);
SQPOptimizer<NLGraph, Config2D> optimizer(graph, ordering, config);
// verify components
CHECK(assert_equal(graph, *(optimizer.graph())));
@ -59,17 +70,18 @@ TEST ( SQPOptimizer, basic ) {
namespace sqp_LinearMapWarp2 {
// binary constraint between landmarks
/** g(x) = x-y = 0 */
Vector g_func(const VectorConfig& config, const list<Symbol>& keys) {
return config[keys.front()]-config[keys.back()];
Vector g_func(const Config2D& config, const PointKey& key1, const PointKey& key2) {
Point2 p = config[key1]-config[key2];
return Vector_(2, p.x(), p.y());
}
/** jacobian at l1 */
Matrix jac_g1(const VectorConfig& config, const list<Symbol>& keys) {
Matrix jac_g1(const Config2D& config) {
return eye(2);
}
/** jacobian at l2 */
Matrix jac_g2(const VectorConfig& config, const list<Symbol>& keys) {
Matrix jac_g2(const Config2D& config) {
return -1*eye(2);
}
} // \namespace sqp_LinearMapWarp2
@ -77,46 +89,49 @@ Matrix jac_g2(const VectorConfig& config, const list<Symbol>& keys) {
namespace sqp_LinearMapWarp1 {
// Unary Constraint on x1
/** g(x) = x -[1;1] = 0 */
Vector g_func(const VectorConfig& config, const list<Symbol>& keys) {
return config[keys.front()]-Vector_(2, 1.0, 1.0);
Vector g_func(const Config2D& config, const PoseKey& key) {
Point2 p = config[key]-Point2(1.0, 1.0);
return Vector_(2, p.x(), p.y());
}
/** jacobian at x1 */
Matrix jac_g(const VectorConfig& config, const list<Symbol>& keys) {
Matrix jac_g(const Config2D& config) {
return eye(2);
}
} // \namespace sqp_LinearMapWarp12
typedef SQPOptimizer<NLGraph, VectorConfig> Optimizer;
typedef SQPOptimizer<NLGraph, Config2D> Optimizer;
/**
* Creates the graph with each robot seeing the landmark, and it is
* known that it is the same landmark
*/
NLGraph linearMapWarpGraph() {
using namespace map_warp_example;
// keys
PoseKey x1(1), x2(2);
PointKey l1(1), l2(2);
// constant constraint on x1
list<Symbol> keyx; keyx += "x1";
boost::shared_ptr<NonlinearConstraint1<VectorConfig> > c1(
new NonlinearConstraint1<VectorConfig>(
boost::bind(sqp_LinearMapWarp1::g_func, _1, keyx),
"x1", boost::bind(sqp_LinearMapWarp1::jac_g, _1, keyx),
shared_ptr<NLC1> c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1),
x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1),
2, "L1"));
// measurement from x1 to l1
Vector z1 = Vector_(2, 0.0, 5.0);
shared f1(new simulated2D::Measurement(z1, sigma, "x1", "l1"));
Point2 z1(0.0, 5.0);
shared f1(new simulated2D::Measurement(z1, sigma, 1,1));
// measurement from x2 to l2
Vector z2 = Vector_(2, -4.0, 0.0);
shared f2(new simulated2D::Measurement(z2, sigma, "x2", "l2"));
Point2 z2(-4.0, 0.0);
shared f2(new simulated2D::Measurement(z2, sigma, 2,2));
// equality constraint between l1 and l2
list<Symbol> keys; keys += "l1", "l2";
boost::shared_ptr<NonlinearConstraint2<VectorConfig> > c2(
new NonlinearConstraint2<VectorConfig>(
boost::bind(sqp_LinearMapWarp2::g_func, _1, keys),
"l1", boost::bind(sqp_LinearMapWarp2::jac_g1, _1, keys),
"l2", boost::bind(sqp_LinearMapWarp2::jac_g2, _1, keys),
shared_ptr<NLC2> c2 (new NLC2(
boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2),
l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1),
l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1),
2, "L12"));
// construct the graph
@ -135,15 +150,19 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
// get a graph
NLGraph graph = linearMapWarpGraph();
// keys
PoseKey x1(1), x2(2);
PointKey l1(1), l2(2);
// create an initial estimate
shared_config initialEstimate(new VectorConfig);
initialEstimate->insert("x1", Vector_(2, 1.0, 1.0));
initialEstimate->insert("l1", Vector_(2, 1.0, 6.0));
initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin
shared_config initialEstimate(new Config2D);
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
// create an initial estimate for the lagrange multiplier
shared_config initLagrange(new VectorConfig);
shared_ptr<VectorConfig> initLagrange(new VectorConfig);
initLagrange->insert("L12", Vector_(2, 1.0, 1.0));
initLagrange->insert("L1", Vector_(2, 1.0, 1.0));
@ -159,12 +178,12 @@ TEST ( SQPOptimizer, map_warp_initLam ) {
Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
// get the config back out and verify
VectorConfig actual = *(oneIteration.config());
VectorConfig expected;
expected.insert("x1", Vector_(2, 1.0, 1.0));
expected.insert("l1", Vector_(2, 1.0, 6.0));
expected.insert("l2", Vector_(2, 1.0, 6.0));
expected.insert("x2", Vector_(2, 5.0, 6.0));
Config2D actual = *(oneIteration.config());
Config2D 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));
}
@ -175,12 +194,16 @@ TEST ( SQPOptimizer, map_warp ) {
NLGraph graph = linearMapWarpGraph();
if (verbose) graph.print("Initial map warp graph");
// keys
PoseKey x1(1), x2(2);
PointKey l1(1), l2(2);
// create an initial estimate
shared_config initialEstimate(new VectorConfig);
initialEstimate->insert("x1", Vector_(2, 1.0, 1.0));
initialEstimate->insert("l1", Vector_(2, 1.0, 6.0));
initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame
initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin
shared_config initialEstimate(new Config2D);
initialEstimate->insert(x1, Point2(1.0, 1.0));
initialEstimate->insert(l1, Point2(.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
// create an ordering
Ordering ordering;
@ -193,12 +216,12 @@ TEST ( SQPOptimizer, map_warp ) {
Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT);
// get the config back out and verify
VectorConfig actual = *(oneIteration.config());
VectorConfig expected;
expected.insert("x1", Vector_(2, 1.0, 1.0));
expected.insert("l1", Vector_(2, 1.0, 6.0));
expected.insert("l2", Vector_(2, 1.0, 6.0));
expected.insert("x2", Vector_(2, 5.0, 6.0));
Config2D actual = *(oneIteration.config());
Config2D 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));
}
@ -209,16 +232,13 @@ TEST ( SQPOptimizer, map_warp ) {
// states, which enforces a minimum distance.
/* ********************************************************************* */
bool vector_compare(const Vector& a, const Vector& b) {
return equal_with_abs_tol(a, b, 1e-5);
}
typedef NonlinearConstraint2<Config2D, PoseKey, Point2, PointKey, Point2> AvoidConstraint;
typedef shared_ptr<AvoidConstraint> shared_a;
typedef NonlinearEquality<Config2D, simulated2D::PoseKey, Point2> PoseConstraint;
typedef shared_ptr<PoseConstraint> shared_pc;
typedef NonlinearEquality<Config2D, simulated2D::PointKey, Point2> ObstacleConstraint;
typedef shared_ptr<ObstacleConstraint> shared_oc;
typedef NonlinearConstraint1<VectorConfig> NLC1;
typedef boost::shared_ptr<NLC1> shared_NLC1;
typedef NonlinearConstraint2<VectorConfig> NLC2;
typedef boost::shared_ptr<NLC2> shared_NLC2;
typedef NonlinearEquality<VectorConfig,Symbol,Vector> NLE;
typedef boost::shared_ptr<NLE> shared_NLE;
namespace sqp_avoid1 {
// avoidance radius
@ -226,54 +246,52 @@ double radius = 1.0;
// binary avoidance constraint
/** g(x) = ||x2-obs||^2 - radius^2 > 0 */
Vector g_func(const VectorConfig& config, const list<Symbol>& keys) {
Vector delta = config[keys.front()]-config[keys.back()];
double dist2 = sum(emul(delta, delta));
Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) {
double dist2 = config[x].dist(config[obs]);
double thresh = radius*radius;
return Vector_(1, dist2-thresh);
}
/** jacobian at pose */
Matrix jac_g1(const VectorConfig& config, const list<Symbol>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) {
Point2 p = config[x]-config[obs];
return Matrix_(1,2, 2.0*p.x(), 2.0*p.y());
}
/** jacobian at obstacle */
Matrix jac_g2(const VectorConfig& config, const list<Symbol>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = -2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) {
Point2 p = config[x]-config[obs];
return Matrix_(1,2, -2.0*p.x(), -2.0*p.y());
}
}
pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
// fix start, end, obstacle positions
VectorConfig feasible;
Vector feas1 = Vector_(2, 0.0, 0.0), feas2 = Vector_(2, 10.0, 0.0), feas3 =
Vector_(2, 5.0, -0.5);
feasible.insert("x1", feas1);
feasible.insert("x3", feas2);
feasible.insert("o", feas3);
shared_NLE e1(new NLE("x1", feas1, vector_compare));
shared_NLE e2(new NLE("x3", feas2, vector_compare));
shared_NLE e3(new NLE("o", feas3, vector_compare));
pair<NLGraph, Config2D> obstacleAvoidGraph() {
// Keys
PoseKey x1(1), x2(2), x3(3);
PointKey l1(1);
// Constrained Points
Point2 pt_x1,
pt_x3(10.0, 0.0),
pt_l1(5.0, -0.5);
shared_pc e1(new PoseConstraint(x1, pt_x1));
shared_pc e2(new PoseConstraint(x3, pt_x3));
shared_oc e3(new ObstacleConstraint(l1, pt_l1));
// measurement from x1 to x2
Vector x1x2 = Vector_(2, 5.0, 0.0);
shared f1(new simulated2D::Odometry(x1x2, sigma, "x1", "x2"));
Point2 x1x2(5.0, 0.0);
shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2));
// measurement from x2 to x3
Vector x2x3 = Vector_(2, 5.0, 0.0);
shared f2(new simulated2D::Odometry(x2x3, sigma, "x2", "x3"));
Point2 x2x3(5.0, 0.0);
shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3));
// create a binary inequality constraint that forces the middle point away from
// the obstacle
list<Symbol> keys; keys += "x2", "o";
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid1::g_func, _1, keys),
"x2", boost::bind(sqp_avoid1::jac_g1, _1, keys),
"o",boost::bind(sqp_avoid1::jac_g2, _1, keys),
shared_a c1(new AvoidConstraint(boost::bind(sqp_avoid1::g_func, _1, x2, l1),
x2, boost::bind(sqp_avoid1::jac_g1, _1, x2, l1),
l1,boost::bind(sqp_avoid1::jac_g2, _1, x2, l1),
1, "L20", false));
// construct the graph
@ -285,22 +303,29 @@ pair<NLGraph, VectorConfig> obstacleAvoidGraph() {
graph.push_back(f1);
graph.push_back(f2);
return make_pair(graph, feasible);
// make a config of the fixed values, for convenience
Config2D config;
config.insert(x1, pt_x1);
config.insert(x3, pt_x3);
config.insert(l1, pt_l1);
return make_pair(graph, config);
}
/* ********************************************************************* */
TEST ( SQPOptimizer, inequality_avoid ) {
// create the graph
NLGraph graph; VectorConfig feasible;
NLGraph graph; Config2D feasible;
boost::tie(graph, feasible) = obstacleAvoidGraph();
// create the rest of the config
shared_config init(new VectorConfig(feasible));
init->insert("x2", Vector_(2, 5.0, 100.0));
shared_ptr<Config2D> init(new Config2D(feasible));
PoseKey x2(2);
init->insert(x2, Point2(5.0, 100.0));
// create an ordering
Ordering ord;
ord += "x1", "x2", "x3", "o";
ord += "x1", "x2", "x3", "l1";
// create an optimizer
Optimizer optimizer(graph, ord, init);
@ -310,32 +335,33 @@ TEST ( SQPOptimizer, inequality_avoid ) {
// so it will violate the constraint after one iteration
Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT);
VectorConfig exp1(feasible);
exp1.insert("x2", Vector_(2, 5.0, 0.0));
Config2D exp1(feasible);
exp1.insert(x2, Point2(5.0, 0.0));
CHECK(assert_equal(exp1, *(afterOneIteration.config())));
// the second iteration will activate the constraint and force the
// config to a viable configuration.
Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT);
VectorConfig exp2(feasible);
exp2.insert("x2", Vector_(2, 5.0, 0.75));
Config2D exp2(feasible);
exp2.insert(x2, Point2(5.0, 0.5));
CHECK(assert_equal(exp2, *(after2ndIteration.config())));
}
/* ********************************************************************* */
TEST ( SQPOptimizer, inequality_avoid_iterative ) {
// create the graph
NLGraph graph; VectorConfig feasible;
NLGraph graph; Config2D feasible;
boost::tie(graph, feasible) = obstacleAvoidGraph();
// create the rest of the config
shared_config init(new VectorConfig(feasible));
init->insert("x2", Vector_(2, 5.0, 100.0));
shared_ptr<Config2D> init(new Config2D(feasible));
PoseKey x2(2);
init->insert(x2, Point2(5.0, 100.0));
// create an ordering
Ordering ord;
ord += "x1", "x2", "x3", "o";
ord += "x1", "x2", "x3", "l1";
// create an optimizer
Optimizer optimizer(graph, ord, init);
@ -346,106 +372,9 @@ TEST ( SQPOptimizer, inequality_avoid_iterative ) {
Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
// verify
VectorConfig exp2(feasible);
exp2.insert("x2", Vector_(2, 5.0, 0.75));
CHECK(assert_equal(exp2, *(final.config())));
}
/* ********************************************************************* */
// Use boost bind to parameterize the function
namespace sqp_avoid2 {
// binary avoidance constraint
/** g(x) = ||x2-obs||^2 - radius^2 > 0 */
Vector g_func(double radius, const VectorConfig& config, const list<Symbol>& keys) {
Vector delta = config[keys.front()]-config[keys.back()];
double dist2 = sum(emul(delta, delta));
double thresh = radius*radius;
return Vector_(1, dist2-thresh);
}
/** jacobian at pose */
Matrix jac_g1(const VectorConfig& config, const list<Symbol>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = 2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
}
/** jacobian at obstacle */
Matrix jac_g2(const VectorConfig& config, const list<Symbol>& keys) {
Vector x2 = config[keys.front()], obs = config[keys.back()];
Vector grad = -2.0*(x2-obs);
return Matrix_(1,2, grad(0), grad(1));
}
}
pair<NLGraph, VectorConfig> obstacleAvoidGraphGeneral() {
// fix start, end, obstacle positions
VectorConfig feasible;
Vector feas1 = Vector_(2, 0.0, 0.0), feas2 = Vector_(2, 10.0, 0.0), feas3 =
Vector_(2, 5.0, -0.5);
feasible.insert("x1", feas1);
feasible.insert("x3", feas2);
feasible.insert("o", feas3);
shared_NLE e1(new NLE("x1", feas1,vector_compare));
shared_NLE e2(new NLE("x3", feas2, vector_compare));
shared_NLE e3(new NLE("o", feas3, vector_compare));
// measurement from x1 to x2
Vector x1x2 = Vector_(2, 5.0, 0.0);
shared f1(new simulated2D::Odometry(x1x2, sigma, "x1", "x2"));
// measurement from x2 to x3
Vector x2x3 = Vector_(2, 5.0, 0.0);
shared f2(new simulated2D::Odometry(x2x3, sigma, "x2", "x3"));
double radius = 1.0;
// create a binary inequality constraint that forces the middle point away from
// the obstacle
list<Symbol> keys; keys += "x2", "o";
shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, keys),
"x2", boost::bind(sqp_avoid2::jac_g1, _1, keys),
"o", boost::bind(sqp_avoid2::jac_g2, _1, keys),
1, "L20", false));
// construct the graph
NLGraph graph;
graph.push_back(e1);
graph.push_back(e2);
graph.push_back(e3);
graph.push_back(c1);
graph.push_back(f1);
graph.push_back(f2);
return make_pair(graph, feasible);
}
/* ********************************************************************* */
TEST ( SQPOptimizer, inequality_avoid_iterative_bind ) {
// create the graph
NLGraph graph; VectorConfig feasible;
boost::tie(graph, feasible) = obstacleAvoidGraphGeneral();
// create the rest of the config
shared_config init(new VectorConfig(feasible));
init->insert("x2", Vector_(2, 5.0, 100.0));
// create an ordering
Ordering ord;
ord += "x1", "x2", "x3", "o";
// create an optimizer
Optimizer optimizer(graph, ord, init);
double relThresh = 1e-5; // minimum change in error between iterations
double absThresh = 1e-5; // minimum error necessary to converge
double constraintThresh = 1e-9; // minimum constraint error to be feasible
Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
// verify
VectorConfig exp2(feasible);
exp2.insert("x2", Vector_(2, 5.0, 0.75));
CHECK(assert_equal(exp2, *(final.config())));
Config2D exp2(feasible);
exp2.insert(x2, Point2(5.0, 0.5));
CHECK(assert_equal(exp2, *(final.config()))); // FAILS
}
/* ************************************************************************* */

5
cpp/testSimulated2D.cpp
View File

@ -18,7 +18,7 @@ using namespace simulated2D;
/* ************************************************************************* */
TEST( simulated2D, Dprior )
{
Vector x(2);x(0)=1;x(1)=-9;
Point2 x(1,-9);
Matrix numerical = numericalDerivative11(prior,x);
Matrix computed;
prior(x,computed);
@ -28,8 +28,7 @@ TEST( simulated2D, Dprior )
/* ************************************************************************* */
TEST( simulated2D, DOdo )
{
Vector x1(2);x1(0)= 1;x1(1)=-9;
Vector x2(2);x2(0)=-5;x2(1)= 6;
Point2 x1(1,-9),x2(-5,6);
Matrix H1,H2;
odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21(odo,x1,x2);

31
cpp/testSubgraphPreconditioner.cpp
View File

@ -21,6 +21,7 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
TEST( SubgraphPreconditioner, planarGraph )
@ -35,7 +36,7 @@ TEST( SubgraphPreconditioner, planarGraph )
// Check canonical ordering
Ordering expected, ordering = planarOrdering(3);
expected += "x33", "x23", "x13", "x32", "x22", "x12", "x31", "x21", "x11";
expected += "x3003", "x2003", "x1003", "x3002", "x2002", "x1002", "x3001", "x2001", "x1001";
CHECK(assert_equal(expected,ordering));
// Check that xtrue is optimal
@ -117,12 +118,12 @@ TEST( SubgraphPreconditioner, system )
// y1 = perturbed y0
VectorConfig y1 = zeros;
y1.getReference("x23") = Vector_(2, 1.0, -1.0);
y1.getReference("x2003") = Vector_(2, 1.0, -1.0);
// Check corresponding x values
VectorConfig expected_x1 = xtrue, x1 = system.x(y1);
expected_x1.getReference("x23") = Vector_(2, 2.01, 2.99);
expected_x1.getReference("x33") = Vector_(2, 3.01, 2.99);
expected_x1.getReference("x2003") = Vector_(2, 2.01, 2.99);
expected_x1.getReference("x3003") = Vector_(2, 3.01, 2.99);
CHECK(assert_equal(xtrue, system.x(y0)));
CHECK(assert_equal(expected_x1,system.x(y1)));
@ -136,21 +137,21 @@ TEST( SubgraphPreconditioner, system )
VectorConfig expected_gx0 = zeros;
VectorConfig expected_gx1 = zeros;
CHECK(assert_equal(expected_gx0,Ab.gradient(xtrue)));
expected_gx1.getReference("x13") = Vector_(2, -100., 100.);
expected_gx1.getReference("x22") = Vector_(2, -100., 100.);
expected_gx1.getReference("x23") = Vector_(2, 200., -200.);
expected_gx1.getReference("x32") = Vector_(2, -100., 100.);
expected_gx1.getReference("x33") = Vector_(2, 100., -100.);
expected_gx1.getReference("x1003") = Vector_(2, -100., 100.);
expected_gx1.getReference("x2002") = Vector_(2, -100., 100.);
expected_gx1.getReference("x2003") = Vector_(2, 200., -200.);
expected_gx1.getReference("x3002") = Vector_(2, -100., 100.);
expected_gx1.getReference("x3003") = Vector_(2, 100., -100.);
CHECK(assert_equal(expected_gx1,Ab.gradient(x1)));
// Test gradient in y
VectorConfig expected_gy0 = zeros;
VectorConfig expected_gy1 = zeros;
expected_gy1.getReference("x13") = Vector_(2, 2., -2.);
expected_gy1.getReference("x22") = Vector_(2, -2., 2.);
expected_gy1.getReference("x23") = Vector_(2, 3., -3.);
expected_gy1.getReference("x32") = Vector_(2, -1., 1.);
expected_gy1.getReference("x33") = Vector_(2, 1., -1.);
expected_gy1.getReference("x1003") = Vector_(2, 2., -2.);
expected_gy1.getReference("x2002") = Vector_(2, -2., 2.);
expected_gy1.getReference("x2003") = Vector_(2, 3., -3.);
expected_gy1.getReference("x3002") = Vector_(2, -1., 1.);
expected_gy1.getReference("x3003") = Vector_(2, 1., -1.);
CHECK(assert_equal(expected_gy0,system.gradient(y0)));
CHECK(assert_equal(expected_gy1,system.gradient(y1)));
@ -189,7 +190,7 @@ TEST( SubgraphPreconditioner, conjugateGradients )
BOOST_FOREACH(const string& j, ordering) y0.insert(j,z2);
VectorConfig y1 = y0;
y1.getReference("x23") = Vector_(2, 1.0, -1.0);
y1.getReference("x2003") = Vector_(2, 1.0, -1.0);
VectorConfig x1 = system.x(y1);
// Solve for the remaining constraints using PCG

1
cpp/testSymbolicBayesNet.cpp
View File

@ -19,6 +19,7 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
TEST( SymbolicBayesNet, constructor )

1
cpp/testSymbolicFactorGraph.cpp
View File

@ -19,6 +19,7 @@ using namespace boost::assign;
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
TEST( SymbolicFactorGraph, symbolicFactorGraph )

7
cpp/testVectorConfig.cpp
View File

@ -24,6 +24,7 @@
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
TEST( VectorConfig, equals1 )
@ -77,7 +78,7 @@ TEST( VectorConfig, contains)
/* ************************************************************************* */
TEST( VectorConfig, expmap)
{
VectorConfig c = createConfig();
VectorConfig c = createVectorConfig();
Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2
CHECK(assert_equal(expmap(c,c),expmap(c,v)));
}
@ -139,13 +140,13 @@ TEST( VectorConfig, update_with_large_delta) {
/* ************************************************************************* */
TEST( VectorConfig, dot) {
VectorConfig c = createConfig();
VectorConfig c = createVectorConfig();
DOUBLES_EQUAL(3.25,dot(c,c),1e-9);
}
/* ************************************************************************* */
TEST( VectorConfig, dim) {
VectorConfig c = createConfig();
VectorConfig c = createVectorConfig();
LONGS_EQUAL(6,c.dim());
}

1
cpp/timeGaussianFactorGraph.cpp
View File

@ -13,6 +13,7 @@
using namespace std;
using namespace gtsam;
using namespace example;
/* ************************************************************************* */
// Create a Kalman smoother for t=1:T and optimize