void spatialGradient( InputArray _src, OutputArray _dx, OutputArray _dy,
int ksize, int borderType )
{
CV_INSTRUMENT_REGION()
// Prepare InputArray src
Mat src = _src.getMat();
CV_Assert( !src.empty() );
CV_Assert( src.type() == CV_8UC1 );
CV_Assert( borderType == BORDER_DEFAULT || borderType == BORDER_REPLICATE );
// Prepare OutputArrays dx, dy
_dx.create( src.size(), CV_16SC1 );
_dy.create( src.size(), CV_16SC1 );
Mat dx = _dx.getMat(),
dy = _dy.getMat();
// TODO: Allow for other kernel sizes
CV_Assert(ksize == 3);
// Get dimensions
const int H = src.rows,
W = src.cols;
// Row, column indices
int i = 0,
j = 0;
// Handle border types
int i_top = 0, // Case for H == 1 && W == 1 && BORDER_REPLICATE
i_bottom = H - 1,
j_offl = 0, // j offset from 0th pixel to reach -1st pixel
j_offr = 0; // j offset from W-1th pixel to reach Wth pixel
if ( borderType == BORDER_DEFAULT ) // Equiv. to BORDER_REFLECT_101
{
if ( H > 1 )
{
i_top = 1;
i_bottom = H - 2;
}
if ( W > 1 )
{
j_offl = 1;
j_offr = -1;
}
}
// Pointer to row vectors
uchar *p_src, *c_src, *n_src; // previous, current, next row
short *c_dx, *c_dy;
int i_start = 0;
int j_start = 0;
#if CV_SIMD128 && CV_SSE2
if(hasSIMD128())
{
uchar *m_src;
short *n_dx, *n_dy;
// Characters in variable names have the following meanings:
// u: unsigned char
// s: signed int
//
// [row][column]
// m: offset -1
// n: offset 0
// p: offset 1
// Example: umn is offset -1 in row and offset 0 in column
for ( i = 0; i < H - 1; i += 2 )
{
if ( i == 0 ) p_src = src.ptr<uchar>(i_top);
else p_src = src.ptr<uchar>(i-1);
c_src = src.ptr<uchar>(i);
n_src = src.ptr<uchar>(i+1);
if ( i == H - 2 ) m_src = src.ptr<uchar>(i_bottom);
else m_src = src.ptr<uchar>(i+2);
c_dx = dx.ptr<short>(i);
c_dy = dy.ptr<short>(i);
n_dx = dx.ptr<short>(i+1);
n_dy = dy.ptr<short>(i+1);
v_uint8x16 v_select_m = v_uint8x16(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0xFF);
// Process rest of columns 16-column chunks at a time
for ( j = 1; j < W - 16; j += 16 )
{
// Load top row for 3x3 Sobel filter
v_uint8x16 v_um = v_load(&p_src[j-1]);
v_uint8x16 v_up = v_load(&p_src[j+1]);
// TODO: Replace _mm_slli_si128 with hal method
v_uint8x16 v_un = v_select(v_select_m, v_uint8x16(_mm_slli_si128(v_up.val, 1)),
v_uint8x16(_mm_srli_si128(v_um.val, 1)));
v_uint16x8 v_um1, v_um2, v_un1, v_un2, v_up1, v_up2;
v_expand(v_um, v_um1, v_um2);
v_expand(v_un, v_un1, v_un2);
v_expand(v_up, v_up1, v_up2);
v_int16x8 v_s1m1 = v_reinterpret_as_s16(v_um1);
v_int16x8 v_s1m2 = v_reinterpret_as_s16(v_um2);
v_int16x8 v_s1n1 = v_reinterpret_as_s16(v_un1);
v_int16x8 v_s1n2 = v_reinterpret_as_s16(v_un2);
v_int16x8 v_s1p1 = v_reinterpret_as_s16(v_up1);
v_int16x8 v_s1p2 = v_reinterpret_as_s16(v_up2);
// Load second row for 3x3 Sobel filter
v_um = v_load(&c_src[j-1]);
v_up = v_load(&c_src[j+1]);
// TODO: Replace _mm_slli_si128 with hal method
v_un = v_select(v_select_m, v_uint8x16(_mm_slli_si128(v_up.val, 1)),
v_uint8x16(_mm_srli_si128(v_um.val, 1)));
v_expand(v_um, v_um1, v_um2);
v_expand(v_un, v_un1, v_un2);
v_expand(v_up, v_up1, v_up2);
v_int16x8 v_s2m1 = v_reinterpret_as_s16(v_um1);
v_int16x8 v_s2m2 = v_reinterpret_as_s16(v_um2);
v_int16x8 v_s2n1 = v_reinterpret_as_s16(v_un1);
v_int16x8 v_s2n2 = v_reinterpret_as_s16(v_un2);
v_int16x8 v_s2p1 = v_reinterpret_as_s16(v_up1);
v_int16x8 v_s2p2 = v_reinterpret_as_s16(v_up2);
// Load third row for 3x3 Sobel filter
v_um = v_load(&n_src[j-1]);
v_up = v_load(&n_src[j+1]);
// TODO: Replace _mm_slli_si128 with hal method
v_un = v_select(v_select_m, v_uint8x16(_mm_slli_si128(v_up.val, 1)),
v_uint8x16(_mm_srli_si128(v_um.val, 1)));
v_expand(v_um, v_um1, v_um2);
v_expand(v_un, v_un1, v_un2);
v_expand(v_up, v_up1, v_up2);
v_int16x8 v_s3m1 = v_reinterpret_as_s16(v_um1);
v_int16x8 v_s3m2 = v_reinterpret_as_s16(v_um2);
v_int16x8 v_s3n1 = v_reinterpret_as_s16(v_un1);
v_int16x8 v_s3n2 = v_reinterpret_as_s16(v_un2);
v_int16x8 v_s3p1 = v_reinterpret_as_s16(v_up1);
v_int16x8 v_s3p2 = v_reinterpret_as_s16(v_up2);
// dx & dy for rows 1, 2, 3
v_int16x8 v_sdx1, v_sdy1;
spatialGradientKernel<v_int16x8>( v_sdx1, v_sdy1,
v_s1m1, v_s1n1, v_s1p1,
v_s2m1, v_s2p1,
v_s3m1, v_s3n1, v_s3p1 );
v_int16x8 v_sdx2, v_sdy2;
spatialGradientKernel<v_int16x8>( v_sdx2, v_sdy2,
v_s1m2, v_s1n2, v_s1p2,
v_s2m2, v_s2p2,
v_s3m2, v_s3n2, v_s3p2 );
// Store
v_store(&c_dx[j], v_sdx1);
v_store(&c_dx[j+8], v_sdx2);
v_store(&c_dy[j], v_sdy1);
v_store(&c_dy[j+8], v_sdy2);
// Load fourth row for 3x3 Sobel filter
v_um = v_load(&m_src[j-1]);
v_up = v_load(&m_src[j+1]);
// TODO: Replace _mm_slli_si128 with hal method
v_un = v_select(v_select_m, v_uint8x16(_mm_slli_si128(v_up.val, 1)),
v_uint8x16(_mm_srli_si128(v_um.val, 1)));
v_expand(v_um, v_um1, v_um2);
v_expand(v_un, v_un1, v_un2);
v_expand(v_up, v_up1, v_up2);
v_int16x8 v_s4m1 = v_reinterpret_as_s16(v_um1);
v_int16x8 v_s4m2 = v_reinterpret_as_s16(v_um2);
v_int16x8 v_s4n1 = v_reinterpret_as_s16(v_un1);
v_int16x8 v_s4n2 = v_reinterpret_as_s16(v_un2);
v_int16x8 v_s4p1 = v_reinterpret_as_s16(v_up1);
v_int16x8 v_s4p2 = v_reinterpret_as_s16(v_up2);
// dx & dy for rows 2, 3, 4
spatialGradientKernel<v_int16x8>( v_sdx1, v_sdy1,
v_s2m1, v_s2n1, v_s2p1,
v_s3m1, v_s3p1,
v_s4m1, v_s4n1, v_s4p1 );
spatialGradientKernel<v_int16x8>( v_sdx2, v_sdy2,
v_s2m2, v_s2n2, v_s2p2,
v_s3m2, v_s3p2,
v_s4m2, v_s4n2, v_s4p2 );
// Store
v_store(&n_dx[j], v_sdx1);
v_store(&n_dx[j+8], v_sdx2);
v_store(&n_dy[j], v_sdy1);
v_store(&n_dy[j+8], v_sdy2);
}
}
}
i_start = i;
j_start = j;
#endif
int j_p, j_n;
uchar v00, v01, v02, v10, v11, v12, v20, v21, v22;
for ( i = 0; i < H; i++ )
{
if ( i == 0 ) p_src = src.ptr<uchar>(i_top);
else p_src = src.ptr<uchar>(i-1);
c_src = src.ptr<uchar>(i);
if ( i == H - 1 ) n_src = src.ptr<uchar>(i_bottom);
else n_src = src.ptr<uchar>(i+1);
c_dx = dx.ptr<short>(i);
c_dy = dy.ptr<short>(i);
// Process left-most column
j = 0;
j_p = j + j_offl;
j_n = 1;
if ( j_n >= W ) j_n = j + j_offr;
v00 = p_src[j_p]; v01 = p_src[j]; v02 = p_src[j_n];
v10 = c_src[j_p]; v11 = c_src[j]; v12 = c_src[j_n];
v20 = n_src[j_p]; v21 = n_src[j]; v22 = n_src[j_n];
spatialGradientKernel<short>( c_dx[0], c_dy[0], v00, v01, v02, v10,
v12, v20, v21, v22 );
v00 = v01; v10 = v11; v20 = v21;
v01 = v02; v11 = v12; v21 = v22;
// Process middle columns
j = i >= i_start ? 1 : j_start;
j_p = j - 1;
v00 = p_src[j_p]; v01 = p_src[j];
v10 = c_src[j_p]; v11 = c_src[j];
v20 = n_src[j_p]; v21 = n_src[j];
for ( ; j < W - 1; j++ )
{
// Get values for next column
j_n = j + 1; v02 = p_src[j_n]; v12 = c_src[j_n]; v22 = n_src[j_n];
spatialGradientKernel<short>( c_dx[j], c_dy[j], v00, v01, v02, v10,
v12, v20, v21, v22 );
// Move values back one column for next iteration
v00 = v01; v10 = v11; v20 = v21;
v01 = v02; v11 = v12; v21 = v22;
}
// Process right-most column
if ( j < W )
{
j_n = j + j_offr; v02 = p_src[j_n]; v12 = c_src[j_n]; v22 = n_src[j_n];
spatialGradientKernel<short>( c_dx[j], c_dy[j], v00, v01, v02, v10,
v12, v20, v21, v22 );
}
}
}
void cv::matchTemplate( InputArray _img, InputArray _templ, OutputArray _result, int method )
{
CV_Assert( CV_TM_SQDIFF <= method && method <= CV_TM_CCOEFF_NORMED );
int numType = method == CV_TM_CCORR || method == CV_TM_CCORR_NORMED ? 0 :
method == CV_TM_CCOEFF || method == CV_TM_CCOEFF_NORMED ? 1 : 2;
bool isNormed = method == CV_TM_CCORR_NORMED ||
method == CV_TM_SQDIFF_NORMED ||
method == CV_TM_CCOEFF_NORMED;
Mat img = _img.getMat(), templ = _templ.getMat();
if( img.rows < templ.rows || img.cols < templ.cols )
std::swap(img, templ);
CV_Assert( (img.depth() == CV_8U || img.depth() == CV_32F) &&
img.type() == templ.type() );
Size corrSize(img.cols - templ.cols + 1, img.rows - templ.rows + 1);
_result.create(corrSize, CV_32F);
Mat result = _result.getMat();
int cn = img.channels();
crossCorr( img, templ, result, result.size(), result.type(), Point(0,0), 0, 0);
if( method == CV_TM_CCORR )
return;
double invArea = 1./((double)templ.rows * templ.cols);
Mat sum, sqsum;
Scalar templMean, templSdv;
double *q0 = 0, *q1 = 0, *q2 = 0, *q3 = 0;
double templNorm = 0, templSum2 = 0;
if( method == CV_TM_CCOEFF )
{
integral(img, sum, CV_64F);
templMean = mean(templ);
}
else
{
integral(img, sum, sqsum, CV_64F);
meanStdDev( templ, templMean, templSdv );
templNorm = CV_SQR(templSdv[0]) + CV_SQR(templSdv[1]) +
CV_SQR(templSdv[2]) + CV_SQR(templSdv[3]);
if( templNorm < DBL_EPSILON && method == CV_TM_CCOEFF_NORMED )
{
result = Scalar::all(1);
return;
}
templSum2 = templNorm +
CV_SQR(templMean[0]) + CV_SQR(templMean[1]) +
CV_SQR(templMean[2]) + CV_SQR(templMean[3]);
if( numType != 1 )
{
templMean = Scalar::all(0);
templNorm = templSum2;
}
templSum2 /= invArea;
templNorm = sqrt(templNorm);
templNorm /= sqrt(invArea); // care of accuracy here
q0 = (double*)sqsum.data;
q1 = q0 + templ.cols*cn;
q2 = (double*)(sqsum.data + templ.rows*sqsum.step);
q3 = q2 + templ.cols*cn;
}
double* p0 = (double*)sum.data;
double* p1 = p0 + templ.cols*cn;
double* p2 = (double*)(sum.data + templ.rows*sum.step);
double* p3 = p2 + templ.cols*cn;
int sumstep = sum.data ? (int)(sum.step / sizeof(double)) : 0;
int sqstep = sqsum.data ? (int)(sqsum.step / sizeof(double)) : 0;
int i, j, k;
for( i = 0; i < result.rows; i++ )
{
float* rrow = (float*)(result.data + i*result.step);
int idx = i * sumstep;
int idx2 = i * sqstep;
for( j = 0; j < result.cols; j++, idx += cn, idx2 += cn )
{
double num = rrow[j], t;
double wndMean2 = 0, wndSum2 = 0;
if( numType == 1 )
{
for( k = 0; k < cn; k++ )
{
t = p0[idx+k] - p1[idx+k] - p2[idx+k] + p3[idx+k];
wndMean2 += CV_SQR(t);
num -= t*templMean[k];
}
wndMean2 *= invArea;
}
if( isNormed || numType == 2 )
{
for( k = 0; k < cn; k++ )
{
t = q0[idx2+k] - q1[idx2+k] - q2[idx2+k] + q3[idx2+k];
wndSum2 += t;
}
if( numType == 2 )
num = wndSum2 - 2*num + templSum2;
}
if( isNormed )
{
t = sqrt(MAX(wndSum2 - wndMean2,0))*templNorm;
if( fabs(num) < t )
num /= t;
else if( fabs(num) < t*1.125 )
num = num > 0 ? 1 : -1;
else
num = method != CV_TM_SQDIFF_NORMED ? 0 : 1;
}
rrow[j] = (float)num;
}
}
}
void BTVL1_Base::process(InputArrayOfArrays _src, OutputArray _dst, InputArrayOfArrays _forwardMotions,
InputArrayOfArrays _backwardMotions, int baseIdx)
{
CV_Assert( scale_ > 1 );
CV_Assert( iterations_ > 0 );
CV_Assert( tau_ > 0.0 );
CV_Assert( alpha_ > 0.0 );
CV_Assert( btvKernelSize_ > 0 );
CV_Assert( blurKernelSize_ > 0 );
CV_Assert( blurSigma_ >= 0.0 );
CV_OCL_RUN(_src.isUMatVector() && _dst.isUMat() && _forwardMotions.isUMatVector() &&
_backwardMotions.isUMatVector(),
ocl_process(_src, _dst, _forwardMotions, _backwardMotions, baseIdx))
std::vector<Mat> & src = *(std::vector<Mat> *)_src.getObj(),
& forwardMotions = *(std::vector<Mat> *)_forwardMotions.getObj(),
& backwardMotions = *(std::vector<Mat> *)_backwardMotions.getObj();
// update blur filter and btv weights
if (blurKernelSize_ != curBlurKernelSize_ || blurSigma_ != curBlurSigma_ || src[0].type() != curSrcType_)
{
//filter_ = createGaussianFilter(src[0].type(), Size(blurKernelSize_, blurKernelSize_), blurSigma_);
curBlurKernelSize_ = blurKernelSize_;
curBlurSigma_ = blurSigma_;
curSrcType_ = src[0].type();
}
if (btvWeights_.empty() || btvKernelSize_ != curBtvKernelSize_ || alpha_ != curAlpha_)
{
calcBtvWeights(btvKernelSize_, alpha_, btvWeights_);
curBtvKernelSize_ = btvKernelSize_;
curAlpha_ = alpha_;
}
// calc high res motions
calcRelativeMotions(forwardMotions, backwardMotions, lowResForwardMotions_, lowResBackwardMotions_, baseIdx, src[0].size());
upscaleMotions(lowResForwardMotions_, highResForwardMotions_, scale_);
upscaleMotions(lowResBackwardMotions_, highResBackwardMotions_, scale_);
forwardMaps_.resize(highResForwardMotions_.size());
backwardMaps_.resize(highResForwardMotions_.size());
for (size_t i = 0; i < highResForwardMotions_.size(); ++i)
buildMotionMaps(highResForwardMotions_[i], highResBackwardMotions_[i], forwardMaps_[i], backwardMaps_[i]);
// initial estimation
const Size lowResSize = src[0].size();
const Size highResSize(lowResSize.width * scale_, lowResSize.height * scale_);
resize(src[baseIdx], highRes_, highResSize, 0, 0, INTER_CUBIC);
// iterations
diffTerm_.create(highResSize, highRes_.type());
a_.create(highResSize, highRes_.type());
b_.create(highResSize, highRes_.type());
c_.create(lowResSize, highRes_.type());
for (int i = 0; i < iterations_; ++i)
{
diffTerm_.setTo(Scalar::all(0));
for (size_t k = 0; k < src.size(); ++k)
{
// a = M * Ih
remap(highRes_, a_, backwardMaps_[k], noArray(), INTER_NEAREST);
// b = HM * Ih
GaussianBlur(a_, b_, Size(blurKernelSize_, blurKernelSize_), blurSigma_);
// c = DHM * Ih
resize(b_, c_, lowResSize, 0, 0, INTER_NEAREST);
diffSign(src[k], c_, c_);
// a = Dt * diff
upscale(c_, a_, scale_);
// b = HtDt * diff
GaussianBlur(a_, b_, Size(blurKernelSize_, blurKernelSize_), blurSigma_);
// a = MtHtDt * diff
remap(b_, a_, forwardMaps_[k], noArray(), INTER_NEAREST);
add(diffTerm_, a_, diffTerm_);
}
if (lambda_ > 0)
{
calcBtvRegularization(highRes_, regTerm_, btvKernelSize_, btvWeights_, ubtvWeights_);
addWeighted(diffTerm_, 1.0, regTerm_, -lambda_, 0.0, diffTerm_);
}
addWeighted(highRes_, 1.0, diffTerm_, tau_, 0.0, highRes_);
}
Rect inner(btvKernelSize_, btvKernelSize_, highRes_.cols - 2 * btvKernelSize_, highRes_.rows - 2 * btvKernelSize_);
highRes_(inner).copyTo(_dst);
}
//----------------------------------------------------------------------------
bool FxCompiler::UpdateShader (Shader* shader, const Program& program,
InputArray& inputs, OutputArray& outputs, ConstantArray& constants,
SamplerArray& samplers)
{
int numInputs = (int)inputs.size();
if (numInputs != shader->GetNumInputs())
{
ReportError("Mismatch in number of inputs.\n");
return false;
}
int numOutputs = (int)outputs.size();
if (numOutputs != shader->GetNumOutputs())
{
ReportError("Mismatch in number of outputs.\n");
return false;
}
int numConstants = (int)constants.size();
if (numConstants != shader->GetNumConstants())
{
ReportError("Mismatch in number of constants.\n");
return false;
}
int numSamplers = (int)samplers.size();
if (numSamplers != shader->GetNumSamplers())
{
ReportError("Mismatch in number of samplers.\n");
return false;
}
std::string message;
int i;
for (i = 0; i < numInputs; ++i)
{
Input& input = inputs[i];
if (input.Name != shader->GetInputName(i))
{
message = "Mismatch in input names '" +
input.Name +
"' and '" +
shader->GetInputName(i);
ReportError(message);
return false;
}
if (input.Type != shader->GetInputType(i))
{
message = "Mismatch in input types '" +
msVTName[input.Type] +
"' and '" +
msVTName[shader->GetInputType(i)];
ReportError(message);
return false;
}
if (input.Semantic != shader->GetInputSemantic(i))
{
message = "Mismatch in input semantics '" +
msVSName[input.Semantic] +
"' and '" +
msVSName[shader->GetInputSemantic(i)];
ReportError(message);
return false;
}
}
for (i = 0; i < numOutputs; ++i)
{
Output& output = outputs[i];
if (output.Name != shader->GetOutputName(i))
{
message = "Mismatch in output names '" +
output.Name +
"' and '" +
shader->GetOutputName(i);
ReportError(message);
return false;
}
if (output.Type != shader->GetOutputType(i))
{
message = "Mismatch in output types '" +
msVTName[output.Type] +
"' and '" +
msVTName[shader->GetOutputType(i)];
ReportError(message);
return false;
}
if (output.Semantic != shader->GetOutputSemantic(i))
{
message = "Mismatch in output semantics '" +
msVSName[output.Semantic] +
"' and '" +
msVSName[shader->GetOutputSemantic(i)];
ReportError(message);
return false;
}
}
for (i = 0; i < numConstants; ++i)
{
Constant& constant = constants[i];
if (constant.Name != shader->GetConstantName(i))
{
message = "Mismatch in constant names '" +
constant.Name +
"' and '" +
shader->GetConstantName(i);
ReportError(message);
return false;
}
if (constant.NumRegistersUsed != shader->GetNumRegistersUsed(i))
{
char number0[8], number1[8];
sprintf(number0, "%d", constant.NumRegistersUsed);
sprintf(number1, "%d", shader->GetNumRegistersUsed(i));
message = "Mismatch in constant registers used '" +
std::string(number0) +
"' and '" +
std::string(number1);
ReportError(message);
return false;
}
shader->SetBaseRegister(mActiveProfile, i, constant.BaseRegister);
}
for (i = 0; i < numSamplers; ++i)
{
Sampler& sampler = samplers[i];
if (sampler.Name != shader->GetSamplerName(i))
{
message = "Mismatch in sampler names '" +
sampler.Name +
"' and '" +
shader->GetSamplerName(i);
ReportError(message);
return false;
}
if (sampler.Type != shader->GetSamplerType(i))
{
message = "Mismatch in sampler types '" +
msSTName[sampler.Type] +
"' and '" +
msSTName[shader->GetSamplerType(i)];
ReportError(message);
return false;
}
shader->SetTextureUnit(mActiveProfile, i, sampler.Unit);
}
shader->SetProgram(mActiveProfile, program.Text);
return true;
}
bool PositionCalculatorImpl::computeStateImpl( double /*time*/, OutputArray _state, OutputArray _covariance )
{
_state.create( 3, 1, CV_64F );
Mat& state = _state.getMatRef();
const int num = static_cast< int >(positions.size());
F.create( 2 * num, 3, CV_64F );
b.create( 2 * num, 1, CV_64F );
for ( int i = 0; i < num; ++i )
{
F.at< double >( i, 0 ) = sin( angles[i].x );
F.at< double >( i, 1 ) = -cos( angles[i].x );
F.at< double >( i, 2 ) = 0;
F.at< double >( num + i, 0 ) = -sin( angles[i].y ) * cos( angles[i].x );
F.at< double >( num + i, 1 ) = -sin( angles[i].y ) * sin( angles[i].x );
F.at< double >( num + i, 2 ) = cos( angles[i].y );
b.at< double >( i ) = ( F( Rect( 0, i, 3, 1 ) ) * positions[i] ).operator Mat().at< double >( 0 );
b.at< double >( num + i ) = ( F( Rect( 0, num + i, 3, 1 ) ) * positions[i] ).operator Mat().at< double
>( 0 );
}
Mat iple;
solve( F, b, iple, DECOMP_SVD );
G.create( 2 * num, 3, CV_64F );
W.create( 2 * num, 2 * num, CV_64F );
W.setTo( 0 );
for ( int i = 0; i < num; ++i )
{
const Point2d new_ang = getAzEl( positions[i], iple );
G.at< double >( i, 0 ) = sin( new_ang.x );
G.at< double >( i, 1 ) = -cos( new_ang.x );
G.at< double >( i, 2 ) = 0;
G.at< double >( num + i, 0 ) = -sin( new_ang.y ) * cos( new_ang.x );
G.at< double >( num + i, 1 ) = -sin( new_ang.y ) * sin( new_ang.x );
G.at< double >( num + i, 2 ) = cos( new_ang.y );
Point2f diff2;
Point3f diff3;
diff2.x = ( iple.at< double >( 0 ) - positions[i].at< double >( 0 ) );
diff2.y = ( iple.at< double >( 1 ) - positions[i].at< double >( 1 ) );
diff3.x = diff2.x;
diff3.y = diff2.y;
diff3.z = ( iple.at< double >( 2 ) - positions[i].at< double >( 2 ) );
W.at< double >( i, i ) = norm( diff2 );
W.at< double >( i + num, i + num ) = ( norm( diff3 ) - W.at< double >( i, i ) );
}
state = ( G.t() * W * F ).operator Mat().inv() * G.t() * W * b;
if ( _covariance.needed() )
{
Mat& cov = _covariance.getMatRef();
cov = G.t() * F / ( num / 2.0 );
multiply( cov, sum( F * state - b )( 0 ), cov );
}
return true;
}
void cv::merge(const Mat* mv, size_t n, OutputArray _dst)
{
CV_Assert( mv && n > 0 );
int depth = mv[0].depth();
bool allch1 = true;
int k, cn = 0;
size_t i;
for( i = 0; i < n; i++ )
{
CV_Assert(mv[i].size == mv[0].size && mv[i].depth() == depth);
allch1 = allch1 && mv[i].channels() == 1;
cn += mv[i].channels();
}
CV_Assert( 0 < cn && cn <= CV_CN_MAX );
_dst.create(mv[0].dims, mv[0].size, CV_MAKETYPE(depth, cn));
Mat dst = _dst.getMat();
if( n == 1 )
{
mv[0].copyTo(dst);
return;
}
if( !allch1 )
{
AutoBuffer<int> pairs(cn*2);
int j, ni=0;
for( i = 0, j = 0; i < n; i++, j += ni )
{
ni = mv[i].channels();
for( k = 0; k < ni; k++ )
{
pairs[(j+k)*2] = j + k;
pairs[(j+k)*2+1] = j + k;
}
}
mixChannels( mv, n, &dst, 1, &pairs[0], cn );
return;
}
size_t esz = dst.elemSize(), esz1 = dst.elemSize1();
int blocksize0 = (int)((BLOCK_SIZE + esz-1)/esz);
AutoBuffer<uchar> _buf((cn+1)*(sizeof(Mat*) + sizeof(uchar*)) + 16);
const Mat** arrays = (const Mat**)(uchar*)_buf;
uchar** ptrs = (uchar**)alignPtr(arrays + cn + 1, 16);
arrays[0] = &dst;
for( k = 0; k < cn; k++ )
arrays[k+1] = &mv[k];
NAryMatIterator it(arrays, ptrs, cn+1);
int total = (int)it.size, blocksize = cn <= 4 ? total : std::min(total, blocksize0);
MergeFunc func = mergeTab[depth];
for( i = 0; i < it.nplanes; i++, ++it )
{
for( int j = 0; j < total; j += blocksize )
{
int bsz = std::min(total - j, blocksize);
func( (const uchar**)&ptrs[1], ptrs[0], bsz, cn );
if( j + blocksize < total )
{
ptrs[0] += bsz*esz;
for( int k = 0; k < cn; k++ )
ptrs[k+1] += bsz*esz1;
}
}
}
}
static void create(OutputArray arr, Size submatSize, int type)
{
int sizes[] = {submatSize.width, submatSize.height};
arr.create(sizeof(sizes)/sizeof(sizes[0]), sizes, type);
}
void convexityDefects( InputArray _points, InputArray _hull, OutputArray _defects )
{
CV_INSTRUMENT_REGION()
Mat points = _points.getMat();
int i, j = 0, npoints = points.checkVector(2, CV_32S);
CV_Assert( npoints >= 0 );
if( npoints <= 3 )
{
_defects.release();
return;
}
Mat hull = _hull.getMat();
int hpoints = hull.checkVector(1, CV_32S);
CV_Assert( hpoints > 2 );
const Point* ptr = points.ptr<Point>();
const int* hptr = hull.ptr<int>();
std::vector<Vec4i> defects;
// 1. recognize co-orientation of the contour and its hull
bool rev_orientation = ((hptr[1] > hptr[0]) + (hptr[2] > hptr[1]) + (hptr[0] > hptr[2])) != 2;
// 2. cycle through points and hull, compute defects
int hcurr = hptr[rev_orientation ? 0 : hpoints-1];
CV_Assert( 0 <= hcurr && hcurr < npoints );
for( i = 0; i < hpoints; i++ )
{
int hnext = hptr[rev_orientation ? hpoints - i - 1 : i];
CV_Assert( 0 <= hnext && hnext < npoints );
Point pt0 = ptr[hcurr], pt1 = ptr[hnext];
double dx0 = pt1.x - pt0.x;
double dy0 = pt1.y - pt0.y;
double scale = dx0 == 0 && dy0 == 0 ? 0. : 1./std::sqrt(dx0*dx0 + dy0*dy0);
int defect_deepest_point = -1;
double defect_depth = 0;
bool is_defect = false;
j=hcurr;
for(;;)
{
// go through points to achieve next hull point
j++;
j &= j >= npoints ? 0 : -1;
if( j == hnext )
break;
// compute distance from current point to hull edge
double dx = ptr[j].x - pt0.x;
double dy = ptr[j].y - pt0.y;
double dist = fabs(-dy0*dx + dx0*dy) * scale;
if( dist > defect_depth )
{
defect_depth = dist;
defect_deepest_point = j;
is_defect = true;
}
}
if( is_defect )
{
int idepth = cvRound(defect_depth*256);
defects.push_back(Vec4i(hcurr, hnext, defect_deepest_point, idepth));
}
hcurr = hnext;
}
Mat(defects).copyTo(_defects);
}
double cv::kmeans( InputArray _data, int K,
InputOutputArray _bestLabels,
TermCriteria criteria, int attempts,
int flags, OutputArray _centers )
{
const int SPP_TRIALS = 3;
Mat data0 = _data.getMat();
bool isrow = data0.rows == 1;
int N = isrow ? data0.cols : data0.rows;
int dims = (isrow ? 1 : data0.cols)*data0.channels();
int type = data0.depth();
attempts = std::max(attempts, 1);
CV_Assert( data0.dims <= 2 && type == CV_32F && K > 0 );
CV_Assert( N >= K );
Mat data(N, dims, CV_32F, data0.ptr(), isrow ? dims * sizeof(float) : static_cast<size_t>(data0.step));
_bestLabels.create(N, 1, CV_32S, -1, true);
Mat _labels, best_labels = _bestLabels.getMat();
if( flags & CV_KMEANS_USE_INITIAL_LABELS )
{
CV_Assert( (best_labels.cols == 1 || best_labels.rows == 1) &&
best_labels.cols*best_labels.rows == N &&
best_labels.type() == CV_32S &&
best_labels.isContinuous());
best_labels.copyTo(_labels);
}
else
{
if( !((best_labels.cols == 1 || best_labels.rows == 1) &&
best_labels.cols*best_labels.rows == N &&
best_labels.type() == CV_32S &&
best_labels.isContinuous()))
best_labels.create(N, 1, CV_32S);
_labels.create(best_labels.size(), best_labels.type());
}
int* labels = _labels.ptr<int>();
Mat centers(K, dims, type), old_centers(K, dims, type), temp(1, dims, type);
std::vector<int> counters(K);
std::vector<Vec2f> _box(dims);
Vec2f* box = &_box[0];
double best_compactness = DBL_MAX, compactness = 0;
RNG& rng = theRNG();
int a, iter, i, j, k;
if( criteria.type & TermCriteria::EPS )
criteria.epsilon = std::max(criteria.epsilon, 0.);
else
criteria.epsilon = FLT_EPSILON;
criteria.epsilon *= criteria.epsilon;
if( criteria.type & TermCriteria::COUNT )
criteria.maxCount = std::min(std::max(criteria.maxCount, 2), 100);
else
criteria.maxCount = 100;
if( K == 1 )
{
attempts = 1;
criteria.maxCount = 2;
}
const float* sample = data.ptr<float>(0);
for( j = 0; j < dims; j++ )
box[j] = Vec2f(sample[j], sample[j]);
for( i = 1; i < N; i++ )
{
sample = data.ptr<float>(i);
for( j = 0; j < dims; j++ )
{
float v = sample[j];
box[j][0] = std::min(box[j][0], v);
box[j][1] = std::max(box[j][1], v);
}
}
for( a = 0; a < attempts; a++ )
{
double max_center_shift = DBL_MAX;
for( iter = 0;; )
{
swap(centers, old_centers);
if( iter == 0 && (a > 0 || !(flags & KMEANS_USE_INITIAL_LABELS)) )
{
if( flags & KMEANS_PP_CENTERS )
generateCentersPP(data, centers, K, rng, SPP_TRIALS);
else
{
for( k = 0; k < K; k++ )
generateRandomCenter(_box, centers.ptr<float>(k), rng);
}
}
else
{
if( iter == 0 && a == 0 && (flags & KMEANS_USE_INITIAL_LABELS) )
{
for( i = 0; i < N; i++ )
CV_Assert( (unsigned)labels[i] < (unsigned)K );
}
// compute centers
centers = Scalar(0);
for( k = 0; k < K; k++ )
counters[k] = 0;
for( i = 0; i < N; i++ )
{
sample = data.ptr<float>(i);
k = labels[i];
float* center = centers.ptr<float>(k);
j=0;
#if CV_ENABLE_UNROLLED
for(; j <= dims - 4; j += 4 )
{
float t0 = center[j] + sample[j];
float t1 = center[j+1] + sample[j+1];
center[j] = t0;
center[j+1] = t1;
t0 = center[j+2] + sample[j+2];
t1 = center[j+3] + sample[j+3];
center[j+2] = t0;
center[j+3] = t1;
}
#endif
for( ; j < dims; j++ )
center[j] += sample[j];
counters[k]++;
}
if( iter > 0 )
max_center_shift = 0;
for( k = 0; k < K; k++ )
{
if( counters[k] != 0 )
continue;
// if some cluster appeared to be empty then:
// 1. find the biggest cluster
// 2. find the farthest from the center point in the biggest cluster
// 3. exclude the farthest point from the biggest cluster and form a new 1-point cluster.
int max_k = 0;
for( int k1 = 1; k1 < K; k1++ )
{
if( counters[max_k] < counters[k1] )
max_k = k1;
}
double max_dist = 0;
int farthest_i = -1;
float* new_center = centers.ptr<float>(k);
float* old_center = centers.ptr<float>(max_k);
float* _old_center = temp.ptr<float>(); // normalized
float scale = 1.f/counters[max_k];
for( j = 0; j < dims; j++ )
_old_center[j] = old_center[j]*scale;
for( i = 0; i < N; i++ )
{
if( labels[i] != max_k )
continue;
sample = data.ptr<float>(i);
double dist = normL2Sqr(sample, _old_center, dims);
if( max_dist <= dist )
{
max_dist = dist;
farthest_i = i;
}
}
counters[max_k]--;
counters[k]++;
labels[farthest_i] = k;
sample = data.ptr<float>(farthest_i);
for( j = 0; j < dims; j++ )
{
old_center[j] -= sample[j];
new_center[j] += sample[j];
}
}
for( k = 0; k < K; k++ )
{
float* center = centers.ptr<float>(k);
CV_Assert( counters[k] != 0 );
float scale = 1.f/counters[k];
for( j = 0; j < dims; j++ )
center[j] *= scale;
if( iter > 0 )
{
double dist = 0;
const float* old_center = old_centers.ptr<float>(k);
for( j = 0; j < dims; j++ )
{
double t = center[j] - old_center[j];
dist += t*t;
}
max_center_shift = std::max(max_center_shift, dist);
}
}
}
if( ++iter == MAX(criteria.maxCount, 2) || max_center_shift <= criteria.epsilon )
break;
// assign labels
Mat dists(1, N, CV_64F);
double* dist = dists.ptr<double>(0);
parallel_for_(Range(0, N),
KMeansDistanceComputer(dist, labels, data, centers));
compactness = 0;
for( i = 0; i < N; i++ )
{
compactness += dist[i];
}
}
if( compactness < best_compactness )
{
best_compactness = compactness;
if( _centers.needed() )
centers.copyTo(_centers);
_labels.copyTo(best_labels);
}
}
return best_compactness;
}
void cv::goodFeaturesToTrack( InputArray _image, OutputArray _corners,
int maxCorners, double qualityLevel, double minDistance,
InputArray _mask, int blockSize,
bool useHarrisDetector, double harrisK )
{
Mat image = _image.getMat(), mask = _mask.getMat();
CV_Assert( qualityLevel > 0 && minDistance >= 0 && maxCorners >= 0 );
CV_Assert( mask.empty() || (mask.type() == CV_8UC1 && mask.size() == image.size()) );
Mat eig, tmp;
if( useHarrisDetector )
cornerHarris( image, eig, blockSize, 3, harrisK );
else
cornerMinEigenVal( image, eig, blockSize, 3 );
double maxVal = 0;
minMaxLoc( eig, 0, &maxVal, 0, 0, mask );
threshold( eig, eig, maxVal*qualityLevel, 0, THRESH_TOZERO );
dilate( eig, tmp, Mat());
Size imgsize = image.size();
vector<const float*> tmpCorners;
// collect list of pointers to features - put them into temporary image
for( int y = 1; y < imgsize.height - 1; y++ )
{
const float* eig_data = (const float*)eig.ptr(y);
const float* tmp_data = (const float*)tmp.ptr(y);
const uchar* mask_data = mask.data ? mask.ptr(y) : 0;
for( int x = 1; x < imgsize.width - 1; x++ )
{
float val = eig_data[x];
if( val != 0 && val == tmp_data[x] && (!mask_data || mask_data[x]) )
tmpCorners.push_back(eig_data + x);
}
}
sort( tmpCorners, greaterThanPtr<float>() );
vector<Point2f> corners;
size_t i, j, total = tmpCorners.size(), ncorners = 0;
if(minDistance >= 1)
{
// Partition the image into larger grids
int w = image.cols;
int h = image.rows;
const int cell_size = cvRound(minDistance);
const int grid_width = (w + cell_size - 1) / cell_size;
const int grid_height = (h + cell_size - 1) / cell_size;
std::vector<std::vector<Point2f> > grid(grid_width*grid_height);
minDistance *= minDistance;
for( i = 0; i < total; i++ )
{
int ofs = (int)((const uchar*)tmpCorners[i] - eig.data);
int y = (int)(ofs / eig.step);
int x = (int)((ofs - y*eig.step)/sizeof(float));
bool good = true;
int x_cell = x / cell_size;
int y_cell = y / cell_size;
int x1 = x_cell - 1;
int y1 = y_cell - 1;
int x2 = x_cell + 1;
int y2 = y_cell + 1;
// boundary check
x1 = std::max(0, x1);
y1 = std::max(0, y1);
x2 = std::min(grid_width-1, x2);
y2 = std::min(grid_height-1, y2);
for( int yy = y1; yy <= y2; yy++ )
{
for( int xx = x1; xx <= x2; xx++ )
{
vector <Point2f> &m = grid[yy*grid_width + xx];
if( m.size() )
{
for(j = 0; j < m.size(); j++)
{
float dx = x - m[j].x;
float dy = y - m[j].y;
if( dx*dx + dy*dy < minDistance )
{
good = false;
goto break_out;
}
}
}
}
}
break_out:
if(good)
{
// printf("%d: %d %d -> %d %d, %d, %d -- %d %d %d %d, %d %d, c=%d\n",
// i,x, y, x_cell, y_cell, (int)minDistance, cell_size,x1,y1,x2,y2, grid_width,grid_height,c);
grid[y_cell*grid_width + x_cell].push_back(Point2f((float)x, (float)y));
corners.push_back(Point2f((float)x, (float)y));
++ncorners;
if( maxCorners > 0 && (int)ncorners == maxCorners )
break;
}
}
}
else
{
for( i = 0; i < total; i++ )
{
int ofs = (int)((const uchar*)tmpCorners[i] - eig.data);
int y = (int)(ofs / eig.step);
int x = (int)((ofs - y*eig.step)/sizeof(float));
corners.push_back(Point2f((float)x, (float)y));
++ncorners;
if( maxCorners > 0 && (int)ncorners == maxCorners )
break;
}
}
Mat(corners).convertTo(_corners, _corners.fixedType() ? _corners.type() : CV_32F);
/*
for( i = 0; i < total; i++ )
{
int ofs = (int)((const uchar*)tmpCorners[i] - eig.data);
int y = (int)(ofs / eig.step);
int x = (int)((ofs - y*eig.step)/sizeof(float));
if( minDistance > 0 )
{
for( j = 0; j < ncorners; j++ )
{
float dx = x - corners[j].x;
float dy = y - corners[j].y;
if( dx*dx + dy*dy < minDistance )
break;
}
if( j < ncorners )
continue;
}
corners.push_back(Point2f((float)x, (float)y));
++ncorners;
if( maxCorners > 0 && (int)ncorners == maxCorners )
break;
}
*/
}
void convexHull( InputArray _points, OutputArray _hull, bool clockwise, bool returnPoints )
{
CV_INSTRUMENT_REGION()
Mat points = _points.getMat();
int i, total = points.checkVector(2), depth = points.depth(), nout = 0;
int miny_ind = 0, maxy_ind = 0;
CV_Assert(total >= 0 && (depth == CV_32F || depth == CV_32S));
if( total == 0 )
{
_hull.release();
return;
}
returnPoints = !_hull.fixedType() ? returnPoints : _hull.type() != CV_32S;
bool is_float = depth == CV_32F;
AutoBuffer<Point*> _pointer(total);
AutoBuffer<int> _stack(total + 2), _hullbuf(total);
Point** pointer = _pointer;
Point2f** pointerf = (Point2f**)pointer;
Point* data0 = points.ptr<Point>();
int* stack = _stack;
int* hullbuf = _hullbuf;
CV_Assert(points.isContinuous());
for( i = 0; i < total; i++ )
pointer[i] = &data0[i];
// sort the point set by x-coordinate, find min and max y
if( !is_float )
{
std::sort(pointer, pointer + total, CHullCmpPoints<int>());
for( i = 1; i < total; i++ )
{
int y = pointer[i]->y;
if( pointer[miny_ind]->y > y )
miny_ind = i;
if( pointer[maxy_ind]->y < y )
maxy_ind = i;
}
}
else
{
std::sort(pointerf, pointerf + total, CHullCmpPoints<float>());
for( i = 1; i < total; i++ )
{
float y = pointerf[i]->y;
if( pointerf[miny_ind]->y > y )
miny_ind = i;
if( pointerf[maxy_ind]->y < y )
maxy_ind = i;
}
}
if( pointer[0]->x == pointer[total-1]->x &&
pointer[0]->y == pointer[total-1]->y )
{
hullbuf[nout++] = 0;
}
else
{
// upper half
int *tl_stack = stack;
int tl_count = !is_float ?
Sklansky_( pointer, 0, maxy_ind, tl_stack, -1, 1) :
Sklansky_( pointerf, 0, maxy_ind, tl_stack, -1, 1);
int *tr_stack = stack + tl_count;
int tr_count = !is_float ?
Sklansky_( pointer, total-1, maxy_ind, tr_stack, -1, -1) :
Sklansky_( pointerf, total-1, maxy_ind, tr_stack, -1, -1);
// gather upper part of convex hull to output
if( !clockwise )
{
std::swap( tl_stack, tr_stack );
std::swap( tl_count, tr_count );
}
for( i = 0; i < tl_count-1; i++ )
hullbuf[nout++] = int(pointer[tl_stack[i]] - data0);
for( i = tr_count - 1; i > 0; i-- )
hullbuf[nout++] = int(pointer[tr_stack[i]] - data0);
int stop_idx = tr_count > 2 ? tr_stack[1] : tl_count > 2 ? tl_stack[tl_count - 2] : -1;
// lower half
int *bl_stack = stack;
int bl_count = !is_float ?
Sklansky_( pointer, 0, miny_ind, bl_stack, 1, -1) :
Sklansky_( pointerf, 0, miny_ind, bl_stack, 1, -1);
int *br_stack = stack + bl_count;
int br_count = !is_float ?
Sklansky_( pointer, total-1, miny_ind, br_stack, 1, 1) :
Sklansky_( pointerf, total-1, miny_ind, br_stack, 1, 1);
if( clockwise )
{
std::swap( bl_stack, br_stack );
std::swap( bl_count, br_count );
}
if( stop_idx >= 0 )
{
int check_idx = bl_count > 2 ? bl_stack[1] :
bl_count + br_count > 2 ? br_stack[2-bl_count] : -1;
if( check_idx == stop_idx || (check_idx >= 0 &&
pointer[check_idx]->x == pointer[stop_idx]->x &&
pointer[check_idx]->y == pointer[stop_idx]->y) )
{
// if all the points lie on the same line, then
// the bottom part of the convex hull is the mirrored top part
// (except the exteme points).
bl_count = MIN( bl_count, 2 );
br_count = MIN( br_count, 2 );
}
}
for( i = 0; i < bl_count-1; i++ )
hullbuf[nout++] = int(pointer[bl_stack[i]] - data0);
for( i = br_count-1; i > 0; i-- )
hullbuf[nout++] = int(pointer[br_stack[i]] - data0);
}
if( !returnPoints )
Mat(nout, 1, CV_32S, hullbuf).copyTo(_hull);
else
{
_hull.create(nout, 1, CV_MAKETYPE(depth, 2));
Mat hull = _hull.getMat();
size_t step = !hull.isContinuous() ? hull.step[0] : sizeof(Point);
for( i = 0; i < nout; i++ )
*(Point*)(hull.ptr() + i*step) = data0[hullbuf[i]];
}
}
void SIFT::operator()(InputArray _image, InputArray _mask,
std::vector<KeyPoint>& keypoints,
OutputArray _descriptors,
bool useProvidedKeypoints) const
{
int firstOctave = -1, actualNOctaves = 0, actualNLayers = 0;
Mat image = _image.getMat(), mask = _mask.getMat();
if( image.empty() || image.depth() != CV_8U )
CV_Error( Error::StsBadArg, "image is empty or has incorrect depth (!=CV_8U)" );
if( !mask.empty() && mask.type() != CV_8UC1 )
CV_Error( Error::StsBadArg, "mask has incorrect type (!=CV_8UC1)" );
if( useProvidedKeypoints )
{
firstOctave = 0;
int maxOctave = INT_MIN;
for( size_t i = 0; i < keypoints.size(); i++ )
{
int octave, layer;
float scale;
unpackOctave(keypoints[i], octave, layer, scale);
firstOctave = std::min(firstOctave, octave);
maxOctave = std::max(maxOctave, octave);
actualNLayers = std::max(actualNLayers, layer-2);
}
firstOctave = std::min(firstOctave, 0);
CV_Assert( firstOctave >= -1 && actualNLayers <= nOctaveLayers );
actualNOctaves = maxOctave - firstOctave + 1;
}
Mat base = createInitialImage(image, firstOctave < 0, (float)sigma);
std::vector<Mat> gpyr, dogpyr;
int nOctaves = actualNOctaves > 0 ? actualNOctaves : cvRound(std::log( (double)std::min( base.cols, base.rows ) ) / std::log(2.) - 2) - firstOctave;
//double t, tf = getTickFrequency();
//t = (double)getTickCount();
buildGaussianPyramid(base, gpyr, nOctaves);
buildDoGPyramid(gpyr, dogpyr);
//t = (double)getTickCount() - t;
//printf("pyramid construction time: %g\n", t*1000./tf);
if( !useProvidedKeypoints )
{
//t = (double)getTickCount();
findScaleSpaceExtrema(gpyr, dogpyr, keypoints);
KeyPointsFilter::removeDuplicated( keypoints );
if( !mask.empty() )
KeyPointsFilter::runByPixelsMask( keypoints, mask );
if( nfeatures > 0 )
KeyPointsFilter::retainBest(keypoints, nfeatures);
//t = (double)getTickCount() - t;
//printf("keypoint detection time: %g\n", t*1000./tf);
if( firstOctave < 0 )
for( size_t i = 0; i < keypoints.size(); i++ )
{
KeyPoint& kpt = keypoints[i];
float scale = 1.f/(float)(1 << -firstOctave);
kpt.octave = (kpt.octave & ~255) | ((kpt.octave + firstOctave) & 255);
kpt.pt *= scale;
kpt.size *= scale;
}
}
else
{
// filter keypoints by mask
//KeyPointsFilter::runByPixelsMask( keypoints, mask );
}
if( _descriptors.needed() )
{
//t = (double)getTickCount();
int dsize = descriptorSize();
_descriptors.create((int)keypoints.size(), dsize, CV_32F);
Mat descriptors = _descriptors.getMat();
calcDescriptors(gpyr, keypoints, descriptors, nOctaveLayers, firstOctave);
//t = (double)getTickCount() - t;
//printf("descriptor extraction time: %g\n", t*1000./tf);
}
}
static bool matchTemplate_CCOEFF_NORMED(InputArray _image, InputArray _templ, OutputArray _result)
{
matchTemplate(_image, _templ, _result, CV_TM_CCORR);
UMat temp, image_sums, image_sqsums;
integral(_image, image_sums, image_sqsums, CV_32F, CV_32F);
int type = image_sums.type(), depth = CV_MAT_DEPTH(type), cn = CV_MAT_CN(type);
ocl::Kernel k("matchTemplate_CCOEFF_NORMED", ocl::imgproc::match_template_oclsrc,
format("-D CCOEFF_NORMED -D type=%s -D elem_type=%s -D cn=%d", ocl::typeToStr(type), ocl::typeToStr(depth), cn));
if (k.empty())
return false;
UMat templ = _templ.getUMat();
Size size = _image.size(), tsize = templ.size();
_result.create(size.height - templ.rows + 1, size.width - templ.cols + 1, CV_32F);
UMat result = _result.getUMat();
float scale = 1.f / tsize.area();
if (cn == 1)
{
float templ_sum = (float)sum(templ)[0];
multiply(templ, templ, temp, 1, CV_32F);
float templ_sqsum = (float)sum(temp)[0];
templ_sqsum -= scale * templ_sum * templ_sum;
templ_sum *= scale;
if (templ_sqsum < DBL_EPSILON)
{
result = Scalar::all(1);
return true;
}
k.args(ocl::KernelArg::ReadOnlyNoSize(image_sums), ocl::KernelArg::ReadOnlyNoSize(image_sqsums),
ocl::KernelArg::ReadWrite(result), templ.rows, templ.cols, scale, templ_sum, templ_sqsum);
}
else
{
Vec4f templ_sum = Vec4f::all(0), templ_sqsum = Vec4f::all(0);
templ_sum = sum(templ);
multiply(templ, templ, temp, 1, CV_32F);
templ_sqsum = sum(temp);
float templ_sqsum_sum = 0;
for (int i = 0; i < cn; i ++)
templ_sqsum_sum += templ_sqsum[i] - scale * templ_sum[i] * templ_sum[i];
templ_sum *= scale;
if (templ_sqsum_sum < DBL_EPSILON)
{
result = Scalar::all(1);
return true;
}
if (cn == 2)
k.args(ocl::KernelArg::ReadOnlyNoSize(image_sums), ocl::KernelArg::ReadOnlyNoSize(image_sqsums),
ocl::KernelArg::ReadWrite(result), templ.rows, templ.cols, scale,
templ_sum[0], templ_sum[1], templ_sqsum_sum);
else
k.args(ocl::KernelArg::ReadOnlyNoSize(image_sums), ocl::KernelArg::ReadOnlyNoSize(image_sqsums),
ocl::KernelArg::ReadWrite(result), templ.rows, templ.cols, scale,
templ_sum[0], templ_sum[1], templ_sum[2], templ_sum[3], templ_sqsum_sum);
}
size_t globalsize[2] = { result.cols, result.rows };
return k.run(2, globalsize, NULL, false);
}
/**
* Function that computes the Harris responses in a
* 2*r x 2*r patch at given points in the image
*/
static void
HarrisResponses(InputArray _img, InputArray _diff_x, InputArray _diff_y,
std::vector<KeyPoint>& pts,
OutputArray _response, int r, float harris_k)
{
size_t ptidx, ptsize = pts.size();
// Get mats
Mat img = _img.getMat(), diff_x = _diff_x.getMat(),
diff_y = _diff_y.getMat(), response;
CV_Assert( img.type() == CV_8UC1 );
bool compute_response = _response.needed();
if (compute_response) response = _response.getMat();
const int* dx00 = diff_x.ptr<int>();
const int* dy00 = diff_y.ptr<int>();
float* r00 = response.ptr<float>();
int step = diff_x.step1();
int r_step = response.step1();
for( ptidx = 0; ptidx < ptsize; ptidx++ )
{
float kp_x = pts[ptidx].pt.x;
float kp_y = pts[ptidx].pt.y;
int x0 = (int)kp_x;
int y0 = (int)kp_y;
float xd = 2;
float yd = 2;
//float xd = 0.5;
//float yd = 0.5;
const int* dx0 = dx00 + (y0)*step + x0;
const int* dy0 = dy00 + (y0)*step + x0;
int a = 0, b = 0, c = 0, d = 0;
float* r0 = r00 + ptidx*r_step;
for( int i = -r; i < r; i++ )
{
for( int j = -r; j < r; j++ )
{
const int ofs = i*step + j;
const int* dx = dx0 + ofs;
const int* dy = dy0 + ofs;
const int Ix = (float)dx[-1]*(xd) + (float)dx[0] + (float)dx[1]*xd + (float)dx[-step]*(yd) + (float)dx[step]*yd;
const int Iy = (float)dy[-1]*(xd) + (float)dy[0] + (float)dy[1]*xd + (float)dy[-step]*(yd) + (float)dy[step]*yd;
a += (Ix*Ix);
b += (Iy*Iy);
c += (Ix*Iy);
d += Ix;
}
}
if (compute_response) {
r0[0] = (float)a;
r0[1] = (float)b;
r0[2] = (float)c;
r0[3] = (float)d;
}
else
pts[ptidx].response = ((float)a * b - (float)c * c -
harris_k * ((float)a + b) * ((float)a + b));
}
}
void cv::decolor(InputArray _src, OutputArray _dst, OutputArray _color_boost)
{
Mat I = _src.getMat();
_dst.create(I.size(), CV_8UC1);
Mat dst = _dst.getMat();
_color_boost.create(I.size(), CV_8UC3);
Mat color_boost = _color_boost.getMat();
if(!I.data )
{
cout << "Could not open or find the image" << endl ;
return;
}
if(I.channels() !=3)
{
cout << "Input Color Image" << endl;
return;
}
// Parameter Setting
int maxIter = 15;
int iterCount = 0;
double tol = .0001;
double E = 0;
double pre_E = std::numeric_limits<double>::infinity();
Decolor obj;
Mat img;
img = Mat(I.size(),CV_32FC3);
I.convertTo(img,CV_32FC3,1.0/255.0);
// Initialization
obj.init();
vector <double> Cg;
vector < vector <double> > polyGrad;
vector < vector < int > > comb;
vector <double> alf;
obj.grad_system(img,polyGrad,Cg,comb);
obj.weak_order(img,alf);
// Solver
Mat Mt = Mat(polyGrad.size(),polyGrad[0].size(), CV_32FC1);
obj.wei_update_matrix(polyGrad,Cg,Mt);
vector <double> wei;
obj.wei_inti(comb,wei);
//////////////////////////////// main loop starting ////////////////////////////////////////
while(sqrt(pow(E-pre_E,2)) > tol)
{
iterCount +=1;
pre_E = E;
vector <double> G_pos;
vector <double> G_neg;
vector <double> temp;
vector <double> temp1;
double val = 0.0;
for(unsigned int i=0;i< polyGrad[0].size();i++)
{
val = 0.0;
for(unsigned int j =0;j<polyGrad.size();j++)
val = val + (polyGrad[j][i] * wei[j]);
temp.push_back(val - Cg[i]);
temp1.push_back(val + Cg[i]);
}
double pos = 0.0;
double neg = 0.0;
for(unsigned int i =0;i<alf.size();i++)
{
pos = ((1 + alf[i])/2) * exp((-1.0 * 0.5 * pow(temp[i],2))/pow(obj.sigma,2));
neg = ((1 - alf[i])/2) * exp((-1.0 * 0.5 * pow(temp1[i],2))/pow(obj.sigma,2));
G_pos.push_back(pos);
G_neg.push_back(neg);
}
vector <double> EXPsum;
vector <double> EXPterm;
for(unsigned int i = 0;i<G_pos.size();i++)
EXPsum.push_back(G_pos[i]+G_neg[i]);
vector <double> temp2;
for(unsigned int i=0;i<EXPsum.size();i++)
{
if(EXPsum[i] == 0)
temp2.push_back(1.0);
else
temp2.push_back(0.0);
}
for(unsigned int i =0; i < G_pos.size();i++)
EXPterm.push_back((G_pos[i] - G_neg[i])/(EXPsum[i] + temp2[i]));
double val1 = 0.0;
vector <double> wei1;
for(unsigned int i=0;i< polyGrad.size();i++)
{
val1 = 0.0;
for(unsigned int j =0;j<polyGrad[0].size();j++)
{
val1 = val1 + (Mt.at<float>(i,j) * EXPterm[j]);
}
wei1.push_back(val1);
}
for(unsigned int i =0;i<wei.size();i++)
wei[i] = wei1[i];
E = obj.energyCalcu(Cg,polyGrad,wei);
if(iterCount > maxIter)
break;
G_pos.clear();
G_neg.clear();
temp.clear();
temp1.clear();
EXPsum.clear();
EXPterm.clear();
temp2.clear();
wei1.clear();
}
Mat Gray = Mat::zeros(img.size(),CV_32FC1);
obj.grayImContruct(wei, img, Gray);
Gray.convertTo(dst,CV_8UC1,255);
/////////////////////////////////// Contrast Boosting /////////////////////////////////
Mat lab = Mat(img.size(),CV_8UC3);
Mat color = Mat(img.size(),CV_8UC3);
cvtColor(I,lab,COLOR_BGR2Lab);
vector <Mat> lab_channel;
split(lab,lab_channel);
dst.copyTo(lab_channel[0]);
merge(lab_channel,lab);
cvtColor(lab,color_boost,COLOR_Lab2BGR);
}
void cvtColorInvariants(InputArray _src, OutputArray _dst, int code, int dcn )
{
Mat src = _src.getMat(), dst;
Size sz = src.size();
int scn = src.channels(), depth = src.depth(), bidx;
int blueIdx;
CV_Assert( depth == CV_8U || depth == CV_16U || depth == CV_32F );
switch(code) {
case COLOR_INVARIANCE_PASSTHROUGH:
dst = src;
break;
case COLOR_INVARIANCE_BGR2GAUSSIAN_OPPONENT:
case COLOR_INVARIANCE_RGB2GAUSSIAN_OPPONENT:
CV_Assert( scn == 3 || scn == 4 );
dcn = 3;
blueIdx = (code == COLOR_INVARIANCE_BGR2GAUSSIAN_OPPONENT) ? 0 : 2;
_dst.create( sz, CV_MAKETYPE(depth, dcn));
dst = _dst.getMat();
if( depth == CV_8U )
CvtColorLoop(src, dst, GaussianOpponent<uchar>(scn, dcn, blueIdx,256));
else if( depth == CV_16U )
CvtColorLoop(src, dst, GaussianOpponent<ushort>(scn, dcn, blueIdx, 2<<16 ) );
else
CvtColorLoop(src, dst, GaussianOpponent<float>(scn, dcn, blueIdx, 1 ));
break;
case COLOR_INVARIANCE_BGR2HInvariant:
CV_Assert( scn == 3 || scn == 4 );
dcn = 1;
blueIdx = (code == COLOR_INVARIANCE_BGR2HInvariant) ? 0 : 2;
_dst.create( sz, CV_MAKETYPE(depth, dcn));
dst = _dst.getMat();
if( depth == CV_8U )
CvtColorLoop(src, dst, HInvariant<uchar>(scn, dcn, blueIdx, 256));
else if( depth == CV_16U )
CvtColorLoop(src, dst, HInvariant<ushort>(scn, dcn, blueIdx, 2<<16 ) );
else
CvtColorLoop(src, dst, HInvariant<float>(scn, dcn, blueIdx, 1 ));
break;
case COLOR_INVARIANCE_Gray2YB:
CV_Assert( scn == 1 );
dcn = 3;
_dst.create( sz, CV_MAKETYPE(CV_8U, 3));
dst = _dst.getMat();
if( depth == CV_8U )
CvtColorLoop(src, dst, Gray2YB<uchar>( 256 ) );
else if( depth == CV_16U )
CvtColorLoop(src, dst, Gray2YB<ushort>( 2<<16 ) );
else
CvtColorLoop(src, dst, Gray2YB<float>( 1 ) );
break;
case COLOR_INVARIANCE_Gray2RG:
CV_Assert( scn == 1 );
dcn = 3;
_dst.create( sz, CV_MAKETYPE(CV_8U, 3));
dst = _dst.getMat();
if( depth == CV_8U )
CvtColorLoop(src, dst, Gray2RG<uchar>( 256 ) );
else if( depth == CV_16U )
CvtColorLoop(src, dst, Gray2RG<ushort>( 2<<16 ) );
else
CvtColorLoop(src, dst, Gray2RG<float>( 1 ) );
break;
default:
CV_Error( CV_StsBadFlag, "Unknown/unsupported color conversion code" );
}
}
template <typename _Tp> static
inline void ulbp_(InputArray _src, OutputArray _dst, int radius, int neighbors, vector<int> m_uniform)
{
if (neighbors != 8 || m_uniform.size() != 58)
{
cout << "neighbor must be 8! and uniform size be 58!\n";
system("pause");
exit(-1);
}
//get matrices
Mat src = _src.getMat();
// allocate memory for result
_dst.create(src.rows-2*radius, src.cols-2*radius, CV_32SC1);
Mat dst = _dst.getMat();
// zero
dst.setTo(0);
for(int n=0; n<neighbors; n++)
{
// sample points
float x = static_cast<float>(-radius) * sin(2.0*CV_PI*n/static_cast<float>(neighbors));
float y = static_cast<float>(radius) * cos(2.0*CV_PI*n/static_cast<float>(neighbors));
// relative indices
int fx = static_cast<int>(floor(x));
int fy = static_cast<int>(floor(y));
int cx = static_cast<int>(ceil(x));
int cy = static_cast<int>(ceil(y));
// fractional part
float ty = y - fy;
float tx = x - fx;
// set interpolation weights
float w1 = (1 - tx) * (1 - ty);
float w2 = tx * (1 - ty);
float w3 = (1 - tx) * ty;
float w4 = tx * ty;
// iterate through your data
for(int i=radius; i < src.rows-radius; i++) {
for(int j=radius; j < src.cols-radius; j++) {
// calculate interpolated value
float t = w1*src.at<_Tp>(i+fy,j+fx) + w2*src.at<_Tp>(i+fy,j+cx) + w3*src.at<_Tp>(i+cy,j+fx) + w4*src.at<_Tp>(i+cy,j+cx);
// floating point precision, so check some machine-dependent epsilon
dst.at<int>(i-radius,j-radius) += ((t > src.at<_Tp>(i,j)) ||
(std::abs(t-src.at<_Tp>(i,j)) < std::numeric_limits<float>::epsilon()))
<< n;
}
}
}
for (int i = 0; i < dst.rows; ++i)
{
for (int j = 0; j < dst.cols; ++j)
{
int data = dst.at<int>(i, j);
vector<int>::iterator iter = find(m_uniform.begin(), m_uniform.end(), data);
if (iter == m_uniform.end())
{
dst.at<int>(i, j) = 0;
}
else
{
int new_data = iter - m_uniform.begin() ;
dst.at<int>(i, j) = new_data + 1;
}
}
}
}
bool doTrain(int startStep, OutputArray logLikelihoods, OutputArray labels, OutputArray probs)
{
int dim = trainSamples.cols;
// Precompute the empty initial train data in the cases of START_E_STEP and START_AUTO_STEP
if(startStep != START_M_STEP)
{
if(covs.empty())
{
CV_Assert(weights.empty());
clusterTrainSamples();
}
}
if(!covs.empty() && covsEigenValues.empty() )
{
CV_Assert(invCovsEigenValues.empty());
decomposeCovs();
}
if(startStep == START_M_STEP)
mStep();
double trainLogLikelihood, prevTrainLogLikelihood = 0.;
int maxIters = (termCrit.type & TermCriteria::MAX_ITER) ?
termCrit.maxCount : DEFAULT_MAX_ITERS;
double epsilon = (termCrit.type & TermCriteria::EPS) ? termCrit.epsilon : 0.;
for(int iter = 0; ; iter++)
{
eStep();
trainLogLikelihood = sum(trainLogLikelihoods)[0];
if(iter >= maxIters - 1)
break;
double trainLogLikelihoodDelta = trainLogLikelihood - prevTrainLogLikelihood;
if( iter != 0 &&
(trainLogLikelihoodDelta < -DBL_EPSILON ||
trainLogLikelihoodDelta < epsilon * std::fabs(trainLogLikelihood)))
break;
mStep();
prevTrainLogLikelihood = trainLogLikelihood;
}
if( trainLogLikelihood <= -DBL_MAX/10000. )
{
clear();
return false;
}
// postprocess covs
covs.resize(nclusters);
for(int clusterIndex = 0; clusterIndex < nclusters; clusterIndex++)
{
if(covMatType == COV_MAT_SPHERICAL)
{
covs[clusterIndex].create(dim, dim, CV_64FC1);
setIdentity(covs[clusterIndex], Scalar(covsEigenValues[clusterIndex].at<double>(0)));
}
else if(covMatType == COV_MAT_DIAGONAL)
{
covs[clusterIndex] = Mat::diag(covsEigenValues[clusterIndex]);
}
}
if(labels.needed())
trainLabels.copyTo(labels);
if(probs.needed())
trainProbs.copyTo(probs);
if(logLikelihoods.needed())
trainLogLikelihoods.copyTo(logLikelihoods);
trainSamples.release();
trainProbs.release();
trainLabels.release();
trainLogLikelihoods.release();
return true;
}
void FeatureShiCorner::computeRawCornerMat( InputArray _image, OutputArray _corner )
{
// TODO check: _corner must be CV_32SC1
const Mat image = _image.getMat();
const int height = image.rows;
const int width = image.cols;
const int radius = 1;
Mat derX( height, width, CV_32SC1, Scalar( 0 ) );
Mat derY( height, width, CV_32SC1, Scalar( 0 ) );
Mat Mx2( height, width, CV_32SC1, Scalar( 0 ) );
Mat My2( height, width, CV_32SC1, Scalar( 0 ) );
Mat Mxy( height, width, CV_32SC1, Scalar( 0 ) );
applyFilter< uchar, int32_t >( _image, derX, &filter_derX[0][0], 3, 1, 0, true );
applyFilter< uchar, int32_t >( _image, derY, &filter_derY[0][0], 1, 3, 0, true );
int normDivisor = 0;
const int * pGauss = &FeatureShiCorner::filter_gauss[0][0];
int const * pGaussE = pGauss + 9;
for(; pGauss != pGaussE; pGauss++ )
{
normDivisor += abs( *pGauss );
}
int32_t maxVal = 0;
for( int y = 0; y < height; y++ )
{
for( int x = 0; x < width; x++ )
{
for( int dy = -radius; dy <= radius; dy++ )
{
for( int dx = -radius; dx <= radius; dx++ )
{
int fx = x + dx;
if( (fx < 0) || (fx >= width) ) { continue; }
int fy = y + dy;
if( (fy < 0) || (fy >= height) ) { continue; }
int f = FeatureShiCorner::filter_gauss[(radius + dx)][(radius + dy)];
Mx2.at< int32_t >( y, x ) += int32_t( f * pow( derX.at< int32_t >( fy, fx ), 2 ) );
My2.at< int32_t >( y, x ) += int32_t( f * pow( derY.at< int32_t >( fy, fx ), 2 ) );
Mxy.at< int32_t >( y, x ) += int32_t( f * derX.at< int32_t >( fy, fx ) * derY.at< int >( fy, fx ) );
}
}
Mx2.at< int32_t >( y, x ) /= normDivisor;
My2.at< int32_t >( y, x ) /= normDivisor;
Mxy.at< int32_t >( y, x ) /= normDivisor;
maxVal = max( Mx2.at< int32_t >( y, x ), maxVal );
maxVal = max( My2.at< int32_t >( y, x ), maxVal );
maxVal = max( Mxy.at< int32_t >( y, x ), maxVal );
}
}
Mat corners = _corner.getMat();
const auto it_cE = corners.end< int32_t >();
auto it_cS = corners.begin< int32_t >();
auto it_Mx2S = Mx2.begin< int32_t >();
auto it_My2S = My2.begin< int32_t >();
auto it_MxyS = Mxy.begin< int32_t >();
// reduce to high values if necessary
// maxval: 0..1 * 255^2, maxval^2 should not overflow for the next step
// reduce to sqrt( 2^31-1 (signed int) ) -> 46340
const int maxValC = 46340;
if( maxVal > maxValC )
{
cout << "maxVal > maxValC | maxVal: " << maxVal << endl;
const double scaleFac = maxValC / (double) maxVal; // scaleFac = 0.xxxx
while( it_cS != it_cE )
{
*it_cS *= int32_t( scaleFac );
*it_Mx2S *= int32_t( scaleFac );
*it_My2S *= int32_t( scaleFac );
*it_MxyS *= int32_t( scaleFac );
it_cS++;
it_Mx2S++;
it_My2S++;
it_MxyS++;
}
// reset iterators
it_cS = corners.begin< int32_t >();
it_Mx2S = Mx2.begin< int32_t >();
it_My2S = My2.begin< int32_t >();
it_MxyS = Mxy.begin< int32_t >();
}
maxVal = 0;
// calc eigenvalues
int32_t trc, det;
double ev_sqrt, trc_halve, eigVal1, eigVal2;
while( it_cS != it_cE )
{
trc = *it_Mx2S + *it_My2S;
det = *it_Mx2S * *it_My2S - *it_MxyS * *it_MxyS;
ev_sqrt = sqrt( ( (trc * trc) / 4 ) - det );
trc_halve = trc / 2.0;
eigVal1 = trc_halve + ev_sqrt;
eigVal2 = trc_halve - ev_sqrt;
if( (eigVal1 < 0) || (eigVal2 < 0) )
{
eigVal1 = 0;
eigVal2 = 0;
}
*it_cS = (int32_t) min( eigVal1, eigVal2 );
maxVal = max( (int32_t) min( eigVal1, eigVal2 ), maxVal );
it_cS++;
it_Mx2S++;
it_My2S++;
it_MxyS++;
}
// *
if( maxVal != 0 )
{
const double threshold = maxVal * 0.2;
for( auto it_cE = corners.end< int32_t >(), it_cS = corners.begin< int32_t >(); it_cS != it_cE; it_cS++ )
{
if( *it_cS < threshold ) { *it_cS = 0; }
}
}
// */
// *
Mat cornersFiltered( height, width, CV_32SC1 );
maxFilter< int32_t >( corners, cornersFiltered, 5, 5 );
// */
if( isDebugMode )
{
Mat derXd, derYd, cornersd;
cornersFiltered.convertTo( cornersd, CV_8UC1 );
derX.convertTo( derXd, CV_8UC1 );
derY.convertTo( derYd, CV_8UC1 );
// Display corners over the image (cross)
Mat cornersdc = image.clone();
auto cornerPoints = genPoints( cornersFiltered );
for( auto p : cornerPoints )
{
for( int dx = -2; dx <= 2; dx++ )
{
int x = p.first + dx;
int y = p.second;
if( ( x < 0) || ( x >= width) ) { continue; }
cornersdc.at< uchar >( y, x ) = 0;
}
for( int dy = -2; dy <= 2; dy++ )
{
int x = p.first;
int y = p.second + dy;
if( ( y < 0) || ( y >= height) ) { continue; }
cornersdc.at< uchar >( y, x ) = 0;
}
}
imshow( "image", image );
imshow( "derX", derXd );
imshow( "derY", derYd );
imshow( "Shi Corner", cornersd );
imshow( "Shi Corner Image", cornersdc );
waitKey( 0 );
destroyAllWindows();
waitKey( 1 );
}
}
void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
int iterationsCount, float reprojectionError, int minInliersCount,
OutputArray _inliers, int flags)
{
Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
CV_Assert(opoints.isContinuous());
CV_Assert(opoints.depth() == CV_32F);
CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
CV_Assert(ipoints.isContinuous());
CV_Assert(ipoints.depth() == CV_32F);
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
_rvec.create(3, 1, CV_64FC1);
_tvec.create(3, 1, CV_64FC1);
Mat rvec = _rvec.getMat();
Mat tvec = _tvec.getMat();
Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
if (minInliersCount <= 0)
minInliersCount = objectPoints.cols;
cv::pnpransac::Parameters params;
params.iterationsCount = iterationsCount;
params.minInliersCount = minInliersCount;
params.reprojectionError = reprojectionError;
params.useExtrinsicGuess = useExtrinsicGuess;
params.camera.init(cameraMatrix, distCoeffs);
params.flags = flags;
vector<int> localInliers;
Mat localRvec, localTvec;
rvec.copyTo(localRvec);
tvec.copyTo(localTvec);
if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
{
parallel_for(BlockedRange(0,iterationsCount), cv::pnpransac::PnPSolver(objectPoints, imagePoints, params,
localRvec, localTvec, localInliers));
}
if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
{
if (flags != CV_P3P)
{
int i, pointsCount = (int)localInliers.size();
Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
for (i = 0; i < pointsCount; i++)
{
int index = localInliers[i];
Mat colInlierImagePoints = inlierImagePoints(Rect(i, 0, 1, 1));
imagePoints.col(index).copyTo(colInlierImagePoints);
Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1));
objectPoints.col(index).copyTo(colInlierObjectPoints);
}
solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, true, flags);
}
localRvec.copyTo(rvec);
localTvec.copyTo(tvec);
if (_inliers.needed())
Mat(localInliers).copyTo(_inliers);
}
else
{
tvec.setTo(Scalar(0));
Mat R = Mat::eye(3, 3, CV_64F);
Rodrigues(R, rvec);
if( _inliers.needed() )
_inliers.release();
}
return;
}
bool GrayCodePattern_Impl::decode( InputArrayOfArrays patternImages, OutputArray disparityMap,
InputArrayOfArrays blackImages, InputArrayOfArrays whitheImages, int flags ) const
{
std::vector<std::vector<Mat> >& acquired_pattern = *( std::vector<std::vector<Mat> >* ) patternImages.getObj();
if( flags == DECODE_3D_UNDERWORLD )
{
// Computing shadows mask
std::vector<Mat> shadowMasks;
computeShadowMasks( blackImages, whitheImages, shadowMasks );
int cam_width = acquired_pattern[0][0].cols;
int cam_height = acquired_pattern[0][0].rows;
Point projPixel;
// Storage for the pixels of the two cams that correspond to the same pixel of the projector
std::vector<std::vector<std::vector<Point> > > camsPixels;
camsPixels.resize( acquired_pattern.size() );
// TODO: parallelize for (k and j)
for( size_t k = 0; k < acquired_pattern.size(); k++ )
{
camsPixels[k].resize( params.height * params.width );
for( int i = 0; i < cam_width; i++ )
{
for( int j = 0; j < cam_height; j++ )
{
//if the pixel is not shadowed, reconstruct
if( shadowMasks[k].at<uchar>( j, i ) )
{
//for a (x,y) pixel of the camera returns the corresponding projector pixel by calculating the decimal number
bool error = getProjPixel( acquired_pattern[k], i, j, projPixel );
if( error )
{
continue;
}
camsPixels[k][projPixel.x * params.height + projPixel.y].push_back( Point( i, j ) );
}
}
}
}
std::vector<Point> cam1Pixs, cam2Pixs;
Mat& disparityMap_ = *( Mat* ) disparityMap.getObj();
disparityMap_ = Mat( cam_height, cam_width, CV_64F, double( 0 ) );
double number_of_pixels_cam1 = 0;
double number_of_pixels_cam2 = 0;
for( int i = 0; i < params.width; i++ )
{
for( int j = 0; j < params.height; j++ )
{
cam1Pixs = camsPixels[0][i * params.height + j];
cam2Pixs = camsPixels[1][i * params.height + j];
if( cam1Pixs.size() == 0 || cam2Pixs.size() == 0 )
continue;
Point p1;
Point p2;
double sump1x = 0;
double sump2x = 0;
number_of_pixels_cam1 += cam1Pixs.size();
number_of_pixels_cam2 += cam2Pixs.size();
for( int c1 = 0; c1 < (int) cam1Pixs.size(); c1++ )
{
p1 = cam1Pixs[c1];
sump1x += p1.x;
}
for( int c2 = 0; c2 < (int) cam2Pixs.size(); c2++ )
{
p2 = cam2Pixs[c2];
sump2x += p2.x;
}
sump2x /= cam2Pixs.size();
sump1x /= cam1Pixs.size();
for( int c1 = 0; c1 < (int) cam1Pixs.size(); c1++ )
{
p1 = cam1Pixs[c1];
disparityMap_.at<double>( p1.y, p1.x ) = ( double ) (sump2x - sump1x);
}
sump2x = 0;
sump1x = 0;
}
}
return true;
} // end if flags
return false;
}
static bool ocl_Laplacian5(InputArray _src, OutputArray _dst,
const Mat & kd, const Mat & ks, double scale, double delta,
int borderType, int depth, int ddepth)
{
const size_t tileSizeX = 16;
const size_t tileSizeYmin = 8;
const ocl::Device dev = ocl::Device::getDefault();
int stype = _src.type();
int sdepth = CV_MAT_DEPTH(stype), cn = CV_MAT_CN(stype), esz = CV_ELEM_SIZE(stype);
bool doubleSupport = dev.doubleFPConfig() > 0;
if (!doubleSupport && (sdepth == CV_64F || ddepth == CV_64F))
return false;
Mat kernelX = kd.reshape(1, 1);
if (kernelX.cols % 2 != 1)
return false;
Mat kernelY = ks.reshape(1, 1);
if (kernelY.cols % 2 != 1)
return false;
CV_Assert(kernelX.cols == kernelY.cols);
size_t wgs = dev.maxWorkGroupSize();
size_t lmsz = dev.localMemSize();
size_t src_step = _src.step(), src_offset = _src.offset();
const size_t tileSizeYmax = wgs / tileSizeX;
// workaround for Nvidia: 3 channel vector type takes 4*elem_size in local memory
int loc_mem_cn = dev.vendorID() == ocl::Device::VENDOR_NVIDIA && cn == 3 ? 4 : cn;
if (((src_offset % src_step) % esz == 0) &&
(
(borderType == BORDER_CONSTANT || borderType == BORDER_REPLICATE) ||
((borderType == BORDER_REFLECT || borderType == BORDER_WRAP || borderType == BORDER_REFLECT_101) &&
(_src.cols() >= (int) (kernelX.cols + tileSizeX) && _src.rows() >= (int) (kernelY.cols + tileSizeYmax)))
) &&
(tileSizeX * tileSizeYmin <= wgs) &&
(LAPLACIAN_LOCAL_MEM(tileSizeX, tileSizeYmin, kernelX.cols, loc_mem_cn * 4) <= lmsz)
)
{
Size size = _src.size(), wholeSize;
Point origin;
int dtype = CV_MAKE_TYPE(ddepth, cn);
int wdepth = CV_32F;
size_t tileSizeY = tileSizeYmax;
while ((tileSizeX * tileSizeY > wgs) || (LAPLACIAN_LOCAL_MEM(tileSizeX, tileSizeY, kernelX.cols, loc_mem_cn * 4) > lmsz))
{
tileSizeY /= 2;
}
size_t lt2[2] = { tileSizeX, tileSizeY};
size_t gt2[2] = { lt2[0] * (1 + (size.width - 1) / lt2[0]), lt2[1] };
char cvt[2][40];
const char * const borderMap[] = { "BORDER_CONSTANT", "BORDER_REPLICATE", "BORDER_REFLECT", "BORDER_WRAP",
"BORDER_REFLECT_101" };
String opts = cv::format("-D BLK_X=%d -D BLK_Y=%d -D RADIUS=%d%s%s"
" -D convertToWT=%s -D convertToDT=%s"
" -D %s -D srcT1=%s -D dstT1=%s -D WT1=%s"
" -D srcT=%s -D dstT=%s -D WT=%s"
" -D CN=%d ",
(int)lt2[0], (int)lt2[1], kernelX.cols / 2,
ocl::kernelToStr(kernelX, wdepth, "KERNEL_MATRIX_X").c_str(),
ocl::kernelToStr(kernelY, wdepth, "KERNEL_MATRIX_Y").c_str(),
ocl::convertTypeStr(sdepth, wdepth, cn, cvt[0]),
ocl::convertTypeStr(wdepth, ddepth, cn, cvt[1]),
borderMap[borderType],
ocl::typeToStr(sdepth), ocl::typeToStr(ddepth), ocl::typeToStr(wdepth),
ocl::typeToStr(CV_MAKETYPE(sdepth, cn)),
ocl::typeToStr(CV_MAKETYPE(ddepth, cn)),
ocl::typeToStr(CV_MAKETYPE(wdepth, cn)),
cn);
ocl::Kernel k("laplacian", ocl::imgproc::laplacian5_oclsrc, opts);
if (k.empty())
return false;
UMat src = _src.getUMat();
_dst.create(size, dtype);
UMat dst = _dst.getUMat();
int src_offset_x = static_cast<int>((src_offset % src_step) / esz);
int src_offset_y = static_cast<int>(src_offset / src_step);
src.locateROI(wholeSize, origin);
k.args(ocl::KernelArg::PtrReadOnly(src), (int)src_step, src_offset_x, src_offset_y,
wholeSize.height, wholeSize.width, ocl::KernelArg::WriteOnly(dst),
static_cast<float>(scale), static_cast<float>(delta));
return k.run(2, gt2, lt2, false);
}
int iscale = cvRound(scale), idelta = cvRound(delta);
bool floatCoeff = std::fabs(delta - idelta) > DBL_EPSILON || std::fabs(scale - iscale) > DBL_EPSILON;
int wdepth = std::max(depth, floatCoeff ? CV_32F : CV_32S), kercn = 1;
if (!doubleSupport && wdepth == CV_64F)
return false;
char cvt[2][40];
ocl::Kernel k("sumConvert", ocl::imgproc::laplacian5_oclsrc,
format("-D ONLY_SUM_CONVERT "
"-D srcT=%s -D WT=%s -D dstT=%s -D coeffT=%s -D wdepth=%d "
"-D convertToWT=%s -D convertToDT=%s%s",
ocl::typeToStr(CV_MAKE_TYPE(depth, kercn)),
ocl::typeToStr(CV_MAKE_TYPE(wdepth, kercn)),
ocl::typeToStr(CV_MAKE_TYPE(ddepth, kercn)),
ocl::typeToStr(wdepth), wdepth,
ocl::convertTypeStr(depth, wdepth, kercn, cvt[0]),
ocl::convertTypeStr(wdepth, ddepth, kercn, cvt[1]),
doubleSupport ? " -D DOUBLE_SUPPORT" : ""));
if (k.empty())
return false;
UMat d2x, d2y;
sepFilter2D(_src, d2x, depth, kd, ks, Point(-1, -1), 0, borderType);
sepFilter2D(_src, d2y, depth, ks, kd, Point(-1, -1), 0, borderType);
UMat dst = _dst.getUMat();
ocl::KernelArg d2xarg = ocl::KernelArg::ReadOnlyNoSize(d2x),
d2yarg = ocl::KernelArg::ReadOnlyNoSize(d2y),
dstarg = ocl::KernelArg::WriteOnly(dst, cn, kercn);
if (wdepth >= CV_32F)
k.args(d2xarg, d2yarg, dstarg, (float)scale, (float)delta);
else
k.args(d2xarg, d2yarg, dstarg, iscale, idelta);
size_t globalsize[] = { dst.cols * cn / kercn, dst.rows };
return k.run(2, globalsize, NULL, false);
}
/*******************************************************************************
* Function: subtractBGOpenDiagonal
* Description: BG subtraction via opening with diagonal structuring elements
* Arguments:
inImg - input image
bgsImg - BG subtracted image
threshVal - threshold value for converting to binary image
seLength - length of structuring elements
* Returns: void
* Comments:
* Revision:
*******************************************************************************/
int
FGExtraction::subtractBGOpenDiagonal(InputArray src, OutputArray dst, int threshVal, int seLength)
{
// generate binary image by thresholding
Mat bin;
double thresh = threshold(src, bin, threshVal, 255, THRESH_BINARY);
// opening by horizontal structuring element
//Mat structElemHorizontal = Mat::ones(1, seLength, CV_8U);
//morphologyEx(bin, dst, MORPH_OPEN, structElemHorizontal);
// opening by vertical structuring element
//Mat structElemVertical = Mat::ones(seLength, 1, CV_8U);
//morphologyEx(dst, dst, MORPH_OPEN, structElemVertical);
//imshow("src", src);
//imshow("bin", bin);
//waitKey(0);
// opening by first diagonal structuring element
Mat structElemBackSlash = Mat::eye(seLength, seLength, CV_8U);
morphologyEx(bin, dst, MORPH_OPEN, structElemBackSlash);
//imshow("dst1", dst);
//waitKey(0);
// opening by second diagonal structuring element
Mat structElemSlash;
flip(structElemBackSlash, structElemSlash, 0);
morphologyEx(dst, dst, MORPH_OPEN, structElemSlash);
//imshow("dst2", dst);
//waitKey(0);
// eliminate small noise
Mat structElemEllip = getStructuringElement(MORPH_ELLIPSE, Size(seLength, seLength));
morphologyEx(dst, dst, MORPH_OPEN, structElemEllip);
//imshow("dst3", dst);
//waitKey(0);
// get object size
Mat dstImg = dst.getMat();
vector<vector<Point>> contours = extractContours(dstImg);
if (contours.size()==0)
return 1;
Mat mask = Mat::zeros(_bgsImg.size(), CV_8U);
vector<int> areas(contours.size());
int cnt = 0;
int argMax = 0;
int max_area = 0;
for(vector<vector<Point> >::const_iterator it = contours.begin(); it != contours.end(); ++it){
Rect uprightBox = boundingRect(*it);
areas[cnt] = uprightBox.height*uprightBox.width;
if (areas[cnt]>max_area) {
max_area = areas[cnt];
argMax = cnt;
}
cnt++;
}
vector<Point> largestContour = contours[argMax]; //***** only use the largest contour
RotatedRect orientedBox = orientedBoundingBox(largestContour);
int updateSeL = int(min(orientedBox.size.width, orientedBox.size.height)/5.0+0.5);
// opening by first diagonal structuring element
structElemBackSlash = Mat::eye(updateSeL, updateSeL, CV_8U);
morphologyEx(bin, dst, MORPH_OPEN, structElemBackSlash);
//imshow("dst1", dst);
//waitKey(0);
// opening by second diagonal structuring element
flip(structElemBackSlash, structElemSlash, 0);
morphologyEx(dst, dst, MORPH_OPEN, structElemSlash);
//imshow("dst2", dst);
//waitKey(0);
// eliminate small noise
structElemEllip = getStructuringElement(MORPH_ELLIPSE, Size(updateSeL, updateSeL));
morphologyEx(dst, dst, MORPH_OPEN, structElemEllip);
//imshow("dst3", dst);
//waitKey(0);
return 0;
}
void cv::Laplacian( InputArray _src, OutputArray _dst, int ddepth, int ksize,
double scale, double delta, int borderType )
{
int stype = _src.type(), sdepth = CV_MAT_DEPTH(stype), cn = CV_MAT_CN(stype);
if (ddepth < 0)
ddepth = sdepth;
_dst.create( _src.size(), CV_MAKETYPE(ddepth, cn) );
#ifdef HAVE_IPP
CV_IPP_CHECK()
{
if ((ksize == 3 || ksize == 5) && ((borderType & BORDER_ISOLATED) != 0 || !_src.isSubmatrix()) &&
((stype == CV_8UC1 && ddepth == CV_16S) || (ddepth == CV_32F && stype == CV_32FC1)) && !ocl::useOpenCL())
{
int iscale = saturate_cast<int>(scale), idelta = saturate_cast<int>(delta);
bool floatScale = std::fabs(scale - iscale) > DBL_EPSILON, needScale = iscale != 1;
bool floatDelta = std::fabs(delta - idelta) > DBL_EPSILON, needDelta = delta != 0;
int borderTypeNI = borderType & ~BORDER_ISOLATED;
Mat src = _src.getMat(), dst = _dst.getMat();
if (src.data != dst.data)
{
Ipp32s bufsize;
IppStatus status = (IppStatus)-1;
IppiSize roisize = { src.cols, src.rows };
IppiMaskSize masksize = ksize == 3 ? ippMskSize3x3 : ippMskSize5x5;
IppiBorderType borderTypeIpp = ippiGetBorderType(borderTypeNI);
#define IPP_FILTER_LAPLACIAN(ippsrctype, ippdsttype, ippfavor) \
do \
{ \
if (borderTypeIpp >= 0 && ippiFilterLaplacianGetBufferSize_##ippfavor##_C1R(roisize, masksize, &bufsize) >= 0) \
{ \
Ipp8u * buffer = ippsMalloc_8u(bufsize); \
status = ippiFilterLaplacianBorder_##ippfavor##_C1R(src.ptr<ippsrctype>(), (int)src.step, dst.ptr<ippdsttype>(), \
(int)dst.step, roisize, masksize, borderTypeIpp, 0, buffer); \
ippsFree(buffer); \
} \
} while ((void)0, 0)
CV_SUPPRESS_DEPRECATED_START
if (sdepth == CV_8U && ddepth == CV_16S && !floatScale && !floatDelta)
{
IPP_FILTER_LAPLACIAN(Ipp8u, Ipp16s, 8u16s);
if (needScale && status >= 0)
status = ippiMulC_16s_C1IRSfs((Ipp16s)iscale, dst.ptr<Ipp16s>(), (int)dst.step, roisize, 0);
if (needDelta && status >= 0)
status = ippiAddC_16s_C1IRSfs((Ipp16s)idelta, dst.ptr<Ipp16s>(), (int)dst.step, roisize, 0);
}
else if (sdepth == CV_32F && ddepth == CV_32F)
{
IPP_FILTER_LAPLACIAN(Ipp32f, Ipp32f, 32f);
if (needScale && status >= 0)
status = ippiMulC_32f_C1IR((Ipp32f)scale, dst.ptr<Ipp32f>(), (int)dst.step, roisize);
if (needDelta && status >= 0)
status = ippiAddC_32f_C1IR((Ipp32f)delta, dst.ptr<Ipp32f>(), (int)dst.step, roisize);
}
CV_SUPPRESS_DEPRECATED_END
if (status >= 0)
{
CV_IMPL_ADD(CV_IMPL_IPP);
return;
}
setIppErrorStatus();
}
}
static void divSpectrums( InputArray _srcA, InputArray _srcB, OutputArray _dst, int flags, bool conjB)
{
Mat srcA = _srcA.getMat(), srcB = _srcB.getMat();
int depth = srcA.depth(), cn = srcA.channels(), type = srcA.type();
int rows = srcA.rows, cols = srcA.cols;
int j, k;
CV_Assert( type == srcB.type() && srcA.size() == srcB.size() );
CV_Assert( type == CV_32FC1 || type == CV_32FC2 || type == CV_64FC1 || type == CV_64FC2 );
_dst.create( srcA.rows, srcA.cols, type );
Mat dst = _dst.getMat();
bool is_1d = (flags & DFT_ROWS) || (rows == 1 || (cols == 1 &&
srcA.isContinuous() && srcB.isContinuous() && dst.isContinuous()));
if( is_1d && !(flags & DFT_ROWS) )
cols = cols + rows - 1, rows = 1;
int ncols = cols*cn;
int j0 = cn == 1;
int j1 = ncols - (cols % 2 == 0 && cn == 1);
if( depth == CV_32F )
{
const float* dataA = srcA.ptr<float>();
const float* dataB = srcB.ptr<float>();
float* dataC = dst.ptr<float>();
float eps = FLT_EPSILON; // prevent div0 problems
size_t stepA = srcA.step/sizeof(dataA[0]);
size_t stepB = srcB.step/sizeof(dataB[0]);
size_t stepC = dst.step/sizeof(dataC[0]);
if( !is_1d && cn == 1 )
{
for( k = 0; k < (cols % 2 ? 1 : 2); k++ )
{
if( k == 1 )
dataA += cols - 1, dataB += cols - 1, dataC += cols - 1;
dataC[0] = dataA[0] / (dataB[0] + eps);
if( rows % 2 == 0 )
dataC[(rows-1)*stepC] = dataA[(rows-1)*stepA] / (dataB[(rows-1)*stepB] + eps);
if( !conjB )
for( j = 1; j <= rows - 2; j += 2 )
{
double denom = (double)dataB[j*stepB]*dataB[j*stepB] +
(double)dataB[(j+1)*stepB]*dataB[(j+1)*stepB] + (double)eps;
double re = (double)dataA[j*stepA]*dataB[j*stepB] +
(double)dataA[(j+1)*stepA]*dataB[(j+1)*stepB];
double im = (double)dataA[(j+1)*stepA]*dataB[j*stepB] -
(double)dataA[j*stepA]*dataB[(j+1)*stepB];
dataC[j*stepC] = (float)(re / denom);
dataC[(j+1)*stepC] = (float)(im / denom);
}
else
for( j = 1; j <= rows - 2; j += 2 )
{
double denom = (double)dataB[j*stepB]*dataB[j*stepB] +
(double)dataB[(j+1)*stepB]*dataB[(j+1)*stepB] + (double)eps;
double re = (double)dataA[j*stepA]*dataB[j*stepB] -
(double)dataA[(j+1)*stepA]*dataB[(j+1)*stepB];
double im = (double)dataA[(j+1)*stepA]*dataB[j*stepB] +
(double)dataA[j*stepA]*dataB[(j+1)*stepB];
dataC[j*stepC] = (float)(re / denom);
dataC[(j+1)*stepC] = (float)(im / denom);
}
if( k == 1 )
dataA -= cols - 1, dataB -= cols - 1, dataC -= cols - 1;
}
}
for( ; rows--; dataA += stepA, dataB += stepB, dataC += stepC )
{
if( is_1d && cn == 1 )
{
dataC[0] = dataA[0] / (dataB[0] + eps);
if( cols % 2 == 0 )
dataC[j1] = dataA[j1] / (dataB[j1] + eps);
}
if( !conjB )
for( j = j0; j < j1; j += 2 )
{
double denom = (double)(dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps);
double re = (double)(dataA[j]*dataB[j] + dataA[j+1]*dataB[j+1]);
double im = (double)(dataA[j+1]*dataB[j] - dataA[j]*dataB[j+1]);
dataC[j] = (float)(re / denom);
dataC[j+1] = (float)(im / denom);
}
else
for( j = j0; j < j1; j += 2 )
{
double denom = (double)(dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps);
double re = (double)(dataA[j]*dataB[j] - dataA[j+1]*dataB[j+1]);
double im = (double)(dataA[j+1]*dataB[j] + dataA[j]*dataB[j+1]);
dataC[j] = (float)(re / denom);
dataC[j+1] = (float)(im / denom);
}
}
}
else
{
const double* dataA = srcA.ptr<double>();
const double* dataB = srcB.ptr<double>();
double* dataC = dst.ptr<double>();
double eps = DBL_EPSILON; // prevent div0 problems
size_t stepA = srcA.step/sizeof(dataA[0]);
size_t stepB = srcB.step/sizeof(dataB[0]);
size_t stepC = dst.step/sizeof(dataC[0]);
if( !is_1d && cn == 1 )
{
for( k = 0; k < (cols % 2 ? 1 : 2); k++ )
{
if( k == 1 )
dataA += cols - 1, dataB += cols - 1, dataC += cols - 1;
dataC[0] = dataA[0] / (dataB[0] + eps);
if( rows % 2 == 0 )
dataC[(rows-1)*stepC] = dataA[(rows-1)*stepA] / (dataB[(rows-1)*stepB] + eps);
if( !conjB )
for( j = 1; j <= rows - 2; j += 2 )
{
double denom = dataB[j*stepB]*dataB[j*stepB] +
dataB[(j+1)*stepB]*dataB[(j+1)*stepB] + eps;
double re = dataA[j*stepA]*dataB[j*stepB] +
dataA[(j+1)*stepA]*dataB[(j+1)*stepB];
double im = dataA[(j+1)*stepA]*dataB[j*stepB] -
dataA[j*stepA]*dataB[(j+1)*stepB];
dataC[j*stepC] = re / denom;
dataC[(j+1)*stepC] = im / denom;
}
else
for( j = 1; j <= rows - 2; j += 2 )
{
double denom = dataB[j*stepB]*dataB[j*stepB] +
dataB[(j+1)*stepB]*dataB[(j+1)*stepB] + eps;
double re = dataA[j*stepA]*dataB[j*stepB] -
dataA[(j+1)*stepA]*dataB[(j+1)*stepB];
double im = dataA[(j+1)*stepA]*dataB[j*stepB] +
dataA[j*stepA]*dataB[(j+1)*stepB];
dataC[j*stepC] = re / denom;
dataC[(j+1)*stepC] = im / denom;
}
if( k == 1 )
dataA -= cols - 1, dataB -= cols - 1, dataC -= cols - 1;
}
}
for( ; rows--; dataA += stepA, dataB += stepB, dataC += stepC )
{
if( is_1d && cn == 1 )
{
dataC[0] = dataA[0] / (dataB[0] + eps);
if( cols % 2 == 0 )
dataC[j1] = dataA[j1] / (dataB[j1] + eps);
}
if( !conjB )
for( j = j0; j < j1; j += 2 )
{
double denom = dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps;
double re = dataA[j]*dataB[j] + dataA[j+1]*dataB[j+1];
double im = dataA[j+1]*dataB[j] - dataA[j]*dataB[j+1];
dataC[j] = re / denom;
dataC[j+1] = im / denom;
}
else
for( j = j0; j < j1; j += 2 )
{
double denom = dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps;
double re = dataA[j]*dataB[j] - dataA[j+1]*dataB[j+1];
double im = dataA[j+1]*dataB[j] + dataA[j]*dataB[j+1];
dataC[j] = re / denom;
dataC[j+1] = im / denom;
}
}
}
}
static void getSobelKernels( OutputArray _kx, OutputArray _ky,
int dx, int dy, int _ksize, bool normalize, int ktype )
{
int i, j, ksizeX = _ksize, ksizeY = _ksize;
if( ksizeX == 1 && dx > 0 )
ksizeX = 3;
if( ksizeY == 1 && dy > 0 )
ksizeY = 3;
CV_Assert( ktype == CV_32F || ktype == CV_64F );
_kx.create(ksizeX, 1, ktype, -1, true);
_ky.create(ksizeY, 1, ktype, -1, true);
Mat kx = _kx.getMat();
Mat ky = _ky.getMat();
if( _ksize % 2 == 0 || _ksize > 31 )
CV_Error( CV_StsOutOfRange, "The kernel size must be odd and not larger than 31" );
std::vector<int> kerI(std::max(ksizeX, ksizeY) + 1);
CV_Assert( dx >= 0 && dy >= 0 && dx+dy > 0 );
for( int k = 0; k < 2; k++ )
{
Mat* kernel = k == 0 ? &kx : &ky;
int order = k == 0 ? dx : dy;
int ksize = k == 0 ? ksizeX : ksizeY;
CV_Assert( ksize > order );
if( ksize == 1 )
kerI[0] = 1;
else if( ksize == 3 )
{
if( order == 0 )
kerI[0] = 1, kerI[1] = 2, kerI[2] = 1;
else if( order == 1 )
kerI[0] = -1, kerI[1] = 0, kerI[2] = 1;
else
kerI[0] = 1, kerI[1] = -2, kerI[2] = 1;
}
else
{
int oldval, newval;
kerI[0] = 1;
for( i = 0; i < ksize; i++ )
kerI[i+1] = 0;
for( i = 0; i < ksize - order - 1; i++ )
{
oldval = kerI[0];
for( j = 1; j <= ksize; j++ )
{
newval = kerI[j]+kerI[j-1];
kerI[j-1] = oldval;
oldval = newval;
}
}
for( i = 0; i < order; i++ )
{
oldval = -kerI[0];
for( j = 1; j <= ksize; j++ )
{
newval = kerI[j-1] - kerI[j];
kerI[j-1] = oldval;
oldval = newval;
}
}
}
Mat temp(kernel->rows, kernel->cols, CV_32S, &kerI[0]);
double scale = !normalize ? 1. : 1./(1 << (ksize-order-1));
temp.convertTo(*kernel, ktype, scale);
}
}
void cv::Laplacian( InputArray _src, OutputArray _dst, int ddepth, int ksize,
double scale, double delta, int borderType )
{
int stype = _src.type(), sdepth = CV_MAT_DEPTH(stype), cn = CV_MAT_CN(stype);
if (ddepth < 0)
ddepth = sdepth;
_dst.create( _src.size(), CV_MAKETYPE(ddepth, cn) );
#ifdef HAVE_TEGRA_OPTIMIZATION
if (scale == 1.0 && delta == 0)
{
Mat src = _src.getMat(), dst = _dst.getMat();
if (ksize == 1 && tegra::laplace1(src, dst, borderType))
return;
if (ksize == 3 && tegra::laplace3(src, dst, borderType))
return;
if (ksize == 5 && tegra::laplace5(src, dst, borderType))
return;
}
#endif
if( ksize == 1 || ksize == 3 )
{
float K[2][9] =
{
{ 0, 1, 0, 1, -4, 1, 0, 1, 0 },
{ 2, 0, 2, 0, -8, 0, 2, 0, 2 }
};
Mat kernel(3, 3, CV_32F, K[ksize == 3]);
if( scale != 1 )
kernel *= scale;
filter2D( _src, _dst, ddepth, kernel, Point(-1, -1), delta, borderType );
}
else
{
Mat src = _src.getMat(), dst = _dst.getMat();
const size_t STRIPE_SIZE = 1 << 14;
int depth = src.depth();
int ktype = std::max(CV_32F, std::max(ddepth, depth));
int wdepth = depth == CV_8U && ksize <= 5 ? CV_16S : depth <= CV_32F ? CV_32F : CV_64F;
int wtype = CV_MAKETYPE(wdepth, src.channels());
Mat kd, ks;
getSobelKernels( kd, ks, 2, 0, ksize, false, ktype );
if( ddepth < 0 )
ddepth = src.depth();
int dtype = CV_MAKETYPE(ddepth, src.channels());
int dy0 = std::min(std::max((int)(STRIPE_SIZE/(getElemSize(src.type())*src.cols)), 1), src.rows);
Ptr<FilterEngine> fx = createSeparableLinearFilter(src.type(),
wtype, kd, ks, Point(-1,-1), 0, borderType, borderType, Scalar() );
Ptr<FilterEngine> fy = createSeparableLinearFilter(src.type(),
wtype, ks, kd, Point(-1,-1), 0, borderType, borderType, Scalar() );
int y = fx->start(src), dsty = 0, dy = 0;
fy->start(src);
const uchar* sptr = src.data + y*src.step;
Mat d2x( dy0 + kd.rows - 1, src.cols, wtype );
Mat d2y( dy0 + kd.rows - 1, src.cols, wtype );
for( ; dsty < src.rows; sptr += dy0*src.step, dsty += dy )
{
fx->proceed( sptr, (int)src.step, dy0, d2x.data, (int)d2x.step );
dy = fy->proceed( sptr, (int)src.step, dy0, d2y.data, (int)d2y.step );
if( dy > 0 )
{
Mat dstripe = dst.rowRange(dsty, dsty + dy);
d2x.rows = d2y.rows = dy; // modify the headers, which should work
d2x += d2y;
d2x.convertTo( dstripe, dtype, scale, delta );
}
}
}
}
bool CustomPattern::findPatternPass(const Mat& image, vector<Point2f>& matched_features, vector<Point3f>& pattern_points,
Mat& H, vector<Point2f>& scene_corners, const double pratio, const double proj_error,
const bool refine_position, const Mat& mask, OutputArray output)
{
if (!initialized) {return false; }
matched_features.clear();
pattern_points.clear();
vector<vector<DMatch> > matches;
vector<KeyPoint> f_keypoints;
Mat f_descriptor;
detector->detect(image, f_keypoints, mask);
if (refine_position) refineKeypointsPos(image, f_keypoints);
descriptorExtractor->compute(image, f_keypoints, f_descriptor);
descriptorMatcher->knnMatch(f_descriptor, descriptor, matches, 2); // k = 2;
vector<DMatch> good_matches;
vector<Point2f> obj_points;
for(int i = 0; i < f_descriptor.rows; ++i)
{
if(matches[i][0].distance < pratio * matches[i][1].distance)
{
const DMatch& dm = matches[i][0];
good_matches.push_back(dm);
// "keypoints1[matches[i].queryIdx] has a corresponding point in keypoints2[matches[i].trainIdx]"
matched_features.push_back(f_keypoints[dm.queryIdx].pt);
pattern_points.push_back(points3d[dm.trainIdx]);
obj_points.push_back(keypoints[dm.trainIdx].pt);
}
}
if (good_matches.size() < MIN_POINTS_FOR_H) return false;
Mat h_mask;
H = findHomography(obj_points, matched_features, RANSAC, proj_error, h_mask);
if (H.empty())
{
// cout << "findHomography() returned empty Mat." << endl;
return false;
}
for(unsigned int i = 0; i < good_matches.size(); ++i)
{
if(!h_mask.data[i])
{
deleteStdVecElem(good_matches, i);
deleteStdVecElem(matched_features, i);
deleteStdVecElem(pattern_points, i);
}
}
if (good_matches.empty()) return false;
size_t numb_elem = good_matches.size();
check_matches(matched_features, obj_points, good_matches, pattern_points, H);
if (good_matches.empty() || numb_elem < good_matches.size()) return false;
// Get the corners from the image
scene_corners = vector<Point2f>(4);
perspectiveTransform(obj_corners, scene_corners, H);
// Check correctnes of H
// Is it a convex hull?
bool cConvex = isContourConvex(scene_corners);
if (!cConvex) return false;
// Is the hull too large or small?
double scene_area = contourArea(scene_corners);
if (scene_area < MIN_CONTOUR_AREA_PX) return false;
double ratio = scene_area/img_roi.size().area();
if ((ratio < MIN_CONTOUR_AREA_RATIO) ||
(ratio > MAX_CONTOUR_AREA_RATIO)) return false;
// Is any of the projected points outside the hull?
for(unsigned int i = 0; i < good_matches.size(); ++i)
{
if(pointPolygonTest(scene_corners, f_keypoints[good_matches[i].queryIdx].pt, false) < 0)
{
deleteStdVecElem(good_matches, i);
deleteStdVecElem(matched_features, i);
deleteStdVecElem(pattern_points, i);
}
}
if (output.needed())
{
Mat out;
drawMatches(image, f_keypoints, img_roi, keypoints, good_matches, out);
// Draw lines between the corners (the mapped object in the scene - image_2 )
line(out, scene_corners[0], scene_corners[1], Scalar(0, 255, 0), 2);
line(out, scene_corners[1], scene_corners[2], Scalar(0, 255, 0), 2);
line(out, scene_corners[2], scene_corners[3], Scalar(0, 255, 0), 2);
line(out, scene_corners[3], scene_corners[0], Scalar(0, 255, 0), 2);
out.copyTo(output);
}
return (!good_matches.empty()); // return true if there are enough good matches
}
// Reconstruction function for API
void
reconstruct(InputArrayOfArrays points2d, OutputArray Ps, OutputArray points3d, InputOutputArray K,
bool is_projective)
{
const int nviews = points2d.total();
CV_Assert( nviews >= 2 );
// OpenCV data types
std::vector<Mat> pts2d;
points2d.getMatVector(pts2d);
const int depth = pts2d[0].depth();
Matx33d Ka = K.getMat();
// Projective reconstruction
if (is_projective)
{
if ( nviews == 2 )
{
// Get Projection matrices
Matx33d F;
Matx34d P, Pp;
normalizedEightPointSolver(pts2d[0], pts2d[1], F);
projectionsFromFundamental(F, P, Pp);
Ps.create(2, 1, depth);
Mat(P).copyTo(Ps.getMatRef(0));
Mat(Pp).copyTo(Ps.getMatRef(1));
// Triangulate and find 3D points using inliers
triangulatePoints(points2d, Ps, points3d);
}
else
{
std::vector<Mat> Rs, Ts;
reconstruct(points2d, Rs, Ts, Ka, points3d, is_projective);
// From Rs and Ts, extract Ps
const int nviews = Rs.size();
Ps.create(nviews, 1, depth);
Matx34d P;
for (size_t i = 0; i < nviews; ++i)
{
projectionFromKRt(Ka, Rs[i], Vec3d(Ts[i]), P);
Mat(P).copyTo(Ps.getMatRef(i));
}
Mat(Ka).copyTo(K.getMat());
}
}
// Affine reconstruction
else
{
// TODO: implement me
CV_Error(Error::StsNotImplemented, "Affine reconstruction not yet implemented");
}
}
