diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 267792f33..aea90da1a 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -224,10 +224,10 @@ namespace gtsam { std::pair hessian(boost::optional optionalOrdering = boost::none) const; /** Return only the diagonal of the Hessian A'*A, as a VectorValues */ - VectorValues hessianDiagonal() const; + virtual VectorValues hessianDiagonal() const; /** Return the block diagonal of the Hessian for this factor */ - std::map hessianBlockDiagonal() const; + virtual std::map hessianBlockDiagonal() const; /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using * the dense elimination function specified in \c function (default EliminatePreferCholesky), diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index 13038bcc2..a81c2243a 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -11,9 +11,10 @@ /** * @file testGaussianFactorGraphUnordered.cpp - * @brief Unit tests for Linear Factor + * @brief Unit tests for Linear Factor Graph * @author Christian Potthast * @author Frank Dellaert + * @author Luca Carlone * @author Richard Roberts **/ @@ -325,10 +326,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd3 ) EXPECT(assert_equal(Y,AtA*X)); double* x = &X[0]; - double* y = &Y[0]; Vector fast_y = gtsam::zero(6); - double* actual = &fast_y[0]; gfg.multiplyHessianAdd(1.0, x, fast_y.data()); EXPECT(assert_equal(Y, fast_y)); @@ -409,6 +408,21 @@ TEST( GaussianFactorGraph, negate ) { EXPECT(assert_equal(expNegation, actNegation)); } +/* ************************************************************************* */ +TEST( GaussianFactorGraph, hessianDiagonal ) +{ + GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); + VectorValues expected; + Matrix infoMatrix = gfg.hessian().first; + Vector d = infoMatrix.diagonal(); + + VectorValues actual = gfg.hessianDiagonal(); + expected.insert(0, d.segment<2>(0)); + expected.insert(1, d.segment<2>(2)); + expected.insert(2, d.segment<2>(4)); + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 94190e687..42bd533cb 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -31,37 +31,61 @@ using namespace std; - namespace gtsam { using boost::adaptors::map_values; /* ************************************************************************* */ -LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator(const std::string &src) const { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "SILENT") return LevenbergMarquardtParams::SILENT; - if (s == "LAMBDA") return LevenbergMarquardtParams::LAMBDA; - if (s == "TRYLAMBDA") return LevenbergMarquardtParams::TRYLAMBDA; - if (s == "TRYCONFIG") return LevenbergMarquardtParams::TRYCONFIG; - if (s == "TRYDELTA") return LevenbergMarquardtParams::TRYDELTA; - if (s == "DAMPED") return LevenbergMarquardtParams::DAMPED; +LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator( + const std::string &src) const { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "SILENT") + return LevenbergMarquardtParams::SILENT; + if (s == "LAMBDA") + return LevenbergMarquardtParams::LAMBDA; + if (s == "TRYLAMBDA") + return LevenbergMarquardtParams::TRYLAMBDA; + if (s == "TRYCONFIG") + return LevenbergMarquardtParams::TRYCONFIG; + if (s == "TRYDELTA") + return LevenbergMarquardtParams::TRYDELTA; + if (s == "DAMPED") + return LevenbergMarquardtParams::DAMPED; /* default is silent */ return LevenbergMarquardtParams::SILENT; } /* ************************************************************************* */ -std::string LevenbergMarquardtParams::verbosityLMTranslator(VerbosityLM value) const { +std::string LevenbergMarquardtParams::verbosityLMTranslator( + VerbosityLM value) const { std::string s; switch (value) { - case LevenbergMarquardtParams::SILENT: s = "SILENT" ; break; - case LevenbergMarquardtParams::TERMINATION: s = "TERMINATION" ; break; - case LevenbergMarquardtParams::LAMBDA: s = "LAMBDA" ; break; - case LevenbergMarquardtParams::TRYLAMBDA: s = "TRYLAMBDA" ; break; - case LevenbergMarquardtParams::TRYCONFIG: s = "TRYCONFIG" ; break; - case LevenbergMarquardtParams::TRYDELTA: s = "TRYDELTA" ; break; - case LevenbergMarquardtParams::DAMPED: s = "DAMPED" ; break; - default: s = "UNDEFINED" ; break; + case LevenbergMarquardtParams::SILENT: + s = "SILENT"; + break; + case LevenbergMarquardtParams::TERMINATION: + s = "TERMINATION"; + break; + case LevenbergMarquardtParams::LAMBDA: + s = "LAMBDA"; + break; + case LevenbergMarquardtParams::TRYLAMBDA: + s = "TRYLAMBDA"; + break; + case LevenbergMarquardtParams::TRYCONFIG: + s = "TRYCONFIG"; + break; + case LevenbergMarquardtParams::TRYDELTA: + s = "TRYDELTA"; + break; + case LevenbergMarquardtParams::DAMPED: + s = "DAMPED"; + break; + default: + s = "UNDEFINED"; + break; } return s; } @@ -73,10 +97,10 @@ void LevenbergMarquardtParams::print(const std::string& str) const { std::cout << " lambdaFactor: " << lambdaFactor << "\n"; std::cout << " lambdaUpperBound: " << lambdaUpperBound << "\n"; std::cout << " lambdaLowerBound: " << lambdaLowerBound << "\n"; - std::cout << " disableInnerIterations: " << disableInnerIterations << "\n"; std::cout << " minModelFidelity: " << minModelFidelity << "\n"; std::cout << " diagonalDamping: " << diagonalDamping << "\n"; - std::cout << " verbosityLM: " << verbosityLMTranslator(verbosityLM) << "\n"; + std::cout << " verbosityLM: " + << verbosityLMTranslator(verbosityLM) << "\n"; std::cout.flush(); } @@ -86,10 +110,10 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { } /* ************************************************************************* */ -void LevenbergMarquardtOptimizer::increaseLambda(){ - if(params_.useFixedLambdaFactor_){ +void LevenbergMarquardtOptimizer::increaseLambda() { + if (params_.useFixedLambdaFactor_) { state_.lambda *= params_.lambdaFactor; - }else{ + } else { state_.lambda *= params_.lambdaFactor; params_.lambdaFactor *= 2.0; } @@ -97,11 +121,11 @@ void LevenbergMarquardtOptimizer::increaseLambda(){ } /* ************************************************************************* */ -void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality){ +void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { - if(params_.useFixedLambdaFactor_){ + if (params_.useFixedLambdaFactor_) { state_.lambda /= params_.lambdaFactor; - }else{ + } else { // CHECK_GT(step_quality, 0.0); state_.lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * stepQuality - 1.0, 3)); params_.lambdaFactor = 2.0; @@ -115,10 +139,6 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality){ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( const GaussianFactorGraph& linear) { - //Set two parameters as Ceres, will move out later - static const double min_diagonal_ = 1e-6; - static const double max_diagonal_ = 1e32; - gttic(damp); if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) cout << "building damped system with lambda " << state_.lambda << endl; @@ -128,7 +148,8 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( state_.hessianDiagonal = linear.hessianDiagonal(); BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { for (int aa = 0; aa < v.size(); aa++) { - v(aa) = std::min(std::max(v(aa), min_diagonal_), max_diagonal_); + v(aa) = std::min(std::max(v(aa), params_.min_diagonal_), + params_.max_diagonal_); v(aa) = sqrt(v(aa)); } } @@ -138,21 +159,31 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( double sigma = 1.0 / std::sqrt(state_.lambda); GaussianFactorGraph damped = linear; damped.reserve(damped.size() + state_.values.size()); - BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { - size_t dim = key_value.value.dim(); - Matrix A = Matrix::Identity(dim, dim); - //Replace the identity matrix with diagonal of Hessian - if (params_.diagonalDamping){ + if (params_.diagonalDamping) { + BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) { + // Fill in the diagonal of A with diag(hessian) try { - A.diagonal() = state_.hessianDiagonal.at(key_value.key); + Matrix A = Eigen::DiagonalMatrix( + state_.hessianDiagonal.at(key_vector.first)); + size_t dim = key_vector.second.size(); + Vector b = Vector::Zero(dim); + SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); + damped += boost::make_shared(key_vector.first, A, b, + model); } catch (std::exception e) { // Don't attempt any damping if no key found in diagonal continue; } } - Vector b = Vector::Zero(dim); - SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); - damped += boost::make_shared(key_value.key, A, b, model); + } else { + // Straightforward damping: + BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { + size_t dim = key_value.value.dim(); + Matrix A = Matrix::Identity(dim, dim); + Vector b = Vector::Zero(dim); + SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); + damped += boost::make_shared(key_value.key, A, b, model); + } } gttoc(damp); return damped; @@ -161,20 +192,22 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( /* ************************************************************************* */ void LevenbergMarquardtOptimizer::iterate() { - gttic (LM_iterate); + gttic(LM_iterate); // Pull out parameters we'll use const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity; const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM; // Linearize graph - if(lmVerbosity >= LevenbergMarquardtParams::DAMPED) cout << "linearizing = " << endl; + if (lmVerbosity >= LevenbergMarquardtParams::DAMPED) + cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); // Keep increasing lambda until we make make progress while (true) { - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "trying lambda = " << state_.lambda << endl; + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "trying lambda = " << state_.lambda << endl; // Build damped system for this lambda (adds prior factors that make it like gradient descent) GaussianFactorGraph dampedSystem = buildDampedSystem(*linear); @@ -183,9 +216,11 @@ void LevenbergMarquardtOptimizer::iterate() { if (!params_.logFile.empty()) { ofstream os(params_.logFile.c_str(), ios::app); - boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time(); + boost::posix_time::ptime currentTime = + boost::posix_time::microsec_clock::universal_time(); - os << state_.totalNumberInnerIterations << "," << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," + os << state_.totalNumberInnerIterations << "," + << 1e-6 * (currentTime - state_.startTime).total_microseconds() << "," << state_.error << "," << state_.lambda << endl; } @@ -203,66 +238,73 @@ void LevenbergMarquardtOptimizer::iterate() { try { delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; - } catch(IndeterminantLinearSystemException& e) { + } catch (IndeterminantLinearSystemException& e) { systemSolvedSuccessfully = false; } - if(systemSolvedSuccessfully) { + if (systemSolvedSuccessfully) { params_.reuse_diagonal_ = true; - if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl; - if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta"); + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "linear delta norm = " << delta.norm() << endl; + if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) + delta.print("delta"); // cost change in the linearized system (old - new) double newlinearizedError = linear->error(delta); double linearizedCostChange = state_.error - newlinearizedError; - if(linearizedCostChange >= 0){ // step is valid + if (linearizedCostChange >= 0) { // step is valid // update values - gttic (retract); + gttic(retract); newValues = state_.values.retract(delta); gttoc(retract); // compute new error - gttic (compute_error); - if(lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "calculating error" << endl; + gttic(compute_error); + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) + cout << "calculating error" << endl; newError = graph_.error(newValues); gttoc(compute_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) { + if (linearizedCostChange > 1e-15) { // the error has to decrease to satify this condition // 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 - } + modelFidelity = costChange / linearizedCostChange; + // if we decrease the error in the nonlinear system and modelFidelity is above threshold + step_is_successful = modelFidelity > params_.minModelFidelity; } else { - stopSearchingLambda = true; + step_is_successful = true; // linearizedCostChange close to zero } + + double minAbsoluteTolerance = params_.relativeErrorTol * state_.error; + // if the change is small we terminate + if (fabs(costChange) < minAbsoluteTolerance) + stopSearchingLambda = true; + } } - if(step_is_successful){ // we have successfully decreased the cost and we have good 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 + } 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; + 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; + 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 { // the change in the cost is very small and it is not worth trying bigger lambdas @@ -276,9 +318,8 @@ void LevenbergMarquardtOptimizer::iterate() { /* ************************************************************************* */ LevenbergMarquardtParams LevenbergMarquardtOptimizer::ensureHasOrdering( - LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const -{ - if(!params.ordering) + LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const { + if (!params.ordering) params.ordering = Ordering::COLAMD(graph); return params; } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index bae82b7c3..47952f9e4 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -51,17 +51,19 @@ public: double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5) double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0) VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity - bool disableInnerIterations; ///< If enabled inner iterations on the linearized system are performed double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] bool diagonalDamping; ///< if true, use diagonal of Hessian - bool reuse_diagonal_; //an additional option in Ceres for diagonalDamping (related to efficiency) - bool useFixedLambdaFactor_; // if true applies constant increase (or decrease) to lambda according to lambdaFactor + bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (related to efficiency) + bool useFixedLambdaFactor_; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor + double min_diagonal_; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) + double max_diagonal_; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) LevenbergMarquardtParams() : lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound( - 0.0), verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity( - 1e-3), diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true) { + 0.0), verbosityLM(SILENT), minModelFidelity(1e-3), + diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true), + min_diagonal_(1e-6), max_diagonal_(1e32) { } virtual ~LevenbergMarquardtParams() { } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 6548b1449..4a7166f38 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -669,28 +669,30 @@ bool writeBAL(const string& filename, SfM_data &data) return true; } -bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ +bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values){ + + SfM_data dataValues = data; // Store poses or cameras in SfM_data Values valuesPoses = values.filter(); - if( valuesPoses.size() == data.number_cameras() ){ // we only estimated camera poses - for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + if( valuesPoses.size() == dataValues.number_cameras() ){ // we only estimated camera poses + for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera Key poseKey = symbol('x',i); Pose3 pose = values.at(poseKey); - Cal3Bundler K = data.cameras[i].calibration(); + Cal3Bundler K = dataValues.cameras[i].calibration(); PinholeCamera camera(pose, K); - data.cameras[i] = camera; + dataValues.cameras[i] = camera; } } else { Values valuesCameras = values.filter< PinholeCamera >(); - if ( valuesCameras.size() == data.number_cameras() ){ // we only estimated camera poses and calibration - for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + if ( valuesCameras.size() == dataValues.number_cameras() ){ // we only estimated camera poses and calibration + for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera Key cameraKey = i; // symbol('c',i); PinholeCamera camera = values.at >(cameraKey); - data.cameras[i] = camera; + dataValues.cameras[i] = camera; } }else{ - cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras() + cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras() <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl; return false; } @@ -698,26 +700,26 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ // Store 3D points in SfM_data Values valuesPoints = values.filter(); - if( valuesPoints.size() != data.number_tracks()){ - cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks() + if( valuesPoints.size() != dataValues.number_tracks()){ + cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks() <<") and values (#points " << valuesPoints.size() << ")!!" << endl; } - for (size_t j = 0; j < data.number_tracks(); j++){ // for each point + for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point Key pointKey = symbol('l',j); if(values.exists(pointKey)){ Point3 point = values.at(pointKey); - data.tracks[j].p = point; + dataValues.tracks[j].p = point; }else{ - data.tracks[j].r = 1.0; - data.tracks[j].g = 0.0; - data.tracks[j].b = 0.0; - data.tracks[j].p = Point3(); + dataValues.tracks[j].r = 1.0; + dataValues.tracks[j].g = 0.0; + dataValues.tracks[j].b = 0.0; + dataValues.tracks[j].p = Point3(); } } // Write SfM_data to file - return writeBAL(filename, data); + return writeBAL(filename, dataValues); } Values initialCamerasEstimate(const SfM_data& db) { diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 070bfc000..64ccd37b5 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -146,7 +146,7 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data); * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 * @return true if the parsing was successful, false otherwise */ -GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values); +GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, const SfM_data &data, Values& values); /** * @brief This function converts an openGL camera pose to an GTSAM camera pose diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index bba13d6eb..b976cca90 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -234,7 +234,7 @@ TEST(NonlinearOptimizer, NullFactor) { } /* ************************************************************************* */ -TEST(NonlinearOptimizer, MoreOptimization) { +TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { NonlinearFactorGraph fg; fg += PriorFactor(0, Pose2(0, 0, 0), @@ -284,57 +284,59 @@ TEST(NonlinearOptimizer, MoreOptimization) { // cout << "===================================================================================" << endl; // 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)); + Values initBetter; + 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(); -// 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)); + + /* This block was made to test the original initial guess "init" + // 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); + */ } }