From 807cffbd6133cefb82ae57ca7c0d9bf6c7ccdfef Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 23 Jan 2010 00:57:54 +0000 Subject: [PATCH] Pose2SLAMOptimizer prototype for use in MATLAB --- .cproject | 2 +- .project | 2 +- cpp/GaussianFactorGraph.cpp | 6 ++ cpp/GaussianFactorGraph.h | 9 ++- cpp/Makefile.am | 4 +- cpp/NoiseModel.cpp | 4 +- cpp/NonlinearOptimizer-inl.h | 10 +-- cpp/Pose2SLAMOptimizer.cpp | 51 +++++++++++++++ cpp/Pose2SLAMOptimizer.h | 109 +++++++++++++++++++++++++++++++ cpp/SubgraphPreconditioner-inl.h | 24 +++++-- cpp/SubgraphPreconditioner.cpp | 4 +- cpp/SubgraphPreconditioner.h | 20 ++++-- cpp/gtsam.h | 7 ++ cpp/testPose2SLAM.cpp | 37 ++++++++--- 14 files changed, 253 insertions(+), 36 deletions(-) create mode 100644 cpp/Pose2SLAMOptimizer.cpp create mode 100644 cpp/Pose2SLAMOptimizer.h diff --git a/.cproject b/.cproject index 635dd1458..fcf87c89d 100644 --- a/.cproject +++ b/.cproject @@ -20,7 +20,7 @@ - + diff --git a/.project b/.project index 893b1174b..c10203bc2 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - -j2 + org.eclipse.cdt.make.core.buildCommand diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 3c5e7b96f..cef54d4f1 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -196,6 +196,12 @@ Errors GaussianFactorGraph::rhs() const { return e; } +/* ************************************************************************* */ +Vector GaussianFactorGraph::rhsVector() const { + Vector v; + return v; +} + /* ************************************************************************* */ pair GaussianFactorGraph::matrix(const Ordering& ordering) const { diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 42d7c38d8..11135f7d3 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -164,6 +164,11 @@ namespace gtsam { */ Errors rhs() const; + /** + * Return RHS (b./sigmas) as Vector + */ + Vector rhsVector() const; + /** * Return (dense) matrix associated with factor graph * @param ordering of variables needed for matrix column order @@ -245,8 +250,8 @@ namespace gtsam { /** * linearize the non-linear graph around the current config */ - GaussianFactorGraph linearize(const NonlinearGraph& g, const Config& config) const { - return g.linearize(config); + boost::shared_ptr linearize(const NonlinearGraph& g, const Config& config) const { + return g.linearize_(config); } }; } diff --git a/cpp/Makefile.am b/cpp/Makefile.am index e2ee61518..2ecc6d84e 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -187,8 +187,8 @@ headers += BetweenFactor.h PriorFactor.h headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h # 2D Pose SLAM -headers += dataset.h -sources += pose2SLAM.cpp dataset.cpp +headers += +sources += pose2SLAM.cpp pose2SLAMOptimizer.cpp dataset.cpp check_PROGRAMS += testPose2Factor testPose2Config testPose2SLAM testPose2Prior testPose2Prior_SOURCES = testPose2Prior.cpp testPose2Prior_LDADD = libgtsam.la diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp index b7eae2931..3ff5c8186 100644 --- a/cpp/NoiseModel.cpp +++ b/cpp/NoiseModel.cpp @@ -262,17 +262,19 @@ namespace gtsam { // Write back result in Ab, imperative as we are // TODO: test that is correct if a column was skipped !!!! size_t i = 0; // start with first row + bool mixed = false; BOOST_FOREACH(const Triple& t, Rd) { const size_t& j = t.get<0>(); const Vector& rd = t.get<1>(); precisions(i) = t.get<2>(); + if (precisions(i)==inf) mixed = true; for (size_t j2=0; j2 VectorConfig NonlinearOptimizer::linearizeAndOptimizeForDelta() const { - L linearized = solver_->linearize(*graph_, *config_); - return solver_->optimize(linearized); + boost::shared_ptr linearized = solver_->linearize(*graph_, *config_); + return solver_->optimize(*linearized); } /* ************************************************************************* */ @@ -170,12 +170,12 @@ namespace gtsam { cout << "lambda = " << lambda_ << endl; // linearize all factors once - L linear = graph_->linearize(*config_); + boost::shared_ptr linear = solver_->linearize(*graph_, *config_); if (verbosity >= LINEAR) - linear.print("linear"); + linear->print("linear"); // try lambda steps with successively larger lambda until we achieve descent - return try_lambda(linear, verbosity, lambdaFactor); + return try_lambda(*linear, verbosity, lambdaFactor); } /* ************************************************************************* */ diff --git a/cpp/Pose2SLAMOptimizer.cpp b/cpp/Pose2SLAMOptimizer.cpp new file mode 100644 index 000000000..f72515e59 --- /dev/null +++ b/cpp/Pose2SLAMOptimizer.cpp @@ -0,0 +1,51 @@ +/* + * Pose2SLAMOptimizer.cpp + * + * Created on: Jan 22, 2010 + * Author: dellaert + */ + +#include "Pose2SLAMOptimizer.h" +#include "pose2SLAM.h" +#include "dataset.h" +#include "SubgraphPreconditioner-inl.h" + +using namespace std; +namespace gtsam { + + using namespace pose2SLAM; + + /* ************************************************************************* */ + Pose2SLAMOptimizer::Pose2SLAMOptimizer(const string& dataset_name, + const string& path) { + + static int maxID = 0; + static bool addNoise = false; + + string filename; + boost::optional noiseModel; + boost::tie(filename, noiseModel) = dataset(dataset_name); + + // read graph and initial estimate + boost::tie(graph_, theta_) = load2D(filename, maxID, noiseModel, addNoise); + graph_->addPrior(theta_->begin()->first, theta_->begin()->second, + noiseModel::Unit::Create(3)); + + // initialize non-linear solver + solver_.initialize(*graph_, *theta_); + } + + /* ************************************************************************* */ + void Pose2SLAMOptimizer::update(const Vector& x) { + VectorConfig X; // TODO + update(X); + } + + /* ************************************************************************* */ + void Pose2SLAMOptimizer::updatePreconditioned(const Vector& y) { + Vector x; + update(x); + } + +/* ************************************************************************* */ +} diff --git a/cpp/Pose2SLAMOptimizer.h b/cpp/Pose2SLAMOptimizer.h new file mode 100644 index 000000000..342c37ad3 --- /dev/null +++ b/cpp/Pose2SLAMOptimizer.h @@ -0,0 +1,109 @@ +/* + * Pose2SLAMOptimizer.h + * + * Created on: Jan 22, 2010 + * Author: dellaert + */ + +#ifndef POSE2SLAMOPTIMIZER_H_ +#define POSE2SLAMOPTIMIZER_H_ + +#include "pose2SLAM.h" +#include "Ordering.h" +#include "SubgraphPreconditioner.h" + +namespace gtsam { + + /** + * Optimizer class for use in MATLAB + * Keeps a Pose2Config estimate + * Returns all relevant matrices so MATLAB can optimize :-) + */ + class Pose2SLAMOptimizer { + private: + + /** Non-linear factor graph */ + boost::shared_ptr graph_; + + /** Current non-linear estimate */ + boost::shared_ptr theta_; + + /** Non-linear solver */ + typedef SubgraphPCG SPCG_Solver; + SPCG_Solver solver_; + + /** Linear Solver */ + boost::shared_ptr system_; + + public: + /** + * Create optimizer: finds spanning tree and ordering + */ + Pose2SLAMOptimizer(const std::string& dataset = "", + const std::string& path = ""); + + /** + * Virtual destructor + */ + virtual ~Pose2SLAMOptimizer() { + } + + /** + * return graph pointer + */ + boost::shared_ptr graph() const { + return graph_; + } + /** + * linearize around current theta + */ + boost::shared_ptr theta() const { + return theta_; + } + /** + * linearize around current theta + */ + void linearize() { + system_ = solver_.linearize(*graph_, *theta_); + } + + /** + * update estimate with pure delta config x + */ + void update(const VectorConfig& x) { + // TODO instead of assigning can we create a new one and replace the shared ptr ? + *theta_ = expmap(*theta_, x); + linearize(); + } + + /** + * linearize around current theta: done at construction and at update + */ + void optimize() { + VectorConfig X = solver_.optimize(*system_); + update(X); + } + + /** + * Return matrices associated with optimization problem + * around current non-linear estimate theta + * Returns [IJS] sparse representation + */ + Matrix Ab1() const { return system_->Ab1(*solver_.ordering()); } + Matrix Ab2() const { return system_->Ab2(*solver_.ordering()); } + + /** + * update estimate with pure delta config x + */ + void update(const Vector& x); + + /** + * update estimate with pre-conditioned delta config y + */ + void updatePreconditioned(const Vector& y); + + }; + +} + +#endif /* POSE2SLAMOPTIMIZER_H_ */ diff --git a/cpp/SubgraphPreconditioner-inl.h b/cpp/SubgraphPreconditioner-inl.h index 889b8006a..425d48b75 100644 --- a/cpp/SubgraphPreconditioner-inl.h +++ b/cpp/SubgraphPreconditioner-inl.h @@ -21,8 +21,13 @@ namespace gtsam { /* ************************************************************************* */ template - SubgraphPCG::SubgraphPCG(const G& g, const T& theta0) : - maxIterations_(100), verbose_(false), epsilon_(1e-4), epsilon_abs_(1e-5) { + SubgraphPCG::SubgraphPCG(const G& g, const T& theta0) { + initialize(g,theta0); + } + + /* ************************************************************************* */ + template + void SubgraphPCG::initialize(const G& g, const T& theta0) { // generate spanning tree PredecessorMap tree = g.template findMinimumSpanningTree(); @@ -42,17 +47,22 @@ namespace gtsam { // compose the approximate solution Key root = keys.back(); theta_bar_ = composePoses (T_, tree, theta0[root]); - } /* ************************************************************************* */ template - SubgraphPreconditioner SubgraphPCG::linearize(const G& g, const T& theta_bar) const { - GaussianFactorGraph Ab1 = T_.linearize(theta_bar); + boost::shared_ptr SubgraphPCG::linearize(const G& g, const T& theta_bar) const { + SubgraphPreconditioner::sharedFG Ab1 = T_.linearize_(theta_bar); SubgraphPreconditioner::sharedFG Ab2 = C_.linearize_(theta_bar); - SubgraphPreconditioner::sharedBayesNet Rc1 = Ab1.eliminate_(*ordering_); +#ifdef TIMING + SubgraphPreconditioner::sharedBayesNet Rc1; + SubgraphPreconditioner::sharedConfig xbar; +#else + GaussianFactorGraph sacrificialAb1 = T_.linearize(theta_bar); // duplicate !!!!! + SubgraphPreconditioner::sharedBayesNet Rc1 = sacrificialAb1.eliminate_(*ordering_); SubgraphPreconditioner::sharedConfig xbar = gtsam::optimize_(*Rc1); - return SubgraphPreconditioner(Rc1, Ab2, xbar); +#endif + return boost::shared_ptr(new SubgraphPreconditioner(Ab1, Ab2, Rc1, xbar)); } /* ************************************************************************* */ diff --git a/cpp/SubgraphPreconditioner.cpp b/cpp/SubgraphPreconditioner.cpp index 843428340..2ee73f096 100644 --- a/cpp/SubgraphPreconditioner.cpp +++ b/cpp/SubgraphPreconditioner.cpp @@ -12,8 +12,8 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - SubgraphPreconditioner::SubgraphPreconditioner(sharedBayesNet& Rc1, sharedFG& Ab2, sharedConfig& xbar) : - Rc1_(Rc1), Ab2_(Ab2), xbar_(xbar), b2bar_(Ab2_->errors_(*xbar)) { + SubgraphPreconditioner::SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedConfig& xbar) : + Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(Ab2_->errors_(*xbar)) { } /* ************************************************************************* */ diff --git a/cpp/SubgraphPreconditioner.h b/cpp/SubgraphPreconditioner.h index ea0fc19ab..87b84a66a 100644 --- a/cpp/SubgraphPreconditioner.h +++ b/cpp/SubgraphPreconditioner.h @@ -29,8 +29,8 @@ namespace gtsam { typedef boost::shared_ptr sharedErrors; private: + sharedFG Ab1_, Ab2_; sharedBayesNet Rc1_; - sharedFG Ab2_; sharedConfig xbar_; sharedErrors b2bar_; /** b2 - A2*xbar */ @@ -42,7 +42,10 @@ namespace gtsam { * @param Ab2: the Graph A2*x=b2 * @param xbar: the solution to R1*x=c1 */ - SubgraphPreconditioner(sharedBayesNet& Rc1, sharedFG& Ab2, sharedConfig& xbar); + SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedConfig& xbar); + + Matrix Ab1(const Ordering& ordering) const { return Ab1_->sparse(ordering); } + Matrix Ab2(const Ordering& ordering) const { return Ab2_->sparse(ordering); } /* x = xbar + inv(R1)*y */ VectorConfig x(const VectorConfig& y) const; @@ -77,9 +80,10 @@ namespace gtsam { typedef typename G::Constraint Constraint; typedef typename G::Pose Pose; - const size_t maxIterations_; - const bool verbose_; - const double epsilon_, epsilon_abs_; + // TODO not hardcode + static const size_t maxIterations_=100; + static const bool verbose_=false; + static const double epsilon_=1e-4, epsilon_abs_=1e-5; /* the ordering derived from the spanning tree */ boost::shared_ptr ordering_; @@ -90,8 +94,12 @@ namespace gtsam { G T_, C_; public: + SubgraphPCG() {} + SubgraphPCG(const G& g, const T& theta0); + void initialize(const G& g, const T& theta0); + boost::shared_ptr ordering() const { return ordering_; } boost::shared_ptr theta_bar() const { return theta_bar_; } @@ -99,7 +107,7 @@ namespace gtsam { /** * linearize the non-linear graph around the current config and build the subgraph preconditioner systme */ - SubgraphPreconditioner linearize(const G& g, const T& theta_bar) const; + boost::shared_ptr linearize(const G& g, const T& theta_bar) const; /** * solve for the optimal displacement in the tangent space, and then solve diff --git a/cpp/gtsam.h b/cpp/gtsam.h index 3b2825271..a93a2c2e2 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -1,3 +1,10 @@ +class Pose2SLAMOptimizer { + Pose2SLAMOptimizer(string dataset_name); + Matrix Ab1() const; + Matrix Ab2() const; + void update(Vector x) const; +}; + class SharedGaussian { SharedGaussian(Matrix covariance); SharedGaussian(Vector sigmas); diff --git a/cpp/testPose2SLAM.cpp b/cpp/testPose2SLAM.cpp index 34aefd4b5..cc2c8518e 100644 --- a/cpp/testPose2SLAM.cpp +++ b/cpp/testPose2SLAM.cpp @@ -17,6 +17,7 @@ using namespace boost::assign; #include "FactorGraph-inl.h" #include "Ordering.h" #include "pose2SLAM.h" +#include "Pose2SLAMOptimizer.h" using namespace std; using namespace gtsam; @@ -143,15 +144,15 @@ TEST(Pose2Graph, optimizeCircle) { initial->insert(4, expmap(hexagon[4],Vector_(3, 0.1,-0.1, 0.1))); initial->insert(5, expmap(hexagon[5],Vector_(3,-0.1, 0.1,-0.1))); - // Choose an ordering and optimize + // Choose an ordering shared_ptr ordering(new Ordering); *ordering += "x0","x1","x2","x3","x4","x5"; - typedef NonlinearOptimizer Optimizer; - Optimizer::shared_solver solver(new Optimizer::solver(ordering)); - Optimizer optimizer0(fg, initial, solver); - Optimizer::verbosityLevel verbosity = Optimizer::SILENT; -// Optimizer::verbosityLevel verbosity = Optimizer::ERROR; - Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); + + // optimize + pose2SLAM::Optimizer::shared_solver solver(new pose2SLAM::Optimizer::solver(ordering)); + pose2SLAM::Optimizer optimizer0(fg, initial, solver); + pose2SLAM::Optimizer::verbosityLevel verbosity = pose2SLAM::Optimizer::SILENT; + pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity); Pose2Config actual = *optimizer.config(); @@ -160,10 +161,29 @@ TEST(Pose2Graph, optimizeCircle) { // Check loop closure CHECK(assert_equal(delta,between(actual[5],actual[0]))); + + // Try PCG class +// Pose2SLAMOptimizer myOptimizer("3"); + +// Matrix Ab1 = myOptimizer.Ab1(); +// CHECK(assert_equal(Matrix_(1,1,1.0),Ab1)); +// +// Matrix Ab2 = myOptimizer.Ab2(); +// CHECK(assert_equal(Matrix_(1,1,1.0),Ab2)); + + // Here, call matlab to + // A=[A1;A2], b=[b1;b2] + // R=qr(A1) + // call pcg on A,b, with preconditioner R -> get x + +// Vector x; +// myOptimizer.update(x); + + // Check with ground truth +// CHECK(assert_equal(hexagon, *myOptimizer.theta())); } /* ************************************************************************* */ -// test optimization with 6 poses arranged in a hexagon and a loop closure TEST(Pose2Graph, findMinimumSpanningTree) { Pose2Graph G, T, C; G.addConstraint(1, 2, Pose2(0.,0.,0.), I3); @@ -178,7 +198,6 @@ TEST(Pose2Graph, findMinimumSpanningTree) { } /* ************************************************************************* */ -// test optimization with 6 poses arranged in a hexagon and a loop closure TEST(Pose2Graph, split) { Pose2Graph G, T, C; G.addConstraint(1, 2, Pose2(0.,0.,0.), I3);