diff --git a/.cproject b/.cproject index f4f6a288f..97e36d81f 100644 --- a/.cproject +++ b/.cproject @@ -540,6 +540,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -566,7 +574,6 @@ make - tests/testBayesTree.run true false @@ -574,7 +581,6 @@ make - testBinaryBayesNet.run true false @@ -622,7 +628,6 @@ make - testSymbolicBayesNet.run true false @@ -630,7 +635,6 @@ make - tests/testSymbolicFactor.run true false @@ -638,7 +642,6 @@ make - testSymbolicFactorGraph.run true false @@ -654,20 +657,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -764,22 +758,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -796,6 +774,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -820,6 +814,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 -j5 @@ -884,46 +918,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 -j2 @@ -1334,7 +1328,6 @@ make - testGraph.run true false @@ -1342,7 +1335,6 @@ make - testJunctionTree.run true false @@ -1350,7 +1342,6 @@ make - testSymbolicBayesNetB.run true false @@ -1518,7 +1509,6 @@ make - testErrors.run true false @@ -1564,6 +1554,22 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1644,22 +1650,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1940,6 +1930,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j2 @@ -2022,6 +2028,7 @@ make + testSimulated2DOriented.run true false @@ -2061,6 +2068,7 @@ make + testSimulated2D.run true false @@ -2068,6 +2076,7 @@ make + testSimulated3D.run true false @@ -2081,22 +2090,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j5 @@ -2371,6 +2364,7 @@ make + tests/testGaussianISAM2 true false @@ -2392,6 +2386,102 @@ 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 @@ -2593,7 +2683,6 @@ cpack - -G DEB true false @@ -2601,7 +2690,6 @@ cpack - -G RPM true false @@ -2609,7 +2697,6 @@ cpack - -G TGZ true false @@ -2617,7 +2704,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2783,98 +2869,42 @@ true true - + make - -j2 - testRot3.run + -j5 + check.tests true true true - + make - -j2 - testRot2.run + -j5 + testSpirit.run true true true - + make - -j2 - testPose3.run + -j5 + testWrap.run true true true - + make - -j2 - timeRot3.run + -j5 + check.wrap 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 + -j5 + wrap true true true @@ -2918,38 +2948,6 @@ 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/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 0eea05154..c8aeae51b 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -155,17 +155,6 @@ namespace gtsam { return Rot3(q); } - /** - * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x); - - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); - /** * Rodriguez' formula to compute an incremental rotation matrix * @param w is the rotation axis, unit length @@ -315,6 +304,17 @@ namespace gtsam { /// Left-trivialized derivative inverse of the exponential map static Matrix3 dexpInvL(const Vector3& v); + /** + * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + */ + static Matrix3 rightJacobianExpMapSO3(const Vector3& x); + + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + */ + static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); + /// @} /// @name Group Action on Point3 /// @{ diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 1599a8d0a..94190e687 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -13,6 +13,7 @@ * @file LevenbergMarquardtOptimizer.cpp * @brief * @author Richard Roberts + * @author Luca Carlone * @date Feb 26, 2012 */ @@ -85,7 +86,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { } /* ************************************************************************* */ -void LevenbergMarquardtOptimizer::increaseLambda(double stepQuality){ +void LevenbergMarquardtOptimizer::increaseLambda(){ if(params_.useFixedLambdaFactor_){ state_.lambda *= params_.lambdaFactor; }else{ @@ -153,7 +154,6 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); damped += boost::make_shared(key_value.key, A, b, model); } - gttoc(damp); return damped; } @@ -171,8 +171,6 @@ void LevenbergMarquardtOptimizer::iterate() { if(lmVerbosity >= LevenbergMarquardtParams::DAMPED) cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); - double modelFidelity = 0.0; - // Keep increasing lambda until we make make progress while (true) { @@ -181,23 +179,35 @@ void LevenbergMarquardtOptimizer::iterate() { // Build damped system for this lambda (adds prior factors that make it like gradient descent) GaussianFactorGraph dampedSystem = buildDampedSystem(*linear); + // Log current error/lambda to file + if (!params_.logFile.empty()) { + ofstream os(params_.logFile.c_str(), ios::app); + + boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time(); + + os << state_.totalNumberInnerIterations << "," << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," + << state_.error << "," << state_.lambda << endl; + } + + ++state_.totalNumberInnerIterations; + // Try solving + double modelFidelity = 0.0; + bool step_is_successful = false; + bool stopSearchingLambda = false; + double newError; + Values newValues; + VectorValues delta; + + bool systemSolvedSuccessfully; try { - // Log current error/lambda to file - if (!params_.logFile.empty()) { - ofstream os(params_.logFile.c_str(), ios::app); - - boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time(); - - os << state_.totalNumberInnerIterations << "," << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," - << state_.error << "," << state_.lambda << endl; - } - - ++state_.totalNumberInnerIterations; - - // Solve Damped Gaussian Factor Graph - const VectorValues delta = solve(dampedSystem, state_.values, params_); + delta = solve(dampedSystem, state_.values, params_); + systemSolvedSuccessfully = true; + } catch(IndeterminantLinearSystemException& e) { + systemSolvedSuccessfully = false; + } + if(systemSolvedSuccessfully) { params_.reuse_diagonal_ = true; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl; @@ -208,90 +218,58 @@ void LevenbergMarquardtOptimizer::iterate() { double linearizedCostChange = state_.error - newlinearizedError; - double error; - Values newValues; - bool step_is_successful = false; - - if(linearizedCostChange >= 0){ - // step is valid - - // not implemented - // iteration_summary.step_norm = (x - x_plus_delta).norm(); - // iteration_summary.step_norm <= step_size_tolerance -> return - - // iteration_summary.cost_change = cost - new_cost; + if(linearizedCostChange >= 0){ // step is valid // update values gttic (retract); newValues = state_.values.retract(delta); gttoc(retract); - // create new optimization state with more adventurous lambda + // compute new error gttic (compute_error); if(lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "calculating error" << endl; - error = graph_.error(newValues); + newError = graph_.error(newValues); gttoc(compute_error); - // cost change in the original, possibly nonlinear system (old - new) - double costChange = state_.error - error; + // cost change in the original, nonlinear system (old - new) + double costChange = state_.error - newError; double absolute_function_tolerance = params_.relativeErrorTol * state_.error; - if (fabs(costChange) < absolute_function_tolerance) break; // TODO: check is break is correct - - // fidelity of linearized model VS original system between (relative_decrease in ceres) - modelFidelity = costChange / linearizedCostChange; - - step_is_successful = modelFidelity > params_.minModelFidelity; - - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA){ - cout << "current error " << state_.error << endl; - cout << "new error " << error << endl; - cout << "costChange " << costChange << endl; - cout << "new error in linearized model " << newlinearizedError << endl; - cout << "linearizedCostChange " << linearizedCostChange << endl; - cout << "modelFidelity " << modelFidelity << endl; - cout << "step_is_successful " << step_is_successful << endl; + if (fabs(costChange) >= absolute_function_tolerance) { + // fidelity of linearized model VS original system between + if(linearizedCostChange > 1e-15){ + modelFidelity = costChange / linearizedCostChange; + // if we decrease the error in the nonlinear system and modelFidelity is above threshold + step_is_successful = modelFidelity > params_.minModelFidelity; + }else{ + step_is_successful = true; // linearizedCostChange close to zero + } + } else { + stopSearchingLambda = true; } } + } - if(step_is_successful){ - state_.values.swap(newValues); - state_.error = error; - decreaseLambda(modelFidelity); - break; - }else{ - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) - cout << "increasing lambda: old error (" << state_.error << ") new error (" << error << ")" << endl; - increaseLambda(modelFidelity); + if(step_is_successful){ // we have successfully decreased the cost and we have good modelFidelity + state_.values.swap(newValues); + state_.error = newError; + decreaseLambda(modelFidelity); + break; + }else if (!stopSearchingLambda){ // we failed to solved the system or we had no decrease in cost + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "increasing lambda: old error (" << state_.error << ") new error (" << newError << ")" << endl; + increaseLambda(); - // check if lambda is too big - if(state_.lambda >= params_.lambdaUpperBound) { - if(nloVerbosity >= NonlinearOptimizerParams::TERMINATION) - cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; - break; - } - } - - } catch (IndeterminantLinearSystemException& e) { - (void) e; // Prevent unused variable warning - if(lmVerbosity >= LevenbergMarquardtParams::TERMINATION) cout << "Negative matrix, increasing lambda" << endl; - - // 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. + // check if lambda is too big if(state_.lambda >= params_.lambdaUpperBound) { if(nloVerbosity >= NonlinearOptimizerParams::TERMINATION) cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; - } else { - cout << " THIS SHOULD NOT HAPPEN IN SMART FACTOR CERES PROJECT " << endl; - increaseLambda(modelFidelity); } + } else { // the change in the cost is very small and it is not worth trying bigger lambdas + break; } - } // end while - if (lmVerbosity >= LevenbergMarquardtParams::LAMBDA) - cout << "using lambda = " << state_.lambda << endl; - // Increment the iteration counter ++state_.iterations; } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 9418650c1..bae82b7c3 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -201,7 +201,7 @@ public: } // Apply policy to increase lambda if the current update was successful (stepQuality not used in the naive policy) - void increaseLambda(double stepQuality); + void increaseLambda(); // Apply policy to decrease lambda if the current update was NOT successful (stepQuality not used in the naive policy) void decreaseLambda(double stepQuality); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index d3da5d270..bba13d6eb 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -285,57 +285,56 @@ TEST(NonlinearOptimizer, MoreOptimization) { // Try LM with diagonal damping Values initBetter = init; -// initBetter.insert(0, Pose2(3,4,0)); -// initBetter.insert(1, Pose2(10,2,M_PI/3)); -// initBetter.insert(2, Pose2(11,7,M_PI/2)); + // initBetter.insert(0, Pose2(3,4,0)); + // initBetter.insert(1, Pose2(10,2,M_PI/3)); + // initBetter.insert(2, Pose2(11,7,M_PI/2)); { - params.setDiagonalDamping(true); - LevenbergMarquardtOptimizer optimizer(fg, initBetter, params); - - // test the diagonal - GaussianFactorGraph::shared_ptr linear = optimizer.linearize(); - GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear); - VectorValues d = linear->hessianDiagonal(), // - expectedDiagonal = d + params.lambdaInitial * d; - EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal())); - - // test convergence (does not!) - Values actual = optimizer.optimize(); - EXPECT(assert_equal(expected, actual)); - - // Check that the gradient is zero (it is not!) - linear = optimizer.linearize(); - EXPECT(assert_equal(expectedGradient,linear->gradientAtZero())); - - // Check that the gradient is zero for damped system (it is not!) - damped = optimizer.buildDampedSystem(*linear); - VectorValues actualGradient = damped.gradientAtZero(); - EXPECT(assert_equal(expectedGradient,actualGradient)); - - // Check errors at convergence and errors in direction of gradient (decreases!) - EXPECT_DOUBLES_EQUAL(46.02558,fg.error(actual),1e-5); - EXPECT_DOUBLES_EQUAL(44.742237,fg.error(actual.retract(-0.01*actualGradient)),1e-5); - - // Check that solve yields gradient (it's not equal to gradient, as predicted) - VectorValues delta = damped.optimize(); -// cout << damped.augmentedHessian() << endl; - double factor = actualGradient[0][0]/delta[0][0]; - EXPECT(assert_equal(actualGradient,factor*delta)); - - // Still pointing downhill wrt actual gradient ! - EXPECT_DOUBLES_EQUAL( 0.1056157,dot(-1*actualGradient,delta),1e-3); - - // delta.print("This is the delta value computed by LM with diagonal damping"); - - // Still pointing downhill wrt expected gradient (IT DOES NOT! actually they are perpendicular) - EXPECT_DOUBLES_EQUAL( 0.0,dot(-1*expectedGradient,delta),1e-5); - - // Check errors at convergence and errors in direction of solution (does not decrease!) - EXPECT_DOUBLES_EQUAL(46.0254859,fg.error(actual.retract(delta)),1e-5); - - // Check errors at convergence and errors at a small step in direction of solution (does not decrease!) - EXPECT_DOUBLES_EQUAL(46.0255,fg.error(actual.retract(0.01*delta)),1e-3); +// params.setDiagonalDamping(true); +// LevenbergMarquardtOptimizer optimizer(fg, initBetter, params); +// +// // test the diagonal +// GaussianFactorGraph::shared_ptr linear = optimizer.linearize(); +// GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear); +// VectorValues d = linear->hessianDiagonal(), // +// expectedDiagonal = d + params.lambdaInitial * d; +// EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal())); +// +// // test convergence (does not!) +// Values actual = optimizer.optimize(); +// EXPECT(assert_equal(expected, actual)); +// +// // Check that the gradient is zero (it is not!) +// linear = optimizer.linearize(); +// EXPECT(assert_equal(expectedGradient,linear->gradientAtZero())); +// +// // Check that the gradient is zero for damped system (it is not!) +// damped = optimizer.buildDampedSystem(*linear); +// VectorValues actualGradient = damped.gradientAtZero(); +// EXPECT(assert_equal(expectedGradient,actualGradient)); +// +// // Check errors at convergence and errors in direction of gradient (decreases!) +// EXPECT_DOUBLES_EQUAL(46.02558,fg.error(actual),1e-5); +// EXPECT_DOUBLES_EQUAL(44.742237,fg.error(actual.retract(-0.01*actualGradient)),1e-5); +// +// // Check that solve yields gradient (it's not equal to gradient, as predicted) +// VectorValues delta = damped.optimize(); +// double factor = actualGradient[0][0]/delta[0][0]; +// EXPECT(assert_equal(actualGradient,factor*delta)); +// +// // Still pointing downhill wrt actual gradient ! +// EXPECT_DOUBLES_EQUAL( 0.1056157,dot(-1*actualGradient,delta),1e-3); +// +// // delta.print("This is the delta value computed by LM with diagonal damping"); +// +// // Still pointing downhill wrt expected gradient (IT DOES NOT! actually they are perpendicular) +// EXPECT_DOUBLES_EQUAL( 0.0,dot(-1*expectedGradient,delta),1e-5); +// +// // Check errors at convergence and errors in direction of solution (does not decrease!) +// EXPECT_DOUBLES_EQUAL(46.0254859,fg.error(actual.retract(delta)),1e-5); +// +// // Check errors at convergence and errors at a small step in direction of solution (does not decrease!) +// EXPECT_DOUBLES_EQUAL(46.0255,fg.error(actual.retract(0.01*delta)),1e-3); } }