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)
{
}