From acde4d99a5a96765a49f56960484eda73e6875c9 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Fri, 22 Oct 2010 01:46:33 +0000 Subject: [PATCH] Removed remaining references to denseQR, some fixes in NonlinearOptimizer --- examples/Makefile.am | 4 +- examples/Pose2SLAMExample_advanced.cpp | 3 +- linear/GaussianMultifrontalSolver.cpp | 2 +- linear/GaussianMultifrontalSolver.h | 2 +- linear/NoiseModel.cpp | 52 +++++++++++++++----------- nonlinear/LieValues.h | 1 - nonlinear/NonlinearOptimization-inl.h | 47 ++++++++++++++--------- nonlinear/NonlinearOptimizer-inl.h | 36 +++++++++--------- nonlinear/NonlinearOptimizer.h | 32 ++++++++-------- 9 files changed, 99 insertions(+), 80 deletions(-) diff --git a/examples/Makefile.am b/examples/Makefile.am index f623abbb5..592241333 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -16,8 +16,8 @@ noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface -noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver -noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver +#noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver +#noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver SUBDIRS = vSLAMexample # does not compile.... #---------------------------------------------------------------------------------------------------- # rules to build local programs diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 50799883d..0676be174 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -66,8 +66,7 @@ int main(int argc, char** argv) { Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial); /* 4.2.2 set up solver and optimize */ - Optimizer::shared_solver solver(new Optimizer::solver(ordering)); - Optimizer optimizer(graph, initial, solver); + Optimizer optimizer(graph, initial, ordering); Optimizer::Parameters::verbosityLevel verbosity = pose2SLAM::Optimizer::Parameters::SILENT; Optimizer optimizer_result = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity); diff --git a/linear/GaussianMultifrontalSolver.cpp b/linear/GaussianMultifrontalSolver.cpp index f09d754a5..06ea42b2e 100644 --- a/linear/GaussianMultifrontalSolver.cpp +++ b/linear/GaussianMultifrontalSolver.cpp @@ -16,7 +16,7 @@ GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph::sharedClique GaussianMultifrontalSolver::eliminate() const { +BayesTree::sharedClique GaussianMultifrontalSolver::eliminate() const { return junctionTree_->eliminate(); } diff --git a/linear/GaussianMultifrontalSolver.h b/linear/GaussianMultifrontalSolver.h index 28912a4b6..1fb8df845 100644 --- a/linear/GaussianMultifrontalSolver.h +++ b/linear/GaussianMultifrontalSolver.h @@ -56,7 +56,7 @@ public: * Eliminate the factor graph sequentially. Uses a column elimination tree * to recursively eliminate. */ - typename BayesTree::sharedClique eliminate() const; + BayesTree::sharedClique eliminate() const; /** * Compute the least-squares solution of the GaussianFactorGraph. This diff --git a/linear/NoiseModel.cpp b/linear/NoiseModel.cpp index b8e903ff2..20eb2fcf9 100644 --- a/linear/NoiseModel.cpp +++ b/linear/NoiseModel.cpp @@ -31,7 +31,6 @@ #include #include -#include namespace ublas = boost::numeric::ublas; typedef ublas::matrix_column column; @@ -220,23 +219,28 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional&> firs // General QR, see also special version in Constrained SharedDiagonal Gaussian::QRColumnWise(ublas::matrix& Ab, vector& firstZeroRows) const { - - // get size(A) and maxRank - // TODO: really no rank problems ? - size_t m = Ab.size1(), n = Ab.size2()-1; - size_t maxRank = min(m,n); - - // pre-whiten everything (cheaply if possible) - WhitenInPlace(Ab); - - // Perform in-place Householder -#ifdef GT_USE_LAPACK - householder_denseqr_colmajor(Ab, &firstZeroRows[0]); -#else - householder(Ab, maxRank); -#endif - - return Unit::Create(maxRank); + Matrix Abresult(Ab); + gtsam::print(Abresult, "Abresult before = "); + SharedDiagonal result = QR(Abresult, firstZeroRows); + gtsam::print(Abresult, "Abresult after = "); + Ab = Abresult; + return result; +// // get size(A) and maxRank +// // TODO: really no rank problems ? +// size_t m = Ab.size1(), n = Ab.size2()-1; +// size_t maxRank = min(m,n); +// +// // pre-whiten everything (cheaply if possible) +// WhitenInPlace(Ab); +// +// // Perform in-place Householder +//#ifdef GT_USE_LAPACK +// householder_denseqr_colmajor(Ab, &firstZeroRows[0]); +//#else +// householder(Ab, maxRank); +//#endif +// +// return Unit::Create(maxRank); } /* ************************************************************************* */ @@ -400,10 +404,14 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional&> f } SharedDiagonal Constrained::QRColumnWise(ublas::matrix& Ab, vector& firstZeroRows) const { - Matrix AbRowWise(Ab); - SharedDiagonal result = this->QR(AbRowWise, firstZeroRows); - Ab = AbRowWise; - return result; +// Matrix AbRowWise(Ab); +// SharedDiagonal result = this->QR(AbRowWise, firstZeroRows); +// Ab = AbRowWise; +// return result; + Matrix Abresult(Ab); + SharedDiagonal result = QR(Abresult, firstZeroRows); + Ab = Abresult; + return result; } /* ************************************************************************* */ diff --git a/nonlinear/LieValues.h b/nonlinear/LieValues.h index 2e19e92d5..cd9689eec 100644 --- a/nonlinear/LieValues.h +++ b/nonlinear/LieValues.h @@ -119,7 +119,6 @@ namespace gtsam { /** Add a delta config to current config and returns a new config */ LieValues expmap(const VectorValues& delta, const Ordering& ordering) const; - /** Get a delta config about a linearization point c0 (*this) */ VectorValues logmap(const LieValues& cp, const Ordering& ordering) const; diff --git a/nonlinear/NonlinearOptimization-inl.h b/nonlinear/NonlinearOptimization-inl.h index 53117acb7..7ab816c59 100644 --- a/nonlinear/NonlinearOptimization-inl.h +++ b/nonlinear/NonlinearOptimization-inl.h @@ -19,7 +19,8 @@ #pragma once -#include +#include +#include #include #include #include @@ -38,10 +39,9 @@ namespace gtsam { Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); // initial optimization state is the same in both cases tested - typedef NonlinearOptimizer Optimizer; - typename Optimizer::shared_solver solver(new Factorization(ordering)); + typedef NonlinearOptimizer Optimizer; Optimizer optimizer(boost::make_shared(graph), - boost::make_shared(initialEstimate), solver); + boost::make_shared(initialEstimate), ordering); // Levenberg-Marquardt Optimizer result = optimizer.levenbergMarquardt(parameters); @@ -53,28 +53,39 @@ namespace gtsam { */ template T optimizeMultiFrontal(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters) { - throw runtime_error("optimizeMultiFrontal: not implemented"); - } - /** - * The multifrontal solver - */ - template - T optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) { + // Use a variable ordering from COLAMD + Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); // initial optimization state is the same in both cases tested - typedef NonlinearOptimizer > SPCGOptimizer; - typename SPCGOptimizer::shared_solver solver(new SubgraphSolver(graph, initialEstimate)); - SPCGOptimizer optimizer( - boost::make_shared(graph), - boost::make_shared(initialEstimate), - solver); + typedef NonlinearOptimizer Optimizer; + Optimizer optimizer(boost::make_shared(graph), + boost::make_shared(initialEstimate), ordering); // Levenberg-Marquardt - SPCGOptimizer result = optimizer.levenbergMarquardt(parameters); + Optimizer result = optimizer.levenbergMarquardt(parameters); return *result.config(); } +// /** +// * The multifrontal solver +// */ +// template +// T optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) { +// +// // initial optimization state is the same in both cases tested +// typedef NonlinearOptimizer > SPCGOptimizer; +// typename SPCGOptimizer::shared_solver solver(new SubgraphSolver(graph, initialEstimate)); +// SPCGOptimizer optimizer( +// boost::make_shared(graph), +// boost::make_shared(initialEstimate), +// solver); +// +// // Levenberg-Marquardt +// SPCGOptimizer result = optimizer.levenbergMarquardt(parameters); +// return *result.config(); +// } + /** * optimization that returns the values */ diff --git a/nonlinear/NonlinearOptimizer-inl.h b/nonlinear/NonlinearOptimizer-inl.h index eaa8482ac..a9663b03b 100644 --- a/nonlinear/NonlinearOptimizer-inl.h +++ b/nonlinear/NonlinearOptimizer-inl.h @@ -74,14 +74,14 @@ namespace gtsam { /* ************************************************************************* */ template NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, - shared_values config, shared_solver solver, double lambda) : - graph_(graph), config_(config), lambda_(lambda), solver_(solver) { + shared_values config, shared_ordering ordering, double lambda) : + graph_(graph), config_(config), ordering_(ordering), lambda_(lambda) { if (!graph) throw std::invalid_argument( "NonlinearOptimizer constructor: graph = NULL"); if (!config) throw std::invalid_argument( "NonlinearOptimizer constructor: config = NULL"); - if (!solver) throw std::invalid_argument( - "NonlinearOptimizer constructor: solver = NULL"); + if (!ordering) throw std::invalid_argument( + "NonlinearOptimizer constructor: ordering = NULL"); error_ = graph->error(*config); } @@ -90,9 +90,9 @@ namespace gtsam { /* ************************************************************************* */ template VectorValues NonlinearOptimizer::linearizeAndOptimizeForDelta() const { - boost::shared_ptr linearized = solver_->linearize(*graph_, *config_); - NonlinearOptimizer prepared(graph_, config_, solver_->prepareLinear(*linearized), error_, lambda_); - return prepared.solver_->optimize(*linearized); + boost::shared_ptr linearized = graph_->linearize(*config_, *ordering_); + NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_); + return *S(*linearized).optimize(); } /* ************************************************************************* */ @@ -109,13 +109,13 @@ namespace gtsam { delta.print("delta"); // take old config and update it - shared_values newValues(new C(solver_->expmap(*config_, delta))); + shared_values newValues(new C(config_->expmap(delta, *ordering_))); // maybe show output if (verbosity >= Parameters::CONFIG) newValues->print("newValues"); - NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newValues, solver_, lambda_); + NonlinearOptimizer newOptimizer = NonlinearOptimizer(graph_, newValues, ordering_, lambda_); if (verbosity >= Parameters::ERROR) cout << "error: " << newOptimizer.error_ << endl; @@ -177,17 +177,17 @@ namespace gtsam { damped.print("damped"); // solve - VectorValues delta = solver_->optimize(damped); + VectorValues delta = *S(damped).optimize(); if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); // update config - shared_values newValues(new C(solver_->expmap(*config_, delta))); // TODO: updateValues + shared_values newValues(new C(config_->expmap(delta, *ordering_))); // TODO: updateValues // if (verbosity >= TRYCONFIG) // newValues->print("config"); // create new optimization state with more adventurous lambda - NonlinearOptimizer next(graph_, newValues, solver_, lambda_ / factor); + NonlinearOptimizer next(graph_, newValues, ordering_, lambda_ / factor); if (verbosity >= Parameters::TRYLAMBDA) cout << "next error = " << next.error_ << endl; if(lambdaMode >= Parameters::CAUTIOUS) { @@ -199,7 +199,7 @@ namespace gtsam { // If we're cautious, see if the current lambda is better // todo: include stopping criterion here? if(lambdaMode == Parameters::CAUTIOUS) { - NonlinearOptimizer sameLambda(graph_, newValues, solver_, lambda_); + NonlinearOptimizer sameLambda(graph_, newValues, ordering_, lambda_); if(sameLambda.error_ <= next.error_) return sameLambda; } @@ -212,7 +212,7 @@ namespace gtsam { // A more adventerous lambda was worse. If we're cautious, try the same lambda. if(lambdaMode == Parameters::CAUTIOUS) { - NonlinearOptimizer sameLambda(graph_, newValues, solver_, lambda_); + NonlinearOptimizer sameLambda(graph_, newValues, ordering_, lambda_); if(sameLambda.error_ <= error_) return sameLambda; } @@ -223,9 +223,9 @@ namespace gtsam { // TODO: can we avoid copying the config ? if(lambdaMode >= Parameters::BOUNDED && lambda_ >= 1.0e5) { - return NonlinearOptimizer(graph_, newValues, solver_, lambda_);; + return NonlinearOptimizer(graph_, newValues, ordering_, lambda_);; } else { - NonlinearOptimizer cautious(graph_, config_, solver_, lambda_ * factor); + NonlinearOptimizer cautious(graph_, config_, ordering_, lambda_ * factor); return cautious.try_lambda(linear, verbosity, factor, lambdaMode); } @@ -248,8 +248,8 @@ namespace gtsam { cout << "lambda = " << lambda_ << endl; // linearize all factors once - boost::shared_ptr linear = solver_->linearize(*graph_, *config_); - NonlinearOptimizer prepared(graph_, config_, solver_->prepareLinear(*linear), error_, lambda_); + boost::shared_ptr linear = graph_->linearize(*config_, *ordering_); + NonlinearOptimizer prepared(graph_, config_, ordering_, error_, lambda_); if (verbosity >= Parameters::LINEAR) linear->print("linear"); diff --git a/nonlinear/NonlinearOptimizer.h b/nonlinear/NonlinearOptimizer.h index cd296595d..c41204762 100644 --- a/nonlinear/NonlinearOptimizer.h +++ b/nonlinear/NonlinearOptimizer.h @@ -63,15 +63,16 @@ namespace gtsam { * * */ - template, class W = NullOptimizerWriter> + template class NonlinearOptimizer { public: // For performance reasons in recursion, we store configs in a shared_ptr typedef boost::shared_ptr shared_values; typedef boost::shared_ptr shared_graph; - typedef boost::shared_ptr shared_solver; - typedef const S solver; + typedef boost::shared_ptr shared_ordering; + //typedef boost::shared_ptr shared_solver; + //typedef const GS solver; typedef NonlinearOptimizationParameters Parameters; private: @@ -85,13 +86,14 @@ namespace gtsam { const shared_values config_; double error_; // TODO FD: no more const because in constructor I need to set it after checking :-( + const shared_ordering ordering_; + // the linear system solver + //const shared_solver solver_; + // keep current lambda for use within LM only // TODO: red flag, should we have an LM class ? const double lambda_; - // the linear system solver - const shared_solver solver_; - // Recursively try to do tempered Gauss-Newton steps until we succeed NonlinearOptimizer try_lambda(const L& linear, Parameters::verbosityLevel verbosity, double factor, Parameters::LambdaMode lambdaMode) const; @@ -101,22 +103,22 @@ namespace gtsam { /** * Constructor that evaluates new error */ - NonlinearOptimizer(shared_graph graph, shared_values config, shared_solver solver, + NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering, const double lambda = 1e-5); /** * Constructor that does not do any computation */ - NonlinearOptimizer(shared_graph graph, shared_values config, shared_solver solver, + NonlinearOptimizer(shared_graph graph, shared_values config, shared_ordering ordering, const double error, const double lambda): graph_(graph), config_(config), - error_(error), lambda_(lambda), solver_(solver) {} + error_(error), ordering_(ordering), lambda_(lambda) {} /** * Copy constructor */ - NonlinearOptimizer(const NonlinearOptimizer &optimizer) : + NonlinearOptimizer(const NonlinearOptimizer &optimizer) : graph_(optimizer.graph_), config_(optimizer.config_), - error_(optimizer.error_), lambda_(optimizer.lambda_), solver_(optimizer.solver_) {} + error_(optimizer.error_), ordering_(optimizer.ordering_), lambda_(optimizer.lambda_) {} /** * Return current error @@ -205,8 +207,8 @@ namespace gtsam { double relativeThreshold = 1e-5, absoluteThreshold = 1e-5; // initial optimization state is the same in both cases tested - shared_solver solver(new S(ordering)); - NonlinearOptimizer optimizer(graph, config, solver); + GS solver(*graph->linearize(*config, *ordering)); + NonlinearOptimizer optimizer(graph, config, ordering); // Levenberg-Marquardt NonlinearOptimizer result = optimizer.levenbergMarquardt(relativeThreshold, @@ -236,8 +238,8 @@ namespace gtsam { double relativeThreshold = 1e-5, absoluteThreshold = 1e-5; // initial optimization state is the same in both cases tested - shared_solver solver(new S(ordering)); - NonlinearOptimizer optimizer(graph, config, solver); + GS solver(*graph->linearize(*config, *ordering)); + NonlinearOptimizer optimizer(graph, config, ordering); // Gauss-Newton NonlinearOptimizer result = optimizer.gaussNewton(relativeThreshold,