Skip to content

Instantly share code, notes, and snippets.

@alasin

alasin/WLSfilter snippet

Created October 4, 2016 09:32
Show Gist options
  • Save alasin/5b31779ce763ced3b1f55882828102ba to your computer and use it in GitHub Desktop.
Save alasin/5b31779ce763ced3b1f55882828102ba to your computer and use it in GitHub Desktop.
Download ZIP
Code for WLS filter for OpenCV version <3.1
Raw
WLSfilter snippet
#define EPS 1e-43f
typedef float WorkType;
typedef Vec<WorkType, 1> WorkVec;
typedef WorkType (*get_weight_op)(WorkType*, unsigned char*,unsigned char*);
inline WorkType get_weight_1channel(WorkType* LUT, unsigned char* p1,unsigned char* p2)
{
return LUT[ (p1[0]-p2[0])*(p1[0]-p2[0]) ];
}
inline WorkType get_weight_3channel(WorkType* LUT, unsigned char* p1,unsigned char* p2)
{
return LUT[ (p1[0]-p2[0])*(p1[0]-p2[0])+
(p1[1]-p2[1])*(p1[1]-p2[1])+
(p1[2]-p2[2])*(p1[2]-p2[2]) ];
}
class FastGlobalSmootherFilter
{
public:
/** @brief Apply smoothing operation to the source image.
@param src source image for filtering with unsigned 8-bit or signed 16-bit or floating-point 32-bit depth and up to 4 channels.
@param dst destination image.
*/
virtual void filter(InputArray src, OutputArray dst) = 0;
};
Ptr<FastGlobalSmootherFilter> createFastGlobalSmootherFilter(InputArray guide, double lambda, double sigma_color, double lambda_attenuation=0.25, int num_iter=3);
void fastGlobalSmootherFilter(InputArray guide, InputArray src, OutputArray dst, double lambda, double sigma_color, double lambda_attenuation=0.25, int num_iter=3);
class FastGlobalSmootherFilterImpl : public FastGlobalSmootherFilter
{
public:
static Ptr<FastGlobalSmootherFilterImpl> create(InputArray guide, double lambda, double sigma_color, int num_iter,double lambda_attenuation);
void filter(InputArray src, OutputArray dst);
protected:
int w,h;
int num_stripes;
float sigmaColor,lambda;
float lambda_attenuation;
int num_iter;
Mat weights_LUT;
Mat Chor, Cvert;
Mat interD;
void init(InputArray guide,double _lambda,double _sigmaColor,int _num_iter,double _lambda_attenuation);
void horizontalPass(Mat& cur);
void verticalPass(Mat& cur);
protected:
struct HorizontalPass_ParBody : public ParallelLoopBody
{
FastGlobalSmootherFilterImpl* fgs;
Mat* cur;
int nstripes, stripe_sz;
int h;
HorizontalPass_ParBody(FastGlobalSmootherFilterImpl &_fgs, Mat& _cur, int _nstripes, int _h);
void operator () (const Range& range) const;
};
inline void process_4row_block(Mat* cur,int i);
inline void process_row(Mat* cur,int i);
struct VerticalPass_ParBody : public ParallelLoopBody
{
FastGlobalSmootherFilterImpl* fgs;
Mat* cur;
int nstripes, stripe_sz;
int w;
VerticalPass_ParBody(FastGlobalSmootherFilterImpl &_fgs, Mat& _cur, int _nstripes, int _w);
void operator () (const Range& range) const;
};
template<get_weight_op get_weight, const int num_ch>
struct ComputeHorizontalWeights_ParBody : public ParallelLoopBody
{
FastGlobalSmootherFilterImpl* fgs;
Mat* guide;
int nstripes, stripe_sz;
int h;
ComputeHorizontalWeights_ParBody(FastGlobalSmootherFilterImpl &_fgs, Mat& _guide, int _nstripes, int _h);
void operator () (const Range& range) const;
};
template<get_weight_op get_weight, const int num_ch>
struct ComputeVerticalWeights_ParBody : public ParallelLoopBody
{
FastGlobalSmootherFilterImpl* fgs;
Mat* guide;
int nstripes, stripe_sz;
int w;
ComputeVerticalWeights_ParBody(FastGlobalSmootherFilterImpl &_fgs, Mat& _guide, int _nstripes, int _w);
void operator () (const Range& range) const;
};
struct ComputeLUT_ParBody : public ParallelLoopBody
{
FastGlobalSmootherFilterImpl* fgs;
WorkType* LUT;
int nstripes, stripe_sz;
int sz;
ComputeLUT_ParBody(FastGlobalSmootherFilterImpl &_fgs, WorkType* _LUT, int _nstripes, int _sz);
void operator () (const Range& range) const;
};
};
void FastGlobalSmootherFilterImpl::init(InputArray guide,double _lambda,double _sigmaColor,int _num_iter,double _lambda_attenuation)
{
CV_Assert( !guide.empty() && _lambda >= 0 && _sigmaColor >= 0 && _num_iter >=1 );
CV_Assert( guide.depth() == CV_8U && (guide.channels() == 1 || guide.channels() == 3) );
sigmaColor = (float)_sigmaColor;
lambda = (float)_lambda;
lambda_attenuation = (float)_lambda_attenuation;
num_iter = _num_iter;
num_stripes = getNumThreads();
int num_levels = 3*256*256;
weights_LUT.create(1,num_levels,WorkVec::type);
WorkType* LUT = (WorkType*)weights_LUT.ptr(0);
parallel_for_(Range(0,num_stripes),ComputeLUT_ParBody(*this,LUT,num_stripes,num_levels));
w = guide.cols();
h = guide.rows();
Chor. create(h,w,WorkVec::type);
Cvert. create(h,w,WorkVec::type);
interD.create(h,w,WorkVec::type);
Mat guideMat = guide.getMat();
if(guide.channels() == 1)
{
parallel_for_(Range(0,num_stripes),ComputeHorizontalWeights_ParBody<get_weight_1channel,1>(*this,guideMat,num_stripes,h));
parallel_for_(Range(0,num_stripes),ComputeVerticalWeights_ParBody <get_weight_1channel,1>(*this,guideMat,num_stripes,w));
}
if(guide.channels() == 3)
{
parallel_for_(Range(0,num_stripes),ComputeHorizontalWeights_ParBody<get_weight_3channel,3>(*this,guideMat,num_stripes,h));
parallel_for_(Range(0,num_stripes),ComputeVerticalWeights_ParBody <get_weight_3channel,3>(*this,guideMat,num_stripes,w));
}
}
Ptr<FastGlobalSmootherFilterImpl> FastGlobalSmootherFilterImpl::create(InputArray guide, double lambda, double sigma_color, int num_iter, double lambda_attenuation)
{
FastGlobalSmootherFilterImpl *fgs = new FastGlobalSmootherFilterImpl();
fgs->init(guide,lambda,sigma_color,num_iter,lambda_attenuation);
return Ptr<FastGlobalSmootherFilterImpl>(fgs);
}
void FastGlobalSmootherFilterImpl::filter(InputArray src, OutputArray dst)
{
CV_Assert(!src.empty() && (src.depth() == CV_8U || src.depth() == CV_16S || src.depth() == CV_32F) && src.channels()<=4);
if (src.rows() != h || src.cols() != w)
{
CV_Error(Error::StsBadSize, "Size of the filtered image must be equal to the size of the guide image");
return;
}
vector<Mat> src_channels;
vector<Mat> dst_channels;
if(src.channels()==1)
src_channels.push_back(src.getMat());
else
split(src,src_channels);
float lambda_ref = lambda;
for(int i=0;i<src.channels();i++)
{
lambda = lambda_ref;
Mat cur_res = src_channels[i].clone();
if(src.depth()!=WorkVec::type)
cur_res.convertTo(cur_res,WorkVec::type);
for(int n=0;n<num_iter;n++)
{
horizontalPass(cur_res);
verticalPass(cur_res);
lambda*=lambda_attenuation;
}
Mat dstMat;
if(src.depth()!=WorkVec::type)
cur_res.convertTo(dstMat,src.depth());
else
dstMat = cur_res;
dst_channels.push_back(dstMat);
}
lambda = lambda_ref;
dst.create(src.size(),src.type());
if(src.channels()==1)
{
Mat& dstMat = dst.getMatRef();
dstMat = dst_channels[0];
}
else
merge(dst_channels,dst);
}
void FastGlobalSmootherFilterImpl::horizontalPass(Mat& cur)
{
parallel_for_(Range(0,num_stripes),HorizontalPass_ParBody(*this,cur,num_stripes,h));
}
void FastGlobalSmootherFilterImpl::verticalPass(Mat& cur)
{
parallel_for_(Range(0,num_stripes),VerticalPass_ParBody(*this,cur,num_stripes,w));
}
FastGlobalSmootherFilterImpl::HorizontalPass_ParBody::HorizontalPass_ParBody(FastGlobalSmootherFilterImpl &_fgs, Mat& _cur, int _nstripes, int _h):
fgs(&_fgs),cur(&_cur), nstripes(_nstripes), h(_h)
{
stripe_sz = (int)ceil(h/(double)nstripes);
}
void FastGlobalSmootherFilterImpl::process_4row_block(Mat* cur,int i)
{
WorkType denom,denom_next,denom_next2,denom_next3;
WorkType *Chor_row = (WorkType*)Chor.ptr (i);
WorkType *interD_row = (WorkType*)interD.ptr(i);
WorkType *cur_row = (WorkType*)cur->ptr (i);
WorkType *Chor_row_next = (WorkType*)Chor.ptr (i+1);
WorkType *interD_row_next = (WorkType*)interD.ptr(i+1);
WorkType *cur_row_next = (WorkType*)cur->ptr (i+1);
WorkType *Chor_row_next2 = (WorkType*)Chor.ptr (i+2);
WorkType *interD_row_next2 = (WorkType*)interD.ptr(i+2);
WorkType *cur_row_next2 = (WorkType*)cur->ptr (i+2);
WorkType *Chor_row_next3 = (WorkType*)Chor.ptr (i+3);
WorkType *interD_row_next3 = (WorkType*)interD.ptr(i+3);
WorkType *cur_row_next3 = (WorkType*)cur->ptr (i+3);
float coef_cur, coef_prev;
float coef_cur_row_next, coef_prev_row_next;
float coef_cur_row_next2,coef_prev_row_next2;
float coef_cur_row_next3,coef_prev_row_next3;
//forward pass:
coef_prev = lambda*Chor_row[0];
coef_prev_row_next = lambda*Chor_row_next[0];
coef_prev_row_next2 = lambda*Chor_row_next2[0];
coef_prev_row_next3 = lambda*Chor_row_next3[0];
interD_row[0] = coef_prev /(1-coef_prev);
interD_row_next[0] = coef_prev_row_next /(1-coef_prev_row_next);
interD_row_next2[0] = coef_prev_row_next2/(1-coef_prev_row_next2);
interD_row_next3[0] = coef_prev_row_next3/(1-coef_prev_row_next3);
cur_row[0] = cur_row[0] /(1-coef_prev);
cur_row_next[0] = cur_row_next[0] /(1-coef_prev_row_next);
cur_row_next2[0] = cur_row_next2[0]/(1-coef_prev_row_next2);
cur_row_next3[0] = cur_row_next3[0]/(1-coef_prev_row_next3);
int j=1;
#if CV_SIMD128
{
v_float32x4 coef_prev_reg(coef_prev,coef_prev_row_next,coef_prev_row_next2,coef_prev_row_next3);
v_float32x4 interD_prev_reg(interD_row[0],interD_row_next[0],interD_row_next2[0],interD_row_next3[0]);
v_float32x4 cur_prev_reg(cur_row[0],cur_row_next[0],cur_row_next2[0],cur_row_next3[0]);
v_float32x4 lambda_reg(lambda,lambda,lambda,lambda);
v_float32x4 one_reg(1.0f,1.0f,1.0f,1.0f);
v_float32x4 a0,a1,a2,a3;
v_float32x4 b0,b1,b2,b3;
v_float32x4 aux0,aux1,aux2,aux3;
#define PROC4(Chor_in,cur_in,coef_prev_in,interD_prev_in,cur_prev_in,interD_out,cur_out,coef_cur_out)\
coef_cur_out = lambda_reg*Chor_in;\
aux0 = interD_prev_in*coef_prev_in;\
aux1 = coef_cur_out+coef_prev_in;\
aux1 = one_reg-aux1;\
aux0 = aux1-aux0;\
interD_out = coef_cur_out/aux0;\
aux1 = cur_prev_in*coef_prev_in;\
aux1 = cur_in - aux1;\
cur_out = aux1/aux0;
for(;j<w-3;j+=4)
{
// processing a 4x4 block:
aux0 = v_load(Chor_row+j);
aux1 = v_load(Chor_row_next+j);
aux2 = v_load(Chor_row_next2+j);
aux3 = v_load(Chor_row_next3+j);
v_transpose4x4(aux0,aux1,aux2,aux3,a0,a1,a2,a3);
aux0 = v_load(cur_row+j);
aux1 = v_load(cur_row_next+j);
aux2 = v_load(cur_row_next2+j);
aux3 = v_load(cur_row_next3+j);
v_transpose4x4(aux0,aux1,aux2,aux3,b0,b1,b2,b3);
PROC4(a0,b0,coef_prev_reg,interD_prev_reg,cur_prev_reg,a0,b0,aux2);
PROC4(a1,b1,aux2,a0,b0,a1,b1,aux3);
PROC4(a2,b2,aux3,a1,b1,a2,b2,aux2);
PROC4(a3,b3,aux2,a2,b2,a3,b3,aux3);
interD_prev_reg = a3;
cur_prev_reg = b3;
coef_prev_reg = aux3;
v_transpose4x4(a0,a1,a2,a3,aux0,aux1,aux2,aux3);
v_store(interD_row+j,aux0);
v_store(interD_row_next+j,aux1);
v_store(interD_row_next2+j,aux2);
v_store(interD_row_next3+j,aux3);
v_transpose4x4(b0,b1,b2,b3,aux0,aux1,aux2,aux3);
v_store(cur_row+j,aux0);
v_store(cur_row_next+j,aux1);
v_store(cur_row_next2+j,aux2);
v_store(cur_row_next3+j,aux3);
}
#undef PROC4
}
#endif
for(;j<w;j++)
{
coef_prev = lambda*Chor_row[j-1];
coef_prev_row_next = lambda*Chor_row_next[j-1];
coef_prev_row_next2 = lambda*Chor_row_next2[j-1];
coef_prev_row_next3 = lambda*Chor_row_next3[j-1];
coef_cur = lambda*Chor_row[j];
coef_cur_row_next = lambda*Chor_row_next[j];
coef_cur_row_next2 = lambda*Chor_row_next2[j];
coef_cur_row_next3 = lambda*Chor_row_next3[j];
denom = (1-coef_prev -coef_cur) -interD_row[j-1] *coef_prev;
denom_next = (1-coef_prev_row_next -coef_cur_row_next) -interD_row_next[j-1] *coef_prev_row_next;
denom_next2 = (1-coef_prev_row_next2-coef_cur_row_next2)-interD_row_next2[j-1]*coef_prev_row_next2;
denom_next3 = (1-coef_prev_row_next3-coef_cur_row_next3)-interD_row_next3[j-1]*coef_prev_row_next3;
interD_row[j] = coef_cur /denom;
interD_row_next[j] = coef_cur_row_next /denom_next;
interD_row_next2[j] = coef_cur_row_next2/denom_next2;
interD_row_next3[j] = coef_cur_row_next3/denom_next3;
cur_row[j] = (cur_row[j] -cur_row[j-1] *coef_prev) /denom;
cur_row_next[j] = (cur_row_next[j] -cur_row_next[j-1] *coef_prev_row_next) /denom_next;
cur_row_next2[j] = (cur_row_next2[j]-cur_row_next2[j-1]*coef_prev_row_next2)/denom_next2;
cur_row_next3[j] = (cur_row_next3[j]-cur_row_next3[j-1]*coef_prev_row_next3)/denom_next3;
}
//backward pass:
j = w-2;
#if CV_SIMD128
{
v_float32x4 cur_next_reg(cur_row[w-1],cur_row_next[w-1],cur_row_next2[w-1],cur_row_next3[w-1]);
v_float32x4 a0,a1,a2,a3;
v_float32x4 b0,b1,b2,b3;
v_float32x4 aux0,aux1,aux2,aux3;
for(j-=3;j>=0;j-=4)
{
//process 4x4 block:
aux0 = v_load(interD_row+j);
aux1 = v_load(interD_row_next+j);
aux2 = v_load(interD_row_next2+j);
aux3 = v_load(interD_row_next3+j);
v_transpose4x4(aux0,aux1,aux2,aux3,a0,a1,a2,a3);
aux0 = v_load(cur_row+j);
aux1 = v_load(cur_row_next+j);
aux2 = v_load(cur_row_next2+j);
aux3 = v_load(cur_row_next3+j);
v_transpose4x4(aux0,aux1,aux2,aux3,b0,b1,b2,b3);
aux0 = a3*cur_next_reg;
b3 = b3-aux0;
aux0 = a2*b3;
b2 = b2-aux0;
aux0 = a1*b2;
b1 = b1-aux0;
aux0 = a0*b1;
b0 = b0-aux0;
cur_next_reg = b0;
v_transpose4x4(b0,b1,b2,b3,aux0,aux1,aux2,aux3);
v_store(cur_row+j,aux0);
v_store(cur_row_next+j,aux1);
v_store(cur_row_next2+j,aux2);
v_store(cur_row_next3+j,aux3);
}
j+=3;
}
#endif
for(;j>=0;j--)
{
cur_row[j] = cur_row[j] -interD_row[j] *cur_row[j+1];
cur_row_next[j] = cur_row_next[j] -interD_row_next[j] *cur_row_next[j+1];
cur_row_next2[j] = cur_row_next2[j]-interD_row_next2[j]*cur_row_next2[j+1];
cur_row_next3[j] = cur_row_next3[j]-interD_row_next3[j]*cur_row_next3[j+1];
}
}
void FastGlobalSmootherFilterImpl::process_row(Mat* cur,int i)
{
WorkType denom;
WorkType *Chor_row = (WorkType*)Chor.ptr(i);
WorkType *interD_row = (WorkType*)interD.ptr(i);
WorkType *cur_row = (WorkType*)cur->ptr(i);
float coef_cur,coef_prev;
//forward pass:
coef_prev = lambda*Chor_row[0];
interD_row[0] = coef_prev/(1-coef_prev);
cur_row[0] = cur_row[0]/(1-coef_prev);
for(int j=1;j<w;j++)
{
coef_cur = lambda*Chor_row[j];
denom = (1-coef_prev-coef_cur)-interD_row[j-1]*coef_prev;
interD_row[j] = coef_cur/denom;
cur_row[j] = (cur_row[j]-cur_row[j-1]*coef_prev)/denom;
coef_prev = coef_cur;
}
//backward pass:
for(int j=w-2;j>=0;j--)
cur_row[j] = cur_row[j]-interD_row[j]*cur_row[j+1];
}
void FastGlobalSmootherFilterImpl::HorizontalPass_ParBody::operator()(const Range& range) const
{
int start = std::min(range.start * stripe_sz, h);
int end = std::min(range.end * stripe_sz, h);
int i=start;
for(;i<end-3;i+=4)
fgs->process_4row_block(cur,i);
for(;i<end;i++)
fgs->process_row(cur,i);
}
FastGlobalSmootherFilterImpl::VerticalPass_ParBody::VerticalPass_ParBody(FastGlobalSmootherFilterImpl &_fgs, Mat& _cur, int _nstripes, int _w):
fgs(&_fgs),cur(&_cur), nstripes(_nstripes), w(_w)
{
stripe_sz = (int)ceil(w/(double)nstripes);
}
void FastGlobalSmootherFilterImpl::VerticalPass_ParBody::operator()(const Range& range) const
{
int start = std::min(range.start * stripe_sz, w);
int end = std::min(range.end * stripe_sz, w);
//float lambda = fgs->lambda;
WorkType denom;
WorkType *Cvert_row, *Cvert_row_prev;
WorkType *interD_row, *interD_row_prev, *cur_row, *cur_row_prev, *cur_row_next;
float coef_cur,coef_prev;
Cvert_row = (WorkType*)fgs->Cvert.ptr(0);
interD_row = (WorkType*)fgs->interD.ptr(0);
cur_row = (WorkType*)cur->ptr(0);
//forward pass:
for(int j=start;j<end;j++)
{
coef_cur = fgs->lambda*Cvert_row[j];
interD_row[j] = coef_cur/(1-coef_cur);
cur_row[j] = cur_row[j]/(1-coef_cur);
}
for(int i=1;i<fgs->h;i++)
{
Cvert_row = (WorkType*)fgs->Cvert.ptr(i);
Cvert_row_prev = (WorkType*)fgs->Cvert.ptr(i-1);
interD_row = (WorkType*)fgs->interD.ptr(i);
interD_row_prev = (WorkType*)fgs->interD.ptr(i-1);
cur_row = (WorkType*)cur->ptr(i);
cur_row_prev = (WorkType*)cur->ptr(i-1);
int j = start;
#if CV_SIMD128
v_float32x4 a,b,c,d,coef_cur_reg,coef_prev_reg;
v_float32x4 one_reg(1.0f,1.0f,1.0f,1.0f);
v_float32x4 lambda_reg(fgs->lambda,fgs->lambda,fgs->lambda,fgs->lambda);
int sz4 = 4*((end-start)/4);
int end4 = start+sz4;
for(;j<end4;j+=4)
{
a = v_load(Cvert_row_prev+j);
b = v_load(Cvert_row+j);
coef_prev_reg = lambda_reg*a;
coef_cur_reg = lambda_reg*b;
a = v_load(interD_row_prev+j);
a = a*coef_prev_reg;
b = coef_prev_reg+coef_cur_reg;
b = b+a;
a = one_reg-b; //computed denom
b = coef_cur_reg/a; //computed interD_row
c = v_load(cur_row_prev+j);
c = c*coef_prev_reg;
d = v_load(cur_row+j);
d = d-c;
d = d/a; //computed cur_row
v_store(interD_row+j,b);
v_store(cur_row+j,d);
}
#endif
for(;j<end;j++)
{
coef_prev = fgs->lambda*Cvert_row_prev[j];
coef_cur = fgs->lambda*Cvert_row[j];
denom = (1-coef_prev-coef_cur)-interD_row_prev[j]*coef_prev;
interD_row[j] = coef_cur/denom;
cur_row[j] = (cur_row[j]-cur_row_prev[j]*coef_prev)/denom;
}
}
//backward pass:
for(int i=fgs->h-2;i>=0;i--)
{
interD_row = (WorkType*)fgs->interD.ptr(i);
cur_row = (WorkType*)cur->ptr(i);
cur_row_next = (WorkType*)cur->ptr(i+1);
int j = start;
#if CV_SIMD128
v_float32x4 a,b;
int sz4 = 4*((end-start)/4);
int end4 = start+sz4;
for(;j<end4;j+=4)
{
a = v_load(interD_row+j);
b = v_load(cur_row_next+j);
b = a*b;
a = v_load(cur_row+j);
b = a-b;
v_store(cur_row+j,b);
}
#endif
for(;j<end;j++)
cur_row[j] = cur_row[j]-interD_row[j]*cur_row_next[j];
}
}
template<get_weight_op get_weight, const int num_ch>
FastGlobalSmootherFilterImpl::ComputeHorizontalWeights_ParBody<get_weight,num_ch>::ComputeHorizontalWeights_ParBody(FastGlobalSmootherFilterImpl &_fgs, Mat& _guide, int _nstripes, int _h):
fgs(&_fgs),guide(&_guide), nstripes(_nstripes), h(_h)
{
stripe_sz = (int)ceil(h/(double)nstripes);
}
template<get_weight_op get_weight, const int num_ch>
void FastGlobalSmootherFilterImpl::ComputeHorizontalWeights_ParBody<get_weight,num_ch>::operator()(const Range& range) const
{
int start = std::min(range.start * stripe_sz, h);
int end = std::min(range.end * stripe_sz, h);
WorkType* LUT = (WorkType*)fgs->weights_LUT.ptr(0);
unsigned char *row;
WorkType *Chor_row;
for(int i=start;i<end;i++)
{
row = guide->ptr(i);
Chor_row = (WorkType*)fgs->Chor.ptr(i);
Chor_row[0] = get_weight(LUT,row,row+num_ch);
row+=num_ch;
for(int j=1;j<fgs->w-1;j++)
{
Chor_row[j] = get_weight(LUT,row,row+num_ch);
row+=num_ch;
}
Chor_row[fgs->w-1]=0;
}
}
template<get_weight_op get_weight, const int num_ch>
FastGlobalSmootherFilterImpl::ComputeVerticalWeights_ParBody<get_weight,num_ch>::ComputeVerticalWeights_ParBody(FastGlobalSmootherFilterImpl &_fgs, Mat& _guide, int _nstripes, int _w):
fgs(&_fgs),guide(&_guide), nstripes(_nstripes), w(_w)
{
stripe_sz = (int)ceil(w/(double)nstripes);
}
template<get_weight_op get_weight, const int num_ch>
void FastGlobalSmootherFilterImpl::ComputeVerticalWeights_ParBody<get_weight,num_ch>::operator()(const Range& range) const
{
int start = std::min(range.start * stripe_sz, w);
int end = std::min(range.end * stripe_sz, w);
WorkType* LUT = (WorkType*)fgs->weights_LUT.ptr(0);
unsigned char *row,*row_next;
WorkType *Cvert_row;
Cvert_row = (WorkType*)fgs->Cvert.ptr(0);
row = guide->ptr(0)+start*num_ch;
row_next = guide->ptr(1)+start*num_ch;
for(int j=start;j<end;j++)
{
Cvert_row[j] = get_weight(LUT,row,row_next);
row+=num_ch;
row_next+=num_ch;
}
for(int i=1;i<fgs->h-1;i++)
{
row = guide->ptr(i)+start*num_ch;
row_next = guide->ptr(i+1)+start*num_ch;
Cvert_row = (WorkType*)fgs->Cvert.ptr(i);
for(int j=start;j<end;j++)
{
Cvert_row[j] = get_weight(LUT,row,row_next);
row+=num_ch;
row_next+=num_ch;
}
}
Cvert_row = (WorkType*)fgs->Cvert.ptr(fgs->h-1);
for(int j=start;j<end;j++)
Cvert_row[j] = 0;
}
FastGlobalSmootherFilterImpl::ComputeLUT_ParBody::ComputeLUT_ParBody(FastGlobalSmootherFilterImpl &_fgs, WorkType *_LUT, int _nstripes, int _sz):
fgs(&_fgs), LUT(_LUT), nstripes(_nstripes), sz(_sz)
{
stripe_sz = (int)ceil(sz/(double)nstripes);
}
void FastGlobalSmootherFilterImpl::ComputeLUT_ParBody::operator()(const Range& range) const
{
int start = std::min(range.start * stripe_sz, sz);
int end = std::min(range.end * stripe_sz, sz);
for(int i=start;i<end;i++)
LUT[i] = (WorkType)(-cv::exp(-sqrt((float)i)/fgs->sigmaColor));
}
////////////////////////////////////////////////////////////////////////////////////////////////
Ptr<FastGlobalSmootherFilter> createFastGlobalSmootherFilter(InputArray guide, double lambda, double sigma_color, double lambda_attenuation, int num_iter)
{
return Ptr<FastGlobalSmootherFilter>(FastGlobalSmootherFilterImpl::create(guide, lambda, sigma_color, num_iter, lambda_attenuation));
}
void fastGlobalSmootherFilter(InputArray guide, InputArray src, OutputArray dst, double lambda, double sigma_color, double lambda_attenuation, int num_iter)
{
Ptr<FastGlobalSmootherFilter> fgs = createFastGlobalSmootherFilter(guide, lambda, sigma_color, lambda_attenuation, num_iter);
fgs->filter(src, dst);
}
class DisparityWLSFilter
{
protected:
int left_offset, right_offset, top_offset, bottom_offset;
Rect valid_disp_ROI;
Rect right_view_valid_disp_ROI;
int min_disp;
bool use_confidence;
Mat confidence_map;
double lambda,sigma_color;
int LRC_thresh,depth_discontinuity_radius;
float depth_discontinuity_roll_off_factor;
float resize_factor;
int num_stripes;
void init(double _lambda, double _sigma_color, bool _use_confidence, int l_offs, int r_offs, int t_offs, int b_offs, int _min_disp);
void computeDepthDiscontinuityMaps(Mat& left_disp, Mat& right_disp, Mat& left_dst, Mat& right_dst);
void computeConfidenceMap(InputArray left_disp, InputArray right_disp);
protected:
struct ComputeDiscontinuityAwareLRC_ParBody : public ParallelLoopBody
{
DisparityWLSFilter* wls;
Mat *left_disp, *right_disp;
Mat *left_disc, *right_disc, *dst;
Rect left_ROI, right_ROI;
int nstripes, stripe_sz;
ComputeDiscontinuityAwareLRC_ParBody(DisparityWLSFilter& _wls, Mat& _left_disp, Mat& _right_disp, Mat& _left_disc, Mat& _right_disc, Mat& _dst, Rect _left_ROI, Rect _right_ROI, int _nstripes);
void operator () (const Range& range) const;
};
struct ComputeDepthDisc_ParBody : public ParallelLoopBody
{
DisparityWLSFilter* wls;
Mat *disp,*disp_squares,*dst;
int nstripes, stripe_sz;
ComputeDepthDisc_ParBody(DisparityWLSFilter& _wls, Mat& _disp, Mat& _disp_squares, Mat& _dst, int _nstripes);
void operator () (const Range& range) const;
};
typedef void (DisparityWLSFilter::*MatOp)(Mat& src, Mat& dst);
struct ParallelMatOp_ParBody : public ParallelLoopBody
{
DisparityWLSFilter* wls;
vector<MatOp> ops;
vector<Mat*> src;
vector<Mat*> dst;
ParallelMatOp_ParBody(DisparityWLSFilter& _wls, vector<MatOp> _ops, vector<Mat*>& _src, vector<Mat*>& _dst);
void operator () (const Range& range) const;
};
void boxFilterOp(Mat& src,Mat& dst)
{
int rad = depth_discontinuity_radius;
boxFilter(src,dst,CV_32F,Size(2*rad+1,2*rad+1),Point(-1,-1));
}
void sqrBoxFilterOp(Mat& src,Mat& dst)
{
int rad = depth_discontinuity_radius;
sqrBoxFilter(src,dst,CV_32F,Size(2*rad+1,2*rad+1),Point(-1,-1));
}
void copyToOp(Mat& src,Mat& dst)
{
src.copyTo(dst);
}
public:
static Ptr<DisparityWLSFilter> create(bool _use_confidence, int l_offs, int r_offs, int t_offs, int b_offs, int min_disp);
void filter(InputArray disparity_map_left, InputArray left_view, OutputArray filtered_disparity_map, InputArray disparity_map_right, Rect ROI, InputArray);
double getLambda() {return lambda;}
void setLambda(double _lambda) {lambda = _lambda;}
double getSigmaColor() {return sigma_color;}
void setSigmaColor(double _sigma_color) {sigma_color = _sigma_color;}
int getLRCthresh() {return LRC_thresh;}
void setLRCthresh(int _LRC_thresh) {LRC_thresh = _LRC_thresh;}
int getDepthDiscontinuityRadius() {return depth_discontinuity_radius;}
void setDepthDiscontinuityRadius(int _disc_radius) {depth_discontinuity_radius = _disc_radius;}
Mat getConfidenceMap() {return confidence_map;}
Rect getROI() {return valid_disp_ROI;}
};
void DisparityWLSFilter::init(double _lambda, double _sigma_color, bool _use_confidence, int l_offs, int r_offs, int t_offs, int b_offs, int _min_disp)
{
left_offset = l_offs; right_offset = r_offs;
top_offset = t_offs; bottom_offset = b_offs;
min_disp = _min_disp;
valid_disp_ROI = Rect();
right_view_valid_disp_ROI = Rect();
min_disp=0;
lambda = _lambda;
sigma_color = _sigma_color;
use_confidence = _use_confidence;
confidence_map = Mat();
LRC_thresh = 24;
depth_discontinuity_radius = 5;
depth_discontinuity_roll_off_factor = 0.001f;
resize_factor = 1.0;
num_stripes = getNumThreads();
}
void DisparityWLSFilter::computeDepthDiscontinuityMaps(Mat& left_disp, Mat& right_disp, Mat& left_dst, Mat& right_dst)
{
Mat left_disp_ROI (left_disp, valid_disp_ROI);
Mat right_disp_ROI(right_disp,right_view_valid_disp_ROI);
Mat ldisp,rdisp,ldisp_squared,rdisp_squared;
{
vector<Mat*> _src; _src.push_back(&left_disp_ROI);_src.push_back(&right_disp_ROI);
_src.push_back(&left_disp_ROI);_src.push_back(&right_disp_ROI);
vector<Mat*> _dst; _dst.push_back(&ldisp); _dst.push_back(&rdisp);
_dst.push_back(&ldisp_squared);_dst.push_back(&rdisp_squared);
vector<MatOp> _ops; _ops.push_back(&DisparityWLSFilter::copyToOp); _ops.push_back(&DisparityWLSFilter::copyToOp);
_ops.push_back(&DisparityWLSFilter::copyToOp); _ops.push_back(&DisparityWLSFilter::copyToOp);
parallel_for_(Range(0,4),ParallelMatOp_ParBody(*this,_ops,_src,_dst));
}
{
vector<Mat*> _src; _src.push_back(&ldisp); _src.push_back(&rdisp);
_src.push_back(&ldisp_squared);_src.push_back(&rdisp_squared);
vector<Mat*> _dst; _dst.push_back(&ldisp); _dst.push_back(&rdisp);
_dst.push_back(&ldisp_squared);_dst.push_back(&rdisp_squared);
vector<MatOp> _ops; _ops.push_back(&DisparityWLSFilter::boxFilterOp); _ops.push_back(&DisparityWLSFilter::boxFilterOp);
_ops.push_back(&DisparityWLSFilter::sqrBoxFilterOp);_ops.push_back(&DisparityWLSFilter::sqrBoxFilterOp);
parallel_for_(Range(0,4),ParallelMatOp_ParBody(*this,_ops,_src,_dst));
}
left_dst = Mat::zeros(left_disp.rows,left_disp.cols,CV_32F);
right_dst = Mat::zeros(right_disp.rows,right_disp.cols,CV_32F);
Mat left_dst_ROI (left_dst,valid_disp_ROI);
Mat right_dst_ROI(right_dst,right_view_valid_disp_ROI);
parallel_for_(Range(0,num_stripes),ComputeDepthDisc_ParBody(*this,ldisp,ldisp_squared,left_dst_ROI ,num_stripes));
parallel_for_(Range(0,num_stripes),ComputeDepthDisc_ParBody(*this,rdisp,rdisp_squared,right_dst_ROI,num_stripes));
}
void DisparityWLSFilter::computeConfidenceMap(InputArray left_disp, InputArray right_disp)
{
Mat ldisp = left_disp.getMat();
Mat rdisp = right_disp.getMat();
Mat depth_discontinuity_map_left,depth_discontinuity_map_right;
right_view_valid_disp_ROI = Rect(ldisp.cols-(valid_disp_ROI.x+valid_disp_ROI.width),valid_disp_ROI.y,
valid_disp_ROI.width,valid_disp_ROI.height);
computeDepthDiscontinuityMaps(ldisp,rdisp,depth_discontinuity_map_left,depth_discontinuity_map_right);
confidence_map = depth_discontinuity_map_left;
parallel_for_(Range(0,num_stripes),ComputeDiscontinuityAwareLRC_ParBody(*this,ldisp,rdisp, depth_discontinuity_map_left,depth_discontinuity_map_right,confidence_map,valid_disp_ROI,right_view_valid_disp_ROI,num_stripes));
confidence_map = 255.0f*confidence_map;
}
Ptr<DisparityWLSFilter> DisparityWLSFilter::create(bool _use_confidence, int l_offs=0, int r_offs=0, int t_offs=0, int b_offs=0, int min_disp=0)
{
DisparityWLSFilter *wls = new DisparityWLSFilter();
wls->init(8000.0,1.0,_use_confidence,l_offs, r_offs, t_offs, b_offs, min_disp);
return Ptr<DisparityWLSFilter>(wls);
}
void DisparityWLSFilter::filter(InputArray disparity_map_left, InputArray left_view, OutputArray filtered_disparity_map, InputArray disparity_map_right, Rect ROI, InputArray)
{
CV_Assert( !disparity_map_left.empty() && (disparity_map_left.depth() == CV_16S) && (disparity_map_left.channels() == 1) );
CV_Assert( !left_view.empty() && (left_view.depth() == CV_8U) && (left_view.channels() == 3 || left_view.channels() == 1) );
Mat disp,src,dst;
if(disparity_map_left.size()!=left_view.size())
resize_factor = disparity_map_left.cols()/(float)left_view.cols();
else
resize_factor = 1.0;
if(ROI.area()!=0) /* user provided a ROI */
valid_disp_ROI = ROI;
else
valid_disp_ROI = Rect(left_offset,top_offset,
disparity_map_left.cols()-left_offset-right_offset,
disparity_map_left.rows()-top_offset-bottom_offset);
if(!use_confidence)
{
Mat disp_full_size = disparity_map_left.getMat();
Mat src_full_size = left_view.getMat();
if(disp_full_size.size!=src_full_size.size)
{
float x_ratio = src_full_size.cols/(float)disp_full_size.cols;
float y_ratio = src_full_size.rows/(float)disp_full_size.rows;
resize(disp_full_size,disp_full_size,src_full_size.size());
disp_full_size = disp_full_size*x_ratio;
ROI = Rect((int)(valid_disp_ROI.x*x_ratio), (int)(valid_disp_ROI.y*y_ratio),
(int)(valid_disp_ROI.width*x_ratio),(int)(valid_disp_ROI.height*y_ratio));
}
else
ROI = valid_disp_ROI;
disp = Mat(disp_full_size,ROI);
src = Mat(src_full_size ,ROI);
filtered_disparity_map.create(disp_full_size.size(), disp_full_size.type());
Mat& dst_full_size = filtered_disparity_map.getMatRef();
dst_full_size = Scalar(16*(min_disp-1));
dst = Mat(dst_full_size,ROI);
Mat filtered_disp;
fastGlobalSmootherFilter(src,disp,filtered_disp,lambda,sigma_color);
filtered_disp.copyTo(dst);
}
else
{
CV_Assert( !disparity_map_right.empty() && (disparity_map_right.depth() == CV_16S) && (disparity_map_right.channels() == 1) );
CV_Assert( (disparity_map_left.cols() == disparity_map_right.cols()) );
CV_Assert( (disparity_map_left.rows() == disparity_map_right.rows()) );
computeConfidenceMap(disparity_map_left,disparity_map_right);
Mat disp_full_size = disparity_map_left.getMat();
Mat src_full_size = left_view.getMat();
if(disp_full_size.size!=src_full_size.size)
{
float x_ratio = src_full_size.cols/(float)disp_full_size.cols;
float y_ratio = src_full_size.rows/(float)disp_full_size.rows;
resize(disp_full_size,disp_full_size,src_full_size.size());
disp_full_size = disp_full_size*x_ratio;
resize(confidence_map,confidence_map,src_full_size.size());
ROI = Rect((int)(valid_disp_ROI.x*x_ratio), (int)(valid_disp_ROI.y*y_ratio),
(int)(valid_disp_ROI.width*x_ratio),(int)(valid_disp_ROI.height*y_ratio));
}
else
ROI = valid_disp_ROI;
disp = Mat(disp_full_size,ROI);
src = Mat(src_full_size ,ROI);
filtered_disparity_map.create(disp_full_size.size(), disp_full_size.type());
Mat& dst_full_size = filtered_disparity_map.getMatRef();
dst_full_size = Scalar(16*(min_disp-1));
dst = Mat(dst_full_size,ROI);
Mat conf(confidence_map,ROI);
Mat disp_mul_conf;
disp.convertTo(disp_mul_conf,CV_32F);
disp_mul_conf = conf.mul(disp_mul_conf);
Mat conf_filtered;
Ptr<FastGlobalSmootherFilter> wls = createFastGlobalSmootherFilter(src,lambda,sigma_color);
wls->filter(disp_mul_conf,disp_mul_conf);
wls->filter(conf,conf_filtered);
disp_mul_conf = disp_mul_conf.mul(1/(conf_filtered+EPS));
disp_mul_conf.convertTo(dst,CV_16S);
}
}
DisparityWLSFilter::ComputeDiscontinuityAwareLRC_ParBody::ComputeDiscontinuityAwareLRC_ParBody(DisparityWLSFilter& _wls, Mat& _left_disp, Mat& _right_disp, Mat& _left_disc, Mat& _right_disc, Mat& _dst, Rect _left_ROI, Rect _right_ROI, int _nstripes):
wls(&_wls),left_disp(&_left_disp),right_disp(&_right_disp),left_disc(&_left_disc),right_disc(&_right_disc),dst(&_dst),left_ROI(_left_ROI),right_ROI(_right_ROI),nstripes(_nstripes)
{
stripe_sz = (int)ceil(left_disp->rows/(double)nstripes);
}
void DisparityWLSFilter::ComputeDiscontinuityAwareLRC_ParBody::operator() (const Range& range) const
{
short* row_left;
float* row_left_conf;
short* row_right;
float* row_right_conf;
float* row_dst;
int right_idx;
int h = left_disp->rows;
int start = std::min(range.start * stripe_sz, h);
int end = std::min(range.end * stripe_sz, h);
int thresh = (int)(wls->resize_factor*wls->LRC_thresh);
for(int i=start;i<end;i++)
{
row_left = (short*)left_disp->ptr(i);
row_left_conf = (float*)left_disc->ptr(i);
row_right = (short*)right_disp->ptr(i);
row_right_conf = (float*)right_disc->ptr(i);
row_dst = (float*)dst->ptr(i);
int j_start = left_ROI.x;
int j_end = left_ROI.x+left_ROI.width;
int right_end = right_ROI.x+right_ROI.width;
for(int j=j_start;j<j_end;j++)
{
right_idx = j-(row_left[j]>>4);
if( right_idx>=right_ROI.x && right_idx<right_end)
{
if(abs(row_left[j] + row_right[right_idx])< thresh)
row_dst[j] = min(row_left_conf[j],row_right_conf[right_idx]);
else
row_dst[j] = 0.0f;
}
}
}
}
DisparityWLSFilter::ComputeDepthDisc_ParBody::ComputeDepthDisc_ParBody(DisparityWLSFilter& _wls, Mat& _disp, Mat& _disp_squares, Mat& _dst, int _nstripes):
wls(&_wls),disp(&_disp),disp_squares(&_disp_squares),dst(&_dst),nstripes(_nstripes)
{
stripe_sz = (int)ceil(disp->rows/(double)nstripes);
}
void DisparityWLSFilter::ComputeDepthDisc_ParBody::operator() (const Range& range) const
{
float* row_disp;
float* row_disp_squares;
float* row_dst;
float variance;
int h = disp->rows;
int w = disp->cols;
int start = std::min(range.start * stripe_sz, h);
int end = std::min(range.end * stripe_sz, h);
float roll_off = wls->depth_discontinuity_roll_off_factor/(wls->resize_factor*wls->resize_factor);
for(int i=start;i<end;i++)
{
row_disp = (float*)disp->ptr(i);
row_disp_squares = (float*)disp_squares->ptr(i);
row_dst = (float*)dst->ptr(i);
for(int j=0;j<w;j++)
{
variance = row_disp_squares[j] - (row_disp[j])*(row_disp[j]);
row_dst[j] = max(1.0f - roll_off*variance,0.0f);
}
}
}
DisparityWLSFilter::ParallelMatOp_ParBody::ParallelMatOp_ParBody(DisparityWLSFilter& _wls, vector<MatOp> _ops, vector<Mat*>& _src, vector<Mat*>& _dst):
wls(&_wls),ops(_ops),src(_src),dst(_dst)
{}
void DisparityWLSFilter::ParallelMatOp_ParBody::operator() (const Range& range) const
{
for(int i=range.start;i<range.end;i++)
(wls->*ops[i])(*src[i],*dst[i]);
}
Ptr<DisparityWLSFilter> createDisparityWLSFilter(Ptr<StereoMatcher> matcher_left)
{
Ptr<DisparityWLSFilter> wls;
matcher_left->setDisp12MaxDiff(1000000);
matcher_left->setSpeckleWindowSize(0);
int min_disp = matcher_left->getMinDisparity();
int num_disp = matcher_left->getNumDisparities();
int wsize = matcher_left->getBlockSize();
int wsize2 = wsize/2;
if(Ptr<StereoBM> bm = matcher_left.dynamicCast<StereoBM>())
{
bm->setTextureThreshold(0);
bm->setUniquenessRatio(0);
wls = DisparityWLSFilter::create(true,max(0,min_disp+num_disp)+wsize2,max(0,-min_disp)+wsize2,wsize2,wsize2,min_disp);
wls->setDepthDiscontinuityRadius((int)ceil(0.33*wsize));
}
else if(Ptr<StereoSGBM> sgbm = matcher_left.dynamicCast<StereoSGBM>())
{
sgbm->setUniquenessRatio(0);
wls = DisparityWLSFilter::create(true,max(0,min_disp+num_disp),max(0,-min_disp),0,0,min_disp);
wls->setDepthDiscontinuityRadius((int)ceil(0.5*wsize));
}
else
CV_Error(Error::StsBadArg, "DisparityWLSFilter natively supports only StereoBM and StereoSGBM");
return wls;
}
Ptr<StereoMatcher> createRightMatcher(Ptr<StereoMatcher> matcher_left)
{
int min_disp = matcher_left->getMinDisparity();
int num_disp = matcher_left->getNumDisparities();
int wsize = matcher_left->getBlockSize();
if(Ptr<StereoBM> bm = matcher_left.dynamicCast<StereoBM>())
{
Ptr<StereoBM> right_bm = StereoBM::create(num_disp,wsize);
right_bm->setMinDisparity(-(min_disp+num_disp)+1);
right_bm->setTextureThreshold(0);
right_bm->setUniquenessRatio(0);
right_bm->setDisp12MaxDiff(1000000);
right_bm->setSpeckleWindowSize(0);
return right_bm;
}
else if(Ptr<StereoSGBM> sgbm = matcher_left.dynamicCast<StereoSGBM>())
{
Ptr<StereoSGBM> right_sgbm = StereoSGBM::create(-(min_disp+num_disp)+1,num_disp,wsize);
right_sgbm->setUniquenessRatio(0);
right_sgbm->setP1(sgbm->getP1());
right_sgbm->setP2(sgbm->getP2());
right_sgbm->setMode(sgbm->getMode());
right_sgbm->setPreFilterCap(sgbm->getPreFilterCap());
right_sgbm->setDisp12MaxDiff(1000000);
right_sgbm->setSpeckleWindowSize(0);
return right_sgbm;
}
else
{
CV_Error(Error::StsBadArg, "createRightMatcher supports only StereoBM and StereoSGBM");
return Ptr<StereoMatcher>();
}
}
Ptr<DisparityWLSFilter> createDisparityWLSFilterGeneric(bool use_confidence)
{
return Ptr<DisparityWLSFilter>(DisparityWLSFilter::create(use_confidence));
}
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
#define UNKNOWN_DISPARITY 16320
int readGT(String src_path,OutputArray dst)
{
Mat src = imread(src_path,IMREAD_UNCHANGED);
dst.create(src.rows,src.cols,CV_16S);
Mat& dstMat = dst.getMatRef();
if(!src.empty() && src.channels()==3 && src.depth()==CV_8U)
{
//MPI-Sintel format:
for(int i=0;i<src.rows;i++)
for(int j=0;j<src.cols;j++)
{
Vec3b bgrPixel = src.at<Vec3b>(i, j);
dstMat.at<short>(i,j) = 64*bgrPixel.val[2]+bgrPixel.val[1]/4; //16-multiplied disparity
}
return 0;
}
else if(!src.empty() && src.channels()==1 && src.depth()==CV_8U)
{
//Middlebury format:
for(int i=0;i<src.rows;i++)
for(int j=0;j<src.cols;j++)
{
short src_val = src.at<unsigned char>(i, j);
if(src_val==0)
dstMat.at<short>(i,j) = UNKNOWN_DISPARITY;
else
dstMat.at<short>(i,j) = 16*src_val; //16-multiplied disparity
}
return 0;
}
else
return 1;
}
double computeMSE(InputArray GT, InputArray src, Rect ROI)
{
CV_Assert( !GT.empty() && (GT.depth() == CV_16S) && (GT.channels() == 1) );
CV_Assert( !src.empty() && (src.depth() == CV_16S) && (src.channels() == 1) );
CV_Assert( src.rows() == GT.rows() && src.cols() == GT.cols() );
double res = 0;
Mat GT_ROI (GT.getMat(), ROI);
Mat src_ROI(src.getMat(),ROI);
int cnt=0;
for(int i=0;i<src_ROI.rows;i++)
for(int j=0;j<src_ROI.cols;j++)
{
if(GT_ROI.at<short>(i,j)!=UNKNOWN_DISPARITY)
{
res += (GT_ROI.at<short>(i,j) - src_ROI.at<short>(i,j))*(GT_ROI.at<short>(i,j) - src_ROI.at<short>(i,j));
cnt++;
}
}
res /= cnt*256;
return res;
}
double computeBadPixelPercent(InputArray GT, InputArray src, Rect ROI, int thresh)
{
CV_Assert( !GT.empty() && (GT.depth() == CV_16S) && (GT.channels() == 1) );
CV_Assert( !src.empty() && (src.depth() == CV_16S) && (src.channels() == 1) );
CV_Assert( src.rows() == GT.rows() && src.cols() == GT.cols() );
int bad_pixel_num = 0;
Mat GT_ROI (GT.getMat(), ROI);
Mat src_ROI(src.getMat(),ROI);
int cnt=0;
for(int i=0;i<src_ROI.rows;i++)
for(int j=0;j<src_ROI.cols;j++)
{
if(GT_ROI.at<short>(i,j)!=UNKNOWN_DISPARITY)
{
if( abs(GT_ROI.at<short>(i,j) - src_ROI.at<short>(i,j))>=thresh )
bad_pixel_num++;
cnt++;
}
}
return (100.0*bad_pixel_num)/cnt;
}
void getDisparityVis(InputArray src,OutputArray dst,double scale)
{
CV_Assert( !src.empty() && (src.depth() == CV_16S) && (src.channels() == 1) );
Mat srcMat = src.getMat();
dst.create(srcMat.rows,srcMat.cols,CV_8UC1);
Mat& dstMat = dst.getMatRef();
for(int i=0;i<dstMat.rows;i++)
for(int j=0;j<dstMat.cols;j++)
{
if(srcMat.at<short>(i,j)==UNKNOWN_DISPARITY)
dstMat.at<unsigned char>(i,j) = 0;
else
dstMat.at<unsigned char>(i,j) = saturate_cast<unsigned char>(scale*srcMat.at<short>(i,j)/16.0);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment