From 3366c673d76a1b22273578f9a180da2637f0f03f Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Thu, 21 Jun 2012 14:28:08 +0000 Subject: [PATCH] (With Duy) add a matlab wrapper for the LM optimizer and parameters in visualSLAM, have an example in SFMExample.m --- gtsam.h | 17 ++++++++++ gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 6 +++- gtsam/slam/tests/testVisualSLAM.cpp | 32 +++++++++++++++++++ gtsam/slam/visualSLAM.h | 11 +++++++ matlab/examples/SFMExample.m | 14 ++++++-- 5 files changed, 77 insertions(+), 3 deletions(-) diff --git a/gtsam.h b/gtsam.h index bafc15790..b1c3b8d02 100644 --- a/gtsam.h +++ b/gtsam.h @@ -777,6 +777,13 @@ class Marginals { Matrix marginalInformation(size_t variable) const; }; +#include +class LevenbergMarquardtParams { + LevenbergMarquardtParams(); + LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose); + void print(string s) const; +}; + }///\namespace gtsam //************************************************************************* @@ -1002,6 +1009,7 @@ class Graph { visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate, size_t verbosity) const; gtsam::Marginals marginals(const visualSLAM::Values& solution) const; + visualSLAM::LevenbergMarquardtOptimizer optimizer(const visualSLAM::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const; }; class ISAM { @@ -1026,4 +1034,13 @@ class ISAM { gtsam::NonlinearFactorGraph getFactorsUnsafe() const; }; +class LevenbergMarquardtOptimizer { + double lambda() const; + void iterate(); + visualSLAM::Values optimize(); + double error() const; + size_t iterations() const; + visualSLAM::Values values() const; +}; + }///\namespace visualSLAM diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 0858b72b9..231df9f4e 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -33,7 +33,7 @@ class LevenbergMarquardtParams : public SuccessiveLinearizationParams { public: /** See LevenbergMarquardtParams::lmVerbosity */ enum VerbosityLM { - SILENT, + SILENT = 0, LAMBDA, TRYLAMBDA, TRYCONFIG, @@ -49,6 +49,9 @@ public: LevenbergMarquardtParams() : lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), verbosityLM(SILENT) {} + LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose) : + lambdaInitial(initial), lambdaFactor(factor), lambdaUpperBound(bound), verbosityLM(VerbosityLM(verbose)) {} + virtual ~LevenbergMarquardtParams() {} virtual void print(const std::string& str = "") const { @@ -56,6 +59,7 @@ public: std::cout << " lambdaInitial: " << lambdaInitial << "\n"; std::cout << " lambdaFactor: " << lambdaFactor << "\n"; std::cout << " lambdaUpperBound: " << lambdaUpperBound << "\n"; + std::cout << " verbosityLM: " << verbosityLM << "\n"; std::cout.flush(); } }; diff --git a/gtsam/slam/tests/testVisualSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp index d9abc67ed..081142038 100644 --- a/gtsam/slam/tests/testVisualSLAM.cpp +++ b/gtsam/slam/tests/testVisualSLAM.cpp @@ -154,6 +154,38 @@ TEST( VisualSLAM, optimizeLM2) CHECK(assert_equal(initialEstimate, optimizer.values())); } +/* ************************************************************************* */ +TEST( VisualSLAM, LMoptimizer) +{ + // build a graph + visualSLAM::Graph graph(testGraph()); + // add 2 camera constraints + graph.addPoseConstraint(X(1), camera1); + graph.addPoseConstraint(X(2), camera2); + + // Create an initial values structure corresponding to the ground truth + Values initialEstimate; + initialEstimate.insert(X(1), camera1); + initialEstimate.insert(X(2), camera2); + initialEstimate.insert(L(1), landmark1); + initialEstimate.insert(L(2), landmark2); + initialEstimate.insert(L(3), landmark3); + initialEstimate.insert(L(4), landmark4); + + // Create an optimizer and check its error + // We expect the initial to be zero because config is the ground truth + LevenbergMarquardtOptimizer optimizer = graph.optimizer(initialEstimate); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + + // Iterate once, and the config should not have changed because we started + // with the ground truth + optimizer.iterate(); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + + // check if correct + CHECK(assert_equal(initialEstimate, optimizer.values())); +} + /* ************************************************************************* */ TEST( VisualSLAM, CHECK_ORDERING) diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 6e5109041..3fb213ee7 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -29,6 +29,7 @@ #include #include #include +#include #include @@ -217,6 +218,16 @@ namespace visualSLAM { */ Values optimize(const Values& initialEstimate, size_t verbosity) const; + /** + * Setup and return a LevenbargMarquardtOptimizer + * @param initialEstimate initial estimate of all variables in the graph + * @param parameters optimizer's parameters + * @return a LevenbergMarquardtOptimizer object, which you can use to control the optimization process + */ + LevenbergMarquardtOptimizer optimizer(const Values& initialEstimate, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) const { + return LevenbergMarquardtOptimizer((*this), initialEstimate, parameters); + } + /// Return a Marginals object Marginals marginals(const Values& solution) const { return Marginals(*this,solution); diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m index ac25a4328..8e56c00b7 100644 --- a/matlab/examples/SFMExample.m +++ b/matlab/examples/SFMExample.m @@ -61,8 +61,18 @@ for j=1:size(truth.points,2) end initialEstimate.print(sprintf('\nInitial estimate:\n ')); -%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,1); +%% One-shot Optimize using Levenberg-Marquardt optimization with an ordering from colamd +%result = graph.optimize(initialEstimate,1); +%result.print(sprintf('\nFinal result:\n ')); + +%% Fine grain optimization, allowing user to iterate step by step + +parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 2); +optimizer = graph.optimizer(initialEstimate, parameters); +for i=1:5 + optimizer.iterate(); +end +result = optimizer.values(); result.print(sprintf('\nFinal result:\n ')); %% Plot results with covariance ellipses