chessboard_stereo_camera_calib.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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/config/CConfigFileMemory.h>
 
#include <mrpt/vision/chessboard_find_corners.h>
#include <mrpt/vision/chessboard_stereo_camera_calib.h>
#include <mrpt/vision/pinhole.h>
#include <mrpt/poses/CPose3DQuat.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/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)
                {
                        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." << 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;
                std::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::img::TCamera& params,
        const CPose3D& cameraPose, mrpt::img::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 std::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 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++)
        {
                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::estimateJacobian(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::estimateJacobian(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::estimateJacobian(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::estimateJacobian(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::estimateJacobian(x0, &eval_deps_D_p, x_incrs, pt_wrt_left , num_dpl_del );
                                mrpt::math::estimateJacobian(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::estimateJacobian(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::estimateJacobian(
                                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(nullptr),
          callback_user_param(nullptr)
{
}
 
TStereoCalibResults::TStereoCalibResults()
        : final_rmse(0), final_iters(0), final_number_good_image_pairs(0)
{
}