/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"	 // Precompiled headers
//
#include <mrpt/config/CConfigFileMemory.h>
#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/math/robust_kernels.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/poses/Lie/SE.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/vision/chessboard_find_corners.h>
#include <mrpt/vision/chessboard_stereo_camera_calib.h>
#include <mrpt/vision/pinhole.h>

#include <Eigen/Dense>
#include <algorithm>  // reverse()

#include "chessboard_stereo_camera_calib_internal.h"

//#define USE_NUMERIC_JACOBIANS
//#define COMPARE_NUMERIC_JACOBIANS

#ifdef USE_NUMERIC_JACOBIANS
#include <mrpt/math/num_jacobian.h>
#endif

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

/* -------------------------------------------------------
				checkerBoardStereoCalibration
   ------------------------------------------------------- */
bool mrpt::vision::checkerBoardStereoCalibration(
	TCalibrationStereoImageList& images, const TStereoCalibParams& p,
	TStereoCalibResults& out)
{
	try
	{
		ASSERT_(p.check_size_x > 2);
		ASSERT_(p.check_size_y > 2);
		ASSERT_(p.check_squares_length_X_meters > 0);
		ASSERT_(p.check_squares_length_Y_meters > 0);
		const bool user_wants_use_robust = p.use_robust_kernel;

		if (images.size() < 1 && p.verbose)
		{
			std::cout << "ERROR: No input images." << std::endl;
			return false;
		}

		const unsigned CORNERS_COUNT = p.check_size_x * p.check_size_y;
		const TImageSize check_size =
			TImageSize(p.check_size_x, p.check_size_y);
		TImageStereoCallbackData cbPars;

		// For each image, find checkerboard corners:
		// -----------------------------------------------
		// Left/Right sizes (may be different)
		TImageSize imgSize[2] = {TImageSize(0, 0), TImageSize(0, 0)};

		vector<size_t> valid_image_pair_indices;  // Indices in images[] which
		// are valid pairs to be used
		// in the optimization

		for (size_t i = 0; i < images.size(); i++)
		{
			// Do loop for each left/right image:
			TImageCalibData* dats[2] = {&images[i].left, &images[i].right};
			bool corners_found[2] = {false, false};

			for (int lr = 0; lr < 2; lr++)
			{
				TImageCalibData& dat = *dats[lr];
				dat.detected_corners.clear();

				// Make grayscale version:
				const CImage img_gray(
					dat.img_original, FAST_REF_OR_CONVERT_TO_GRAY);

				if (!i)
				{
					imgSize[lr] = img_gray.getSize();
					if (lr == 0)
					{
						out.cam_params.leftCamera.ncols = imgSize[lr].x;
						out.cam_params.leftCamera.nrows = imgSize[lr].y;
					}
					else
					{
						out.cam_params.rightCamera.ncols = imgSize[lr].x;
						out.cam_params.rightCamera.nrows = imgSize[lr].y;
					}
				}
				else
				{
					if (imgSize[lr].y != (int)img_gray.getHeight() ||
						imgSize[lr].x != (int)img_gray.getWidth())
					{
						std::cout << "ERROR: All the images in each left/right "
									 "channel must have the same size."
								  << std::endl;
						return false;
					}
				}

				// Do detection (this includes the "refine corners" with
				// cvFindCornerSubPix):
				corners_found[lr] = mrpt::vision::findChessboardCorners(
					img_gray, dat.detected_corners, p.check_size_x,
					p.check_size_y,
					p.normalize_image  // normalize_image
				);

				if (corners_found[lr] &&
					dat.detected_corners.size() != CORNERS_COUNT)
					corners_found[lr] = false;

				if (p.verbose)
					cout << format(
						"%s img #%u: %s\n", lr == 0 ? "LEFT" : "RIGHT",
						static_cast<unsigned int>(i),
						corners_found[lr] ? "DETECTED" : "NOT DETECTED");
				// User Callback?
				if (p.callback)
				{
					cbPars.calibRound = -1;	 // Detecting corners
					cbPars.current_iter = 0;
					cbPars.current_rmse = 0;
					cbPars.nImgsProcessed = i * 2 + lr + 1;
					cbPars.nImgsToProcess = images.size() * 2;
					(*p.callback)(cbPars, p.callback_user_param);
				}

				if (corners_found[lr])
				{
					// Draw the checkerboard in the corresponding image:
					if (!dat.img_original.isExternallyStored())
					{
						// Checkboad as color image:
						dat.img_original.colorImage(dat.img_checkboard);
						dat.img_checkboard.drawChessboardCorners(
							dat.detected_corners, check_size.x, check_size.y);
					}
				}

			}  // end for lr

			// We just finished detecting corners in a left/right pair.
			// Only if corners were detected perfectly in BOTH images, add this
			// image pair as a good one for optimization:
			if (corners_found[0] && corners_found[1])
			{
				valid_image_pair_indices.push_back(i);

				// Consistency between left/right pair: the corners MUST BE
				// ORDERED so they match to each other,
				// and the criterion must be the same in ALL pairs to avoid
				// rubbish optimization:

				// Key idea: Generate a representative vector that goes along
				// rows and columns.
				// Check the angle of those director vectors between L/R images.
				// That can be done
				// via the dot product. Swap rows/columns order as needed.
				bool has_to_redraw_corners = false;

				const TPixelCoordf pt_l0 = images[i].left.detected_corners[0],
								   pt_l1 = images[i].left.detected_corners[1],
								   pt_r0 = images[i].right.detected_corners[0],
								   pt_r1 = images[i].right.detected_corners[1];
				const auto Al =
					TPoint2D(pt_l1.x, pt_l1.y) - TPoint2D(pt_l0.x, pt_l0.y);
				const auto Ar =
					TPoint2D(pt_r1.x, pt_r1.y) - TPoint2D(pt_r0.x, pt_r0.y);

				// If the dot product is negative, we have INVERTED order of
				// corners:
				if (Al.x * Ar.x + Al.y * Ar.y < 0)
				{
					has_to_redraw_corners = true;
					// Invert all corners:
					std::reverse(
						images[i].right.detected_corners.begin(),
						images[i].right.detected_corners.end());
				}

				if (has_to_redraw_corners)
				{
					// Checkboad as color image:
					images[i].right.img_original.colorImage(
						images[i].right.img_checkboard);
					images[i].right.img_checkboard.drawChessboardCorners(
						images[i].right.detected_corners, check_size.x,
						check_size.y);
				}
			}

		}  // end find corners

		if (p.verbose)
			std::cout << valid_image_pair_indices.size()
					  << " valid image pairs.\n";
		if (valid_image_pair_indices.empty())
		{
			if (p.verbose)
				std::cerr << "ERROR: No valid images. Perhaps the checkerboard "
							 "size is incorrect?\n";
			return false;
		}

		//  Create reference coordinates of the detected corners, in "global"
		//  coordinates:
		// -------------------------------------------------------------------------------
		vector<TPoint3D> obj_points;
		obj_points.reserve(CORNERS_COUNT);
		for (unsigned int y = 0; y < p.check_size_y; y++)
			for (unsigned int x = 0; x < p.check_size_x; x++)
				obj_points.emplace_back(
					p.check_squares_length_X_meters * x,
					p.check_squares_length_Y_meters * y, 0);

		// ----------------------------------------------------------------------------------
		//  Levenberg-Marquardt algorithm
		//
		//  State space (poses are actually on-manifold increments):
		//     N*left_cam_pose + right2left_pose + left_cam_params +
		//     right_cam_params
		//       N * 6         +        6        +        9        +         9
		//       = 6N+24
		// ----------------------------------------------------------------------------------
		const size_t N = valid_image_pair_indices.size();
		const size_t nObs = 2 * N * CORNERS_COUNT;	// total number of valid
		// observations (px,py)
		// taken into account in the
		// optimization

		const double tau = 0.3;
		const double t1 = 1e-8;
		const double t2 = 1e-8;
		const double MAX_LAMBDA = 1e20;

		// Initial state:
		// Within lm_stat: CVectorFixedDouble<9> left_cam_params,
		// right_cam_params; // [fx fy cx cy k1 k2 k3 t1 t2]
		lm_stat_t lm_stat(images, valid_image_pair_indices, obj_points);

		lm_stat.left_cam_params[0] = imgSize[0].x * 0.9;
		lm_stat.left_cam_params[1] = imgSize[0].y * 0.9;
		lm_stat.left_cam_params[2] = imgSize[0].x * 0.5;
		lm_stat.left_cam_params[3] = imgSize[0].y * 0.5;
		lm_stat.left_cam_params[4] = lm_stat.left_cam_params[5] =
			lm_stat.left_cam_params[6] = lm_stat.left_cam_params[7] =
				lm_stat.left_cam_params[8] = 0;

		lm_stat.right_cam_params[0] = imgSize[1].x * 0.9;
		lm_stat.right_cam_params[1] = imgSize[1].y * 0.9;
		lm_stat.right_cam_params[2] = imgSize[1].x * 0.5;
		lm_stat.right_cam_params[3] = imgSize[1].y * 0.5;
		lm_stat.right_cam_params[4] = lm_stat.right_cam_params[5] =
			lm_stat.right_cam_params[6] = lm_stat.right_cam_params[7] =
				lm_stat.right_cam_params[8] = 0;

		// ===============================================================================
		//   Run stereo calibration in two stages:
		//   (0) Estimate all parameters except distortion
		//   (1) Estimate all parameters, using as starting point the guess from
		//   (0)
		// ===============================================================================
		size_t nUnknownsCamParams = 0;
		size_t iter = 0;
		double err = 0;
		std::vector<size_t> vars_to_optimize;
		// Hessian matrix  (Declared here so it's accessible as the final
		// uncertainty measure)
		mrpt::math::CMatrixDouble H;

		for (int calibRound = 0; calibRound < 2; calibRound++)
		{
			cbPars.calibRound = calibRound;
			if (p.verbose)
				cout
					<< ((calibRound == 0) ? "LM calibration round #0: WITHOUT "
											"distortion ----------\n"
										  : "LM calibration round #1: ALL "
											"parameters --------------\n");

			// Select which cam. parameters to optimize (for each camera!) and
			// which to leave fixed:
			// Indices:  [0   1  2  3  4  5  6  7  8]
			// Variables:[fx fy cx cy k1 k2 k3 t1 t2]
			vars_to_optimize.clear();
			vars_to_optimize.push_back(0);
			vars_to_optimize.push_back(1);
			vars_to_optimize.push_back(2);
			vars_to_optimize.push_back(3);
			if (calibRound == 1)
			{
				if (p.optimize_k1) vars_to_optimize.push_back(4);
				if (p.optimize_k2) vars_to_optimize.push_back(5);
				if (p.optimize_t1) vars_to_optimize.push_back(6);
				if (p.optimize_t2) vars_to_optimize.push_back(7);
				if (p.optimize_k3) vars_to_optimize.push_back(8);
			}

			nUnknownsCamParams = vars_to_optimize.size();

			//  * N x Manifold Epsilon of left camera pose (6)
			//  * Manifold Epsilon of right2left pose (6)
			//  * Left-cam-params (<=9)
			//  * Right-cam-params (<=9)
			const size_t nUnknowns = N * 6 + 6 + 2 * nUnknownsCamParams;

			// Residuals & Jacobians:
			TResidualJacobianList res_jacob;
			err = recompute_errors_and_Jacobians(
				lm_stat, res_jacob, false /* no robust */,
				p.robust_kernel_param);

			// Build linear system:
			mrpt::math::CVectorDynamic<double> minus_g;	 // minus gradient
			build_linear_system(res_jacob, vars_to_optimize, minus_g, H);

			ASSERT_EQUAL_(nUnknowns, (size_t)H.cols());
			// Lev-Marq. parameters:
			double nu = 2;
			double lambda = tau * H.asEigen().diagonal().array().maxCoeff();
			bool done = (minus_g.array().abs().maxCoeff() < t1);
			int numItersImproving = 0;
			bool use_robust = false;

			// Lev-Marq. main loop:
			iter = 0;
			while (iter < p.maxIters && !done)
			{
				// User Callback?
				if (p.callback)
				{
					cbPars.current_iter = iter;
					cbPars.current_rmse = std::sqrt(err / nObs);
					(*p.callback)(cbPars, p.callback_user_param);
				}

				// Solve for increment: (H + \lambda I) eps = -gradient
				auto HH = H;
				for (size_t i = 0; i < nUnknowns; i++)
					HH(i, i) += lambda;
				// HH(i,i)*= (1.0 + lambda);

				Eigen::LLT<Eigen::MatrixXd> llt(
					HH.asEigen().selfadjointView<Eigen::Lower>());
				if (llt.info() != Eigen::Success)
				{
					lambda *= nu;
					nu *= 2;
					done = (lambda > MAX_LAMBDA);

					if (p.verbose && !done)
						cout << "LM iter#" << iter
							 << ": Couldn't solve LLt, retrying with larger "
								"lambda="
							 << lambda << endl;
					continue;
				}
				const auto eps = mrpt::math::CVectorDynamic<double>(
					llt.solve(minus_g.asEigen()).eval());

				const double eps_norm = eps.norm();
				if (eps_norm < t2 * (eps_norm + t2))
				{
					if (p.verbose)
						cout << "Termination criterion: eps_norm < "
								"t2*(eps_norm+t2): "
							 << eps_norm << " < " << t2 * (eps_norm + t2)
							 << endl;
					done = true;
					break;
				}

				// Tentative new state:
				lm_stat_t new_lm_stat(
					lm_stat);  // images,valid_image_pair_indices,obj_points);
				add_lm_increment(eps, vars_to_optimize, new_lm_stat);

				TResidualJacobianList new_res_jacob;

				// discriminant:
				double err_new = recompute_errors_and_Jacobians(
					new_lm_stat, new_res_jacob, use_robust,
					p.robust_kernel_param);

				if (err_new < err)
				{
					// const double l = (err-err_new)/ (eps.dot( lambda*eps +
					// minus_g) );

					if (numItersImproving++ > 5)
						use_robust = user_wants_use_robust;

					// Good: Accept new values
					if (p.verbose)
						cout << "LM iter#" << iter
							 << ": Avr.err(px):" << std::sqrt(err / nObs)
							 << "->" << std::sqrt(err_new / nObs)
							 << " lambda:" << lambda << endl;

					// swap: faster than "lm_stat <- new_lm_stat"
					lm_stat.swap(new_lm_stat);
					res_jacob.swap(new_res_jacob);

					err = err_new;
					build_linear_system(
						res_jacob, vars_to_optimize, minus_g, H);

					// Too small gradient?
					done = (minus_g.array().abs().maxCoeff() < t1);
					// lambda *= max(1.0/3.0, 1-std::pow(2*l-1,3.0) );
					lambda *= 0.6;
					lambda = std::max(lambda, 1e-100);
					nu = 2.0;

					iter++;
				}
				else
				{
					lambda *= nu;
					if (p.verbose)
						cout << "LM iter#" << iter << ": No update: err=" << err
							 << " -> err_new=" << err_new
							 << " retrying with larger lambda=" << lambda
							 << endl;
					nu *= 2.0;
					done = (lambda > MAX_LAMBDA);
				}

			}  // end while LevMarq.

		}  // end for each calibRound = [0,1]
		// -------------------------------------------------------------------------------
		//  End of Levenberg-Marquardt
		// -------------------------------------------------------------------------------

		// Save final optimum values to the output structure
		out.final_rmse = std::sqrt(err / nObs);
		out.final_iters = iter;
		out.final_number_good_image_pairs = N;

		// [fx fy cx cy k1 k2 k3 t1 t2]
		out.cam_params.leftCamera.fx(lm_stat.left_cam_params[0]);
		out.cam_params.leftCamera.fy(lm_stat.left_cam_params[1]);
		out.cam_params.leftCamera.cx(lm_stat.left_cam_params[2]);
		out.cam_params.leftCamera.cy(lm_stat.left_cam_params[3]);
		out.cam_params.leftCamera.k1(lm_stat.left_cam_params[4]);
		out.cam_params.leftCamera.k2(lm_stat.left_cam_params[5]);
		out.cam_params.leftCamera.k3(lm_stat.left_cam_params[6]);
		out.cam_params.leftCamera.p1(lm_stat.left_cam_params[7]);
		out.cam_params.leftCamera.p2(lm_stat.left_cam_params[8]);

		// [fx fy cx cy k1 k2 k3 t1 t2]
		out.cam_params.rightCamera.fx(lm_stat.right_cam_params[0]);
		out.cam_params.rightCamera.fy(lm_stat.right_cam_params[1]);
		out.cam_params.rightCamera.cx(lm_stat.right_cam_params[2]);
		out.cam_params.rightCamera.cy(lm_stat.right_cam_params[3]);
		out.cam_params.rightCamera.k1(lm_stat.right_cam_params[4]);
		out.cam_params.rightCamera.k2(lm_stat.right_cam_params[5]);
		out.cam_params.rightCamera.k3(lm_stat.right_cam_params[6]);
		out.cam_params.rightCamera.p1(lm_stat.right_cam_params[7]);
		out.cam_params.rightCamera.p2(lm_stat.right_cam_params[8]);

		const auto l2r_pose = -mrpt::poses::CPose3D(lm_stat.right2left_pose);
		mrpt::math::CQuaternionDouble l2r_quat;
		l2r_pose.getAsQuaternion(l2r_quat);

		out.cam_params.rightCameraPose.x = l2r_pose.x();
		out.cam_params.rightCameraPose.y = l2r_pose.y();
		out.cam_params.rightCameraPose.z = l2r_pose.z();
		out.cam_params.rightCameraPose.qr = l2r_quat.r();
		out.cam_params.rightCameraPose.qx = l2r_quat.x();
		out.cam_params.rightCameraPose.qy = l2r_quat.y();
		out.cam_params.rightCameraPose.qz = l2r_quat.z();

		// R2L pose:
		out.right2left_camera_pose =
			mrpt::poses::CPose3D(lm_stat.right2left_pose);

		// All the estimated camera poses:
		out.left_cam_poses = lm_stat.left_cam_poses;

		out.image_pair_was_used.assign(images.size(), false);
		for (unsigned long valid_image_pair_indice : valid_image_pair_indices)
			out.image_pair_was_used[valid_image_pair_indice] = true;

		// Uncertainties ---------------------
		// The order of inv. variances in the diagonal of the Hessian is:
		//  * N x Manifold Epsilon of left camera pose (6)
		//  * Manifold Epsilon of right2left pose (6)
		//  * Left-cam-params (<=9)
		//  * Right-cam-params (<=9)
		out.left_params_inv_variance.fill(0);
		out.right_params_inv_variance.fill(0);
		const size_t base_idx_H_CPs = H.cols() - 2 * nUnknownsCamParams;
		for (size_t i = 0; i < nUnknownsCamParams; i++)
		{
			out.left_params_inv_variance[vars_to_optimize[i]] =
				H(base_idx_H_CPs + i, base_idx_H_CPs + i);
			out.right_params_inv_variance[vars_to_optimize[i]] =
				H(base_idx_H_CPs + nUnknownsCamParams + i,
				  base_idx_H_CPs + nUnknownsCamParams + i);
		}

		// Draw projected points
		for (unsigned long idx : valid_image_pair_indices)
		{
			TImageCalibData& dat_l = images[idx].left;
			TImageCalibData& dat_r = images[idx].right;

			// Rectify image.
			dat_l.img_original.colorImage(dat_l.img_rectified);
			dat_r.img_original.colorImage(dat_r.img_rectified);

			// Camera poses:
			dat_l.reconstructed_camera_pose =
				-mrpt::poses::CPose3D(lm_stat.left_cam_poses[idx]);
			dat_r.reconstructed_camera_pose =
				-(mrpt::poses::CPose3D(lm_stat.right2left_pose) +
				  mrpt::poses::CPose3D(lm_stat.left_cam_poses[idx]));

			// Project distorted images:
			mrpt::vision::pinhole::projectPoints_with_distortion(
				obj_points, out.cam_params.leftCamera,
				CPose3DQuat(dat_l.reconstructed_camera_pose),
				dat_l.projectedPoints_distorted, true);
			mrpt::vision::pinhole::projectPoints_with_distortion(
				obj_points, out.cam_params.rightCamera,
				CPose3DQuat(dat_r.reconstructed_camera_pose),
				dat_r.projectedPoints_distorted, true);

			// Draw corners:
			dat_l.img_rectified.drawChessboardCorners(
				dat_l.projectedPoints_distorted, check_size.x, check_size.y);
			dat_r.img_rectified.drawChessboardCorners(
				dat_r.projectedPoints_distorted, check_size.x, check_size.y);
		}

		return true;
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
		return false;
	}
}

/*
Jacobian:

 d b( [px py pz] )
------------------  (5x3) =
  d{ px py pz }

\left(\begin{array}{ccc} \frac{1}{\mathrm{pz}} & 0 &
-\frac{\mathrm{px}}{{\mathrm{pz}}^2}\\ 0 & \frac{1}{\mathrm{pz}} &
-\frac{\mathrm{py}}{{\mathrm{pz}}^2} \end{array}\right)

*/

static void jacob_db_dp(
	const TPoint3D& p,	// 3D coordinates wrt the camera
	mrpt::math::CMatrixFixed<double, 2, 3>& G)
{
	const double pz_ = 1 / p.z;
	const double pz_2 = pz_ * pz_;

	G(0, 0) = pz_;
	G(0, 1) = 0;
	G(0, 2) = -p.x * pz_2;

	G(1, 0) = 0;
	G(1, 1) = pz_;
	G(1, 2) = -p.y * pz_2;
}

/*
Jacobian:

 dh( b,c )
----------- = Hb  | 2x2  (b=[x=px/pz y=py/pz])
  d{ b }

\left(\begin{array}{cc} \mathrm{fx}\, \left(\mathrm{k2}\, {\left(x^2 +
y^2\right)}^2 + \mathrm{k3}\, {\left(x^2 + y^2\right)}^3 + 6\, \mathrm{t2}\, x +
2\, \mathrm{t1}\, y + x\, \left(2\, \mathrm{k1}\, x + 4\, \mathrm{k2}\, x\,
\left(x^2 + y^2\right) + 6\, \mathrm{k3}\, x\, {\left(x^2 + y^2\right)}^2\right)
+ \mathrm{k1}\, \left(x^2 + y^2\right) + 1\right) & \mathrm{fx}\, \left(2\,
\mathrm{t1}\, x + 2\, \mathrm{t2}\, y + x\, \left(2\, \mathrm{k1}\, y + 4\,
\mathrm{k2}\, y\, \left(x^2 + y^2\right) + 6\, \mathrm{k3}\, y\, {\left(x^2 +
y^2\right)}^2\right)\right)\\ \mathrm{fy}\, \left(2\, \mathrm{t1}\, x + 2\,
\mathrm{t2}\, y + y\, \left(2\, \mathrm{k1}\, x + 4\, \mathrm{k2}\, x\,
\left(x^2 + y^2\right) + 6\, \mathrm{k3}\, x\, {\left(x^2 +
y^2\right)}^2\right)\right) & \mathrm{fy}\, \left(\mathrm{k2}\, {\left(x^2 +
y^2\right)}^2 + \mathrm{k3}\, {\left(x^2 + y^2\right)}^3 + 2\, \mathrm{t2}\, x +
6\, \mathrm{t1}\, y + y\, \left(2\, \mathrm{k1}\, y + 4\, \mathrm{k2}\, y\,
\left(x^2 + y^2\right) + 6\, \mathrm{k3}\, y\, {\left(x^2 + y^2\right)}^2\right)
+ \mathrm{k1}\, \left(x^2 + y^2\right) + 1\right) \end{array}\right)

 dh( b,c )
----------- = Hc  | 2x9
  d{ c }

\left(\begin{array}{ccccccccc} \mathrm{t2}\, \left(3\, x^2 + y^2\right) + x\,
\left(\mathrm{k2}\, {\left(x^2 + y^2\right)}^2 + \mathrm{k3}\, {\left(x^2 +
y^2\right)}^3 + \mathrm{k1}\, \left(x^2 + y^2\right) + 1\right) + 2\,
\mathrm{t1}\, x\, y & 0 & 1 & 0 & \mathrm{fx}\, x\, \left(x^2 + y^2\right) &
\mathrm{fx}\, x\, {\left(x^2 + y^2\right)}^2 & \mathrm{fx}\, x\, {\left(x^2 +
y^2\right)}^3 & 2\, \mathrm{fx}\, x\, y & \mathrm{fx}\, \left(3\, x^2 +
y^2\right)\\ 0 & \mathrm{t1}\, \left(x^2 + 3\, y^2\right) + y\,
\left(\mathrm{k2}\, {\left(x^2 + y^2\right)}^2 + \mathrm{k3}\, {\left(x^2 +
y^2\right)}^3 + \mathrm{k1}\, \left(x^2 + y^2\right) + 1\right) + 2\,
\mathrm{t2}\, x\, y & 0 & 1 & \mathrm{fy}\, y\, \left(x^2 + y^2\right) &
\mathrm{fy}\, y\, {\left(x^2 + y^2\right)}^2 & \mathrm{fy}\, y\, {\left(x^2 +
y^2\right)}^3 & \mathrm{fy}\, \left(x^2 + 3\, y^2\right) & 2\, \mathrm{fy}\, x\,
y \end{array}\right)

*/

