CStereoRectifyMap.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, 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    |
   +---------------------------------------------------------------------------+ */
#ifndef mrpt_CStereoRectifyMap_H
#define mrpt_CStereoRectifyMap_H

#include <mrpt/utils/TStereoCamera.h>
#include <mrpt/utils/CImage.h>
#include <mrpt/obs/CObservationStereoImages.h>
#include <mrpt/poses/CPose3DQuat.h>

#include <mrpt/vision/link_pragmas.h>

namespace mrpt
{
        namespace 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::CObservationStereoImagesPtr 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: http://www.mrpt.org/Rectifying_stereo_images
                  *
                  * \sa CUndistortMap, mrpt::obs::CObservationStereoImages, mrpt::utils::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 VISION_IMPEXP  CStereoRectifyMap
                {
                public:
                        CStereoRectifyMap(); //!< Default ctor

                /** @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::utils::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::utils::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::utils::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::utils::TStereoCamera & getRectifiedImageParams() const;

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

                        /** 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 */
                        mrpt::utils::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::utils::TInterpolationMethod interp) {
                                m_interpolation_method = interp;
                        }

                        /** Get the currently selected interpolation method \sa setInterpolationMethod */
                        mrpt::utils::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::utils::CImage &in_left_image,
                                const mrpt::utils::CImage &in_right_image,
                                mrpt::utils::CImage &out_left_image,
                                mrpt::utils::CImage &out_right_image) const;

                        /** Overloaded version for in-place rectification: replace input images with their rectified versions
                          * 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.
                          */
                        void rectify(
                                mrpt::utils::CImage &left_image,
                                mrpt::utils::CImage &right_image,
                                const bool use_internal_mem_cache = true ) 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;

                        /** Just like rectify() but directly works with OpenCV's "IplImage*", which must be passed as "void*" to avoid header dependencies
                          *  Output images CANNOT coincide with the input images. */
                        void rectify_IPL(
                                const void* in_left_image,
                                const void* in_right_image,
                                void* out_left_image,
                                void* out_right_image) const;

                /** @} */

                private:
                        double   m_alpha;
                        bool     m_resize_output;
                        bool     m_enable_both_centers_coincide;
                        mrpt::utils::TImageSize m_resize_output_value;
                        mrpt::utils::TInterpolationMethod m_interpolation_method;

                        mutable mrpt::utils::CImage  m_cache1, m_cache2; //!< Memory caches for in-place rectification speed-up.

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

                        mrpt::utils::TStereoCamera  m_camera_params; //!< A copy of the data provided by the user
                        mrpt::utils::TStereoCamera  m_rectified_image_params; //!< Resulting images params

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

                        void internal_invalidate();

                }; // end class

        } // end namespace
} // end namespace
#endif