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 buffer for dense point cloud
00180   mutable cv::Mat_<cv::Vec3f> dense_points_;
00181 };
00182 
00183 
00184 inline int StereoProcessor::getInterpolation() const
00185 {
00186   return mono_processor_.interpolation_;
00187 }
00188 
00189 inline void StereoProcessor::setInterpolation(int interp)
00190 {
00191   mono_processor_.interpolation_ = interp;
00192 }
00193 
00194 // For once, a macro is used just to avoid errors
00195 #define STEREO_IMAGE_PROC_OPENCV2(GET, SET, TYPE, PARAM) \
00196 inline TYPE StereoProcessor::GET() const \
00197 { \
00198   if (current_stereo_algorithm_ == BM) \
00199     return block_matcher_.state->PARAM; \
00200   return sg_block_matcher_.PARAM; \
00201 } \
00202  \
00203 inline void StereoProcessor::SET(TYPE param) \
00204 { \
00205   block_matcher_.state->PARAM = param; \
00206   sg_block_matcher_.PARAM = param; \
00207 }
00208 
00209 #define STEREO_IMAGE_PROC_OPENCV3(GET, SET, TYPE, GET_OPENCV, SET_OPENCV) \
00210 inline TYPE StereoProcessor::GET() const \
00211 { \
00212   if (current_stereo_algorithm_ == BM) \
00213     return block_matcher_->GET_OPENCV(); \
00214   return sg_block_matcher_->GET_OPENCV(); \
00215 } \
00216 \
00217 inline void StereoProcessor::SET(TYPE param) \
00218 { \
00219   block_matcher_->SET_OPENCV(param); \
00220   sg_block_matcher_->SET_OPENCV(param); \
00221 }
00222 
00223 #if CV_MAJOR_VERSION == 3
00224 STEREO_IMAGE_PROC_OPENCV3(getPreFilterCap, setPreFilterCap, int, getPreFilterCap, setPreFilterCap)
00225 STEREO_IMAGE_PROC_OPENCV3(getCorrelationWindowSize, setCorrelationWindowSize, int, getBlockSize, setBlockSize)
00226 STEREO_IMAGE_PROC_OPENCV3(getMinDisparity, setMinDisparity, int, getMinDisparity, setMinDisparity)
00227 STEREO_IMAGE_PROC_OPENCV3(getDisparityRange, setDisparityRange, int, getNumDisparities, setNumDisparities)
00228 STEREO_IMAGE_PROC_OPENCV3(getUniquenessRatio, setUniquenessRatio, float, getUniquenessRatio, setUniquenessRatio)
00229 STEREO_IMAGE_PROC_OPENCV3(getSpeckleSize, setSpeckleSize, int, getSpeckleWindowSize, setSpeckleWindowSize)
00230 STEREO_IMAGE_PROC_OPENCV3(getSpeckleRange, setSpeckleRange, int, getSpeckleRange, setSpeckleRange)
00231 #else
00232 STEREO_IMAGE_PROC_OPENCV2(getPreFilterCap, setPreFilterCap, int, preFilterCap)
00233 STEREO_IMAGE_PROC_OPENCV2(getCorrelationWindowSize, setCorrelationWindowSize, int, SADWindowSize)
00234 STEREO_IMAGE_PROC_OPENCV2(getMinDisparity, setMinDisparity, int, minDisparity)
00235 STEREO_IMAGE_PROC_OPENCV2(getDisparityRange, setDisparityRange, int, numberOfDisparities)
00236 STEREO_IMAGE_PROC_OPENCV2(getUniquenessRatio, setUniquenessRatio, float, uniquenessRatio)
00237 STEREO_IMAGE_PROC_OPENCV2(getSpeckleSize, setSpeckleSize, int, speckleWindowSize)
00238 STEREO_IMAGE_PROC_OPENCV2(getSpeckleRange, setSpeckleRange, int, speckleRange)
00239 #endif
00240 
00241 #define STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(GET, SET, TYPE, PARAM) \
00242 inline TYPE StereoProcessor::GET() const \
00243 { \
00244   return block_matcher_.state->PARAM; \
00245 } \
00246 \
00247 inline void StereoProcessor::SET(TYPE param) \
00248 { \
00249   block_matcher_.state->PARAM = param; \
00250 }
00251 
00252 #define STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(GET, SET, TYPE, PARAM) \
00253 inline TYPE StereoProcessor::GET() const \
00254 { \
00255   return sg_block_matcher_.PARAM; \
00256 } \
00257 \
00258 inline void StereoProcessor::SET(TYPE param) \
00259 { \
00260   sg_block_matcher_.PARAM = param; \
00261 }
00262 
00263 #define STEREO_IMAGE_PROC_ONLY_OPENCV3(MEMBER, GET, SET, TYPE, GET_OPENCV, SET_OPENCV) \
00264 inline TYPE StereoProcessor::GET() const \
00265 { \
00266   return MEMBER->GET_OPENCV(); \
00267 } \
00268 \
00269 inline void StereoProcessor::SET(TYPE param) \
00270 { \
00271   MEMBER->SET_OPENCV(param); \
00272 }
00273 
00274 // BM only
00275 #if CV_MAJOR_VERSION == 3
00276 STEREO_IMAGE_PROC_ONLY_OPENCV3(block_matcher_, getPreFilterSize, setPreFilterSize, int, getPreFilterSize, setPreFilterSize)
00277 STEREO_IMAGE_PROC_ONLY_OPENCV3(block_matcher_, getTextureThreshold, setTextureThreshold, int, getTextureThreshold, setTextureThreshold)
00278 #else
00279 STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(getPreFilterSize, setPreFilterSize, int, preFilterSize)
00280 STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(getTextureThreshold, setTextureThreshold, int, textureThreshold)
00281 #endif
00282 
00283 // SGBM specific
00284 #if CV_MAJOR_VERSION == 3
00285 // getSgbmMode can return MODE_SGBM = 0, MODE_HH = 1. FullDP == 1 was MODE_HH so we're good
00286 STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getSgbmMode, setSgbmMode, int, getMode, setMode)
00287 STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getP1, setP1, int, getP1, setP1)
00288 STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getP2, setP2, int, getP2, setP2)
00289 STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getDisp12MaxDiff, setDisp12MaxDiff, int, getDisp12MaxDiff, setDisp12MaxDiff)
00290 #else
00291 STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getSgbmMode, setSgbmMode, int, fullDP)
00292 STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getP1, setP1, int, P1)
00293 STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getP2, setP2, int, P2)
00294 STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getDisp12MaxDiff, setDisp12MaxDiff, int, disp12MaxDiff)
00295 #endif
00296 
00297 } //namespace stereo_image_proc
00298 
00299 #endif