static void jacob_dh_db_and_dh_dc(
	const TPoint3D& nP,	 // Point in relative coords wrt the camera
	const mrpt::math::CVectorFixedDouble<9>& c,	 // camera parameters
	mrpt::math::CMatrixFixed<double, 2, 2>& Hb,
	mrpt::math::CMatrixFixed<double, 2, 9>& Hc)
{
	const double x = nP.x / nP.z;
	const double y = nP.y / nP.z;

	const double r2 = x * x + y * y;
	const double r = std::sqrt(r2);
	const double r6 = r2 * r2 * r2;	 // (x^2+y^2)^3 = r^6

	// c=[fx fy cx cy k1 k2 k3 t1 t2]
	const double fx = c[0], fy = c[1];
	// const double cx=c[2], cy=c[3]; // Un-unused
	const double k1 = c[4], k2 = c[5], k3 = c[6];
	const double t1 = c[7], t2 = c[8];

	// Hb = dh(b,c)/db  (2x2)
	Hb(0, 0) = fx *
		(k2 * r2 + k3 * r6 + 6 * t2 * x + 2 * t1 * y +
		 x * (2 * k1 * x + 4 * k2 * x * r + 6 * k3 * x * r2) + k1 * r + 1);
	Hb(0, 1) = fx *
		(2 * t1 * x + 2 * t2 * y +
		 x * (2 * k1 * y + 4 * k2 * y * r + 6 * k3 * y * r2));

	Hb(1, 0) = fy *
		(2 * t1 * x + 2 * t2 * y +
		 y * (2 * k1 * x + 4 * k2 * x * r + 6 * k3 * x * r2));
	Hb(1, 1) = fy *
		(k2 * r2 + k3 * r6 + 2 * t2 * x + 6 * t1 * y +
		 y * (2 * k1 * y + 4 * k2 * y * r + 6 * k3 * y * r2) + k1 * r + 1);

	// Hc = dh(b,c)/dc  (2x9)
	Hc(0, 0) = t2 * (3 * x * x + y * y) + x * (k2 * r2 + k3 * r6 + k1 * r + 1) +
		2 * t1 * x * y;
	Hc(0, 1) = 0;
	Hc(0, 2) = 1;
	Hc(0, 3) = 0;
	Hc(0, 4) = fx * x * r;
	Hc(0, 5) = fx * x * r2;
	Hc(0, 6) = fx * x * r6;
	Hc(0, 7) = 2 * fx * x * y;
	Hc(0, 8) = fx * (3 * x * x + y * y);

	Hc(1, 0) = 0;
	Hc(1, 1) = t1 * (x * x + 3 * y * y) + y * (k2 * r2 + k3 * r6 + k1 * r + 1) +
		2 * t2 * x * y;
	Hc(1, 2) = 0;
	Hc(1, 3) = 1;
	Hc(1, 4) = fy * y * r;
	Hc(1, 5) = fy * y * r2;
	Hc(1, 6) = fy * y * r6;
	Hc(1, 7) = fy * (x * x + 3 * y * y);
	Hc(1, 8) = 2 * fy * x * y;
}

