From: Vadim Pisarevsky Date: Thu, 28 Feb 2013 22:24:46 +0000 (+0400) Subject: new API for StereoMatchers X-Git-Tag: submit/tizen_ivi/20141117.190038~2^2~1152^2~1 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=b6efe305273eff055d2c1e7cf1f5d0b07681123c;p=profile%2Fivi%2Fopencv.git new API for StereoMatchers --- diff --git a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp index 04f7796..4a67d4b 100644 --- a/modules/calib3d/include/opencv2/calib3d/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d/calib3d.hpp @@ -669,18 +669,35 @@ CV_EXPORTS_W void triangulatePoints( InputArray projMatr1, InputArray projMatr2, CV_EXPORTS_W void correctMatches( InputArray F, InputArray points1, InputArray points2, OutputArray newPoints1, OutputArray newPoints2 ); -template<> CV_EXPORTS void Ptr::delete_obj(); -/*! - Block Matching Stereo Correspondence Algorithm +class CV_EXPORTS_W StereoMatcher : public Algorithm +{ +public: + CV_WRAP virtual void compute( InputArray left, InputArray right, + OutputArray disparity ) = 0; +}; - The class implements BM stereo correspondence algorithm by K. Konolige. -*/ +enum { STEREO_DISP_SCALE=16, STEREO_PREFILTER_NORMALIZED_RESPONSE = 0, STEREO_PREFILTER_XSOBEL = 1, + STEREOBM_BASIC_PRESET=0, STEREOBM_FISH_EYE_PRESET=1, STEREOBM_NARROW_PRESET=2 }; + +CV_EXPORTS Ptr createStereoBM(int preset, int numDisparities=0, int SADWindowSize=21); + +CV_EXPORTS Ptr createStereoSGBM(int minDisparity, int numDisparities, int SADWindowSize, + int P1=0, int P2=0, int disp12MaxDiff=0, + int preFilterCap=0, int uniquenessRatio=0, + int speckleWindowSize=0, int speckleRange=0, + bool fullDP=false); + +template<> CV_EXPORTS void Ptr::delete_obj(); + +// to be moved to "compat" module class CV_EXPORTS_W StereoBM { public: enum { PREFILTER_NORMALIZED_RESPONSE = 0, PREFILTER_XSOBEL = 1, - BASIC_PRESET=0, FISH_EYE_PRESET=1, NARROW_PRESET=2 }; + BASIC_PRESET=STEREOBM_BASIC_PRESET, + FISH_EYE_PRESET=STEREOBM_FISH_EYE_PRESET, + NARROW_PRESET=STEREOBM_NARROW_PRESET }; //! the default constructor CV_WRAP StereoBM(); @@ -697,11 +714,7 @@ public: }; -/*! - Semi-Global Block Matching Stereo Correspondence Algorithm - - The class implements the original SGBM stereo correspondence algorithm by H. Hirschmuller and some its modification. - */ +// to be moved to "compat" module class CV_EXPORTS_W StereoSGBM { public: @@ -736,7 +749,7 @@ public: CV_PROP_RW bool fullDP; protected: - Mat buffer; + Ptr sm; }; //! filters off speckles (small regions of incorrectly computed disparity) diff --git a/modules/calib3d/src/compat_stereo.cpp b/modules/calib3d/src/compat_stereo.cpp new file mode 100644 index 0000000..58ee048 --- /dev/null +++ b/modules/calib3d/src/compat_stereo.cpp @@ -0,0 +1,217 @@ +//M*////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000, Intel Corporation, all rights reserved. +// Copyright (C) 2013, OpenCV Foundation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +#include "precomp.hpp" + +CvStereoBMState* cvCreateStereoBMState( int /*preset*/, int numberOfDisparities ) +{ + CvStereoBMState* state = (CvStereoBMState*)cvAlloc( sizeof(*state) ); + if( !state ) + return 0; + + state->preFilterType = CV_STEREO_BM_XSOBEL; //CV_STEREO_BM_NORMALIZED_RESPONSE; + state->preFilterSize = 9; + state->preFilterCap = 31; + state->SADWindowSize = 15; + state->minDisparity = 0; + state->numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : 64; + state->textureThreshold = 10; + state->uniquenessRatio = 15; + state->speckleRange = state->speckleWindowSize = 0; + state->trySmallerWindows = 0; + state->roi1 = state->roi2 = cvRect(0,0,0,0); + state->disp12MaxDiff = -1; + + state->preFilteredImg0 = state->preFilteredImg1 = state->slidingSumBuf = + state->disp = state->cost = 0; + + return state; +} + +void cvReleaseStereoBMState( CvStereoBMState** state ) +{ + if( !state ) + CV_Error( CV_StsNullPtr, "" ); + + if( !*state ) + return; + + cvReleaseMat( &(*state)->preFilteredImg0 ); + cvReleaseMat( &(*state)->preFilteredImg1 ); + cvReleaseMat( &(*state)->slidingSumBuf ); + cvReleaseMat( &(*state)->disp ); + cvReleaseMat( &(*state)->cost ); + cvFree( state ); +} + +template<> void cv::Ptr::delete_obj() +{ cvReleaseStereoBMState(&obj); } + + +void cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr, + CvArr* disparr, CvStereoBMState* state ) +{ + cv::Mat left = cv::cvarrToMat(leftarr), right = cv::cvarrToMat(rightarr); + const cv::Mat disp = cv::cvarrToMat(disparr); + + CV_Assert( state != 0 ); + + cv::Ptr sm = cv::createStereoBM(cv::STEREOBM_BASIC_PRESET, + state->numberOfDisparities, + state->SADWindowSize); + sm->set("preFilterType", state->preFilterType); + sm->set("preFilterSize", state->preFilterSize); + sm->set("preFilterCap", state->preFilterCap); + sm->set("SADWindowSize", state->SADWindowSize); + sm->set("numDisparities", state->numberOfDisparities > 0 ? state->numberOfDisparities : 64); + sm->set("textureThreshold", state->textureThreshold); + sm->set("uniquenessRatio", state->uniquenessRatio); + sm->set("speckleRange", state->speckleRange); + sm->set("speckleWindowSize", state->speckleWindowSize); + sm->set("disp12MaxDiff", state->disp12MaxDiff); + + sm->compute(left, right, disp); +} + +CvRect cvGetValidDisparityROI( CvRect roi1, CvRect roi2, int minDisparity, + int numberOfDisparities, int SADWindowSize ) +{ + return (CvRect)cv::getValidDisparityROI( roi1, roi2, minDisparity, + numberOfDisparities, SADWindowSize ); +} + +void cvValidateDisparity( CvArr* _disp, const CvArr* _cost, int minDisparity, + int numberOfDisparities, int disp12MaxDiff ) +{ + cv::Mat disp = cv::cvarrToMat(_disp), cost = cv::cvarrToMat(_cost); + cv::validateDisparity( disp, cost, minDisparity, numberOfDisparities, disp12MaxDiff ); +} + +namespace cv +{ + +StereoBM::StereoBM() +{ init(STEREOBM_BASIC_PRESET); } + +StereoBM::StereoBM(int _preset, int _ndisparities, int _SADWindowSize) +{ init(_preset, _ndisparities, _SADWindowSize); } + +void StereoBM::init(int _preset, int _ndisparities, int _SADWindowSize) +{ + state = cvCreateStereoBMState(_preset, _ndisparities); + state->SADWindowSize = _SADWindowSize; +} + +void StereoBM::operator()( InputArray _left, InputArray _right, + OutputArray _disparity, int disptype ) +{ + Mat left = _left.getMat(), right = _right.getMat(); + CV_Assert( disptype == CV_16S || disptype == CV_32F ); + _disparity.create(left.size(), disptype); + Mat disp = _disparity.getMat(); + + CvMat left_c = left, right_c = right, disp_c = disp; + cvFindStereoCorrespondenceBM(&left_c, &right_c, &disp_c, state); +} + + +StereoSGBM::StereoSGBM() +{ + minDisparity = numberOfDisparities = 0; + SADWindowSize = 0; + P1 = P2 = 0; + disp12MaxDiff = 0; + preFilterCap = 0; + uniquenessRatio = 0; + speckleWindowSize = 0; + speckleRange = 0; + fullDP = false; + + sm = createStereoSGBM(0, 0, 0); +} + +StereoSGBM::StereoSGBM( int _minDisparity, int _numDisparities, int _SADWindowSize, + int _P1, int _P2, int _disp12MaxDiff, int _preFilterCap, + int _uniquenessRatio, int _speckleWindowSize, int _speckleRange, + bool _fullDP ) +{ + minDisparity = _minDisparity; + numberOfDisparities = _numDisparities; + SADWindowSize = _SADWindowSize; + P1 = _P1; + P2 = _P2; + disp12MaxDiff = _disp12MaxDiff; + preFilterCap = _preFilterCap; + uniquenessRatio = _uniquenessRatio; + speckleWindowSize = _speckleWindowSize; + speckleRange = _speckleRange; + fullDP = _fullDP; + + sm = createStereoSGBM(0, 0, 0); +} + +StereoSGBM::~StereoSGBM() +{ +} + +void StereoSGBM::operator ()( InputArray _left, InputArray _right, + OutputArray _disp ) +{ + sm->set("minDisparity", minDisparity); + sm->set("numDisparities", numberOfDisparities); + sm->set("SADWindowSize", SADWindowSize); + sm->set("P1", P1); + sm->set("P2", P2); + sm->set("disp12MaxDiff", disp12MaxDiff); + sm->set("preFilterCap", preFilterCap); + sm->set("uniquenessRatio", uniquenessRatio); + sm->set("speckleWindowSize", speckleWindowSize); + sm->set("speckleRange", speckleRange); + sm->set("fullDP", fullDP); + + sm->compute(_left, _right, _disp); +} + +} + + + diff --git a/modules/calib3d/src/stereobm.cpp b/modules/calib3d/src/stereobm.cpp index fcf8f13..a70ecc1 100644 --- a/modules/calib3d/src/stereobm.cpp +++ b/modules/calib3d/src/stereobm.cpp @@ -7,10 +7,11 @@ // copy or use the software. // // -// Intel License Agreement +// License Agreement // For Open Source Computer Vision Library // // Copyright (C) 2000, Intel Corporation, all rights reserved. +// Copyright (C) 2013, OpenCV Foundation, all rights reserved. // Third party copyrights are property of their respective owners. // // Redistribution and use in source and binary forms, with or without modification, @@ -23,7 +24,7 @@ // this list of conditions and the following disclaimer in the documentation // and/or other materials provided with the distribution. // -// * The name of Intel Corporation may not be used to endorse or promote products +// * The name of the copyright holders may not be used to endorse or promote products // derived from this software without specific prior written permission. // // This software is provided by the copyright holders and contributors "as is" and @@ -46,57 +47,44 @@ #include "precomp.hpp" #include - -//#undef CV_SSE2 -//#define CV_SSE2 0 -//#include "emmintrin.h" - - #include -CV_IMPL CvStereoBMState* cvCreateStereoBMState( int /*preset*/, int numberOfDisparities ) +namespace cv { - CvStereoBMState* state = (CvStereoBMState*)cvAlloc( sizeof(*state) ); - if( !state ) - return 0; - - state->preFilterType = CV_STEREO_BM_XSOBEL; //CV_STEREO_BM_NORMALIZED_RESPONSE; - state->preFilterSize = 9; - state->preFilterCap = 31; - state->SADWindowSize = 15; - state->minDisparity = 0; - state->numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : 64; - state->textureThreshold = 10; - state->uniquenessRatio = 15; - state->speckleRange = state->speckleWindowSize = 0; - state->trySmallerWindows = 0; - state->roi1 = state->roi2 = cvRect(0,0,0,0); - state->disp12MaxDiff = -1; - - state->preFilteredImg0 = state->preFilteredImg1 = state->slidingSumBuf = - state->disp = state->cost = 0; - - return state; -} -CV_IMPL void cvReleaseStereoBMState( CvStereoBMState** state ) +struct StereoBMParams { - if( !state ) - CV_Error( CV_StsNullPtr, "" ); - - if( !*state ) - return; - - cvReleaseMat( &(*state)->preFilteredImg0 ); - cvReleaseMat( &(*state)->preFilteredImg1 ); - cvReleaseMat( &(*state)->slidingSumBuf ); - cvReleaseMat( &(*state)->disp ); - cvReleaseMat( &(*state)->cost ); - cvFree( state ); -} + StereoBMParams(int _preset=STEREOBM_BASIC_PRESET, int _numDisparities=64, int _SADWindowSize=21) + { + preFilterType = STEREO_PREFILTER_XSOBEL; + preFilterSize = 9; + preFilterCap = 31; + SADWindowSize = _SADWindowSize; + minDisparity = 0; + numDisparities = _numDisparities > 0 ? _numDisparities : 64; + textureThreshold = 10; + uniquenessRatio = 15; + speckleRange = speckleWindowSize = 0; + roi1 = roi2 = Rect(0,0,0,0); + disp12MaxDiff = -1; + dispType = CV_16S; + } -namespace cv -{ + int preFilterType; + int preFilterSize; + int preFilterCap; + int SADWindowSize; + int minDisparity; + int numDisparities; + int textureThreshold; + int uniquenessRatio; + int speckleRange; + int speckleWindowSize; + Rect roi1, roi2; + int disp12MaxDiff; + int dispType; +}; + static void prefilterNorm( const Mat& src, Mat& dst, int winsize, int ftzero, uchar* buf ) { @@ -191,11 +179,11 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero ) dptr0[0] = dptr0[size.width-1] = dptr1[0] = dptr1[size.width-1] = val0; x = 1; - #if CV_SSE2 +#if CV_SSE2 if( useSIMD ) { __m128i z = _mm_setzero_si128(), ftz = _mm_set1_epi16((short)ftzero), - ftz2 = _mm_set1_epi8(CV_CAST_8U(ftzero*2)); + ftz2 = _mm_set1_epi8(CV_CAST_8U(ftzero*2)); for( ; x <= size.width-9; x += 8 ) { __m128i c0 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow0 + x - 1)), z); @@ -223,12 +211,12 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero ) _mm_storel_epi64((__m128i*)(dptr1 + x), _mm_unpackhi_epi64(v0, v0)); } } - #endif +#endif for( ; x < size.width-1; x++ ) { int d0 = srow0[x+1] - srow0[x-1], d1 = srow1[x+1] - srow1[x-1], - d2 = srow2[x+1] - srow2[x-1], d3 = srow3[x+1] - srow3[x-1]; + d2 = srow2[x+1] - srow2[x-1], d3 = srow3[x+1] - srow3[x-1]; int v0 = tab[d0 + d1*2 + d2 + OFS]; int v1 = tab[d1 + d2*2 + d3 + OFS]; dptr0[x] = (uchar)v0; @@ -249,14 +237,14 @@ static const int DISPARITY_SHIFT = 4; #if CV_SSE2 static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right, - Mat& disp, Mat& cost, CvStereoBMState& state, - uchar* buf, int _dy0, int _dy1 ) + Mat& disp, Mat& cost, StereoBMParams& state, + uchar* buf, int _dy0, int _dy1 ) { const int ALIGN = 16; int x, y, d; int wsz = state.SADWindowSize, wsz2 = wsz/2; int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1); - int ndisp = state.numberOfDisparities; + int ndisp = state.numDisparities; int mindisp = state.minDisparity; int lofs = MAX(ndisp - 1 + mindisp, 0); int rofs = -MIN(ndisp - 1 + mindisp, 0); @@ -343,7 +331,7 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right, rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep; for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp, - hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep ) + hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep ) { int lval = lptr[0]; __m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128(); @@ -464,7 +452,7 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right, __m128i thresh8 = _mm_set1_epi16((short)(thresh + 1)); __m128i d1 = _mm_set1_epi16((short)(mind-1)), d2 = _mm_set1_epi16((short)(mind+1)); __m128i dd_16 = _mm_add_epi16(dd_8, dd_8); - d8 = _mm_sub_epi16(d0_8, dd_16); + d8 = _mm_sub_epi16(d0_8, dd_16); for( d = 0; d < ndisp; d += 16 ) { @@ -507,14 +495,14 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right, static void findStereoCorrespondenceBM( const Mat& left, const Mat& right, - Mat& disp, Mat& cost, const CvStereoBMState& state, - uchar* buf, int _dy0, int _dy1 ) + Mat& disp, Mat& cost, const StereoBMParams& state, + uchar* buf, int _dy0, int _dy1 ) { const int ALIGN = 16; int x, y, d; int wsz = state.SADWindowSize, wsz2 = wsz/2; int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1); - int ndisp = state.numberOfDisparities; + int ndisp = state.numDisparities; int mindisp = state.minDisparity; int lofs = MAX(ndisp - 1 + mindisp, 0); int rofs = -MIN(ndisp - 1 + mindisp, 0); @@ -592,7 +580,7 @@ findStereoCorrespondenceBM( const Mat& left, const Mat& right, rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep; for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp, - hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep ) + hsad += ndisp, lptr += sstep, lptr_sub += sstep, rptr += sstep ) { int lval = lptr[0]; for( d = 0; d < ndisp; d++ ) @@ -662,21 +650,21 @@ findStereoCorrespondenceBM( const Mat& left, const Mat& right, } { - sad[-1] = sad[1]; - sad[ndisp] = sad[ndisp-2]; - int p = sad[mind+1], n = sad[mind-1]; - d = p + n - 2*sad[mind] + std::abs(p - n); - dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) >> 4); - costptr[y*coststep] = sad[mind]; + sad[-1] = sad[1]; + sad[ndisp] = sad[ndisp-2]; + int p = sad[mind+1], n = sad[mind-1]; + d = p + n - 2*sad[mind] + std::abs(p - n); + dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) >> 4); + costptr[y*coststep] = sad[mind]; } } } } -struct PrefilterInvoker +struct PrefilterInvoker : public ParallelLoopBody { PrefilterInvoker(const Mat& left0, const Mat& right0, Mat& left, Mat& right, - uchar* buf0, uchar* buf1, CvStereoBMState* _state ) + uchar* buf0, uchar* buf1, StereoBMParams* _state) { imgs0[0] = &left0; imgs0[1] = &right0; imgs[0] = &left; imgs[1] = &right; @@ -684,41 +672,47 @@ struct PrefilterInvoker state = _state; } - void operator()( int ind ) const + void operator()( const Range& range ) const { - if( state->preFilterType == CV_STEREO_BM_NORMALIZED_RESPONSE ) - prefilterNorm( *imgs0[ind], *imgs[ind], state->preFilterSize, state->preFilterCap, buf[ind] ); - else - prefilterXSobel( *imgs0[ind], *imgs[ind], state->preFilterCap ); + for( int i = range.start; i < range.end; i++ ) + { + if( state->preFilterType == STEREO_PREFILTER_NORMALIZED_RESPONSE ) + prefilterNorm( *imgs0[i], *imgs[i], state->preFilterSize, state->preFilterCap, buf[i] ); + else + prefilterXSobel( *imgs0[i], *imgs[i], state->preFilterCap ); + } } const Mat* imgs0[2]; Mat* imgs[2]; uchar* buf[2]; - CvStereoBMState *state; + StereoBMParams* state; }; -struct FindStereoCorrespInvoker +struct FindStereoCorrespInvoker : public ParallelLoopBody { FindStereoCorrespInvoker( const Mat& _left, const Mat& _right, - Mat& _disp, CvStereoBMState* _state, - int _nstripes, int _stripeBufSize, - bool _useShorts, Rect _validDisparityRect ) + Mat& _disp, StereoBMParams* _state, + int _nstripes, size_t _stripeBufSize, + bool _useShorts, Rect _validDisparityRect, + Mat& _slidingSumBuf, Mat& _cost ) { left = &_left; right = &_right; disp = &_disp; state = _state; nstripes = _nstripes; stripeBufSize = _stripeBufSize; useShorts = _useShorts; validDisparityRect = _validDisparityRect; + slidingSumBuf = &_slidingSumBuf; + cost = &_cost; } - void operator()( const BlockedRange& range ) const + void operator()( const Range& range ) const { int cols = left->cols, rows = left->rows; - int _row0 = std::min(cvRound(range.begin() * rows / nstripes), rows); - int _row1 = std::min(cvRound(range.end() * rows / nstripes), rows); - uchar *ptr = state->slidingSumBuf->data.ptr + range.begin() * stripeBufSize; + int _row0 = std::min(cvRound(range.start * rows / nstripes), rows); + int _row1 = std::min(cvRound(range.end * rows / nstripes), rows); + uchar *ptr = slidingSumBuf->data + range.start * stripeBufSize; int FILTERED = (state->minDisparity - 1)*16; Rect roi = validDisparityRect & Rect(0, _row0, cols, _row1 - _row0); @@ -742,7 +736,7 @@ struct FindStereoCorrespInvoker Mat left_i = left->rowRange(row0, row1); Mat right_i = right->rowRange(row0, row1); Mat disp_i = disp->rowRange(row0, row1); - Mat cost_i = state->disp12MaxDiff >= 0 ? Mat(state->cost).rowRange(row0, row1) : Mat(); + Mat cost_i = state->disp12MaxDiff >= 0 ? cost->rowRange(row0, row1) : Mat(); #if CV_SSE2 if( useShorts ) @@ -752,7 +746,7 @@ struct FindStereoCorrespInvoker findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 ); if( state->disp12MaxDiff >= 0 ) - validateDisparity( disp_i, cost_i, state->minDisparity, state->numberOfDisparities, state->disp12MaxDiff ); + validateDisparity( disp_i, cost_i, state->minDisparity, state->numDisparities, state->disp12MaxDiff ); if( roi.x > 0 ) { @@ -768,185 +762,174 @@ struct FindStereoCorrespInvoker protected: const Mat *left, *right; - Mat* disp; - CvStereoBMState *state; + Mat* disp, *slidingSumBuf, *cost; + StereoBMParams *state; int nstripes; - int stripeBufSize; + size_t stripeBufSize; bool useShorts; Rect validDisparityRect; }; -static void findStereoCorrespondenceBM( const Mat& left0, const Mat& right0, Mat& disp0, CvStereoBMState* state) + +class StereoBMImpl : public StereoMatcher { - if (left0.size() != right0.size() || disp0.size() != left0.size()) - CV_Error( CV_StsUnmatchedSizes, "All the images must have the same size" ); +public: + StereoBMImpl() + { + params = StereoBMParams(); + } - if (left0.type() != CV_8UC1 || right0.type() != CV_8UC1) - CV_Error( CV_StsUnsupportedFormat, "Both input images must have CV_8UC1" ); + StereoBMImpl( int _preset, int _numDisparities, int _SADWindowSize ) + { + params = StereoBMParams(_preset, _numDisparities, _SADWindowSize); + } + + void compute( InputArray leftarr, InputArray rightarr, OutputArray disparr ) + { + Mat left0 = leftarr.getMat(), right0 = rightarr.getMat(); + int dtype = disparr.fixedType() ? disparr.type() : params.dispType; - if (disp0.type() != CV_16SC1 && disp0.type() != CV_32FC1) - CV_Error( CV_StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format" ); + if (left0.size() != right0.size()) + CV_Error( CV_StsUnmatchedSizes, "All the images must have the same size" ); - if( !state ) - CV_Error( CV_StsNullPtr, "Stereo BM state is NULL." ); + if (left0.type() != CV_8UC1 || right0.type() != CV_8UC1) + CV_Error( CV_StsUnsupportedFormat, "Both input images must have CV_8UC1" ); - if( state->preFilterType != CV_STEREO_BM_NORMALIZED_RESPONSE && state->preFilterType != CV_STEREO_BM_XSOBEL ) - CV_Error( CV_StsOutOfRange, "preFilterType must be = CV_STEREO_BM_NORMALIZED_RESPONSE" ); + if (dtype != CV_16SC1 && dtype != CV_32FC1) + CV_Error( CV_StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format" ); - if( state->preFilterSize < 5 || state->preFilterSize > 255 || state->preFilterSize % 2 == 0 ) - CV_Error( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" ); + disparr.create(left0.size(), dtype); + Mat disp0 = disparr.getMat(); - if( state->preFilterCap < 1 || state->preFilterCap > 63 ) - CV_Error( CV_StsOutOfRange, "preFilterCap must be within 1..63" ); + if( params.preFilterType != STEREO_PREFILTER_NORMALIZED_RESPONSE && + params.preFilterType != STEREO_PREFILTER_XSOBEL ) + CV_Error( CV_StsOutOfRange, "preFilterType must be = CV_STEREO_BM_NORMALIZED_RESPONSE" ); - if( state->SADWindowSize < 5 || state->SADWindowSize > 255 || state->SADWindowSize % 2 == 0 || - state->SADWindowSize >= std::min(left0.cols, left0.rows) ) - CV_Error( CV_StsOutOfRange, "SADWindowSize must be odd, be within 5..255 and be not larger than image width or height" ); + if( params.preFilterSize < 5 || params.preFilterSize > 255 || params.preFilterSize % 2 == 0 ) + CV_Error( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" ); - if( state->numberOfDisparities <= 0 || state->numberOfDisparities % 16 != 0 ) - CV_Error( CV_StsOutOfRange, "numberOfDisparities must be positive and divisble by 16" ); + if( params.preFilterCap < 1 || params.preFilterCap > 63 ) + CV_Error( CV_StsOutOfRange, "preFilterCap must be within 1..63" ); - if( state->textureThreshold < 0 ) - CV_Error( CV_StsOutOfRange, "texture threshold must be non-negative" ); + if( params.SADWindowSize < 5 || params.SADWindowSize > 255 || params.SADWindowSize % 2 == 0 || + params.SADWindowSize >= std::min(left0.cols, left0.rows) ) + CV_Error( CV_StsOutOfRange, "SADWindowSize must be odd, be within 5..255 and be not larger than image width or height" ); - if( state->uniquenessRatio < 0 ) - CV_Error( CV_StsOutOfRange, "uniqueness ratio must be non-negative" ); + if( params.numDisparities <= 0 || params.numDisparities % 16 != 0 ) + CV_Error( CV_StsOutOfRange, "numDisparities must be positive and divisble by 16" ); - if( !state->preFilteredImg0 || state->preFilteredImg0->cols * state->preFilteredImg0->rows < left0.cols * left0.rows ) - { - cvReleaseMat( &state->preFilteredImg0 ); - cvReleaseMat( &state->preFilteredImg1 ); - cvReleaseMat( &state->cost ); + if( params.textureThreshold < 0 ) + CV_Error( CV_StsOutOfRange, "texture threshold must be non-negative" ); - state->preFilteredImg0 = cvCreateMat( left0.rows, left0.cols, CV_8U ); - state->preFilteredImg1 = cvCreateMat( left0.rows, left0.cols, CV_8U ); - state->cost = cvCreateMat( left0.rows, left0.cols, CV_16S ); - } - Mat left(left0.size(), CV_8U, state->preFilteredImg0->data.ptr); - Mat right(right0.size(), CV_8U, state->preFilteredImg1->data.ptr); + if( params.uniquenessRatio < 0 ) + CV_Error( CV_StsOutOfRange, "uniqueness ratio must be non-negative" ); - int mindisp = state->minDisparity; - int ndisp = state->numberOfDisparities; + preFilteredImg0.create( left0.size(), CV_8U ); + preFilteredImg1.create( left0.size(), CV_8U ); + cost.create( left0.size(), CV_16S ); - int width = left0.cols; - int height = left0.rows; - int lofs = std::max(ndisp - 1 + mindisp, 0); - int rofs = -std::min(ndisp - 1 + mindisp, 0); - int width1 = width - rofs - ndisp + 1; - int FILTERED = (state->minDisparity - 1) << DISPARITY_SHIFT; + Mat left = preFilteredImg0, right = preFilteredImg1; - if( lofs >= width || rofs >= width || width1 < 1 ) - { - disp0 = Scalar::all( FILTERED * ( disp0.type() < CV_32F ? 1 : 1./(1 << DISPARITY_SHIFT) ) ); - return; - } + int mindisp = params.minDisparity; + int ndisp = params.numDisparities; - Mat disp = disp0; + int width = left0.cols; + int height = left0.rows; + int lofs = std::max(ndisp - 1 + mindisp, 0); + int rofs = -std::min(ndisp - 1 + mindisp, 0); + int width1 = width - rofs - ndisp + 1; + int FILTERED = (params.minDisparity - 1) << DISPARITY_SHIFT; - if( disp0.type() == CV_32F) - { - if( !state->disp || state->disp->rows != disp0.rows || state->disp->cols != disp0.cols ) + if( lofs >= width || rofs >= width || width1 < 1 ) { - cvReleaseMat( &state->disp ); - state->disp = cvCreateMat(disp0.rows, disp0.cols, CV_16S); + disp0 = Scalar::all( FILTERED * ( disp0.type() < CV_32F ? 1 : 1./(1 << DISPARITY_SHIFT) ) ); + return; } - disp = cv::cvarrToMat(state->disp); - } - int wsz = state->SADWindowSize; - int bufSize0 = (int)((ndisp + 2)*sizeof(int)); - bufSize0 += (int)((height+wsz+2)*ndisp*sizeof(int)); - bufSize0 += (int)((height + wsz + 2)*sizeof(int)); - bufSize0 += (int)((height+wsz+2)*ndisp*(wsz+2)*sizeof(uchar) + 256); + Mat disp = disp0; + if( dtype == CV_32F ) + { + dispbuf.create(disp0.size(), CV_16S); + disp = dispbuf; + } - int bufSize1 = (int)((width + state->preFilterSize + 2) * sizeof(int) + 256); - int bufSize2 = 0; - if( state->speckleRange >= 0 && state->speckleWindowSize > 0 ) - bufSize2 = width*height*(sizeof(cv::Point_) + sizeof(int) + sizeof(uchar)); + int wsz = params.SADWindowSize; + int bufSize0 = (int)((ndisp + 2)*sizeof(int)); + bufSize0 += (int)((height+wsz+2)*ndisp*sizeof(int)); + bufSize0 += (int)((height + wsz + 2)*sizeof(int)); + bufSize0 += (int)((height+wsz+2)*ndisp*(wsz+2)*sizeof(uchar) + 256); -#if CV_SSE2 - bool useShorts = state->preFilterCap <= 31 && state->SADWindowSize <= 21 && checkHardwareSupport(CV_CPU_SSE2); -#else - const bool useShorts = false; -#endif + int bufSize1 = (int)((width + params.preFilterSize + 2) * sizeof(int) + 256); + int bufSize2 = 0; + if( params.speckleRange >= 0 && params.speckleWindowSize > 0 ) + bufSize2 = width*height*(sizeof(Point_) + sizeof(int) + sizeof(uchar)); -#ifdef HAVE_TBB - const double SAD_overhead_coeff = 10.0; - double N0 = 8000000 / (useShorts ? 1 : 4); // approx tbb's min number instructions reasonable for one thread - double maxStripeSize = std::min(std::max(N0 / (width * ndisp), (wsz-1) * SAD_overhead_coeff), (double)height); - int nstripes = cvCeil(height / maxStripeSize); +#if CV_SSE2 + bool useShorts = params.preFilterCap <= 31 && params.SADWindowSize <= 21 && checkHardwareSupport(CV_CPU_SSE2); #else - const int nstripes = 1; + const bool useShorts = false; #endif - int bufSize = std::max(bufSize0 * nstripes, std::max(bufSize1 * 2, bufSize2)); - - if( !state->slidingSumBuf || state->slidingSumBuf->cols < bufSize ) - { - cvReleaseMat( &state->slidingSumBuf ); - state->slidingSumBuf = cvCreateMat( 1, bufSize, CV_8U ); + const double SAD_overhead_coeff = 10.0; + double N0 = 8000000 / (useShorts ? 1 : 4); // approx tbb's min number instructions reasonable for one thread + double maxStripeSize = std::min(std::max(N0 / (width * ndisp), (wsz-1) * SAD_overhead_coeff), (double)height); + int nstripes = cvCeil(height / maxStripeSize); + int bufSize = std::max(bufSize0 * nstripes, std::max(bufSize1 * 2, bufSize2)); + + if( slidingSumBuf.cols < bufSize ) + slidingSumBuf.create( 1, bufSize, CV_8U ); + + uchar *_buf = slidingSumBuf.data; + parallel_for_(Range(0, 2), PrefilterInvoker(left0, right0, left, right, _buf, _buf + bufSize1, ¶ms), 1); + + Rect validDisparityRect(0, 0, width, height), R1 = params.roi1, R2 = params.roi2; + validDisparityRect = getValidDisparityROI(R1.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect, + R2.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect, + params.minDisparity, params.numDisparities, + params.SADWindowSize); + + parallel_for_(Range(0, nstripes), + FindStereoCorrespInvoker(left, right, disp, ¶ms, nstripes, + bufSize0, useShorts, validDisparityRect, + slidingSumBuf, cost)); + + if( params.speckleRange >= 0 && params.speckleWindowSize > 0 ) + filterSpeckles(disp, FILTERED, params.speckleWindowSize, params.speckleRange, slidingSumBuf); + + if (disp0.data != disp.data) + disp.convertTo(disp0, disp0.type(), 1./(1 << DISPARITY_SHIFT), 0); } - uchar *_buf = state->slidingSumBuf->data.ptr; - int idx[] = {0,1}; - parallel_do(idx, idx+2, PrefilterInvoker(left0, right0, left, right, _buf, _buf + bufSize1, state)); - - Rect validDisparityRect(0, 0, width, height), R1 = state->roi1, R2 = state->roi2; - validDisparityRect = getValidDisparityROI(R1.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect, - R2.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect, - state->minDisparity, state->numberOfDisparities, - state->SADWindowSize); - - parallel_for(BlockedRange(0, nstripes), - FindStereoCorrespInvoker(left, right, disp, state, nstripes, - bufSize0, useShorts, validDisparityRect)); + AlgorithmInfo* info() const; - if( state->speckleRange >= 0 && state->speckleWindowSize > 0 ) - { - Mat buf(state->slidingSumBuf); - filterSpeckles(disp, FILTERED, state->speckleWindowSize, state->speckleRange, buf); - } - - if (disp0.data != disp.data) - disp.convertTo(disp0, disp0.type(), 1./(1 << DISPARITY_SHIFT), 0); -} - -StereoBM::StereoBM() -{ state = cvCreateStereoBMState(); } - -StereoBM::StereoBM(int _preset, int _ndisparities, int _SADWindowSize) -{ init(_preset, _ndisparities, _SADWindowSize); } - -void StereoBM::init(int _preset, int _ndisparities, int _SADWindowSize) -{ - state = cvCreateStereoBMState(_preset, _ndisparities); - state->SADWindowSize = _SADWindowSize; -} - -void StereoBM::operator()( InputArray _left, InputArray _right, - OutputArray _disparity, int disptype ) -{ - Mat left = _left.getMat(), right = _right.getMat(); - CV_Assert( disptype == CV_16S || disptype == CV_32F ); - _disparity.create(left.size(), disptype); - Mat disparity = _disparity.getMat(); - - findStereoCorrespondenceBM(left, right, disparity, state); -} + StereoBMParams params; + Mat preFilteredImg0, preFilteredImg1, cost, dispbuf; + Mat slidingSumBuf; +}; -template<> void Ptr::delete_obj() -{ cvReleaseStereoBMState(&obj); } +#define add_param(n) \ + obj.info()->addParam(obj, #n, obj.params.n) + +CV_INIT_ALGORITHM(StereoBMImpl, "StereoMatcher.BM", + add_param(preFilterType); + add_param(preFilterSize); + add_param(preFilterCap); + add_param(SADWindowSize); + add_param(minDisparity); + add_param(numDisparities); + add_param(textureThreshold); + add_param(uniquenessRatio); + add_param(speckleRange); + add_param(speckleWindowSize); + add_param(disp12MaxDiff); + add_param(dispType)); } -CV_IMPL void cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr, - CvArr* disparr, CvStereoBMState* state ) +cv::Ptr cv::createStereoBM(int _preset, int _numDisparities, int _SADWindowSize) { - cv::Mat left = cv::cvarrToMat(leftarr), - right = cv::cvarrToMat(rightarr), - disp = cv::cvarrToMat(disparr); - cv::findStereoCorrespondenceBM(left, right, disp, state); + return new StereoBMImpl(_preset, _numDisparities, _SADWindowSize); } /* End of file. */ diff --git a/modules/calib3d/src/stereosgbm.cpp b/modules/calib3d/src/stereosgbm.cpp index 4028004..026201f 100644 --- a/modules/calib3d/src/stereosgbm.cpp +++ b/modules/calib3d/src/stereosgbm.cpp @@ -12,6 +12,7 @@ // // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. // Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Copyright (C) 2013, OpenCV Foundation, all rights reserved. // Third party copyrights are property of their respective owners. // // Redistribution and use in source and binary forms, with or without modification, @@ -61,42 +62,52 @@ typedef short DispType; enum { NR = 16, NR2 = NR/2 }; -StereoSGBM::StereoSGBM() -{ - minDisparity = numberOfDisparities = 0; - SADWindowSize = 0; - P1 = P2 = 0; - disp12MaxDiff = 0; - preFilterCap = 0; - uniquenessRatio = 0; - speckleWindowSize = 0; - speckleRange = 0; - fullDP = false; -} - -StereoSGBM::StereoSGBM( int _minDisparity, int _numDisparities, int _SADWindowSize, - int _P1, int _P2, int _disp12MaxDiff, int _preFilterCap, - int _uniquenessRatio, int _speckleWindowSize, int _speckleRange, - bool _fullDP ) +struct StereoSGBMParams { - minDisparity = _minDisparity; - numberOfDisparities = _numDisparities; - SADWindowSize = _SADWindowSize; - P1 = _P1; - P2 = _P2; - disp12MaxDiff = _disp12MaxDiff; - preFilterCap = _preFilterCap; - uniquenessRatio = _uniquenessRatio; - speckleWindowSize = _speckleWindowSize; - speckleRange = _speckleRange; - fullDP = _fullDP; -} + StereoSGBMParams() + { + minDisparity = numDisparities = 0; + SADWindowSize = 0; + P1 = P2 = 0; + disp12MaxDiff = 0; + preFilterCap = 0; + uniquenessRatio = 0; + speckleWindowSize = 0; + speckleRange = 0; + fullDP = false; + } + StereoSGBMParams( int _minDisparity, int _numDisparities, int _SADWindowSize, + int _P1, int _P2, int _disp12MaxDiff, int _preFilterCap, + int _uniquenessRatio, int _speckleWindowSize, int _speckleRange, + bool _fullDP ) + { + minDisparity = _minDisparity; + numDisparities = _numDisparities; + SADWindowSize = _SADWindowSize; + P1 = _P1; + P2 = _P2; + disp12MaxDiff = _disp12MaxDiff; + preFilterCap = _preFilterCap; + uniquenessRatio = _uniquenessRatio; + speckleWindowSize = _speckleWindowSize; + speckleRange = _speckleRange; + fullDP = _fullDP; + } -StereoSGBM::~StereoSGBM() -{ -} + int minDisparity; + int numDisparities; + int SADWindowSize; + int preFilterCap; + int uniquenessRatio; + int P1; + int P2; + int speckleWindowSize; + int speckleRange; + int disp12MaxDiff; + bool fullDP; +}; /* For each pixel row1[x], max(-maxD, 0) <= minX <= x < maxX <= width - max(0, -minD), @@ -289,7 +300,7 @@ static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y, final after all the tiles are processed. the disparity in disp1buf is written with sub-pixel accuracy - (4 fractional bits, see CvStereoSGBM::DISP_SCALE), + (4 fractional bits, see StereoSGBM::DISP_SCALE), using quadratic interpolation, while the disparity in disp2buf is written as is, without interpolation. @@ -297,7 +308,7 @@ static void calcPixelCostBT( const Mat& img1, const Mat& img2, int y, It contains the minimum current cost, used to find the best disparity, corresponding to the minimal cost. */ static void computeDisparitySGBM( const Mat& img1, const Mat& img2, - Mat& disp1, const StereoSGBM& params, + Mat& disp1, const StereoSGBMParams& params, Mat& buffer ) { #if CV_SSE2 @@ -321,7 +332,7 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2, const int DISP_SCALE = StereoSGBM::DISP_SCALE; const CostType MAX_COST = SHRT_MAX; - int minD = params.minDisparity, maxD = minD + params.numberOfDisparities; + int minD = params.minDisparity, maxD = minD + params.numDisparities; Size SADWindowSize; SADWindowSize.width = SADWindowSize.height = params.SADWindowSize > 0 ? params.SADWindowSize : 5; int ftzero = std::max(params.preFilterCap, 15) | 1; @@ -817,26 +828,80 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2, } } -typedef cv::Point_ Point2s; -void StereoSGBM::operator ()( InputArray _left, InputArray _right, - OutputArray _disp ) +class StereoSGBMImpl : public StereoMatcher { - Mat left = _left.getMat(), right = _right.getMat(); - CV_Assert( left.size() == right.size() && left.type() == right.type() && - left.depth() == DataType::depth ); +public: + StereoSGBMImpl() + { + params = StereoSGBMParams(); + } - _disp.create( left.size(), CV_16S ); - Mat disp = _disp.getMat(); + StereoSGBMImpl( int _minDisparity, int _numDisparities, int _SADWindowSize, + int _P1, int _P2, int _disp12MaxDiff, int _preFilterCap, + int _uniquenessRatio, int _speckleWindowSize, int _speckleRange, + bool _fullDP ) + { + params = StereoSGBMParams( _minDisparity, _numDisparities, _SADWindowSize, + _P1, _P2, _disp12MaxDiff, _preFilterCap, + _uniquenessRatio, _speckleWindowSize, _speckleRange, + _fullDP ); + } + + void compute( InputArray leftarr, InputArray rightarr, OutputArray disparr ) + { + Mat left = leftarr.getMat(), right = rightarr.getMat(); + CV_Assert( left.size() == right.size() && left.type() == right.type() && + left.depth() == CV_8U ); - computeDisparitySGBM( left, right, disp, *this, buffer ); - medianBlur(disp, disp, 3); + disparr.create( left.size(), CV_16S ); + Mat disp = disparr.getMat(); - if( speckleWindowSize > 0 ) - filterSpeckles(disp, (minDisparity - 1)*DISP_SCALE, speckleWindowSize, DISP_SCALE*speckleRange, buffer); + computeDisparitySGBM( left, right, disp, params, buffer ); + medianBlur(disp, disp, 3); + + if( params.speckleWindowSize > 0 ) + filterSpeckles(disp, (params.minDisparity - 1)*STEREO_DISP_SCALE, params.speckleWindowSize, + STEREO_DISP_SCALE*params.speckleRange, buffer); + } + + AlgorithmInfo* info() const; + + StereoSGBMParams params; + Mat buffer; +}; + + +Ptr createStereoSGBM(int minDisparity, int numDisparities, int SADWindowSize, + int P1, int P2, int disp12MaxDiff, + int preFilterCap, int uniquenessRatio, + int speckleWindowSize, int speckleRange, + bool fullDP) +{ + return new StereoSGBMImpl(minDisparity, numDisparities, SADWindowSize, + P1, P2, disp12MaxDiff, + preFilterCap, uniquenessRatio, + speckleWindowSize, speckleRange, + fullDP); } +#define add_param(n) \ + obj.info()->addParam(obj, #n, obj.params.n) + +CV_INIT_ALGORITHM(StereoSGBMImpl, "StereoMatcher.SGBM", + add_param(minDisparity); + add_param(numDisparities); + add_param(SADWindowSize); + add_param(preFilterCap); + add_param(uniquenessRatio); + add_param(P1); + add_param(P2); + add_param(speckleWindowSize); + add_param(speckleRange); + add_param(disp12MaxDiff); + add_param(fullDP)); + Rect getValidDisparityROI( Rect roi1, Rect roi2, int minDisparity, int numberOfDisparities, @@ -855,108 +920,107 @@ Rect getValidDisparityROI( Rect roi1, Rect roi2, return r.width > 0 && r.height > 0 ? r : Rect(); } -} +typedef cv::Point_ Point2s; -namespace +template +void filterSpecklesImpl(cv::Mat& img, int newVal, int maxSpeckleSize, int maxDiff, cv::Mat& _buf) { - template - void filterSpecklesImpl(cv::Mat& img, int newVal, int maxSpeckleSize, int maxDiff, cv::Mat& _buf) + using namespace cv; + + int width = img.cols, height = img.rows, npixels = width*height; + size_t bufSize = npixels*(int)(sizeof(Point2s) + sizeof(int) + sizeof(uchar)); + if( !_buf.isContinuous() || !_buf.data || _buf.cols*_buf.rows*_buf.elemSize() < bufSize ) + _buf.create(1, (int)bufSize, CV_8U); + + uchar* buf = _buf.data; + int i, j, dstep = (int)(img.step/sizeof(T)); + int* labels = (int*)buf; + buf += npixels*sizeof(labels[0]); + Point2s* wbuf = (Point2s*)buf; + buf += npixels*sizeof(wbuf[0]); + uchar* rtype = (uchar*)buf; + int curlabel = 0; + + // clear out label assignments + memset(labels, 0, npixels*sizeof(labels[0])); + + for( i = 0; i < height; i++ ) { - using namespace cv; - - int width = img.cols, height = img.rows, npixels = width*height; - size_t bufSize = npixels*(int)(sizeof(Point2s) + sizeof(int) + sizeof(uchar)); - if( !_buf.isContinuous() || !_buf.data || _buf.cols*_buf.rows*_buf.elemSize() < bufSize ) - _buf.create(1, (int)bufSize, CV_8U); - - uchar* buf = _buf.data; - int i, j, dstep = (int)(img.step/sizeof(T)); - int* labels = (int*)buf; - buf += npixels*sizeof(labels[0]); - Point2s* wbuf = (Point2s*)buf; - buf += npixels*sizeof(wbuf[0]); - uchar* rtype = (uchar*)buf; - int curlabel = 0; - - // clear out label assignments - memset(labels, 0, npixels*sizeof(labels[0])); - - for( i = 0; i < height; i++ ) - { - T* ds = img.ptr(i); - int* ls = labels + width*i; + T* ds = img.ptr(i); + int* ls = labels + width*i; - for( j = 0; j < width; j++ ) + for( j = 0; j < width; j++ ) + { + if( ds[j] != newVal ) // not a bad disparity { - if( ds[j] != newVal ) // not a bad disparity + if( ls[j] ) // has a label, check for bad label { - if( ls[j] ) // has a label, check for bad label - { - if( rtype[ls[j]] ) // small region, zero out disparity - ds[j] = (T)newVal; - } - // no label, assign and propagate - else + if( rtype[ls[j]] ) // small region, zero out disparity + ds[j] = (T)newVal; + } + // no label, assign and propagate + else + { + Point2s* ws = wbuf; // initialize wavefront + Point2s p((short)j, (short)i); // current pixel + curlabel++; // next label + int count = 0; // current region size + ls[j] = curlabel; + + // wavefront propagation + while( ws >= wbuf ) // wavefront not empty { - Point2s* ws = wbuf; // initialize wavefront - Point2s p((short)j, (short)i); // current pixel - curlabel++; // next label - int count = 0; // current region size - ls[j] = curlabel; - - // wavefront propagation - while( ws >= wbuf ) // wavefront not empty - { - count++; - // put neighbors onto wavefront - T* dpp = &img.at(p.y, p.x); - T dp = *dpp; - int* lpp = labels + width*p.y + p.x; - - if( p.x < width-1 && !lpp[+1] && dpp[+1] != newVal && std::abs(dp - dpp[+1]) <= maxDiff ) - { - lpp[+1] = curlabel; - *ws++ = Point2s(p.x+1, p.y); - } - - if( p.x > 0 && !lpp[-1] && dpp[-1] != newVal && std::abs(dp - dpp[-1]) <= maxDiff ) - { - lpp[-1] = curlabel; - *ws++ = Point2s(p.x-1, p.y); - } + count++; + // put neighbors onto wavefront + T* dpp = &img.at(p.y, p.x); + T dp = *dpp; + int* lpp = labels + width*p.y + p.x; - if( p.y < height-1 && !lpp[+width] && dpp[+dstep] != newVal && std::abs(dp - dpp[+dstep]) <= maxDiff ) - { - lpp[+width] = curlabel; - *ws++ = Point2s(p.x, p.y+1); - } + if( p.x < width-1 && !lpp[+1] && dpp[+1] != newVal && std::abs(dp - dpp[+1]) <= maxDiff ) + { + lpp[+1] = curlabel; + *ws++ = Point2s(p.x+1, p.y); + } - if( p.y > 0 && !lpp[-width] && dpp[-dstep] != newVal && std::abs(dp - dpp[-dstep]) <= maxDiff ) - { - lpp[-width] = curlabel; - *ws++ = Point2s(p.x, p.y-1); - } + if( p.x > 0 && !lpp[-1] && dpp[-1] != newVal && std::abs(dp - dpp[-1]) <= maxDiff ) + { + lpp[-1] = curlabel; + *ws++ = Point2s(p.x-1, p.y); + } - // pop most recent and propagate - // NB: could try least recent, maybe better convergence - p = *--ws; + if( p.y < height-1 && !lpp[+width] && dpp[+dstep] != newVal && std::abs(dp - dpp[+dstep]) <= maxDiff ) + { + lpp[+width] = curlabel; + *ws++ = Point2s(p.x, p.y+1); } - // assign label type - if( count <= maxSpeckleSize ) // speckle region + if( p.y > 0 && !lpp[-width] && dpp[-dstep] != newVal && std::abs(dp - dpp[-dstep]) <= maxDiff ) { - rtype[ls[j]] = 1; // small region label - ds[j] = (T)newVal; + lpp[-width] = curlabel; + *ws++ = Point2s(p.x, p.y-1); } - else - rtype[ls[j]] = 0; // large region label + + // pop most recent and propagate + // NB: could try least recent, maybe better convergence + p = *--ws; + } + + // assign label type + if( count <= maxSpeckleSize ) // speckle region + { + rtype[ls[j]] = 1; // small region label + ds[j] = (T)newVal; } + else + rtype[ls[j]] = 0; // large region label } } } } } +} + void cv::filterSpeckles( InputOutputArray _img, double _newval, int maxSpeckleSize, double _maxDiff, InputOutputArray __buf ) { @@ -1054,16 +1118,3 @@ void cv::validateDisparity( InputOutputArray _disp, InputArray _cost, int minDis } } -CvRect cvGetValidDisparityROI( CvRect roi1, CvRect roi2, int minDisparity, - int numberOfDisparities, int SADWindowSize ) -{ - return (CvRect)cv::getValidDisparityROI( roi1, roi2, minDisparity, - numberOfDisparities, SADWindowSize ); -} - -void cvValidateDisparity( CvArr* _disp, const CvArr* _cost, int minDisparity, - int numberOfDisparities, int disp12MaxDiff ) -{ - cv::Mat disp = cv::cvarrToMat(_disp), cost = cv::cvarrToMat(_cost); - cv::validateDisparity( disp, cost, minDisparity, numberOfDisparities, disp12MaxDiff ); -}