diff --git a/.cproject b/.cproject index 6fb1f3e80..97694da3a 100644 --- a/.cproject +++ b/.cproject @@ -769,6 +769,13 @@ true true + +make +testSimulated2D.run +true +false +true + make -j2 diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 378db8394..1c2483c3c 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -95,7 +95,7 @@ public: As_.insert(terms[i]); } - /** Construct an n-ary factor with multiple sigmas*/ + /** Construct an n-ary factor with a multiple sigmas*/ GaussianFactor(const std::vector > &terms, const Vector &b, const Vector& sigmas) : b_(b), model_(noiseModel::Diagonal::Sigmas(sigmas)) { @@ -139,9 +139,6 @@ public: /** get a copy of sigmas */ const Vector& get_sigmas() const { return model_->sigmas(); } - /** get a copy of sigmas */ - const sharedDiagonal& get_model() const { return model_; } - /** * get a copy of the A matrix from a specific node * O(log n) diff --git a/cpp/GaussianISAM2.h b/cpp/GaussianISAM2.h index 9fff986f5..7f6bc661e 100644 --- a/cpp/GaussianISAM2.h +++ b/cpp/GaussianISAM2.h @@ -11,11 +11,12 @@ #include "ISAM2.h" #include "GaussianConditional.h" #include "GaussianFactor.h" +#include "simulated2D.h" #include "planarSLAM.h" namespace gtsam { - typedef ISAM2 GaussianISAM2; + typedef ISAM2 GaussianISAM2; // recursively optimize this conditional and all subtrees void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result); diff --git a/cpp/Makefile.am b/cpp/Makefile.am index 4ed82e481..4d9084f1e 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -141,18 +141,21 @@ testKey_SOURCES = testKey.cpp testKey_LDADD = libgtsam.la # Nonlinear constraints -headers += SQPOptimizer.h SQPOptimizer-inl.h headers += NonlinearConstraint.h NonlinearConstraint-inl.h headers += NonlinearEquality.h -check_PROGRAMS += testNonlinearConstraint testNonlinearEquality testSQP testSQPOptimizer +check_PROGRAMS += testNonlinearConstraint testNonlinearEquality testNonlinearConstraint_SOURCES = testNonlinearConstraint.cpp testNonlinearConstraint_LDADD = libgtsam.la testNonlinearEquality_SOURCES = testNonlinearEquality.cpp testNonlinearEquality_LDADD = libgtsam.la -testSQP_SOURCES = testSQP.cpp + +# SQP +headers += SQPOptimizer.h SQPOptimizer-inl.h +check_PROGRAMS += testSQP testSQPOptimizer +testSQP_SOURCES = $(example) testSQP.cpp testSQP_LDADD = libgtsam.la -testSQPOptimizer_SOURCES = testSQPOptimizer.cpp -testSQPOptimizer_LDADD = libgtsam.la +testSQPOptimizer_SOURCES = testSQPOptimizer.cpp +testSQPOptimizer_LDADD = libgtsam.la # geometry headers += Lie.h Lie-inl.h diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 14be905dc..bdabdce28 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -22,7 +22,7 @@ NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key size_t dim_lagrange, Vector (*g)(const Config& config), bool isEquality) -: NonlinearFactor(1.0),z_(zero(dim_lagrange)), +: NonlinearFactor(1.0), lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality), g_(boost::bind(g, _1)) {} @@ -33,7 +33,7 @@ NonlinearConstraint::NonlinearConstraint(const std::string& lagrange_key boost::function g, bool isEquality) : NonlinearFactor(noiseModel::Constrained::All(dim_lagrange)), - lagrange_key_(lagrange_key), p_(dim_lagrange),z_(zero(dim_lagrange)), + lagrange_key_(lagrange_key), p_(dim_lagrange), g_(g), isEquality_(isEquality) {} /* ************************************************************************* */ @@ -46,10 +46,10 @@ bool NonlinearConstraint::active(const Config& config) const { // Implementations of unary nonlinear constraints /* ************************************************************************* */ -template -NonlinearConstraint1::NonlinearConstraint1( +template +NonlinearConstraint1::NonlinearConstraint1( Vector (*g)(const Config& config), - const std::string& key, + const Key& key, Matrix (*gradG)(const Config& config), size_t dim_constraint, const std::string& lagrange_key, @@ -59,16 +59,16 @@ NonlinearConstraint1::NonlinearConstraint1( { // set a good lagrange key here // TODO:should do something smart to find a unique one - if (lagrange_key == "") - this->lagrange_key_ = "L_" + key; - this->keys_.push_front(key); +// if (lagrange_key == "") +// this->lagrange_key_ = "L0" +// this->keys_.push_front(key); } /* ************************************************************************* */ -template -NonlinearConstraint1::NonlinearConstraint1( +template +NonlinearConstraint1::NonlinearConstraint1( boost::function g, - const std::string& key, + const Key& key, boost::function gradG, size_t dim_constraint, const std::string& lagrange_key, @@ -78,39 +78,39 @@ NonlinearConstraint1::NonlinearConstraint1( { // set a good lagrange key here // TODO:should do something smart to find a unique one - if (lagrange_key == "") - this->lagrange_key_ = "L_" + key; - this->keys_.push_front(key); +// if (lagrange_key == "") +// this->lagrange_key_ = "L_" + key; +// this->keys_.push_front(key); } /* ************************************************************************* */ -template -void NonlinearConstraint1::print(const std::string& s) const { - std::cout << "NonlinearConstraint1 [" << s << "]:\n" - << " key: " << key_ << "\n" - << " p: " << this->p_ << "\n" - << " lambda key: " << this->lagrange_key_ << "\n"; - if (this->isEquality_) - std::cout << " Equality Factor" << std::endl; - else - std::cout << " Inequality Factor" << std::endl; +template +void NonlinearConstraint1::print(const std::string& s) const { + std::cout << "NonlinearConstraint1 [" << s << "]:\n"; +// << " key: " << key_ << "\n" +// << " p: " << this->p_ << "\n" +// << " lambda key: " << this->lagrange_key_ << "\n"; +// if (this->isEquality_) +// std::cout << " Equality Factor" << std::endl; +// else +// std::cout << " Inequality Factor" << std::endl; } /* ************************************************************************* */ -template -bool NonlinearConstraint1::equals(const Factor& f, double tol) const { - const NonlinearConstraint1* p = dynamic_cast*> (&f); +template +bool NonlinearConstraint1::equals(const Factor& f, double tol) const { + const NonlinearConstraint1* p = dynamic_cast*> (&f); if (p == NULL) return false; - if (key_ != p->key_) return false; + if (!(key_ == p->key_)) return false; if (this->lagrange_key_ != p->lagrange_key_) return false; if (this->isEquality_ != p->isEquality_) return false; return this->p_ == p->p_; } /* ************************************************************************* */ -template +template std::pair -NonlinearConstraint1::linearize(const Config& config, const VectorConfig& lagrange) const { +NonlinearConstraint1::linearize(const Config& config, const VectorConfig& lagrange) const { // extract lagrange multiplier Vector lambda = lagrange[this->lagrange_key_]; @@ -136,12 +136,12 @@ NonlinearConstraint1::linearize(const Config& config, const VectorConfig /* ************************************************************************* */ /* ************************************************************************* */ -template -NonlinearConstraint2::NonlinearConstraint2( +template +NonlinearConstraint2::NonlinearConstraint2( Vector (*g)(const Config& config), - const std::string& key1, + const Key1& key1, Matrix (*G1)(const Config& config), - const std::string& key2, + const Key2& key2, Matrix (*G2)(const Config& config), size_t dim_constraint, const std::string& lagrange_key, @@ -152,19 +152,19 @@ NonlinearConstraint2::NonlinearConstraint2( { // set a good lagrange key here // TODO:should do something smart to find a unique one - if (lagrange_key == "") - this->lagrange_key_ = "L_" + key1 + key2; - this->keys_.push_front(key1); - this->keys_.push_back(key2); +// if (lagrange_key == "") +// this->lagrange_key_ = "L_" + key1 + key2; +// this->keys_.push_front(key1); +// this->keys_.push_back(key2); } /* ************************************************************************* */ -template -NonlinearConstraint2::NonlinearConstraint2( +template +NonlinearConstraint2::NonlinearConstraint2( boost::function g, - const std::string& key1, + const Key1& key1, boost::function G1, - const std::string& key2, + const Key2& key2, boost::function G2, size_t dim_constraint, const std::string& lagrange_key, @@ -175,38 +175,38 @@ NonlinearConstraint2::NonlinearConstraint2( { // set a good lagrange key here // TODO:should do something smart to find a unique one - if (lagrange_key == "") - this->lagrange_key_ = "L_" + key1 + key2; - this->keys_.push_front(key1); - this->keys_.push_back(key2); +// if (lagrange_key == "") +// this->lagrange_key_ = "L_" + key1 + key2; +// this->keys_.push_front(key1); +// this->keys_.push_back(key2); } /* ************************************************************************* */ -template -void NonlinearConstraint2::print(const std::string& s) const { - std::cout << "NonlinearConstraint2 [" << s << "]:\n" - << " key1: " << key1_ << "\n" - << " key2: " << key2_ << "\n" - << " p: " << this->p_ << "\n" - << " lambda key: " << this->lagrange_key_ << std::endl; +template +void NonlinearConstraint2::print(const std::string& s) const { + std::cout << "NonlinearConstraint2 [" << s << "]:\n"; +// << " key1: " << key1_ << "\n" +// << " key2: " << key2_ << "\n" +// << " p: " << this->p_ << "\n" +// << " lambda key: " << this->lagrange_key_ << std::endl; } /* ************************************************************************* */ -template -bool NonlinearConstraint2::equals(const Factor& f, double tol) const { - const NonlinearConstraint2* p = dynamic_cast*> (&f); +template +bool NonlinearConstraint2::equals(const Factor& f, double tol) const { + const NonlinearConstraint2* p = dynamic_cast*> (&f); if (p == NULL) return false; - if (key1_ != p->key1_) return false; - if (key2_ != p->key2_) return false; + if (!(key1_ == p->key1_)) return false; + if (!(key2_ == p->key2_)) return false; if (this->lagrange_key_ != p->lagrange_key_) return false; if (this->isEquality_ != p->isEquality_) return false; return this->p_ == p->p_; } /* ************************************************************************* */ -template +template std::pair NonlinearConstraint2< - Config>::linearize(const Config& config, const VectorConfig& lagrange) const { + Config, Key1, X1, Key2, X2>::linearize(const Config& config, const VectorConfig& lagrange) const { // extract lagrange multiplier Vector lambda = lagrange[this->lagrange_key_]; diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h index 5c4f30188..7b65eb621 100644 --- a/cpp/NonlinearConstraint.h +++ b/cpp/NonlinearConstraint.h @@ -28,9 +28,6 @@ class NonlinearConstraint : public NonlinearFactor { protected: - /** Lagrange multipliers? */ - Vector z_; - /** key for the lagrange multipliers */ std::string lagrange_key_; @@ -121,7 +118,7 @@ public: /** * A unary constraint with arbitrary cost and jacobian functions */ -template +template class NonlinearConstraint1 : public NonlinearConstraint { private: @@ -136,7 +133,7 @@ private: boost::function G_; /** key for the constrained variable */ - std::string key_; + Key key_; public: @@ -151,7 +148,7 @@ public: */ NonlinearConstraint1( Vector (*g)(const Config& config), - const std::string& key, + const Key& key, Matrix (*G)(const Config& config), size_t dim_constraint, const std::string& lagrange_key="", @@ -168,7 +165,7 @@ public: */ NonlinearConstraint1( boost::function g, - const std::string& key, + const Key& key, boost::function G, size_t dim_constraint, const std::string& lagrange_key="", @@ -194,7 +191,7 @@ public: /** * A binary constraint with arbitrary cost and jacobian functions */ -template +template class NonlinearConstraint2 : public NonlinearConstraint { private: @@ -210,8 +207,8 @@ private: boost::function G2_; /** keys for the constrained variables */ - std::string key1_; - std::string key2_; + Key1 key1_; + Key2 key2_; public: @@ -226,9 +223,9 @@ public: */ NonlinearConstraint2( Vector (*g)(const Config& config), - const std::string& key1, + const Key1& key1, Matrix (*G1)(const Config& config), - const std::string& key2, + const Key2& key2, Matrix (*G2)(const Config& config), size_t dim_constraint, const std::string& lagrange_key="", @@ -246,9 +243,9 @@ public: */ NonlinearConstraint2( boost::function g, - const std::string& key1, + const Key1& key1, boost::function G1, - const std::string& key2, + const Key2& key2, boost::function G2, size_t dim_constraint, const std::string& lagrange_key="", diff --git a/cpp/simulated2D.cpp b/cpp/simulated2D.cpp index 94c4036de..2a84a82ac 100644 --- a/cpp/simulated2D.cpp +++ b/cpp/simulated2D.cpp @@ -5,20 +5,28 @@ */ #include "simulated2D.h" +#include "TupleConfig-inl.h" namespace gtsam { + + using namespace simulated2D; +// INSTANTIATE_LIE_CONFIG(PointKey, Point2) + INSTANTIATE_PAIR_CONFIG(PoseKey, Point2, PointKey, Point2) +// INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config) +// INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config) + namespace simulated2D { static Matrix I = gtsam::eye(2); /* ************************************************************************* */ - Vector prior(const Vector& x, boost::optional H) { + Point2 prior(const Point2& x, boost::optional H) { if (H) *H = I; return x; } /* ************************************************************************* */ - Vector odo(const Vector& x1, const Vector& x2, boost::optional H1, + Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1, boost::optional H2) { if (H1) *H1 = -I; if (H2) *H2 = I; @@ -26,7 +34,7 @@ namespace gtsam { } /* ************************************************************************* */ - Vector mea(const Vector& x, const Vector& l, boost::optional H1, + Point2 mea(const Point2& x, const Point2& l, boost::optional H1, boost::optional H2) { if (H1) *H1 = -I; if (H2) *H2 = I; diff --git a/cpp/simulated2D.h b/cpp/simulated2D.h index 57444038d..6c3aadbe3 100644 --- a/cpp/simulated2D.h +++ b/cpp/simulated2D.h @@ -8,9 +8,9 @@ #pragma once -#include "VectorConfig.h" +#include "Point2.h" +#include "TupleConfig.h" #include "NonlinearFactor.h" -#include "Key.h" // \namespace @@ -18,91 +18,95 @@ namespace gtsam { namespace simulated2D { - typedef gtsam::VectorConfig VectorConfig; - typedef gtsam::Symbol PoseKey; - typedef gtsam::Symbol PointKey; + // Simulated2D robots have no orientation, just a position + typedef TypedSymbol PoseKey; + typedef TypedSymbol PointKey; + typedef PairConfig Config; - /** - * Prior on a single pose, and optional derivative version - */ - inline Vector prior(const Vector& x) {return x;} - Vector prior(const Vector& x, boost::optional H = boost::none); - - /** - * odometry between two poses, and optional derivative version - */ - inline Vector odo(const Vector& x1, const Vector& x2) {return x2-x1;} - Vector odo(const Vector& x1, const Vector& x2, boost::optional H1 = - boost::none, boost::optional 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 H1 = - boost::none, boost::optional H2 = boost::none); - - /** - * Unary factor encoding a soft prior on a vector - */ - struct Prior: public NonlinearFactor1 { - - Vector z_; - - Prior(const Vector& z, const sharedGaussian& model, - const PoseKey& key) : - NonlinearFactor1(model, key), - z_(z) { + /** + * Prior on a single pose, and optional derivative version + */ + inline Point2 prior(const Point2& x) { + return x; } + Point2 prior(const Point2& x, boost::optional H = boost::none); - Vector evaluateError(const Vector& x, boost::optional H = - boost::none) const { - return prior(x, H) - z_; + /** + * odometry between two poses, and optional derivative version + */ + inline Point2 odo(const Point2& x1, const Point2& x2) { + return x2 - x1; } + Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none); - }; - - /** - * Binary factor simulating "odometry" between two Vectors - */ - struct Odometry: public NonlinearFactor2 { - Vector z_; - - Odometry(const Vector& z, const sharedGaussian& model, - const PoseKey& j1, const PoseKey& j2) : - z_(z), NonlinearFactor2(model, j1, j2) { + /** + * measurement between landmark and pose, and optional derivative version + */ + inline Point2 mea(const Point2& x, const Point2& l) { + return l - x; } + Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = + boost::none, boost::optional H2 = boost::none); - Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< - Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { - return odo(x1, x2, H1, H2) - z_; - } + /** + * Unary factor encoding a soft prior on a vector + */ + struct Prior: public NonlinearFactor1 { - }; + Point2 z_; - /** - * Binary factor simulating "measurement" between two Vectors - */ - struct Measurement: public NonlinearFactor2 { + Prior(const Point2& z, const sharedGaussian& model, const PoseKey& key) : + NonlinearFactor1 (model, key), z_(z) { + } - Vector z_; + Vector evaluateError(const Point2& x, boost::optional 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(model, j1, j2) { - } + }; - Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional< - Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { - return mea(x1, x2, H1, H2) - z_; - } + /** + * Binary factor simulating "odometry" between two Vectors + */ + struct Odometry: public NonlinearFactor2 { + Point2 z_; - }; + Odometry(const Point2& z, const sharedGaussian& model, const PoseKey& j1, + const PoseKey& j2) : + z_(z), NonlinearFactor2 ( + model, j1, j2) { + } + + Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { + return (odo(x1, x2, H1, H2) - z_).vector(); + } + + }; + + /** + * Binary factor simulating "measurement" between two Vectors + */ + struct Measurement: public NonlinearFactor2 { + + Point2 z_; + + Measurement(const Point2& z, const sharedGaussian& model, + const PoseKey& j1, const PointKey& j2) : + z_(z), NonlinearFactor2 ( + model, j1, j2) { + } + + Vector evaluateError(const Point2& x1, const Point2& x2, boost::optional< + Matrix&> H1 = boost::none, boost::optional H2 = boost::none) const { + return (mea(x1, x2, H1, H2) - z_).vector(); + } + + }; } // namespace simulated2D } // namespace gtsam diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 495204d41..4fce28b6f 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -20,62 +20,65 @@ using namespace std; #include "Matrix.h" #include "NonlinearFactor.h" #include "smallExample.h" -#include "simulated2D.h" // template definitions #include "FactorGraph-inl.h" #include "NonlinearFactorGraph-inl.h" namespace gtsam { +namespace example { - typedef boost::shared_ptr > shared; + typedef boost::shared_ptr > shared; /* ************************************************************************* */ - boost::shared_ptr sharedNonlinearFactorGraph() { + boost::shared_ptr sharedNonlinearFactorGraph() { // Create - boost::shared_ptr nlfg( - new ExampleNonlinearFactorGraph); + boost::shared_ptr nlfg( + new Graph); // prior on x1 double sigma1 = 0.1; - Vector mu = zero(2); - shared f1(new simulated2D::Prior(mu, sigma1, "x1")); + Point2 mu; + shared f1(new simulated2D::Prior(mu, sigma1, 1)); nlfg->push_back(f1); // odometry between x1 and x2 double sigma2 = 0.1; - Vector z2(2); - z2(0) = 1.5; - z2(1) = 0; - shared f2(new simulated2D::Odometry(z2, sigma2, "x1", "x2")); + Point2 z2(1.5, 0); + shared f2(new simulated2D::Odometry(z2, sigma2, 1, 2)); nlfg->push_back(f2); // measurement between x1 and l1 double sigma3 = 0.2; - Vector z3(2); - z3(0) = 0.; - z3(1) = -1.; - shared f3(new simulated2D::Measurement(z3, sigma3, "x1", "l1")); + Point2 z3(0, -1); + shared f3(new simulated2D::Measurement(z3, sigma3, 1, 1)); nlfg->push_back(f3); // measurement between x2 and l1 double sigma4 = 0.2; - Vector z4(2); - z4(0) = -1.5; - z4(1) = -1.; - shared f4(new simulated2D::Measurement(z4, sigma4, "x2", "l1")); + Point2 z4(-1.5, -1.); + shared f4(new simulated2D::Measurement(z4, sigma4, 2, 1)); nlfg->push_back(f4); return nlfg; } /* ************************************************************************* */ - ExampleNonlinearFactorGraph createNonlinearFactorGraph() { + Graph createNonlinearFactorGraph() { return *sharedNonlinearFactorGraph(); } /* ************************************************************************* */ - VectorConfig createConfig() { + Config createConfig() { + Config c; + c.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0)); + c.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0)); + c.insert(simulated2D::PointKey(1), Point2(0.0, -1.0)); + return c; + } + + /* ************************************************************************* */ + VectorConfig createVectorConfig() { VectorConfig c; c.insert("x1", Vector_(2, 0.0, 0.0)); c.insert("x2", Vector_(2, 1.5, 0.0)); @@ -84,16 +87,16 @@ namespace gtsam { } /* ************************************************************************* */ - boost::shared_ptr sharedNoisyConfig() { - boost::shared_ptr c(new VectorConfig); - c->insert("x1", Vector_(2, 0.1, 0.1)); - c->insert("x2", Vector_(2, 1.4, 0.2)); - c->insert("l1", Vector_(2, 0.1, -1.1)); + boost::shared_ptr sharedNoisyConfig() { + boost::shared_ptr c(new Config); + c->insert(simulated2D::PoseKey(1), Point2(0.1, 0.1)); + c->insert(simulated2D::PoseKey(2), Point2(1.4, 0.2)); + c->insert(simulated2D::PointKey(1), Point2(0.1, -1.1)); return c; } /* ************************************************************************* */ - VectorConfig createNoisyConfig() { + Config createNoisyConfig() { return *sharedNoisyConfig(); } @@ -118,14 +121,13 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph createGaussianFactorGraph() { Matrix I = eye(2); - VectorConfig c = createNoisyConfig(); // Create empty graph GaussianFactorGraph fg; // linearized prior on x1: c["x1"]+x1=0 i.e. x1=-c["x1"] double sigma1 = 0.1; - Vector b1 = -c["x1"]; + Vector b1 = -Vector_(2,0.1, 0.1); fg.add("x1", I, b1, sigma1); // odometry between x1 and x2: x2-x1=[0.2;-0.1] @@ -176,30 +178,31 @@ namespace gtsam { /* ************************************************************************* */ namespace smallOptimize { - Vector h(const Vector& v) { - double x = v(0); - return Vector_(2, cos(x), sin(x)); + Point2 h(const Point2& v) { + return Point2(cos(v.x()), sin(v.y())); } - Matrix H(const Vector& v) { - double x = v(0); - return Matrix_(2, 1, -sin(x), cos(x)); + Matrix H(const Point2& v) { + return Matrix_(2, 2, + -sin(v.x()), 0.0, + 0.0, cos(v.y())); } - struct UnaryFactor: public gtsam::NonlinearFactor1 { + struct UnaryFactor: public gtsam::NonlinearFactor1 { - Vector z_; + Point2 z_; - UnaryFactor(const Vector& z, double sigma, const std::string& key) : - gtsam::NonlinearFactor1(sigma, key), - z_(z) { + UnaryFactor(const Point2& z, const sharedGaussian& model, + const simulated2D::PoseKey& key) : + gtsam::NonlinearFactor1(model, key), z_(z) { } - Vector evaluateError(const Vector& x, boost::optional A = + Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { if (A) *A = H(x); - return h(x) - z_; + return (h(x) - z_).vector(); } }; @@ -207,53 +210,50 @@ namespace gtsam { } /* ************************************************************************* */ - boost::shared_ptr sharedReallyNonlinearFactorGraph() { - boost::shared_ptr fg( - new ExampleNonlinearFactorGraph); + boost::shared_ptr sharedReallyNonlinearFactorGraph() { + boost::shared_ptr fg( + new Graph); Vector z = Vector_(2, 1.0, 0.0); double sigma = 0.1; - boost::shared_ptr factor(new smallOptimize::UnaryFactor( - z, sigma, "x")); + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), 1)); fg->push_back(factor); return fg; } - ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() { + Graph createReallyNonlinearFactorGraph() { return *sharedReallyNonlinearFactorGraph(); } /* ************************************************************************* */ - pair createNonlinearSmoother(int T) { + pair createNonlinearSmoother(int T) { // noise on measurements and odometry, respectively double sigma1 = 1, sigma2 = 1; // Create - ExampleNonlinearFactorGraph nlfg; - VectorConfig poses; + Graph nlfg; + Config poses; // prior on x1 - Vector x1 = Vector_(2, 1.0, 0.0); - string key1 = symbol('x', 1); - shared prior(new simulated2D::Prior(x1, sigma1, key1)); + Point2 x1(1.0, 0.0); + shared prior(new simulated2D::Prior(x1, sigma1, 1)); nlfg.push_back(prior); - poses.insert(key1, x1); + poses.insert(simulated2D::PoseKey(1), x1); for (int t = 2; t <= T; t++) { // odometry between x_t and x_{t-1} - Vector odo = Vector_(2, 1.0, 0.0); - string key = symbol('x', t); - shared odometry(new simulated2D::Odometry(odo, sigma2, symbol('x', t - 1), - key)); + Point2 odo(1.0, 0.0); + shared odometry(new simulated2D::Odometry(odo, sigma2, t - 1, t)); nlfg.push_back(odometry); // measurement on x_t is like perfect GPS - Vector xt = Vector_(2, (double) t, 0.0); - shared measurement(new simulated2D::Prior(xt, sigma1, key)); + Point2 xt(t, 0); + shared measurement(new simulated2D::Prior(xt, sigma1, t)); nlfg.push_back(measurement); // initial estimate - poses.insert(key, xt); + poses.insert(simulated2D::PoseKey(t), xt); } return make_pair(nlfg, poses); @@ -261,8 +261,8 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph createSmoother(int T) { - ExampleNonlinearFactorGraph nlfg; - VectorConfig poses; + Graph nlfg; + Config poses; boost::tie(nlfg, poses) = createNonlinearSmoother(T); GaussianFactorGraph lfg = nlfg.linearize(poses); @@ -516,27 +516,25 @@ namespace gtsam { /* ************************************************************************* */ // Create key for simulated planar graph - string key(int x, int y) { - stringstream ss; - ss << "x" << x << y; - return ss.str(); + simulated2D::PoseKey key(int x, int y) { + return simulated2D::PoseKey(1000*x+y); } /* ************************************************************************* */ pair planarGraph(size_t N) { // create empty graph - NonlinearFactorGraph nlfg; + NonlinearFactorGraph nlfg; // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal double sigma0 = 1e-3; - shared constraint(new simulated2D::Prior(Vector_(2, 1.0, 1.0), sigma0, "x11")); + shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sigma0, key(1,1))); nlfg.push_back(constraint); double sigma = 0.01; // Create horizontal constraints, 1...N*(N-1) - Vector z1 = Vector_(2, 1.0, 0.0); // move right + Point2 z1(1.0, 0.0); // move right for (size_t x = 1; x < N; x++) for (size_t y = 1; y <= N; y++) { shared f(new simulated2D::Odometry(z1, sigma, key(x, y), key(x + 1, y))); @@ -544,7 +542,7 @@ namespace gtsam { } // Create vertical constraints, N*(N-1)+1..2*N*(N-1) - Vector z2 = Vector_(2, 0.0, 1.0); // move up + Point2 z2(0.0, 1.0); // move up for (size_t x = 1; x <= N; x++) for (size_t y = 1; y < N; y++) { shared f(new simulated2D::Odometry(z2, sigma, key(x, y), key(x, y + 1))); @@ -552,11 +550,13 @@ namespace gtsam { } // Create linearization and ground xtrue config - VectorConfig zeros, xtrue; + Config zeros; + VectorConfig xtrue; + Point2 zero; for (size_t x = 1; x <= N; x++) for (size_t y = 1; y <= N; y++) { - zeros.add(key(x, y), zero(2)); - xtrue.add(key(x, y), Vector_(2, (double) x, double(y))); + zeros.insert(key(x, y), zero); + xtrue.add((Symbol)key(x, y), Point2(x,y).vector()); } // linearize around zero @@ -601,4 +601,5 @@ namespace gtsam { /* ************************************************************************* */ +} // example } // namespace gtsam diff --git a/cpp/smallExample.h b/cpp/smallExample.h index 77ed68a09..2e7225178 100644 --- a/cpp/smallExample.h +++ b/cpp/smallExample.h @@ -12,156 +12,163 @@ #include #include "NonlinearFactorGraph.h" +#include "simulated2D.h" namespace gtsam { + namespace example { - typedef NonlinearFactorGraph ExampleNonlinearFactorGraph; + typedef simulated2D::Config Config; + typedef NonlinearFactorGraph Graph; - /** - * Create small example for non-linear factor graph - */ - boost::shared_ptr sharedNonlinearFactorGraph(); - ExampleNonlinearFactorGraph createNonlinearFactorGraph(); + /** + * Create small example for non-linear factor graph + */ + boost::shared_ptr sharedNonlinearFactorGraph(); + Graph createNonlinearFactorGraph(); - /** - * Create configuration to go with it - * The ground truth configuration for the example above - */ - VectorConfig createConfig(); + /** + * Create configuration to go with it + * The ground truth configuration for the example above + */ + Config createConfig(); - /** - * create a noisy configuration for a nonlinear factor graph - */ - boost::shared_ptr sharedNoisyConfig(); - VectorConfig createNoisyConfig(); + /** Vector Config equivalent */ + VectorConfig createVectorConfig(); - /** - * Zero delta config - */ - VectorConfig createZeroDelta(); + /** + * create a noisy configuration for a nonlinear factor graph + */ + boost::shared_ptr sharedNoisyConfig(); + Config createNoisyConfig(); - /** - * Delta config that, when added to noisyConfig, returns the ground truth - */ - VectorConfig createCorrectDelta(); + /** + * Zero delta config + */ + VectorConfig createZeroDelta(); - /** - * create a linear factor graph - * The non-linear graph above evaluated at NoisyConfig - */ - GaussianFactorGraph createGaussianFactorGraph(); + /** + * Delta config that, when added to noisyConfig, returns the ground truth + */ + VectorConfig createCorrectDelta(); - /** - * create small Chordal Bayes Net x <- y - */ - GaussianBayesNet createSmallGaussianBayesNet(); + /** + * create a linear factor graph + * The non-linear graph above evaluated at NoisyConfig + */ + GaussianFactorGraph createGaussianFactorGraph(); - /** - * Create really non-linear factor graph (cos/sin) - */ - boost::shared_ptr sharedReallyNonlinearFactorGraph(); - ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph(); + /** + * create small Chordal Bayes Net x <- y + */ + GaussianBayesNet createSmallGaussianBayesNet(); - /** - * Create a full nonlinear smoother - * @param T number of time-steps - */ - std::pair createNonlinearSmoother(int T); + /** + * Create really non-linear factor graph (cos/sin) + */ + boost::shared_ptr + sharedReallyNonlinearFactorGraph(); + Graph createReallyNonlinearFactorGraph(); - /** - * Create a Kalman smoother by linearizing a non-linear factor graph - * @param T number of time-steps - */ - GaussianFactorGraph createSmoother(int T); + /** + * Create a full nonlinear smoother + * @param T number of time-steps + */ + std::pair 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 - * one binary equality constraint that sets x = y - */ - GaussianFactorGraph createSimpleConstraintGraph(); - VectorConfig createSimpleConstraintConfig(); + /** + * Creates a simple constrained graph with one linear factor and + * one binary equality constraint that sets x = y + */ + GaussianFactorGraph createSimpleConstraintGraph(); + VectorConfig createSimpleConstraintConfig(); - /** - * Creates a simple constrained graph with one linear factor and - * one binary constraint. - */ - GaussianFactorGraph createSingleConstraintGraph(); - VectorConfig createSingleConstraintConfig(); + /** + * Creates a simple constrained graph with one linear factor and + * one binary constraint. + */ + GaussianFactorGraph createSingleConstraintGraph(); + VectorConfig createSingleConstraintConfig(); - /** - * Creates a constrained graph with a linear factor and two - * binary constraints that share a node - */ - GaussianFactorGraph createMultiConstraintGraph(); - VectorConfig createMultiConstraintConfig(); + /** + * Creates a constrained graph with a linear factor and two + * binary constraints that share a node + */ + GaussianFactorGraph createMultiConstraintGraph(); + VectorConfig createMultiConstraintConfig(); - /** - * These are the old examples from the EqualityFactor/DeltaFunction - * They should be updated for use at some point, but are disabled for now - */ - /** - * Create configuration for constrained example - * This is the ground truth version - */ - //VectorConfig createConstrainedConfig(); + /** + * These are the old examples from the EqualityFactor/DeltaFunction + * They should be updated for use at some point, but are disabled for now + */ + /** + * Create configuration for constrained example + * This is the ground truth version + */ + //VectorConfig createConstrainedConfig(); - /** - * Create a noisy configuration for linearization - */ - //VectorConfig createConstrainedLinConfig(); + /** + * Create a noisy configuration for linearization + */ + //VectorConfig createConstrainedLinConfig(); - /** - * Create the correct delta configuration - */ - //VectorConfig createConstrainedCorrectDelta(); + /** + * Create the correct delta configuration + */ + //VectorConfig createConstrainedCorrectDelta(); - /** - * Create small example constrained factor graph - */ - //GaussianFactorGraph createConstrainedGaussianFactorGraph(); + /** + * Create small example constrained factor graph + */ + //GaussianFactorGraph createConstrainedGaussianFactorGraph(); - /** - * Create small example constrained nonlinear factor graph - */ -// ConstrainedNonlinearFactorGraph,VectorConfig> -// createConstrainedNonlinearFactorGraph(); + /** + * Create small example constrained nonlinear factor graph + */ + // ConstrainedNonlinearFactorGraph,VectorConfig> + // 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 - * x13-x23-x33 - * | | | - * x12-x22-x32 - * | | | - * -x11-x21-x31 - * with x11 clamped at (1,1), and others related by 2D odometry. - */ - std::pair planarGraph(size_t N); + /* + * Create factor graph with N^2 nodes, for example for N=3 + * x13-x23-x33 + * | | | + * x12-x22-x32 + * | | | + * -x11-x21-x31 + * with x11 clamped at (1,1), and others related by 2D odometry. + */ + std::pair planarGraph(size_t N); - /* - * Create canonical ordering for planar graph that also works for tree - * With x11 the root, e.g. for N=3 - * x33 x23 x13 x32 x22 x12 x31 x21 x11 - */ - Ordering planarOrdering(size_t N); + /* + * Create canonical ordering for planar graph that also works for tree + * With x11 the root, e.g. for N=3 + * x33 x23 x13 x32 x22 x12 x31 x21 x11 + */ + Ordering planarOrdering(size_t N); - /* - * Split graph into tree and loop closing constraints, e.g., with N=3 - * x13-x23-x33 - * | - * x12-x22-x32 - * | - * -x11-x21-x31 - */ - std::pair splitOffPlanarTree(size_t N, - const GaussianFactorGraph& original); + /* + * Split graph into tree and loop closing constraints, e.g., with N=3 + * x13-x23-x33 + * | + * x12-x22-x32 + * | + * -x11-x21-x31 + */ + std::pair splitOffPlanarTree( + size_t N, const GaussianFactorGraph& original); + } // example } // gtsam diff --git a/cpp/testBayesNetPreconditioner.cpp b/cpp/testBayesNetPreconditioner.cpp index f2918a89a..aa7a3c1db 100644 --- a/cpp/testBayesNetPreconditioner.cpp +++ b/cpp/testBayesNetPreconditioner.cpp @@ -17,6 +17,7 @@ using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ TEST( BayesNetPreconditioner, operators ) @@ -88,7 +89,7 @@ TEST( BayesNetPreconditioner, conjugateGradients ) BOOST_FOREACH(const Symbol& j, ordering) y0.insert(j,z2); VectorConfig y1 = y0; - y1.getReference("x23") = Vector_(2, 1.0, -1.0); + y1.getReference("x2003") = Vector_(2, 1.0, -1.0); VectorConfig x1 = system.x(y1); // Solve using PCG diff --git a/cpp/testGaussianBayesNet.cpp b/cpp/testGaussianBayesNet.cpp index dca43d8c1..d4aacf154 100644 --- a/cpp/testGaussianBayesNet.cpp +++ b/cpp/testGaussianBayesNet.cpp @@ -28,6 +28,7 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ TEST( GaussianBayesNet, constructor ) diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index 773ddf976..acded2ba7 100644 --- a/cpp/testGaussianFactor.cpp +++ b/cpp/testGaussianFactor.cpp @@ -24,6 +24,7 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ TEST( GaussianFactor, linearFactor ) diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 99726cddb..0e85c7b36 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -26,6 +26,7 @@ using namespace boost::assign; #include "inference-inl.h" // needed for eliminate and marginals using namespace gtsam; +using namespace example; double tol=1e-4; @@ -581,13 +582,13 @@ TEST( GaussianFactorGraph, gradient ) TEST( GaussianFactorGraph, multiplication ) { GaussianFactorGraph A = createGaussianFactorGraph(); - VectorConfig x = createConfig(); + VectorConfig x = createCorrectDelta(); Errors actual = A * x; Errors expected; - expected += Vector_(2, 0.0, 0.0); - expected += Vector_(2,15.0, 0.0); - expected += Vector_(2, 0.0,-5.0); - expected += Vector_(2,-7.5,-5.0); + expected += Vector_(2,-1.0,-1.0); + expected += Vector_(2, 2.0,-1.0); + expected += Vector_(2, 0.0, 1.0); + expected += Vector_(2,-1.0, 1.5); CHECK(assert_equal(expected,actual)); } diff --git a/cpp/testGaussianISAM.cpp b/cpp/testGaussianISAM.cpp index 1573b7efb..e22f96063 100644 --- a/cpp/testGaussianISAM.cpp +++ b/cpp/testGaussianISAM.cpp @@ -20,6 +20,7 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests diff --git a/cpp/testGaussianISAM2.cpp b/cpp/testGaussianISAM2.cpp index e239765fc..0639f0ca0 100644 --- a/cpp/testGaussianISAM2.cpp +++ b/cpp/testGaussianISAM2.cpp @@ -20,12 +20,13 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ TEST( ISAM2, solving ) { - ExampleNonlinearFactorGraph nlfg = createNonlinearFactorGraph(); - VectorConfig noisy = createNoisyConfig(); + Graph nlfg = createNonlinearFactorGraph(); + Config noisy = createNoisyConfig(); Ordering ordering; ordering += symbol('x', 1); ordering += symbol('x', 2); @@ -34,8 +35,8 @@ TEST( ISAM2, solving ) VectorConfig actualDelta = optimize2(btree); VectorConfig delta = createCorrectDelta(); CHECK(assert_equal(delta, actualDelta)); - VectorConfig actualSolution = noisy+actualDelta; - VectorConfig solution = createConfig(); + Config actualSolution = noisy.expmap(actualDelta); + Config solution = createConfig(); CHECK(assert_equal(solution, actualSolution)); } @@ -43,14 +44,14 @@ TEST( ISAM2, solving ) TEST( ISAM2, ISAM2_smoother ) { // Create smoother with 7 nodes - ExampleNonlinearFactorGraph smoother; - VectorConfig poses; + Graph smoother; + Config poses; boost::tie(smoother, poses) = createNonlinearSmoother(7); // run ISAM2 for every factor GaussianISAM2 actual; - BOOST_FOREACH(boost::shared_ptr > factor, smoother) { - ExampleNonlinearFactorGraph factorGraph; + BOOST_FOREACH(boost::shared_ptr > factor, smoother) { + Graph factorGraph; factorGraph.push_back(factor); actual.update(factorGraph, poses); } @@ -76,18 +77,18 @@ TEST( ISAM2, ISAM2_smoother ) TEST( ISAM2, ISAM2_smoother2 ) { // Create smoother with 7 nodes - ExampleNonlinearFactorGraph smoother; - VectorConfig poses; + Graph smoother; + Config poses; boost::tie(smoother, poses) = createNonlinearSmoother(7); // Create initial tree from first 4 timestamps in reverse order ! Ordering ord; ord += "x4","x3","x2","x1"; - ExampleNonlinearFactorGraph factors1; + Graph factors1; for (int i=0;i<7;i++) factors1.push_back(smoother[i]); GaussianISAM2 actual(factors1, ord, poses); // run ISAM2 with remaining factors - ExampleNonlinearFactorGraph factors2; + Graph factors2; for (int i=7;i<13;i++) factors2.push_back(smoother[i]); actual.update(factors2, poses); diff --git a/cpp/testInference.cpp b/cpp/testInference.cpp index 58ab84668..c4b259b72 100644 --- a/cpp/testInference.cpp +++ b/cpp/testInference.cpp @@ -14,6 +14,7 @@ using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ // The tests below test the *generic* inference algorithms. Some of these have diff --git a/cpp/testIterative.cpp b/cpp/testIterative.cpp index 37569b47f..9e0400c99 100644 --- a/cpp/testIterative.cpp +++ b/cpp/testIterative.cpp @@ -22,9 +22,9 @@ using namespace boost::assign; #include "NonlinearFactorGraph-inl.h" #include "iterative-inl.h" - using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ TEST( Iterative, steepestDescent ) diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index e5e80d4b6..65bf1610b 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -19,6 +19,11 @@ using namespace std; using namespace gtsam; using namespace boost::assign; +typedef TypedSymbol Key; +typedef NonlinearConstraint1 NLC1; +typedef NonlinearConstraint2 NLC2; + + /* ************************************************************************* */ // unary functions with scalar variables /* ************************************************************************* */ @@ -44,14 +49,15 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) { // the lagrange multipliers will be expected on L_x1 // and there is only one multiplier size_t p = 1; - list keys; keys += "x"; - NonlinearConstraint1 c1(boost::bind(test1::g, _1, keys), - "x", boost::bind(test1::G, _1, keys), - p, "L1"); + list keys; keys += "x1"; + Key x1(1); + NLC1 c1(boost::bind(test1::g, _1, keys), + x1, boost::bind(test1::G, _1, keys), + p, "L1"); // get a configuration to use for finding the error VectorConfig config; - config.insert("x", Vector_(1, 1.0)); + config.insert("x1", Vector_(1, 1.0)); // calculate the error Vector actual = c1.unwhitenedError(config); @@ -62,14 +68,15 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_scalar_linearize ) { size_t p = 1; - list keys; keys += "x"; - NonlinearConstraint1 c1(boost::bind(test1::g, _1, keys), - "x", boost::bind(test1::G, _1, keys), - p, "L1"); + list keys; keys += "x1"; + Key x1(1); + NLC1 c1(boost::bind(test1::g, _1, keys), + x1, boost::bind(test1::G, _1, keys), + p, "L1"); // get a configuration to use for linearization VectorConfig realconfig; - realconfig.insert("x", Vector_(1, 1.0)); + realconfig.insert(x1, Vector_(1, 1.0)); // get a configuration of Lagrange multipliers VectorConfig lagrangeConfig; @@ -80,20 +87,21 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) { boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig); // verify - GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0); - GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0); + GaussianFactor expectedFactor(x1, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0); + GaussianFactor expectedConstraint(x1, Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0); CHECK(assert_equal(*actualFactor, expectedFactor)); CHECK(assert_equal(*actualConstraint, expectedConstraint)); } /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_scalar_equal ) { - list keys1, keys2; keys1 += "x"; keys2 += "y"; - NonlinearConstraint1 - c1(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1", true), - c2(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 1, "L_x1"), - c3(boost::bind(test1::g, _1, keys1), "x", boost::bind(test1::G, _1, keys1), 2, "L_x1"), - c4(boost::bind(test1::g, _1, keys2), "y", boost::bind(test1::G, _1, keys2), 1, "L_x1"); + list keys1, keys2; keys1 += "x0"; keys2 += "x1"; + Key x(0), y(1); + NLC1 + c1(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, "L_x1", true), + c2(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 1, "L_x1"), + c3(boost::bind(test1::g, _1, keys1), x, boost::bind(test1::G, _1, keys1), 2, "L_x1"), + c4(boost::bind(test1::g, _1, keys2), y, boost::bind(test1::G, _1, keys2), 1, "L_x1"); CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c2, c1)); @@ -133,17 +141,18 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) { // the lagrange multipliers will be expected on L_xy // and there is only one multiplier size_t p = 1; - list keys; keys += "x", "y"; - NonlinearConstraint2 c1( + list keys; keys += "x0", "x1"; + Key x0(0), x1(1); + NLC2 c1( boost::bind(test2::g, _1, keys), - "x", boost::bind(test2::G1, _1, keys), - "y", boost::bind(test2::G1, _1, keys), + x0, boost::bind(test2::G1, _1, keys), + x1, boost::bind(test2::G1, _1, keys), p, "L12"); // get a configuration to use for finding the error VectorConfig config; - config.insert("x", Vector_(1, 1.0)); - config.insert("y", Vector_(1, 2.0)); + config.insert("x0", Vector_(1, 1.0)); + config.insert("x1", Vector_(1, 2.0)); // calculate the error Vector actual = c1.unwhitenedError(config); @@ -155,17 +164,18 @@ TEST( NonlinearConstraint2, binary_scalar_construction ) { TEST( NonlinearConstraint2, binary_scalar_linearize ) { // create a constraint size_t p = 1; - list keys; keys += "x", "y"; - NonlinearConstraint2 c1( + list keys; keys += "x0", "x1"; + Key x0(0), x1(1); + NLC2 c1( boost::bind(test2::g, _1, keys), - "x", boost::bind(test2::G1, _1, keys), - "y", boost::bind(test2::G2, _1, keys), + x0, boost::bind(test2::G1, _1, keys), + x1, boost::bind(test2::G2, _1, keys), p, "L12"); // get a configuration to use for finding the error VectorConfig realconfig; - realconfig.insert("x", Vector_(1, 1.0)); - realconfig.insert("y", Vector_(1, 2.0)); + realconfig.insert(x0, Vector_(1, 1.0)); + realconfig.insert(x1, Vector_(1, 2.0)); // get a configuration of Lagrange multipliers VectorConfig lagrangeConfig; @@ -176,25 +186,26 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) { boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig); // verify - GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), - "y", Matrix_(1,1, -3.0), + GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), + x1, Matrix_(1,1, -3.0), "L12", eye(1), zero(1), 1.0); - GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), - "y", Matrix_(1,1, -1.0), + GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), + x1, Matrix_(1,1, -1.0), Vector_(1, 6.0), 0.0); CHECK(assert_equal(*actualFactor, expectedFactor)); - CHECK(assert_equal(*actualConstraint, expectedConstraint)); + CHECK(assert_equal(*actualConstraint, expectedConstraint)); //FAILS - wrong b value } /* ************************************************************************* */ TEST( NonlinearConstraint2, binary_scalar_equal ) { list keys1, keys2, keys3; - keys1 += "x", "y"; keys2 += "y", "x"; keys3 += "x", "z"; - NonlinearConstraint2 - c1(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 1, "L_xy"), - c2(boost::bind(test2::g, _1, keys1), "x", boost::bind(test2::G1, _1, keys1), "y", boost::bind(test2::G2, _1, keys1), 1, "L_xy"), - c3(boost::bind(test2::g, _1, keys2), "y", boost::bind(test2::G1, _1, keys2), "x", boost::bind(test2::G2, _1, keys2), 1, "L_xy"), - c4(boost::bind(test2::g, _1, keys3), "x", boost::bind(test2::G1, _1, keys3), "z", boost::bind(test2::G2, _1, keys3), 3, "L_xy"); + keys1 += "x0", "x1"; keys2 += "x1", "x0"; keys3 += "x0", "z"; + Key x0(0), x1(1), x2(2); + NLC2 + c1(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, "L_xy"), + c2(boost::bind(test2::g, _1, keys1), x0, boost::bind(test2::G1, _1, keys1), x1, boost::bind(test2::G2, _1, keys1), 1, "L_xy"), + c3(boost::bind(test2::g, _1, keys2), x1, boost::bind(test2::G1, _1, keys2), x0, boost::bind(test2::G2, _1, keys2), 1, "L_xy"), + c4(boost::bind(test2::g, _1, keys3), x0, boost::bind(test2::G1, _1, keys3), x2, boost::bind(test2::G2, _1, keys3), 3, "L_xy"); CHECK(assert_equal(c1, c2)); CHECK(assert_equal(c2, c1)); @@ -208,15 +219,15 @@ TEST( NonlinearConstraint2, binary_scalar_equal ) { namespace inequality1 { /** p = 1, g(x) x^2 - 5 > 0 */ - Vector g(const VectorConfig& config, const list& keys) { - double x = config[keys.front()](0); + Vector g(const VectorConfig& config, const Key& key) { + double x = config[key](0); double g = x * x - 5; return Vector_(1, g); // return the actual cost } /** p = 1, jacobianG(x) = 2*x */ - Matrix G(const VectorConfig& config, const list& keys) { - double x = config[keys.front()](0); + Matrix G(const VectorConfig& config, const Key& key) { + double x = config[key](0); return Matrix_(1, 1, 2 * x); } @@ -225,16 +236,16 @@ namespace inequality1 { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_inequality ) { size_t p = 1; - list keys; keys += "x"; - NonlinearConstraint1 c1(boost::bind(inequality1::g, _1, keys), - "x", boost::bind(inequality1::G, _1, keys), - p, "L1", - false); // inequality constraint + Key x0(0); + NLC1 c1(boost::bind(inequality1::g, _1, x0), + x0, boost::bind(inequality1::G, _1, x0), + p, "L1", + false); // inequality constraint // get configurations to use for evaluation VectorConfig config1, config2; - config1.insert("x", Vector_(1, 10.0)); // should be inactive - config2.insert("x", Vector_(1, 1.0)); // should have nonzero error + config1.insert(x0, Vector_(1, 10.0)); // should be inactive + config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error // check error CHECK(!c1.active(config1)); @@ -246,16 +257,16 @@ TEST( NonlinearConstraint1, unary_inequality ) { /* ************************************************************************* */ TEST( NonlinearConstraint1, unary_inequality_linearize ) { size_t p = 1; - list keys; keys += "x"; - NonlinearConstraint1 c1(boost::bind(inequality1::g, _1, keys), - "x", boost::bind(inequality1::G, _1, keys), - p, "L1", - false); // inequality constraint + Key x0(0); + NLC1 c1(boost::bind(inequality1::g, _1, x0), + x0, boost::bind(inequality1::G, _1, x0), + p, "L1", + false); // inequality constraint // get configurations to use for linearization VectorConfig config1, config2; - config1.insert("x", Vector_(1, 10.0)); // should have zero error - config2.insert("x", Vector_(1, 1.0)); // should have nonzero error + config1.insert(x0, Vector_(1, 10.0)); // should have zero error + config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error // get a configuration of Lagrange multipliers VectorConfig lagrangeConfig; @@ -274,8 +285,8 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) { CHECK(c1.active(config2)); // verify - GaussianFactor expectedFactor("x", Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0); - GaussianFactor expectedConstraint("x", Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0); + GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), 1.0); + GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), 0.0); CHECK(assert_equal(*actualFactor2, expectedFactor)); CHECK(assert_equal(*actualConstraint2, expectedConstraint)); } @@ -286,16 +297,16 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) { namespace binding1 { /** p = 1, g(x) x^2 - r > 0 */ - Vector g(double r, const VectorConfig& config, const list& keys) { - double x = config[keys.front()](0); + Vector g(double r, const VectorConfig& config, const Key& key) { + double x = config[key](0); double g = x * x - r; return Vector_(1, g); // return the actual cost } /** p = 1, jacobianG(x) = 2*x */ Matrix G(double coeff, const VectorConfig& config, - const list& keys) { - double x = config[keys.front()](0); + const Key& key) { + double x = config[key](0); return Matrix_(1, 1, coeff * x); } @@ -306,17 +317,16 @@ TEST( NonlinearConstraint1, unary_binding ) { size_t p = 1; double coeff = 2; double radius = 5; - list keys; keys += "x"; - NonlinearConstraint1 c1( - boost::bind(binding1::g, radius, _1, keys), - "x", boost::bind(binding1::G, coeff, _1, keys), - p, "L1", - false); // inequality constraint + Key x0(0); + NLC1 c1(boost::bind(binding1::g, radius, _1, x0), + x0, boost::bind(binding1::G, coeff, _1, x0), + p, "L1", + false); // inequality constraint // get configurations to use for evaluation VectorConfig config1, config2; - config1.insert("x", Vector_(1, 10.0)); // should have zero error - config2.insert("x", Vector_(1, 1.0)); // should have nonzero error + config1.insert(x0, Vector_(1, 10.0)); // should have zero error + config2.insert(x0, Vector_(1, 1.0)); // should have nonzero error // check error CHECK(!c1.active(config1)); @@ -329,21 +339,20 @@ TEST( NonlinearConstraint1, unary_binding ) { namespace binding2 { /** p = 1, g(x) = x^2-5 -y = 0 */ - Vector g(double r, const VectorConfig& config, const list& keys) { - double x = config[keys.front()](0); - double y = config[keys.back()](0); + Vector g(double r, const VectorConfig& config, const Key& k1, const Key& k2) { + double x = config[k1](0); + double y = config[k2](0); return Vector_(1, x * x - r - y); } /** jacobian for x, jacobianG(x,y) in x: 2x*/ - Matrix G1(double c, const VectorConfig& config, const list& keys) { - double x = config[keys.front()](0); + Matrix G1(double c, const VectorConfig& config, const Key& key) { + double x = config[key](0); return Matrix_(1, 1, c * x); } /** jacobian for y, jacobianG(x,y) in y: -1 */ - Matrix G2(double c, const VectorConfig& config, const list& keys) { - double x = config[keys.back()](0); + Matrix G2(double c, const VectorConfig& config) { return Matrix_(1, 1, -1.0 * c); } @@ -358,17 +367,16 @@ TEST( NonlinearConstraint2, binary_binding ) { double a = 2.0; double b = 1.0; double r = 5.0; - list keys; keys += "x", "y"; - NonlinearConstraint2 c1( - boost::bind(binding2::g, r, _1, keys), - "x", boost::bind(binding2::G1, a, _1, keys), - "y", boost::bind(binding2::G2, b, _1, keys), + Key x0(0), x1(1); + NLC2 c1(boost::bind(binding2::g, r, _1, x0, x1), + x0, boost::bind(binding2::G1, a, _1, x0), + x1, boost::bind(binding2::G2, b, _1), p, "L1"); // get a configuration to use for finding the error VectorConfig config; - config.insert("x", Vector_(1, 1.0)); - config.insert("y", Vector_(1, 2.0)); + config.insert(x0, Vector_(1, 1.0)); + config.insert(x1, Vector_(1, 2.0)); // calculate the error Vector actual = c1.unwhitenedError(config); diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp index c8225c702..4d20afe65 100644 --- a/cpp/testNonlinearFactor.cpp +++ b/cpp/testNonlinearFactor.cpp @@ -22,21 +22,22 @@ using namespace std; using namespace gtsam; +using namespace example; typedef boost::shared_ptr > shared_nlf; /* ************************************************************************* */ TEST( NonlinearFactor, equals ) { - double sigma = 1.0; + sharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0)); // create two nonlinear2 factors - Vector z3 = Vector_(2,0.,-1.); - simulated2D::Measurement f0(z3, sigma, "x1", "l1"); + Point2 z3(0.,-1.); + simulated2D::Measurement f0(z3, sigma, 1,1); // measurement between x2 and l1 - Vector z4 = Vector_(2,-1.5, -1.); - simulated2D::Measurement f1(z4, sigma, "x2", "l1"); + Point2 z4(-1.5, -1.); + simulated2D::Measurement f1(z4, sigma, 2,1); CHECK(assert_equal(f0,f0)); CHECK(f0.equals(f0)); @@ -48,10 +49,10 @@ TEST( NonlinearFactor, equals ) TEST( NonlinearFactor, equals2 ) { // create a non linear factor graph - ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); + Graph fg = createNonlinearFactorGraph(); // get two factors - shared_nlf f0 = fg[0], f1 = fg[1]; + Graph::sharedFactor f0 = fg[0], f1 = fg[1]; CHECK(f0->equals(*f0)); CHECK(!f0->equals(*f1)); @@ -62,13 +63,13 @@ TEST( NonlinearFactor, equals2 ) TEST( NonlinearFactor, NonlinearFactor ) { // create a non linear factor graph - ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); + Graph fg = createNonlinearFactorGraph(); // create a configuration for the non linear factor graph - VectorConfig cfg = createNoisyConfig(); + Config cfg = createNoisyConfig(); // get the factor "f1" from the factor graph - shared_nlf factor = fg[0]; + Graph::sharedFactor factor = fg[0]; // calculate the error_vector from the factor "f1" // the expected value for the whitened error from the factor @@ -88,7 +89,7 @@ TEST( NonlinearFactor, NonlinearFactor ) TEST( NonlinearFactor, linearize_f1 ) { // Grab a non-linear factor - ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); + Graph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[0]); @@ -110,7 +111,7 @@ TEST( NonlinearFactor, linearize_f1 ) TEST( NonlinearFactor, linearize_f2 ) { // Grab a non-linear factor - ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); + Graph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[1]); @@ -128,7 +129,7 @@ TEST( NonlinearFactor, linearize_f2 ) TEST( NonlinearFactor, linearize_f3 ) { // Grab a non-linear factor - ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); + Graph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[2]); @@ -146,7 +147,7 @@ TEST( NonlinearFactor, linearize_f3 ) TEST( NonlinearFactor, linearize_f4 ) { // Grab a non-linear factor - ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph(); + Graph nfg = createNonlinearFactorGraph(); boost::shared_ptr nlf = boost::static_pointer_cast(nfg[3]); @@ -164,15 +165,14 @@ TEST( NonlinearFactor, linearize_f4 ) TEST( NonlinearFactor, size ) { // create a non linear factor graph - ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); + Graph fg = createNonlinearFactorGraph(); // create a configuration for the non linear factor graph - VectorConfig cfg = createNoisyConfig(); + Config cfg = createNoisyConfig(); // get some factors from the graph - shared_nlf factor1 = fg[0]; - shared_nlf factor2 = fg[1]; - shared_nlf factor3 = fg[2]; + Graph::sharedFactor factor1 = fg[0], factor2 = fg[1], + factor3 = fg[2]; CHECK(factor1->size() == 1); CHECK(factor2->size() == 2); diff --git a/cpp/testNonlinearFactorGraph.cpp b/cpp/testNonlinearFactorGraph.cpp index 83ace8b69..bc8c12588 100644 --- a/cpp/testNonlinearFactorGraph.cpp +++ b/cpp/testNonlinearFactorGraph.cpp @@ -23,43 +23,44 @@ using namespace boost::assign; #include "NonlinearFactorGraph-inl.h" using namespace gtsam; +using namespace example; /* ************************************************************************* */ -TEST( ExampleNonlinearFactorGraph, equals ) +TEST( Graph, equals ) { - ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); - ExampleNonlinearFactorGraph fg2 = createNonlinearFactorGraph(); + Graph fg = createNonlinearFactorGraph(); + Graph fg2 = createNonlinearFactorGraph(); CHECK( fg.equals(fg2) ); } /* ************************************************************************* */ -TEST( ExampleNonlinearFactorGraph, error ) +TEST( Graph, error ) { - ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); - VectorConfig c1 = createConfig(); + Graph fg = createNonlinearFactorGraph(); + Config c1 = createConfig(); double actual1 = fg.error(c1); DOUBLES_EQUAL( 0.0, actual1, 1e-9 ); - VectorConfig c2 = createNoisyConfig(); + Config c2 = createNoisyConfig(); double actual2 = fg.error(c2); DOUBLES_EQUAL( 5.625, actual2, 1e-9 ); } /* ************************************************************************* */ -TEST( ExampleNonlinearFactorGraph, GET_ORDERING) +TEST( Graph, GET_ORDERING) { Ordering expected; expected += "l1","x1","x2"; - ExampleNonlinearFactorGraph nlfg = createNonlinearFactorGraph(); + Graph nlfg = createNonlinearFactorGraph(); Ordering actual = nlfg.getOrdering(); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( ExampleNonlinearFactorGraph, probPrime ) +TEST( Graph, probPrime ) { - ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); - VectorConfig cfg = createConfig(); + Graph fg = createNonlinearFactorGraph(); + Config cfg = createConfig(); // evaluate the probability of the factor graph double actual = fg.probPrime(cfg); @@ -69,10 +70,10 @@ TEST( ExampleNonlinearFactorGraph, probPrime ) /* ************************************************************************* * // TODO: Commented out until noise model is passed to Gaussian factor graph -TEST( ExampleNonlinearFactorGraph, linearize ) +TEST( Graph, linearize ) { - ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph(); - VectorConfig initial = createNoisyConfig(); + Graph fg = createNonlinearFactorGraph(); + Config initial = createNoisyConfig(); GaussianFactorGraph linearized = fg.linearize(initial); GaussianFactorGraph expected = createGaussianFactorGraph(); CHECK(assert_equal(expected,linearized)); diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index b8303c8d9..181dc279a 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -30,13 +30,15 @@ using namespace boost; #include "SubgraphPreconditioner-inl.h" using namespace gtsam; +using namespace example; -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ TEST( NonlinearOptimizer, delta ) { - shared_ptr fg(new ExampleNonlinearFactorGraph(createNonlinearFactorGraph())); + shared_ptr fg(new Graph( + createNonlinearFactorGraph())); Optimizer::shared_config initial = sharedNoisyConfig(); // Expected configuration is the difference between the noisy config @@ -81,19 +83,21 @@ TEST( NonlinearOptimizer, delta ) TEST( NonlinearOptimizer, iterateLM ) { // really non-linear factor graph - shared_ptr fg(new ExampleNonlinearFactorGraph(createReallyNonlinearFactorGraph())); + shared_ptr fg(new Graph( + createReallyNonlinearFactorGraph())); // config far from minimum - Vector x0 = Vector_(1, 3.0); - boost::shared_ptr config(new VectorConfig); - config->insert("x", x0); + Point2 x0(3,0); + boost::shared_ptr config(new Config); + config->insert(simulated2D::PoseKey(1), x0); // ordering shared_ptr ord(new Ordering()); - ord->push_back("x"); + ord->push_back("x1"); // create initial optimization state, with lambda=0 - Optimizer::shared_solver solver(new Factorization); + Optimizer::shared_solver solver(new Factorization< + Graph, Config> ); Optimizer optimizer(fg, ord, config, solver, 0.); // normal iterate @@ -117,23 +121,24 @@ TEST( NonlinearOptimizer, iterateLM ) /* ************************************************************************* */ TEST( NonlinearOptimizer, optimize ) { - shared_ptr fg(new ExampleNonlinearFactorGraph(createReallyNonlinearFactorGraph())); + shared_ptr fg(new Graph( + createReallyNonlinearFactorGraph())); // test error at minimum - Vector xstar = Vector_(1, 0.0); - VectorConfig cstar; - cstar.insert("x", xstar); + Point2 xstar(0,0); + Config cstar; + cstar.insert(simulated2D::PoseKey(1), xstar); DOUBLES_EQUAL(0.0,fg->error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = - Vector x0 = Vector_(1, 3.0); - boost::shared_ptr c0(new VectorConfig); - c0->insert("x", x0); + Point2 x0(3,3); + boost::shared_ptr c0(new Config); + c0->insert(simulated2D::PoseKey(1), x0); DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3); // optimize parameters shared_ptr ord(new Ordering()); - ord->push_back("x"); + ord->push_back("x1"); double relativeThreshold = 1e-5; double absoluteThreshold = 1e-5; @@ -143,12 +148,12 @@ TEST( NonlinearOptimizer, optimize ) // Gauss-Newton Optimizer actual1 = optimizer.gaussNewton(relativeThreshold, absoluteThreshold); - CHECK(assert_equal(*(actual1.config()),cstar)); + DOUBLES_EQUAL(0,fg->error(*(actual1.config())),1e-3); // Levenberg-Marquardt Optimizer actual2 = optimizer.levenbergMarquardt(relativeThreshold, absoluteThreshold, Optimizer::SILENT); - CHECK(assert_equal(*(actual2.config()),cstar)); + DOUBLES_EQUAL(0,fg->error(*(actual2.config())),1e-3); } /* ************************************************************************* */ diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 41e69aa3f..29da30308 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -16,6 +16,7 @@ #define GTSAM_MAGIC_GAUSSIAN 2 #define GTSAM_MAGIC_KEY +#include #include #include #include @@ -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 NLGraph; -typedef boost::shared_ptr > shared; -typedef boost::shared_ptr > shared_c; +typedef simulated2D::Config Config2D; +typedef NonlinearFactorGraph NLGraph; +typedef NonlinearEquality NLE; +typedef boost::shared_ptr shared; +typedef NonlinearOptimizer Optimizer; -typedef NonlinearEquality NLE; -typedef boost::shared_ptr shared_eq; -typedef boost::shared_ptr shared_cfg; -typedef NonlinearOptimizer Optimizer; +typedef TypedSymbol LamKey; -/* ********************************************************************* +/* * Determining a ground truth linear system * with two poses seeing one landmark, with each pose * constrained to a particular value */ TEST (SQP, two_pose_truth ) { bool verbose = false; + + // create a graph + shared_ptr graph(new NLGraph); + + // add the constraints on the ends // position (1, 1) constraint for x1 // position (5, 6) constraint for x2 - VectorConfig feas; - Vector feas1 = Vector_(2, 1.0, 1.0); - Vector feas2 = Vector_(2, 5.0, 6.0); - feas.insert("x1", feas1); - feas.insert("x2", feas2); - - // constant constraint on x1 - shared_eq ef1(new NLE("x1", feas1, vector_compare)); - - // constant constraint on x2 - shared_eq ef2(new NLE("x2", feas2, vector_compare)); - - // measurement from x1 to l1 - Vector z1 = Vector_(2, 0.0, 5.0); - double sigma1 = 0.1; - shared f1(new simulated2D::Measurement(z1, sigma1, "x1", "l1")); - - // measurement from x2 to l1 - Vector z2 = Vector_(2, -4.0, 0.0); - double sigma2 = 0.1; - shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l1")); - - // construct the graph - shared_ptr graph(new NLGraph()); + simulated2D::PoseKey x1(1), x2(2); + simulated2D::PointKey l1(1); + Point2 pt_x1(1.0, 1.0), + pt_x2(5.0, 6.0); + shared_ptr ef1(new NLE(x1, pt_x1)), + ef2(new NLE(x2, pt_x2)); graph->push_back(ef1); graph->push_back(ef2); + + // measurement from x1 to l1 + Point2 z1(0.0, 5.0); + sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); + shared f1(new simulated2D::Measurement(z1, sigma, x1,l1)); graph->push_back(f1); + + // measurement from x2 to l1 + Point2 z2(-4.0, 0.0); + shared f2(new simulated2D::Measurement(z2, sigma, x2,l1)); graph->push_back(f2); // create an initial estimate - boost::shared_ptr initialEstimate(new VectorConfig(feas)); // must start with feasible set - initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth - //initialEstimate->insert("l1", Vector_(2, 1.2, 5.6)); // with small error + Point2 pt_l1( + 1.0, 6.0 // ground truth + //1.2, 5.6 // small error + ); + shared_ptr initialEstimate(new Config2D); + initialEstimate->insert(l1, pt_l1); + initialEstimate->insert(x1, pt_x1); + initialEstimate->insert(x2, pt_x2); // optimize the graph shared_ptr ordering(new Ordering()); @@ -305,12 +302,14 @@ TEST (SQP, two_pose_truth ) { double relativeThreshold = 1e-5; double absoluteThreshold = 1e-5; Optimizer act_opt = optimizer.gaussNewton(relativeThreshold, absoluteThreshold); - boost::shared_ptr actual = act_opt.config(); + boost::shared_ptr actual = act_opt.config(); if (verbose) actual->print("Configuration after optimization"); // verify - VectorConfig expected(feas); - expected.insert("l1", Vector_(2, 1.0, 6.0)); + Config2D expected; + expected.insert(x1, pt_x1); + expected.insert(x2, pt_x2); + expected.insert(l1, Point2(1.0, 6.0)); CHECK(assert_equal(expected, *actual, 1e-5)); } @@ -319,36 +318,43 @@ namespace sqp_test1 { // binary constraint between landmarks /** g(x) = x-y = 0 */ - Vector g(const VectorConfig& config, const list& keys) { - return config[keys.front()] - config[keys.back()]; + Vector g(const Config2D& config, const list& keys) { + Point2 pt1, pt2; + pt1 = config[simulated2D::PointKey(keys.front().index())]; + pt2 = config[simulated2D::PointKey(keys.back().index())]; + return Vector_(2, pt1.x() - pt2.x(), pt1.y() - pt2.y()); } /** jacobian at l1 */ - Matrix G1(const VectorConfig& config, const list& keys) { + Matrix G1(const Config2D& config, const list& keys) { return eye(2); } /** jacobian at l2 */ - Matrix G2(const VectorConfig& config, const list& keys) { + Matrix G2(const Config2D& config, const list& keys) { return -1 * eye(2); } } // \namespace sqp_test1 -namespace sqp_test2 { +//namespace sqp_test2 { +// +// // Unary Constraint on x1 +// /** g(x) = x -[1;1] = 0 */ +// Vector g(const Config2D& config, const list& keys) { +// return config[keys.front()] - Vector_(2, 1.0, 1.0); +// } +// +// /** jacobian at x1 */ +// Matrix G(const Config2D& config, const list& keys) { +// return eye(2); +// } +// +//} // \namespace sqp_test2 - // Unary Constraint on x1 - /** g(x) = x -[1;1] = 0 */ - Vector g(const VectorConfig& config, const list& keys) { - return config[keys.front()] - Vector_(2, 1.0, 1.0); - } - /** jacobian at x1 */ - Matrix G(const VectorConfig& config, const list& keys) { - return eye(2); - } - -} // \namespace sqp_test2 +typedef NonlinearConstraint2< + Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; /* ********************************************************************* * Version that actually uses nonlinear equality constraints @@ -359,68 +365,65 @@ namespace sqp_test2 { */ TEST (SQP, two_pose ) { bool verbose = false; - // position (1, 1) constraint for x1 - VectorConfig feas; - feas.insert("x1", Vector_(2, 1.0, 1.0)); - // constant constraint on x1 - list keys1; keys1 += "x1"; - boost::shared_ptr > c1( - new NonlinearConstraint1( - boost::bind(sqp_test2::g, _1, keys1), - "x1", boost::bind(sqp_test2::G, _1, keys1), - 2, "L1")); + // create the graph + shared_ptr graph(new NLGraph); + + // add the constraints on the ends + // position (1, 1) constraint for x1 + // position (5, 6) constraint for x2 + simulated2D::PoseKey x1(1), x2(2); + simulated2D::PointKey l1(1), l2(2); + Point2 pt_x1(1.0, 1.0), + pt_x2(5.0, 6.0); + shared_ptr ef1(new NLE(x1, pt_x1)); + graph->push_back(ef1); // measurement from x1 to l1 - Vector z1 = Vector_(2, 0.0, 5.0); - double sigma1 = 0.1; - shared f1(new simulated2D::Measurement(z1, sigma1, "x1", "l1")); + Point2 z1(0.0, 5.0); + sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); + shared f1(new simulated2D::Measurement(z1, sigma, x1,l1)); + graph->push_back(f1); // measurement from x2 to l2 - Vector z2 = Vector_(2, -4.0, 0.0); - double sigma2 = 0.1; - shared f2(new simulated2D::Measurement(z2, sigma2, "x2", "l2")); + Point2 z2(-4.0, 0.0); + shared f2(new simulated2D::Measurement(z2, sigma, x2,l2)); + graph->push_back(f2); // equality constraint between l1 and l2 list keys2; keys2 += "l1", "l2"; - boost::shared_ptr > c2( - new NonlinearConstraint2( + boost::shared_ptr c2(new NLC2( boost::bind(sqp_test1::g, _1, keys2), - "l1", boost::bind(sqp_test1::G1, _1, keys2), - "l2", boost::bind(sqp_test1::G2, _1, keys2), - 2, "L12")); - - // construct the graph - NLGraph graph; - graph.push_back(c1); - graph.push_back(c2); - graph.push_back(f1); - graph.push_back(f2); + l1, boost::bind(sqp_test1::G1, _1, keys2), + l2, boost::bind(sqp_test1::G2, _1, keys2), + 2, "L1")); + graph->push_back(c2); // create an initial estimate - shared_cfg initialEstimate(new VectorConfig(feas)); // must start with feasible set - initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); // ground truth - initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame - initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin + shared_ptr initialEstimate(new Config2D); + initialEstimate->insert(x1, pt_x1); + initialEstimate->insert(x2, Point2()); + initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth + initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame // create an initial estimate for the lagrange multiplier - shared_cfg initLagrange(new VectorConfig); - initLagrange->insert("L12", Vector_(2, 1.0, 1.0)); - initLagrange->insert("L1", Vector_(2, 1.0, 1.0)); + shared_ptr initLagrange(new VectorConfig); + initLagrange->insert(LamKey(1), Vector_(2, 1.0, 1.0)); // connect the landmarks // create state config variables and initialize them - VectorConfig state(*initialEstimate), state_lambda(*initLagrange); + Config2D state(*initialEstimate); + VectorConfig state_lambda(*initLagrange); // optimization loop int maxIt = 1; for (int i = 0; i >::const_iterator const_iterator; - typedef NonlinearConstraint NLConstraint; + typedef FactorGraph >::const_iterator const_iterator; // iterate over all factors - for (const_iterator factor = graph.begin(); factor < graph.end(); factor++) { - const shared_c constraint = boost::shared_dynamic_cast(*factor); + for (const_iterator factor = graph->begin(); factor < graph->end(); factor++) { + const shared_ptr constraint = boost::shared_dynamic_cast(*factor); if (constraint == NULL) { // if a regular factor, linearize using the default linearization GaussianFactor::shared_ptr f = (*factor)->linearize(state); @@ -433,11 +436,12 @@ TEST (SQP, two_pose ) { fg.push_back(c); } } + if (verbose) fg.print("Linearized graph"); // create an ordering Ordering ordering; - ordering += "x1", "x2", "l1", "l2", "L12", "L1"; + ordering += "x1", "x2", "l1", "l2", "L1"; // optimize linear graph to get full delta config VectorConfig delta = fg.optimize(ordering); @@ -451,10 +455,11 @@ TEST (SQP, two_pose ) { } // verify - VectorConfig expected(feas); - expected.insert("l1", Vector_(2, 1.0, 6.0)); - expected.insert("l2", Vector_(2, 1.0, 6.0)); - expected.insert("x2", Vector_(2, 5.0, 6.0)); + Config2D expected; + expected.insert(x1, pt_x1); + expected.insert(l1, Point2(1.0, 6.0)); + expected.insert(l2, Point2(1.0, 6.0)); + expected.insert(x2, Point2(5.0, 6.0)); CHECK(assert_equal(expected, state, 1e-5)); } @@ -471,9 +476,14 @@ using namespace gtsam::visualSLAM; using namespace boost; // typedefs for visual SLAM example +typedef visualSLAM::Config VConfig; +typedef visualSLAM::Graph VGraph; typedef boost::shared_ptr shared_vf; -typedef NonlinearOptimizer VOptimizer; -typedef SQPOptimizer SOptimizer; +typedef NonlinearOptimizer VOptimizer; +typedef SQPOptimizer VSOptimizer; +typedef NonlinearConstraint2< + VConfig, visualSLAM::PointKey, Pose3, visualSLAM::PointKey, Pose3> VNLC2; + /** * Ground truth for a visual SLAM example with stereo vision @@ -494,13 +504,13 @@ TEST (SQP, stereo_truth ) { Point3 landmarkNoisy(1.0, 6.0, 0.0); // create truth config - boost::shared_ptr truthConfig(new Config); + boost::shared_ptr truthConfig(new Config); truthConfig->insert(1, camera1.pose()); truthConfig->insert(2, camera2.pose()); truthConfig->insert(1, landmark); // create graph - shared_ptr graph(new Graph()); + shared_ptr graph(new VGraph()); // create equality constraints for poses graph->push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); @@ -550,9 +560,9 @@ TEST (SQP, stereo_truth_noisy ) { // create initial estimates Rot3 faceDownY(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - 0.0, 1.0, 0.0)); + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, 1.0, 0.0)); Pose3 pose1(faceDownY, Point3()); // origin, left camera SimpleCamera camera1(K, pose1); Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left @@ -576,8 +586,8 @@ TEST (SQP, stereo_truth_noisy ) { shared_ptr graph(new Graph()); // create equality constraints for poses - graph->push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); - graph->push_back(shared_ptr(new PoseConstraint(2, camera2.pose()))); + graph->push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); + graph->push_back(shared_ptr(new PoseConstraint(2, camera2.pose()))); // create VSLAM factors Point2 z1 = camera1.project(landmark); @@ -619,12 +629,6 @@ TEST (SQP, stereo_truth_noisy ) { CHECK(assert_equal(*truthConfig,*(optimizer.config()))); } -/* ********************************************************************* */ -// Utility function to strip out a landmark number from a string key -//int getNum(const string& key) { -// return atoi(key.substr(1, key.size()-1).c_str()); -//} - /* ********************************************************************* */ namespace sqp_stereo { @@ -648,12 +652,12 @@ namespace sqp_stereo { } // \namespace sqp_stereo /* ********************************************************************* */ -Graph stereoExampleGraph() { +VGraph stereoExampleGraph() { // create initial estimates Rot3 faceDownY(Matrix_(3,3, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - 0.0, 1.0, 0.0)); + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, 1.0, 0.0)); Pose3 pose1(faceDownY, Point3()); // origin, left camera SimpleCamera camera1(K, pose1); Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left @@ -662,11 +666,11 @@ Graph stereoExampleGraph() { Point3 landmark2(1.0, 5.0, 0.0); // create graph - Graph graph; + VGraph graph; // create equality constraints for poses - graph.push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); - graph.push_back(shared_ptr(new PoseConstraint(2, camera2.pose()))); + graph.push_back(shared_ptr(new PoseConstraint(1, camera1.pose()))); + graph.push_back(shared_ptr(new PoseConstraint(2, camera2.pose()))); // create factors Point2 z1 = camera1.project(landmark1); @@ -680,19 +684,19 @@ Graph stereoExampleGraph() { // NOTE: this is really just a linear constraint that is exactly the same // as the previous examples list keys; keys += "l1", "l2"; - boost::shared_ptr > c2( - new NonlinearConstraint2( - boost::bind(sqp_stereo::g, _1, keys), - "l1", boost::bind(sqp_stereo::G1, _1, keys), - "l2", boost::bind(sqp_stereo::G2, _1, keys), - 3, "L12")); + visualSLAM::PointKey l1(1), l2(2); + shared_ptr c2( + new VNLC2(boost::bind(sqp_stereo::g, _1, keys), + l1, boost::bind(sqp_stereo::G1, _1, keys), + l2, boost::bind(sqp_stereo::G2, _1, keys), + 3, "L12")); graph.push_back(c2); return graph; } /* ********************************************************************* */ -boost::shared_ptr stereoExampleTruthConfig() { +boost::shared_ptr stereoExampleTruthConfig() { // create initial estimates Rot3 faceDownY(Matrix_(3,3, 1.0, 0.0, 0.0, @@ -723,21 +727,21 @@ TEST (SQP, stereo_sqp ) { bool verbose = false; // get a graph - Graph graph = stereoExampleGraph(); + VGraph graph = stereoExampleGraph(); if (verbose) graph.print("Graph after construction"); // get the truth config - boost::shared_ptr truthConfig = stereoExampleTruthConfig(); + boost::shared_ptr truthConfig = stereoExampleTruthConfig(); // create ordering Ordering ord; ord += "x1", "x2", "l1", "l2"; // create optimizer - SOptimizer optimizer(graph, ord, truthConfig); + VSOptimizer optimizer(graph, ord, truthConfig); // optimize - SOptimizer afterOneIteration = optimizer.iterate(); + VSOptimizer afterOneIteration = optimizer.iterate(); // check if correct CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); @@ -775,7 +779,7 @@ TEST (SQP, stereo_sqp_noisy ) { ord += "x1", "x2", "l1", "l2"; // create optimizer - SOptimizer optimizer(graph, ord, initConfig); + VSOptimizer optimizer(graph, ord, initConfig); // optimize double start_error = optimizer.error(); @@ -786,9 +790,9 @@ TEST (SQP, stereo_sqp_noisy ) { //if (verbose) optimizer.graph()->print(); if (verbose) optimizer.config()->print(); if (verbose) - optimizer = optimizer.iterate(SOptimizer::FULL); + optimizer = optimizer.iterate(VSOptimizer::FULL); else - optimizer = optimizer.iterate(SOptimizer::SILENT); + optimizer = optimizer.iterate(VSOptimizer::SILENT); } if (verbose) cout << "Initial Error: " << start_error << "\n" @@ -840,11 +844,11 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) { ord += "x1", "x2", "l1", "l2", "L12"; // create lagrange multipliers - SOptimizer::shared_vconfig initLagrangeConfig(new VectorConfig); + VSOptimizer::shared_vconfig initLagrangeConfig(new VectorConfig); initLagrangeConfig->insert("L12", Vector_(3, 0.0, 0.0, 0.0)); // create optimizer - SOptimizer optimizer(graph, ord, initConfig, initLagrangeConfig); + VSOptimizer optimizer(graph, ord, initConfig, initLagrangeConfig); // optimize double start_error = optimizer.error(); @@ -855,10 +859,10 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) { << " Iteration: " << i << endl; optimizer.config()->print("Config Before Iteration"); optimizer.configLagrange()->print("Lagrange Before Iteration"); - optimizer = optimizer.iterate(SOptimizer::FULL); + optimizer = optimizer.iterate(VSOptimizer::FULL); } else - optimizer = optimizer.iterate(SOptimizer::SILENT); + optimizer = optimizer.iterate(VSOptimizer::SILENT); } if (verbose) cout << "Initial Error: " << start_error << "\n" diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index e8cbc8edf..cb3ffbe9b 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -27,23 +27,34 @@ using namespace std; using namespace gtsam; +using namespace boost; using namespace boost::assign; +using namespace simulated2D; static sharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); // typedefs -typedef boost::shared_ptr shared_config; -typedef NonlinearFactorGraph NLGraph; -typedef boost::shared_ptr > shared; -typedef boost::shared_ptr > shared_c; +typedef simulated2D::Config Config2D; +typedef boost::shared_ptr shared_config; +typedef NonlinearFactorGraph NLGraph; +typedef boost::shared_ptr > shared; +namespace map_warp_example { +typedef NonlinearConstraint1< + Config2D, simulated2D::PoseKey, Point2> NLC1; +typedef NonlinearConstraint2< + Config2D, simulated2D::PointKey, Point2, simulated2D::PointKey, Point2> NLC2; +} // \namespace map_warp_example + + +/* ********************************************************************* */ TEST ( SQPOptimizer, basic ) { // create a basic optimizer NLGraph graph; Ordering ordering; - shared_config config(new VectorConfig); + shared_config config(new Config2D); - SQPOptimizer optimizer(graph, ordering, config); + SQPOptimizer optimizer(graph, ordering, config); // verify components CHECK(assert_equal(graph, *(optimizer.graph()))); @@ -59,17 +70,18 @@ TEST ( SQPOptimizer, basic ) { namespace sqp_LinearMapWarp2 { // binary constraint between landmarks /** g(x) = x-y = 0 */ -Vector g_func(const VectorConfig& config, const list& keys) { - return config[keys.front()]-config[keys.back()]; +Vector g_func(const Config2D& config, const PointKey& key1, const PointKey& key2) { + Point2 p = config[key1]-config[key2]; + return Vector_(2, p.x(), p.y()); } /** jacobian at l1 */ -Matrix jac_g1(const VectorConfig& config, const list& keys) { +Matrix jac_g1(const Config2D& config) { return eye(2); } /** jacobian at l2 */ -Matrix jac_g2(const VectorConfig& config, const list& keys) { +Matrix jac_g2(const Config2D& config) { return -1*eye(2); } } // \namespace sqp_LinearMapWarp2 @@ -77,47 +89,50 @@ Matrix jac_g2(const VectorConfig& config, const list& keys) { namespace sqp_LinearMapWarp1 { // Unary Constraint on x1 /** g(x) = x -[1;1] = 0 */ -Vector g_func(const VectorConfig& config, const list& keys) { - return config[keys.front()]-Vector_(2, 1.0, 1.0); +Vector g_func(const Config2D& config, const PoseKey& key) { + Point2 p = config[key]-Point2(1.0, 1.0); + return Vector_(2, p.x(), p.y()); } /** jacobian at x1 */ -Matrix jac_g(const VectorConfig& config, const list& keys) { +Matrix jac_g(const Config2D& config) { return eye(2); } } // \namespace sqp_LinearMapWarp12 -typedef SQPOptimizer Optimizer; +typedef SQPOptimizer Optimizer; /** * Creates the graph with each robot seeing the landmark, and it is * known that it is the same landmark */ NLGraph linearMapWarpGraph() { + using namespace map_warp_example; + // keys + PoseKey x1(1), x2(2); + PointKey l1(1), l2(2); + // constant constraint on x1 list keyx; keyx += "x1"; - boost::shared_ptr > c1( - new NonlinearConstraint1( - boost::bind(sqp_LinearMapWarp1::g_func, _1, keyx), - "x1", boost::bind(sqp_LinearMapWarp1::jac_g, _1, keyx), - 2, "L1")); + shared_ptr c1(new NLC1(boost::bind(sqp_LinearMapWarp1::g_func, _1, x1), + x1, boost::bind(sqp_LinearMapWarp1::jac_g, _1), + 2, "L1")); // measurement from x1 to l1 - Vector z1 = Vector_(2, 0.0, 5.0); - shared f1(new simulated2D::Measurement(z1, sigma, "x1", "l1")); + Point2 z1(0.0, 5.0); + shared f1(new simulated2D::Measurement(z1, sigma, 1,1)); // measurement from x2 to l2 - Vector z2 = Vector_(2, -4.0, 0.0); - shared f2(new simulated2D::Measurement(z2, sigma, "x2", "l2")); + Point2 z2(-4.0, 0.0); + shared f2(new simulated2D::Measurement(z2, sigma, 2,2)); // equality constraint between l1 and l2 list keys; keys += "l1", "l2"; - boost::shared_ptr > c2( - new NonlinearConstraint2( - boost::bind(sqp_LinearMapWarp2::g_func, _1, keys), - "l1", boost::bind(sqp_LinearMapWarp2::jac_g1, _1, keys), - "l2", boost::bind(sqp_LinearMapWarp2::jac_g2, _1, keys), - 2, "L12")); + shared_ptr c2 (new NLC2( + boost::bind(sqp_LinearMapWarp2::g_func, _1, l1, l2), + l1, boost::bind(sqp_LinearMapWarp2::jac_g1, _1), + l2, boost::bind(sqp_LinearMapWarp2::jac_g2, _1), + 2, "L12")); // construct the graph NLGraph graph; @@ -135,15 +150,19 @@ TEST ( SQPOptimizer, map_warp_initLam ) { // get a graph NLGraph graph = linearMapWarpGraph(); + // keys + PoseKey x1(1), x2(2); + PointKey l1(1), l2(2); + // create an initial estimate - shared_config initialEstimate(new VectorConfig); - initialEstimate->insert("x1", Vector_(2, 1.0, 1.0)); - initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); - initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame - initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin + shared_config initialEstimate(new Config2D); + initialEstimate->insert(x1, Point2(1.0, 1.0)); + initialEstimate->insert(l1, Point2(1.0, 6.0)); + initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame + initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin // create an initial estimate for the lagrange multiplier - shared_config initLagrange(new VectorConfig); + shared_ptr initLagrange(new VectorConfig); initLagrange->insert("L12", Vector_(2, 1.0, 1.0)); initLagrange->insert("L1", Vector_(2, 1.0, 1.0)); @@ -159,12 +178,12 @@ TEST ( SQPOptimizer, map_warp_initLam ) { Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); // get the config back out and verify - VectorConfig actual = *(oneIteration.config()); - VectorConfig expected; - expected.insert("x1", Vector_(2, 1.0, 1.0)); - expected.insert("l1", Vector_(2, 1.0, 6.0)); - expected.insert("l2", Vector_(2, 1.0, 6.0)); - expected.insert("x2", Vector_(2, 5.0, 6.0)); + Config2D actual = *(oneIteration.config()); + Config2D expected; + expected.insert(x1, Point2(1.0, 1.0)); + expected.insert(l1, Point2(1.0, 6.0)); + expected.insert(l2, Point2(1.0, 6.0)); + expected.insert(x2, Point2(5.0, 6.0)); CHECK(assert_equal(expected, actual)); } @@ -175,12 +194,16 @@ TEST ( SQPOptimizer, map_warp ) { NLGraph graph = linearMapWarpGraph(); if (verbose) graph.print("Initial map warp graph"); + // keys + PoseKey x1(1), x2(2); + PointKey l1(1), l2(2); + // create an initial estimate - shared_config initialEstimate(new VectorConfig); - initialEstimate->insert("x1", Vector_(2, 1.0, 1.0)); - initialEstimate->insert("l1", Vector_(2, 1.0, 6.0)); - initialEstimate->insert("l2", Vector_(2, -4.0, 0.0)); // starting with a separate reference frame - initialEstimate->insert("x2", Vector_(2, 0.0, 0.0)); // other pose starts at origin + shared_config initialEstimate(new Config2D); + initialEstimate->insert(x1, Point2(1.0, 1.0)); + initialEstimate->insert(l1, Point2(.0, 6.0)); + initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame + initialEstimate->insert(x2, Point2(0.0, 0.0)); // other pose starts at origin // create an ordering Ordering ordering; @@ -193,12 +216,12 @@ TEST ( SQPOptimizer, map_warp ) { Optimizer oneIteration = optimizer.iterate(Optimizer::SILENT); // get the config back out and verify - VectorConfig actual = *(oneIteration.config()); - VectorConfig expected; - expected.insert("x1", Vector_(2, 1.0, 1.0)); - expected.insert("l1", Vector_(2, 1.0, 6.0)); - expected.insert("l2", Vector_(2, 1.0, 6.0)); - expected.insert("x2", Vector_(2, 5.0, 6.0)); + Config2D actual = *(oneIteration.config()); + Config2D expected; + expected.insert(x1, Point2(1.0, 1.0)); + expected.insert(l1, Point2(1.0, 6.0)); + expected.insert(l2, Point2(1.0, 6.0)); + expected.insert(x2, Point2(5.0, 6.0)); CHECK(assert_equal(expected, actual)); } @@ -209,16 +232,13 @@ TEST ( SQPOptimizer, map_warp ) { // states, which enforces a minimum distance. /* ********************************************************************* */ -bool vector_compare(const Vector& a, const Vector& b) { - return equal_with_abs_tol(a, b, 1e-5); -} +typedef NonlinearConstraint2 AvoidConstraint; +typedef shared_ptr shared_a; +typedef NonlinearEquality PoseConstraint; +typedef shared_ptr shared_pc; +typedef NonlinearEquality ObstacleConstraint; +typedef shared_ptr shared_oc; -typedef NonlinearConstraint1 NLC1; -typedef boost::shared_ptr shared_NLC1; -typedef NonlinearConstraint2 NLC2; -typedef boost::shared_ptr shared_NLC2; -typedef NonlinearEquality NLE; -typedef boost::shared_ptr shared_NLE; namespace sqp_avoid1 { // avoidance radius @@ -226,54 +246,52 @@ double radius = 1.0; // binary avoidance constraint /** g(x) = ||x2-obs||^2 - radius^2 > 0 */ -Vector g_func(const VectorConfig& config, const list& keys) { - Vector delta = config[keys.front()]-config[keys.back()]; - double dist2 = sum(emul(delta, delta)); +Vector g_func(const Config2D& config, const PoseKey& x, const PointKey& obs) { + double dist2 = config[x].dist(config[obs]); double thresh = radius*radius; return Vector_(1, dist2-thresh); } /** jacobian at pose */ -Matrix jac_g1(const VectorConfig& config, const list& keys) { - Vector x2 = config[keys.front()], obs = config[keys.back()]; - Vector grad = 2.0*(x2-obs); - return Matrix_(1,2, grad(0), grad(1)); +Matrix jac_g1(const Config2D& config, const PoseKey& x, const PointKey& obs) { + Point2 p = config[x]-config[obs]; + return Matrix_(1,2, 2.0*p.x(), 2.0*p.y()); } /** jacobian at obstacle */ -Matrix jac_g2(const VectorConfig& config, const list& keys) { - Vector x2 = config[keys.front()], obs = config[keys.back()]; - Vector grad = -2.0*(x2-obs); - return Matrix_(1,2, grad(0), grad(1)); +Matrix jac_g2(const Config2D& config, const PoseKey& x, const PointKey& obs) { + Point2 p = config[x]-config[obs]; + return Matrix_(1,2, -2.0*p.x(), -2.0*p.y()); } } -pair obstacleAvoidGraph() { - // fix start, end, obstacle positions - VectorConfig feasible; - Vector feas1 = Vector_(2, 0.0, 0.0), feas2 = Vector_(2, 10.0, 0.0), feas3 = - Vector_(2, 5.0, -0.5); - feasible.insert("x1", feas1); - feasible.insert("x3", feas2); - feasible.insert("o", feas3); - shared_NLE e1(new NLE("x1", feas1, vector_compare)); - shared_NLE e2(new NLE("x3", feas2, vector_compare)); - shared_NLE e3(new NLE("o", feas3, vector_compare)); +pair obstacleAvoidGraph() { + // Keys + PoseKey x1(1), x2(2), x3(3); + PointKey l1(1); + + // Constrained Points + Point2 pt_x1, + pt_x3(10.0, 0.0), + pt_l1(5.0, -0.5); + + shared_pc e1(new PoseConstraint(x1, pt_x1)); + shared_pc e2(new PoseConstraint(x3, pt_x3)); + shared_oc e3(new ObstacleConstraint(l1, pt_l1)); // measurement from x1 to x2 - Vector x1x2 = Vector_(2, 5.0, 0.0); - shared f1(new simulated2D::Odometry(x1x2, sigma, "x1", "x2")); + Point2 x1x2(5.0, 0.0); + shared f1(new simulated2D::Odometry(x1x2, sigma, 1,2)); // measurement from x2 to x3 - Vector x2x3 = Vector_(2, 5.0, 0.0); - shared f2(new simulated2D::Odometry(x2x3, sigma, "x2", "x3")); + Point2 x2x3(5.0, 0.0); + shared f2(new simulated2D::Odometry(x2x3, sigma, 2,3)); // create a binary inequality constraint that forces the middle point away from // the obstacle - list keys; keys += "x2", "o"; - shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid1::g_func, _1, keys), - "x2", boost::bind(sqp_avoid1::jac_g1, _1, keys), - "o",boost::bind(sqp_avoid1::jac_g2, _1, keys), + shared_a c1(new AvoidConstraint(boost::bind(sqp_avoid1::g_func, _1, x2, l1), + x2, boost::bind(sqp_avoid1::jac_g1, _1, x2, l1), + l1,boost::bind(sqp_avoid1::jac_g2, _1, x2, l1), 1, "L20", false)); // construct the graph @@ -285,22 +303,29 @@ pair obstacleAvoidGraph() { graph.push_back(f1); graph.push_back(f2); - return make_pair(graph, feasible); + // make a config of the fixed values, for convenience + Config2D config; + config.insert(x1, pt_x1); + config.insert(x3, pt_x3); + config.insert(l1, pt_l1); + + return make_pair(graph, config); } /* ********************************************************************* */ TEST ( SQPOptimizer, inequality_avoid ) { // create the graph - NLGraph graph; VectorConfig feasible; + NLGraph graph; Config2D feasible; boost::tie(graph, feasible) = obstacleAvoidGraph(); // create the rest of the config - shared_config init(new VectorConfig(feasible)); - init->insert("x2", Vector_(2, 5.0, 100.0)); + shared_ptr init(new Config2D(feasible)); + PoseKey x2(2); + init->insert(x2, Point2(5.0, 100.0)); // create an ordering Ordering ord; - ord += "x1", "x2", "x3", "o"; + ord += "x1", "x2", "x3", "l1"; // create an optimizer Optimizer optimizer(graph, ord, init); @@ -310,32 +335,33 @@ TEST ( SQPOptimizer, inequality_avoid ) { // so it will violate the constraint after one iteration Optimizer afterOneIteration = optimizer.iterate(Optimizer::SILENT); - VectorConfig exp1(feasible); - exp1.insert("x2", Vector_(2, 5.0, 0.0)); + Config2D exp1(feasible); + exp1.insert(x2, Point2(5.0, 0.0)); CHECK(assert_equal(exp1, *(afterOneIteration.config()))); // the second iteration will activate the constraint and force the // config to a viable configuration. Optimizer after2ndIteration = afterOneIteration.iterate(Optimizer::SILENT); - VectorConfig exp2(feasible); - exp2.insert("x2", Vector_(2, 5.0, 0.75)); + Config2D exp2(feasible); + exp2.insert(x2, Point2(5.0, 0.5)); CHECK(assert_equal(exp2, *(after2ndIteration.config()))); } /* ********************************************************************* */ TEST ( SQPOptimizer, inequality_avoid_iterative ) { // create the graph - NLGraph graph; VectorConfig feasible; + NLGraph graph; Config2D feasible; boost::tie(graph, feasible) = obstacleAvoidGraph(); // create the rest of the config - shared_config init(new VectorConfig(feasible)); - init->insert("x2", Vector_(2, 5.0, 100.0)); + shared_ptr init(new Config2D(feasible)); + PoseKey x2(2); + init->insert(x2, Point2(5.0, 100.0)); // create an ordering Ordering ord; - ord += "x1", "x2", "x3", "o"; + ord += "x1", "x2", "x3", "l1"; // create an optimizer Optimizer optimizer(graph, ord, init); @@ -346,106 +372,9 @@ TEST ( SQPOptimizer, inequality_avoid_iterative ) { Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh); // verify - VectorConfig exp2(feasible); - exp2.insert("x2", Vector_(2, 5.0, 0.75)); - CHECK(assert_equal(exp2, *(final.config()))); -} - -/* ********************************************************************* */ -// Use boost bind to parameterize the function -namespace sqp_avoid2 { -// binary avoidance constraint -/** g(x) = ||x2-obs||^2 - radius^2 > 0 */ -Vector g_func(double radius, const VectorConfig& config, const list& 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& 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& 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 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 keys; keys += "x2", "o"; - shared_NLC2 c1(new NLC2(boost::bind(sqp_avoid2::g_func, radius, _1, keys), - "x2", boost::bind(sqp_avoid2::jac_g1, _1, keys), - "o", boost::bind(sqp_avoid2::jac_g2, _1, keys), - 1, "L20", false)); - - // construct the graph - NLGraph graph; - graph.push_back(e1); - graph.push_back(e2); - graph.push_back(e3); - graph.push_back(c1); - graph.push_back(f1); - graph.push_back(f2); - - return make_pair(graph, feasible); -} - -/* ********************************************************************* */ -TEST ( SQPOptimizer, inequality_avoid_iterative_bind ) { - // create the graph - NLGraph graph; VectorConfig feasible; - boost::tie(graph, feasible) = obstacleAvoidGraphGeneral(); - - // create the rest of the config - shared_config init(new VectorConfig(feasible)); - init->insert("x2", Vector_(2, 5.0, 100.0)); - - // create an ordering - Ordering ord; - ord += "x1", "x2", "x3", "o"; - - // create an optimizer - Optimizer optimizer(graph, ord, init); - - double relThresh = 1e-5; // minimum change in error between iterations - double absThresh = 1e-5; // minimum error necessary to converge - double constraintThresh = 1e-9; // minimum constraint error to be feasible - Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh); - - // verify - VectorConfig exp2(feasible); - exp2.insert("x2", Vector_(2, 5.0, 0.75)); - CHECK(assert_equal(exp2, *(final.config()))); + Config2D exp2(feasible); + exp2.insert(x2, Point2(5.0, 0.5)); + CHECK(assert_equal(exp2, *(final.config()))); // FAILS } /* ************************************************************************* */ diff --git a/cpp/testSimulated2D.cpp b/cpp/testSimulated2D.cpp index 69b8b91f4..ec8fc136b 100644 --- a/cpp/testSimulated2D.cpp +++ b/cpp/testSimulated2D.cpp @@ -18,7 +18,7 @@ using namespace simulated2D; /* ************************************************************************* */ TEST( simulated2D, Dprior ) { - Vector x(2);x(0)=1;x(1)=-9; + Point2 x(1,-9); Matrix numerical = numericalDerivative11(prior,x); Matrix computed; prior(x,computed); @@ -28,8 +28,7 @@ TEST( simulated2D, Dprior ) /* ************************************************************************* */ TEST( simulated2D, DOdo ) { - Vector x1(2);x1(0)= 1;x1(1)=-9; - Vector x2(2);x2(0)=-5;x2(1)= 6; + Point2 x1(1,-9),x2(-5,6); Matrix H1,H2; odo(x1,x2,H1,H2); Matrix A1 = numericalDerivative21(odo,x1,x2); diff --git a/cpp/testSubgraphPreconditioner.cpp b/cpp/testSubgraphPreconditioner.cpp index 491369248..3a0dbab0e 100644 --- a/cpp/testSubgraphPreconditioner.cpp +++ b/cpp/testSubgraphPreconditioner.cpp @@ -21,6 +21,7 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ TEST( SubgraphPreconditioner, planarGraph ) @@ -35,7 +36,7 @@ TEST( SubgraphPreconditioner, planarGraph ) // Check canonical ordering Ordering expected, ordering = planarOrdering(3); - expected += "x33", "x23", "x13", "x32", "x22", "x12", "x31", "x21", "x11"; + expected += "x3003", "x2003", "x1003", "x3002", "x2002", "x1002", "x3001", "x2001", "x1001"; CHECK(assert_equal(expected,ordering)); // Check that xtrue is optimal @@ -117,12 +118,12 @@ TEST( SubgraphPreconditioner, system ) // y1 = perturbed y0 VectorConfig y1 = zeros; - y1.getReference("x23") = Vector_(2, 1.0, -1.0); + y1.getReference("x2003") = Vector_(2, 1.0, -1.0); // Check corresponding x values VectorConfig expected_x1 = xtrue, x1 = system.x(y1); - expected_x1.getReference("x23") = Vector_(2, 2.01, 2.99); - expected_x1.getReference("x33") = Vector_(2, 3.01, 2.99); + expected_x1.getReference("x2003") = Vector_(2, 2.01, 2.99); + expected_x1.getReference("x3003") = Vector_(2, 3.01, 2.99); CHECK(assert_equal(xtrue, system.x(y0))); CHECK(assert_equal(expected_x1,system.x(y1))); @@ -136,21 +137,21 @@ TEST( SubgraphPreconditioner, system ) VectorConfig expected_gx0 = zeros; VectorConfig expected_gx1 = zeros; CHECK(assert_equal(expected_gx0,Ab.gradient(xtrue))); - expected_gx1.getReference("x13") = Vector_(2, -100., 100.); - expected_gx1.getReference("x22") = Vector_(2, -100., 100.); - expected_gx1.getReference("x23") = Vector_(2, 200., -200.); - expected_gx1.getReference("x32") = Vector_(2, -100., 100.); - expected_gx1.getReference("x33") = Vector_(2, 100., -100.); + expected_gx1.getReference("x1003") = Vector_(2, -100., 100.); + expected_gx1.getReference("x2002") = Vector_(2, -100., 100.); + expected_gx1.getReference("x2003") = Vector_(2, 200., -200.); + expected_gx1.getReference("x3002") = Vector_(2, -100., 100.); + expected_gx1.getReference("x3003") = Vector_(2, 100., -100.); CHECK(assert_equal(expected_gx1,Ab.gradient(x1))); // Test gradient in y VectorConfig expected_gy0 = zeros; VectorConfig expected_gy1 = zeros; - expected_gy1.getReference("x13") = Vector_(2, 2., -2.); - expected_gy1.getReference("x22") = Vector_(2, -2., 2.); - expected_gy1.getReference("x23") = Vector_(2, 3., -3.); - expected_gy1.getReference("x32") = Vector_(2, -1., 1.); - expected_gy1.getReference("x33") = Vector_(2, 1., -1.); + expected_gy1.getReference("x1003") = Vector_(2, 2., -2.); + expected_gy1.getReference("x2002") = Vector_(2, -2., 2.); + expected_gy1.getReference("x2003") = Vector_(2, 3., -3.); + expected_gy1.getReference("x3002") = Vector_(2, -1., 1.); + expected_gy1.getReference("x3003") = Vector_(2, 1., -1.); CHECK(assert_equal(expected_gy0,system.gradient(y0))); CHECK(assert_equal(expected_gy1,system.gradient(y1))); @@ -189,7 +190,7 @@ TEST( SubgraphPreconditioner, conjugateGradients ) BOOST_FOREACH(const string& j, ordering) y0.insert(j,z2); VectorConfig y1 = y0; - y1.getReference("x23") = Vector_(2, 1.0, -1.0); + y1.getReference("x2003") = Vector_(2, 1.0, -1.0); VectorConfig x1 = system.x(y1); // Solve for the remaining constraints using PCG diff --git a/cpp/testSymbolicBayesNet.cpp b/cpp/testSymbolicBayesNet.cpp index b26fa1816..766236fb3 100644 --- a/cpp/testSymbolicBayesNet.cpp +++ b/cpp/testSymbolicBayesNet.cpp @@ -19,6 +19,7 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ TEST( SymbolicBayesNet, constructor ) diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index f81d13aa8..2bb3b853a 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -19,6 +19,7 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ TEST( SymbolicFactorGraph, symbolicFactorGraph ) diff --git a/cpp/testVectorConfig.cpp b/cpp/testVectorConfig.cpp index 0c1cf23bc..947acd0ef 100644 --- a/cpp/testVectorConfig.cpp +++ b/cpp/testVectorConfig.cpp @@ -24,7 +24,8 @@ using namespace std; using namespace gtsam; - +using namespace example; + /* ************************************************************************* */ TEST( VectorConfig, equals1 ) { @@ -77,7 +78,7 @@ TEST( VectorConfig, contains) /* ************************************************************************* */ TEST( VectorConfig, expmap) { - VectorConfig c = createConfig(); + VectorConfig c = createVectorConfig(); Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2 CHECK(assert_equal(expmap(c,c),expmap(c,v))); } @@ -139,13 +140,13 @@ TEST( VectorConfig, update_with_large_delta) { /* ************************************************************************* */ TEST( VectorConfig, dot) { - VectorConfig c = createConfig(); + VectorConfig c = createVectorConfig(); DOUBLES_EQUAL(3.25,dot(c,c),1e-9); } /* ************************************************************************* */ TEST( VectorConfig, dim) { - VectorConfig c = createConfig(); + VectorConfig c = createVectorConfig(); LONGS_EQUAL(6,c.dim()); } diff --git a/cpp/timeGaussianFactorGraph.cpp b/cpp/timeGaussianFactorGraph.cpp index 6963ae05e..178836325 100644 --- a/cpp/timeGaussianFactorGraph.cpp +++ b/cpp/timeGaussianFactorGraph.cpp @@ -13,6 +13,7 @@ using namespace std; using namespace gtsam; +using namespace example; /* ************************************************************************* */ // Create a Kalman smoother for t=1:T and optimize