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> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>

5
cpp/GaussianFactor.h
View File

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

3
cpp/GaussianISAM2.h
View File

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

13
cpp/Makefile.am
View File

@ -141,18 +141,21 @@ testKey_SOURCES = testKey.cpp
testKey_LDADD = libgtsam.la testKey_LDADD = libgtsam.la
# Nonlinear constraints # Nonlinear constraints
headers += SQPOptimizer.h SQPOptimizer-inl.h
headers += NonlinearConstraint.h NonlinearConstraint-inl.h headers += NonlinearConstraint.h NonlinearConstraint-inl.h
headers += NonlinearEquality.h headers += NonlinearEquality.h
check_PROGRAMS += testNonlinearConstraint testNonlinearEquality testSQP testSQPOptimizer check_PROGRAMS += testNonlinearConstraint testNonlinearEquality
testNonlinearConstraint_SOURCES = testNonlinearConstraint.cpp testNonlinearConstraint_SOURCES = testNonlinearConstraint.cpp
testNonlinearConstraint_LDADD = libgtsam.la testNonlinearConstraint_LDADD = libgtsam.la
testNonlinearEquality_SOURCES = testNonlinearEquality.cpp testNonlinearEquality_SOURCES = testNonlinearEquality.cpp
testNonlinearEquality_LDADD = libgtsam.la 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 testSQP_LDADD = libgtsam.la
testSQPOptimizer_SOURCES = testSQPOptimizer.cpp testSQPOptimizer_SOURCES = testSQPOptimizer.cpp
testSQPOptimizer_LDADD = libgtsam.la testSQPOptimizer_LDADD = libgtsam.la
# geometry # geometry
headers += Lie.h Lie-inl.h headers += Lie.h Lie-inl.h

120
cpp/NonlinearConstraint-inl.h
View File

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

25
cpp/NonlinearConstraint.h
View File

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

14
cpp/simulated2D.cpp
View File

