Skip to content

O
opencv
Commit 816adcfd authored by Andrey Kamaev's avatar Andrey Kamaev Committed by OpenCV Buildbot
Browse files
Options

Merge pull request #605 from vpisarev:c2cpp_calib3d_stereo

parents b9ab5939 df89f30b
Hide whitespace changes
Inline Side-by-side
Showing with 663 additions and 402 deletions
modules/calib3d/include/opencv2/calib3d/calib3d.hpp
View file @ 816adcfd
...@@ -669,18 +669,32 @@ CV_EXPORTS_W void triangulatePoints( InputArray projMatr1, InputArray projMatr2, ...@@ -669,18 +669,32 @@ 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 );
template<> CV_EXPORTS void Ptr<CvStereoBMState>::delete_obj();
/*! class CV_EXPORTS_W StereoMatcher : public Algorithm
Block Matching Stereo Correspondence Algorithm {
public:
CV_WRAP virtual void compute( InputArray left, InputArray right,
OutputArray disparity ) = 0;
};
The class implements BM stereo correspondence algorithm by K. Konolige. enum { STEREO_DISP_SCALE=16, STEREO_PREFILTER_NORMALIZED_RESPONSE = 0, STEREO_PREFILTER_XSOBEL = 1 };
*/
CV_EXPORTS Ptr<StereoMatcher> createStereoBM(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();
// to be moved to "compat" module
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=0, FISH_EYE_PRESET=1, NARROW_PRESET=2 };
//! the default constructor //! the default constructor
CV_WRAP StereoBM(); CV_WRAP StereoBM();
...@@ -697,11 +711,7 @@ public: ...@@ -697,11 +711,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 +746,7 @@ public: ...@@ -736,7 +746,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)
......
modules/calib3d/src/compat_stereo.cpp 0 → 100644
View file @ 816adcfd
//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(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(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);
}
}
modules/calib3d/src/stereobm.cpp
View file @ 816adcfd
...@@ -7,10 +7,11 @@ ...@@ -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 @@ ...@@ -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,57 +47,44 @@ ...@@ -46,57 +47,44 @@
#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 ) namespace cv
{ {
CvStereoBMState* state = (CvStereoBMState*)cvAlloc( sizeof(*state) );
if( !state )
return 0;
state->preFilterType = CV_STEREO_BM_XSOBEL; //CV_STEREO_BM_NORMALIZED_RESPONSE;
state->preFilterSize = 9;
state->preFilterCap = 31;
state->SADWindowSize = 15;
state->minDisparity = 0;
state->numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : 64;
state->textureThreshold = 10;
state->uniquenessRatio = 15;
state->speckleRange = state->speckleWindowSize = 0;
state->trySmallerWindows = 0;
state->roi1 = state->roi2 = cvRect(0,0,0,0);
state->disp12MaxDiff = -1;
state->preFilteredImg0 = state->preFilteredImg1 = state->slidingSumBuf =
state->disp = state->cost = 0;
return state;
}
CV_IMPL void cvReleaseStereoBMState( CvStereoBMState** state ) struct StereoBMParams
{ {
if( !state ) StereoBMParams(int _numDisparities=64, int _SADWindowSize=21)
CV_Error( CV_StsNullPtr, "" ); {
preFilterType = STEREO_PREFILTER_XSOBEL;
if( !*state ) preFilterSize = 9;
return; preFilterCap = 31;
SADWindowSize = _SADWindowSize;
cvReleaseMat( &(*state)->preFilteredImg0 ); minDisparity = 0;
cvReleaseMat( &(*state)->preFilteredImg1 ); numDisparities = _numDisparities > 0 ? _numDisparities : 64;
cvReleaseMat( &(*state)->slidingSumBuf ); textureThreshold = 10;
cvReleaseMat( &(*state)->disp ); uniquenessRatio = 15;
cvReleaseMat( &(*state)->cost ); speckleRange = speckleWindowSize = 0;
cvFree( state ); roi1 = roi2 = Rect(0,0,0,0);
} disp12MaxDiff = -1;
dispType = CV_16S;
}
namespace cv int preFilterType;
{ int preFilterSize;
int preFilterCap;
int SADWindowSize;
int minDisparity;
int numDisparities;
int textureThreshold;
int uniquenessRatio;
int speckleRange;
int speckleWindowSize;
Rect roi1, roi2;
int disp12MaxDiff;
int dispType;
};
static void prefilterNorm( const Mat& src, Mat& dst, int winsize, int ftzero, uchar* buf ) static void prefilterNorm( const Mat& src, Mat& dst, int winsize, int ftzero, uchar* buf )
{ {
...@@ -191,11 +179,11 @@ prefilterXSobel( const Mat& src, Mat& dst, int ftzero ) ...@@ -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 ) ...@@ -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; ...@@ -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, ...@@ -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, ...@@ -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, ...@@ -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, ...@@ -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, ...@@ -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 ...@@ -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 ...@@ -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 ...@@ -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 ...@@ -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()
{
params = StereoBMParams();
}
if (left0.type() != CV_8UC1 || right0.type() != CV_8UC1) StereoBMImpl( int _numDisparities, int _SADWindowSize )
CV_Error( CV_StsUnsupportedFormat, "Both input images must have CV_8UC1" ); {
params = StereoBMParams(_numDisparities, _SADWindowSize);
}
void compute( InputArray leftarr, InputArray rightarr, OutputArray disparr )
{
Mat left0 = leftarr.getMat(), right0 = rightarr.getMat();
int dtype = disparr.fixedType() ? disparr.type() : params.dispType;
if (disp0.type() != CV_16SC1 && disp0.type() != CV_32FC1) if (left0.size() != right0.size())
CV_Error( CV_StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format" ); CV_Error( CV_StsUnmatchedSizes, "All the images must have the same size" );
if( !state ) if (left0.type() != CV_8UC1 || right0.type() != CV_8UC1)
CV_Error( CV_StsNullPtr, "Stereo BM state is NULL." ); CV_Error( CV_StsUnsupportedFormat, "Both input images must have CV_8UC1" );
if( state->preFilterType != CV_STEREO_BM_NORMALIZED_RESPONSE && state->preFilterType != CV_STEREO_BM_XSOBEL ) if (dtype != CV_16SC1 && dtype != CV_32FC1)
CV_Error( CV_StsOutOfRange, "preFilterType must be = CV_STEREO_BM_NORMALIZED_RESPONSE" ); CV_Error( CV_StsUnsupportedFormat, "Disparity image must have CV_16SC1 or CV_32FC1 format" );
if( state->preFilterSize < 5 || state->preFilterSize > 255 || state->preFilterSize % 2 == 0 ) disparr.create(left0.size(), dtype);
CV_Error( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" ); Mat disp0 = disparr.getMat();
if( state->preFilterCap < 1 || state->preFilterCap > 63 ) if( params.preFilterType != STEREO_PREFILTER_NORMALIZED_RESPONSE &&
CV_Error( CV_StsOutOfRange, "preFilterCap must be within 1..63" ); params.preFilterType != STEREO_PREFILTER_XSOBEL )
CV_Error( CV_StsOutOfRange, "preFilterType must be = CV_STEREO_BM_NORMALIZED_RESPONSE" );
if( state->SADWindowSize < 5 || state->SADWindowSize > 255 || state->SADWindowSize % 2 == 0 || if( params.preFilterSize < 5 || params.preFilterSize > 255 || params.preFilterSize % 2 == 0 )
state->SADWindowSize >= std::min(left0.cols, left0.rows) ) CV_Error( CV_StsOutOfRange, "preFilterSize must be odd and be within 5..255" );
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 ) if( params.preFilterCap < 1 || params.preFilterCap > 63 )
CV_Error( CV_StsOutOfRange, "numberOfDisparities must be positive and divisble by 16" ); CV_Error( CV_StsOutOfRange, "preFilterCap must be within 1..63" );
if( state->textureThreshold < 0 ) if( params.SADWindowSize < 5 || params.SADWindowSize > 255 || params.SADWindowSize % 2 == 0 ||
CV_Error( CV_StsOutOfRange, "texture threshold must be non-negative" ); params.SADWindowSize >= std::min(left0.cols, left0.rows) )
CV_Error( CV_StsOutOfRange, "SADWindowSize must be odd, be within 5..255 and be not larger than image width or height" );
if( state->uniquenessRatio < 0 ) if( params.numDisparities <= 0 || params.numDisparities % 16 != 0 )
CV_Error( CV_StsOutOfRange, "uniqueness ratio must be non-negative" ); CV_Error( CV_StsOutOfRange, "numDisparities must be positive and divisble by 16" );
if( !state->preFilteredImg0 || state->preFilteredImg0->cols * state->preFilteredImg0->rows < left0.cols * left0.rows ) if( params.textureThreshold < 0 )
{ CV_Error( CV_StsOutOfRange, "texture threshold must be non-negative" );
cvReleaseMat( &state->preFilteredImg0 );
cvReleaseMat( &state->preFilteredImg1 );
cvReleaseMat( &state->cost );
state->preFilteredImg0 = cvCreateMat( left0.rows, left0.cols, CV_8U ); if( params.uniquenessRatio < 0 )
state->preFilteredImg1 = cvCreateMat( left0.rows, left0.cols, CV_8U ); CV_Error( CV_StsOutOfRange, "uniqueness ratio must be non-negative" );
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; preFilteredImg0.create( left0.size(), CV_8U );
int ndisp = state->numberOfDisparities; preFilteredImg1.create( left0.size(), CV_8U );
cost.create( left0.size(), CV_16S );
int width = left0.cols; Mat left = preFilteredImg0, right = preFilteredImg1;
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 ) int mindisp = params.minDisparity;
{ int ndisp = params.numDisparities;
disp0 = Scalar::all( FILTERED * ( disp0.type() < CV_32F ? 1 : 1./(1 << DISPARITY_SHIFT) ) );
return;
}
Mat disp = disp0; int width = left0.cols;
int height = left0.rows;
int lofs = std::max(ndisp - 1 + mindisp, 0);
int rofs = -std::min(ndisp - 1 + mindisp, 0);
int width1 = width - rofs - ndisp + 1;
int FILTERED = (params.minDisparity - 1) << DISPARITY_SHIFT;
if( disp0.type() == CV_32F) if( lofs >= width || rofs >= width || width1 < 1 )
{
if( !state->disp || state->disp->rows != disp0.rows || state->disp->cols != disp0.cols )
{ {
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);
#if CV_SSE2 int bufSize1 = (int)((width + params.preFilterSize + 2) * sizeof(int) + 256);
bool useShorts = state->preFilterCap <= 31 && state->SADWindowSize <= 21 && checkHardwareSupport(CV_CPU_SSE2); int bufSize2 = 0;
#else if( params.speckleRange >= 0 && params.speckleWindowSize > 0 )
const bool useShorts = false; bufSize2 = width*height*(sizeof(Point_<short>) + sizeof(int) + sizeof(uchar));
#endif
#ifdef HAVE_TBB #if CV_SSE2
const double SAD_overhead_coeff = 10.0; bool useShorts = params.preFilterCap <= 31 && params.SADWindowSize <= 21 && checkHardwareSupport(CV_CPU_SSE2);
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 #else
const int nstripes = 1; const bool useShorts = false;
#endif #endif
int bufSize = std::max(bufSize0 * nstripes, std::max(bufSize1 * 2, bufSize2)); const double SAD_overhead_coeff = 10.0;
double N0 = 8000000 / (useShorts ? 1 : 4); // approx tbb's min number instructions reasonable for one thread
if( !state->slidingSumBuf || state->slidingSumBuf->cols < bufSize ) double maxStripeSize = std::min(std::max(N0 / (width * ndisp), (wsz-1) * SAD_overhead_coeff), (double)height);
{ int nstripes = cvCeil(height / maxStripeSize);
cvReleaseMat( &state->slidingSumBuf ); int bufSize = std::max(bufSize0 * nstripes, std::max(bufSize1 * 2, bufSize2));
state->slidingSumBuf = cvCreateMat( 1, bufSize, CV_8U );
if( slidingSumBuf.cols < bufSize )
slidingSumBuf.create( 1, bufSize, CV_8U );
uchar *_buf = slidingSumBuf.data;
parallel_for_(Range(0, 2), PrefilterInvoker(left0, right0, left, right, _buf, _buf + bufSize1, &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; 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;
validDisparityRect = getValidDisparityROI(R1.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect,
R2.area() > 0 ? Rect(0, 0, width, height) : validDisparityRect,
state->minDisparity, state->numberOfDisparities,
state->SADWindowSize);
parallel_for(BlockedRange(0, nstripes),
FindStereoCorrespInvoker(left, right, disp, state, nstripes,
bufSize0, useShorts, validDisparityRect));
if( state->speckleRange >= 0 && state->speckleWindowSize > 0 ) StereoBMParams params;
{ Mat preFilteredImg0, preFilteredImg1, cost, dispbuf;
Mat buf(state->slidingSumBuf); Mat slidingSumBuf;
filterSpeckles(disp, FILTERED, state->speckleWindowSize, state->speckleRange, buf); };
}
if (disp0.data != disp.data)
disp.convertTo(disp0, disp0.type(), 1./(1 << DISPARITY_SHIFT), 0);
}
StereoBM::StereoBM()
{ state = cvCreateStereoBMState(); }
StereoBM::StereoBM(int _preset, int _ndisparities, int _SADWindowSize)
{ init(_preset, _ndisparities, _SADWindowSize); }
void StereoBM::init(int _preset, int _ndisparities, int _SADWindowSize)
{
state = cvCreateStereoBMState(_preset, _ndisparities);
state->SADWindowSize = _SADWindowSize;
}
void StereoBM::operator()( InputArray _left, InputArray _right,
OutputArray _disparity, int disptype )
{
Mat left = _left.getMat(), right = _right.getMat();
CV_Assert( disptype == CV_16S || disptype == CV_32F );
_disparity.create(left.size(), disptype);
Mat disparity = _disparity.getMat();
findStereoCorrespondenceBM(left, right, disparity, state);
}
template<> void Ptr<CvStereoBMState>::delete_obj() #define add_param(n) \
{ cvReleaseStereoBMState(&obj); } obj.info()->addParam(obj, #n, obj.params.n)
CV_INIT_ALGORITHM(StereoBMImpl, "StereoMatcher.BM",
add_param(preFilterType);
add_param(preFilterSize);
add_param(preFilterCap);
add_param(SADWindowSize);
add_param(minDisparity);
add_param(numDisparities);
add_param(textureThreshold);
add_param(uniquenessRatio);
add_param(speckleRange);
add_param(speckleWindowSize);
add_param(disp12MaxDiff);
add_param(dispType));
} }
CV_IMPL void cvFindStereoCorrespondenceBM( const CvArr* leftarr, const CvArr* rightarr, cv::Ptr<cv::StereoMatcher> cv::createStereoBM(int _numDisparities, int _SADWindowSize)
CvArr* disparr, CvStereoBMState* state )
{ {
cv::Mat left = cv::cvarrToMat(leftarr), return new StereoBMImpl(_numDisparities, _SADWindowSize);
right = cv::cvarrToMat(rightarr),
disp = cv::cvarrToMat(disparr);
cv::findStereoCorrespondenceBM(left, right, disp, state);
} }
/* End of file. */ /* End of file. */
modules/calib3d/src/stereosgbm.cpp
View file @ 816adcfd
...@@ -12,6 +12,7 @@ ...@@ -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; ...@@ -61,42 +62,52 @@ typedef short DispType;
enum { NR = 16, NR2 = NR/2 }; enum { NR = 16, NR2 = NR/2 };
StereoSGBM::StereoSGBM()
{
minDisparity = numberOfDisparities = 0;
SADWindowSize = 0;
P1 = P2 = 0;
disp12MaxDiff = 0;
preFilterCap = 0;
uniquenessRatio = 0;
speckleWindowSize = 0;
speckleRange = 0;
fullDP = false;
}
StereoSGBM::StereoSGBM( int _minDisparity, int _numDisparities, int _SADWindowSize, struct StereoSGBMParams
int _P1, int _P2, int _disp12MaxDiff, int _preFilterCap,
int _uniquenessRatio, int _speckleWindowSize, int _speckleRange,
bool _fullDP )
{ {
minDisparity = _minDisparity; StereoSGBMParams()
numberOfDisparities = _numDisparities; {
SADWindowSize = _SADWindowSize; minDisparity = numDisparities = 0;
P1 = _P1; SADWindowSize = 0;
P2 = _P2; P1 = P2 = 0;
disp12MaxDiff = _disp12MaxDiff; disp12MaxDiff = 0;
preFilterCap = _preFilterCap; preFilterCap = 0;
uniquenessRatio = _uniquenessRatio; uniquenessRatio = 0;
speckleWindowSize = _speckleWindowSize; speckleWindowSize = 0;
speckleRange = _speckleRange; speckleRange = 0;
fullDP = _fullDP; fullDP = false;
} }
StereoSGBMParams( int _minDisparity, int _numDisparities, int _SADWindowSize,
int _P1, int _P2, int _disp12MaxDiff, int _preFilterCap,
int _uniquenessRatio, int _speckleWindowSize, int _speckleRange,
bool _fullDP )
{
minDisparity = _minDisparity;
numDisparities = _numDisparities;
SADWindowSize = _SADWindowSize;
P1 = _P1;
P2 = _P2;
disp12MaxDiff = _disp12MaxDiff;
preFilterCap = _preFilterCap;
uniquenessRatio = _uniquenessRatio;
speckleWindowSize = _speckleWindowSize;
speckleRange = _speckleRange;
fullDP = _fullDP;
}
StereoSGBM::~StereoSGBM() int minDisparity;
{ int numDisparities;
} int SADWindowSize;
int preFilterCap;
int uniquenessRatio;
int P1;
int P2;
int speckleWindowSize;
int speckleRange;
int disp12MaxDiff;
bool fullDP;
};
/* /*
For each pixel row1[x], max(-maxD, 0) <= minX <= x < maxX <= width - max(0, -minD), 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, ...@@ -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, ...@@ -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, ...@@ -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, ...@@ -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 );
}
void compute( InputArray leftarr, InputArray rightarr, OutputArray disparr )
{
Mat left = leftarr.getMat(), right = rightarr.getMat();
CV_Assert( left.size() == right.size() && left.type() == right.type() &&
left.depth() == CV_8U );
computeDisparitySGBM( left, right, disp, *this, buffer ); disparr.create( left.size(), CV_16S );
medianBlur(disp, disp, 3); Mat disp = disparr.getMat();
if( speckleWindowSize > 0 ) computeDisparitySGBM( left, right, disp, params, buffer );
filterSpeckles(disp, (minDisparity - 1)*DISP_SCALE, speckleWindowSize, DISP_SCALE*speckleRange, 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, ...@@ -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;
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);
int* ls = labels + width*i;
for( j = 0; j < width; j++ ) for( j = 0; j < width; j++ )
{
if( ds[j] != newVal ) // not a bad disparity
{ {
if( ds[j] != newVal ) // not a bad disparity if( ls[j] ) // has a label, check for bad label
{ {
if( ls[j] ) // has a label, check for bad label if( rtype[ls[j]] ) // small region, zero out disparity
{ ds[j] = (T)newVal;
if( rtype[ls[j]] ) // small region, zero out disparity }
ds[j] = (T)newVal; // no label, assign and propagate
} else
// no label, assign and propagate {
else Point2s* ws = wbuf; // initialize wavefront
Point2s p((short)j, (short)i); // current pixel
curlabel++; // next label
int count = 0; // current region size
ls[j] = curlabel;
// wavefront propagation
while( ws >= wbuf ) // wavefront not empty
{ {
Point2s* ws = wbuf; // initialize wavefront count++;
Point2s p((short)j, (short)i); // current pixel // put neighbors onto wavefront
curlabel++; // next label T* dpp = &img.at<T>(p.y, p.x);
int count = 0; // current region size T dp = *dpp;
ls[j] = curlabel; int* lpp = labels + width*p.y + p.x;
// 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 ) if( p.x < width-1 && !lpp[+1] && dpp[+1] != newVal && std::abs(dp - dpp[+1]) <= maxDiff )
{ {
lpp[+width] = curlabel; lpp[+1] = curlabel;
*ws++ = Point2s(p.x, p.y+1); *ws++ = Point2s(p.x+1, p.y);
} }
if( p.y > 0 && !lpp[-width] && dpp[-dstep] != newVal && std::abs(dp - dpp[-dstep]) <= maxDiff ) if( p.x > 0 && !lpp[-1] && dpp[-1] != newVal && std::abs(dp - dpp[-1]) <= maxDiff )
{ {
lpp[-width] = curlabel; lpp[-1] = curlabel;
*ws++ = Point2s(p.x, p.y-1); *ws++ = Point2s(p.x-1, p.y);
} }
// pop most recent and propagate if( p.y < height-1 && !lpp[+width] && dpp[+dstep] != newVal && std::abs(dp - dpp[+dstep]) <= maxDiff )
// NB: could try least recent, maybe better convergence {
p = *--ws; lpp[+width] = curlabel;
*ws++ = Point2s(p.x, p.y+1);
} }
// assign label type if( p.y > 0 && !lpp[-width] && dpp[-dstep] != newVal && std::abs(dp - dpp[-dstep]) <= maxDiff )
if( count <= maxSpeckleSize ) // speckle region
{ {
rtype[ls[j]] = 1; // small region label lpp[-width] = curlabel;
ds[j] = (T)newVal; *ws++ = Point2s(p.x, p.y-1);
} }
else
rtype[ls[j]] = 0; // large region label // pop most recent and propagate
// NB: could try least recent, maybe better convergence
p = *--ws;
}
// assign label type
if( count <= maxSpeckleSize ) // speckle region
{
rtype[ls[j]] = 1; // small region label
ds[j] = (T)newVal;
} }
else
rtype[ls[j]] = 0; // large region label
} }
} }
} }
} }
} }
}
void cv::filterSpeckles( InputOutputArray _img, double _newval, int maxSpeckleSize, 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 ...@@ -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 );
}
samples/cpp/stereo_match.cpp
View file @ 816adcfd
...@@ -66,8 +66,8 @@ int main(int argc, char** argv) ...@@ -66,8 +66,8 @@ int main(int argc, char** argv)
bool no_display = false; bool no_display = false;
float scale = 1.f; float scale = 1.f;
StereoBM bm; Ptr<StereoMatcher> bm = createStereoBM(16,9);
StereoSGBM sgbm; Ptr<StereoMatcher> sgbm = createStereoSGBM(0,16,3);
StereoVar var; StereoVar var;
for( int i = 1; i < argc; i++ ) for( int i = 1; i < argc; i++ )
...@@ -220,32 +220,33 @@ int main(int argc, char** argv) ...@@ -220,32 +220,33 @@ int main(int argc, char** argv)
numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width/8) + 15) & -16; numberOfDisparities = numberOfDisparities > 0 ? numberOfDisparities : ((img_size.width/8) + 15) & -16;
bm.state->roi1 = roi1; //bm->set("roi1", roi1);
bm.state->roi2 = roi2; //bm->set("roi2", roi2);
bm.state->preFilterCap = 31; bm->set("preFilterCap", 31);
bm.state->SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 9; bm->set("SADWindowSize", SADWindowSize > 0 ? SADWindowSize : 9);
bm.state->minDisparity = 0; bm->set("minDisparity", 0);
bm.state->numberOfDisparities = numberOfDisparities; bm->set("numDisparities", numberOfDisparities);
bm.state->textureThreshold = 10; bm->set("textureThreshold", 10);
bm.state->uniquenessRatio = 15; bm->set("uniquenessRatio", 15);
bm.state->speckleWindowSize = 100; bm->set("speckleWindowSize", 100);
bm.state->speckleRange = 32; bm->set("speckleRange", 32);
bm.state->disp12MaxDiff = 1; bm->set("disp12MaxDiff", 1);
sgbm.preFilterCap = 63; sgbm->set("preFilterCap", 63);
sgbm.SADWindowSize = SADWindowSize > 0 ? SADWindowSize : 3; int sgbmWinSize = SADWindowSize > 0 ? SADWindowSize : 3;
sgbm->set("SADWindowSize", sgbmWinSize);
int cn = img1.channels(); int cn = img1.channels();
sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize; sgbm->set("P1", 8*cn*sgbmWinSize*sgbmWinSize);
sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize; sgbm->set("P2", 32*cn*sgbmWinSize*sgbmWinSize);
sgbm.minDisparity = 0; sgbm->set("minDisparity", 0);
sgbm.numberOfDisparities = numberOfDisparities; sgbm->set("numDisparities", numberOfDisparities);
sgbm.uniquenessRatio = 10; sgbm->set("uniquenessRatio", 10);
sgbm.speckleWindowSize = bm.state->speckleWindowSize; sgbm->set("speckleWindowSize", 100);
sgbm.speckleRange = bm.state->speckleRange; sgbm->set("speckleRange", 32);
sgbm.disp12MaxDiff = 1; sgbm->set("disp12MaxDiff", 1);
sgbm.fullDP = alg == STEREO_HH; sgbm->set("fullDP", alg == STEREO_HH);
var.levels = 3; // ignored with USE_AUTO_PARAMS var.levels = 3; // ignored with USE_AUTO_PARAMS
var.pyrScale = 0.5; // ignored with USE_AUTO_PARAMS var.pyrScale = 0.5; // ignored with USE_AUTO_PARAMS
...@@ -267,12 +268,12 @@ int main(int argc, char** argv) ...@@ -267,12 +268,12 @@ int main(int argc, char** argv)
int64 t = getTickCount(); int64 t = getTickCount();
if( alg == STEREO_BM ) if( alg == STEREO_BM )
bm(img1, img2, disp); bm->compute(img1, img2, disp);
else if( alg == STEREO_VAR ) { else if( alg == STEREO_VAR ) {
var(img1, img2, disp); var(img1, img2, disp);
} }
else if( alg == STEREO_SGBM || alg == STEREO_HH ) else if( alg == STEREO_SGBM || alg == STEREO_HH )
sgbm(img1, img2, disp); sgbm->compute(img1, img2, disp);
t = getTickCount() - t; t = getTickCount() - t;
printf("Time elapsed: %fms\n", t*1000/getTickFrequency()); printf("Time elapsed: %fms\n", t*1000/getTickFrequency());
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment