/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/containers/stl_containers_utils.h>  // find_in_vector()
#include <mrpt/containers/yaml.h>
#include <mrpt/graphslam/types.h>
#include <mrpt/math/CSparseMatrix.h>
#include <mrpt/system/CTimeLogger.h>
// (Must come *after* "types.h" above)
#include <mrpt/graphslam/levmarq_impl.h>  // Aux classes

#include <map>
#include <memory>

namespace mrpt::graphslam
{
/** Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA)
 *sparse representation and a Levenberg-Marquardt optimizer.
 *  This method works for all types of graphs derived from \a CNetworkOfPoses
 *(see its reference mrpt::graphs::CNetworkOfPoses for the list).
 *  The input data are all the pose constraints in \a graph (graph.edges), and
 *the gross first estimations of the "global" pose coordinates (in
 *graph.nodes).
 *
 *  Note that these first coordinates can be obtained with
 *mrpt::graphs::CNetworkOfPoses::dijkstra_nodes_estimate().
 *
 * The method implemented in this file is based on this work:
 *  - "Efficient Sparse Pose Adjustment for 2D Mapping", Kurt Konolige et al.,
 *2010.
 * , but generalized for not only 2D but 2D and 3D poses, and using on-manifold
 *optimization.
 *
 * \param[in,out] graph The input edges and output poses.
 * \param[out] out_info Some basic output information on the process.
 * \param[in] nodes_to_optimize The list of nodes whose global poses are to be
 *optimized. If nullptr (default), all the node IDs are optimized (but that
 *marked as \a root in the graph).
 * \param[in] extra_params Optional parameters, see below.
 * \param[in] functor_feedback Optional: a pointer to a user function can be
 *set here to be called on each LM loop iteration (eg to refresh the current
 *state and error, refresh a GUI, etc.)
 *
 * List of optional parameters by name in "extra_params":
 *		- "verbose": (default=0) If !=0, produce verbose ouput.
 *		- "max_iterations": (default=100) Maximum number of Lev-Marq.
 *iterations.
 *		- "initial_lambda": (default=0) <=0 means auto guess, otherwise, initial
 *lambda value for the lev-marq algorithm.
 *		- "tau": (default=1e-6) Initial tau value for the lev-marq algorithm.
 *		- "e1": (default=1e-6) Lev-marq algorithm iteration stopping criterion
 *#1:
 *|gradient| < e1
 *		- "e2": (default=1e-6) Lev-marq algorithm iteration stopping criterion
 *#2:
 *|delta_incr| < e2*(x_norm+e2)
 *
 * \note The following graph types are supported:
 *mrpt::graphs::CNetworkOfPoses2D, mrpt::graphs::CNetworkOfPoses3D,
 *mrpt::graphs::CNetworkOfPoses2DInf, mrpt::graphs::CNetworkOfPoses3DInf
 *
 * \tparam GRAPH_T Normally a
 *mrpt::graphs::CNetworkOfPoses<EDGE_TYPE,MAPS_IMPLEMENTATION>. Users won't
 *have to write this template argument by hand, since the compiler will
 *auto-fit it depending on the type of the graph object.
 * \sa The example "graph_slam_demo"
 * \ingroup mrpt_graphslam_grp
 * \note Implementation can be found in file \a levmarq_impl.h
 */
template <
    class GRAPH_T,
    class FEEDBACK_CALLABLE = typename graphslam_traits<GRAPH_T>::TFunctorFeedback>
void optimize_graph_spa_levmarq(
    GRAPH_T& graph,
    TResultInfoSpaLevMarq& out_info,
    const std::set<mrpt::graphs::TNodeID>* in_nodes_to_optimize = nullptr,
    const mrpt::containers::yaml& extra_params = {},
    FEEDBACK_CALLABLE functor_feedback = FEEDBACK_CALLABLE())
{
  using namespace mrpt;
  using namespace mrpt::poses;
  using namespace mrpt::graphslam;
  using namespace mrpt::math;
  using mrpt::graphs::TNodeID;
  using namespace std;

  MRPT_START

  // Typedefs to make life easier:
  using gst = graphslam_traits<GRAPH_T>;

  // The size of things here (because size matters...)
  constexpr auto DIMS_POSE = gst::SE_TYPE::DOFs;

  // Read extra params:
  const bool verbose = extra_params.getOrDefault<bool>("verbose", false);
  const size_t max_iters = extra_params.getOrDefault<size_t>("max_iterations", 100);
  const bool enable_profiler = extra_params.getOrDefault<bool>("profiler", false);
  // LM params:
  // <=0: means auto guess
  const double initial_lambda = extra_params.getOrDefault<double>("initial_lambda", 0);
  const double tau = extra_params.getOrDefault<double>("tau", 1e-3);
  const double e1 = extra_params.getOrDefault<double>("e1", 1e-6);
  const double e2 = extra_params.getOrDefault<double>("e2", 1e-6);

  mrpt::system::CTimeLogger profiler(enable_profiler);
  profiler.enter("optimize_graph_spa_levmarq (entire)");

  // Make list of node IDs to optimize, since the user may want only a subset
  // of them to be optimized:
  profiler.enter("optimize_graph_spa_levmarq.list_IDs");
  const set<TNodeID>* nodes_to_optimize;
  set<TNodeID> nodes_to_optimize_auxlist;  // Used only if
  // in_nodes_to_optimize==nullptr
  if (in_nodes_to_optimize)
  {
    nodes_to_optimize = in_nodes_to_optimize;
  }
  else
  {
    // We have to make the list of IDs:
    for (const auto& n : graph.nodes)
      if (n.first != graph.root)  // Root node is fixed.
        nodes_to_optimize_auxlist.insert(
            nodes_to_optimize_auxlist.end(),
            n.first);  // Provide the "first guess" insert position
    // for efficiency
    nodes_to_optimize = &nodes_to_optimize_auxlist;
  }
  profiler.leave("optimize_graph_spa_levmarq.list_IDs");  // ---------------/

  // Number of nodes to optimize, or free variables:
  const size_t nFreeNodes = nodes_to_optimize->size();
  ASSERTDEB_GT_(nFreeNodes, 0);
  if (verbose)
  {
    cout << "[optimize_graph_spa_levmarq] " << nFreeNodes << " nodes to optimize: ";
    if (nFreeNodes < 14)
    {
      ostream_iterator<TNodeID> out_it(cout, ", ");
      std::copy(nodes_to_optimize->begin(), nodes_to_optimize->end(), out_it);
    }
    cout << endl;
  }

  // The list of those edges that will be considered in this optimization
  // (many may be discarded
  //  if we are optimizing just a subset of all the nodes):
  using observation_info_t = typename gst::observation_info_t;
  vector<observation_info_t> lstObservationData;

  // Note: We'll need those Jacobians{i->j} where at least one "i" or "j"
  //        is a free variable (i.e. it's in nodes_to_optimize)
  // Now, build the list of all relevent "observations":
  for (const auto& e : graph.edges)
  {
    const auto& ids = e.first;
    const auto& edge = e.second;

    if (nodes_to_optimize->find(ids.first) == nodes_to_optimize->end() &&
        nodes_to_optimize->find(ids.second) == nodes_to_optimize->end())
      continue;  // Skip this edge, none of the IDs are free variables.

    // get the current global poses of both nodes in this constraint:
    auto itP1 = graph.nodes.find(ids.first);
    auto itP2 = graph.nodes.find(ids.second);
    ASSERTMSG_(itP1 != graph.nodes.end(), "Edge node1 has no global pose");
    ASSERTMSG_(itP2 != graph.nodes.end(), "Edge node2 has no global pose");

    const auto& EDGE_POSE = edge.getPoseMean();

    // Add all the data to the list of relevant observations:
    typename gst::observation_info_t new_entry;
    new_entry.edge = &e;
    new_entry.edge_mean = &EDGE_POSE;
    new_entry.P1 = &itP1->second;
    new_entry.P2 = &itP2->second;

    lstObservationData.push_back(new_entry);
  }

  // The number of constraints, or observations actually implied in this
  // problem:
  const size_t nObservations = lstObservationData.size();
  ASSERTDEB_GT_(nObservations, 0);
  // Cholesky object, as a pointer to reuse it between iterations:

  using SparseCholeskyDecompPtr = std::unique_ptr<CSparseMatrix::CholeskyDecomp>;
  SparseCholeskyDecompPtr ptrCh;

  // The list of Jacobians: for each constraint i->j,
  //  we need the pair of Jacobians: { dh(xi,xj)_dxi, dh(xi,xj)_dxj },
  //  which are "first" and "second" in each pair.
  // Index of the map are the node IDs {i,j} for each contraint.
  typename gst::map_pairIDs_pairJacobs_t lstJacobians;
  // The vector of errors: err_k = SE(2/3)::pseudo_Ln( P_i * EDGE_ij *
  // inv(P_j) )
  // Separated vectors for each edge. i \in [0,nObservations-1], in
  // same order than lstObservationData
  std::vector<typename gst::Array_O> errs;

  // ===================================
  // Compute Jacobians & errors
  // ===================================
  profiler.enter("optimize_graph_spa_levmarq.Jacobians&err");
  double total_sqr_err =
      computeJacobiansAndErrors<GRAPH_T>(graph, lstObservationData, lstJacobians, errs);
  profiler.leave("optimize_graph_spa_levmarq.Jacobians&err");

  // Only once (since this will be static along iterations), build a quick
  // look-up table with the
  //  indices of the free nodes associated to the (first_id,second_id) of each
  //  Jacobian pair:
  // ------------------------------------------------------------------------
  vector<pair<size_t, size_t>> obsIdx2fnIdx;
  // "relatedFreeNodeIndex" is in [0,nFreeNodes-1], or "-1" if that node
  // is fixed, as defined by "nodes_to_optimize"
  obsIdx2fnIdx.reserve(nObservations);
  ASSERTDEB_(lstJacobians.size() == nObservations);
  for (auto itJ = lstJacobians.begin(); itJ != lstJacobians.end(); ++itJ)
  {
    const TNodeID id1 = itJ->first.first;
    const TNodeID id2 = itJ->first.second;
    obsIdx2fnIdx.emplace_back(
        mrpt::containers::find_in_vector(id1, *nodes_to_optimize),
        mrpt::containers::find_in_vector(id2, *nodes_to_optimize));
  }

  // other important vars for the main loop:
  CVectorDouble grad(nFreeNodes * DIMS_POSE);
  grad.setZero();
  using map_ID2matrix_TxT_t = std::map<TNodeID, typename gst::matrix_TxT>;

  double lambda = initial_lambda;  // Will be actually set on first iteration.
  double v = 1;                    // was 2, changed since it's modified in the first pass.
  bool have_to_recompute_H_and_grad = true;

  // -----------------------------------------------------------
  // Main iterative loop of Levenberg-Marquardt algorithm
  //  For notation and overall algorithm overview, see:
  //  https://www.mrpt.org/Levenberg-Marquardt_algorithm
  // -----------------------------------------------------------
  size_t last_iter = 0;

  for (size_t iter = 0; iter < max_iters; ++iter)
  {
    vector<map_ID2matrix_TxT_t> H_map(nFreeNodes);
    last_iter = iter;

    // This will be false only when the delta leads to a worst solution and
    // only a change in lambda is needed.
    if (have_to_recompute_H_and_grad)
    {
      have_to_recompute_H_and_grad = false;
      // ======================================================================
      // Compute the gradient: grad = J^t * errs
      // ======================================================================
      //  "grad" can be seen as composed of N independent arrays, each one
      //  being:
      //   grad_i = \sum_k J^t_{k->i} errs_k
      // that is: g_i is the "dot-product" of the i'th (transposed)
      // block-column of J and the vector of errors "errs"
      profiler.enter("optimize_graph_spa_levmarq.grad");

      grad.setZero();

      // "lstJacobians" is sorted in the same order than
      // "lstObservationData":
      ASSERTDEB_EQUAL_(lstJacobians.size(), lstObservationData.size());
      {
        size_t idx_obs;
        typename gst::map_pairIDs_pairJacobs_t::const_iterator itJ;

        for (idx_obs = 0, itJ = lstJacobians.begin(); itJ != lstJacobians.end(); ++itJ, ++idx_obs)
        {
          // make sure they're in the expected order!
          ASSERTDEB_(itJ->first == lstObservationData[idx_obs].edge->first);

          //  grad[k] += J^t_{i->k} * Inf.Matrix * errs_i
          //    k: [0,nFreeNodes-1]     <-- IDs.first & IDs.second
          //    i: [0,nObservations-1]  <--- idx_obs

          // Get the corresponding indices in the vector of "free
          // variables" being optimized:
          const size_t idx1 = obsIdx2fnIdx[idx_obs].first;
          const size_t idx2 = obsIdx2fnIdx[idx_obs].second;

          if (idx1 != string::npos)
          {
            typename gst::Array_O grad_idx1;
            detail::AuxErrorEval<typename gst::edge_t, gst>::multiply_Jt_W_err(
                itJ->second.first /* J */, lstObservationData[idx_obs].edge /* W */,
                errs[idx_obs] /* err */, grad_idx1 /* out */
            );
            for (unsigned int i = 0; i < DIMS_POSE; i++) grad[DIMS_POSE * idx1 + i] += grad_idx1[i];
          }

          if (idx2 != string::npos)
          {
            typename gst::Array_O grad_idx2;
            detail::AuxErrorEval<typename gst::edge_t, gst>::multiply_Jt_W_err(
                itJ->second.second /* J */, lstObservationData[idx_obs].edge /* W */,
                errs[idx_obs] /* err */, grad_idx2 /* out */
            );
            for (unsigned int i = 0; i < DIMS_POSE; i++) grad[DIMS_POSE * idx2 + i] += grad_idx2[i];
          }
        }
      }
      profiler.leave("optimize_graph_spa_levmarq.grad");

      // End condition #1
      const double grad_norm_inf =
          math::norm_inf(grad);  // inf-norm (abs. maximum value) of the gradient
      if (grad_norm_inf <= e1)
      {
        // Change is too small
        if (verbose)
          cout << "[optimize_graph_spa_levmarq] "
               << mrpt::format(
                      "End condition #1: math::norm_inf(g)<=e1 "
                      ":%f<=%f\n",
                      grad_norm_inf, e1);
        break;
      }

      profiler.enter("optimize_graph_spa_levmarq.sp_H:build map");
      // ======================================================================
      // Build sparse representation of the upper triangular part of
      //  the Hessian matrix H = J^t * J
      //
      // Sparse memory structure is a vector of maps, such as:
      //  - H_map[i]: corresponds to the i'th column of H.
      //              Here "i" corresponds to [0,N-1] indices of
      //              appearance in the map "*nodes_to_optimize".
      //  - H_map[i][j] is the entry for the j'th row, with "j" also in
      //  the range [0,N-1] as ordered in "*nodes_to_optimize".
      // ======================================================================
      {
        size_t idxObs;
        typename gst::map_pairIDs_pairJacobs_t::const_iterator itJacobPair;

        for (idxObs = 0, itJacobPair = lstJacobians.begin(); idxObs < nObservations;
             ++itJacobPair, ++idxObs)
        {
          const bool Hij_upper_triang = itJacobPair->first.first < itJacobPair->first.second;

          // Indices in the "H_map" vector:
          const size_t idx_i = obsIdx2fnIdx[idxObs].first;
          const size_t idx_j = obsIdx2fnIdx[idxObs].second;

          const bool is_i_free_node = idx_i != string::npos;
          const bool is_j_free_node = idx_j != string::npos;

          // Take references to both Jacobians (wrt pose "i" and pose
          // "j"), taking into account the possible
          // switch in their order:
          const typename gst::matrix_TxT& J1 = itJacobPair->second.first;
          const typename gst::matrix_TxT& J2 = itJacobPair->second.second;

          // Is "i" a free (to be optimized) node? -> Ji^t * Inf *  Ji
          if (is_i_free_node)
          {
            typename gst::matrix_TxT JtJ(mrpt::math::UNINITIALIZED_MATRIX);
            detail::AuxErrorEval<typename gst::edge_t, gst>::multiplyJtLambdaJ(
                J1, JtJ, lstObservationData[idxObs].edge);
            H_map[idx_i][idx_i] += JtJ;
          }
          // Is "j" a free (to be optimized) node? -> Jj^t * Inf *  Jj
          if (is_j_free_node)
          {
            typename gst::matrix_TxT JtJ(mrpt::math::UNINITIALIZED_MATRIX);
            detail::AuxErrorEval<typename gst::edge_t, gst>::multiplyJtLambdaJ(
                J2, JtJ, lstObservationData[idxObs].edge);
            H_map[idx_j][idx_j] += JtJ;
          }
          // Are both "i" and "j" free nodes? -> Ji^t * Inf *  Jj
          if (is_i_free_node && is_j_free_node)
          {
            typename gst::matrix_TxT JtJ(mrpt::math::UNINITIALIZED_MATRIX);
            detail::AuxErrorEval<typename gst::edge_t, gst>::multiplyJ1tLambdaJ2(
                J1, J2, JtJ, lstObservationData[idxObs].edge);
            // We sort IDs such as "i" < "j" and we can build just
            // the upper triangular part of the Hessian:
            if (Hij_upper_triang)  // H_map[col][row]
              H_map[idx_j][idx_i] += JtJ;
            else
              H_map[idx_i][idx_j].sum_At(JtJ);
          }
        }
      }
      profiler.leave("optimize_graph_spa_levmarq.sp_H:build map");

      // Just in the first iteration, we need to calculate an estimate for
      // the first value of "lamdba":
      if (lambda <= 0 && iter == 0)
      {
        profiler.enter("optimize_graph_spa_levmarq.lambda_init");  // ---\  .
        double H_diagonal_max = 0;
        for (size_t i = 0; i < nFreeNodes; i++)
          for (auto it = H_map[i].begin(); it != H_map[i].end(); ++it)
          {
            const size_t j = it->first;  // entry submatrix is for (i,j).
            if (i == j)
            {
              for (size_t k = 0; k < DIMS_POSE; k++)
                mrpt::keep_max(H_diagonal_max, it->second(k, k));
            }
          }
        lambda = tau * H_diagonal_max;

        profiler.leave("optimize_graph_spa_levmarq.lambda_init");  // ---/
      }
      else
      {
        // After recomputing H and the grad, we update lambda:
        lambda *= 0.1;  // std::max(0.333, 1-pow(2*rho-1,3.0) );
      }
      mrpt::keep_max(lambda, 1e-200);  // JL: Avoids underflow!
      v = 2;
    }  // end "have_to_recompute_H_and_grad"

    if (verbose)
      cout << "[optimize_graph_spa_levmarq] Iter: " << iter << " ,total sqr. err: " << total_sqr_err
           << ", avrg. err per edge: " << std::sqrt(total_sqr_err / nObservations)
           << " lambda: " << lambda << endl;

    // Feedback to the user:
    if (functor_feedback)
    {
      functor_feedback(graph, iter, max_iters, total_sqr_err);
    }

    profiler.enter("optimize_graph_spa_levmarq.sp_H:build");
    // Now, build the actual sparse matrix H:
    // Note: we only need to fill out the upper diagonal part, since
    // Cholesky will later on ignore the other part.
    CSparseMatrix sp_H(nFreeNodes * DIMS_POSE, nFreeNodes * DIMS_POSE);
    for (size_t i = 0; i < nFreeNodes; i++)
    {
      const size_t i_offset = i * DIMS_POSE;

      for (auto it = H_map[i].begin(); it != H_map[i].end(); ++it)
      {
        const size_t j = it->first;  // entry submatrix is for (i,j).
        const size_t j_offset = j * DIMS_POSE;

        // For i==j (diagonal blocks), it's different, since we only
        // need to insert their
        // upper-diagonal half and also we have to add the lambda*I to
        // the diagonal from the Lev-Marq. algorithm:
        if (i == j)
        {
          for (size_t r = 0; r < DIMS_POSE; r++)
          {
            // c=r: add lambda from LM
            sp_H.insert_entry_fast(j_offset + r, i_offset + r, it->second(r, r) + lambda);
            // c>r:
            for (size_t c = r + 1; c < DIMS_POSE; c++)
            {
              sp_H.insert_entry_fast(j_offset + r, i_offset + c, it->second(r, c));
            }
          }
        }
        else
        {
          sp_H.insert_submatrix(j_offset, i_offset, it->second);
        }
      }
    }  // end for each free node, build sp_H

    sp_H.compressFromTriplet();
    profiler.leave("optimize_graph_spa_levmarq.sp_H:build");

    // Use the cparse Cholesky decomposition to efficiently solve:
    //   (H+\lambda*I) \delta = -J^t * (f(x)-z)
    //          A         x   =  b         -->       x = A^{-1} * b
    //
    CVectorDouble delta;  // The (minus) increment to be added to the
    // current solution in this step
    try
    {
      profiler.enter("optimize_graph_spa_levmarq.sp_H:chol");
      if (!ptrCh.get())
        ptrCh = std::make_unique<CSparseMatrix::CholeskyDecomp>(sp_H);
      else
        ptrCh.get()->update(sp_H);
      profiler.leave("optimize_graph_spa_levmarq.sp_H:chol");

      profiler.enter("optimize_graph_spa_levmarq.sp_H:backsub");
      ptrCh->backsub(grad, delta);
      profiler.leave("optimize_graph_spa_levmarq.sp_H:backsub");
    }
    catch (CExceptionNotDefPos&)
    {
      // not positive definite so increase mu and try again
      if (verbose)
        cout << "[optimize_graph_spa_levmarq] Got non-definite "
                "positive matrix, retrying with a "
                "larger lambda...\n";
      lambda *= v;
      v *= 2;
      if (lambda > 1e9)
      {  // enough!
        break;
      }
      continue;  // try again with this params
    }

    // Compute norm of the increment vector:
    profiler.enter("optimize_graph_spa_levmarq.delta_norm");
    const double delta_norm = math::norm(delta);
    profiler.leave("optimize_graph_spa_levmarq.delta_norm");

    // Compute norm of the current solution vector:
    profiler.enter("optimize_graph_spa_levmarq.x_norm");
    double x_norm = 0;
    {
      for (unsigned long it : *nodes_to_optimize)
      {
        auto itP = graph.nodes.find(it);
        const typename gst::graph_t::constraint_t::type_value& P = itP->second;
        for (size_t i = 0; i < DIMS_POSE; i++) x_norm += square(P[i]);
      }
      x_norm = std::sqrt(x_norm);
    }
    profiler.leave("optimize_graph_spa_levmarq.x_norm");

    // Test end condition #2:
    const double thres_norm = e2 * (x_norm + e2);
    if (delta_norm < thres_norm)
    {
      // The change is too small: we're done here...
      if (verbose)
        cout << "[optimize_graph_spa_levmarq] "
             << format("End condition #2: %e < %e\n", delta_norm, thres_norm);
      break;
    }
    else
    {
      // =====================================================================================
      // Accept this delta? Try it and look at the increase/decrease of
      // the error:
      //  new_x = old_x [+] (-delta)    , with [+] being the "manifold
      //  exp()+add" operation.
      // =====================================================================================
      typename gst::graph_t::global_poses_t old_poses_backup;

      {
        ASSERTDEB_(delta.size() == int(nodes_to_optimize->size() * DIMS_POSE));
        const double* delta_ptr = &delta[0];
        for (unsigned long it : *nodes_to_optimize)
        {
          typename gst::Array_O exp_delta;
          for (size_t i = 0; i < DIMS_POSE; i++) exp_delta[i] = -*delta_ptr++;
          // The "-" above is for the missing "-" dragged from the
          // Gauss-Newton formula above.

          // new_x_i =  exp_delta_i (+) old_x_i
          auto it_old_value = graph.nodes.find(it);
          old_poses_backup[it] = it_old_value->second;  // back up the old pose as a copy

          // Update estimate:
          it_old_value->second = it_old_value->second + gst::SE_TYPE::exp(exp_delta);
        }
      }

      // =============================================================
      // Compute Jacobians & errors with the new "graph.nodes" info:
      // =============================================================
      typename gst::map_pairIDs_pairJacobs_t new_lstJacobians;
      std::vector<typename gst::Array_O> new_errs;

      profiler.enter("optimize_graph_spa_levmarq.Jacobians&err");
      double new_total_sqr_err =
          computeJacobiansAndErrors<GRAPH_T>(graph, lstObservationData, new_lstJacobians, new_errs);
      profiler.leave("optimize_graph_spa_levmarq.Jacobians&err");

      // Now, to decide whether to accept the change:
      if (new_total_sqr_err < total_sqr_err)  // rho>0)
      {
        // Accept the new point:
        new_lstJacobians.swap(lstJacobians);
        new_errs.swap(errs);
        std::swap(new_total_sqr_err, total_sqr_err);

        // Instruct to recompute H and grad from the new Jacobians.
        have_to_recompute_H_and_grad = true;
      }
      else
      {
        // Nope...
        // We have to revert the "graph.nodes" to "old_poses_backup"
        for (auto it = old_poses_backup.begin(); it != old_poses_backup.end(); ++it)
          graph.nodes[it->first] = it->second;

        if (verbose)
          cout << "[optimize_graph_spa_levmarq] Got larger error=" << new_total_sqr_err
               << ", retrying with a larger lambda...\n";
        // Change params and try again:
        lambda *= v;
        v *= 2;
      }

    }  // end else end condition #2

  }  // end for each iter

  profiler.leave("optimize_graph_spa_levmarq (entire)");

  // Fill out basic output data:
  // ------------------------------
  out_info.num_iters = last_iter;
  out_info.final_total_sq_error = total_sqr_err;

  MRPT_END
}  // end of optimize_graph_spa_levmarq()

/**  @} */  // end of grouping

}  // namespace mrpt::graphslam
