diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index 0b47dee17..f2f52b318 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -214,7 +214,7 @@ namespace gtsam { /* ************************************************************************* */ template - NonlinearOptimizer NonlinearOptimizer::iterateLM(){ + NonlinearOptimizer NonlinearOptimizer::iterateLM() { const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const double lambda = parameters_->lambda_ ; @@ -236,7 +236,6 @@ namespace gtsam { } /* ************************************************************************* */ - template NonlinearOptimizer NonlinearOptimizer::levenbergMarquardt() { @@ -274,6 +273,132 @@ 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_; + + 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 + S solver(*damped); // not solver_ !! + + 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 newValuesErrorLambda_(next_values, next_error, lambda); + } + + /* ************************************************************************* */ + 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 0313344d4..d9300b6a2 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -115,16 +115,18 @@ namespace gtsam { // 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): graph_(graph), values_(values), - error_(error), ordering_(ordering), solver_(solver), parameters_(parameters), dimensions_(dimensions) {} + 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) {} /** 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_); } + return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, newSolver, parameters_, dimensions_, iterations_); } This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const { - return NonlinearOptimizer(graph_, newValues, newError, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_); } + return NonlinearOptimizer(graph_, newValues, newError, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_, iterations_); } + This newIterations_(int iterations) const { + return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_, dimensions_, iterations); } /* This newLambda_(double newLambda) const { @@ -148,7 +150,7 @@ namespace gtsam { This newMaxIterations_(int maxIterations) const { return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_->newMaxIterations_(maxIterations), dimensions_); } - */ + */ public: /** @@ -278,6 +280,14 @@ namespace gtsam { // 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 diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 4eeac4965..b3ed87049 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -177,6 +177,39 @@ 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); + 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); + 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(); + DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol); +} + /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleLMOptimizer ) {