diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index ac9bd8812..9bfbf1a9c 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -24,6 +24,8 @@ #include #include #include +#include +#include using namespace std; @@ -171,6 +173,14 @@ void LevenbergMarquardtOptimizer::iterate() { if (lmVerbosity >= LevenbergMarquardtParams::LAMBDA) cout << "using lambda = " << state_.lambda << endl; + if (!params_.logFile.empty()) { + ofstream os(params_.logFile.c_str(), ios::app); + time_t rawtime; + time (&rawtime); + os << state_.iterations << "," << rawtime << "," << state_.error << "," << state_.lambda << endl; + } + + // Increment the iteration counter ++state_.iterations; } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 5e95382ad..90c753df0 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -42,10 +42,17 @@ public: DAMPED }; +private: + VerbosityLM verbosityLMTranslator(const std::string &s) const; + std::string verbosityLMTranslator(VerbosityLM value) const; + +public: + double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-5) double lambdaFactor; ///< The amount by which to multiply or divide lambda when adjusting lambda (default: 10.0) double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5) VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity + std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] LevenbergMarquardtParams() : lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), verbosityLM(SILENT) {} virtual ~LevenbergMarquardtParams() {} @@ -56,15 +63,13 @@ public: inline double getlambdaFactor() const { return lambdaFactor; } inline double getlambdaUpperBound() const { return lambdaUpperBound; } inline std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM); } + inline std::string getLogFile() const { return logFile; } inline void setlambdaInitial(double value) { lambdaInitial = value; } inline void setlambdaFactor(double value) { lambdaFactor = value; } inline void setlambdaUpperBound(double value) { lambdaUpperBound = value; } inline void setVerbosityLM(const std::string &s) { verbosityLM = verbosityLMTranslator(s); } - -private: - VerbosityLM verbosityLMTranslator(const std::string &s) const; - std::string verbosityLMTranslator(VerbosityLM value) const; + inline void setLogFile(const std::string &s) { logFile = s;} }; /** diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 7737fdde0..36da0fb0e 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -37,6 +37,7 @@ using namespace boost::assign; #include +#include using namespace std; using namespace gtsam; @@ -348,6 +349,34 @@ TEST(NonlinearOptimizer, subclass_solver) { EXPECT(assert_equal(expected, actual, 1e-4)); } +/* ************************************************************************* */ +#include +TEST( NonlinearOptimizer, logfile ) +{ + NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + + Point2 x0(3,3); + Values c0; + c0.insert(X(1), x0); + + // Levenberg-Marquardt + LevenbergMarquardtParams lmParams; + static const string filename("testNonlinearOptimizer.log"); + lmParams.setLogFile(filename); + CHECK(lmParams.getLogFile()==filename); + LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize(); + +// stringstream expected,actual; +// ifstream ifs(("../../gtsam/tests/" + filename).c_str()); +// if(!ifs) throw std::runtime_error(filename); +// expected << ifs.rdbuf(); +// ifs.close(); +// ifstream ifs2(filename.c_str()); +// if(!ifs2) throw std::runtime_error(filename); +// actual << ifs2.rdbuf(); +// EXPECT(actual.str()==expected.str()); +} + /* ************************************************************************* */ int main() { TestResult tr;