From 7407843214751c09fe742a7c9544bf6139942349 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 10 Feb 2011 16:01:29 +0000 Subject: [PATCH] Reworked nonlinear optimizer so that it only uses a solver member variable if you are using spcg. SPCG may be broken at this point, and its member variable will be removed soon. --- gtsam/nonlinear/NonlinearOptimizer-inl.h | 190 +------ gtsam/nonlinear/NonlinearOptimizer.h | 641 +++++++++++------------ tests/testNonlinearOptimizer.cpp | 39 -- 3 files changed, 327 insertions(+), 543 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index be57f0870..dc82dec65 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -31,8 +31,6 @@ using namespace std; namespace gtsam { - /* ************************************************************************* */ - /* ************************************************************************* */ template NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, @@ -47,50 +45,37 @@ namespace gtsam { "NonlinearOptimizer constructor: ordering = NULL"); } + /* ************************************************************************* */ template - NonlinearOptimizer::NonlinearOptimizer( - shared_graph graph, - shared_values values, - shared_ordering ordering, - shared_solver solver, - shared_parameters parameters): - graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), solver_(solver), - parameters_(parameters), iterations_(0), dimensions_(new vector(values->dims(*ordering))) { + NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, + shared_values values, shared_ordering ordering, + shared_solver spcg_solver, shared_parameters parameters) : + graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), + parameters_(parameters), iterations_(0), + dimensions_(new vector(values->dims(*ordering))), + spcg_solver_(spcg_solver) { if (!graph) throw std::invalid_argument( "NonlinearOptimizer constructor: graph = NULL"); if (!values) throw std::invalid_argument( "NonlinearOptimizer constructor: values = NULL"); - if (!ordering) throw std::invalid_argument( - "NonlinearOptimizer constructor: ordering = NULL"); - if (!solver) throw std::invalid_argument( - "NonlinearOptimizer constructor: solver = NULL"); - } - - /* ************************************************************************* */ - // linearize and optimize - /* ************************************************************************* */ - template - VectorValues NonlinearOptimizer::linearizeAndOptimizeForDelta() const { - boost::shared_ptr linearized = graph_->linearize(*values_, *ordering_)->template dynamicCastFactors(); -// NonlinearOptimizer prepared(graph_, values_, ordering_, error_, lambda_); - return *S(*linearized).optimize(); + if (!spcg_solver) throw std::invalid_argument( + "NonlinearOptimizer constructor: spcg_solver = NULL"); } /* ************************************************************************* */ // One iteration of Gauss Newton /* ************************************************************************* */ - template NonlinearOptimizer NonlinearOptimizer::iterate() const { Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; - boost::shared_ptr linearized = graph_->linearize(*values_, *ordering_)->template dynamicCastFactors(); - shared_solver newSolver = solver_; - if(newSolver) newSolver->replaceFactors(linearized); - else newSolver.reset(new S(*linearized)); + // FIXME: allow for passing variable index through createSolver() + // FIXME: get rid of spcg solver + if (spcg_solver_) spcg_solver_->replaceFactors(linearize()); + shared_solver solver = (spcg_solver_) ? spcg_solver_ : createSolver(); - VectorValues delta = *newSolver->optimize(); + VectorValues delta = *solver->optimize(); // maybe show output if (verbosity >= Parameters::DELTA) delta.print("delta"); @@ -101,14 +86,13 @@ namespace gtsam { // maybe show output if (verbosity >= Parameters::VALUES) newValues->print("newValues"); - NonlinearOptimizer newOptimizer = newValuesSolver_(newValues, newSolver); + NonlinearOptimizer newOptimizer = newValues_(newValues); if (verbosity >= Parameters::ERROR) cout << "error: " << newOptimizer.error_ << endl; return newOptimizer; } /* ************************************************************************* */ - template NonlinearOptimizer NonlinearOptimizer::gaussNewton() const { static W writer(error_); @@ -157,6 +141,7 @@ namespace gtsam { if (verbosity >= Parameters::TRYLAMBDA) cout << "trying lambda = " << lambda << endl; // add prior-factors + // TODO: replace this dampening with a backsubstitution approach typename L::shared_ptr damped(new L(linear)); { double sigma = 1.0 / sqrt(lambda); @@ -174,14 +159,15 @@ namespace gtsam { if (verbosity >= Parameters::DAMPED) damped->print("damped"); // solve - if(solver_) solver_->replaceFactors(damped); - else solver_.reset(new S(*damped)); - - VectorValues delta = *solver_->optimize(); + // FIXME: incorporate variable index + // FIXME: remove spcg specific code + if (spcg_solver_) spcg_solver_->replaceFactors(damped); + shared_solver solver = (spcg_solver_) ? spcg_solver_ : shared_solver(new S(*damped)); + VectorValues delta = *solver->optimize(); if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); // update values - shared_values newValues(new C(values_->expmap(delta, *ordering_))); // TODO: updateValues + shared_values newValues(new C(values_->expmap(delta, *ordering_))); // create new optimization state with more adventurous lambda double error = graph_->error(*newValues); @@ -212,7 +198,6 @@ namespace gtsam { /* ************************************************************************* */ // One iteration of Levenberg Marquardt /* ************************************************************************* */ - template NonlinearOptimizer NonlinearOptimizer::iterateLM() { @@ -273,133 +258,4 @@ namespace gtsam { iterations_++; } } - - /* ************************************************************************* */ - // New version of functional approach - /* ************************************************************************* */ - - /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::try_lambdaRecursive(const L& linear) const { - - const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; - double lambda = parameters_->lambda_ ; - const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ; - const double factor = parameters_->lambdaFactor_ ; - - if( lambdaMode >= Parameters::CAUTIOUS) throw runtime_error("CAUTIOUS mode not working yet, please use BOUNDED."); - - double next_error = error_; - - shared_values next_values = values_; - shared_solver solver = solver_; - - while(true) { - if (verbosity >= Parameters::TRYLAMBDA) cout << "trying lambda = " << lambda << endl; - - // add prior-factors - typename L::shared_ptr damped(new L(linear)); - { - double sigma = 1.0 / sqrt(lambda); - damped->reserve(damped->size() + dimensions_->size()); - // for each of the variables, add a prior - for(Index j=0; jsize(); ++j) { - size_t dim = (*dimensions_)[j]; - Matrix A = eye(dim); - Vector b = zero(dim); - SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma); - GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model)); - damped->push_back(prior); - } - } - if (verbosity >= Parameters::DAMPED) damped->print("damped"); - - // solve - solver.reset(new S(*damped)); - - VectorValues delta = *solver->optimize(); - if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); - - // update values - shared_values newValues(new C(values_->expmap(delta, *ordering_))); // TODO: updateValues - - // create new optimization state with more adventurous lambda - double error = graph_->error(*newValues); - - if (verbosity >= Parameters::TRYLAMBDA) cout << "next error = " << error << endl; - - if( error <= error_ ) { - next_values = newValues; - next_error = error; - lambda /= factor; - break; - } else { - // Either we're not cautious, or the same lambda was worse than the current error. - // The more adventurous lambda was worse too, so make lambda more conservative - // and keep the same values. - if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) { - break; - } else { - lambda *= factor; - } - } - } // end while - - return NonlinearOptimizer(graph_, next_values, next_error, ordering_, solver, - parameters_->newLambda_(lambda), dimensions_, iterations_); - } - - /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::iterateLMRecursive() const { - - const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; - const double lambda = parameters_->lambda_ ; - - // show output - if (verbosity >= Parameters::VALUES) values_->print("values"); - if (verbosity >= Parameters::ERROR) cout << "error: " << error_ << endl; - if (verbosity >= Parameters::LAMBDA) cout << "lambda = " << lambda << endl; - - // linearize all factors once - boost::shared_ptr linear = graph_->linearize(*values_, *ordering_)->template dynamicCastFactors(); - - if (verbosity >= Parameters::LINEAR) linear->print("linear"); - - // try lambda steps with successively larger lambda until we achieve descent - if (verbosity >= Parameters::LAMBDA) cout << "Trying Lambda for the first time" << endl; - - return try_lambdaRecursive(*linear); - } - - /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::levenbergMarquardtRecursive() const { - - const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; - - // check if we're already close enough - if (error_ < parameters_->sumError_) { - if ( verbosity >= Parameters::ERROR ) - cout << "Exiting, as sumError = " << error_ << " < " << parameters_->sumError_ << endl; - return *this; - } - - // do one iteration of LM - NonlinearOptimizer next = iterateLMRecursive(); - - // check convergence - // TODO: move convergence checks here and incorporate in verbosity levels - bool converged = gtsam::check_convergence(*parameters_, error_, next.error_); - - if(iterations_ >= parameters_->maxIterations_ || converged) { - if (verbosity >= Parameters::VALUES) values_->print("final values"); - if (verbosity >= Parameters::ERROR) cout << "final error: " << error_ << endl; - if (verbosity >= Parameters::LAMBDA) cout << "final lambda = " << lambda() << endl; - return *this; - } else { - return next.newIterations_(iterations_ + 1).levenbergMarquardtRecursive(); - } - } - } diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index d9300b6a2..4801a8ceb 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -29,359 +29,326 @@ namespace gtsam { - class NullOptimizerWriter { - public: - NullOptimizerWriter(double error) {} - virtual void write(double error) {} - }; +class NullOptimizerWriter { +public: + NullOptimizerWriter(double error) {} + virtual void write(double error) {} +}; + +/** + * The class NonlinearOptimizer encapsulates an optimization state. + * Typically it is instantiated with a NonlinearFactorGraph and initial values + * and then one of the optimization routines is called. These iterate + * until convergence. All methods are functional and return a new state. + * + * The class is parameterized by the Graph type $G$, Values class type $T$, + * linear system class $L$, the non linear solver type $S$, and the writer type $W$ + * + * The values class type $T$ is in order to be able to optimize over non-vector values structures. + * + * A nonlinear system solver $S$ + * Concept NonLinearSolver implements + * linearize: G * T -> L + * solve : L -> T + * + * The writer $W$ generates output to disk or the screen. + * + * For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values, + * $L$ can be GaussianFactorGraph and $S$ can be Factorization. + * The solver class has two main functions: linearize and optimize. The first one + * linearizes the nonlinear cost function around the current estimate, and the second + * one optimizes the linearized system using various methods. + * + * To use the optimizer in code, include in your cpp file + * + * + */ +template +class NonlinearOptimizer { +public: + + // For performance reasons in recursion, we store values in a shared_ptr + typedef boost::shared_ptr shared_values; + typedef boost::shared_ptr shared_graph; + typedef boost::shared_ptr shared_linear; + typedef boost::shared_ptr shared_ordering; + typedef boost::shared_ptr shared_solver; + typedef NonlinearOptimizationParameters Parameters; + typedef boost::shared_ptr shared_parameters ; + +private: + + typedef NonlinearOptimizer This; + typedef boost::shared_ptr > shared_dimensions; + + // keep a reference to const version of the graph + // These normally do not change + const shared_graph graph_; + + // keep a values structure and its error + // These typically change once per iteration (in a functional way) + shared_values values_; + + // current error for this state + double error_; + + // the variable ordering + const shared_ordering ordering_; + + // storage for parameters, lambda, thresholds, etc. + shared_parameters parameters_; + + // for performance track + size_t iterations_; + + // The dimensions of each linearized variable + const shared_dimensions dimensions_; + + // solver used only for SPCG + // FIXME: remove this! + shared_solver spcg_solver_; /** - * The class NonlinearOptimizer encapsulates an optimization state. - * Typically it is instantiated with a NonlinearFactorGraph and an initial values - * and then one of the optimization routines is called. These recursively iterate - * until convergence. All methods are functional and return a new state. - * - * The class is parameterized by the Graph type $G$, Values class type $T$, - * linear system class $L$, the non linear solver type $S$, and the writer type $W$ - * - * The values class type $T$ is in order to be able to optimize over non-vector values structures. - * - * A nonlinear system solver $S$ - * Concept NonLinearSolver implements - * linearize: G * T -> L - * solve : L -> T - * - * The writer $W$ generates output to disk or the screen. - * - * For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values, - * $L$ can be GaussianFactorGraph and $S$ can be Factorization. - * The solver class has two main functions: linearize and optimize. The first one - * linearizes the nonlinear cost function around the current estimate, and the second - * one optimizes the linearized system using various methods. - * - * To use the optimizer in code, include in your cpp file - * - * + * Constructor that does not do any computation */ - template - class NonlinearOptimizer { - public: - - // For performance reasons in recursion, we store values in a shared_ptr - typedef boost::shared_ptr shared_values; - typedef boost::shared_ptr shared_graph; - typedef boost::shared_ptr shared_ordering; - typedef boost::shared_ptr shared_solver; - typedef NonlinearOptimizationParameters Parameters; - typedef boost::shared_ptr shared_parameters ; - - private: - - typedef NonlinearOptimizer This; - typedef boost::shared_ptr > shared_dimensions; - - // keep a reference to const version of the graph - // These normally do not change - const shared_graph graph_; - - // keep a values structure and its error - // These typically change once per iteration (in a functional way) - shared_values values_; - double error_; - - // the variable ordering - const shared_ordering ordering_; - - // the linear system solver - shared_solver solver_; - - shared_parameters parameters_; - - // for performance track - size_t iterations_; - -// // keep current lambda for use within LM only -// // TODO: red flag, should we have an LM class ? -// const double lambda_; - - // The dimensions of each linearized variable - const shared_dimensions dimensions_; - - /** - * Constructor that does not do any computation - */ -// NonlinearOptimizer(shared_graph graph, shared_values values, const double error, shared_ordering ordering, -// shared_solver solver, const double lambda, shared_dimensions dimensions): graph_(graph), values_(values), -// error_(error), ordering_(ordering), solver_(solver), lambda_(lambda), dimensions_(dimensions) {} - NonlinearOptimizer(shared_graph graph, shared_values values, const double error, shared_ordering ordering, - shared_solver solver, shared_parameters parameters, shared_dimensions dimensions, size_t iterations): graph_(graph), values_(values), - error_(error), ordering_(ordering), solver_(solver), parameters_(parameters), iterations_(iterations), dimensions_(dimensions) {} + shared_parameters parameters, shared_dimensions dimensions, size_t iterations): graph_(graph), values_(values), + error_(error), ordering_(ordering), parameters_(parameters), iterations_(iterations), dimensions_(dimensions) {} - /** Create a new NonlinearOptimizer with a different lambda */ - This newValuesSolver_(shared_values newValues, shared_solver newSolver) const { - return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, newSolver, parameters_, dimensions_, iterations_); } + /** constructors to replace specific components */ - This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const { - return NonlinearOptimizer(graph_, newValues, newError, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_, iterations_); } + This newValues_(shared_values newValues) const { + return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, parameters_, dimensions_, iterations_); } - This newIterations_(int iterations) const { - return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_, dimensions_, iterations); } + This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const { + return NonlinearOptimizer(graph_, newValues, newError, ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); } - /* - This newLambda_(double newLambda) const { - return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_); } + This newIterations_(int iterations) const { + return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters_, dimensions_, iterations); } - This newValuesSolver_(shared_values newValues, shared_solver newSolver) const { - return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, newSolver, parameters_, dimensions_); } + This newLambda_(double newLambda) const { + return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); } - This newValuesSolverLambda_(shared_values newValues, shared_solver newSolver, double newLambda) const { - return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, newSolver, parameters_->newLambda_(newLambda), dimensions_); } + This newValuesLambda_(shared_values newValues, double newLambda) const { + return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); } - This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const { - return NonlinearOptimizer(graph_, newValues, newError, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_); } + This newParameters_(shared_parameters parameters) const { + return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters, dimensions_, iterations_); } - - This newVerbosity_(Parameters::verbosityLevel verbosity) const { - return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_->newVerbosity_(verbosity), dimensions_); } - - This newParameters_(shared_parameters parameters) const { - return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters, dimensions_); } - - This newMaxIterations_(int maxIterations) const { - return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_->newMaxIterations_(maxIterations), dimensions_); } - */ - - public: - /** - * Constructor that evaluates new error - */ - - // suggested constructors - NonlinearOptimizer(shared_graph graph, - shared_values values, - shared_ordering ordering, - shared_parameters parameters = boost::make_shared()); - - // suggested constructors - NonlinearOptimizer(shared_graph graph, - shared_values values, - shared_ordering ordering, - shared_solver solver, - shared_parameters parameters = boost::make_shared()); - - /** - * Copy constructor - */ -// NonlinearOptimizer(const NonlinearOptimizer &optimizer) : -// graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_), -// ordering_(optimizer.ordering_), solver_(optimizer.solver_), lambda_(optimizer.lambda_), dimensions_(optimizer.dimensions_) {} - - NonlinearOptimizer(const NonlinearOptimizer &optimizer) : - graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_), - ordering_(optimizer.ordering_), solver_(optimizer.solver_), - parameters_(optimizer.parameters_), iterations_(0), dimensions_(optimizer.dimensions_) {} - - /** - * Return current error - */ - double error() const { return error_; } - - /** - * Return current lambda - */ - double lambda() const { return parameters_->lambda_; } - - /** - * Return the values - */ - shared_values values() const { return values_; } - - /** - * Return the graph - */ - shared_graph graph() const { return graph_; } - - /** - * Return the itertions - */ - size_t iterations() const { return iterations_; } - - /** - * Return the solver - */ - shared_solver solver() const { return solver_; } - - /** - * Return the ordering - */ - shared_ordering ordering() const { return ordering_; } - - /** - * Return the parameters - */ - shared_parameters parameters() const { return parameters_; } - - /** - * Return mean and covariance on a single variable - */ - std::pair marginalCovariance(Symbol j) const { - return solver_->marginalCovariance((*ordering_)[j]); - } - - /** - * linearize and optimize - * This returns an VectorValues, i.e., vectors in tangent space of T - */ - VectorValues linearizeAndOptimizeForDelta() const; - - /** - * Do one Gauss-Newton iteration and return next state - */ - - // suggested interface - NonlinearOptimizer iterate() const; - - /** - * Optimize a solution for a non linear factor graph - * @param relativeTreshold - * @param absoluteTreshold - * @param verbosity Integer specifying how much output to provide - */ - - // suggested interface - NonlinearOptimizer gaussNewton() const; - - /** Recursively try to do tempered Gauss-Newton steps until we succeed */ - NonlinearOptimizer try_lambda(const L& linear); - - /** - * One iteration of Levenberg Marquardt - */ - - // suggested interface - NonlinearOptimizer iterateLM(); - - /** - * Optimize using Levenberg-Marquardt. Really Levenberg's - * algorithm at this moment, as we just add I*\lambda to Hessian - * H'H. The probabilistic explanation is very simple: every - * variable gets an extra Gaussian prior that biases staying at - * current value, with variance 1/lambda. This is done very easily - * (but perhaps wastefully) by adding a prior factor for each of - * the variables, after linearization. - * - * @param relativeThreshold - * @param absoluteThreshold - * @param verbosity Integer specifying how much output to provide - * @param lambdaFactor Factor by which to decrease/increase lambda - */ - - // suggested interface - NonlinearOptimizer levenbergMarquardt(); - - /** - * The following are const versions of the LM algorithm for comparison and - * testing - functions are largely identical, but maintain constness - */ - NonlinearOptimizer try_lambdaRecursive(const L& linear) const; - NonlinearOptimizer iterateLMRecursive() const; - NonlinearOptimizer levenbergMarquardtRecursive() const; - - /** - * Static interface to LM optimization using default ordering and thresholds - * @param graph Nonlinear factor graph to optimize - * @param values Initial values - * @param verbosity Integer specifying how much output to provide - * @return an optimized values structure - */ - - static shared_values optimizeLM(shared_graph graph, - shared_values values, - shared_parameters parameters = boost::make_shared()) { - - // Use a variable ordering from COLAMD - Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); - // initial optimization state is the same in both cases tested - //GS solver(*graph->linearize(*values, *ordering)); - - NonlinearOptimizer optimizer(graph, values, ordering, parameters); - NonlinearOptimizer result = optimizer.levenbergMarquardt(); - return result.values(); - } - - static shared_values optimizeLM(shared_graph graph, - shared_values values, - Parameters::verbosityLevel verbosity) - { - return optimizeLM(graph, values, Parameters::newVerbosity(verbosity)); - } - /** - * Static interface to LM optimization (no shared_ptr arguments) - see above - */ - - static shared_values optimizeLM(const G& graph, - const T& values, - const Parameters parameters = Parameters()) { - return optimizeLM(boost::make_shared(graph), - boost::make_shared(values), - boost::make_shared(parameters)); - } - - static shared_values optimizeLM(const G& graph, - const T& values, - Parameters::verbosityLevel verbosity) { - return optimizeLM(boost::make_shared(graph), - boost::make_shared(values), - verbosity); - } - - /** - * Static interface to GN optimization using default ordering and thresholds - * @param graph Nonlinear factor graph to optimize - * @param values Initial values - * @param verbosity Integer specifying how much output to provide - * @return an optimized values structure - */ - - // suggested interface - static shared_values optimizeGN(shared_graph graph, - shared_values values, - shared_parameters parameters = boost::make_shared()) { - - Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); - // initial optimization state is the same in both cases tested - GS solver(*graph->linearize(*values, *ordering)); - - NonlinearOptimizer optimizer(graph, values, ordering, parameters); - NonlinearOptimizer result = optimizer.gaussNewton(); - return result.values(); - } - - /** - * Static interface to GN optimization (no shared_ptr arguments) - see above - */ - - // suggested interface - static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) { - return optimizeGN(boost::make_shared(graph), - boost::make_shared(values), - boost::make_shared(parameters)); - } - }; +public: /** - * Check convergence + * Constructor that evaluates new error */ - bool check_convergence ( - double relativeErrorTreshold, - double absoluteErrorTreshold, - double errorThreshold, - double currentError, double newError, int verbosity); + NonlinearOptimizer(shared_graph graph, + shared_values values, + shared_ordering ordering, + shared_parameters parameters = boost::make_shared()); - bool check_convergence ( - const NonlinearOptimizationParameters ¶meters, - double currentError, double newError); + /** + * Constructor that takes a solver directly - only useful for SPCG + * FIXME: REMOVE THIS FUNCTION! + */ + NonlinearOptimizer(shared_graph graph, + shared_values values, + shared_ordering ordering, + shared_solver spcg_solver, + shared_parameters parameters = boost::make_shared()); + + /** + * Copy constructor + */ + NonlinearOptimizer(const NonlinearOptimizer &optimizer) : + graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_), + ordering_(optimizer.ordering_), parameters_(optimizer.parameters_), + iterations_(0), dimensions_(optimizer.dimensions_) {} + + // access to member variables + + /** Return current error */ + double error() const { return error_; } + + /** Return current lambda */ + double lambda() const { return parameters_->lambda_; } + + /** Return the values */ + shared_values values() const { return values_; } + + /** Return the graph */ + shared_graph graph() const { return graph_; } + + /** Return the itertions */ + size_t iterations() const { return iterations_; } + + /** Return the ordering */ + shared_ordering ordering() const { return ordering_; } + + /** Return the parameters */ + shared_parameters parameters() const { return parameters_; } + + /** + * Return a linearized graph at the current graph/values/ordering + */ + shared_linear linearize() const { + return graph_->linearize(*values_, *ordering_)->template dynamicCastFactors(); + } + + /** + * create a solver around the current graph/values + * NOTE: this will actually solve a linear system + */ + shared_solver createSolver() const { + shared_linear linearGraph = linearize(); + shared_solver solver(new GS(*linearGraph)); + return solver; + } + + /** + * Return mean and covariance on a single variable + */ + std::pair marginalCovariance(Symbol j) const { + return createSolver()->marginalCovariance((*ordering_)[j]); + } + + /** + * linearize and optimize + * This returns an VectorValues, i.e., vectors in tangent space of T + */ + VectorValues linearizeAndOptimizeForDelta() const { + return *createSolver()->optimize(); + } + + /** + * Do one Gauss-Newton iteration and return next state + * suggested interface + */ + NonlinearOptimizer iterate() const; + + /** + * Optimize a solution for a non linear factor graph + * @param relativeTreshold + * @param absoluteTreshold + * @param verbosity Integer specifying how much output to provide + */ + + // suggested interface + NonlinearOptimizer gaussNewton() const; + + /** Recursively try to do tempered Gauss-Newton steps until we succeed */ + NonlinearOptimizer try_lambda(const L& linear); + + /** + * One iteration of Levenberg Marquardt + */ + NonlinearOptimizer iterateLM(); + + /** + * Optimize using Levenberg-Marquardt. Really Levenberg's + * algorithm at this moment, as we just add I*\lambda to Hessian + * H'H. The probabilistic explanation is very simple: every + * variable gets an extra Gaussian prior that biases staying at + * current value, with variance 1/lambda. This is done very easily + * (but perhaps wastefully) by adding a prior factor for each of + * the variables, after linearization. + * + * @param relativeThreshold + * @param absoluteThreshold + * @param verbosity Integer specifying how much output to provide + * @param lambdaFactor Factor by which to decrease/increase lambda + */ + NonlinearOptimizer levenbergMarquardt(); + + // static interfaces to LM and GN optimization techniques + + /** + * Static interface to LM optimization using default ordering and thresholds + * @param graph Nonlinear factor graph to optimize + * @param values Initial values + * @param verbosity Integer specifying how much output to provide + * @return an optimized values structure + */ + static shared_values optimizeLM(shared_graph graph, + shared_values values, + shared_parameters parameters = boost::make_shared()) { + + // Use a variable ordering from COLAMD + Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); + // initial optimization state is the same in both cases tested + //GS solver(*graph->linearize(*values, *ordering)); + + NonlinearOptimizer optimizer(graph, values, ordering, parameters); + NonlinearOptimizer result = optimizer.levenbergMarquardt(); + return result.values(); + } + + static shared_values optimizeLM(shared_graph graph, + shared_values values, + Parameters::verbosityLevel verbosity) + { + return optimizeLM(graph, values, Parameters::newVerbosity(verbosity)); + } + + /** + * Static interface to LM optimization (no shared_ptr arguments) - see above + */ + static shared_values optimizeLM(const G& graph, + const T& values, + const Parameters parameters = Parameters()) { + return optimizeLM(boost::make_shared(graph), + boost::make_shared(values), + boost::make_shared(parameters)); + } + + static shared_values optimizeLM(const G& graph, + const T& values, + Parameters::verbosityLevel verbosity) { + return optimizeLM(boost::make_shared(graph), + boost::make_shared(values), + verbosity); + } + + /** + * Static interface to GN optimization using default ordering and thresholds + * @param graph Nonlinear factor graph to optimize + * @param values Initial values + * @param verbosity Integer specifying how much output to provide + * @return an optimized values structure + */ + static shared_values optimizeGN(shared_graph graph, + shared_values values, + shared_parameters parameters = boost::make_shared()) { + + Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); + // initial optimization state is the same in both cases tested + GS solver(*graph->linearize(*values, *ordering)); + + NonlinearOptimizer optimizer(graph, values, ordering, parameters); + NonlinearOptimizer result = optimizer.gaussNewton(); + return result.values(); + } + + /** + * Static interface to GN optimization (no shared_ptr arguments) - see above + */ + static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) { + return optimizeGN(boost::make_shared(graph), + boost::make_shared(values), + boost::make_shared(parameters)); + } +}; + +/** + * Check convergence + */ +bool check_convergence ( + double relativeErrorTreshold, + double absoluteErrorTreshold, + double errorThreshold, + double currentError, double newError, int verbosity); + +bool check_convergence ( + const NonlinearOptimizationParameters ¶meters, + double currentError, double newError); } // gtsam diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 0e74ee996..4eeac4965 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -177,45 +177,6 @@ TEST( NonlinearOptimizer, optimize ) DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol); } -/* ************************************************************************* */ -TEST( NonlinearOptimizer, optimize_LM_recursive ) -{ - shared_ptr fg(new example::Graph( - example::createReallyNonlinearFactorGraph())); - - // test error at minimum - Point2 xstar(0,0); - example::Values cstar; - cstar.insert(simulated2D::PoseKey(1), xstar); - EXPECT_DOUBLES_EQUAL(0.0,fg->error(cstar),0.0); - - // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = - Point2 x0(3,3); - boost::shared_ptr c0(new example::Values); - c0->insert(simulated2D::PoseKey(1), x0); - EXPECT_DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3); - - // optimize parameters - shared_ptr ord(new Ordering()); - ord->push_back("x1"); - - // initial optimization state is the same in both cases tested - boost::shared_ptr params = boost::make_shared(); - params->relDecrease_ = 1e-5; - params->absDecrease_ = 1e-5; - Optimizer optimizer(fg, c0, ord, params); - - // Levenberg-Marquardt - Optimizer actual2 = optimizer.levenbergMarquardtRecursive(); - EXPECT_DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol); - - // calculate the marginal - Matrix actualCovariance; Vector mean; - boost::tie(mean, actualCovariance) = actual2.marginalCovariance("x1"); - Matrix expectedCovariance = Matrix_(2,2, 8.60817108, 0.0, 0.0, 0.01); - EXPECT(assert_equal(expectedCovariance, actualCovariance, tol)); -} - /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleLMOptimizer ) {