static void jacob_deps_D_p_deps(
	const TPoint3D& p_D,  // D (+) p
	mrpt::math::CMatrixFixed<double, 3, 6>& dpl_del)
{
	// Jacobian 10.3.4 in technical report "A tutorial on SE(3) transformation
	// parameterizations and on-manifold optimization"
	dpl_del.block<3, 3>(0, 0).setIdentity();
	dpl_del.block<3, 3>(0, 3) = mrpt::math::skew_symmetric3_neg(p_D).asEigen();
}

static void jacob_dA_eps_D_p_deps(
	const CPose3D& A, const CPose3D& D, const TPoint3D& p,
	mrpt::math::CMatrixFixed<double, 3, 6>& dp_deps)
{
	// Jacobian 10.3.7 in technical report "A tutorial on SE(3) transformation
	// parameterizations and on-manifold optimization"
	const Eigen::Matrix<double, 1, 3> P(p.x, p.y, p.z);
	const auto dr1 = D.getRotationMatrix().asEigen().block<1, 3>(0, 0);
	const auto dr2 = D.getRotationMatrix().asEigen().block<1, 3>(1, 0);
	const auto dr3 = D.getRotationMatrix().asEigen().block<1, 3>(2, 0);

	const Eigen::Matrix<double, 1, 3> v(
		P.dot(dr1) + D.x(), P.dot(dr2) + D.y(), P.dot(dr3) + D.z());

	Eigen::Matrix<double, 3, 6> H;
	H.block<3, 3>(0, 0).setIdentity();
	H.block<3, 3>(0, 3) = mrpt::math::skew_symmetric3_neg(v).asEigen();

	dp_deps = A.getRotationMatrix().asEigen() * H;
}

