Optimizer Improvements

gtsam/linear/IterativeSolver.cpp

@@ -46,6 +46,12 @@ void IterativeOptimizationParameters::print(ostream &os) const {
       << verbosityTranslator(verbosity_) << endl;
 }
 
+/*****************************************************************************/
+bool IterativeOptimizationParameters::equals(
+    const IterativeOptimizationParameters &other, double tol) const {
+  return verbosity_ == other.verbosity();
+}
+
 /*****************************************************************************/
 ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
   p.print(os);

gtsam/linear/IterativeSolver.h

@@ -41,15 +41,14 @@ class VectorValues;
  * parameters for iterative linear solvers
  */
 class IterativeOptimizationParameters {
-
-public:
-
+ public:
   typedef std::shared_ptr<IterativeOptimizationParameters> shared_ptr;
-  enum Verbosity {
-    SILENT = 0, COMPLEXITY, ERROR
-  } verbosity_;
+  enum Verbosity { SILENT = 0, COMPLEXITY, ERROR };
 
-public:
+ protected:
+  Verbosity verbosity_;
+
+ public:
 
   IterativeOptimizationParameters(Verbosity v = SILENT) :
       verbosity_(v) {
@@ -71,6 +70,9 @@ class IterativeOptimizationParameters {
   /* virtual print function */
   GTSAM_EXPORT virtual void print(std::ostream &os) const;
 
+  GTSAM_EXPORT virtual bool equals(const IterativeOptimizationParameters &other,
+                                   double tol = 1e-9) const;
+
   /* for serialization */
   GTSAM_EXPORT friend std::ostream &operator<<(
       std::ostream &os, const IterativeOptimizationParameters &p);

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -236,9 +236,9 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
     if (currentState->iterations == 0) {
       cout << "iter      cost      cost_change    lambda  success iter_time" << endl;
     }
-    cout << setw(4) << currentState->iterations << " " << setw(8) << newError << " " << setw(3) << setprecision(2)
-         << costChange << " " << setw(3) << setprecision(2) << currentState->lambda << " " << setw(4)
-         << systemSolvedSuccessfully << " " << setw(3) << setprecision(2) << iterationTime << endl;
+    cout << setw(4) << currentState->iterations << " " << setw(12) << newError << " " << setw(12) << setprecision(2)
+         << costChange << " " << setw(10) << setprecision(2) << currentState->lambda << " " << setw(6)
+         << systemSolvedSuccessfully << " " << setw(10) << setprecision(2) << iterationTime << endl;
   }
   if (step_is_successful) {
     // we have successfully decreased the cost and we have good modelFidelity

gtsam/nonlinear/NonlinearOptimizerParams.cpp

@@ -123,6 +123,28 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
   std::cout.flush();
 }
 
+/* ************************************************************************* */
+bool NonlinearOptimizerParams::equals(const NonlinearOptimizerParams& other,
+                                      double tol) const {
+  // Check for equality of shared ptrs
+  bool iterative_params_equal = iterativeParams == other.iterativeParams;
+  // Check equality of components
+  if (iterativeParams && other.iterativeParams) {
+    iterative_params_equal = iterativeParams->equals(*other.iterativeParams);
+  } else {
+    // Check if either is null. If both are null, then true
+    iterative_params_equal = !iterativeParams && !other.iterativeParams;
+  }
+
+  return maxIterations == other.getMaxIterations() &&
+         std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol &&
+         std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol &&
+         std::abs(errorTol - other.getErrorTol()) <= tol &&
+         verbosityTranslator(verbosity) == other.getVerbosity() &&
+         orderingType == other.orderingType && ordering == other.ordering &&
+         linearSolverType == other.linearSolverType && iterative_params_equal;
+}
+
 /* ************************************************************************* */
 std::string NonlinearOptimizerParams::linearSolverTranslator(
     LinearSolverType linearSolverType) const {

gtsam/nonlinear/NonlinearOptimizerParams.h

@@ -114,16 +114,7 @@ class GTSAM_EXPORT NonlinearOptimizerParams {
 
   virtual void print(const std::string& str = "") const;
 
-  bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const {
-    return maxIterations == other.getMaxIterations()
-        && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol
-        && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol
-        && std::abs(errorTol - other.getErrorTol()) <= tol
-        && verbosityTranslator(verbosity) == other.getVerbosity();
-    //  && orderingType.equals(other.getOrderingType()_;
-    // && linearSolverType == other.getLinearSolverType();
-    // TODO: check ordering, iterativeParams, and iterationsHook
-  }
+  bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const;
 
   inline bool isMultifrontal() const {
     return (linearSolverType == MULTIFRONTAL_CHOLESKY)