generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests 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, CV_EXPORTS_W void correctMatches( InputArray F, InputArray points1, InputArray points2,
OutputArray newPoints1, OutputArray newPoints2 ); 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(); template<> CV_EXPORTS void Ptr<CvStereoBMState>::delete_obj();
/*! // to be moved to "compat" module
Block Matching Stereo Correspondence Algorithm
The class implements BM stereo correspondence algorithm by K. Konolige.
*/
class CV_EXPORTS_W StereoBM class CV_EXPORTS_W StereoBM
{ {
public: public:
enum { PREFILTER_NORMALIZED_RESPONSE = 0, PREFILTER_XSOBEL = 1, 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 //! the default constructor
CV_WRAP StereoBM(); CV_WRAP StereoBM();
@ -697,11 +714,7 @@ public:
}; };
/*! // to be moved to "compat" module
Semi-Global Block Matching Stereo Correspondence Algorithm
The class implements the original SGBM stereo correspondence algorithm by H. Hirschmuller and some its modification.
*/
class CV_EXPORTS_W StereoSGBM class CV_EXPORTS_W StereoSGBM
{ {
public: public:
@ -736,7 +749,7 @@ public:
CV_PROP_RW bool fullDP; CV_PROP_RW bool fullDP;
protected: protected:
Mat buffer; Ptr<StereoMatcher> sm;
}; };
//! filters off speckles (small regions of incorrectly computed disparity) //! 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. // copy or use the software.
// //
// //
// Intel License Agreement // License Agreement
// For Open Source Computer Vision Library // For Open Source Computer Vision Library
// //
// Copyright (C) 2000, Intel Corporation, all rights reserved. // 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. // Third party copyrights are property of their respective owners.
// //
// Redistribution and use in source and binary forms, with or without modification, // 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 // this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution. // 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. // derived from this software without specific prior written permission.
// //
// This software is provided by the copyright holders and contributors "as is" and // This software is provided by the copyright holders and contributors "as is" and
@ -46,58 +47,45 @@
#include "precomp.hpp" #include "precomp.hpp"
#include <stdio.h> #include <stdio.h>
//#undef CV_SSE2
//#define CV_SSE2 0
//#include "emmintrin.h"
#include <limits> #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 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 ) static void prefilterNorm( const Mat& src, Mat& dst, int winsize, int ftzero, uchar* buf )
{ {
int x, y, wsz2 = winsize/2; 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; dptr0[0] = dptr0[size.width-1] = dptr1[0] = dptr1[size.width-1] = val0;
x = 1; x = 1;
#if CV_SSE2 #if CV_SSE2
if( useSIMD ) if( useSIMD )
{ {
__m128i z = _mm_setzero_si128(), ftz = _mm_set1_epi16((short)ftzero), __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 ) for( ; x <= size.width-9; x += 8 )
{ {
__m128i c0 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow0 + x - 1)), z); __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)); _mm_storel_epi64((__m128i*)(dptr1 + x), _mm_unpackhi_epi64(v0, v0));
} }
} }
#endif #endif
for( ; x < size.width-1; x++ ) for( ; x < size.width-1; x++ )
{ {
int d0 = srow0[x+1] - srow0[x-1], d1 = srow1[x+1] - srow1[x-1], 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 v0 = tab[d0 + d1*2 + d2 + OFS];
int v1 = tab[d1 + d2*2 + d3 + OFS]; int v1 = tab[d1 + d2*2 + d3 + OFS];
dptr0[x] = (uchar)v0; dptr0[x] = (uchar)v0;
@ -249,14 +237,14 @@ static const int DISPARITY_SHIFT = 4;
#if CV_SSE2 #if CV_SSE2
static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right, static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
Mat& disp, Mat& cost, CvStereoBMState& state, Mat& disp, Mat& cost, StereoBMParams& state,
uchar* buf, int _dy0, int _dy1 ) uchar* buf, int _dy0, int _dy1 )
{ {
const int ALIGN = 16; const int ALIGN = 16;
int x, y, d; int x, y, d;
int wsz = state.SADWindowSize, wsz2 = wsz/2; int wsz = state.SADWindowSize, wsz2 = wsz/2;
int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1); int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);
int ndisp = state.numberOfDisparities; int ndisp = state.numDisparities;
int mindisp = state.minDisparity; int mindisp = state.minDisparity;
int lofs = MAX(ndisp - 1 + mindisp, 0); int lofs = MAX(ndisp - 1 + mindisp, 0);
int rofs = -MIN(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; rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep;
for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp, 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]; int lval = lptr[0];
__m128i lv = _mm_set1_epi8((char)lval), z = _mm_setzero_si128(); __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 thresh8 = _mm_set1_epi16((short)(thresh + 1));
__m128i d1 = _mm_set1_epi16((short)(mind-1)), d2 = _mm_set1_epi16((short)(mind+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); __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 ) for( d = 0; d < ndisp; d += 16 )
{ {
@ -507,14 +495,14 @@ static void findStereoCorrespondenceBM_SSE2( const Mat& left, const Mat& right,
static void static void
findStereoCorrespondenceBM( const Mat& left, const Mat& right, findStereoCorrespondenceBM( const Mat& left, const Mat& right,
Mat& disp, Mat& cost, const CvStereoBMState& state, Mat& disp, Mat& cost, const StereoBMParams& state,
uchar* buf, int _dy0, int _dy1 ) uchar* buf, int _dy0, int _dy1 )
{ {
const int ALIGN = 16; const int ALIGN = 16;
int x, y, d; int x, y, d;
int wsz = state.SADWindowSize, wsz2 = wsz/2; int wsz = state.SADWindowSize, wsz2 = wsz/2;
int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1); int dy0 = MIN(_dy0, wsz2+1), dy1 = MIN(_dy1, wsz2+1);
int ndisp = state.numberOfDisparities; int ndisp = state.numDisparities;
int mindisp = state.minDisparity; int mindisp = state.minDisparity;
int lofs = MAX(ndisp - 1 + mindisp, 0); int lofs = MAX(ndisp - 1 + mindisp, 0);
int rofs = -MIN(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; rptr = rptr0 + MIN(MAX(x1, -rofs), width-1-rofs) - dy0*sstep;
for( y = -dy0; y < height + dy1; y++, cbuf += ndisp, cbuf_sub += ndisp, 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]; int lval = lptr[0];
for( d = 0; d < ndisp; d++ ) for( d = 0; d < ndisp; d++ )
@ -662,21 +650,21 @@ findStereoCorrespondenceBM( const Mat& left, const Mat& right,
} }
{ {
sad[-1] = sad[1]; sad[-1] = sad[1];
sad[ndisp] = sad[ndisp-2]; sad[ndisp] = sad[ndisp-2];
int p = sad[mind+1], n = sad[mind-1]; int p = sad[mind+1], n = sad[mind-1];
d = p + n - 2*sad[mind] + std::abs(p - n); 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); dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*256/d : 0) + 15) >> 4);
costptr[y*coststep] = sad[mind]; costptr[y*coststep] = sad[mind];
} }
} }
} }
} }
struct PrefilterInvoker struct PrefilterInvoker : public ParallelLoopBody
{ {
PrefilterInvoker(const Mat& left0, const Mat& right0, Mat& left, Mat& right, 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; imgs0[0] = &left0; imgs0[1] = &right0;
imgs[0] = &left; imgs[1] = &right; imgs[0] = &left; imgs[1] = &right;
@ -684,41 +672,47 @@ struct PrefilterInvoker
state = _state; state = _state;
} }
void operator()( int ind ) const void operator()( const Range& range ) const
{ {
if( state->preFilterType == CV_STEREO_BM_NORMALIZED_RESPONSE ) for( int i = range.start; i < range.end; i++ )
prefilterNorm( *imgs0[ind], *imgs[ind], state->preFilterSize, state->preFilterCap, buf[ind] ); {
else if( state->preFilterType == STEREO_PREFILTER_NORMALIZED_RESPONSE )
prefilterXSobel( *imgs0[ind], *imgs[ind], state->preFilterCap ); prefilterNorm( *imgs0[i], *imgs[i], state->preFilterSize, state->preFilterCap, buf[i] );
else
prefilterXSobel( *imgs0[i], *imgs[i], state->preFilterCap );
}
} }
const Mat* imgs0[2]; const Mat* imgs0[2];
Mat* imgs[2]; Mat* imgs[2];
uchar* buf[2]; uchar* buf[2];
CvStereoBMState *state; StereoBMParams* state;
}; };
struct FindStereoCorrespInvoker struct FindStereoCorrespInvoker : public ParallelLoopBody
{ {
FindStereoCorrespInvoker( const Mat& _left, const Mat& _right, FindStereoCorrespInvoker( const Mat& _left, const Mat& _right,
Mat& _disp, CvStereoBMState* _state, Mat& _disp, StereoBMParams* _state,
int _nstripes, int _stripeBufSize, int _nstripes, size_t _stripeBufSize,
bool _useShorts, Rect _validDisparityRect ) bool _useShorts, Rect _validDisparityRect,
Mat& _slidingSumBuf, Mat& _cost )
{ {
left = &_left; right = &_right; left = &_left; right = &_right;
disp = &_disp; state = _state; disp = &_disp; state = _state;
nstripes = _nstripes; stripeBufSize = _stripeBufSize; nstripes = _nstripes; stripeBufSize = _stripeBufSize;
useShorts = _useShorts; useShorts = _useShorts;
validDisparityRect = _validDisparityRect; 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 cols = left->cols, rows = left->rows;
int _row0 = std::min(cvRound(range.begin() * rows / nstripes), rows); int _row0 = std::min(cvRound(range.start * rows / nstripes), rows);
int _row1 = std::min(cvRound(range.end() * rows / nstripes), rows); int _row1 = std::min(cvRound(range.end * rows / nstripes), rows);
uchar *ptr = state->slidingSumBuf->data.ptr + range.begin() * stripeBufSize; uchar *ptr = slidingSumBuf->data + range.start * stripeBufSize;
int FILTERED = (state->minDisparity - 1)*16; int FILTERED = (state->minDisparity - 1)*16;
Rect roi = validDisparityRect & Rect(0, _row0, cols, _row1 - _row0); Rect roi = validDisparityRect & Rect(0, _row0, cols, _row1 - _row0);
@ -742,7 +736,7 @@ struct FindStereoCorrespInvoker
Mat left_i = left->rowRange(row0, row1); Mat left_i = left->rowRange(row0, row1);
Mat right_i = right->rowRange(row0, row1); Mat right_i = right->rowRange(row0, row1);
Mat disp_i = disp->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 CV_SSE2
if( useShorts ) if( useShorts )
@ -752,7 +746,7 @@ struct FindStereoCorrespInvoker
findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 ); findStereoCorrespondenceBM( left_i, right_i, disp_i, cost_i, *state, ptr, row0, rows - row1 );
if( state->disp12MaxDiff >= 0 ) 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 ) if( roi.x > 0 )
{ {
@ -768,185 +762,174 @@ struct FindStereoCorrespInvoker
protected: protected:
const Mat *left, *right; const Mat *left, *right;
Mat* disp; Mat* disp, *slidingSumBuf, *cost;
CvStereoBMState *state; StereoBMParams *state;
int nstripes; int nstripes;
int stripeBufSize; size_t stripeBufSize;
bool useShorts; bool useShorts;
Rect validDisparityRect; 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()) public:
CV_Error( CV_StsUnmatchedSizes, "All the images must have the same size" ); StereoBMImpl()
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 )
{ {
cvReleaseMat( &state->preFilteredImg0 ); params = StereoBMParams();
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;
} }
Mat disp = disp0; StereoBMImpl( int _preset, int _numDisparities, int _SADWindowSize )
if( disp0.type() == CV_32F)
{ {
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 ); disp0 = Scalar::all( FILTERED * ( disp0.type() < CV_32F ? 1 : 1./(1 << DISPARITY_SHIFT) ) );
state->disp = cvCreateMat(disp0.rows, disp0.cols, CV_16S); return;
} }
disp = cv::cvarrToMat(state->disp);
}
int wsz = state->SADWindowSize; Mat disp = disp0;
int bufSize0 = (int)((ndisp + 2)*sizeof(int)); if( dtype == CV_32F )
bufSize0 += (int)((height+wsz+2)*ndisp*sizeof(int)); {
bufSize0 += (int)((height + wsz + 2)*sizeof(int)); dispbuf.create(disp0.size(), CV_16S);
bufSize0 += (int)((height+wsz+2)*ndisp*(wsz+2)*sizeof(uchar) + 256); disp = dispbuf;
}
int bufSize1 = (int)((width + state->preFilterSize + 2) * sizeof(int) + 256); int wsz = params.SADWindowSize;
int bufSize2 = 0; int bufSize0 = (int)((ndisp + 2)*sizeof(int));
if( state->speckleRange >= 0 && state->speckleWindowSize > 0 ) bufSize0 += (int)((height+wsz+2)*ndisp*sizeof(int));
bufSize2 = width*height*(sizeof(cv::Point_<short>) + sizeof(int) + sizeof(uchar)); 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 #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 #else
const bool useShorts = false; const bool useShorts = false;
#endif #endif
#ifdef HAVE_TBB const double SAD_overhead_coeff = 10.0;
const double SAD_overhead_coeff = 10.0; double N0 = 8000000 / (useShorts ? 1 : 4); // approx tbb's min number instructions reasonable for one thread
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);
double maxStripeSize = std::min(std::max(N0 / (width * ndisp), (wsz-1) * SAD_overhead_coeff), (double)height); int nstripes = cvCeil(height / maxStripeSize);
int nstripes = cvCeil(height / maxStripeSize); int bufSize = std::max(bufSize0 * nstripes, std::max(bufSize1 * 2, bufSize2));
#else
const int nstripes = 1;
#endif
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 ) uchar *_buf = slidingSumBuf.data;
{ parallel_for_(Range(0, 2), PrefilterInvoker(left0, right0, left, right, _buf, _buf + bufSize1, &params), 1);
cvReleaseMat( &state->slidingSumBuf );
state->slidingSumBuf = cvCreateMat( 1, bufSize, CV_8U ); 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; AlgorithmInfo* info() const;
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; StereoBMParams params;
validDisparityRect = getValidDisparityROI(R1.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect, Mat preFilteredImg0, preFilteredImg1, cost, dispbuf;
R2.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect, Mat slidingSumBuf;
state->minDisparity, state->numberOfDisparities, };
state->SADWindowSize);
parallel_for(BlockedRange(0, nstripes), #define add_param(n) \
FindStereoCorrespInvoker(left, right, disp, state, nstripes, obj.info()->addParam(obj, #n, obj.params.n)
bufSize0, useShorts, validDisparityRect));
if( state->speckleRange >= 0 && state->speckleWindowSize > 0 ) CV_INIT_ALGORITHM(StereoBMImpl, "StereoMatcher.BM",
{ add_param(preFilterType);
Mat buf(state->slidingSumBuf); add_param(preFilterSize);
filterSpeckles(disp, FILTERED, state->speckleWindowSize, state->speckleRange, buf); 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() cv::Ptr<cv::StereoMatcher> cv::createStereoBM(int _preset, int _numDisparities, int _SADWindowSize)
{ 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); return new StereoBMImpl(_preset, _numDisparities, _SADWindowSize);
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);
} }
/* End of file. */ /* 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) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., 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. // Third party copyrights are property of their respective owners.
// //
// Redistribution and use in source and binary forms, with or without modification, // 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 }; enum { NR = 16, NR2 = NR/2 };
StereoSGBM::StereoSGBM()
struct StereoSGBMParams
{ {
minDisparity = numberOfDisparities = 0; StereoSGBMParams()
SADWindowSize = 0; {
P1 = P2 = 0; minDisparity = numDisparities = 0;
disp12MaxDiff = 0; SADWindowSize = 0;
preFilterCap = 0; P1 = P2 = 0;
uniquenessRatio = 0; disp12MaxDiff = 0;
speckleWindowSize = 0; preFilterCap = 0;
speckleRange = 0; uniquenessRatio = 0;
fullDP = false; 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 minDisparity;
int _P1, int _P2, int _disp12MaxDiff, int _preFilterCap, int numDisparities;
int _uniquenessRatio, int _speckleWindowSize, int _speckleRange, int SADWindowSize;
bool _fullDP ) int preFilterCap;
{ int uniquenessRatio;
minDisparity = _minDisparity; int P1;
numberOfDisparities = _numDisparities; int P2;
SADWindowSize = _SADWindowSize; int speckleWindowSize;
P1 = _P1; int speckleRange;
P2 = _P2; int disp12MaxDiff;
disp12MaxDiff = _disp12MaxDiff; bool fullDP;
preFilterCap = _preFilterCap; };
uniquenessRatio = _uniquenessRatio;
speckleWindowSize = _speckleWindowSize;
speckleRange = _speckleRange;
fullDP = _fullDP;
}
StereoSGBM::~StereoSGBM()
{
}
/* /*
For each pixel row1[x], max(-maxD, 0) <= minX <= x < maxX <= width - max(0, -minD), 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. final after all the tiles are processed.
the disparity in disp1buf is written with sub-pixel accuracy 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 using quadratic interpolation, while the disparity in disp2buf
is written as is, without interpolation. 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. 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, static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
Mat& disp1, const StereoSGBM& params, Mat& disp1, const StereoSGBMParams& params,
Mat& buffer ) Mat& buffer )
{ {
#if CV_SSE2 #if CV_SSE2
@ -321,7 +332,7 @@ static void computeDisparitySGBM( const Mat& img1, const Mat& img2,
const int DISP_SCALE = StereoSGBM::DISP_SCALE; const int DISP_SCALE = StereoSGBM::DISP_SCALE;
const CostType MAX_COST = SHRT_MAX; const CostType MAX_COST = SHRT_MAX;
int minD = params.minDisparity, maxD = minD + params.numberOfDisparities; int minD = params.minDisparity, maxD = minD + params.numDisparities;
Size SADWindowSize; Size SADWindowSize;
SADWindowSize.width = SADWindowSize.height = params.SADWindowSize > 0 ? params.SADWindowSize : 5; SADWindowSize.width = SADWindowSize.height = params.SADWindowSize > 0 ? params.SADWindowSize : 5;
int ftzero = std::max(params.preFilterCap, 15) | 1; 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, class StereoSGBMImpl : public StereoMatcher
OutputArray _disp )
{ {
Mat left = _left.getMat(), right = _right.getMat(); public:
CV_Assert( left.size() == right.size() && left.type() == right.type() && StereoSGBMImpl()
left.depth() == DataType<PixType>::depth ); {
params = StereoSGBMParams();
}
_disp.create( left.size(), CV_16S ); StereoSGBMImpl( int _minDisparity, int _numDisparities, int _SADWindowSize,
Mat disp = _disp.getMat(); 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 ); void compute( InputArray leftarr, InputArray rightarr, OutputArray disparr )
medianBlur(disp, disp, 3); {
Mat left = leftarr.getMat(), right = rightarr.getMat();
CV_Assert( left.size() == right.size() && left.type() == right.type() &&
left.depth() == CV_8U );
if( speckleWindowSize > 0 ) disparr.create( left.size(), CV_16S );
filterSpeckles(disp, (minDisparity - 1)*DISP_SCALE, speckleWindowSize, DISP_SCALE*speckleRange, buffer); 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, Rect getValidDisparityROI( Rect roi1, Rect roi2,
int minDisparity, int minDisparity,
int numberOfDisparities, int numberOfDisparities,
@ -855,108 +920,107 @@ Rect getValidDisparityROI( Rect roi1, Rect roi2,
return r.width > 0 && r.height > 0 ? r : Rect(); 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> using namespace cv;
void filterSpecklesImpl(cv::Mat& img, int newVal, int maxSpeckleSize, int maxDiff, cv::Mat& _buf)
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; for( j = 0; j < width; j++ )
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<T>(i); if( ds[j] != newVal ) // not a bad disparity
int* ls = labels + width*i;
for( j = 0; j < width; j++ )
{ {
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 count++;
ds[j] = (T)newVal; // 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 else
{ rtype[ls[j]] = 0; // large region label
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
}
} }
} }
} }
} }
} }
}
void cv::filterSpeckles( InputOutputArray _img, double _newval, int maxSpeckleSize, void cv::filterSpeckles( InputOutputArray _img, double _newval, int maxSpeckleSize,
double _maxDiff, InputOutputArray __buf ) 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 );
}