static void project_point(
	const mrpt::math::TPoint3D& P, const mrpt::img::TCamera& params,
	const mrpt::math::TPose3D& cameraPose, mrpt::img::TPixelCoordf& px)
{
	// Change the reference system to that wrt the camera
	TPoint3D nP;
	cameraPose.composePoint(P, nP);

	// Pinhole model:
	const double x = nP.x / nP.z;
	const double y = nP.y / nP.z;

	// Radial distortion:
	const double r2 = square(x) + square(y);
	const double r4 = square(r2);
	const double r6 = r2 * r4;
	const double A =
		1 + params.dist[0] * r2 + params.dist[1] * r4 + params.dist[4] * r6;
	const double B = 2 * x * y;

	px.x = params.cx() +
		params.fx() *
			(x * A + params.dist[2] * B +
			 params.dist[3] * (r2 + 2 * square(x)));
	px.y = params.cy() +
		params.fy() *
			(y * A + params.dist[3] * B +
			 params.dist[2] * (r2 + 2 * square(y)));
}

// Build the "-gradient" and the Hessian matrix:
// Variables are in these order:
//  * N x Manifold Epsilon of left camera pose (6)
//  * Manifold Epsilon of right2left pose (6)
//  * Left-cam-params (<=9)
//  * Right-cam-params (<=9)
void mrpt::vision::build_linear_system(
	const TResidualJacobianList& res_jac, const std::vector<size_t>& var_indxs,
	mrpt::math::CVectorDynamic<double>& minus_g, mrpt::math::CMatrixDouble& H)
{
	const size_t N = res_jac.size();  // Number of stereo image pairs
	const size_t nMaxUnknowns = N * 6 + 6 + 9 + 9;

	// Reset to zeros:
	auto minus_g_tot =
		mrpt::math::CVectorDynamic<double>::Zero(nMaxUnknowns, 1);
	auto H_tot = mrpt::math::CMatrixDouble::Zero(nMaxUnknowns, nMaxUnknowns);

	// Sum the contribution from each observation:
	for (size_t i = 0; i < N; i++)
	{
		const size_t nObs = res_jac[i].size();
		for (size_t j = 0; j < nObs; j++)
		{
			const TResidJacobElement& rje = res_jac[i][j];
			// Sub-Hessian & sub-gradient are variables in this order:
			//  eps_left_cam, eps_right2left_cam, left_cam_params,
			//  right_cam_params
			const Eigen::Matrix<double, 30, 30> Hij = rje.J.transpose() * rje.J;
			const Eigen::Matrix<double, 30, 1> gij =
				rje.J.transpose() * rje.residual;

			// Assemble in their place:
			minus_g_tot.block<6, 1>(i * 6, 0) -= gij.block<6, 1>(0, 0);
			minus_g_tot.block<6 + 9 + 9, 1>(N * 6, 0) -=
				gij.block<6 + 9 + 9, 1>(6, 0);

			H_tot.block<6, 6>(i * 6, i * 6) += Hij.block<6, 6>(0, 0);

			H_tot.block<6 + 9 + 9, 6 + 9 + 9>(N * 6, N * 6) +=
				Hij.block<6 + 9 + 9, 6 + 9 + 9>(6, 6);
			H_tot.block<6, 6 + 9 + 9>(i * 6, N * 6) +=
				Hij.block<6, 6 + 9 + 9>(0, 6);
			H_tot.block<6 + 9 + 9, 6>(N * 6, i * 6) +=
				Hij.block<6 + 9 + 9, 6>(6, 0);
		}
	}

#if 0
	// Also include additional cost terms to stabilize estimation:
	// -----------------------------------------------------------------
	// In the left->right pose increment: Add cost if we know that both cameras are almost parallel:
	const double cost_lr_angular = 1e10;
	H_tot.block<3,3>(N*6+3,N*6+3) += Eigen::Matrix<double,3,3>::Identity() * cost_lr_angular;
#endif

	// Ignore those derivatives of "fixed" (non-optimizable) variables in camera
	// parameters
	// --------------------------------------------------------------------------------------
	const size_t N_Cs = var_indxs.size();  // [0-8]
	const size_t nUnknowns = N * 6 + 6 + 2 * N_Cs;
	const size_t nUnkPoses = N * 6 + 6;

	minus_g.setZero(nUnknowns, 1);
	H.setZero(nUnknowns, nUnknowns);
	// Copy unmodified all but the cam. params:
	minus_g.asEigen().block(0, 0, nUnkPoses, 1) =
		minus_g_tot.asEigen().block(0, 0, nUnkPoses, 1);
	H.asEigen().block(0, 0, nUnkPoses, nUnkPoses) =
		H_tot.asEigen().block(0, 0, nUnkPoses, nUnkPoses);

	// Selective copy cam. params parts:
	for (size_t i = 0; i < N_Cs; i++)
	{
		minus_g[nUnkPoses + i] = minus_g_tot[nUnkPoses + var_indxs[i]];
		minus_g[nUnkPoses + N_Cs + i] =
			minus_g_tot[nUnkPoses + 9 + var_indxs[i]];
	}

	for (size_t k = 0; k < nUnkPoses; k++)
	{
		for (size_t i = 0; i < N_Cs; i++)
		{
			H(nUnkPoses + i, k) = H(k, nUnkPoses + i) =
				H_tot(k, nUnkPoses + var_indxs[i]);
			H(nUnkPoses + i + N_Cs, k) = H(k, nUnkPoses + i + N_Cs) =
				H_tot(k, nUnkPoses + 9 + var_indxs[i]);
		}
	}

	for (size_t i = 0; i < N_Cs; i++)
	{
		for (size_t j = 0; j < N_Cs; j++)
		{
			H(nUnkPoses + i, nUnkPoses + j) =
				H_tot(nUnkPoses + var_indxs[i], nUnkPoses + var_indxs[j]);
			H(nUnkPoses + N_Cs + i, nUnkPoses + N_Cs + j) = H_tot(
				nUnkPoses + 9 + var_indxs[i], nUnkPoses + 9 + var_indxs[j]);
		}
	}

#if 0
	{
		CMatrixDouble M1 = H_tot;
		CMatrixDouble g1 = minus_g_tot;
		M1.saveToTextFile("H1.txt");
		g1.saveToTextFile("g1.txt");

		CMatrixDouble M2 = H;
		CMatrixDouble g2 = minus_g;
		M2.saveToTextFile("H2.txt");
		g2.saveToTextFile("g2.txt");
		mrpt::system::pause();
	}
#endif

}  // end of build_linear_system

