mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-11-28 21:20:18 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

new API for StereoMatchers

Browse Source
This commit is contained in:
Vadim Pisarevsky 2013-03-01 02:24:46 +04:00
parent 891d7da6ee
commit b6efe30527
4 changed files with 657 additions and 393 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
modules/calib3d/include/opencv2/calib3d/calib3d.hpp
View File

@ -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 );
class CV_EXPORTS_W StereoMatcher : public Algorithm
{
public:
CV_WRAP virtual void compute( InputArray left, InputArray right,
OutputArray disparity ) = 0;
};
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<StereoMatcher> createStereoBM(int preset, int numDisparities=0, int SADWindowSize=21);
CV_EXPORTS Ptr<StereoMatcher> 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<CvStereoBMState>::delete_obj();
/*!
Block Matching Stereo Correspondence Algorithm
The class implements BM stereo correspondence algorithm by K. Konolige.
*/
// 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<StereoMatcher> sm;
};
//! filters off speckles (small regions of incorrectly computed disparity)

217
modules/calib3d/src/compat_stereo.cpp Normal file
View File

@ -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<CvStereoBMState>::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<cv::StereoMatcher> 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);
}
}

451
modules/calib3d/src/stereobm.cpp
View File

@ -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,58 +47,45 @@
#include "precomp.hpp"
#include <stdio.h>
//#undef CV_SSE2
//#define CV_SSE2 0
//#include "emmintrin.h"
#include <limits>
CV_IMPL 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;
}
CV_IMPL 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 );
}
namespace cv
{
struct StereoBMParams
{
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;
}
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 )
{
int x, y, wsz2 = winsize/2;
@ -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" );
if (left0.type() != CV_8UC1 || right0.type() != CV_8UC1)
CV_Error( CV_StsUnsupportedFormat, "Both input images must have CV_8UC1" );
if (disp0.type() != CV_16SC1 && disp0.type() != CV_32FC1)
CV_Error( CV_StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format" );
if( !state )
CV_Error( CV_StsNullPtr, "Stereo BM state is NULL." );
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( state->preFilterSize < 5 || state->preFilterSize > 255 || state->preFilterSize % 2 == 0 )
CV_Error( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" );
if( state->preFilterCap < 1 || state->preFilterCap > 63 )
CV_Error( CV_StsOutOfRange, "preFilterCap must be within 1..63" );
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( state->numberOfDisparities <= 0 || state->numberOfDisparities % 16 != 0 )
CV_Error( CV_StsOutOfRange, "numberOfDisparities must be positive and divisble by 16" );
if( state->textureThreshold < 0 )
CV_Error( CV_StsOutOfRange, "texture threshold must be non-negative" );
if( state->uniquenessRatio < 0 )
CV_Error( CV_StsOutOfRange, "uniqueness ratio must be non-negative" );
if( !state->preFilteredImg0 || state->preFilteredImg0->cols * state->preFilteredImg0->rows < left0.cols * left0.rows )
public:
StereoBMImpl()
{
cvReleaseMat( &state->preFilteredImg0 );
cvReleaseMat( &state->preFilteredImg1 );
cvReleaseMat( &state->cost );
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);
int mindisp = state->minDisparity;
int ndisp = state->numberOfDisparities;
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;
if( lofs >= width || rofs >= width || width1 < 1 )
{
disp0 = Scalar::all( FILTERED * ( disp0.type() < CV_32F ? 1 : 1./(1 << DISPARITY_SHIFT) ) );
return;
params = StereoBMParams();
}
Mat disp = disp0;
if( disp0.type() == CV_32F)
StereoBMImpl( int _preset, int _numDisparities, int _SADWindowSize )
{
if( !state->disp || state->disp->rows != disp0.rows || state->disp->cols != disp0.cols )
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 (left0.size() != right0.size())
CV_Error( CV_StsUnmatchedSizes, "All the images must have the same size" );
if (left0.type() != CV_8UC1 || right0.type() != CV_8UC1)
CV_Error( CV_StsUnsupportedFormat, "Both input images must have CV_8UC1" );
if (dtype != CV_16SC1 && dtype != CV_32FC1)
CV_Error( CV_StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format" );
disparr.create(left0.size(), dtype);
Mat disp0 = disparr.getMat();
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( params.preFilterSize < 5 || params.preFilterSize > 255 || params.preFilterSize % 2 == 0 )
CV_Error( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" );
if( params.preFilterCap < 1 || params.preFilterCap > 63 )
CV_Error( CV_StsOutOfRange, "preFilterCap must be within 1..63" );
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( params.numDisparities <= 0 || params.numDisparities % 16 != 0 )
CV_Error( CV_StsOutOfRange, "numDisparities must be positive and divisble by 16" );
if( params.textureThreshold < 0 )
CV_Error( CV_StsOutOfRange, "texture threshold must be non-negative" );
if( params.uniquenessRatio < 0 )
CV_Error( CV_StsOutOfRange, "uniqueness ratio must be non-negative" );
preFilteredImg0.create( left0.size(), CV_8U );
preFilteredImg1.create( left0.size(), CV_8U );
cost.create( left0.size(), CV_16S );
Mat left = preFilteredImg0, right = preFilteredImg1;
int mindisp = params.minDisparity;
int ndisp = params.numDisparities;
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( 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_<short>) + 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);
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_<short>) + sizeof(int) + sizeof(uchar));
#if CV_SSE2
bool useShorts = state->preFilterCap <= 31 && state->SADWindowSize <= 21 && checkHardwareSupport(CV_CPU_SSE2);
bool useShorts = params.preFilterCap <= 31 && params.SADWindowSize <= 21 && checkHardwareSupport(CV_CPU_SSE2);
#else
const bool useShorts = false;
const bool useShorts = false;
#endif
#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);
#else
const int nstripes = 1;
#endif
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));
int bufSize = std::max(bufSize0 * nstripes, std::max(bufSize1 * 2, bufSize2));
if( slidingSumBuf.cols < bufSize )
slidingSumBuf.create( 1, bufSize, CV_8U );
if( !state->slidingSumBuf || state->slidingSumBuf->cols < bufSize )
{
cvReleaseMat( &state->slidingSumBuf );
state->slidingSumBuf = cvCreateMat( 1, bufSize, CV_8U );
uchar *_buf = slidingSumBuf.data;
parallel_for_(Range(0, 2), PrefilterInvoker(left0, right0, left, right, _buf, _buf + bufSize1, &params), 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, &params, 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));
AlgorithmInfo* info() const;
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);
StereoBMParams params;
Mat preFilteredImg0, preFilteredImg1, cost, dispbuf;
Mat slidingSumBuf;
};
parallel_for(BlockedRange(0, nstripes),
FindStereoCorrespInvoker(left, right, disp, state, nstripes,
bufSize0, useShorts, validDisparityRect));
#define add_param(n) \
obj.info()->addParam(obj, #n, obj.params.n)
if( state->speckleRange >= 0 && state->speckleWindowSize > 0 )
{
Mat buf(state->slidingSumBuf);
filterSpeckles(disp, FILTERED, state->speckleWindowSize, state->speckleRange, buf);
}
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));
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)
cv::Ptr<cv::StereoMatcher> cv::createStereoBM(int _preset, int _numDisparities, 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);
}
template<> void Ptr<CvStereoBMState>::delete_obj()
{ cvReleaseStereoBMState(&obj); }
}
CV_IMPL void cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr,
CvArr* disparr, CvStereoBMState* state )
{
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. */

345
modules/calib3d/src/stereosgbm.cpp
View File

@ -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()
struct StereoSGBMParams
{
minDisparity = numberOfDisparities = 0;
SADWindowSize = 0;
P1 = P2 = 0;
disp12MaxDiff = 0;
preFilterCap = 0;
uniquenessRatio = 0;
speckleWindowSize = 0;
speckleRange = 0;
fullDP = false;
}
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 _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;
}
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_<short> 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<PixType>::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 );
}
computeDisparitySGBM( left, right, disp, *this, buffer );
medianBlur(disp, disp, 3);
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 );
if( speckleWindowSize > 0 )
filterSpeckles(disp, (minDisparity - 1)*DISP_SCALE, speckleWindowSize, DISP_SCALE*speckleRange, buffer);
disparr.create( left.size(), CV_16S );
Mat disp = disparr.getMat();
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<StereoMatcher> 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_<short> Point2s;
namespace
template <typename T>
void filterSpecklesImpl(cv::Mat& img, int newVal, int maxSpeckleSize, int maxDiff, cv::Mat& _buf)
{
template <typename T>
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;
T* ds = img.ptr<T>(i);
int* ls = labels + width*i;
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++ )
for( j = 0; j < width; j++ )
{
T* ds = img.ptr<T>(i);
int* ls = labels + width*i;
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
{
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
{
if( rtype[ls[j]] ) // small region, zero out disparity
ds[j] = (T)newVal;
count++;
// put neighbors onto wavefront
T* dpp = &img.at<T>(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);
}
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.y > 0 && !lpp[-width] && dpp[-dstep] != newVal && std::abs(dp - dpp[-dstep]) <= maxDiff )
{
lpp[-width] = curlabel;
*ws++ = Point2s(p.x, p.y-1);
}
// 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;
}
// 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
{
count++;
// put neighbors onto wavefront
T* dpp = &img.at<T>(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);
}
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.y > 0 && !lpp[-width] && dpp[-dstep] != newVal && std::abs(dp - dpp[-dstep]) <= maxDiff )
{
lpp[-width] = curlabel;
*ws++ = Point2s(p.x, p.y-1);
}
// 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
}
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 );
}