/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "vision-precomp.h"   // Precompiled headers

#include <mrpt/system/filesystem.h>
#include <mrpt/utils/CConfigFileMemory.h>

#include <mrpt/vision/chessboard_find_corners.h>
#include <mrpt/vision/chessboard_stereo_camera_calib.h>
#include <mrpt/vision/pinhole.h>
#include <mrpt/math/robust_kernels.h>
#include <mrpt/math/wrap2pi.h>
#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/jacobians.h>
#endif

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::utils;
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)
		{
			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 mrpt::math::TPoint2D
					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 mrpt::math::TPoint2D Al = pt_l1 - pt_l0;
				const mrpt::math::TPoint2D Ar = pt_r1 - pt_r0;

				// 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." << std::endl;
		if (valid_image_pair_indices.empty() )
		{
			std::cout << "ERROR: No valid images. Perhaps the checkerboard size is incorrect?" << std::endl;
			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.push_back( TPoint3D(
					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: CArrayDouble<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;
		vector_size_t  vars_to_optimize;
		Eigen::MatrixXd  H;       // Hessian matrix  (Declared here so it's accessible as the final uncertainty measure)

		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:
			Eigen::VectorXd  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.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
				Eigen::MatrixXd  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.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 Eigen::VectorXd eps = llt.solve(minus_g);

				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] );

		// R2L pose:
		out.right2left_camera_pose = 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 (size_t i=0;i<valid_image_pair_indices.size();i++)
			out.image_pair_was_used[valid_image_pair_indices[i]]=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.setConstant(0);
		out.right_params_inv_variance.setConstant(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 (size_t i=0;i<valid_image_pair_indices.size();i++)
		{
			//mrpt::poses::CPose3D			reconstructed_camera_pose;   //!< At output, the reconstructed pose of the camera.
			//std::vector<TPixelCoordf>		projectedPoints_distorted;   //!< At output, only will have an empty vector if the checkerboard was not found in this image, or the predicted (reprojected) corners, which were used to estimate the average square error.
			//std::vector<TPixelCoordf>		projectedPoints_undistorted; //!< At output, like projectedPoints_distorted but for the undistorted image.
			const size_t idx = valid_image_pair_indices[i];

			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 = - lm_stat.left_cam_poses[idx];
			dat_r.reconstructed_camera_pose = -(lm_stat.right2left_pose + 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(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)

*/

void jacob_db_dp(
	const TPoint3D &p,   // 3D coordinates wrt the camera
	Eigen::Matrix<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)

*/

void jacob_dh_db_and_dh_dc(
	const TPoint3D & nP,  // Point in relative coords wrt the camera
	const Eigen::Matrix<double,9,1>  & c,  // camera parameters
	Eigen::Matrix<double,2,2>  & Hb,
	Eigen::Matrix<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;

}


void jacob_deps_D_p_deps(
	const TPoint3D &p_D,   // D (+) p
	Eigen::Matrix<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);
}

void jacob_dA_eps_D_p_deps(
	const CPose3D  &A,
	const CPose3D  &D,
	const TPoint3D &p,
	Eigen::Matrix<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 Eigen::Matrix<double,1,3> dr1 = D.getRotationMatrix().block<1,3>(0,0);
	const Eigen::Matrix<double,1,3> dr2 = D.getRotationMatrix().block<1,3>(1,0);
	const Eigen::Matrix<double,1,3> dr3 = D.getRotationMatrix().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);

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


void project_point(
	const mrpt::math::TPoint3D & P,
	const mrpt::utils::TCamera & params,
	const CPose3D              & cameraPose,
	mrpt::utils::TPixelCoordf & px
	)
{
	// Change the reference system to that wrt the camera
	TPoint3D nP;
	cameraPose.composePoint( P.x, P.y, P.z, nP.x, nP.y, nP.z );

	// 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 vector_size_t & var_indxs,
	Eigen::VectorXd &minus_g,
	Eigen::MatrixXd &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:
	Eigen::VectorXd minus_g_tot = Eigen::VectorXd::Zero(nMaxUnknowns);
	Eigen::MatrixXd H_tot       = Eigen::MatrixXd::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);
	H.setZero(nUnknowns,nUnknowns);
	// Copy unmodified all but the cam. params:
	minus_g.block(0,0, nUnkPoses,1 )  = minus_g_tot.block(0,0, nUnkPoses,1);
	H.block(0,0, nUnkPoses,nUnkPoses) = H_tot.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 Eigen::VectorXd & eps,
	const 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++)
	{
		CPose3D &cam_pose = lm_stat.left_cam_poses[ lm_stat.valid_image_pair_indices[i] ];

		// Use the Lie Algebra methods for the increment:
		const CArrayDouble<6> incr( &eps[6*i] );
		const CPose3D         incrPose = CPose3D::exp(incr);

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

	// Increment of the right-left pose:
	{
		// Use the Lie Algebra methods for the increment:
		const CArrayDouble<6> incr( &eps[6*N] );
		const CPose3D         incrPose = CPose3D::exp(incr);

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

	// 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 CArrayDouble<30> &x,
		const TNumJacobData &dat,
		CArrayDouble<4> &out)
	{
		// Recover the state out from "x":
		const CArrayDouble<6> incr_l( &x[0] );
		const CPose3D         incrPose_l = CPose3D::exp(incr_l);
		const CArrayDouble<6> incr_rl( &x[6] );
		const CPose3D         incrPose_rl = CPose3D::exp(incr_rl);

		// [fx fy cx cy k1 k2 k3 t1 t2]
		TStereoCamera cam_params;
		CArrayDouble<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]
		CArrayDouble<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 CArrayDouble<2> &X,
		const TCamera &params,
		CArrayDouble<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 CArrayDouble<3> &P,
		const int &dummy,
		CArrayDouble<2> &b)
	{
		// Radial distortion:
		b[0] = P[0]/P[2];
		b[1] = P[1]/P[2];
	}


	void eval_deps_D_p(
		const CArrayDouble<6> &eps,
		const TPoint3D &D_p,
		CArrayDouble<3> &out)
	{
		const CArrayDouble<6> incr( &eps[0] );
		const CPose3D         incrPose = CPose3D::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 CArrayDouble<6> &eps,
		const TEvalData_A_eps_D_p  &dat,
		CArrayDouble<3> &out)
	{
		const CArrayDouble<6> incr( &eps[0] );
		const CPose3D  incrPose = CPose3D::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: CArrayDouble<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 );
			project_point( lm_stat.obj_points[i], camparam_r, lm_stat.right2left_pose+lm_stat.left_cam_poses[k_idx], 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 -----
			Eigen::Matrix<double,2,6> dhl_del, dhr_del, dhr_der;
			Eigen::Matrix<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+lm_stat.left_cam_poses[k_idx]).composePoint(lm_stat.obj_points[i], pt_wrt_right);

			// Build partial Jacobians:
			Eigen::Matrix<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 );

#if 0
			// Almost exact....
			{
				CArrayDouble<2> x0;
				TPoint3D nP = pt_wrt_left;
				x0[0] = nP.x/nP.z;
				x0[1] = nP.y/nP.z;

				CArrayDouble<2> x_incrs;
				x_incrs.setConstant(1e-6);

				Eigen::Matrix<double,2,2> num_dhl_dbl, num_dhr_dbr;
				mrpt::math::jacobians::jacob_numeric_estimate(x0, &eval_h_b, x_incrs, camparam_l, num_dhl_dbl );

				nP = pt_wrt_right;
				x0[0] = nP.x/nP.z;
				x0[1] = nP.y/nP.z;
				mrpt::math::jacobians::jacob_numeric_estimate(x0, &eval_h_b, x_incrs, camparam_r, num_dhr_dbr );

				cout << "num_dhl_dbl:\n" << num_dhl_dbl << "\ndiff dhl_dbl:\n" << dhl_dbl-num_dhl_dbl << endl << endl;
				cout << "num_dhr_dbr:\n" << num_dhr_dbr << "\ndiff dhr_dbr:\n" << dhr_dbr-num_dhr_dbr << endl << endl;

			}
#endif

			Eigen::Matrix<double,2,3> dbl_dpl, dbr_dpr;
			jacob_db_dp(pt_wrt_left,  dbl_dpl);
			jacob_db_dp(pt_wrt_right, dbr_dpr);

#if 0
			// OK! 100% exact.
			{
				CArrayDouble<3> x0;
				x0[0]=pt_wrt_left.x;
				x0[1]=pt_wrt_left.y;
				x0[2]=pt_wrt_left.z;

				CArrayDouble<3> x_incrs;
				x_incrs.setConstant(1e-8);

				Eigen::Matrix<double,2,3> num_dbl_dpl, num_dbr_dpr;
				const int dumm=0;
				mrpt::math::jacobians::jacob_numeric_estimate(x0, &eval_b_p, x_incrs, dumm, num_dbl_dpl );

				x0[0]=pt_wrt_right.x;
				x0[1]=pt_wrt_right.y;
				x0[2]=pt_wrt_right.z;
				mrpt::math::jacobians::jacob_numeric_estimate(x0, &eval_b_p, x_incrs, dumm, num_dbr_dpr );

				cout << "num_dbl_dpl:\n" << num_dbl_dpl << "\ndbl_dpl:\n" << dbl_dpl << endl << endl;
				cout << "num_dbr_dpr:\n" << num_dbr_dpr << "\ndbr_dpr:\n" << dbr_dpr << endl << endl;

			}
#endif

			// p_l = exp(epsilon_l) (+) pose_left (+) point_ij
			// p_l = [exp(epsilon_r) (+) pose_right2left] (+) [exp(epsilon_l) (+) pose_left] (+) point_ij
			Eigen::Matrix<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);
			jacob_dA_eps_D_p_deps(lm_stat.right2left_pose, lm_stat.left_cam_poses[k_idx],lm_stat.obj_points[i], dpr_del);

#if 0
			// 100% Exact.
			{
				// Test jacob_deps_D_p_deps:
				CArrayDouble<6> x0;
				x0.setConstant(0);

				CArrayDouble<6> x_incrs;
				x_incrs.setConstant(1e-8);

				Eigen::Matrix<double,3,6> num_dpl_del, num_dpr_der;
				mrpt::math::jacobians::jacob_numeric_estimate(x0, &eval_deps_D_p, x_incrs, pt_wrt_left , num_dpl_del );
				mrpt::math::jacobians::jacob_numeric_estimate(x0, &eval_deps_D_p, x_incrs, pt_wrt_right, num_dpr_der );

				cout << "num_dpl_del:\n" << num_dpl_del << "\ndiff dpl_del:\n" << dpl_del-num_dpl_del << endl << endl;
				cout << "num_dpr_der:\n" << num_dpr_der << "\ndiff dpr_der:\n" << dpr_der-num_dpr_der << endl << endl;
			}
#endif

#if 0
			// 100% Exact.
			{
				// Test jacob_dA_eps_D_p_deps:
				CArrayDouble<6> x0;
				x0.setConstant(0);

				CArrayDouble<6> x_incrs;
				x_incrs.setConstant(1e-8);

				TEvalData_A_eps_D_p dat;
				dat.A = lm_stat.right2left_pose;
				dat.D = lm_stat.left_cam_poses[k_idx];
				dat.p = lm_stat.obj_points[i];

				Eigen::Matrix<double,3,6> num_dpr_del;
				mrpt::math::jacobians::jacob_numeric_estimate(x0, &eval_dA_eps_D_p, x_incrs,dat , num_dpr_del );

				cout << "num_dpr_del:\n" << num_dpr_del << "\ndiff dpr_del:\n" << num_dpr_del-dpr_del << endl << endl;
			}
#endif

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

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

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

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

			CArrayDouble<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 CArrayDouble<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::jacobians::jacob_numeric_estimate(x0, &numeric_jacob_eval_function, x_incrs, dat, rje.J );

#	if defined(COMPARE_NUMERIC_JACOBIANS)
			const Eigen::Matrix<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() :
	check_size_x(7),check_size_y(9),
	check_squares_length_X_meters(0.02),check_squares_length_Y_meters(0.02),
	normalize_image(true),
	skipDrawDetectedImgs(false),
	verbose(true),
	maxIters(2000),
	optimize_k1(true), optimize_k2(true), optimize_k3(false),
	optimize_t1(false),optimize_t2(false),
	use_robust_kernel(false),
	robust_kernel_param(10),
	callback(NULL),
	callback_user_param(NULL)
{
}

TStereoCalibResults::TStereoCalibResults() :
	final_rmse (0),
	final_iters(0),
	final_number_good_image_pairs(0)
{
}