//  * N x Manifold Epsilon of left camera pose (6)
//  * Manifold Epsilon of right2left pose (6)
//  * Left-cam-params (<=9)
//  * Right-cam-params (<=9)
void mrpt::vision::add_lm_increment(
	const mrpt::math::CVectorDynamic<double>& eps,
	const std::vector<size_t>& var_indxs, lm_stat_t& lm_stat)
{
	// Increment of the N cam poses
	const size_t N = lm_stat.valid_image_pair_indices.size();
	for (size_t i = 0; i < N; i++)
	{
		auto& cam_pose =
			lm_stat.left_cam_poses[lm_stat.valid_image_pair_indices[i]];

		// Use the Lie Algebra methods for the increment:
		const CVectorFixedDouble<6> incr(eps.asEigen().block<6, 1>(6 * i, 0));
		const CPose3D incrPose = Lie::SE<3>::exp(incr);

		// new_pose =  old_pose  [+] delta
		//         = exp(delta) (+) old_pose
		cam_pose = (incrPose + CPose3D(cam_pose)).asTPose();
	}

	// Increment of the right-left pose:
	{
		// Use the Lie Algebra methods for the increment:
		const CVectorFixedDouble<6> incr(eps.asEigen().block<6, 1>(6 * N, 0));
		const CPose3D incrPose = Lie::SE<3>::exp(incr);

		// new_pose =  old_pose  [+] delta
		//         = exp(delta) (+) old_pose
		lm_stat.right2left_pose =
			(incrPose + CPose3D(lm_stat.right2left_pose)).asTPose();
	}

	// Increment of the camera params:
	const size_t idx = 6 * N + 6;
	const size_t nPC =
		var_indxs.size();  // Number of camera parameters being optimized
	for (size_t i = 0; i < nPC; i++)
	{
		lm_stat.left_cam_params[var_indxs[i]] += eps[idx + i];
		lm_stat.right_cam_params[var_indxs[i]] += eps[idx + nPC + i];
	}

}  // end of add_lm_increment

#ifdef USE_NUMERIC_JACOBIANS
// ---------------------------------------------------------------
// Aux. function, only if we evaluate Jacobians numerically:
// ---------------------------------------------------------------
struct TNumJacobData
{
	const lm_stat_t& lm_stat;
	const TPoint3D& obj_point;
	const CPose3D& left_cam;
	const CPose3D& right2left;
	const Eigen::Matrix<double, 4, 1>& real_obs;

	TNumJacobData(
		const lm_stat_t& _lm_stat, const TPoint3D& _obj_point,
		const CPose3D& _left_cam, const CPose3D& _right2left,
		const Eigen::Matrix<double, 4, 1>& _real_obs)
		: lm_stat(_lm_stat),
		  obj_point(_obj_point),
		  left_cam(_left_cam),
		  right2left(_right2left),
		  real_obs(_real_obs)
	{
	}
};

