Merge pull request #66 from prime-slam/feature/LM-terminal

update to LM update fidelity model and terminal condition, now to be …

Author: anastasiia-kornilova

python_examples/FGraph_M3500.py

@@ -97,7 +97,7 @@ def print_2d_graph(graph):
 print_2d_graph(graph)
 print('current initial chi2 = ', graph.chi2() )
 start = time.time()
-graph.solve(mrob.LM, 50)
+graph.solve(mrob.LM, 50, verbose=True)
 end = time.time()
 print('\nLM chi2 = ', graph.chi2() , ', total time on calculation [s] = ', 1e0*(end - start))
 print('solution drawn')

python_examples/FGraph_sphere.py

@@ -161,7 +161,7 @@ def plot_from_vertex(vertex):
     # uncomment for visualization
     #print_3d_graph(graph)
     start = time.time()
-    graph.solve(mrob.LM,20)
+    graph.solve(mrob.LM,20, verbose=True)
     end = time.time()
     print(', chi2 = ', graph.chi2() , ', time on calculation [s] = ', 1e0*(end - start))
     # uncomment for visualization

src/FGraph/factor_graph_solve.cpp

@@ -49,7 +49,7 @@ FGraphSolve::~FGraphSolve() = default;
 
 
 uint_t FGraphSolve::solve(optimMethod method, uint_t maxIters,
-        matData_t lambda, matData_t solutionTolerance, bool verbose)
+        matData_t lambdaParam, matData_t solutionTolerance, bool verbose)
 {
     /**
      * 2800 2D nodes on M3500
@@ -60,7 +60,7 @@ uint_t FGraphSolve::solve(optimMethod method, uint_t maxIters,
      *               1.3959 % update values,
      *
      */
-    lambda_ = lambda;
+    lambda_ = lambdaParam;
     solutionTolerance_ = solutionTolerance;
     verbose_ = verbose;
     time_profiles_.reset();
@@ -168,7 +168,7 @@ uint_t FGraphSolve::optimize_levenberg_marquardt(uint_t maxIters)
     // LM trust region as described in Bertsekas (p.105)
 
     // 0) parameter initialization
-    lambda_ = 1e-5;
+    //lambda_ = 1e-5;
     // sigma reference to the fidelity of the model at the proposed solution \in [0,1]
     matData_t sigma1(0.25), sigma2(0.8);// 0 < sigma1 < sigma2 < 1
     matData_t beta1(2.0), beta2(0.25); // lambda updates multiplier values, beta1 > 1 > beta2 >0
@@ -206,7 +206,7 @@ uint_t FGraphSolve::optimize_levenberg_marquardt(uint_t maxIters)
         }
 
         // 1.3) check for convergence
-        if (deltaChi2 < solutionTolerance_)
+        if (deltaChi2/currentChi2 < solutionTolerance_) //in ratio
         {
             if (verbose_)
                 std::cout << "\nFGraphSolve::optimize_levenberg_marquardt: Converged Successfully" << std::endl;
@@ -218,9 +218,14 @@ uint_t FGraphSolve::optimize_levenberg_marquardt(uint_t maxIters)
         // f = chi2(x_k) - chi2(x_k + dx)
         //     chi2(x_k) - m_k(dx)
         // where m_k is the quadratized model = ||r||^2 - dx'*J' r + 0.5 dx'(J'J + lambda*D2)dx
-        modelFidelity = deltaChi2 / (dx_.dot(b_) - 0.5*dx_.dot(L_* dx_));
+        //modelFidelity = deltaChi2 / (dx_.dot(b_) - 0.5*dx_.dot(L_* dx_));
+        // Update, according to H.B. Nielson, Damping Parameter In Marquardt’s Method, Technical Report IMM-REP-1999-05, Dept. of Mathematical Modeling, Technical University Denmark
+        // (J'J ) dx = J'r , so this is substituted.
+        modelFidelity = 2.0 * deltaChi2 / (dx_.dot(b_) - dx_.dot(lambda_ * diagL_* dx_));
         if (verbose_)
+        {
             std::cout << "model fidelity = " << modelFidelity << " and m_k = " << dx_.dot(b_) << std::endl;
+        }
 
         //3) update lambda
         if (modelFidelity < sigma1)
@@ -613,4 +618,4 @@ std::vector<bool> FGraphSolve::get_factors_robust_mask()
     }
 
     return results;
-}
+}

src/pybind/FGraphPy.cpp

@@ -376,13 +376,13 @@ void init_FGraph(py::module &m)
                     "Options:\n method = mrob.GN (Gauss Newton). It carries out a SINGLE iteration.\n"
                     "                  = mrob.LM (Levenberg-Marquard), default option,it has several parameters:\n"
                     " - marIters = 20 (by default). Only for LM\n"
-                    " - lambda = 1-6, LM paramter for the size of the update\n"
+                    " - lambda = 1-5, LM paramter for the size of the update\n"
                     " - solutionTolerance: convergence criteria\n"
                     " - verbose: by default false. If you want output on optim, set to true.",
                     py::arg("method") =  FGraphSolve::optimMethod::LM,
                     py::arg("maxIters") = 20,
-                    py::arg("lambda") = 1e-6,
-                    py::arg("solutionTolerance") = 1e-2,
+                    py::arg("lambdaParam") = 1e-5,
+                    py::arg("solutionTolerance") = 1e-6,
                     py::arg("verbose") = false)
             .def("chi2", &FGraphSolve::chi2,
                     "Calculated the chi2 of the problem.\n"