CStereoRectifyMap.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/obs/CObservationStereoImages.h>
#include <mrpt/poses/CPose3DQuat.h>

namespace mrpt::vision
{
/** Use this class to rectify stereo images if the same distortion maps are
 * reused over and over again.
 *  The rectify maps are cached internally and only computed once for the
 * camera parameters.
 * The stereo camera calibration must be supplied in a
 * mrpt::util::TStereoCamera structure
 *  (which provides method for loading from a plain text config file) or
 * directly from the
 *  parameters of a mrpt::obs::CObservationStereoImages object.
 *
 * Remember that the rectified images have a different set of intrinsic
 * parameters than the
 *  original images, which can be retrieved with \a getRectifiedImageParams()
 *
 *  Works with grayscale or color images.
 *
 *  Refer to the program stereo-calib-gui for a tool that generates the
 * required stereo camera parameters
 *  from a set of stereo images of a checkerboard.
 *
 *  Example of usage with mrpt::obs::CObservationStereoImages:
 *
 * \code
 *   CStereoRectifyMap   rectify_map;
 *   // Set options as desired:
 *   // rectify_map.setAlpha(...);
 *   // rectify_map.enableBothCentersCoincide(...);
 *
 *   while (true) {
 *     mrpt::obs::CObservationStereoImages::Ptr obs_stereo = ... // Grab stereo
 * observation from wherever
 *
 *     // Only once, construct the rectification maps:
 *     if (!rectify_map.isSet())
 *       rectify_map.setFromCamParams(*obs_stereo);
 *
 *     // Rectify in place:
 *     unmap.rectify(*obs_stereo);
 *     // Rectified images are now in: obs_stereo->imageLeft &
 * obs_stereo->imageRight
 *   }
 * \endcode
 *
 *  Read also the tutorial page online:
 * https://www.mrpt.org/Rectifying_stereo_images
 *
 * \sa CUndistortMap, mrpt::obs::CObservationStereoImages,
 * mrpt::img::TCamera, the application <a
 * href="http://www.mrpt.org/Application:camera-calib" >camera-calib</a> for
 * calibrating a camera.
 *
 * \note This class provides a uniform wrap over different OpenCV versions. The
 * "alpha" parameter is ignored if built against OpenCV 2.0.X
 *
 * \ingroup mrpt_vision_grp
 */
class CStereoRectifyMap
{
   public:
    CStereoRectifyMap() = default;

    /** @name Rectify map preparation and setting/getting of parameters
        @{ */
    /** Returns true if \a setFromCamParams() has been already called, false
     * otherwise.
     *  Can be used within loops to determine the first usage of the object and
     * when it needs to be initialized.
     */
    inline bool isSet() const { return !m_dat_mapx_left.empty(); }
    /** Prepares the mapping from the intrinsic, distortion and relative pose
     * parameters of a stereo camera.
     * Must be called before invoking \a rectify().
     * The \a alpha parameter can be changed with \a setAlpha() before invoking
     * this method; otherwise, the current rectification maps will be marked as
     * invalid and should be prepared again.
     * \sa setAlpha()
     */
    void setFromCamParams(const mrpt::img::TStereoCamera& params);

    /** A wrapper to \a setFromCamParams() which takes the parameters from an
     * stereo observation object */
    void setFromCamParams(const mrpt::obs::CObservationStereoImages& stereo_obs)
    {
        mrpt::img::TStereoCamera params;
        stereo_obs.getStereoCameraParams(params);
        setFromCamParams(params);
    }

    /** Returns the camera parameters which were used to generate the distortion
     * map, as passed by the user to \a setFromCamParams */
    inline const mrpt::img::TStereoCamera& getCameraParams() const
    {
        return m_camera_params;
    }

    /** After computing the rectification maps, this method retrieves the
     * calibration parameters of the rectified images
     *  (which won't have any distortion).
     * \exception std::exception If the rectification maps have not been
     * computed.
     */
    const mrpt::img::TStereoCamera& getRectifiedImageParams() const;

    /** Just like \a getRectifiedImageParams() but for the left camera only */
    const mrpt::img::TCamera& getRectifiedLeftImageParams() const;
    /** Just like \a getRectifiedImageParams() but for the right camera only */
    const mrpt::img::TCamera& getRectifiedRightImageParams() const;

    /** Sets the \a alpha parameter which controls the zoom in/out of the
     * rectified images, such that:
     *  - alpha=0 => rectified images are zoom in so that only valid pixels are
     * visible
     *  - alpha=1 => rectified images will contain large "black areas" but no
     * pixel from the original image will be lost.
     * Intermediary values leads to intermediary results.
     * Its default value (-1) means auto guess by the OpenCV's algorithm.
     * \note Call this method before building the rectification maps, otherwise
     * they'll be marked as invalid.
     */
    void setAlpha(double alpha);

    /** Return the \a alpha parameter \sa setAlpha */
    inline double getAlpha() const { return m_alpha; }
    /** If enabled, the computed maps will rectify images to a size different
     * than their original size.
     * \note Call this method before building the rectification maps, otherwise
     * they'll be marked as invalid.
     */
    void enableResizeOutput(
        bool enable, unsigned int target_width = 0,
        unsigned int target_height = 0);