@ -5,20 +5,28 @@
*/ */
#include "simulated2D.h" #include "simulated2D.h"
#include "TupleConfig-inl.h"
namespace gtsam { 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 { namespace simulated2D {
static Matrix I = gtsam::eye(2); 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; if (H) *H = I;
return x; 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) { boost::optional<Matrix&> H2) {
if (H1) *H1 = -I; if (H1) *H1 = -I;
if (H2) *H2 = 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) { boost::optional<Matrix&> H2) {
if (H1) *H1 = -I; if (H1) *H1 = -I;
if (H2) *H2 = I; if (H2) *H2 = I;

152
cpp/simulated2D.h
View File

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

153
cpp/smallExample.cpp
View File

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

255
cpp/smallExample.h
View File

@ -12,156 +12,163 @@
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include "NonlinearFactorGraph.h" #include "NonlinearFactorGraph.h"
#include "simulated2D.h"
namespace gtsam { namespace gtsam {
namespace example {
typedef NonlinearFactorGraph<VectorConfig> ExampleNonlinearFactorGraph; typedef simulated2D::Config Config;
typedef NonlinearFactorGraph<Config> Graph;
/** /**
* Create small example for non-linear factor graph * Create small example for non-linear factor graph
*/ */
boost::shared_ptr<const ExampleNonlinearFactorGraph > sharedNonlinearFactorGraph(); boost::shared_ptr<const Graph> sharedNonlinearFactorGraph();
ExampleNonlinearFactorGraph createNonlinearFactorGraph(); Graph createNonlinearFactorGraph();
/** /**
* Create configuration to go with it * Create configuration to go with it
* The ground truth configuration for the example above * The ground truth configuration for the example above
*/ */
VectorConfig createConfig(); Config createConfig();
/** /** Vector Config equivalent */
* create a noisy configuration for a nonlinear factor graph VectorConfig createVectorConfig();
*/
boost::shared_ptr<const VectorConfig> sharedNoisyConfig();
VectorConfig createNoisyConfig();
/** /**
* Zero delta config * create a noisy configuration for a nonlinear factor graph
*/ */
VectorConfig createZeroDelta(); boost::shared_ptr<const Config> sharedNoisyConfig();
Config createNoisyConfig();
/** /**
* Delta config that, when added to noisyConfig, returns the ground truth * Zero delta config
*/ */
VectorConfig createCorrectDelta(); VectorConfig createZeroDelta();
/** /**
* create a linear factor graph * Delta config that, when added to noisyConfig, returns the ground truth
* The non-linear graph above evaluated at NoisyConfig */
*/ VectorConfig createCorrectDelta();
GaussianFactorGraph createGaussianFactorGraph();
/** /**
* create small Chordal Bayes Net x <- y * create a linear factor graph
*/ * The non-linear graph above evaluated at NoisyConfig
GaussianBayesNet createSmallGaussianBayesNet(); */
GaussianFactorGraph createGaussianFactorGraph();
/** /**
* Create really non-linear factor graph (cos/sin) * create small Chordal Bayes Net x <- y
*/ */
boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph(); GaussianBayesNet createSmallGaussianBayesNet();
ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph();
/** /**
* Create a full nonlinear smoother * Create really non-linear factor graph (cos/sin)
* @param T number of time-steps */
*/ boost::shared_ptr<const Graph>
std::pair<ExampleNonlinearFactorGraph, VectorConfig> createNonlinearSmoother(int T); sharedReallyNonlinearFactorGraph();
Graph createReallyNonlinearFactorGraph();
/** /**
* Create a Kalman smoother by linearizing a non-linear factor graph * Create a full nonlinear smoother
* @param T number of time-steps * @param T number of time-steps
*/ */
GaussianFactorGraph createSmoother(int T); std::pair<Graph, Config> createNonlinearSmoother(int T);
/**
* Create a Kalman smoother by linearizing a non-linear factor graph
* @param T number of time-steps
*/
GaussianFactorGraph createSmoother(int T);
/* ******************************************************* */ /* ******************************************************* */
// Constrained Examples // Constrained Examples
/* ******************************************************* */ /* ******************************************************* */
/** /**
* Creates a simple constrained graph with one linear factor and * Creates a simple constrained graph with one linear factor and
* one binary equality constraint that sets x = y * one binary equality constraint that sets x = y
*/ */
GaussianFactorGraph createSimpleConstraintGraph(); GaussianFactorGraph createSimpleConstraintGraph();
VectorConfig createSimpleConstraintConfig(); VectorConfig createSimpleConstraintConfig();
/** /**
* Creates a simple constrained graph with one linear factor and * Creates a simple constrained graph with one linear factor and
* one binary constraint. * one binary constraint.
*/ */
GaussianFactorGraph createSingleConstraintGraph(); GaussianFactorGraph createSingleConstraintGraph();
VectorConfig createSingleConstraintConfig(); VectorConfig createSingleConstraintConfig();
/** /**
* Creates a constrained graph with a linear factor and two * Creates a constrained graph with a linear factor and two
* binary constraints that share a node * binary constraints that share a node
*/ */
GaussianFactorGraph createMultiConstraintGraph(); GaussianFactorGraph createMultiConstraintGraph();
VectorConfig createMultiConstraintConfig(); VectorConfig createMultiConstraintConfig();
/** /**
* These are the old examples from the EqualityFactor/DeltaFunction * These are the old examples from the EqualityFactor/DeltaFunction
* They should be updated for use at some point, but are disabled for now * They should be updated for use at some point, but are disabled for now
*/ */
/** /**
* Create configuration for constrained example * Create configuration for constrained example
* This is the ground truth version * This is the ground truth version
*/ */
//VectorConfig createConstrainedConfig(); //VectorConfig createConstrainedConfig();
/** /**
* Create a noisy configuration for linearization * Create a noisy configuration for linearization
*/ */
//VectorConfig createConstrainedLinConfig(); //VectorConfig createConstrainedLinConfig();
/** /**
* Create the correct delta configuration * Create the correct delta configuration
*/ */
//VectorConfig createConstrainedCorrectDelta(); //VectorConfig createConstrainedCorrectDelta();
/** /**
* Create small example constrained factor graph * Create small example constrained factor graph
*/ */
//GaussianFactorGraph createConstrainedGaussianFactorGraph(); //GaussianFactorGraph createConstrainedGaussianFactorGraph();
/** /**
* Create small example constrained nonlinear factor graph * Create small example constrained nonlinear factor graph
*/ */
// ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig>,VectorConfig> // ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig>,VectorConfig>
// createConstrainedNonlinearFactorGraph(); // createConstrainedNonlinearFactorGraph();
/* ******************************************************* */ /* ******************************************************* */
// Planar graph with easy subtree for SubgraphPreconditioner // Planar graph with easy subtree for SubgraphPreconditioner
/* ******************************************************* */ /* ******************************************************* */
/* /*
* Create factor graph with N^2 nodes, for example for N=3 * Create factor graph with N^2 nodes, for example for N=3
* x13-x23-x33 * x13-x23-x33
* | | | * | | |
* x12-x22-x32 * x12-x22-x32
* | | | * | | |
* -x11-x21-x31 * -x11-x21-x31
* with x11 clamped at (1,1), and others related by 2D odometry. * with x11 clamped at (1,1), and others related by 2D odometry.
*/ */
std::pair<GaussianFactorGraph, VectorConfig> planarGraph(size_t N); std::pair<GaussianFactorGraph, VectorConfig> planarGraph(size_t N);
/* /*
* Create canonical ordering for planar graph that also works for tree * Create canonical ordering for planar graph that also works for tree
* With x11 the root, e.g. for N=3 * With x11 the root, e.g. for N=3
* x33 x23 x13 x32 x22 x12 x31 x21 x11 * x33 x23 x13 x32 x22 x12 x31 x21 x11
*/ */
Ordering planarOrdering(size_t N); Ordering planarOrdering(size_t N);
/* /*
* Split graph into tree and loop closing constraints, e.g., with N=3 * Split graph into tree and loop closing constraints, e.g., with N=3
* x13-x23-x33 * x13-x23-x33
* | * |
* x12-x22-x32 * x12-x22-x32
* | * |
* -x11-x21-x31 * -x11-x21-x31
*/ */
std::pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(size_t N, std::pair<GaussianFactorGraph, GaussianFactorGraph> splitOffPlanarTree(
const GaussianFactorGraph& original); size_t N, const GaussianFactorGraph& original);
} // example
} // gtsam } // gtsam

3
cpp/testBayesNetPreconditioner.cpp
View File

@ -17,6 +17,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace example;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesNetPreconditioner, operators ) TEST( BayesNetPreconditioner, operators )
@ -88,7 +89,7 @@ TEST( BayesNetPreconditioner, conjugateGradients )
BOOST_FOREACH(const Symbol& j, ordering) y0.insert(j,z2); BOOST_FOREACH(const Symbol& j, ordering) y0.insert(j,z2);
VectorConfig y1 = y0; 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); VectorConfig x1 = system.x(y1);
// Solve using PCG // Solve using PCG

1
cpp/testGaussianBayesNet.cpp
View File

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

1
cpp/testGaussianFactor.cpp
View File

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

1
cpp/testGaussianISAM.cpp
View File

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

1
cpp/testInference.cpp
View File

@ -14,6 +14,7 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace example;
/* ************************************************************************* */ /* ************************************************************************* */
// The tests below test the *generic* inference algorithms. Some of these have // 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 "NonlinearFactorGraph-inl.h"
#include "iterative-inl.h" #include "iterative-inl.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace example;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Iterative, steepestDescent ) TEST( Iterative, steepestDescent )

182
cpp/testNonlinearConstraint.cpp
View File

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

38
cpp/testNonlinearFactor.cpp
View File

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

31
cpp/testNonlinearFactorGraph.cpp
View File

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

41
cpp/testNonlinearOptimizer.cpp
View File

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

290
cpp/testSQP.cpp
View File

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

337
cpp/testSQPOptimizer.cpp
View File

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

5
cpp/testSimulated2D.cpp
View File

@ -18,7 +18,7 @@ using namespace simulated2D;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated2D, Dprior ) TEST( simulated2D, Dprior )
{ {
Vector x(2);x(0)=1;x(1)=-9; Point2 x(1,-9);
Matrix numerical = numericalDerivative11(prior,x); Matrix numerical = numericalDerivative11(prior,x);
Matrix computed; Matrix computed;
prior(x,computed); prior(x,computed);
@ -28,8 +28,7 @@ TEST( simulated2D, Dprior )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( simulated2D, DOdo ) TEST( simulated2D, DOdo )
{ {
Vector x1(2);x1(0)= 1;x1(1)=-9; Point2 x1(1,-9),x2(-5,6);
Vector x2(2);x2(0)=-5;x2(1)= 6;
Matrix H1,H2; Matrix H1,H2;
odo(x1,x2,H1,H2); odo(x1,x2,H1,H2);
Matrix A1 = numericalDerivative21(odo,x1,x2); 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 std;
using namespace gtsam; using namespace gtsam;
using namespace example;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SubgraphPreconditioner, planarGraph ) TEST( SubgraphPreconditioner, planarGraph )
@ -35,7 +36,7 @@ TEST( SubgraphPreconditioner, planarGraph )
// Check canonical ordering // Check canonical ordering
Ordering expected, ordering = planarOrdering(3); 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(assert_equal(expected,ordering));
// Check that xtrue is optimal // Check that xtrue is optimal
@ -117,12 +118,12 @@ TEST( SubgraphPreconditioner, system )
// y1 = perturbed y0 // y1 = perturbed y0
VectorConfig y1 = zeros; 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 // Check corresponding x values
VectorConfig expected_x1 = xtrue, x1 = system.x(y1); VectorConfig expected_x1 = xtrue, x1 = system.x(y1);
expected_x1.getReference("x23") = Vector_(2, 2.01, 2.99); expected_x1.getReference("x2003") = Vector_(2, 2.01, 2.99);
expected_x1.getReference("x33") = Vector_(2, 3.01, 2.99); expected_x1.getReference("x3003") = Vector_(2, 3.01, 2.99);
CHECK(assert_equal(xtrue, system.x(y0))); CHECK(assert_equal(xtrue, system.x(y0)));
CHECK(assert_equal(expected_x1,system.x(y1))); CHECK(assert_equal(expected_x1,system.x(y1)));
@ -136,21 +137,21 @@ TEST( SubgraphPreconditioner, system )
VectorConfig expected_gx0 = zeros; VectorConfig expected_gx0 = zeros;
VectorConfig expected_gx1 = zeros; VectorConfig expected_gx1 = zeros;
CHECK(assert_equal(expected_gx0,Ab.gradient(xtrue))); CHECK(assert_equal(expected_gx0,Ab.gradient(xtrue)));
expected_gx1.getReference("x13") = Vector_(2, -100., 100.); expected_gx1.getReference("x1003") = Vector_(2, -100., 100.);
expected_gx1.getReference("x22") = Vector_(2, -100., 100.); expected_gx1.getReference("x2002") = Vector_(2, -100., 100.);
expected_gx1.getReference("x23") = Vector_(2, 200., -200.); expected_gx1.getReference("x2003") = Vector_(2, 200., -200.);
expected_gx1.getReference("x32") = Vector_(2, -100., 100.); expected_gx1.getReference("x3002") = Vector_(2, -100., 100.);
expected_gx1.getReference("x33") = Vector_(2, 100., -100.); expected_gx1.getReference("x3003") = Vector_(2, 100., -100.);
CHECK(assert_equal(expected_gx1,Ab.gradient(x1))); CHECK(assert_equal(expected_gx1,Ab.gradient(x1)));
// Test gradient in y // Test gradient in y
VectorConfig expected_gy0 = zeros; VectorConfig expected_gy0 = zeros;
VectorConfig expected_gy1 = zeros; VectorConfig expected_gy1 = zeros;
expected_gy1.getReference("x13") = Vector_(2, 2., -2.); expected_gy1.getReference("x1003") = Vector_(2, 2., -2.);
expected_gy1.getReference("x22") = Vector_(2, -2., 2.); expected_gy1.getReference("x2002") = Vector_(2, -2., 2.);
expected_gy1.getReference("x23") = Vector_(2, 3., -3.); expected_gy1.getReference("x2003") = Vector_(2, 3., -3.);
expected_gy1.getReference("x32") = Vector_(2, -1., 1.); expected_gy1.getReference("x3002") = Vector_(2, -1., 1.);
expected_gy1.getReference("x33") = Vector_(2, 1., -1.); expected_gy1.getReference("x3003") = Vector_(2, 1., -1.);
CHECK(assert_equal(expected_gy0,system.gradient(y0))); CHECK(assert_equal(expected_gy0,system.gradient(y0)));
CHECK(assert_equal(expected_gy1,system.gradient(y1))); 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); BOOST_FOREACH(const string& j, ordering) y0.insert(j,z2);
VectorConfig y1 = y0; 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); VectorConfig x1 = system.x(y1);
// Solve for the remaining constraints using PCG // 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 std;
using namespace gtsam; using namespace gtsam;
using namespace example;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SymbolicBayesNet, constructor ) TEST( SymbolicBayesNet, constructor )

1
cpp/testSymbolicFactorGraph.cpp
View File

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

9
cpp/testVectorConfig.cpp
View File

@ -24,7 +24,8 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace example;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( VectorConfig, equals1 ) TEST( VectorConfig, equals1 )
{ {
@ -77,7 +78,7 @@ TEST( VectorConfig, contains)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( VectorConfig, expmap) 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 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))); CHECK(assert_equal(expmap(c,c),expmap(c,v)));
} }
@ -139,13 +140,13 @@ TEST( VectorConfig, update_with_large_delta) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( VectorConfig, dot) { TEST( VectorConfig, dot) {
VectorConfig c = createConfig(); VectorConfig c = createVectorConfig();
DOUBLES_EQUAL(3.25,dot(c,c),1e-9); DOUBLES_EQUAL(3.25,dot(c,c),1e-9);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( VectorConfig, dim) { TEST( VectorConfig, dim) {
VectorConfig c = createConfig(); VectorConfig c = createVectorConfig();
LONGS_EQUAL(6,c.dim()); LONGS_EQUAL(6,c.dim());
} }

1
cpp/timeGaussianFactorGraph.cpp
View File

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