void numeric_jacob_eval_function(
	const CVectorFixedDouble<30>& x, const TNumJacobData& dat,
	CVectorFixedDouble<4>& out)
{
	// Recover the state out from "x":
	const CVectorFixedDouble<6> incr_l(&x[0]);
	const CPose3D incrPose_l = Lie::SE<3>::exp(incr_l);
	const CVectorFixedDouble<6> incr_rl(&x[6]);
	const CPose3D incrPose_rl = Lie::SE<3>::exp(incr_rl);

	// [fx fy cx cy k1 k2 k3 t1 t2]
	TStereoCamera cam_params;
	CVectorFixedDouble<9> left_cam_params = x.segment<9>(6 + 6);
	cam_params.leftCamera.fx(left_cam_params[0]);
	cam_params.leftCamera.fy(left_cam_params[1]);
	cam_params.leftCamera.cx(left_cam_params[2]);
	cam_params.leftCamera.cy(left_cam_params[3]);
	cam_params.leftCamera.k1(left_cam_params[4]);
	cam_params.leftCamera.k2(left_cam_params[5]);
	cam_params.leftCamera.k3(left_cam_params[6]);
	cam_params.leftCamera.p1(left_cam_params[7]);
	cam_params.leftCamera.p2(left_cam_params[8]);

	// [fx fy cx cy k1 k2 k3 t1 t2]
	CVectorFixedDouble<9> right_cam_params = x.segment<9>(6 + 6 + 9);
	cam_params.rightCamera.fx(right_cam_params[0]);
	cam_params.rightCamera.fy(right_cam_params[1]);
	cam_params.rightCamera.cx(right_cam_params[2]);
	cam_params.rightCamera.cy(right_cam_params[3]);
	cam_params.rightCamera.k1(right_cam_params[4]);
	cam_params.rightCamera.k2(right_cam_params[5]);
	cam_params.rightCamera.k3(right_cam_params[6]);
	cam_params.rightCamera.p1(right_cam_params[7]);
	cam_params.rightCamera.p2(right_cam_params[8]);

	TPixelCoordf px_l;
	project_point(
		dat.obj_point, cam_params.leftCamera, incrPose_l + dat.left_cam, px_l);
	TPixelCoordf px_r;
	project_point(
		dat.obj_point, cam_params.rightCamera,
		incrPose_rl + dat.right2left + incrPose_l + dat.left_cam, px_r);

	const Eigen::Matrix<double, 4, 1> predicted_obs(
		px_l.x, px_l.y, px_r.x, px_r.y);

	// Residual:
	out = predicted_obs;  // - dat.real_obs;
}

void eval_h_b(
	const CVectorFixedDouble<2>& X, const TCamera& params,
	CVectorFixedDouble<2>& out)
{
	// Radial distortion:
	const double x = X[0], y = X[1];
	const double r2 = square(x) + square(y);
	const double r4 = square(r2);
	const double r6 = r2 * r4;
	const double A =
		1 + params.dist[0] * r2 + params.dist[1] * r4 + params.dist[4] * r6;
	const double B = 2 * x * y;

	out[0] = params.cx() +
		params.fx() *
			(x * A + params.dist[2] * B +
			 params.dist[3] * (r2 + 2 * square(x)));
	out[1] = params.cy() +
		params.fy() *
			(y * A + params.dist[3] * B +
			 params.dist[2] * (r2 + 2 * square(y)));
}

void eval_b_p(
	const CVectorFixedDouble<3>& P, int dummy, CVectorFixedDouble<2>& b)
{
	// Radial distortion:
	b[0] = P[0] / P[2];
	b[1] = P[1] / P[2];
}

void eval_deps_D_p(
	const CVectorFixedDouble<6>& eps, const TPoint3D& D_p,
	CVectorFixedDouble<3>& out)
{
	const CVectorFixedDouble<6> incr(&eps[0]);
	const CPose3D incrPose = Lie::SE<3>::exp(incr);
	TPoint3D D_p_out;
	incrPose.composePoint(D_p, D_p_out);
	for (int i = 0; i < 3; i++)
		out[i] = D_p_out[i];
}

struct TEvalData_A_eps_D_p
{
	CPose3D A, D;
	TPoint3D p;
};

void eval_dA_eps_D_p(
	const CVectorFixedDouble<6>& eps, const TEvalData_A_eps_D_p& dat,
	CVectorFixedDouble<3>& out)
{
	const CVectorFixedDouble<6> incr(&eps[0]);
	const CPose3D incrPose = Lie::SE<3>::exp(incr);
	const CPose3D A_eps_D = dat.A + (incrPose + dat.D);
	TPoint3D pt;
	A_eps_D.composePoint(dat.p, pt);
	for (int i = 0; i < 3; i++)
		out[i] = pt[i];
}
// ---------------------------------------------------------------
// End of aux. function, only if we evaluate Jacobians numerically:
// ---------------------------------------------------------------
#endif

