cleaned code to log LM iterations

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -192,6 +192,19 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
   return dampedPtr;
 }
 
+/* ************************************************************************* */
+// Log current error/lambda to file
+inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){
+  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 << /*inner iterations*/ state_.totalNumberInnerIterations << ","
+        << 1e-6 * (currentTime - state_.startTime).total_microseconds() << ","
+        << /*current error*/ currentError << "," << state_.lambda << ","
+        << /*outer iterations*/ state_.iterations << endl;
+  }
+}
+
 /* ************************************************************************* */
 void LevenbergMarquardtOptimizer::iterate() {
 
@@ -206,6 +219,9 @@ void LevenbergMarquardtOptimizer::iterate() {
     cout << "linearizing = " << endl;
   GaussianFactorGraph::shared_ptr linear = linearize();
 
+  if(state_.totalNumberInnerIterations==0) // write initial error
+    writeLogFile(state_.error);
+
   // Keep increasing lambda until we make make progress
   while (true) {
 
@@ -288,36 +304,29 @@ void LevenbergMarquardtOptimizer::iterate() {
       }
     }
 
-    // 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 << /*inner iterations*/ state_.totalNumberInnerIterations << ","
-          << 1e-6 * (currentTime - state_.startTime).total_microseconds() << ","
-          << /*current error*/ state_.error << "," << state_.lambda << ","
-          << /*outer iterations*/ state_.iterations << endl;
-    }
     ++state_.totalNumberInnerIterations;
 
     if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity
       state_.values.swap(newValues);
       state_.error = newError;
       decreaseLambda(modelFidelity);
+      writeLogFile(state_.error);
       break;
     } else if (!stopSearchingLambda) { // we failed to solved the system or we had no decrease in cost
       if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
         cout << "increasing lambda" << endl;
       increaseLambda();
+      writeLogFile(state_.error);
 
       // check if lambda is too big
       if (state_.lambda >= params_.lambdaUpperBound) {
         if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION)
-          cout
+          cout << "Warning:  Levenberg-Marquardt giving up because "
-              << "Warning:  Levenberg-Marquardt giving up because cannot decrease error with maximum lambda"
+              "cannot decrease error with maximum lambda" << endl;
-              << endl;
         break;
       }
     } else { // the change in the cost is very small and it is not worth trying bigger lambdas
+      writeLogFile(state_.error);
       if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
               cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl;
       break;

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -113,7 +113,6 @@ public:
   inline void setDiagonalDamping(bool flag) {
     diagonalDamping = flag;
   }
-
   inline void setUseFixedLambdaFactor(bool flag) {
     useFixedLambdaFactor_ = flag;
   }
@@ -258,6 +257,8 @@ public:
   GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear);
   friend class ::NonlinearOptimizerMoreOptimizationTest;
 
+  void writeLogFile(double currentError);
+
   /// @}
 
 protected: