chessboard_stereo_camera_calib.h

Go to the documentation of this file.

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

#pragma once

#include <mrpt/img/CImage.h>
#include <mrpt/img/TStereoCamera.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/vision/chessboard_camera_calib.h>
#include <mrpt/vision/types.h>
#include <array>

namespace mrpt::vision
{
/** \addtogroup chessboard_calib Chessboard calibration
 *  \ingroup mrpt_vision_grp
 *  @{  */

/** Data associated to each stereo image in the calibration process
 * mrpt::vision::checkerBoardCameraCalibration (All the information can be left
 * empty and will be filled up in the calibration method).
 */
struct TImageStereoCalibData
{
    TImageCalibData left, right;

    /** Empty all the data */
    void clear() { *this = TImageStereoCalibData(); }
};

/** Params of the optional callback provided by the user */
struct TImageStereoCallbackData
{
    /** =-1:Processing images;  =0: Initial calib without distortion, =1: Calib
     * of all parameters */
    int calibRound{-1};
    size_t current_iter{0};
    /** Current root-mean square reprojection error (in pixels) */
    double current_rmse{0};
    /** Info for calibRound==-1 */
    unsigned int nImgsProcessed{0}, nImgsToProcess{0};
};

/** Prototype of optional user callback function. */
using TSteroCalibCallbackFunctor =
    void (*)(const TImageStereoCallbackData& d, void* user_data);

/** Input parameters for mrpt::vision::checkerBoardStereoCalibration */
struct TStereoCalibParams
{
    /** The number of squares in the checkerboard in the "X" & "Y" direction. */
    unsigned int check_size_x{7}, check_size_y{9};
    /** The size of each square in the checkerboard, in meters, in the "X" & Y"
     * axes. */
    double check_squares_length_X_meters{0.02},
        check_squares_length_Y_meters{0.02};
    bool normalize_image{true};
    bool skipDrawDetectedImgs{false};
    /** Show progress messages to std::cout console (default=true) */
    bool verbose{true};
    /** Maximum number of iterations of the optimizer (default=300) */
    size_t maxIters{2000};

    /** Select which distortion parameters (of both left/right cameras) will be
     * optimzed:
     *  k1,k2,k3 are the r^2, r^4 and r^6 radial distorion coeficients, and t1
     * and t2 are the tangential distortion coeficients (see
     * mrpt::img::TCamera).
     * Those set to false will be assumed to be fixed to zero (no distortion).
     * \note Default values are to only assume distortion via k1 and k2 (the
     * rest are zeros).
     */
    bool optimize_k1{true}, optimize_k2{true}, optimize_k3{false},
        optimize_t1{false}, optimize_t2{false};

    /** Employ a Pseudo-Huber robustifier kernel (Default: false) */
    bool use_robust_kernel{false};
    /** The parameter of the robust kernel, in pixels (only if
     * use_robust_kernel=true) (Default=10) */
    double robust_kernel_param{10};

    /** If set to !=NULL, this function will be called within each Lev-Marq.
     * iteration (don't do heavy stuff here since performance will degrade) */
    TSteroCalibCallbackFunctor callback{nullptr};
    /** If using a callback function, you can use this to pass custom data to
     * your callback. */
    void* callback_user_param{nullptr};

    // Ctor: Set default values
    TStereoCalibParams();
};

/** Output results for mrpt::vision::checkerBoardStereoCalibration */
struct TStereoCalibResults
{
    TStereoCalibResults();

    /** Recovered parameters of the stereo camera */
    mrpt::img::TStereoCamera cam_params;
    /** The pose of the left camera as seen from the right camera */
    mrpt::poses::CPose3D right2left_camera_pose;

    /** Poses of the origin of coordinates of the pattern wrt the left camera
     * (i.e. the origin of coordinates, as seen from the different camera poses)
     */
    std::vector<mrpt::math::TPose3D> left_cam_poses;
    /** true if a checkerboard was correctly detected in both left/right images.
     * false if it wasn't, so the image pair didn't make it to the optimization.
     */
    std::vector<bool> image_pair_was_used;

    /** Final reprojection square Root Mean Square Error (in pixels). */
    double final_rmse{0};
    /** Final number of optimization iterations executed. */
    size_t final_iters{0};
    /** Number of image pairs in which valid checkerboards were correctly
     * detected. */
    size_t final_number_good_image_pairs{0};

    /** The inverse variance (information/precision) of each of the 9 left/right
     * camera parameters [fx fy cx cy k1 k2 k3 t1 t2].
     *  Those not estimated as indicated in TStereoCalibParams will be zeros
     * (i.e. an "infinite uncertainty")
     */
    std::array<double, 9> left_params_inv_variance, right_params_inv_variance;
};

/**  A list of images, used in checkerBoardStereoCalibration
 * \sa checkerBoardStereoCalibration
 */
using TCalibrationStereoImageList = std::vector<TImageStereoCalibData>;

/** Optimize the calibration parameters of a stereo camera or a RGB+D (Kinect)
 * camera.
 *  This computes the projection and distortion parameters of each camera, and
 * their relative spatial pose,
 *  from a sequence of pairs of captured images of a checkerboard.
 *  A custom implementation of an optimizer (Levenberg-Marquartd) seeks for the
 * set of selected parameters to estimate that minimize the reprojection errors.
 *
 *  \param input_images [IN/OUT] At input, this list must have one entry for
 * each image to process. At output the original, detected checkboard and
 * rectified images can be found here. See TImageCalibData.
 *  \param params [IN] Mandatory: the user must provide the size of the
 * checkerboard, which parameters to optimize and which to leave fixed to zero,
 * etc.
 *  \param out_results [OUT] The results of the calibration, and its
 * uncertainty measure, will be found here upon return.
 *
 * \return false on any error (more info will be dumped to cout), or true on
 * success.
 * \note See also the ready-to-use application: <a
 * href="http://www.mrpt.org/list-of-mrpt-apps/application-kinect-calibrate"
 * >kinect-calibrate</a>
 * \sa CImage::findChessboardCorners, checkerBoardCameraCalibration,
 * mrpt::hwdrivers::CKinect
 */
bool checkerBoardStereoCalibration(
    TCalibrationStereoImageList& images, const TStereoCalibParams& params,
    TStereoCalibResults& out_results);

/** @}  */  // end of grouping
}  // namespace mrpt::vision