// the 4x1 prediction are the (u,v) pixel coordinates for the left / right
// cameras:
double mrpt::vision::recompute_errors_and_Jacobians(
	const lm_stat_t& lm_stat, TResidualJacobianList& res_jac,
	bool use_robust_kernel, double kernel_param)
{
	double total_err = 0;
	const size_t N = lm_stat.valid_image_pair_indices.size();
	res_jac.resize(N);

	// Parse lm_stat data: CVectorFixedDouble<9> left_cam_params,
	// right_cam_params; // [fx fy cx cy k1 k2 k3 t1 t2]
	TCamera camparam_l;
	camparam_l.fx(lm_stat.left_cam_params[0]);
	camparam_l.fy(lm_stat.left_cam_params[1]);
	camparam_l.cx(lm_stat.left_cam_params[2]);
	camparam_l.cy(lm_stat.left_cam_params[3]);
	camparam_l.k1(lm_stat.left_cam_params[4]);
	camparam_l.k2(lm_stat.left_cam_params[5]);
	camparam_l.k3(lm_stat.left_cam_params[6]);
	camparam_l.p1(lm_stat.left_cam_params[7]);
	camparam_l.p2(lm_stat.left_cam_params[8]);
	TCamera camparam_r;
	camparam_r.fx(lm_stat.right_cam_params[0]);
	camparam_r.fy(lm_stat.right_cam_params[1]);
	camparam_r.cx(lm_stat.right_cam_params[2]);
	camparam_r.cy(lm_stat.right_cam_params[3]);
	camparam_r.k1(lm_stat.right_cam_params[4]);
	camparam_r.k2(lm_stat.right_cam_params[5]);
	camparam_r.k3(lm_stat.right_cam_params[6]);
	camparam_r.p1(lm_stat.right_cam_params[7]);
	camparam_r.p2(lm_stat.right_cam_params[8]);

	// process all points from all N image pairs:
	for (size_t k = 0; k < N; k++)
	{
		const size_t k_idx = lm_stat.valid_image_pair_indices[k];
		const size_t nPts = lm_stat.obj_points.size();
		res_jac[k].resize(nPts);
		// For each point in the pattern:
		for (size_t i = 0; i < nPts; i++)
		{
			// Observed corners:
			const TPixelCoordf& px_obs_l =
				lm_stat.images[k_idx].left.detected_corners[i];
			const TPixelCoordf& px_obs_r =
				lm_stat.images[k_idx].right.detected_corners[i];
			const Eigen::Matrix<double, 4, 1> obs(
				px_obs_l.x, px_obs_l.y, px_obs_r.x, px_obs_r.y);

			TResidJacobElement& rje = res_jac[k][i];

			// Predict i'th corner on left & right cameras:
			// ---------------------------------------------
			TPixelCoordf px_l, px_r;
			project_point(
				lm_stat.obj_points[i], camparam_l,
				lm_stat.left_cam_poses[k_idx], px_l);

			TPose3D auxPose;
			lm_stat.right2left_pose.composePose(
				lm_stat.left_cam_poses[k_idx], auxPose);

			project_point(lm_stat.obj_points[i], camparam_r, auxPose, px_r);
			rje.predicted_obs =
				Eigen::Matrix<double, 4, 1>(px_l.x, px_l.y, px_r.x, px_r.y);

			// Residual:
			rje.residual = rje.predicted_obs - obs;

			// Accum. total squared error:
			const double err_sqr_norm = rje.residual.squaredNorm();
			if (use_robust_kernel)
			{
				RobustKernel<rkPseudoHuber> rk;
				rk.param_sq = kernel_param;

				double kernel_1st_deriv, kernel_2nd_deriv;
				const double scaled_err =
					rk.eval(err_sqr_norm, kernel_1st_deriv, kernel_2nd_deriv);

				rje.residual *= kernel_1st_deriv;
				total_err += scaled_err;
			}
			else
				total_err += err_sqr_norm;

// ---------------------------------------------------------------------------------
// Jacobian: (4x30)
// See technical report cited in the headers for the theory behind these
// formulas.
// ---------------------------------------------------------------------------------
#if !defined(USE_NUMERIC_JACOBIANS) || defined(COMPARE_NUMERIC_JACOBIANS)
			// ----- Theoretical Jacobians -----
			mrpt::math::CMatrixFixed<double, 2, 6> dhl_del, dhr_del, dhr_der;
			mrpt::math::CMatrixFixed<double, 2, 9> dhl_dcl, dhr_dcr;

			// 3D coordinates of the corner point wrt both cameras:
			TPoint3D pt_wrt_left, pt_wrt_right;
			lm_stat.left_cam_poses[k_idx].composePoint(
				lm_stat.obj_points[i], pt_wrt_left);

			lm_stat.right2left_pose.composePose(
				lm_stat.left_cam_poses[k_idx], auxPose);

			auxPose.composePoint(lm_stat.obj_points[i], pt_wrt_right);

			// Build partial Jacobians:
			mrpt::math::CMatrixFixed<double, 2, 2> dhl_dbl, dhr_dbr;
			jacob_dh_db_and_dh_dc(
				pt_wrt_left, lm_stat.left_cam_params, dhl_dbl, dhl_dcl);
			jacob_dh_db_and_dh_dc(
				pt_wrt_right, lm_stat.right_cam_params, dhr_dbr, dhr_dcr);

			mrpt::math::CMatrixFixed<double, 2, 3> dbl_dpl, dbr_dpr;
			jacob_db_dp(pt_wrt_left, dbl_dpl);
			jacob_db_dp(pt_wrt_right, dbr_dpr);

			// p_l = exp(epsilon_l) (+) pose_left (+) point_ij
			// p_l = [exp(epsilon_r) (+) pose_right2left] (+) [exp(epsilon_l)
			// (+) pose_left] (+) point_ij
			mrpt::math::CMatrixFixed<double, 3, 6> dpl_del, dpr_del, dpr_der;
			jacob_deps_D_p_deps(pt_wrt_left, dpl_del);
			jacob_deps_D_p_deps(pt_wrt_right, dpr_der);

			const auto r2l_p = mrpt::poses::CPose3D(lm_stat.right2left_pose);
			const auto lcp =
				mrpt::poses::CPose3D(lm_stat.left_cam_poses[k_idx]);
			jacob_dA_eps_D_p_deps(r2l_p, lcp, lm_stat.obj_points[i], dpr_del);

			// Jacobian chain rule:
			dhl_del = dhl_dbl.asEigen() * dbl_dpl.asEigen() * dpl_del.asEigen();
			dhr_del = dhr_dbr.asEigen() * dbr_dpr.asEigen() * dpr_del.asEigen();
			dhr_der = dhr_dbr.asEigen() * dbr_dpr.asEigen() * dpr_der.asEigen();

			rje.J.setZero(4, 30);
			rje.J.block<2, 6>(0, 0) = dhl_del.asEigen();
			rje.J.block<2, 6>(2, 0) = dhr_del.asEigen();
			rje.J.block<2, 6>(2, 6) = dhr_der.asEigen();
			rje.J.block<2, 9>(0, 12) = dhl_dcl.asEigen();
			rje.J.block<2, 9>(2, 21) = dhr_dcr.asEigen();

#if defined(COMPARE_NUMERIC_JACOBIANS)
			const mrpt::math::CMatrixFixed<double, 4, 30> J_theor = rje.J;
#endif
// ---- end of theoretical Jacobians ----
#endif

#if defined(USE_NUMERIC_JACOBIANS) || defined(COMPARE_NUMERIC_JACOBIANS)
			// ----- Numeric Jacobians ----

			CVectorFixedDouble<30>
				x0;	 // eps_l (6) + eps_lr (6) + l_camparams (9) +
			// r_camparams (9)
			x0.setZero();
			x0.segment<9>(6 + 6) = lm_stat.left_cam_params;
			x0.segment<9>(6 + 6 + 9) = lm_stat.right_cam_params;

			const double x_incrs_val[30] = {
				1e-6, 1e-6, 1e-6, 1e-7, 1e-7, 1e-7,	 // eps_l
				1e-6, 1e-6, 1e-6, 1e-7, 1e-7, 1e-7,	 // eps_r
				1e-3, 1e-3, 1e-3, 1e-3, 1e-8, 1e-8, 1e-8, 1e-8, 1e-4,  // cam_l
				1e-3, 1e-3, 1e-3, 1e-3, 1e-8, 1e-8, 1e-8, 1e-8, 1e-4  // cam_rl
			};
			const CVectorFixedDouble<30> x_incrs(x_incrs_val);
			TNumJacobData dat(
				lm_stat, lm_stat.obj_points[i], lm_stat.left_cam_poses[k_idx],
				lm_stat.right2left_pose, obs);

			mrpt::math::estimateJacobian(
				x0, &numeric_jacob_eval_function, x_incrs, dat, rje.J);

#if defined(COMPARE_NUMERIC_JACOBIANS)
			const mrpt::math::CMatrixFixed<double, 4, 30> J_num = rje.J;
#endif
#endif	// ---- end of numeric Jacobians ----

// Only for debugging:
#if defined(COMPARE_NUMERIC_JACOBIANS)
			// if ( (J_num-J_theor).array().abs().maxCoeff()>1e-2)
			{
				ofstream f;
				f.open("dbg.txt", ios_base::out | ios_base::app);
				f << "J_num:\n"
				  << J_num << endl
				  << "J_theor:\n"
				  << J_theor << endl
				  << "diff:\n"
				  << J_num - J_theor << endl
				  << "diff (ratio):\n"
				  << (J_num - J_theor).cwiseQuotient(J_num) << endl
				  << endl;
			}
#endif
		}  // for i
	}  // for k

	return total_err;
}  // end of recompute_errors_and_Jacobians

// Ctor:
TStereoCalibParams::TStereoCalibParams() = default;

TStereoCalibResults::TStereoCalibResults() = default;
