template<class CastOp, class VecOp> void
pyrDown_( const Mat& _src, Mat& _dst, int borderType )
{
const int PD_SZ = 5;
typedef typename CastOp::type1 WT;
typedef typename CastOp::rtype T;
CV_Assert( !_src.empty() );
Size ssize = _src.size(), dsize = _dst.size();
int cn = _src.channels();
int bufstep = (int)alignSize(dsize.width*cn, 16);
AutoBuffer<WT> _buf(bufstep*PD_SZ + 16);
WT* buf = alignPtr((WT*)_buf, 16);
int tabL[CV_CN_MAX*(PD_SZ+2)], tabR[CV_CN_MAX*(PD_SZ+2)];
AutoBuffer<int> _tabM(dsize.width*cn);
int* tabM = _tabM;
WT* rows[PD_SZ];
CastOp castOp;
VecOp vecOp;
CV_Assert( std::abs(dsize.width*2 - ssize.width) <= 2 &&
std::abs(dsize.height*2 - ssize.height) <= 2 );
int k, x, sy0 = -PD_SZ/2, sy = sy0, width0 = std::min((ssize.width-PD_SZ/2-1)/2 + 1, dsize.width);
for( x = 0; x <= PD_SZ+1; x++ )
{
int sx0 = borderInterpolate(x - PD_SZ/2, ssize.width, borderType)*cn;
int sx1 = borderInterpolate(x + width0*2 - PD_SZ/2, ssize.width, borderType)*cn;
for( k = 0; k < cn; k++ )
{
tabL[x*cn + k] = sx0 + k;
tabR[x*cn + k] = sx1 + k;
}
}
ssize.width *= cn;
dsize.width *= cn;
width0 *= cn;
for( x = 0; x < dsize.width; x++ )
tabM[x] = (x/cn)*2*cn + x % cn;
for( int y = 0; y < dsize.height; y++ )
{
T* dst = (T*)(_dst.data + _dst.step*y);
WT *row0, *row1, *row2, *row3, *row4;
// fill the ring buffer (horizontal convolution and decimation)
for( ; sy <= y*2 + 2; sy++ )
{
WT* row = buf + ((sy - sy0) % PD_SZ)*bufstep;
int _sy = borderInterpolate(sy, ssize.height, borderType);
const T* src = (const T*)(_src.data + _src.step*_sy);
int limit = cn;
const int* tab = tabL;
for( x = 0;;)
{
for( ; x < limit; x++ )
{
row[x] = src[tab[x+cn*2]]*6 + (src[tab[x+cn]] + src[tab[x+cn*3]])*4 +
src[tab[x]] + src[tab[x+cn*4]];
}
if( x == dsize.width )
break;
if( cn == 1 )
{
for( ; x < width0; x++ )
row[x] = src[x*2]*6 + (src[x*2 - 1] + src[x*2 + 1])*4 +
src[x*2 - 2] + src[x*2 + 2];
}
else if( cn == 3 )
{
for( ; x < width0; x += 3 )
{
const T* s = src + x*2;
WT t0 = s[0]*6 + (s[-3] + s[3])*4 + s[-6] + s[6];
WT t1 = s[1]*6 + (s[-2] + s[4])*4 + s[-5] + s[7];
WT t2 = s[2]*6 + (s[-1] + s[5])*4 + s[-4] + s[8];
row[x] = t0; row[x+1] = t1; row[x+2] = t2;
}
}
else if( cn == 4 )
{
for( ; x < width0; x += 4 )
{
const T* s = src + x*2;
WT t0 = s[0]*6 + (s[-4] + s[4])*4 + s[-8] + s[8];
WT t1 = s[1]*6 + (s[-3] + s[5])*4 + s[-7] + s[9];
row[x] = t0; row[x+1] = t1;
t0 = s[2]*6 + (s[-2] + s[6])*4 + s[-6] + s[10];
t1 = s[3]*6 + (s[-1] + s[7])*4 + s[-5] + s[11];
row[x+2] = t0; row[x+3] = t1;
}
}
else
{
for( ; x < width0; x++ )
{
int sx = tabM[x];
row[x] = src[sx]*6 + (src[sx - cn] + src[sx + cn])*4 +
src[sx - cn*2] + src[sx + cn*2];
}
}
limit = dsize.width;
tab = tabR - x;
}
}
// do vertical convolution and decimation and write the result to the destination image
for( k = 0; k < PD_SZ; k++ )
rows[k] = buf + ((y*2 - PD_SZ/2 + k - sy0) % PD_SZ)*bufstep;
row0 = rows[0]; row1 = rows[1]; row2 = rows[2]; row3 = rows[3]; row4 = rows[4];
x = vecOp(rows, dst, (int)_dst.step, dsize.width);
for( ; x < dsize.width; x++ )
dst[x] = castOp(row2[x]*6 + (row1[x] + row3[x])*4 + row0[x] + row4[x]);
}
}
template<class CastOp, class VecOp> void
pyrUp_( const Mat& _src, Mat& _dst, int)
{
const int PU_SZ = 3;
typedef typename CastOp::type1 WT;
typedef typename CastOp::rtype T;
Size ssize = _src.size(), dsize = _dst.size();
int cn = _src.channels();
int bufstep = (int)alignSize((dsize.width+1)*cn, 16);
AutoBuffer<WT> _buf(bufstep*PU_SZ + 16);
WT* buf = alignPtr((WT*)_buf, 16);
AutoBuffer<int> _dtab(ssize.width*cn);
int* dtab = _dtab;
WT* rows[PU_SZ];
CastOp castOp;
VecOp vecOp;
CV_Assert( std::abs(dsize.width - ssize.width*2) == dsize.width % 2 &&
std::abs(dsize.height - ssize.height*2) == dsize.height % 2);
int k, x, sy0 = -PU_SZ/2, sy = sy0;
ssize.width *= cn;
dsize.width *= cn;
for( x = 0; x < ssize.width; x++ )
dtab[x] = (x/cn)*2*cn + x % cn;
for( int y = 0; y < ssize.height; y++ )
{
T* dst0 = (T*)(_dst.data + _dst.step*y*2);
T* dst1 = (T*)(_dst.data + _dst.step*(y*2+1));
WT *row0, *row1, *row2;
if( y*2+1 >= dsize.height )
dst1 = dst0;
// fill the ring buffer (horizontal convolution and decimation)
for( ; sy <= y + 1; sy++ )
{
WT* row = buf + ((sy - sy0) % PU_SZ)*bufstep;
int _sy = borderInterpolate(sy*2, dsize.height, BORDER_REFLECT_101)/2;
const T* src = (const T*)(_src.data + _src.step*_sy);
if( ssize.width == cn )
{
for( x = 0; x < cn; x++ )
row[x] = row[x + cn] = src[x]*8;
continue;
}
for( x = 0; x < cn; x++ )
{
int dx = dtab[x];
WT t0 = src[x]*6 + src[x + cn]*2;
WT t1 = (src[x] + src[x + cn])*4;
row[dx] = t0; row[dx + cn] = t1;
dx = dtab[ssize.width - cn + x];
int sx = ssize.width - cn + x;
t0 = src[sx - cn] + src[sx]*7;
t1 = src[sx]*8;
row[dx] = t0; row[dx + cn] = t1;
}
for( x = cn; x < ssize.width - cn; x++ )
{
int dx = dtab[x];
WT t0 = src[x-cn] + src[x]*6 + src[x+cn];
WT t1 = (src[x] + src[x+cn])*4;
row[dx] = t0;
row[dx+cn] = t1;
}
}
// do vertical convolution and decimation and write the result to the destination image
for( k = 0; k < PU_SZ; k++ )
rows[k] = buf + ((y - PU_SZ/2 + k - sy0) % PU_SZ)*bufstep;
row0 = rows[0]; row1 = rows[1]; row2 = rows[2];
x = vecOp(rows, dst0, (int)_dst.step, dsize.width);
for( ; x < dsize.width; x++ )
{
T t1 = castOp((row1[x] + row2[x])*4);
T t0 = castOp(row0[x] + row1[x]*6 + row2[x]);
dst1[x] = t1; dst0[x] = t0;
}
}
}
/*
* Print reply error info
*/
char *
clnt_sperror(
CLIENT *rpch,
char *s)
{
struct rpc_err e;
char *err;
char *str = _buf();
char *strstart = str;
if (str == 0)
return (0);
CLNT_GETERR(rpch, &e);
(void) sprintf(str, "%s: ", s);
str += strlen(str);
(void) strcpy(str, clnt_sperrno(e.re_status));
str += strlen(str);
switch (e.re_status) {
case RPC_SUCCESS:
case RPC_CANTENCODEARGS:
case RPC_CANTDECODERES:
case RPC_TIMEDOUT:
case RPC_PROGUNAVAIL:
case RPC_PROCUNAVAIL:
case RPC_CANTDECODEARGS:
case RPC_SYSTEMERROR:
case RPC_UNKNOWNHOST:
case RPC_UNKNOWNPROTO:
case RPC_PMAPFAILURE:
case RPC_PROGNOTREGISTERED:
case RPC_FAILED:
break;
case RPC_CANTSEND:
case RPC_CANTRECV:
(void) sprintf(str, "; errno = %s",
strerror(e.re_errno));
str += strlen(str);
break;
case RPC_VERSMISMATCH:
(void) sprintf(str,
"; low version = %lu, high version = %lu",
e.re_vers.low, e.re_vers.high);
str += strlen(str);
break;
case RPC_AUTHERROR:
err = auth_errmsg(e.re_why);
(void) sprintf(str,"; why = ");
str += strlen(str);
if (err != NULL) {
(void) sprintf(str, "%s",err);
} else {
(void) sprintf(str,
"(unknown authentication error - %d)",
(int) e.re_why);
}
str += strlen(str);
break;
case RPC_PROGVERSMISMATCH:
(void) sprintf(str,
"; low version = %lu, high version = %lu",
e.re_vers.low, e.re_vers.high);
str += strlen(str);
break;
default: /* unknown */
(void) sprintf(str,
"; s1 = %lu, s2 = %lu",
e.re_lb.s1, e.re_lb.s2);
str += strlen(str);
break;
}
(void) sprintf(str, "\n");
return(strstart) ;
}
int CvMLData::read_csv(const char* filename)
{
const int M = 1000000;
const char str_delimiter[3] = { ' ', delimiter, '\0' };
FILE* file = 0;
CvMemStorage* storage;
CvSeq* seq;
char *ptr;
float* el_ptr;
CvSeqReader reader;
int cols_count = 0;
uchar *var_types_ptr = 0;
clear();
file = fopen( filename, "rt" );
if( !file )
return -1;
// read the first line and determine the number of variables
std::vector<char> _buf(M);
char* buf = &_buf[0];
if( !fgets_chomp( buf, M, file ))
{
fclose(file);
return -1;
}
for( ptr = buf; *ptr != '\0'; ptr++ )
cols_count += (*ptr == delimiter);
if ( cols_count == 0)
{
fclose(file);
return -1;
}
cols_count++;
// create temporary memory storage to store the whole database
el_ptr = new float[cols_count];
storage = cvCreateMemStorage();
seq = cvCreateSeq( 0, sizeof(*seq), cols_count*sizeof(float), storage );
var_types = cvCreateMat( 1, cols_count, CV_8U );
cvZero( var_types );
var_types_ptr = var_types->data.ptr;
for(;;)
{
char *token = NULL;
int type;
token = strtok(buf, str_delimiter);
if (!token)
{
fclose(file);
return -1;
}
for (int i = 0; i < cols_count-1; i++)
{
str_to_flt_elem( token, el_ptr[i], type);
var_types_ptr[i] |= type;
token = strtok(NULL, str_delimiter);
if (!token)
{
fclose(file);
return -1;
}
}
str_to_flt_elem( token, el_ptr[cols_count-1], type);
var_types_ptr[cols_count-1] |= type;
cvSeqPush( seq, el_ptr );
if( !fgets_chomp( buf, M, file ) || !strchr( buf, delimiter ) )
break;
}
fclose(file);
values = cvCreateMat( seq->total, cols_count, CV_32FC1 );
missing = cvCreateMat( seq->total, cols_count, CV_8U );
var_idx_mask = cvCreateMat( 1, values->cols, CV_8UC1 );
cvSet( var_idx_mask, cvRealScalar(1) );
train_sample_count = seq->total;
cvStartReadSeq( seq, &reader );
for(int i = 0; i < seq->total; i++ )
{
const float* sdata = (float*)reader.ptr;
float* ddata = values->data.fl + cols_count*i;
uchar* dm = missing->data.ptr + cols_count*i;
for( int j = 0; j < cols_count; j++ )
{
ddata[j] = sdata[j];
dm[j] = ( fabs( MISS_VAL - sdata[j] ) <= FLT_EPSILON );
}
CV_NEXT_SEQ_ELEM( seq->elem_size, reader );
}
if ( cvNorm( missing, 0, CV_L1 ) <= FLT_EPSILON )
cvReleaseMat( &missing );
cvReleaseMemStorage( &storage );
delete []el_ptr;
return 0;
}
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 = getMergeFunc(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 t = 0; t < cn; t++ )
ptrs[t+1] += bsz*esz1;
}
}
}
}
void FAST_t(InputArray _img, std::vector<KeyPoint>& keypoints, int threshold, bool nonmax_suppression)
{
Mat img = _img.getMat();
const int K = patternSize/2, N = patternSize + K + 1;
#if CV_SSE2
const int quarterPatternSize = patternSize/4;
(void)quarterPatternSize;
#endif
int i, j, k, pixel[25];
makeOffsets(pixel, (int)img.step, patternSize);
keypoints.clear();
threshold = std::min(std::max(threshold, 0), 255);
#if CV_SSE2
__m128i delta = _mm_set1_epi8(-128), t = _mm_set1_epi8((char)threshold), K16 = _mm_set1_epi8((char)K);
(void)K16;
(void)delta;
(void)t;
#endif
uchar threshold_tab[512];
for( i = -255; i <= 255; i++ )
threshold_tab[i+255] = (uchar)(i < -threshold ? 1 : i > threshold ? 2 : 0);
AutoBuffer<uchar> _buf((img.cols+16)*3*(sizeof(int) + sizeof(uchar)) + 128);
uchar* buf[3];
buf[0] = _buf; buf[1] = buf[0] + img.cols; buf[2] = buf[1] + img.cols;
int* cpbuf[3];
cpbuf[0] = (int*)alignPtr(buf[2] + img.cols, sizeof(int)) + 1;
cpbuf[1] = cpbuf[0] + img.cols + 1;
cpbuf[2] = cpbuf[1] + img.cols + 1;
memset(buf[0], 0, img.cols*3);
for(i = 3; i < img.rows-2; i++)
{
const uchar* ptr = img.ptr<uchar>(i) + 3;
uchar* curr = buf[(i - 3)%3];
int* cornerpos = cpbuf[(i - 3)%3];
memset(curr, 0, img.cols);
int ncorners = 0;
if( i < img.rows - 3 )
{
j = 3;
#if CV_SSE2
if( patternSize == 16 )
{
for(; j < img.cols - 16 - 3; j += 16, ptr += 16)
{
__m128i m0, m1;
__m128i v0 = _mm_loadu_si128((const __m128i*)ptr);
__m128i v1 = _mm_xor_si128(_mm_subs_epu8(v0, t), delta);
v0 = _mm_xor_si128(_mm_adds_epu8(v0, t), delta);
__m128i x0 = _mm_sub_epi8(_mm_loadu_si128((const __m128i*)(ptr + pixel[0])), delta);
__m128i x1 = _mm_sub_epi8(_mm_loadu_si128((const __m128i*)(ptr + pixel[quarterPatternSize])), delta);
__m128i x2 = _mm_sub_epi8(_mm_loadu_si128((const __m128i*)(ptr + pixel[2*quarterPatternSize])), delta);
__m128i x3 = _mm_sub_epi8(_mm_loadu_si128((const __m128i*)(ptr + pixel[3*quarterPatternSize])), delta);
m0 = _mm_and_si128(_mm_cmpgt_epi8(x0, v0), _mm_cmpgt_epi8(x1, v0));
m1 = _mm_and_si128(_mm_cmpgt_epi8(v1, x0), _mm_cmpgt_epi8(v1, x1));
m0 = _mm_or_si128(m0, _mm_and_si128(_mm_cmpgt_epi8(x1, v0), _mm_cmpgt_epi8(x2, v0)));
m1 = _mm_or_si128(m1, _mm_and_si128(_mm_cmpgt_epi8(v1, x1), _mm_cmpgt_epi8(v1, x2)));
m0 = _mm_or_si128(m0, _mm_and_si128(_mm_cmpgt_epi8(x2, v0), _mm_cmpgt_epi8(x3, v0)));
m1 = _mm_or_si128(m1, _mm_and_si128(_mm_cmpgt_epi8(v1, x2), _mm_cmpgt_epi8(v1, x3)));
m0 = _mm_or_si128(m0, _mm_and_si128(_mm_cmpgt_epi8(x3, v0), _mm_cmpgt_epi8(x0, v0)));
m1 = _mm_or_si128(m1, _mm_and_si128(_mm_cmpgt_epi8(v1, x3), _mm_cmpgt_epi8(v1, x0)));
m0 = _mm_or_si128(m0, m1);
int mask = _mm_movemask_epi8(m0);
if( mask == 0 )
continue;
if( (mask & 255) == 0 )
{
j -= 8;
ptr -= 8;
continue;
}
__m128i c0 = _mm_setzero_si128(), c1 = c0, max0 = c0, max1 = c0;
for( k = 0; k < N; k++ )
{
__m128i x = _mm_xor_si128(_mm_loadu_si128((const __m128i*)(ptr + pixel[k])), delta);
m0 = _mm_cmpgt_epi8(x, v0);
m1 = _mm_cmpgt_epi8(v1, x);
c0 = _mm_and_si128(_mm_sub_epi8(c0, m0), m0);
c1 = _mm_and_si128(_mm_sub_epi8(c1, m1), m1);
max0 = _mm_max_epu8(max0, c0);
max1 = _mm_max_epu8(max1, c1);
}
max0 = _mm_max_epu8(max0, max1);
int m = _mm_movemask_epi8(_mm_cmpgt_epi8(max0, K16));
for( k = 0; m > 0 && k < 16; k++, m >>= 1 )
if(m & 1)
{
cornerpos[ncorners++] = j+k;
if(nonmax_suppression)
curr[j+k] = (uchar)cornerScore<patternSize>(ptr+k, pixel, threshold);
}
}
}
#endif
for( ; j < img.cols - 3; j++, ptr++ )
{
int v = ptr[0];
const uchar* tab = &threshold_tab[0] - v + 255;
int d = tab[ptr[pixel[0]]] | tab[ptr[pixel[8]]];
if( d == 0 )
continue;
d &= tab[ptr[pixel[2]]] | tab[ptr[pixel[10]]];
d &= tab[ptr[pixel[4]]] | tab[ptr[pixel[12]]];
d &= tab[ptr[pixel[6]]] | tab[ptr[pixel[14]]];
if( d == 0 )
continue;
d &= tab[ptr[pixel[1]]] | tab[ptr[pixel[9]]];
d &= tab[ptr[pixel[3]]] | tab[ptr[pixel[11]]];
d &= tab[ptr[pixel[5]]] | tab[ptr[pixel[13]]];
d &= tab[ptr[pixel[7]]] | tab[ptr[pixel[15]]];
if( d & 1 )
{
int vt = v - threshold, count = 0;
for( k = 0; k < N; k++ )
{
int x = ptr[pixel[k]];
if(x < vt)
{
if( ++count > K )
{
cornerpos[ncorners++] = j;
if(nonmax_suppression)
curr[j] = (uchar)cornerScore<patternSize>(ptr, pixel, threshold);
break;
}
}
else
count = 0;
}
}
if( d & 2 )
{
int vt = v + threshold, count = 0;
for( k = 0; k < N; k++ )
{
int x = ptr[pixel[k]];
if(x > vt)
{
if( ++count > K )
{
cornerpos[ncorners++] = j;
if(nonmax_suppression)
curr[j] = (uchar)cornerScore<patternSize>(ptr, pixel, threshold);
break;
}
}
else
count = 0;
}
}
}
}
cornerpos[-1] = ncorners;
if( i == 3 )
continue;
const uchar* prev = buf[(i - 4 + 3)%3];
const uchar* pprev = buf[(i - 5 + 3)%3];
cornerpos = cpbuf[(i - 4 + 3)%3];
ncorners = cornerpos[-1];
for( k = 0; k < ncorners; k++ )
{
j = cornerpos[k];
int score = prev[j];
if( !nonmax_suppression ||
(score > prev[j+1] && score > prev[j-1] &&
score > pprev[j-1] && score > pprev[j] && score > pprev[j+1] &&
score > curr[j-1] && score > curr[j] && score > curr[j+1]) )
{
keypoints.push_back(KeyPoint((float)j, (float)(i-1), 7.f, -1, (float)score));
}
}
}
