From 2a3575fcbc79fdda43c1c1ddccdd00792fc46c2d Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 27 Jan 2014 22:02:02 -0500 Subject: [PATCH] added measure of fit between original and linearized model in LM --- .cproject | 410 +++++++++--------- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 38 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 8 +- 3 files changed, 241 insertions(+), 215 deletions(-) diff --git a/.cproject b/.cproject index 19005c63f..40360ea59 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -542,14 +540,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -576,6 +566,7 @@ make + tests/testBayesTree.run true false @@ -583,6 +574,7 @@ make + testBinaryBayesNet.run true false @@ -630,6 +622,7 @@ make + testSymbolicBayesNet.run true false @@ -637,6 +630,7 @@ make + tests/testSymbolicFactor.run true false @@ -644,6 +638,7 @@ make + testSymbolicFactorGraph.run true false @@ -659,11 +654,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -760,22 +764,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -792,6 +780,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -816,46 +820,6 @@ true true - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run - true - true - true - make -j5 @@ -920,6 +884,46 @@ true true + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j6 -j8 + testWhiteNoiseFactor.run + true + true + true + make -j2 @@ -1330,6 +1334,7 @@ make + testGraph.run true false @@ -1337,6 +1342,7 @@ make + testJunctionTree.run true false @@ -1344,6 +1350,7 @@ make + testSymbolicBayesNetB.run true false @@ -1511,6 +1518,7 @@ make + testErrors.run true false @@ -1556,22 +1564,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1652,6 +1644,22 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -2014,7 +2022,6 @@ make - testSimulated2DOriented.run true false @@ -2054,7 +2061,6 @@ make - testSimulated2D.run true false @@ -2062,7 +2068,6 @@ make - testSimulated3D.run true false @@ -2350,7 +2355,6 @@ make - tests/testGaussianISAM2 true false @@ -2372,102 +2376,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2669,6 +2577,7 @@ cpack + -G DEB true false @@ -2676,6 +2585,7 @@ cpack + -G RPM true false @@ -2683,6 +2593,7 @@ cpack + -G TGZ true false @@ -2690,6 +2601,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2855,34 +2767,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2926,6 +2902,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 4493a0b16..ed91112f7 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -75,12 +76,12 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { } /* ************************************************************************* */ -void LevenbergMarquardtOptimizer::increaseLambda(){ +void LevenbergMarquardtOptimizer::increaseLambda(double stepQuality){ state_.lambda *= params_.lambdaFactor; } /* ************************************************************************* */ -void LevenbergMarquardtOptimizer::decreaseLambda(){ +void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality){ state_.lambda /= params_.lambdaFactor; } @@ -98,6 +99,8 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); + double modelMismatch = std::numeric_limits::max(); + // Keep increasing lambda until we make make progress while (true) { ++state_.totalNumberInnerIterations; @@ -153,10 +156,25 @@ void LevenbergMarquardtOptimizer::iterate() { if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "next error = " << error << endl; + // oldCost - newCost + double costChange = state_.error - error; + + // newLinearizedCost (scalar) = 1/2 [f + J * step]^2 = 1/2 [ f'f + 2f'J * step + step' * J' * J * step ] + // linearizedCostChange = oldCost - newLinearizedCost = f'f/2 - 1/2 [ f'f + 2f'J * step + step' * J' * J * step] + // = -f'J * step - step' * J' * J * step / 2 = -(f' + modelResidual') * (modelResidual) + // (with modelResidual = J * step) + Errors modelResidualList = (*linear) * delta; // modelResidual = A * delta + Vector modelResidual = concatVectors(modelResidualList); // TODO: is this an ordered list? + Vector residuals = - linear->jacobian().second; // TODO: optimize this computation, TODO: is there a minus sign? + double linearizedCostChange = dot(- modelResidual, (residuals + modelResidual / 2.0) ); + + // Measure of mismatch between original (usually nonlinear) system and its linearized version + modelMismatch = costChange / linearizedCostChange; + if (error <= state_.error) { state_.values.swap(newValues); state_.error = error; - decreaseLambda(); + decreaseLambda(modelMismatch); break; } else { // Either we're not cautious, or the same lambda was worse than the current error. @@ -170,7 +188,7 @@ void LevenbergMarquardtOptimizer::iterate() { if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "increasing lambda: old error (" << state_.error << ") new error (" << error << ")" << endl; - increaseLambda(); + increaseLambda(modelMismatch); } } } catch (IndeterminantLinearSystemException& e) { @@ -185,13 +203,13 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { - increaseLambda(); + increaseLambda(modelMismatch); } } -// Frank asks: why would we do that? -// catch(...) { -// throw; -// } + // Frank asks: why would we do that? + // catch(...) { + // throw; + // } } // end while if (lmVerbosity >= LevenbergMarquardtParams::LAMBDA) @@ -203,7 +221,7 @@ void LevenbergMarquardtOptimizer::iterate() { /* ************************************************************************* */ LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( - LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const + LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { if(!params.ordering) params.ordering = Ordering::COLAMD(graph); diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index bfd242bd6..b0a604135 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -181,11 +181,11 @@ public: return state_.lambda; } - // Apply policy to increase lambda if the current update was successful - virtual void increaseLambda(); + // Apply policy to increase lambda if the current update was successful (stepQuality not used in the naive policy) + virtual void increaseLambda(double stepQuality); - // Apply policy to decrease lambda if the current update was NOT successful - virtual void decreaseLambda(); + // Apply policy to decrease lambda if the current update was NOT successful (stepQuality not used in the naive policy) + virtual void decreaseLambda(double stepQuality); /// Access the current number of inner iterations int getInnerIterations() const {