    /** Returns whether resizing is enabled (default=false) \sa
     * enableResizeOutput */
    bool isEnabledResizeOutput() const { return m_resize_output; }
    /** Only when \a isEnabledResizeOutput() returns true, this gets the target
     * size  \sa enableResizeOutput */
    inline mrpt::img::TImageSize getResizeOutputSize() const
    {
        return m_resize_output_value;
    }

    /** Change remap interpolation method (default=Lineal). This parameter can
     * be safely changed at any instant without consequences. */
    void setInterpolationMethod(const mrpt::img::TInterpolationMethod interp)
    {
        m_interpolation_method = interp;
    }

    /** Get the currently selected interpolation method \sa
     * setInterpolationMethod */
    mrpt::img::TInterpolationMethod getInterpolationMethod() const
    {
        return m_interpolation_method;
    }

    /** If enabled (default=false), the principal points in both output images
     * will coincide.
     * \note Call this method before building the rectification maps, otherwise
     * they'll be marked as invalid.
     */
    void enableBothCentersCoincide(bool enable = true);

    /** \sa enableBothCentersCoincide */
    bool isEnabledBothCentersCoincide() const
    {
        return m_enable_both_centers_coincide;
    }

    /** After computing the rectification maps, get the rotation applied to the
     * left/right camera so their virtual image plane is the same after
     * rectification */
    const mrpt::poses::CPose3DQuat& getLeftCameraRot() const
    {
        return m_rot_left;
    }
    /** See \a getLeftCameraRot()  */
    const mrpt::poses::CPose3DQuat& getRightCameraRot() const
    {
        return m_rot_right;
    }
    /** Direct input access to rectify maps */
    void setRectifyMaps(
        const std::vector<int16_t>& left_x, const std::vector<uint16_t>& left_y,
        const std::vector<int16_t>& right_x,
        const std::vector<uint16_t>& right_y);

    /** Direct input access to rectify maps. This method swaps the vectors so
     * the inputs are no longer available.*/
    void setRectifyMapsFast(
        std::vector<int16_t>& left_x, std::vector<uint16_t>& left_y,
        std::vector<int16_t>& right_x, std::vector<uint16_t>& right_y);

    /** @} */

    /** @name Rectify methods
        @{ */

    /** Rectify the input image pair and save the result in a different output
     * images - \a setFromCamParams() must have been set prior to calling this.
     * The previous contents of the output images are completely ignored, but
     * if they are already of the
     * correct size and type, allocation time will be saved.
     * Recall that \a getRectifiedImageParams() provides you the new intrinsic
     * parameters of these images.
     * \exception std::exception If the rectification maps have not been
     * computed.
     * \note The same image CANNOT be at the same time input and output, in
     * which case an exception will be raised (but see the overloaded version
     * for in-place rectification)
     */
    void rectify(
        const mrpt::img::CImage& in_left_image,
        const mrpt::img::CImage& in_right_image,
        mrpt::img::CImage& out_left_image,
        mrpt::img::CImage& out_right_image) const;

    /** Overloaded version for in-place rectification of image pairs stored in a
     * mrpt::obs::CObservationStereoImages.
     *  Upon return, the new camera intrinsic parameters will be already stored
     * in the observation object.
     * If \a use_internal_mem_cache is set to \a true (recommended), will reuse
     * over and over again the same
     * auxiliary images (kept internally to this object) needed for in-place
     * rectification.
     * The only reason not to enable this cache is when multiple threads can
     * invoke this method simultaneously.
     * \note This method uses the left & right camera rotations computed by the
     * rectification map to update
     *         mrpt::obs::CObservationStereoImages::cameraPose (left camera wrt
     * the robot frame) and
     *         mrpt::obs::CObservationStereoImages::rightCameraPose (right wrt
     * left camera).
     */
    void rectify(
        mrpt::obs::CObservationStereoImages& stereo_image_observation,
        const bool use_internal_mem_cache = true) const;

    /** @} */

   private:
    double m_alpha{-1};
    bool m_resize_output{false};
    bool m_enable_both_centers_coincide{false};
    mrpt::img::TImageSize m_resize_output_value{0, 0};
    mrpt::img::TInterpolationMethod m_interpolation_method{
        mrpt::img::IMG_INTERP_LINEAR};

    std::vector<int16_t> m_dat_mapx_left, m_dat_mapx_right;
    std::vector<uint16_t> m_dat_mapy_left, m_dat_mapy_right;

    /** A copy of the data provided by the user */
    mrpt::img::TStereoCamera m_camera_params;
    /** Resulting images params */
    mrpt::img::TStereoCamera m_rectified_image_params;

    /** The rotation applied to the left/right camera so their virtual image
     * plane is the same after rectification. */
    mrpt::poses::CPose3DQuat m_rot_left, m_rot_right;

    void internal_invalidate();

};  // end class

}  // namespace mrpt::vision