stereo_image_proc: processor.h Source File

Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #ifndef STEREO_IMAGE_PROC_PROCESSOR_H
00035 #define STEREO_IMAGE_PROC_PROCESSOR_H
00036 
00037 #include <image_proc/processor.h>
00038 #include <image_geometry/stereo_camera_model.h>
00039 #include <stereo_msgs/DisparityImage.h>
00040 #include <sensor_msgs/PointCloud.h>
00041 #include <sensor_msgs/PointCloud2.h>
00042 
00043 namespace stereo_image_proc {
00044 
00045 struct StereoImageSet
00046 {
00047   image_proc::ImageSet left;
00048   image_proc::ImageSet right;
00049   stereo_msgs::DisparityImage disparity;
00050   sensor_msgs::PointCloud points;
00051   sensor_msgs::PointCloud2 points2;
00052 };
00053 
00054 class StereoProcessor
00055 {
00056 public:
00057   
00058   StereoProcessor()
00059 #if CV_MAJOR_VERSION == 3
00060   {
00061     block_matcher_ = cv::StereoBM::create();
00062     sg_block_matcher_ = cv::StereoSGBM::create(1, 1, 10);
00063 #else
00064     : block_matcher_(cv::StereoBM::BASIC_PRESET),
00065       sg_block_matcher_()
00066   {
00067 #endif
00068   }
00069 
00070   enum StereoType
00071   {
00072     BM, SGBM
00073   };
00074 
00075   enum {
00076     LEFT_MONO        = 1 << 0,
00077     LEFT_RECT        = 1 << 1,
00078     LEFT_COLOR       = 1 << 2,
00079     LEFT_RECT_COLOR  = 1 << 3,
00080     RIGHT_MONO       = 1 << 4,
00081     RIGHT_RECT       = 1 << 5,
00082     RIGHT_COLOR      = 1 << 6,
00083     RIGHT_RECT_COLOR = 1 << 7,
00084     DISPARITY        = 1 << 8,
00085     POINT_CLOUD      = 1 << 9,
00086     POINT_CLOUD2     = 1 << 10,
00087 
00088     LEFT_ALL = LEFT_MONO | LEFT_RECT | LEFT_COLOR | LEFT_RECT_COLOR,
00089     RIGHT_ALL = RIGHT_MONO | RIGHT_RECT | RIGHT_COLOR | RIGHT_RECT_COLOR,
00090     STEREO_ALL = DISPARITY | POINT_CLOUD | POINT_CLOUD2,
00091     ALL = LEFT_ALL | RIGHT_ALL | STEREO_ALL
00092   };
00093 
00094   inline
00095   StereoType getStereoType() const {return current_stereo_algorithm_;}
00096   inline
00097   void setStereoType(StereoType type) {current_stereo_algorithm_ = type;}
00098 
00099   int getInterpolation() const;
00100   void setInterpolation(int interp);
00101 
00102   // Disparity pre-filtering parameters
00103 
00104   int getPreFilterSize() const;
00105   void setPreFilterSize(int size);
00106 
00107   int getPreFilterCap() const;
00108   void setPreFilterCap(int cap);
00109 
00110   // Disparity correlation parameters
00111 
00112   int getCorrelationWindowSize() const;
00113   void setCorrelationWindowSize(int size);
00114 
00115   int getMinDisparity() const;
00116   void setMinDisparity(int min_d);
00117 
00118   int getDisparityRange() const;
00119   void setDisparityRange(int range); // Number of pixels to search
00120 
00121   // Disparity post-filtering parameters
00122 
00123   int getTextureThreshold() const;
00124   void setTextureThreshold(int threshold);
00125 
00126   float getUniquenessRatio() const;
00127   void setUniquenessRatio(float ratio);
00128 
00129   int getSpeckleSize() const;
00130   void setSpeckleSize(int size);
00131 
00132   int getSpeckleRange() const;
00133   void setSpeckleRange(int range);
00134 
00135   // SGBM only
00136   int getSgbmMode() const;
00137   void setSgbmMode(int fullDP);
00138 
00139   int getP1() const;
00140   void setP1(int P1);
00141 
00142   int getP2() const;
00143   void setP2(int P2);
00144 
00145   int getDisp12MaxDiff() const;
00146   void setDisp12MaxDiff(int disp12MaxDiff);
00147 
00148   // Do all the work!
00149   bool process(const sensor_msgs::ImageConstPtr& left_raw,
00150                const sensor_msgs::ImageConstPtr& right_raw,
00151                const image_geometry::StereoCameraModel& model,
00152                StereoImageSet& output, int flags) const;
00153 
00154   void processDisparity(const cv::Mat& left_rect, const cv::Mat& right_rect,
00155                         const image_geometry::StereoCameraModel& model,
00156                         stereo_msgs::DisparityImage& disparity) const;
00157 
00158   void processPoints(const stereo_msgs::DisparityImage& disparity,
00159                      const cv::Mat& color, const std::string& encoding,
00160                      const image_geometry::StereoCameraModel& model,
00161                      sensor_msgs::PointCloud& points) const;
00162   void processPoints2(const stereo_msgs::DisparityImage& disparity,
00163                       const cv::Mat& color, const std::string& encoding,
00164                       const image_geometry::StereoCameraModel& model,
00165                       sensor_msgs::PointCloud2& points) const;
00166 
00167 private:
00168   image_proc::Processor mono_processor_;
00169   
00170   mutable cv::Mat_<int16_t> disparity16_; // scratch buffer for 16-bit signed disparity image
00171 #if CV_MAJOR_VERSION == 3
00172   mutable cv::Ptr<cv::StereoBM> block_matcher_; // contains scratch buffers for block matching
00173   mutable cv::Ptr<cv::StereoSGBM> sg_block_matcher_;
00174 #else
00175   mutable cv::StereoBM block_matcher_; // contains scratch buffers for block matching
00176   mutable cv::StereoSGBM sg_block_matcher_;
00177 #endif
00178   StereoType current_stereo_algorithm_;
00179   // scratch buffers for speckle filtering
00180   mutable cv::Mat_<uint32_t> labels_;
00181   mutable cv::Mat_<uint32_t> wavefront_;
00182   mutable cv::Mat_<uint8_t> region_types_;
00183   // scratch buffer for dense point cloud
00184   mutable cv::Mat_<cv::Vec3f> dense_points_;
00185 };
00186 
00187 
00188 inline int StereoProcessor::getInterpolation() const
00189 {
00190   return mono_processor_.interpolation_;
00191 }
00192 
00193 inline void StereoProcessor::setInterpolation(int interp)
00194 {
00195   mono_processor_.interpolation_ = interp;
00196 }
00197 
00198 // For once, a macro is used just to avoid errors
00199 #define STEREO_IMAGE_PROC_OPENCV2(GET, SET, TYPE, PARAM) \
00200 inline TYPE StereoProcessor::GET() const \
00201 { \
00202   if (current_stereo_algorithm_ == BM) \
00203     return block_matcher_.state->PARAM; \
00204   return sg_block_matcher_.PARAM; \
00205 } \
00206  \
00207 inline void StereoProcessor::SET(TYPE param) \
00208 { \
00209   block_matcher_.state->PARAM = param; \
00210   sg_block_matcher_.PARAM = param; \
00211 }
00212 
00213 #define STEREO_IMAGE_PROC_OPENCV3(GET, SET, TYPE, GET_OPENCV, SET_OPENCV) \
00214 inline TYPE StereoProcessor::GET() const \
00215 { \
00216   if (current_stereo_algorithm_ == BM) \
00217     return block_matcher_->GET_OPENCV(); \
00218   return sg_block_matcher_->GET_OPENCV(); \
00219 } \
00220 \
00221 inline void StereoProcessor::SET(TYPE param) \
00222 { \
00223   block_matcher_->SET_OPENCV(param); \
00224   sg_block_matcher_->SET_OPENCV(param); \
00225 }
00226 
00227 #if CV_MAJOR_VERSION == 3
00228 STEREO_IMAGE_PROC_OPENCV3(getPreFilterCap, setPreFilterCap, int, getPreFilterCap, setPreFilterCap)
00229 STEREO_IMAGE_PROC_OPENCV3(getCorrelationWindowSize, setCorrelationWindowSize, int, getBlockSize, setBlockSize)
00230 STEREO_IMAGE_PROC_OPENCV3(getMinDisparity, setMinDisparity, int, getMinDisparity, setMinDisparity)
00231 STEREO_IMAGE_PROC_OPENCV3(getDisparityRange, setDisparityRange, int, getNumDisparities, setNumDisparities)
00232 STEREO_IMAGE_PROC_OPENCV3(getUniquenessRatio, setUniquenessRatio, float, getUniquenessRatio, setUniquenessRatio)
00233 STEREO_IMAGE_PROC_OPENCV3(getSpeckleSize, setSpeckleSize, int, getSpeckleWindowSize, setSpeckleWindowSize)
00234 STEREO_IMAGE_PROC_OPENCV3(getSpeckleRange, setSpeckleRange, int, getSpeckleRange, setSpeckleRange)
00235 #else
00236 STEREO_IMAGE_PROC_OPENCV2(getPreFilterCap, setPreFilterCap, int, preFilterCap)
00237 STEREO_IMAGE_PROC_OPENCV2(getCorrelationWindowSize, setCorrelationWindowSize, int, SADWindowSize)
00238 STEREO_IMAGE_PROC_OPENCV2(getMinDisparity, setMinDisparity, int, minDisparity)
00239 STEREO_IMAGE_PROC_OPENCV2(getDisparityRange, setDisparityRange, int, numberOfDisparities)
00240 STEREO_IMAGE_PROC_OPENCV2(getUniquenessRatio, setUniquenessRatio, float, uniquenessRatio)
00241 STEREO_IMAGE_PROC_OPENCV2(getSpeckleSize, setSpeckleSize, int, speckleWindowSize)
00242 STEREO_IMAGE_PROC_OPENCV2(getSpeckleRange, setSpeckleRange, int, speckleRange)
00243 #endif
00244 
00245 #define STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(GET, SET, TYPE, PARAM) \
00246 inline TYPE StereoProcessor::GET() const \
00247 { \
00248   return block_matcher_.state->PARAM; \
00249 } \
00250 \
00251 inline void StereoProcessor::SET(TYPE param) \
00252 { \
00253   block_matcher_.state->PARAM = param; \
00254 }
00255 
00256 #define STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(GET, SET, TYPE, PARAM) \
00257 inline TYPE StereoProcessor::GET() const \
00258 { \
00259   return sg_block_matcher_.PARAM; \
00260 } \
00261 \
00262 inline void StereoProcessor::SET(TYPE param) \
00263 { \
00264   sg_block_matcher_.PARAM = param; \
00265 }
00266 
00267 #define STEREO_IMAGE_PROC_ONLY_OPENCV3(MEMBER, GET, SET, TYPE, GET_OPENCV, SET_OPENCV) \
00268 inline TYPE StereoProcessor::GET() const \
00269 { \
00270   return MEMBER->GET_OPENCV(); \
00271 } \
00272 \
00273 inline void StereoProcessor::SET(TYPE param) \
00274 { \
00275   MEMBER->SET_OPENCV(param); \
00276 }
00277 
00278 // BM only
00279 #if CV_MAJOR_VERSION == 3
00280 STEREO_IMAGE_PROC_ONLY_OPENCV3(block_matcher_, getPreFilterSize, setPreFilterSize, int, getPreFilterSize, setPreFilterSize)
00281 STEREO_IMAGE_PROC_ONLY_OPENCV3(block_matcher_, getTextureThreshold, setTextureThreshold, int, getTextureThreshold, setTextureThreshold)
00282 #else
00283 STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(getPreFilterSize, setPreFilterSize, int, preFilterSize)
00284 STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(getTextureThreshold, setTextureThreshold, int, textureThreshold)
00285 #endif
00286 
00287 // SGBM specific
00288 #if CV_MAJOR_VERSION == 3
00289 // getSgbmMode can return MODE_SGBM = 0, MODE_HH = 1. FullDP == 1 was MODE_HH so we're good
00290 STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getSgbmMode, setSgbmMode, int, getMode, setMode)
00291 STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getP1, setP1, int, getP1, setP1)
00292 STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getP2, setP2, int, getP2, setP2)
00293 STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getDisp12MaxDiff, setDisp12MaxDiff, int, getDisp12MaxDiff, setDisp12MaxDiff)
00294 #else
00295 STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getSgbmMode, setSgbmMode, int, fullDP)
00296 STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getP1, setP1, int, P1)
00297 STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getP2, setP2, int, P2)
00298 STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getDisp12MaxDiff, setDisp12MaxDiff, int, disp12MaxDiff)
00299 #endif
00300 
00301 } //namespace stereo_image_proc
00302 
00303 #endif