/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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
