void CvMorphology::init( int _operation, int _max_width, int _src_dst_type,
int _element_shape, CvMat* _element,
CvSize _ksize, CvPoint _anchor,
int _border_mode, CvScalar _border_value )
{
CV_FUNCNAME( "CvMorphology::init" );
__BEGIN__;
int depth = CV_MAT_DEPTH(_src_dst_type);
int el_type = 0, nz = -1;
if( _operation != ERODE && _operation != DILATE )
CV_ERROR( CV_StsBadArg, "Unknown/unsupported morphological operation" );
if( _element_shape == CUSTOM )
{
if( !CV_IS_MAT(_element) )
CV_ERROR( CV_StsBadArg,
"structuring element should be valid matrix if CUSTOM element shape is specified" );
el_type = CV_MAT_TYPE(_element->type);
if( el_type != CV_8UC1 && el_type != CV_32SC1 )
CV_ERROR( CV_StsUnsupportedFormat, "the structuring element must have 8uC1 or 32sC1 type" );
_ksize = cvGetMatSize(_element);
CV_CALL( nz = cvCountNonZero(_element));
if( nz == _ksize.width*_ksize.height )
_element_shape = RECT;
}
operation = _operation;
el_shape = _element_shape;
CV_CALL( CvBaseImageFilter::init( _max_width, _src_dst_type, _src_dst_type,
_element_shape == RECT, _ksize, _anchor, _border_mode, _border_value ));
if( el_shape == RECT )
{
if( operation == ERODE )
{
if( depth == CV_8U )
x_func = (CvRowFilterFunc)icvErodeRectRow_8u,
y_func = (CvColumnFilterFunc)icvErodeRectCol_8u;
else if( depth == CV_16U )
x_func = (CvRowFilterFunc)icvErodeRectRow_16u,
y_func = (CvColumnFilterFunc)icvErodeRectCol_16u;
else if( depth == CV_32F )
x_func = (CvRowFilterFunc)icvErodeRectRow_32f,
y_func = (CvColumnFilterFunc)icvErodeRectCol_32f;
}
else
{
assert( operation == DILATE );
if( depth == CV_8U )
x_func = (CvRowFilterFunc)icvDilateRectRow_8u,
y_func = (CvColumnFilterFunc)icvDilateRectCol_8u;
else if( depth == CV_16U )
x_func = (CvRowFilterFunc)icvDilateRectRow_16u,
y_func = (CvColumnFilterFunc)icvDilateRectCol_16u;
else if( depth == CV_32F )
x_func = (CvRowFilterFunc)icvDilateRectRow_32f,
y_func = (CvColumnFilterFunc)icvDilateRectCol_32f;
}
}
else
{
int i, j, k = 0;
int cn = CV_MAT_CN(src_type);
CvPoint* nz_loc;
if( !(element && el_sparse &&
_ksize.width == element->cols && _ksize.height == element->rows) )
{
cvReleaseMat( &element );
cvFree( &el_sparse );
CV_CALL( element = cvCreateMat( _ksize.height, _ksize.width, CV_8UC1 ));
CV_CALL( el_sparse = (uchar*)cvAlloc(
ksize.width*ksize.height*(2*sizeof(int) + sizeof(uchar*))));
}
if( el_shape == CUSTOM )
{
CV_CALL( cvConvert( _element, element ));
}
else
{
CV_CALL( init_binary_element( element, el_shape, anchor ));
}
if( operation == ERODE )
{
if( depth == CV_8U )
y_func = (CvColumnFilterFunc)icvErodeAny_8u;
else if( depth == CV_16U )
y_func = (CvColumnFilterFunc)icvErodeAny_16u;
else if( depth == CV_32F )
y_func = (CvColumnFilterFunc)icvErodeAny_32f;
}
else
{
assert( operation == DILATE );
if( depth == CV_8U )
y_func = (CvColumnFilterFunc)icvDilateAny_8u;
else if( depth == CV_16U )
y_func = (CvColumnFilterFunc)icvDilateAny_16u;
else if( depth == CV_32F )
y_func = (CvColumnFilterFunc)icvDilateAny_32f;
}
nz_loc = (CvPoint*)el_sparse;
for( i = 0; i < ksize.height; i++ )
for( j = 0; j < ksize.width; j++ )
{
if( element->data.ptr[i*element->step+j] )
nz_loc[k++] = cvPoint(j*cn,i);
}
if( k == 0 )
nz_loc[k++] = cvPoint(anchor.x*cn,anchor.y);
el_sparse_count = k;
}
if( depth == CV_32F && border_mode == IPL_BORDER_CONSTANT )
{
int i, cn = CV_MAT_CN(src_type);
int* bt = (int*)border_tab;
for( i = 0; i < cn; i++ )
bt[i] = CV_TOGGLE_FLT(bt[i]);
}
__END__;
}
CV_IMPL CvSeq*
cvConvexHull2( const CvArr* array, void* hull_storage,
int orientation, int return_points )
{
union { CvContour* c; CvSeq* s; } hull;
CvPoint** pointer = 0;
CvPoint2D32f** pointerf = 0;
int* stack = 0;
CV_FUNCNAME( "cvConvexHull2" );
hull.s = 0;
__BEGIN__;
CvMat* mat = 0;
CvSeqReader reader;
CvSeqWriter writer;
CvContour contour_header;
union { CvContour c; CvSeq s; } hull_header;
CvSeqBlock block, hullblock;
CvSeq* ptseq = 0;
CvSeq* hullseq = 0;
int is_float;
int* t_stack;
int t_count;
int i, miny_ind = 0, maxy_ind = 0, total;
int hulltype;
int stop_idx;
sklansky_func sklansky;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_ERROR( CV_StsBadArg, "Unsupported sequence type" );
if( hull_storage == 0 )
hull_storage = ptseq->storage;
}
else
{
CV_CALL( ptseq = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
}
if( CV_IS_STORAGE( hull_storage ))
{
if( return_points )
{
CV_CALL( hullseq = cvCreateSeq(
CV_SEQ_KIND_CURVE|CV_SEQ_ELTYPE(ptseq)|
CV_SEQ_FLAG_CLOSED|CV_SEQ_FLAG_CONVEX,
sizeof(CvContour), sizeof(CvPoint),(CvMemStorage*)hull_storage ));
}
else
{
CV_CALL( hullseq = cvCreateSeq(
CV_SEQ_KIND_CURVE|CV_SEQ_ELTYPE_PPOINT|
CV_SEQ_FLAG_CLOSED|CV_SEQ_FLAG_CONVEX,
sizeof(CvContour), sizeof(CvPoint*), (CvMemStorage*)hull_storage ));
}
}
else
{
if( !CV_IS_MAT( hull_storage ))
CV_ERROR(CV_StsBadArg, "Destination must be valid memory storage or matrix");
mat = (CvMat*)hull_storage;
if( mat->cols != 1 && mat->rows != 1 || !CV_IS_MAT_CONT(mat->type))
CV_ERROR( CV_StsBadArg,
"The hull matrix should be continuous and have a single row or a single column" );
if( mat->cols + mat->rows - 1 < ptseq->total )
CV_ERROR( CV_StsBadSize, "The hull matrix size might be not enough to fit the hull" );
if( CV_MAT_TYPE(mat->type) != CV_SEQ_ELTYPE(ptseq) &&
CV_MAT_TYPE(mat->type) != CV_32SC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"The hull matrix must have the same type as input or 32sC1 (integers)" );
CV_CALL( hullseq = cvMakeSeqHeaderForArray(
CV_SEQ_KIND_CURVE|CV_MAT_TYPE(mat->type)|CV_SEQ_FLAG_CLOSED,
sizeof(contour_header), CV_ELEM_SIZE(mat->type), mat->data.ptr,
mat->cols + mat->rows - 1, &hull_header.s, &hullblock ));
cvClearSeq( hullseq );
}
total = ptseq->total;
if( total == 0 )
{
if( mat )
CV_ERROR( CV_StsBadSize,
"Point sequence can not be empty if the output is matrix" );
EXIT;
}
cvStartAppendToSeq( hullseq, &writer );
is_float = CV_SEQ_ELTYPE(ptseq) == CV_32FC2;
hulltype = CV_SEQ_ELTYPE(hullseq);
sklansky = !is_float ? (sklansky_func)icvSklansky_32s :
(sklansky_func)icvSklansky_32f;
CV_CALL( pointer = (CvPoint**)cvAlloc( ptseq->total*sizeof(pointer[0]) ));
CV_CALL( stack = (int*)cvAlloc( (ptseq->total + 2)*sizeof(stack[0]) ));
pointerf = (CvPoint2D32f**)pointer;
cvStartReadSeq( ptseq, &reader );
for( i = 0; i < total; i++ )
{
pointer[i] = (CvPoint*)reader.ptr;
CV_NEXT_SEQ_ELEM( ptseq->elem_size, reader );
}
// sort the point set by x-coordinate, find min and max y
if( !is_float )
{
icvSortPointsByPointers_32s( pointer, total, 0 );
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
{
icvSortPointsByPointers_32f( pointerf, total, 0 );
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 )
{
if( hulltype == CV_SEQ_ELTYPE_PPOINT )
{
CV_WRITE_SEQ_ELEM( pointer[0], writer );
}
else if( hulltype == CV_SEQ_ELTYPE_INDEX )
{
int index = 0;
CV_WRITE_SEQ_ELEM( index, writer );
}
else
{
CvPoint pt = pointer[0][0];
CV_WRITE_SEQ_ELEM( pt, writer );
}
goto finish_hull;
}
/*upper half */
{
int *tl_stack = stack;
int tl_count = sklansky( pointer, 0, maxy_ind, tl_stack, -1, 1 );
int *tr_stack = tl_stack + tl_count;
int tr_count = sklansky( pointer, ptseq->total - 1, maxy_ind, tr_stack, -1, -1 );
/* gather upper part of convex hull to output */
if( orientation == CV_COUNTER_CLOCKWISE )
{
CV_SWAP( tl_stack, tr_stack, t_stack );
CV_SWAP( tl_count, tr_count, t_count );
}
if( hulltype == CV_SEQ_ELTYPE_PPOINT )
{
for( i = 0; i < tl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[tl_stack[i]], writer );
for( i = tr_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[tr_stack[i]], writer );
}
else if( hulltype == CV_SEQ_ELTYPE_INDEX )
{
CV_CALL( icvCalcAndWritePtIndices( pointer, tl_stack,
0, tl_count-1, ptseq, &writer ));
CV_CALL( icvCalcAndWritePtIndices( pointer, tr_stack,
tr_count-1, 0, ptseq, &writer ));
}
else
{
for( i = 0; i < tl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[tl_stack[i]][0], writer );
for( i = tr_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[tr_stack[i]][0], writer );
}
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 = sklansky( pointer, 0, miny_ind, bl_stack, 1, -1 );
int *br_stack = stack + bl_count;
int br_count = sklansky( pointer, ptseq->total - 1, miny_ind, br_stack, 1, 1 );
if( orientation != CV_COUNTER_CLOCKWISE )
{
CV_SWAP( bl_stack, br_stack, t_stack );
CV_SWAP( bl_count, br_count, t_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 );
}
}
if( hulltype == CV_SEQ_ELTYPE_PPOINT )
{
for( i = 0; i < bl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[bl_stack[i]], writer );
for( i = br_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[br_stack[i]], writer );
}
else if( hulltype == CV_SEQ_ELTYPE_INDEX )
{
CV_CALL( icvCalcAndWritePtIndices( pointer, bl_stack,
0, bl_count-1, ptseq, &writer ));
CV_CALL( icvCalcAndWritePtIndices( pointer, br_stack,
br_count-1, 0, ptseq, &writer ));
}
else
{
for( i = 0; i < bl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[bl_stack[i]][0], writer );
for( i = br_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[br_stack[i]][0], writer );
}
}
finish_hull:
CV_CALL( cvEndWriteSeq( &writer ));
if( mat )
{
if( mat->rows > mat->cols )
mat->rows = hullseq->total;
else
mat->cols = hullseq->total;
}
else
{
hull.s = hullseq;
hull.c->rect = cvBoundingRect( ptseq,
ptseq->header_size < (int)sizeof(CvContour) ||
&ptseq->flags == &contour_header.flags );
/*if( ptseq != (CvSeq*)&contour_header )
hullseq->v_prev = ptseq;*/
}
__END__;
cvFree( &pointer );
cvFree( &stack );
return hull.s;
}
// Do checking part of loop of cross-validations metod.
ML_IMPL
void cvCrossValCheckClassifier (CvStatModel* estimateModel,
const CvStatModel* model,
const CvMat* trainData,
int sample_t_flag,
const CvMat* trainClasses)
{
CV_FUNCNAME ("cvCrossValCheckClassifier ");
__BEGIN__
CvCrossValidationModel* crVal = (CvCrossValidationModel*) estimateModel;
int i, j, k;
int* data;
float* responses_fl;
int step;
float* responses_result;
int* responses_i;
double te, te1;
double sum_c, sum_p, sum_pp, sum_cp, sum_cc, sq_err;
// Check input data to correct values.
if (!CV_IS_CROSSVAL (estimateModel))
{
CV_ERROR (CV_StsBadArg,"First parameter point to not CvCrossValidationModel");
}
if (!CV_IS_STAT_MODEL (model))
{
CV_ERROR (CV_StsBadArg, "Second parameter point to not CvStatModel");
}
if (!CV_IS_MAT (trainData))
{
CV_ERROR (CV_StsBadArg, "Third parameter point to not CvMat");
}
if (!CV_IS_MAT (trainClasses))
{
CV_ERROR (CV_StsBadArg, "Fifth parameter point to not CvMat");
}
if (crVal->is_checked)
{
CV_ERROR (CV_StsInternal, "This iterations already was checked");
}
// Initialize.
k = crVal->sampleIdxEval->cols;
data = crVal->sampleIdxEval->data.i;
// Eval tested feature vectors.
CV_CALL (cvStatModelMultiPredict (model, trainData, sample_t_flag,
crVal->predict_results, NULL, crVal->sampleIdxEval));
// Count number if correct results.
responses_result = crVal->predict_results->data.fl;
if (crVal->is_regression)
{
sum_c = sum_p = sum_pp = sum_cp = sum_cc = sq_err = 0;
if (CV_MAT_TYPE (trainClasses->type) == CV_32FC1)
{
responses_fl = trainClasses->data.fl;
step = trainClasses->rows == 1 ? 1 : trainClasses->step / sizeof(float);
for (i = 0; i < k; i++)
{
te = responses_result[*data];
te1 = responses_fl[*data * step];
sum_c += te1;
sum_p += te;
sum_cc += te1 * te1;
sum_pp += te * te;
sum_cp += te1 * te;
te -= te1;
sq_err += te * te;
data++;
}
}
else
{
responses_i = trainClasses->data.i;
step = trainClasses->rows == 1 ? 1 : trainClasses->step / sizeof(int);
for (i = 0; i < k; i++)
{
te = responses_result[*data];
te1 = responses_i[*data * step];
sum_c += te1;
sum_p += te;
sum_cc += te1 * te1;
sum_pp += te * te;
sum_cp += te1 * te;
te -= te1;
sq_err += te * te;
data++;
}
}
// Fixing new internal values of accuracy.
crVal->sum_correct += sum_c;
crVal->sum_predict += sum_p;
crVal->sum_cc += sum_cc;
crVal->sum_pp += sum_pp;
crVal->sum_cp += sum_cp;
crVal->sq_error += sq_err;
}
else
{
if (CV_MAT_TYPE (trainClasses->type) == CV_32FC1)
{
responses_fl = trainClasses->data.fl;
step = trainClasses->rows == 1 ? 1 : trainClasses->step / sizeof(float);
for (i = 0, j = 0; i < k; i++)
{
if (cvRound (responses_result[*data]) == cvRound (responses_fl[*data * step]))
j++;
data++;
}
}
else
{
responses_i = trainClasses->data.i;
step = trainClasses->rows == 1 ? 1 : trainClasses->step / sizeof(int);
for (i = 0, j = 0; i < k; i++)
{
if (cvRound (responses_result[*data]) == responses_i[*data * step])
j++;
data++;
}
}
// Fixing new internal values of accuracy.
crVal->correct_results += j;
}
// Fixing that this fold already checked.
crVal->all_results += k;
crVal->is_checked = 1;
__END__
} // End of cvCrossValCheckClassifier
CV_IMPL void cvRandShuffle( CvArr* arr, CvRNG* rng, double iter_factor )
{
CV_FUNCNAME( "cvRandShuffle" );
__BEGIN__;
const int sizeof_int = (int)sizeof(int);
CvMat stub, *mat = (CvMat*)arr;
int i, j, k, iters, delta = 0;
int cont_flag, arr_size, elem_size, cols, step;
const int pair_buf_sz = 100;
int* pair_buf = (int*)cvStackAlloc( pair_buf_sz*sizeof(pair_buf[0])*2 );
CvMat _pair_buf = cvMat( 1, pair_buf_sz*2, CV_32S, pair_buf );
CvRNG _rng = cvRNG(-1);
uchar* data = 0;
int* idata = 0;
if( !CV_IS_MAT(mat) )
CV_CALL( mat = cvGetMat( mat, &stub ));
if( !rng )
rng = &_rng;
cols = mat->cols;
step = mat->step;
arr_size = cols*mat->rows;
iters = cvRound(iter_factor*arr_size)*2;
cont_flag = CV_IS_MAT_CONT(mat->type);
elem_size = CV_ELEM_SIZE(mat->type);
if( elem_size % sizeof_int == 0 && (cont_flag || step % sizeof_int == 0) )
{
idata = mat->data.i;
step /= sizeof_int;
elem_size /= sizeof_int;
}
else
data = mat->data.ptr;
for( i = 0; i < iters; i += delta )
{
delta = MIN( iters - i, pair_buf_sz*2 );
_pair_buf.cols = delta;
cvRandArr( rng, &_pair_buf, CV_RAND_UNI, cvRealScalar(0), cvRealScalar(arr_size) );
if( cont_flag )
{
if( idata )
for( j = 0; j < delta; j += 2 )
{
int* p = idata + pair_buf[j]*elem_size, *q = idata + pair_buf[j+1]*elem_size, t;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
else
for( j = 0; j < delta; j += 2 )
{
uchar* p = data + pair_buf[j]*elem_size, *q = data + pair_buf[j+1]*elem_size, t;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
}
else
{
if( idata )
for( j = 0; j < delta; j += 2 )
{
int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2;
int* p, *q, t;
row1 = idx1/step; row2 = idx2/step;
p = idata + row1*step + (idx1 - row1*cols)*elem_size;
q = idata + row2*step + (idx2 - row2*cols)*elem_size;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
else
for( j = 0; j < delta; j += 2 )
{
int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2;
uchar* p, *q, t;
row1 = idx1/step; row2 = idx2/step;
p = data + row1*step + (idx1 - row1*cols)*elem_size;
q = data + row2*step + (idx2 - row2*cols)*elem_size;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
}
}
__END__;
}
/*!
\fn CvGaborFeature::_XM2VTSMulti_F(const char *pathname, const CvMat* picIndex, const CvMat* subIndex) const
*/
CvTrainingData* CvGaborFeature::_XM2VTSMulti_F(const char *pathname, const CvMat* picIndex, const CvMat* subIndex) const
{
int nosub = 200;
int nopic = 8;
/* Generate filename */
char *filename = new char[100];
char *ch_scale = new char[5];
char *ch_orient = new char[5];
char *ch_name = new char[10];
strcpy( filename, pathname );
sprintf( ch_scale, "%d", iNu );
strcat(filename, ch_scale);
strcat(filename, "/");
sprintf( ch_orient, "%d", iMu );
strcat( filename, ch_orient );
strcat(filename, "/");
sprintf(ch_name, "%d_%d.xml", ix, iy);
strcat(filename, ch_name);
delete [] ch_scale;
delete [] ch_orient;
delete [] ch_name;
/* Generate filename */
CvMat* mat = (CvMat*)cvLoad( filename, NULL, NULL, NULL );
//printf("Open %s \n", filename);
//assert( mat );
if(!CV_IS_MAT( mat ))
{
//retry 10 times
for(int i = 0; i < 10; i++)
{
time_t rawtime;
struct tm * timeinfo;
time ( &rawtime );
timeinfo = localtime ( &rawtime );
perror( asctime (timeinfo) );
perror("Can not get a matrix from ");
perror(filename);
sleep( 10 ); // sleep for 10 sec
mat = (CvMat*)cvLoad( filename, NULL, NULL, NULL );
if (CV_IS_MAT( mat )) break;
}
}
CvSize size = cvGetSize( picIndex );
CvSize size1 = cvGetSize( mat );
CvSize size2 = cvGetSize(subIndex);
int num = size1.height;
int ntpic = size.width;
int ntsub = size2.width;
int numsample = ntsub*ntpic;
CvTrainingData *bindata = new CvTrainingData;
CvMat* tmpmat = cvCreateMat(numsample, 1, CV_32FC1);
bindata->init(ntsub, numsample, 1);
int n = 0;
int m = 0;
double v;
for(int sub = 1; sub <= nosub; sub++)
{
for(int pic = 1; pic <= nopic; pic++)
{
for(int j = 0; j < ntsub; j++)
{
for(int i = 0; i < ntpic; i++)
{
if((pic == cvGetReal1D(picIndex, i))&&(sub == cvGetReal1D(subIndex, j)))
{
v = cvGetReal1D(mat, n);
cvSetReal1D(tmpmat, m, v);
bindata->setclsidxofsample(sub, m);
m++;
}
}
}
n++;
}
}
bindata->setdata(tmpmat);
cvReleaseMat(&tmpmat);
cvReleaseMat(&mat);
delete [] filename;
bindata->statclsdist();
return bindata;
}
CV_IMPL void
cvInitUndistortMap( const CvMat* A, const CvMat* dist_coeffs,
CvArr* mapxarr, CvArr* mapyarr )
{
uchar* buffer = 0;
CV_FUNCNAME( "cvInitUndistortMap" );
__BEGIN__;
float a[9], k[4];
int coi1 = 0, coi2 = 0;
CvMat mapxstub, *_mapx = (CvMat*)mapxarr;
CvMat mapystub, *_mapy = (CvMat*)mapyarr;
float *mapx, *mapy;
CvMat _a = cvMat( 3, 3, CV_32F, a ), _k;
int mapxstep, mapystep;
int u, v;
float u0, v0, fx, fy, _fx, _fy, k1, k2, p1, p2;
CvSize size;
CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 ));
CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 ));
if( coi1 != 0 || coi2 != 0 )
CV_ERROR( CV_BadCOI, "The function does not support COI" );
if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 )
CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" );
if( !CV_ARE_TYPES_EQ( _mapx, _mapy ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( !CV_ARE_SIZES_EQ( _mapx, _mapy ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 ||
CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1 )
CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
if( !CV_IS_MAT(dist_coeffs) || dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
cvConvert( A, &_a );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k );
cvConvert( dist_coeffs, &_k );
u0 = a[2]; v0 = a[5];
fx = a[0]; fy = a[4];
_fx = 1.f/fx; _fy = 1.f/fy;
k1 = k[0]; k2 = k[1];
p1 = k[2]; p2 = k[3];
mapxstep = _mapx->step ? _mapx->step : CV_STUB_STEP;
mapystep = _mapy->step ? _mapy->step : CV_STUB_STEP;
mapx = _mapx->data.fl;
mapy = _mapy->data.fl;
size = cvGetMatSize(_mapx);
/*if( icvUndistortGetSize_p && icvCreateMapCameraUndistort_32f_C1R_p )
{
int buf_size = 0;
if( icvUndistortGetSize_p( size, &buf_size ) && buf_size > 0 )
{
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
if( icvCreateMapCameraUndistort_32f_C1R_p(
mapx, mapxstep, mapy, mapystep, size,
a[0], a[4], a[2], a[5], k[0], k[1], k[2], k[3], buffer ) >= 0 )
EXIT;
}
}*/
mapxstep /= sizeof(mapx[0]);
mapystep /= sizeof(mapy[0]);
for( v = 0; v < size.height; v++, mapx += mapxstep, mapy += mapystep )
{
float y = (v - v0)*_fy;
float y2 = y*y;
float _2p1y = 2*p1*y;
float _3p1y2 = 3*p1*y2;
float p2y2 = p2*y2;
for( u = 0; u < size.width; u++ )
{
float x = (u - u0)*_fx;
float x2 = x*x;
float r2 = x2 + y2;
float d = 1 + (k1 + k2*r2)*r2;
float _u = fx*(x*(d + _2p1y) + p2y2 + (3*p2)*x2) + u0;
float _v = fy*(y*(d + (2*p2)*x) + _3p1y2 + p1*x2) + v0;
mapx[u] = _u;
mapy[u] = _v;
}
}
__END__;
cvFree( &buffer );
}
CV_IMPL void cvNormalize( const CvArr* src, CvArr* dst,
double a, double b, int norm_type, const CvArr* mask )
{
CvMat* tmp = 0;
CV_FUNCNAME( "cvNormalize" );
__BEGIN__;
double scale, shift;
if( norm_type == CV_MINMAX )
{
double smin = 0, smax = 0;
double dmin = MIN( a, b ), dmax = MAX( a, b );
cvMinMaxLoc( src, &smin, &smax, 0, 0, mask );
scale = (dmax - dmin)*(smax - smin > DBL_EPSILON ? 1./(smax - smin) : 0);
shift = dmin - smin*scale;
}
else if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C )
{
CvMat *s = (CvMat*)src, *d = (CvMat*)dst;
if( CV_IS_MAT(s) && CV_IS_MAT(d) && CV_IS_MAT_CONT(s->type & d->type) &&
CV_ARE_TYPES_EQ(s,d) && CV_ARE_SIZES_EQ(s,d) && !mask &&
s->cols*s->rows <= CV_MAX_INLINE_MAT_OP_SIZE*CV_MAX_INLINE_MAT_OP_SIZE )
{
int i, len = s->cols*s->rows;
double norm = 0, v;
if( CV_MAT_TYPE(s->type) == CV_32FC1 )
{
const float* sptr = s->data.fl;
float* dptr = d->data.fl;
if( norm_type == CV_L2 )
{
for( i = 0; i < len; i++ )
{
v = sptr[i];
norm += v*v;
}
norm = sqrt(norm);
}
else if( norm_type == CV_L1 )
for( i = 0; i < len; i++ )
{
v = fabs((double)sptr[i]);
norm += v;
}
else
for( i = 0; i < len; i++ )
{
v = fabs((double)sptr[i]);
norm = MAX(norm,v);
}
norm = norm > DBL_EPSILON ? 1./norm : 0.;
for( i = 0; i < len; i++ )
dptr[i] = (float)(sptr[i]*norm);
EXIT;
}
if( CV_MAT_TYPE(s->type) == CV_64FC1 )
{
const double* sptr = s->data.db;
double* dptr = d->data.db;
if( norm_type == CV_L2 )
{
for( i = 0; i < len; i++ )
{
v = sptr[i];
norm += v*v;
}
norm = sqrt(norm);
}
else if( norm_type == CV_L1 )
for( i = 0; i < len; i++ )
{
v = fabs(sptr[i]);
norm += v;
}
else
for( i = 0; i < len; i++ )
{
v = fabs(sptr[i]);
norm = MAX(norm,v);
}
norm = norm > DBL_EPSILON ? 1./norm : 0.;
for( i = 0; i < len; i++ )
dptr[i] = sptr[i]*norm;
EXIT;
}
}
scale = cvNorm( src, 0, norm_type, mask );
scale = scale > DBL_EPSILON ? 1./scale : 0.;
shift = 0;
}
else
CV_ERROR( CV_StsBadArg, "Unknown/unsupported norm type" );
if( !mask )
cvConvertScale( src, dst, scale, shift );
else
{
CvMat stub, *dmat;
CV_CALL( dmat = cvGetMat(dst, &stub));
CV_CALL( tmp = cvCreateMat(dmat->rows, dmat->cols, dmat->type) );
cvConvertScale( src, tmp, scale, shift );
cvCopy( tmp, dst, mask );
}
__END__;
if( tmp )
cvReleaseMat( &tmp );
}
CV_IMPL void
cvReduce( const CvArr* srcarr, CvArr* dstarr, int dim, int op )
{
CvMat* temp = 0;
CV_FUNCNAME( "cvReduce" );
__BEGIN__;
CvMat sstub, *src = (CvMat*)srcarr;
CvMat dstub, *dst = (CvMat*)dstarr, *dst0;
int sdepth, ddepth, cn, op0 = op;
CvSize size;
if( !CV_IS_MAT(src) )
CV_CALL( src = cvGetMat( src, &sstub ));
if( !CV_IS_MAT(dst) )
CV_CALL( dst = cvGetMat( dst, &dstub ));
if( !CV_ARE_CNS_EQ(src, dst) )
CV_ERROR( CV_StsUnmatchedFormats, "Input and output arrays must have the same number of channels" );
sdepth = CV_MAT_DEPTH(src->type);
ddepth = CV_MAT_DEPTH(dst->type);
cn = CV_MAT_CN(src->type);
dst0 = dst;
size = cvGetMatSize(src);
if( dim < 0 )
dim = src->rows > dst->rows ? 0 : src->cols > dst->cols ? 1 : dst->cols == 1;
if( dim > 1 )
CV_ERROR( CV_StsOutOfRange, "The reduced dimensionality index is out of range" );
if( dim == 0 && (dst->cols != src->cols || dst->rows != 1) ||
dim == 1 && (dst->rows != src->rows || dst->cols != 1) )
CV_ERROR( CV_StsBadSize, "The output array size is incorrect" );
if( op == CV_REDUCE_AVG )
{
int ttype = sdepth == CV_8U ? CV_MAKETYPE(CV_32S,cn) : dst->type;
if( ttype != dst->type )
CV_CALL( dst = temp = cvCreateMat( dst->rows, dst->cols, ttype ));
op = CV_REDUCE_SUM;
ddepth = CV_MAT_DEPTH(ttype);
}
if( op != CV_REDUCE_SUM && op != CV_REDUCE_MAX && op != CV_REDUCE_MIN )
CV_ERROR( CV_StsBadArg, "Unknown reduce operation index, must be one of CV_REDUCE_*" );
if( dim == 0 )
{
CvReduceToRowFunc rfunc =
op == CV_REDUCE_SUM ?
(sdepth == CV_8U && ddepth == CV_32S ? (CvReduceToRowFunc)icvSumRows_8u32s_C1R :
sdepth == CV_8U && ddepth == CV_32F ? (CvReduceToRowFunc)icvSumRows_8u32f_C1R :
sdepth == CV_16U && ddepth == CV_32F ? (CvReduceToRowFunc)icvSumRows_16u32f_C1R :
sdepth == CV_16U && ddepth == CV_64F ? (CvReduceToRowFunc)icvSumRows_16u64f_C1R :
sdepth == CV_16S && ddepth == CV_32F ? (CvReduceToRowFunc)icvSumRows_16s32f_C1R :
sdepth == CV_16S && ddepth == CV_64F ? (CvReduceToRowFunc)icvSumRows_16s64f_C1R :
sdepth == CV_32F && ddepth == CV_32F ? (CvReduceToRowFunc)icvSumRows_32f_C1R :
sdepth == CV_32F && ddepth == CV_64F ? (CvReduceToRowFunc)icvSumRows_32f64f_C1R :
sdepth == CV_64F && ddepth == CV_64F ? (CvReduceToRowFunc)icvSumRows_64f_C1R : 0) :
op == CV_REDUCE_MAX ?
(sdepth == CV_8U && ddepth == CV_8U ? (CvReduceToRowFunc)icvMaxRows_8u_C1R :
sdepth == CV_32F && ddepth == CV_32F ? (CvReduceToRowFunc)icvMaxRows_32f_C1R :
sdepth == CV_64F && ddepth == CV_64F ? (CvReduceToRowFunc)icvMaxRows_64f_C1R : 0) :
(sdepth == CV_8U && ddepth == CV_8U ? (CvReduceToRowFunc)icvMinRows_8u_C1R :
sdepth == CV_32F && ddepth == CV_32F ? (CvReduceToRowFunc)icvMinRows_32f_C1R :
sdepth == CV_64F && ddepth == CV_64F ? (CvReduceToRowFunc)icvMinRows_64f_C1R : 0);
if( !rfunc )
CV_ERROR( CV_StsUnsupportedFormat,
"Unsupported combination of input and output array formats" );
size.width *= cn;
IPPI_CALL( rfunc( src->data.ptr, src->step ? src->step : CV_STUB_STEP,
dst->data.ptr, size ));
}
else
{
CvReduceToColFunc cfunc =
op == CV_REDUCE_SUM ?
(sdepth == CV_8U && ddepth == CV_32S ?
(CvReduceToColFunc)(cn == 1 ? icvSumCols_8u32s_C1R :
cn == 3 ? icvSumCols_8u32s_C3R :
cn == 4 ? icvSumCols_8u32s_C4R : 0) :
sdepth == CV_8U && ddepth == CV_32F ?
(CvReduceToColFunc)(cn == 1 ? icvSumCols_8u32f_C1R :
cn == 3 ? icvSumCols_8u32f_C3R :
cn == 4 ? icvSumCols_8u32f_C4R : 0) :
sdepth == CV_16U && ddepth == CV_32F ?
(CvReduceToColFunc)(cn == 1 ? icvSumCols_16u32f_C1R : 0) :
sdepth == CV_16U && ddepth == CV_64F ?
(CvReduceToColFunc)(cn == 1 ? icvSumCols_16u64f_C1R : 0) :
sdepth == CV_16S && ddepth == CV_32F ?
(CvReduceToColFunc)(cn == 1 ? icvSumCols_16s32f_C1R : 0) :
sdepth == CV_16S && ddepth == CV_64F ?
(CvReduceToColFunc)(cn == 1 ? icvSumCols_16s64f_C1R : 0) :
sdepth == CV_32F && ddepth == CV_32F ?
(CvReduceToColFunc)(cn == 1 ? icvSumCols_32f_C1R :
cn == 3 ? icvSumCols_32f_C3R :
cn == 4 ? icvSumCols_32f_C4R : 0) :
sdepth == CV_32F && ddepth == CV_64F ?
(CvReduceToColFunc)(cn == 1 ? icvSumCols_32f64f_C1R : 0) :
sdepth == CV_64F && ddepth == CV_64F ?
(CvReduceToColFunc)(cn == 1 ? icvSumCols_64f_C1R :
cn == 3 ? icvSumCols_64f_C3R :
cn == 4 ? icvSumCols_64f_C4R : 0) : 0) :
op == CV_REDUCE_MAX && cn == 1 ?
(sdepth == CV_8U && ddepth == CV_8U ? (CvReduceToColFunc)icvMaxCols_8u_C1R :
sdepth == CV_32F && ddepth == CV_32F ? (CvReduceToColFunc)icvMaxCols_32f_C1R :
sdepth == CV_64F && ddepth == CV_64F ? (CvReduceToColFunc)icvMaxCols_64f_C1R : 0) :
op == CV_REDUCE_MIN && cn == 1 ?
(sdepth == CV_8U && ddepth == CV_8U ? (CvReduceToColFunc)icvMinCols_8u_C1R :
sdepth == CV_32F && ddepth == CV_32F ? (CvReduceToColFunc)icvMinCols_32f_C1R :
sdepth == CV_64F && ddepth == CV_64F ? (CvReduceToColFunc)icvMinCols_64f_C1R : 0) : 0;
if( !cfunc )
CV_ERROR( CV_StsUnsupportedFormat,
"Unsupported combination of input and output array formats" );
IPPI_CALL( cfunc( src->data.ptr, src->step ? src->step : CV_STUB_STEP,
dst->data.ptr, dst->step ? dst->step : CV_STUB_STEP, size ));
}
if( op0 == CV_REDUCE_AVG )
cvScale( dst, dst0, 1./(dim == 0 ? src->rows : src->cols) );
__END__;
if( temp )
cvReleaseMat( &temp );
}
float CvANN_MLP::predict( const CvMat* _inputs, CvMat* _outputs ) const
{
CV_FUNCNAME( "CvANN_MLP::predict" );
__BEGIN__;
double* buf;
int i, j, n, dn = 0, l_count, dn0, buf_sz, min_buf_sz;
if( !layer_sizes )
CV_ERROR( CV_StsError, "The network has not been initialized" );
if( !CV_IS_MAT(_inputs) || !CV_IS_MAT(_outputs) ||
!CV_ARE_TYPES_EQ(_inputs,_outputs) ||
CV_MAT_TYPE(_inputs->type) != CV_32FC1 &&
CV_MAT_TYPE(_inputs->type) != CV_64FC1 ||
_inputs->rows != _outputs->rows )
CV_ERROR( CV_StsBadArg, "Both input and output must be floating-point matrices "
"of the same type and have the same number of rows" );
if( _inputs->cols != layer_sizes->data.i[0] )
CV_ERROR( CV_StsBadSize, "input matrix must have the same number of columns as "
"the number of neurons in the input layer" );
if( _outputs->cols != layer_sizes->data.i[layer_sizes->cols - 1] )
CV_ERROR( CV_StsBadSize, "output matrix must have the same number of columns as "
"the number of neurons in the output layer" );
n = dn0 = _inputs->rows;
min_buf_sz = 2*max_count;
buf_sz = n*min_buf_sz;
if( buf_sz > max_buf_sz )
{
dn0 = max_buf_sz/min_buf_sz;
dn0 = MAX( dn0, 1 );
buf_sz = dn0*min_buf_sz;
}
buf = (double*)cvStackAlloc( buf_sz*sizeof(buf[0]) );
l_count = layer_sizes->cols;
for( i = 0; i < n; i += dn )
{
CvMat hdr[2], _w, *layer_in = &hdr[0], *layer_out = &hdr[1], *temp;
dn = MIN( dn0, n - i );
cvGetRows( _inputs, layer_in, i, i + dn );
cvInitMatHeader( layer_out, dn, layer_in->cols, CV_64F, buf );
scale_input( layer_in, layer_out );
CV_SWAP( layer_in, layer_out, temp );
for( j = 1; j < l_count; j++ )
{
double* data = buf + (j&1 ? max_count*dn0 : 0);
int cols = layer_sizes->data.i[j];
cvInitMatHeader( layer_out, dn, cols, CV_64F, data );
cvInitMatHeader( &_w, layer_in->cols, layer_out->cols, CV_64F, weights[j] );
cvGEMM( layer_in, &_w, 1, 0, 0, layer_out );
calc_activ_func( layer_out, _w.data.db + _w.rows*_w.cols );
CV_SWAP( layer_in, layer_out, temp );
}
cvGetRows( _outputs, layer_out, i, i + dn );
scale_output( layer_in, layer_out );
}
__END__;
return 0.f;
}
CV_IMPL CvScalar
cvSum( const CvArr* arr )
{
static CvBigFuncTable sum_tab;
static CvFuncTable sumcoi_tab;
static int inittab = 0;
CvScalar sum = {{0,0,0,0}};
CV_FUNCNAME("cvSum");
__BEGIN__;
int type, coi = 0;
int mat_step;
CvSize size;
CvMat stub, *mat = (CvMat*)arr;
if( !inittab )
{
icvInitSumRTable( &sum_tab );
icvInitSumCnCRTable( &sumcoi_tab );
inittab = 1;
}
if( !CV_IS_MAT(mat) )
{
if( CV_IS_MATND(mat) )
{
void* matnd = (void*)mat;
CvMatND nstub;
CvNArrayIterator iterator;
int pass_hint;
CV_CALL( cvInitNArrayIterator( 1, &matnd, 0, &nstub, &iterator ));
type = CV_MAT_TYPE(iterator.hdr[0]->type);
if( CV_MAT_CN(type) > 4 )
CV_ERROR( CV_StsOutOfRange, "The input array must have at most 4 channels" );
pass_hint = CV_MAT_DEPTH(type) == CV_32F;
if( !pass_hint )
{
CvFunc2D_1A1P func = (CvFunc2D_1A1P)(sum_tab.fn_2d[type]);
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
do
{
CvScalar temp = {{0,0,0,0}};
IPPI_CALL( func( iterator.ptr[0], CV_STUB_STEP,
iterator.size, temp.val ));
sum.val[0] += temp.val[0];
sum.val[1] += temp.val[1];
sum.val[2] += temp.val[2];
sum.val[3] += temp.val[3];
}
while( cvNextNArraySlice( &iterator ));
}
else
{
CvFunc2D_1A1P1I func = (CvFunc2D_1A1P1I)(sum_tab.fn_2d[type]);
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
do
{
CvScalar temp = {{0,0,0,0}};
IPPI_CALL( func( iterator.ptr[0], CV_STUB_STEP,
iterator.size, temp.val, cvAlgHintAccurate ));
sum.val[0] += temp.val[0];
sum.val[1] += temp.val[1];
sum.val[2] += temp.val[2];
sum.val[3] += temp.val[3];
}
while( cvNextNArraySlice( &iterator ));
}
EXIT;
}
else
CV_CALL( mat = cvGetMat( mat, &stub, &coi ));
}
type = CV_MAT_TYPE(mat->type);
size = cvGetMatSize( mat );
mat_step = mat->step;
if( CV_IS_MAT_CONT( mat->type ))
{
size.width *= size.height;
if( size.width <= CV_MAX_INLINE_MAT_OP_SIZE )
{
if( type == CV_32FC1 )
{
float* data = mat->data.fl;
do
{
sum.val[0] += data[size.width - 1];
}
while( --size.width );
EXIT;
}
if( type == CV_64FC1 )
{
double* data = mat->data.db;
do
{
sum.val[0] += data[size.width - 1];
}
while( --size.width );
EXIT;
}
}
size.height = 1;
mat_step = CV_STUB_STEP;
}
if( CV_MAT_CN(type) == 1 || coi == 0 )
{
int pass_hint = CV_MAT_DEPTH(type) == CV_32F;
if( CV_MAT_CN(type) > 4 )
CV_ERROR( CV_StsOutOfRange, "The input array must have at most 4 channels" );
if( !pass_hint )
{
CvFunc2D_1A1P func = (CvFunc2D_1A1P)(sum_tab.fn_2d[type]);
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step, size, sum.val ));
}
else
{
CvFunc2D_1A1P1I func = (CvFunc2D_1A1P1I)(sum_tab.fn_2d[type]);
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step, size, sum.val, cvAlgHintAccurate ));
}
}
else
{
CvFunc2DnC_1A1P func = (CvFunc2DnC_1A1P)(sumcoi_tab.fn_2d[CV_MAT_DEPTH(type)]);
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step, size,
CV_MAT_CN(type), coi, sum.val ));
}
__END__;
return sum;
}
CV_IMPL int
cvCountNonZero( const CvArr* arr )
{
static CvFuncTable nz_tab;
static CvFuncTable nzcoi_tab;
static int inittab = 0;
int count = 0;
CV_FUNCNAME("cvCountNonZero");
__BEGIN__;
int type, coi = 0;
int mat_step;
CvSize size;
CvMat stub, *mat = (CvMat*)arr;
if( !inittab )
{
icvInitCountNonZeroC1RTable( &nz_tab );
icvInitCountNonZeroCnCRTable( &nzcoi_tab );
inittab = 1;
}
if( !CV_IS_MAT(mat) )
{
if( CV_IS_MATND(mat) )
{
void* matnd = (void*)arr;
CvMatND nstub;
CvNArrayIterator iterator;
CvFunc2D_1A1P func;
CV_CALL( cvInitNArrayIterator( 1, &matnd, 0, &nstub, &iterator ));
type = CV_MAT_TYPE(iterator.hdr[0]->type);
if( CV_MAT_CN(type) != 1 )
CV_ERROR( CV_BadNumChannels,
"Only single-channel array are supported here" );
func = (CvFunc2D_1A1P)(nz_tab.fn_2d[CV_MAT_DEPTH(type)]);
if( !func )
CV_ERROR( CV_StsUnsupportedFormat, "" );
do
{
int temp;
IPPI_CALL( func( iterator.ptr[0], CV_STUB_STEP,
iterator.size, &temp ));
count += temp;
}
while( cvNextNArraySlice( &iterator ));
EXIT;
}
else
CV_CALL( mat = cvGetMat( mat, &stub, &coi ));
}
type = CV_MAT_TYPE(mat->type);
size = cvGetMatSize( mat );
mat_step = mat->step;
if( CV_IS_MAT_CONT( mat->type ))
{
size.width *= size.height;
size.height = 1;
mat_step = CV_STUB_STEP;
}
if( CV_MAT_CN(type) == 1 || coi == 0 )
{
CvFunc2D_1A1P func = (CvFunc2D_1A1P)(nz_tab.fn_2d[CV_MAT_DEPTH(type)]);
if( CV_MAT_CN(type) != 1 )
CV_ERROR( CV_BadNumChannels,
"The function can handle only a single channel at a time (use COI)");
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step, size, &count ));
}
else
{
CvFunc2DnC_1A1P func = (CvFunc2DnC_1A1P)(nzcoi_tab.fn_2d[CV_MAT_DEPTH(type)]);
if( !func )
CV_ERROR( CV_StsBadArg, cvUnsupportedFormat );
IPPI_CALL( func( mat->data.ptr, mat_step, size, CV_MAT_CN(type), coi, &count ));
}
__END__;
return count;
}
DMZ_INTERNAL CvLinePolar llcv_hough(const CvArr *src_image, IplImage *dx, IplImage *dy, float rho, float theta, int threshold, float theta_min, float theta_max, bool vertical, float gradient_angle_threshold) {
CvMat img_stub, *img = (CvMat*)src_image;
img = cvGetMat(img, &img_stub);
CvMat dx_stub, *dx_mat = (CvMat*)dx;
dx_mat = cvGetMat(dx_mat, &dx_stub);
CvMat dy_stub, *dy_mat = (CvMat*)dy;
dy_mat = cvGetMat(dy_mat, &dy_stub);
if(!CV_IS_MASK_ARR(img)) {
CV_Error(CV_StsBadArg, "The source image must be 8-bit, single-channel");
}
if(rho <= 0 || theta <= 0 || threshold <= 0) {
CV_Error(CV_StsOutOfRange, "rho, theta and threshold must be positive");
}
if(theta_max < theta_min + theta) {
CV_Error(CV_StsBadArg, "theta + theta_min (param1) must be <= theta_max (param2)");
}
cv::AutoBuffer<int> _accum;
cv::AutoBuffer<int> _tabSin, _tabCos;
const uchar* image;
int step, width, height;
int numangle, numrho;
float ang;
int r, n;
int i, j;
float irho = 1 / rho;
float scale;
CV_Assert( CV_IS_MAT(img) && CV_MAT_TYPE(img->type) == CV_8UC1 );
image = img->data.ptr;
step = img->step;
width = img->cols;
height = img->rows;
const uint8_t *dx_mat_ptr = (uint8_t *)(dx_mat->data.ptr);
int dx_step = dx_mat->step;
const uint8_t *dy_mat_ptr = (uint8_t *)(dy_mat->data.ptr);
int dy_step = dy_mat->step;
numangle = cvRound((theta_max - theta_min) / theta);
numrho = cvRound(((width + height) * 2 + 1) / rho);
_accum.allocate((numangle+2) * (numrho+2));
_tabSin.allocate(numangle);
_tabCos.allocate(numangle);
int *accum = _accum;
int *tabSin = _tabSin, *tabCos = _tabCos;
memset(accum, 0, sizeof(accum[0]) * (numangle + 2) * (numrho + 2));
#define FIXED_POINT_EXPONENT 10
#define FIXED_POINT_MULTIPLIER (1 << FIXED_POINT_EXPONENT)
for(ang = theta_min, n = 0; n < numangle; ang += theta, n++) {
tabSin[n] = (int)floorf(FIXED_POINT_MULTIPLIER * sinf(ang) * irho);
tabCos[n] = (int)floorf(FIXED_POINT_MULTIPLIER * cosf(ang) * irho);
}
float slope_bound_a, slope_bound_b;
if(vertical) {
slope_bound_a = tanf((float)TO_RADIANS(180 - gradient_angle_threshold));
slope_bound_b = tanf((float)TO_RADIANS(180 + gradient_angle_threshold));
} else {
slope_bound_a = tanf((float)TO_RADIANS(90 - gradient_angle_threshold));
slope_bound_b = tanf((float)TO_RADIANS(90 + gradient_angle_threshold));
}
// stage 1. fill accumulator
for(i = 0; i < height; i++) {
int16_t *dx_row_ptr = (int16_t *)(dx_mat_ptr + i * dx_step);
int16_t *dy_row_ptr = (int16_t *)(dy_mat_ptr + i * dy_step);
for(j = 0; j < width; j++) {
if(image[i * step + j] != 0) {
int16_t del_x = dx_row_ptr[j];
int16_t del_y = dy_row_ptr[j];
bool use_pixel = false;
if(dmz_likely(del_x != 0)) { // avoid div by 0
float slope = (float)del_y / (float)del_x;
if(vertical) {
if(slope >= slope_bound_a && slope <= slope_bound_b) {
use_pixel = true;
}
} else {
if(slope >= slope_bound_a || slope <= slope_bound_b) {
use_pixel = true;
}
}
} else {
use_pixel = !vertical;
}
if(use_pixel) {
for(n = 0; n < numangle; n++) {
r = (j * tabCos[n] + i * tabSin[n]) >> FIXED_POINT_EXPONENT;
r += (numrho - 1) / 2;
accum[(n+1) * (numrho+2) + r+1]++;
}
}
}
}
}
static void
icvMorphOp( const void* srcarr, void* dstarr, IplConvKernel* element,
int iterations, int mop )
{
CvMorphology morphology;
void* buffer = 0;
int local_alloc = 0;
void* morphstate = 0;
CvMat* temp = 0;
CV_FUNCNAME( "icvMorphOp" );
__BEGIN__;
int i, coi1 = 0, coi2 = 0;
CvMat srcstub, *src = (CvMat*)srcarr;
CvMat dststub, *dst = (CvMat*)dstarr;
CvMat el_hdr, *el = 0;
CvSize size, el_size;
CvPoint el_anchor;
int el_shape;
int type;
bool inplace;
if( !CV_IS_MAT(src) )
CV_CALL( src = cvGetMat( src, &srcstub, &coi1 ));
if( src != &srcstub )
{
srcstub = *src;
src = &srcstub;
}
if( dstarr == srcarr )
dst = src;
else
{
CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 ));
if( !CV_ARE_TYPES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
}
if( dst != &dststub )
{
dststub = *dst;
dst = &dststub;
}
if( coi1 != 0 || coi2 != 0 )
CV_ERROR( CV_BadCOI, "" );
type = CV_MAT_TYPE( src->type );
size = cvGetMatSize( src );
inplace = src->data.ptr == dst->data.ptr;
if( iterations == 0 || (element && element->nCols == 1 && element->nRows == 1))
{
if( src->data.ptr != dst->data.ptr )
cvCopy( src, dst );
EXIT;
}
if( element )
{
el_size = cvSize( element->nCols, element->nRows );
el_anchor = cvPoint( element->anchorX, element->anchorY );
el_shape = (int)(element->nShiftR);
el_shape = el_shape < CV_SHAPE_CUSTOM ? el_shape : CV_SHAPE_CUSTOM;
}
else
{
el_size = cvSize(3,3);
el_anchor = cvPoint(1,1);
el_shape = CV_SHAPE_RECT;
}
if( el_shape == CV_SHAPE_RECT && iterations > 1 )
{
el_size.width = 1 + (el_size.width-1)*iterations;
el_size.height = 1 + (el_size.height-1)*iterations;
el_anchor.x *= iterations;
el_anchor.y *= iterations;
iterations = 1;
}
if( el_shape == CV_SHAPE_RECT && icvErodeRect_GetBufSize_8u_C1R_p )
{
CvMorphRectFunc_IPP rect_func = 0;
CvMorphRectGetBufSizeFunc_IPP rect_getbufsize_func = 0;
if( mop == 0 )
{
if( type == CV_8UC1 )
rect_getbufsize_func = icvErodeRect_GetBufSize_8u_C1R_p,
rect_func = icvErodeRect_8u_C1R_p;
else if( type == CV_8UC3 )
rect_getbufsize_func = icvErodeRect_GetBufSize_8u_C3R_p,
rect_func = icvErodeRect_8u_C3R_p;
else if( type == CV_8UC4 )
rect_getbufsize_func = icvErodeRect_GetBufSize_8u_C4R_p,
rect_func = icvErodeRect_8u_C4R_p;
else if( type == CV_16UC1 )
rect_getbufsize_func = icvErodeRect_GetBufSize_16u_C1R_p,
rect_func = icvErodeRect_16u_C1R_p;
else if( type == CV_16UC3 )
rect_getbufsize_func = icvErodeRect_GetBufSize_16u_C3R_p,
rect_func = icvErodeRect_16u_C3R_p;
else if( type == CV_16UC4 )
rect_getbufsize_func = icvErodeRect_GetBufSize_16u_C4R_p,
rect_func = icvErodeRect_16u_C4R_p;
else if( type == CV_32FC1 )
rect_getbufsize_func = icvErodeRect_GetBufSize_32f_C1R_p,
rect_func = icvErodeRect_32f_C1R_p;
else if( type == CV_32FC3 )
rect_getbufsize_func = icvErodeRect_GetBufSize_32f_C3R_p,
rect_func = icvErodeRect_32f_C3R_p;
else if( type == CV_32FC4 )
rect_getbufsize_func = icvErodeRect_GetBufSize_32f_C4R_p,
rect_func = icvErodeRect_32f_C4R_p;
}
else
{
if( type == CV_8UC1 )
rect_getbufsize_func = icvDilateRect_GetBufSize_8u_C1R_p,
rect_func = icvDilateRect_8u_C1R_p;
else if( type == CV_8UC3 )
rect_getbufsize_func = icvDilateRect_GetBufSize_8u_C3R_p,
rect_func = icvDilateRect_8u_C3R_p;
else if( type == CV_8UC4 )
rect_getbufsize_func = icvDilateRect_GetBufSize_8u_C4R_p,
rect_func = icvDilateRect_8u_C4R_p;
else if( type == CV_16UC1 )
rect_getbufsize_func = icvDilateRect_GetBufSize_16u_C1R_p,
rect_func = icvDilateRect_16u_C1R_p;
else if( type == CV_16UC3 )
rect_getbufsize_func = icvDilateRect_GetBufSize_16u_C3R_p,
rect_func = icvDilateRect_16u_C3R_p;
else if( type == CV_16UC4 )
rect_getbufsize_func = icvDilateRect_GetBufSize_16u_C4R_p,
rect_func = icvDilateRect_16u_C4R_p;
else if( type == CV_32FC1 )
rect_getbufsize_func = icvDilateRect_GetBufSize_32f_C1R_p,
rect_func = icvDilateRect_32f_C1R_p;
else if( type == CV_32FC3 )
rect_getbufsize_func = icvDilateRect_GetBufSize_32f_C3R_p,
rect_func = icvDilateRect_32f_C3R_p;
else if( type == CV_32FC4 )
rect_getbufsize_func = icvDilateRect_GetBufSize_32f_C4R_p,
rect_func = icvDilateRect_32f_C4R_p;
}
if( rect_getbufsize_func && rect_func )
{
int bufsize = 0;
CvStatus status = rect_getbufsize_func( size.width, el_size, &bufsize );
if( status >= 0 && bufsize > 0 )
{
if( bufsize < CV_MAX_LOCAL_SIZE )
{
buffer = cvStackAlloc( bufsize );
local_alloc = 1;
}
else
CV_CALL( buffer = cvAlloc( bufsize ));
}
if( status >= 0 )
{
int src_step, dst_step = dst->step ? dst->step : CV_STUB_STEP;
if( inplace )
{
CV_CALL( temp = cvCloneMat( dst ));
src = temp;
}
src_step = src->step ? src->step : CV_STUB_STEP;
status = rect_func( src->data.ptr, src_step, dst->data.ptr,
dst_step, size, el_size, el_anchor, buffer );
}
if( status >= 0 )
EXIT;
}
}
else if( el_shape == CV_SHAPE_CUSTOM && icvMorphInitAlloc_8u_C1R_p && icvMorphFree_p &&
src->data.ptr != dst->data.ptr )
{
CvMorphCustomFunc_IPP custom_func = 0;
CvMorphCustomInitAllocFunc_IPP custom_initalloc_func = 0;
const int bordertype = 1; // replication border
if( type == CV_8UC1 )
custom_initalloc_func = icvMorphInitAlloc_8u_C1R_p,
custom_func = mop == 0 ? icvErode_8u_C1R_p : icvDilate_8u_C1R_p;
else if( type == CV_8UC3 )
custom_initalloc_func = icvMorphInitAlloc_8u_C3R_p,
custom_func = mop == 0 ? icvErode_8u_C3R_p : icvDilate_8u_C3R_p;
else if( type == CV_8UC4 )
custom_initalloc_func = icvMorphInitAlloc_8u_C4R_p,
custom_func = mop == 0 ? icvErode_8u_C4R_p : icvDilate_8u_C4R_p;
else if( type == CV_16UC1 )
custom_initalloc_func = icvMorphInitAlloc_16u_C1R_p,
custom_func = mop == 0 ? icvErode_16u_C1R_p : icvDilate_16u_C1R_p;
else if( type == CV_16UC3 )
custom_initalloc_func = icvMorphInitAlloc_16u_C3R_p,
custom_func = mop == 0 ? icvErode_16u_C3R_p : icvDilate_16u_C3R_p;
else if( type == CV_16UC4 )
custom_initalloc_func = icvMorphInitAlloc_16u_C4R_p,
custom_func = mop == 0 ? icvErode_16u_C4R_p : icvDilate_16u_C4R_p;
else if( type == CV_32FC1 )
custom_initalloc_func = icvMorphInitAlloc_32f_C1R_p,
custom_func = mop == 0 ? icvErode_32f_C1R_p : icvDilate_32f_C1R_p;
else if( type == CV_32FC3 )
custom_initalloc_func = icvMorphInitAlloc_32f_C3R_p,
custom_func = mop == 0 ? icvErode_32f_C3R_p : icvDilate_32f_C3R_p;
else if( type == CV_32FC4 )
custom_initalloc_func = icvMorphInitAlloc_32f_C4R_p,
custom_func = mop == 0 ? icvErode_32f_C4R_p : icvDilate_32f_C4R_p;
if( custom_initalloc_func && custom_func )
{
uchar *src_ptr, *dst_ptr = dst->data.ptr;
int src_step, dst_step = dst->step ? dst->step : CV_STUB_STEP;
int el_len = el_size.width*el_size.height;
uchar* el_mask = (uchar*)cvStackAlloc( el_len );
CvStatus status;
for( i = 0; i < el_len; i++ )
el_mask[i] = (uchar)(element->values[i] != 0);
status = custom_initalloc_func( size.width, el_mask, el_size,
el_anchor, &morphstate );
if( status >= 0 && (inplace || iterations > 1) )
{
CV_CALL( temp = cvCloneMat( src ));
src = temp;
}
src_ptr = src->data.ptr;
src_step = src->step ? src->step : CV_STUB_STEP;
for( i = 0; i < iterations && status >= 0 && morphstate; i++ )
{
uchar* t_ptr;
int t_step;
status = custom_func( src_ptr, src_step, dst_ptr, dst_step,
size, bordertype, morphstate );
CV_SWAP( src_ptr, dst_ptr, t_ptr );
CV_SWAP( src_step, dst_step, t_step );
if( i == 0 && temp )
{
dst_ptr = temp->data.ptr;
dst_step = temp->step ? temp->step : CV_STUB_STEP;
}
}
if( status >= 0 )
{
if( iterations % 2 == 0 )
cvCopy( temp, dst );
EXIT;
}
}
}
if( el_shape != CV_SHAPE_RECT )
{
el_hdr = cvMat( element->nRows, element->nCols, CV_32SC1, element->values );
el = &el_hdr;
el_shape = CV_SHAPE_CUSTOM;
}
CV_CALL( morphology.init( mop, src->cols, src->type,
el_shape, el, el_size, el_anchor ));
for( i = 0; i < iterations; i++ )
{
CV_CALL( morphology.process( src, dst ));
src = dst;
}
__END__;
if( !local_alloc )
cvFree( &buffer );
if( morphstate )
icvMorphFree_p( morphstate );
cvReleaseMat( &temp );
}
void CvMorphology::init_binary_element( CvMat* element, int element_shape, CvPoint anchor )
{
CV_FUNCNAME( "CvMorphology::init_binary_element" );
__BEGIN__;
int type;
int i, j, cols, rows;
int r = 0, c = 0;
double inv_r2 = 0;
if( !CV_IS_MAT(element) )
CV_ERROR( CV_StsBadArg, "element must be valid matrix" );
type = CV_MAT_TYPE(element->type);
if( type != CV_8UC1 && type != CV_32SC1 )
CV_ERROR( CV_StsUnsupportedFormat, "element must have 8uC1 or 32sC1 type" );
if( anchor.x == -1 )
anchor.x = element->cols/2;
if( anchor.y == -1 )
anchor.y = element->rows/2;
if( (unsigned)anchor.x >= (unsigned)element->cols ||
(unsigned)anchor.y >= (unsigned)element->rows )
CV_ERROR( CV_StsOutOfRange, "anchor is outside of element" );
if( element_shape != RECT && element_shape != CROSS && element_shape != ELLIPSE )
CV_ERROR( CV_StsBadArg, "Unknown/unsupported element shape" );
rows = element->rows;
cols = element->cols;
if( rows == 1 || cols == 1 )
element_shape = RECT;
if( element_shape == ELLIPSE )
{
r = rows/2;
c = cols/2;
inv_r2 = r ? 1./((double)r*r) : 0;
}
for( i = 0; i < rows; i++ )
{
uchar* ptr = element->data.ptr + i*element->step;
int j1 = 0, j2 = 0, jx, t = 0;
if( element_shape == RECT || (element_shape == CROSS && i == anchor.y) )
j2 = cols;
else if( element_shape == CROSS )
j1 = anchor.x, j2 = j1 + 1;
else
{
int dy = i - r;
if( abs(dy) <= r )
{
int dx = cvRound(c*sqrt(((double)r*r - dy*dy)*inv_r2));
j1 = MAX( c - dx, 0 );
j2 = MIN( c + dx + 1, cols );
}
}
for( j = 0, jx = j1; j < cols; )
{
for( ; j < jx; j++ )
{
if( type == CV_8UC1 )
ptr[j] = (uchar)t;
else
((int*)ptr)[j] = t;
}
if( jx == j2 )
jx = cols, t = 0;
else
jx = j2, t = 1;
}
}
__END__;
}
CV_IMPL void
cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr,
CvMat *rotMatr, CvMat *posVect,
CvMat *rotMatrX, CvMat *rotMatrY,
CvMat *rotMatrZ, CvPoint3D64f *eulerAngles)
{
CvMat *tmpProjMatr = 0;
CvMat *tmpMatrixD = 0;
CvMat *tmpMatrixV = 0;
CvMat *tmpMatrixM = 0;
CV_FUNCNAME("cvDecomposeProjectionMatrix");
__BEGIN__;
/* Validate parameters. */
if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0)
CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect))
CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
if(projMatr->cols != 4 || projMatr->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!");
if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!");
if(posVect->cols != 1 || posVect->rows != 4)
CV_ERROR(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!");
CV_CALL(tmpProjMatr = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixD = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixV = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixM = cvCreateMat(3, 3, CV_64F));
/* Compute position vector. */
cvSetZero(tmpProjMatr); // Add zero row to make matrix square.
int i, k;
for(i = 0; i < 3; i++)
for(k = 0; k < 4; k++)
cvmSet(tmpProjMatr, i, k, cvmGet(projMatr, i, k));
CV_CALL(cvSVD(tmpProjMatr, tmpMatrixD, NULL, tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T));
/* Save position vector. */
for(i = 0; i < 4; i++)
cvmSet(posVect, i, 0, cvmGet(tmpMatrixV, 3, i)); // Solution is last row of V.
/* Compute calibration and rotation matrices via RQ decomposition. */
cvGetCols(projMatr, tmpMatrixM, 0, 3); // M is first square matrix of P.
assert(cvDet(tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0].
CV_CALL(cvRQDecomp3x3(tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles));
__END__;
cvReleaseMat(&tmpProjMatr);
cvReleaseMat(&tmpMatrixD);
cvReleaseMat(&tmpMatrixV);
cvReleaseMat(&tmpMatrixM);
}
bool CvANN_MLP::prepare_to_train( const CvMat* _inputs, const CvMat* _outputs,
const CvMat* _sample_weights, const CvMat* _sample_idx,
CvVectors* _ivecs, CvVectors* _ovecs, double** _sw, int _flags )
{
bool ok = false;
CvMat* sample_idx = 0;
CvVectors ivecs, ovecs;
double* sw = 0;
int count = 0;
CV_FUNCNAME( "CvANN_MLP::prepare_to_train" );
ivecs.data.ptr = ovecs.data.ptr = 0;
assert( _ivecs && _ovecs );
__BEGIN__;
const int* sidx = 0;
int i, sw_type = 0, sw_count = 0;
int sw_step = 0;
double sw_sum = 0;
if( !layer_sizes )
CV_ERROR( CV_StsError,
"The network has not been created. Use method create or the appropriate constructor" );
if( !CV_IS_MAT(_inputs) || CV_MAT_TYPE(_inputs->type) != CV_32FC1 &&
CV_MAT_TYPE(_inputs->type) != CV_64FC1 || _inputs->cols != layer_sizes->data.i[0] )
CV_ERROR( CV_StsBadArg,
"input training data should be a floating-point matrix with"
"the number of rows equal to the number of training samples and "
"the number of columns equal to the size of 0-th (input) layer" );
if( !CV_IS_MAT(_outputs) || CV_MAT_TYPE(_outputs->type) != CV_32FC1 &&
CV_MAT_TYPE(_outputs->type) != CV_64FC1 ||
_outputs->cols != layer_sizes->data.i[layer_sizes->cols - 1] )
CV_ERROR( CV_StsBadArg,
"output training data should be a floating-point matrix with"
"the number of rows equal to the number of training samples and "
"the number of columns equal to the size of last (output) layer" );
if( _inputs->rows != _outputs->rows )
CV_ERROR( CV_StsUnmatchedSizes, "The numbers of input and output samples do not match" );
if( _sample_idx )
{
CV_CALL( sample_idx = cvPreprocessIndexArray( _sample_idx, _inputs->rows ));
sidx = sample_idx->data.i;
count = sample_idx->cols + sample_idx->rows - 1;
}
else
count = _inputs->rows;
if( _sample_weights )
{
if( !CV_IS_MAT(_sample_weights) )
CV_ERROR( CV_StsBadArg, "sample_weights (if passed) must be a valid matrix" );
sw_type = CV_MAT_TYPE(_sample_weights->type);
sw_count = _sample_weights->cols + _sample_weights->rows - 1;
if( sw_type != CV_32FC1 && sw_type != CV_64FC1 ||
_sample_weights->cols != 1 && _sample_weights->rows != 1 ||
sw_count != count && sw_count != _inputs->rows )
CV_ERROR( CV_StsBadArg,
"sample_weights must be 1d floating-point vector containing weights "
"of all or selected training samples" );
sw_step = CV_IS_MAT_CONT(_sample_weights->type) ? 1 :
_sample_weights->step/CV_ELEM_SIZE(sw_type);
CV_CALL( sw = (double*)cvAlloc( count*sizeof(sw[0]) ));
}
CV_CALL( ivecs.data.ptr = (uchar**)cvAlloc( count*sizeof(ivecs.data.ptr[0]) ));
CV_CALL( ovecs.data.ptr = (uchar**)cvAlloc( count*sizeof(ovecs.data.ptr[0]) ));
ivecs.type = CV_MAT_TYPE(_inputs->type);
ovecs.type = CV_MAT_TYPE(_outputs->type);
ivecs.count = ovecs.count = count;
for( i = 0; i < count; i++ )
{
int idx = sidx ? sidx[i] : i;
ivecs.data.ptr[i] = _inputs->data.ptr + idx*_inputs->step;
ovecs.data.ptr[i] = _outputs->data.ptr + idx*_outputs->step;
if( sw )
{
int si = sw_count == count ? i : idx;
double w = sw_type == CV_32FC1 ?
(double)_sample_weights->data.fl[si*sw_step] :
_sample_weights->data.db[si*sw_step];
sw[i] = w;
if( w < 0 )
CV_ERROR( CV_StsOutOfRange, "some of sample weights are negative" );
sw_sum += w;
}
}
// normalize weights
if( sw )
{
sw_sum = sw_sum > DBL_EPSILON ? 1./sw_sum : 0;
for( i = 0; i < count; i++ )
sw[i] *= sw_sum;
}
calc_input_scale( &ivecs, _flags );
CV_CALL( calc_output_scale( &ovecs, _flags ));
ok = true;
__END__;
if( !ok )
{
cvFree( &ivecs.data.ptr );
cvFree( &ovecs.data.ptr );
cvFree( &sw );
}
cvReleaseMat( &sample_idx );
*_ivecs = ivecs;
*_ovecs = ovecs;
*_sw = sw;
return ok;
}
CV_IMPL void
cvUndistort2( const CvArr* _src, CvArr* _dst, const CvMat* A, const CvMat* dist_coeffs )
{
static int inittab = 0;
uchar* buffer = 0;
CV_FUNCNAME( "cvUndistort2" );
__BEGIN__;
float a[9], k[4];
int coi1 = 0, coi2 = 0;
CvMat srcstub, *src = (CvMat*)_src;
CvMat dststub, *dst = (CvMat*)_dst;
CvMat _a = cvMat( 3, 3, CV_32F, a ), _k;
int cn, src_step, dst_step;
CvSize size;
if( !inittab )
{
icvInitLinearCoeffTab();
icvInitCubicCoeffTab();
inittab = 1;
}
CV_CALL( src = cvGetMat( src, &srcstub, &coi1 ));
CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 ));
if( coi1 != 0 || coi2 != 0 )
CV_ERROR( CV_BadCOI, "The function does not support COI" );
if( CV_MAT_DEPTH(src->type) != CV_8U )
CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit images are supported" );
if( src->data.ptr == dst->data.ptr )
CV_ERROR( CV_StsNotImplemented, "In-place undistortion is not implemented" );
if( !CV_ARE_TYPES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 ||
CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1 )
CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
if( !CV_IS_MAT(dist_coeffs) || dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
cvConvert( A, &_a );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k );
cvConvert( dist_coeffs, &_k );
cn = CV_MAT_CN(src->type);
size = cvGetMatSize(src);
src_step = src->step ? src->step : CV_STUB_STEP;
dst_step = dst->step ? dst->step : CV_STUB_STEP;
if( fabs((double)k[2]) < 1e-5 && fabs((double)k[3]) < 1e-5 && icvUndistortGetSize_p )
{
int buf_size = 0;
CvUndistortRadialIPPFunc func =
cn == 1 ? (CvUndistortRadialIPPFunc)icvUndistortRadial_8u_C1R_p :
(CvUndistortRadialIPPFunc)icvUndistortRadial_8u_C3R_p;
if( func && icvUndistortGetSize_p( size, &buf_size ) >= 0 && buf_size > 0 )
{
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
if( func( src->data.ptr, src_step, dst->data.ptr,
dst_step, size, a[0], a[4],
a[2], a[5], k[0], k[1], buffer ) >= 0 )
EXIT;
}
}
icvUnDistort_8u_CnR( src->data.ptr, src_step,
dst->data.ptr, dst_step, size, a, k, cn );
__END__;
cvFree( &buffer );
}
CV_IMPL void
cvGetQuadrangleSubPix( const void* srcarr, void* dstarr, const CvMat* mat )
{
static CvFuncTable gq_tab[2];
static int inittab = 0;
CvMat srcstub, *src = (CvMat*)srcarr;
CvMat dststub, *dst = (CvMat*)dstarr;
CvSize src_size, dst_size;
CvGetQuadrangleSubPixFunc func;
float m[6];
int k, cn;
if( !inittab )
{
icvInitGetQuadrangleSubPixC1RTable( gq_tab + 0 );
icvInitGetQuadrangleSubPixC3RTable( gq_tab + 1 );
inittab = 1;
}
if( !CV_IS_MAT(src))
src = cvGetMat( src, &srcstub );
if( !CV_IS_MAT(dst))
dst = cvGetMat( dst, &dststub );
if( !CV_IS_MAT(mat))
CV_Error( CV_StsBadArg, "map matrix is not valid" );
cn = CV_MAT_CN( src->type );
if( (cn != 1 && cn != 3) || !CV_ARE_CNS_EQ( src, dst ))
CV_Error( CV_StsUnsupportedFormat, "" );
src_size = cvGetMatSize( src );
dst_size = cvGetMatSize( dst );
/*if( dst_size.width > src_size.width || dst_size.height > src_size.height )
CV_ERROR( CV_StsBadSize, "destination ROI must not be larger than source ROI" );*/
if( mat->rows != 2 || mat->cols != 3 )
CV_Error( CV_StsBadArg,
"Transformation matrix must be 2x3" );
if( CV_MAT_TYPE( mat->type ) == CV_32FC1 )
{
for( k = 0; k < 3; k++ )
{
m[k] = mat->data.fl[k];
m[3 + k] = ((float*)(mat->data.ptr + mat->step))[k];
}
}
else if( CV_MAT_TYPE( mat->type ) == CV_64FC1 )
{
for( k = 0; k < 3; k++ )
{
m[k] = (float)mat->data.db[k];
m[3 + k] = (float)((double*)(mat->data.ptr + mat->step))[k];
}
}
else
CV_Error( CV_StsUnsupportedFormat,
"The transformation matrix should have 32fC1 or 64fC1 type" );
if( CV_ARE_DEPTHS_EQ( src, dst ))
{
func = (CvGetQuadrangleSubPixFunc)(gq_tab[cn != 1].fn_2d[CV_MAT_DEPTH(src->type)]);
}
else
{
if( CV_MAT_DEPTH( src->type ) != CV_8U || CV_MAT_DEPTH( dst->type ) != CV_32F )
CV_Error( CV_StsUnsupportedFormat, "" );
func = (CvGetQuadrangleSubPixFunc)(gq_tab[cn != 1].fn_2d[1]);
}
if( !func )
CV_Error( CV_StsUnsupportedFormat, "" );
IPPI_CALL( func( src->data.ptr, src->step, src_size,
dst->data.ptr, dst->step, dst_size, m ));
}
/*
Finds real roots of cubic, quadratic or linear equation.
The original code has been taken from Ken Turkowski web page
(http://www.worldserver.com/turk/opensource/) and adopted for OpenCV.
Here is the copyright notice.
-----------------------------------------------------------------------
Copyright (C) 1978-1999 Ken Turkowski. <*****@*****.**>
All rights reserved.
Warranty Information
Even though I have reviewed this software, I make no warranty
or representation, either express or implied, with respect to this
software, its quality, accuracy, merchantability, or fitness for a
particular purpose. As a result, this software is provided "as is,"
and you, its user, are assuming the entire risk as to its quality
and accuracy.
This code may be used and freely distributed as long as it includes
this copyright notice and the above warranty information.
-----------------------------------------------------------------------
*/
CV_IMPL int
cvSolveCubic( const CvMat* coeffs, CvMat* roots )
{
int n = 0;
CV_FUNCNAME( "cvSolveCubic" );
__BEGIN__;
double a0 = 1., a1, a2, a3;
double x0 = 0., x1 = 0., x2 = 0.;
int step = 1, coeff_count;
if( !CV_IS_MAT(coeffs) )
CV_ERROR( !coeffs ? CV_StsNullPtr : CV_StsBadArg, "Input parameter is not a valid matrix" );
if( !CV_IS_MAT(roots) )
CV_ERROR( !roots ? CV_StsNullPtr : CV_StsBadArg, "Output parameter is not a valid matrix" );
if( CV_MAT_TYPE(coeffs->type) != CV_32FC1 && CV_MAT_TYPE(coeffs->type) != CV_64FC1 ||
CV_MAT_TYPE(roots->type) != CV_32FC1 && CV_MAT_TYPE(roots->type) != CV_64FC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"Both matrices should be floating-point (single or double precision)" );
coeff_count = coeffs->rows + coeffs->cols - 1;
if( coeffs->rows != 1 && coeffs->cols != 1 || coeff_count != 3 && coeff_count != 4 )
CV_ERROR( CV_StsBadSize,
"The matrix of coefficients must be 1-dimensional vector of 3 or 4 elements" );
if( roots->rows != 1 && roots->cols != 1 ||
roots->rows + roots->cols - 1 != 3 )
CV_ERROR( CV_StsBadSize,
"The matrix of roots must be 1-dimensional vector of 3 elements" );
if( CV_MAT_TYPE(coeffs->type) == CV_32FC1 )
{
const float* c = coeffs->data.fl;
if( coeffs->rows > 1 )
step = coeffs->step/sizeof(c[0]);
if( coeff_count == 4 )
a0 = c[0], c += step;
a1 = c[0];
a2 = c[step];
a3 = c[step*2];
}
else
{
const double* c = coeffs->data.db;
if( coeffs->rows > 1 )
step = coeffs->step/sizeof(c[0]);
if( coeff_count == 4 )
a0 = c[0], c += step;
a1 = c[0];
a2 = c[step];
a3 = c[step*2];
}
if( a0 == 0 )
{
if( a1 == 0 )
{
if( a2 == 0 )
n = a3 == 0 ? -1 : 0;
else
{
// linear equation
x0 = a3/a2;
n = 1;
}
}
else
{
// quadratic equation
double d = a2*a2 - 4*a1*a3;
if( d >= 0 )
{
d = sqrt(d);
double q = (-a2 + (a2 < 0 ? -d : d)) * 0.5;
x0 = q / a1;
x1 = a3 / q;
n = d > 0 ? 2 : 1;
}
}
}
else
{
a0 = 1./a0;
a1 *= a0;
a2 *= a0;
a3 *= a0;
double Q = (a1 * a1 - 3 * a2) * (1./9);
double R = (2 * a1 * a1 * a1 - 9 * a1 * a2 + 27 * a3) * (1./54);
double Qcubed = Q * Q * Q;
double d = Qcubed - R * R;
if( d >= 0 )
{
double theta = acos(R / sqrt(Qcubed));
double sqrtQ = sqrt(Q);
double t0 = -2 * sqrtQ;
double t1 = theta * (1./3);
double t2 = a1 * (1./3);
x0 = t0 * cos(t1) - t2;
x1 = t0 * cos(t1 + (2.*CV_PI/3)) - t2;
x2 = t0 * cos(t1 + (4.*CV_PI/3)) - t2;
n = 3;
}
else
{
double e;
d = sqrt(-d);
e = pow(d + fabs(R), 0.333333333333);
if( R > 0 )
e = -e;
x0 = (e + Q / e) - a1 * (1./3);
n = 1;
}
}
step = 1;
if( CV_MAT_TYPE(roots->type) == CV_32FC1 )
{
float* r = roots->data.fl;
if( roots->rows > 1 )
step = roots->step/sizeof(r[0]);
r[0] = (float)x0;
r[step] = (float)x1;
r[step*2] = (float)x2;
}
else
{
double* r = roots->data.db;
if( roots->rows > 1 )
step = roots->step/sizeof(r[0]);
r[0] = x0;
r[step] = x1;
r[step*2] = x2;
}
__END__;
return n;
}
/* Warps source into destination by a perspective transform */
static void cvWarpPerspective( CvArr* src, CvArr* dst, double quad[4][2] )
{
CV_FUNCNAME( "cvWarpPerspective" );
__BEGIN__;
#ifdef __IPL_H__
IplImage src_stub, dst_stub;
IplImage* src_img;
IplImage* dst_img;
CV_CALL( src_img = cvGetImage( src, &src_stub ) );
CV_CALL( dst_img = cvGetImage( dst, &dst_stub ) );
iplWarpPerspectiveQ( src_img, dst_img, quad, IPL_WARP_R_TO_Q,
IPL_INTER_CUBIC | IPL_SMOOTH_EDGE );
#else
int fill_value = 0;
double c[3][3]; /* transformation coefficients */
double q[4][2]; /* rearranged quad */
int left = 0;
int right = 0;
int next_right = 0;
int next_left = 0;
double y_min = 0;
double y_max = 0;
double k_left, b_left, k_right, b_right;
uchar* src_data;
int src_step;
CvSize src_size;
uchar* dst_data;
int dst_step;
CvSize dst_size;
double d = 0;
int direction = 0;
int i;
if( !src || (!CV_IS_IMAGE( src ) && !CV_IS_MAT( src )) ||
cvGetElemType( src ) != CV_8UC1 ||
cvGetDims( src ) != 2 )
{
CV_ERROR( CV_StsBadArg,
"Source must be two-dimensional array of CV_8UC1 type." );
}
if( !dst || (!CV_IS_IMAGE( dst ) && !CV_IS_MAT( dst )) ||
cvGetElemType( dst ) != CV_8UC1 ||
cvGetDims( dst ) != 2 )
{
CV_ERROR( CV_StsBadArg,
"Destination must be two-dimensional array of CV_8UC1 type." );
}
CV_CALL( cvGetRawData( src, &src_data, &src_step, &src_size ) );
CV_CALL( cvGetRawData( dst, &dst_data, &dst_step, &dst_size ) );
CV_CALL( cvGetPerspectiveTransform( src_size, quad, c ) );
/* if direction > 0 then vertices in quad follow in a CW direction,
otherwise they follow in a CCW direction */
direction = 0;
for( i = 0; i < 4; ++i )
{
int ni = i + 1; if( ni == 4 ) ni = 0;
int pi = i - 1; if( pi == -1 ) pi = 3;
d = (quad[i][0] - quad[pi][0])*(quad[ni][1] - quad[i][1]) -
(quad[i][1] - quad[pi][1])*(quad[ni][0] - quad[i][0]);
int cur_direction = CV_SIGN(d);
if( direction == 0 )
{
direction = cur_direction;
}
else if( direction * cur_direction < 0 )
{
direction = 0;
break;
}
}
if( direction == 0 )
{
CV_ERROR( CV_StsBadArg, "Quadrangle is nonconvex or degenerated." );
}
/* <left> is the index of the topmost quad vertice
if there are two such vertices <left> is the leftmost one */
left = 0;
for( i = 1; i < 4; ++i )
{
if( (quad[i][1] < quad[left][1]) ||
((quad[i][1] == quad[left][1]) && (quad[i][0] < quad[left][0])) )
{
left = i;
}
}
/* rearrange <quad> vertices in such way that they follow in a CW
direction and the first vertice is the topmost one and put them
into <q> */
if( direction > 0 )
{
for( i = left; i < 4; ++i )
{
q[i-left][0] = quad[i][0];
q[i-left][1] = quad[i][1];
}
for( i = 0; i < left; ++i )
{
q[4-left+i][0] = quad[i][0];
q[4-left+i][1] = quad[i][1];
}
}
else
{
for( i = left; i >= 0; --i )
{
q[left-i][0] = quad[i][0];
q[left-i][1] = quad[i][1];
}
for( i = 3; i > left; --i )
{
q[4+left-i][0] = quad[i][0];
q[4+left-i][1] = quad[i][1];
}
}
left = right = 0;
/* if there are two topmost points, <right> is the index of the rightmost one
otherwise <right> */
if( q[left][1] == q[left+1][1] )
{
right = 1;
}
/* <next_left> follows <left> in a CCW direction */
next_left = 3;
/* <next_right> follows <right> in a CW direction */
next_right = right + 1;
/* subtraction of 1 prevents skipping of the first row */
y_min = q[left][1] - 1;
/* left edge equation: y = k_left * x + b_left */
k_left = (q[left][0] - q[next_left][0]) /
(q[left][1] - q[next_left][1]);
b_left = (q[left][1] * q[next_left][0] -
q[left][0] * q[next_left][1]) /
(q[left][1] - q[next_left][1]);
/* right edge equation: y = k_right * x + b_right */
k_right = (q[right][0] - q[next_right][0]) /
(q[right][1] - q[next_right][1]);
b_right = (q[right][1] * q[next_right][0] -
q[right][0] * q[next_right][1]) /
(q[right][1] - q[next_right][1]);
for(;;)
{
int x, y;
y_max = MIN( q[next_left][1], q[next_right][1] );
int iy_min = MAX( cvRound(y_min), 0 ) + 1;
int iy_max = MIN( cvRound(y_max), dst_size.height - 1 );
double x_min = k_left * iy_min + b_left;
double x_max = k_right * iy_min + b_right;
/* walk through the destination quadrangle row by row */
for( y = iy_min; y <= iy_max; ++y )
{
int ix_min = MAX( cvRound( x_min ), 0 );
int ix_max = MIN( cvRound( x_max ), dst_size.width - 1 );
for( x = ix_min; x <= ix_max; ++x )
{
/* calculate coordinates of the corresponding source array point */
double div = (c[2][0] * x + c[2][1] * y + c[2][2]);
double src_x = (c[0][0] * x + c[0][1] * y + c[0][2]) / div;
double src_y = (c[1][0] * x + c[1][1] * y + c[1][2]) / div;
int isrc_x = cvFloor( src_x );
int isrc_y = cvFloor( src_y );
double delta_x = src_x - isrc_x;
double delta_y = src_y - isrc_y;
uchar* s = src_data + isrc_y * src_step + isrc_x;
int i00, i10, i01, i11;
i00 = i10 = i01 = i11 = (int) fill_value;
/* linear interpolation using 2x2 neighborhood */
if( isrc_x >= 0 && isrc_x <= src_size.width &&
isrc_y >= 0 && isrc_y <= src_size.height )
{
i00 = s[0];
}
if( isrc_x >= -1 && isrc_x < src_size.width &&
isrc_y >= 0 && isrc_y <= src_size.height )
{
i10 = s[1];
}
if( isrc_x >= 0 && isrc_x <= src_size.width &&
isrc_y >= -1 && isrc_y < src_size.height )
{
i01 = s[src_step];
}
if( isrc_x >= -1 && isrc_x < src_size.width &&
isrc_y >= -1 && isrc_y < src_size.height )
{
i11 = s[src_step+1];
}
double i0 = i00 + (i10 - i00)*delta_x;
double i1 = i01 + (i11 - i01)*delta_x;
((uchar*)(dst_data + y * dst_step))[x] = (uchar) (i0 + (i1 - i0)*delta_y);
}
x_min += k_left;
x_max += k_right;
}
if( (next_left == next_right) ||
(next_left+1 == next_right && q[next_left][1] == q[next_right][1]) )
{
break;
}
if( y_max == q[next_left][1] )
{
left = next_left;
next_left = left - 1;
k_left = (q[left][0] - q[next_left][0]) /
(q[left][1] - q[next_left][1]);
b_left = (q[left][1] * q[next_left][0] -
q[left][0] * q[next_left][1]) /
(q[left][1] - q[next_left][1]);
}
if( y_max == q[next_right][1] )
{
right = next_right;
next_right = right + 1;
k_right = (q[right][0] - q[next_right][0]) /
(q[right][1] - q[next_right][1]);
b_right = (q[right][1] * q[next_right][0] -
q[right][0] * q[next_right][1]) /
(q[right][1] - q[next_right][1]);
}
y_min = y_max;
}
#endif /* #ifndef __IPL_H__ */
__END__;
}
CV_IMPL void
cvKMeans2( const CvArr* samples_arr, int cluster_count,
CvArr* labels_arr, CvTermCriteria termcrit )
{
CvMat* centers = 0;
CvMat* old_centers = 0;
CvMat* counters = 0;
CV_FUNCNAME( "cvKMeans2" );
__BEGIN__;
CvMat samples_stub, labels_stub;
CvMat* samples = (CvMat*)samples_arr;
CvMat* labels = (CvMat*)labels_arr;
CvMat* temp = 0;
CvRNG rng = CvRNG(-1);
int i, j, k, sample_count, dims;
int ids_delta, iter;
double max_dist;
if( !CV_IS_MAT( samples ))
CV_CALL( samples = cvGetMat( samples, &samples_stub ));
if( !CV_IS_MAT( labels ))
CV_CALL( labels = cvGetMat( labels, &labels_stub ));
if( cluster_count < 1 )
CV_ERROR( CV_StsOutOfRange, "Number of clusters should be positive" );
if( CV_MAT_DEPTH(samples->type) != CV_32F || CV_MAT_TYPE(labels->type) != CV_32SC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"samples should be floating-point matrix, cluster_idx - integer vector" );
if( labels->rows != 1 && (labels->cols != 1 || !CV_IS_MAT_CONT(labels->type)) ||
labels->rows + labels->cols - 1 != samples->rows )
CV_ERROR( CV_StsUnmatchedSizes,
"cluster_idx should be 1D vector of the same number of elements as samples' number of rows" );
CV_CALL( termcrit = cvCheckTermCriteria( termcrit, 1e-6, 100 ));
termcrit.epsilon *= termcrit.epsilon;
sample_count = samples->rows;
if( cluster_count > sample_count )
cluster_count = sample_count;
dims = samples->cols*CV_MAT_CN(samples->type);
ids_delta = labels->step ? labels->step/(int)sizeof(int) : 1;
CV_CALL( centers = cvCreateMat( cluster_count, dims, CV_64FC1 ));
CV_CALL( old_centers = cvCreateMat( cluster_count, dims, CV_64FC1 ));
CV_CALL( counters = cvCreateMat( 1, cluster_count, CV_32SC1 ));
// init centers
for( i = 0; i < sample_count; i++ )
labels->data.i[i] = cvRandInt(&rng) % cluster_count;
counters->cols = cluster_count; // cut down counters
max_dist = termcrit.epsilon*2;
for( iter = 0; iter < termcrit.max_iter; iter++ )
{
// computer centers
cvZero( centers );
cvZero( counters );
for( i = 0; i < sample_count; i++ )
{
float* s = (float*)(samples->data.ptr + i*samples->step);
k = labels->data.i[i*ids_delta];
double* c = (double*)(centers->data.ptr + k*centers->step);
for( j = 0; j <= dims - 4; j += 4 )
{
double t0 = c[j] + s[j];
double t1 = c[j+1] + s[j+1];
c[j] = t0;
c[j+1] = t1;
t0 = c[j+2] + s[j+2];
t1 = c[j+3] + s[j+3];
c[j+2] = t0;
c[j+3] = t1;
}
for( ; j < dims; j++ )
c[j] += s[j];
counters->data.i[k]++;
}
if( iter > 0 )
max_dist = 0;
for( k = 0; k < cluster_count; k++ )
{
double* c = (double*)(centers->data.ptr + k*centers->step);
if( counters->data.i[k] != 0 )
{
double scale = 1./counters->data.i[k];
for( j = 0; j < dims; j++ )
c[j] *= scale;
}
else
{
i = cvRandInt( &rng ) % sample_count;
float* s = (float*)(samples->data.ptr + i*samples->step);
for( j = 0; j < dims; j++ )
c[j] = s[j];
}
if( iter > 0 )
{
double dist = 0;
double* c_o = (double*)(old_centers->data.ptr + k*old_centers->step);
for( j = 0; j < dims; j++ )
{
double t = c[j] - c_o[j];
dist += t*t;
}
if( max_dist < dist )
max_dist = dist;
}
}
// assign labels
for( i = 0; i < sample_count; i++ )
{
float* s = (float*)(samples->data.ptr + i*samples->step);
int k_best = 0;
double min_dist = DBL_MAX;
for( k = 0; k < cluster_count; k++ )
{
double* c = (double*)(centers->data.ptr + k*centers->step);
double dist = 0;
j = 0;
for( ; j <= dims - 4; j += 4 )
{
double t0 = c[j] - s[j];
double t1 = c[j+1] - s[j+1];
dist += t0*t0 + t1*t1;
t0 = c[j+2] - s[j+2];
t1 = c[j+3] - s[j+3];
dist += t0*t0 + t1*t1;
}
for( ; j < dims; j++ )
{
double t = c[j] - s[j];
dist += t*t;
}
if( min_dist > dist )
{
min_dist = dist;
k_best = k;
}
}
labels->data.i[i*ids_delta] = k_best;
}
if( max_dist < termcrit.epsilon )
break;
CV_SWAP( centers, old_centers, temp );
}
cvZero( counters );
for( i = 0; i < sample_count; i++ )
counters->data.i[labels->data.i[i]]++;
// ensure that we do not have empty clusters
for( k = 0; k < cluster_count; k++ )
if( counters->data.i[k] == 0 )
for(;;)
{
i = cvRandInt(&rng) % sample_count;
j = labels->data.i[i];
if( counters->data.i[j] > 1 )
{
labels->data.i[i] = k;
counters->data.i[j]--;
counters->data.i[k]++;
break;
}
}
__END__;
cvReleaseMat( ¢ers );
cvReleaseMat( &old_centers );
cvReleaseMat( &counters );
}
bool CvCalibFilter::FindEtalon( CvMat** mats )
{
bool result = true;
if( !mats || etalonPointCount == 0 )
{
assert(0);
result = false;
}
if( result )
{
int i, tempPointCount0 = etalonPointCount*2;
for( i = 0; i < cameraCount; i++ )
{
if( !latestPoints[i] )
latestPoints[i] = (CvPoint2D32f*)
cvAlloc( tempPointCount0*2*sizeof(latestPoints[0]));
}
for( i = 0; i < cameraCount; i++ )
{
CvSize size;
int tempPointCount = tempPointCount0;
bool found = false;
if( !CV_IS_MAT(mats[i]) && !CV_IS_IMAGE(mats[i]))
{
assert(0);
break;
}
size = cvGetSize(mats[i]);
if( size.width != imgSize.width || size.height != imgSize.height )
{
imgSize = size;
}
if( !grayImg || grayImg->width != imgSize.width ||
grayImg->height != imgSize.height )
{
cvReleaseMat( &grayImg );
cvReleaseMat( &tempImg );
grayImg = cvCreateMat( imgSize.height, imgSize.width, CV_8UC1 );
tempImg = cvCreateMat( imgSize.height, imgSize.width, CV_8UC1 );
}
if( !storage )
storage = cvCreateMemStorage();
switch( etalonType )
{
case CV_CALIB_ETALON_CHESSBOARD:
if( CV_MAT_CN(cvGetElemType(mats[i])) == 1 )
cvCopy( mats[i], grayImg );
else
cvCvtColor( mats[i], grayImg, CV_BGR2GRAY );
found = cvFindChessBoardCornerGuesses( grayImg, tempImg, storage,
cvSize( cvRound(etalonParams[0]),
cvRound(etalonParams[1])),
latestPoints[i], &tempPointCount ) != 0;
if( found )
cvFindCornerSubPix( grayImg, latestPoints[i], tempPointCount,
cvSize(5,5), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,10,0.1));
break;
default:
assert(0);
result = false;
break;
}
latestCounts[i] = found ? tempPointCount : -tempPointCount;
result = result && found;
}
}
if( storage )
cvClearMemStorage( storage );
return result;
}
CV_IMPL CvArr*
cvRange( CvArr* arr, double start, double end )
{
int ok = 0;
CV_FUNCNAME( "cvRange" );
__BEGIN__;
CvMat stub, *mat = (CvMat*)arr;
double delta;
int type, step;
double val = start;
int i, j;
int rows, cols;
if( !CV_IS_MAT(mat) )
CV_CALL( mat = cvGetMat( mat, &stub) );
rows = mat->rows;
cols = mat->cols;
type = CV_MAT_TYPE(mat->type);
delta = (end-start)/(rows*cols);
if( CV_IS_MAT_CONT(mat->type) )
{
cols *= rows;
rows = 1;
step = 1;
}
else
step = mat->step / CV_ELEM_SIZE(type);
if( type == CV_32SC1 )
{
int* idata = mat->data.i;
int ival = cvRound(val), idelta = cvRound(delta);
if( fabs(val - ival) < DBL_EPSILON &&
fabs(delta - idelta) < DBL_EPSILON )
{
for( i = 0; i < rows; i++, idata += step )
for( j = 0; j < cols; j++, ival += idelta )
idata[j] = ival;
}
else
{
for( i = 0; i < rows; i++, idata += step )
for( j = 0; j < cols; j++, val += delta )
idata[j] = cvRound(val);
}
}
else if( type == CV_32FC1 )
{
float* fdata = mat->data.fl;
for( i = 0; i < rows; i++, fdata += step )
for( j = 0; j < cols; j++, val += delta )
fdata[j] = (float)val;
}
else
CV_ERROR( CV_StsUnsupportedFormat, "The function only supports 32sC1 and 32fC1 datatypes" );
ok = 1;
__END__;
return ok ? arr : 0;
}
CV_IMPL void
cvCopyMakeBorder( const CvArr* srcarr, CvArr* dstarr, CvPoint offset,
int bordertype, CvScalar value )
{
CV_FUNCNAME( "cvCopyMakeBorder" );
__BEGIN__;
CvMat srcstub, *src = (CvMat*)srcarr;
CvMat dststub, *dst = (CvMat*)dstarr;
CvSize srcsize, dstsize;
int srcstep, dststep;
int pix_size, type;
if( !CV_IS_MAT(src) )
CV_CALL( src = cvGetMat( src, &srcstub ));
if( !CV_IS_MAT(dst) )
CV_CALL( dst = cvGetMat( dst, &dststub ));
if( offset.x < 0 || offset.y < 0 )
CV_ERROR( CV_StsOutOfRange, "Offset (left/top border width) is negative" );
if( src->rows + offset.y > dst->rows || src->cols + offset.x > dst->cols )
CV_ERROR( CV_StsBadSize, "Source array is too big or destination array is too small" );
if( !CV_ARE_TYPES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
type = CV_MAT_TYPE(src->type);
pix_size = CV_ELEM_SIZE(type);
srcsize = cvGetMatSize(src);
dstsize = cvGetMatSize(dst);
srcstep = src->step;
dststep = dst->step;
if( srcstep == 0 )
srcstep = CV_STUB_STEP;
if( dststep == 0 )
dststep = CV_STUB_STEP;
if( bordertype == IPL_BORDER_REPLICATE )
{
icvCopyReplicateBorder_8u( src->data.ptr, srcstep, srcsize,
dst->data.ptr, dststep, dstsize,
offset.y, offset.x, pix_size );
}
else if( bordertype == IPL_BORDER_REFLECT_101 )
{
icvCopyReflect101Border_8u( src->data.ptr, srcstep, srcsize,
dst->data.ptr, dststep, dstsize,
offset.y, offset.x, pix_size );
}
else if( bordertype == IPL_BORDER_CONSTANT )
{
double buf[4];
cvScalarToRawData( &value, buf, src->type, 0 );
icvCopyConstBorder_8u( src->data.ptr, srcstep, srcsize,
dst->data.ptr, dststep, dstsize,
offset.y, offset.x, pix_size, (uchar*)buf );
}
else
CV_ERROR( CV_StsBadFlag, "Unknown/unsupported border type" );
__END__;
}
CV_IMPL void
cvEigenVV( CvArr* srcarr, CvArr* evectsarr, CvArr* evalsarr, double eps )
{
CV_FUNCNAME( "cvEigenVV" );
__BEGIN__;
CvMat sstub, *src = (CvMat*)srcarr;
CvMat estub1, *evects = (CvMat*)evectsarr;
CvMat estub2, *evals = (CvMat*)evalsarr;
if( !CV_IS_MAT( src ))
CV_CALL( src = cvGetMat( src, &sstub ));
if( !CV_IS_MAT( evects ))
CV_CALL( evects = cvGetMat( evects, &estub1 ));
if( !CV_IS_MAT( evals ))
CV_CALL( evals = cvGetMat( evals, &estub2 ));
if( src->cols != src->rows )
CV_ERROR( CV_StsUnmatchedSizes, "source is not quadratic matrix" );
if( !CV_ARE_SIZES_EQ( src, evects) )
CV_ERROR( CV_StsUnmatchedSizes, "eigenvectors matrix has inappropriate size" );
if( (evals->rows != src->rows || evals->cols != 1) &&
(evals->cols != src->rows || evals->rows != 1))
CV_ERROR( CV_StsBadSize, "eigenvalues vector has inappropriate size" );
if( !CV_ARE_TYPES_EQ( src, evects ) || !CV_ARE_TYPES_EQ( src, evals ))
CV_ERROR( CV_StsUnmatchedFormats,
"input matrix, eigenvalues and eigenvectors must have the same type" );
if( !CV_IS_MAT_CONT( src->type & evals->type & evects->type ))
CV_ERROR( CV_BadStep, "all the matrices must be continuous" );
if( CV_MAT_TYPE(src->type) == CV_32FC1 )
{
IPPI_CALL( icvJacobiEigens_32f( src->data.fl,
evects->data.fl,
evals->data.fl, src->cols, (float)eps ));
}
else if( CV_MAT_TYPE(src->type) == CV_64FC1 )
{
IPPI_CALL( icvJacobiEigens_64d( src->data.db,
evects->data.db,
evals->data.db, src->cols, eps ));
}
else
{
CV_ERROR( CV_StsUnsupportedFormat, "Only 32fC1 and 64fC1 types are supported" );
}
CV_CHECK_NANS( evects );
CV_CHECK_NANS( evals );
__END__;
}
void CvOneWayDescriptor::EstimatePosePCA(CvArr* patch, int& pose_idx, float& distance, CvMat* avg, CvMat* eigenvectors) const
{
if(avg == 0)
{
// do not use pca
if (!CV_IS_MAT(patch))
{
EstimatePose((IplImage*)patch, pose_idx, distance);
}
else
{
}
return;
}
CvRect roi;
if (!CV_IS_MAT(patch))
{
roi = cvGetImageROI((IplImage*)patch);
if(roi.width != GetPatchSize().width || roi.height != GetPatchSize().height)
{
cvResize(patch, m_input_patch);
patch = m_input_patch;
roi = cvGetImageROI((IplImage*)patch);
}
}
CvMat* pca_coeffs = cvCreateMat(1, m_pca_dim_low, CV_32FC1);
if (CV_IS_MAT(patch))
{
cvCopy((CvMat*)patch, pca_coeffs);
}
else
{
IplImage* patch_32f = cvCreateImage(cvSize(roi.width, roi.height), IPL_DEPTH_32F, 1);
float sum = cvSum(patch).val[0];
cvConvertScale(patch, patch_32f, 1.0f/sum);
ProjectPCASample(patch_32f, avg, eigenvectors, pca_coeffs);
cvReleaseImage(&patch_32f);
}
distance = 1e10;
pose_idx = -1;
for(int i = 0; i < m_pose_count; i++)
{
float dist = cvNorm(m_pca_coeffs[i], pca_coeffs);
// float dist = 0;
// float data1, data2;
// //CvMat* pose_pca_coeffs = m_pca_coeffs[i];
// for (int x=0; x < pca_coeffs->width; x++)
// for (int y =0 ; y < pca_coeffs->height; y++)
// {
// data1 = ((float*)(pca_coeffs->data.ptr + pca_coeffs->step*x))[y];
// data2 = ((float*)(m_pca_coeffs[i]->data.ptr + m_pca_coeffs[i]->step*x))[y];
// dist+=(data1-data2)*(data1-data2);
// }
////#if 1
// for (int j = 0; j < m_pca_dim_low; j++)
// {
// dist += (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j])*(pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j]);
// }
//#else
// for (int j = 0; j <= m_pca_dim_low - 4; j += 4)
// {
// dist += (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j])*
// (pose_pca_coeffs->data.fl[j]- pca_coeffs->data.fl[j]);
// dist += (pose_pca_coeffs->data.fl[j+1]- pca_coeffs->data.fl[j+1])*
// (pose_pca_coeffs->data.fl[j+1]- pca_coeffs->data.fl[j+1]);
// dist += (pose_pca_coeffs->data.fl[j+2]- pca_coeffs->data.fl[j+2])*
// (pose_pca_coeffs->data.fl[j+2]- pca_coeffs->data.fl[j+2]);
// dist += (pose_pca_coeffs->data.fl[j+3]- pca_coeffs->data.fl[j+3])*
// (pose_pca_coeffs->data.fl[j+3]- pca_coeffs->data.fl[j+3]);
// }
//#endif
if(dist < distance)
{
distance = dist;
pose_idx = i;
}
}
cvReleaseMat(&pca_coeffs);
}
/* it must have more than 3 points */
CV_IMPL CvSeq*
cvConvexityDefects( const CvArr* array,
const CvArr* hullarray,
CvMemStorage* storage )
{
CvSeq* defects = 0;
CV_FUNCNAME( "cvConvexityDefects" );
__BEGIN__;
int i, index;
CvPoint* hull_cur;
/* is orientation of hull different from contour one */
int rev_orientation;
CvContour contour_header;
union { CvContour c; CvSeq s; } hull_header;
CvSeqBlock block, hullblock;
CvSeq *ptseq = (CvSeq*)array, *hull = (CvSeq*)hullarray;
CvSeqReader hull_reader;
CvSeqReader ptseq_reader;
CvSeqWriter writer;
int is_index;
if( CV_IS_SEQ( ptseq ))
{
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_ERROR( CV_StsUnsupportedFormat,
"Input sequence is not a sequence of points" );
if( !storage )
storage = ptseq->storage;
}
else
{
CV_CALL( ptseq = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
}
if( CV_SEQ_ELTYPE( ptseq ) != CV_32SC2 )
CV_ERROR( CV_StsUnsupportedFormat,
"Floating-point coordinates are not supported here" );
if( CV_IS_SEQ( hull ))
{
int hulltype = CV_SEQ_ELTYPE( hull );
if( hulltype != CV_SEQ_ELTYPE_PPOINT && hulltype != CV_SEQ_ELTYPE_INDEX )
CV_ERROR( CV_StsUnsupportedFormat,
"Convex hull must represented as a sequence "
"of indices or sequence of pointers" );
if( !storage )
storage = hull->storage;
}
else
{
CvMat* mat = (CvMat*)hull;
if( !CV_IS_MAT( hull ))
CV_ERROR(CV_StsBadArg, "Convex hull is neither sequence nor matrix");
if( mat->cols != 1 && mat->rows != 1 ||
!CV_IS_MAT_CONT(mat->type) || CV_MAT_TYPE(mat->type) != CV_32SC1 )
CV_ERROR( CV_StsBadArg,
"The matrix should be 1-dimensional and continuous array of int's" );
if( mat->cols + mat->rows - 1 > ptseq->total )
CV_ERROR( CV_StsBadSize, "Convex hull is larger than the point sequence" );
CV_CALL( hull = cvMakeSeqHeaderForArray(
CV_SEQ_KIND_CURVE|CV_MAT_TYPE(mat->type)|CV_SEQ_FLAG_CLOSED,
sizeof(CvContour), CV_ELEM_SIZE(mat->type), mat->data.ptr,
mat->cols + mat->rows - 1, &hull_header.s, &hullblock ));
}
is_index = CV_SEQ_ELTYPE(hull) == CV_SEQ_ELTYPE_INDEX;
if( !storage )
CV_ERROR( CV_StsNullPtr, "NULL storage pointer" );
CV_CALL( defects = cvCreateSeq( CV_SEQ_KIND_GENERIC, sizeof(CvSeq),
sizeof(CvConvexityDefect), storage ));
if( ptseq->total < 4 || hull->total < 3)
{
//CV_ERROR( CV_StsBadSize,
// "point seq size must be >= 4, convex hull size must be >= 3" );
EXIT;
}
/* recognize co-orientation of ptseq and its hull */
{
int sign = 0;
int index1, index2, index3;
if( !is_index )
{
CvPoint* pos = *CV_SEQ_ELEM( hull, CvPoint*, 0 );
CV_CALL( index1 = cvSeqElemIdx( ptseq, pos ));
pos = *CV_SEQ_ELEM( hull, CvPoint*, 1 );
CV_CALL( index2 = cvSeqElemIdx( ptseq, pos ));
pos = *CV_SEQ_ELEM( hull, CvPoint*, 2 );
CV_CALL( index3 = cvSeqElemIdx( ptseq, pos ));
}
else
{
CV_IMPL void
cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ,
CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz,
CvPoint3D64f *eulerAngles)
{
CV_FUNCNAME("cvRQDecomp3x3");
__BEGIN__;
double _M[3][3], _R[3][3], _Q[3][3];
CvMat M = cvMat(3, 3, CV_64F, _M);
CvMat R = cvMat(3, 3, CV_64F, _R);
CvMat Q = cvMat(3, 3, CV_64F, _Q);
double z, c, s;
/* Validate parameters. */
CV_ASSERT( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) &&
matrixM->cols == 3 && matrixM->rows == 3 &&
CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ));
cvConvert(matrixM, &M);
{
/* Find Givens rotation Q_x for x axis (left multiplication). */
/*
( 1 0 0 )
Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2)
( 0 -s c )
*/
s = _M[2][1];
c = _M[2][2];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} };
CvMat Qx = cvMat(3, 3, CV_64F, _Qx);
cvMatMul(&M, &Qx, &R);
assert(fabs(_R[2][1]) < FLT_EPSILON);
_R[2][1] = 0;
/* Find Givens rotation for y axis. */
/*
( c 0 s )
Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = m31/sqrt(m31^2 + m33^2)
(-s 0 c )
*/
s = _R[2][0];
c = _R[2][2];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qy[3][3] = { {c, 0, s}, {0, 1, 0}, {-s, 0, c} };
CvMat Qy = cvMat(3, 3, CV_64F, _Qy);
cvMatMul(&R, &Qy, &M);
assert(fabs(_M[2][0]) < FLT_EPSILON);
_M[2][0] = 0;
/* Find Givens rotation for z axis. */
/*
( c s 0 )
Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2)
( 0 0 1 )
*/
s = _M[1][0];
c = _M[1][1];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} };
CvMat Qz = cvMat(3, 3, CV_64F, _Qz);
cvMatMul(&M, &Qz, &R);
assert(fabs(_R[1][0]) < FLT_EPSILON);
_R[1][0] = 0;
// Solve the decomposition ambiguity.
// Diagonal entries of R, except the last one, shall be positive.
// Further rotate R by 180 degree if necessary
if( _R[0][0] < 0 )
{
if( _R[1][1] < 0 )
{
// rotate around z for 180 degree, i.e. a rotation matrix of
// [-1, 0, 0],
// [ 0, -1, 0],
// [ 0, 0, 1]
_R[0][0] *= -1;
_R[0][1] *= -1;
_R[1][1] *= -1;
_Qz[0][0] *= -1;
_Qz[0][1] *= -1;
_Qz[1][0] *= -1;
_Qz[1][1] *= -1;
}
else
{
// rotate around y for 180 degree, i.e. a rotation matrix of
// [-1, 0, 0],
// [ 0, 1, 0],
// [ 0, 0, -1]
_R[0][0] *= -1;
_R[0][2] *= -1;
_R[1][2] *= -1;
_R[2][2] *= -1;
cvTranspose( &Qz, &Qz );
_Qy[0][0] *= -1;
_Qy[0][2] *= -1;
_Qy[2][0] *= -1;
_Qy[2][2] *= -1;
}
}
else if( _R[1][1] < 0 )
{
// ??? for some reason, we never get here ???
// rotate around x for 180 degree, i.e. a rotation matrix of
// [ 1, 0, 0],
// [ 0, -1, 0],
// [ 0, 0, -1]
_R[0][1] *= -1;
_R[0][2] *= -1;
_R[1][1] *= -1;
_R[1][2] *= -1;
_R[2][2] *= -1;
cvTranspose( &Qz, &Qz );
cvTranspose( &Qy, &Qy );
_Qx[1][1] *= -1;
_Qx[1][2] *= -1;
_Qx[2][1] *= -1;
_Qx[2][2] *= -1;
}
// calculate the euler angle
if( eulerAngles )
{
eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
eulerAngles->y = acos(_Qy[0][0]) * (_Qy[0][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI);
}
/* Calulate orthogonal matrix. */
/*
Q = QzT * QyT * QxT
*/
cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T );
cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T );
/* Save R and Q matrices. */
cvConvert( &R, matrixR );
cvConvert( &Q, matrixQ );
if( matrixQx )
cvConvert(&Qx, matrixQx);
if( matrixQy )
cvConvert(&Qy, matrixQy);
if( matrixQz )
cvConvert(&Qz, matrixQz);
}
__END__;
}
// This function create cross-validation EstimateModel.
ML_IMPL CvStatModel*
cvCreateCrossValidationEstimateModel(
int samples_all,
const CvStatModelParams* estimateParams,
const CvMat* sampleIdx)
{
CvStatModel* model = NULL;
CvCrossValidationModel* crVal = NULL;
CV_FUNCNAME ("cvCreateCrossValidationEstimateModel");
__BEGIN__
int k_fold = 10;
int i, j, k, s_len;
int samples_selected;
CvRNG rng;
CvRNG* prng;
int* res_s_data;
int* te_s_data;
int* folds;
rng = cvRNG(cvGetTickCount());
cvRandInt (&rng);
cvRandInt (&rng);
cvRandInt (&rng);
cvRandInt (&rng);
// Check input parameters.
if (estimateParams)
k_fold = ((CvCrossValidationParams*)estimateParams)->k_fold;
if (!k_fold)
{
CV_ERROR (CV_StsBadArg, "Error in parameters of cross-validation (k_fold == 0)!");
}
if (samples_all <= 0)
{
CV_ERROR (CV_StsBadArg, "<samples_all> should be positive!");
}
// Alloc memory and fill standart StatModel's fields.
CV_CALL (crVal = (CvCrossValidationModel*)cvCreateStatModel (
CV_STAT_MODEL_MAGIC_VAL | CV_CROSSVAL_MAGIC_VAL,
sizeof(CvCrossValidationModel),
cvReleaseCrossValidationModel,
NULL, NULL));
crVal->current_fold = -1;
crVal->folds_all = k_fold;
if (estimateParams && ((CvCrossValidationParams*)estimateParams)->is_regression)
crVal->is_regression = 1;
else
crVal->is_regression = 0;
if (estimateParams && ((CvCrossValidationParams*)estimateParams)->rng)
prng = ((CvCrossValidationParams*)estimateParams)->rng;
else
prng = &rng;
// Check and preprocess sample indices.
if (sampleIdx)
{
int s_step;
int s_type = 0;
if (!CV_IS_MAT (sampleIdx))
CV_ERROR (CV_StsBadArg, "Invalid sampleIdx array");
if (sampleIdx->rows != 1 && sampleIdx->cols != 1)
CV_ERROR (CV_StsBadSize, "sampleIdx array must be 1-dimensional");
s_len = sampleIdx->rows + sampleIdx->cols - 1;
s_step = sampleIdx->rows == 1 ?
1 : sampleIdx->step / CV_ELEM_SIZE(sampleIdx->type);
s_type = CV_MAT_TYPE (sampleIdx->type);
switch (s_type)
{
case CV_8UC1:
case CV_8SC1:
{
uchar* s_data = sampleIdx->data.ptr;
// sampleIdx is array of 1's and 0's -
// i.e. it is a mask of the selected samples
if( s_len != samples_all )
CV_ERROR (CV_StsUnmatchedSizes,
"Sample mask should contain as many elements as the total number of samples");
samples_selected = 0;
for (i = 0; i < s_len; i++)
samples_selected += s_data[i * s_step] != 0;
if (samples_selected == 0)
CV_ERROR (CV_StsOutOfRange, "No samples is selected!");
}
s_len = samples_selected;
break;
case CV_32SC1:
if (s_len > samples_all)
CV_ERROR (CV_StsOutOfRange,
"sampleIdx array may not contain more elements than the total number of samples");
samples_selected = s_len;
break;
default:
CV_ERROR (CV_StsUnsupportedFormat, "Unsupported sampleIdx array data type "
"(it should be 8uC1, 8sC1 or 32sC1)");
}
// Alloc additional memory for internal Idx and fill it.
/*!!*/ CV_CALL (res_s_data = crVal->sampleIdxAll =
(int*)cvAlloc (2 * s_len * sizeof(int)));
if (s_type < CV_32SC1)
{
uchar* s_data = sampleIdx->data.ptr;
for (i = 0; i < s_len; i++)
if (s_data[i * s_step])
{
*res_s_data++ = i;
}
res_s_data = crVal->sampleIdxAll;
}
else
{
int* s_data = sampleIdx->data.i;
int out_of_order = 0;
for (i = 0; i < s_len; i++)
{
res_s_data[i] = s_data[i * s_step];
if (i > 0 && res_s_data[i] < res_s_data[i - 1])
out_of_order = 1;
}
if (out_of_order)
qsort (res_s_data, s_len, sizeof(res_s_data[0]), icvCmpIntegers);
if (res_s_data[0] < 0 ||
res_s_data[s_len - 1] >= samples_all)
CV_ERROR (CV_StsBadArg, "There are out-of-range sample indices");
for (i = 1; i < s_len; i++)
if (res_s_data[i] <= res_s_data[i - 1])
CV_ERROR (CV_StsBadArg, "There are duplicated");
}
}
else // if (sampleIdx)
{
// Alloc additional memory for internal Idx and fill it.
s_len = samples_all;
CV_CALL (res_s_data = crVal->sampleIdxAll = (int*)cvAlloc (2 * s_len * sizeof(int)));
for (i = 0; i < s_len; i++)
{
*res_s_data++ = i;
}
res_s_data = crVal->sampleIdxAll;
} // if (sampleIdx) ... else
// Resort internal Idx.
te_s_data = res_s_data + s_len;
for (i = s_len; i > 1; i--)
{
j = cvRandInt (prng) % i;
k = *(--te_s_data);
*te_s_data = res_s_data[j];
res_s_data[j] = k;
}
// Duplicate resorted internal Idx.
// It will be used to simplify operation of getting trainIdx.
te_s_data = res_s_data + s_len;
for (i = 0; i < s_len; i++)
{
*te_s_data++ = *res_s_data++;
}
// Cut sampleIdxAll to parts.
if (k_fold > 0)
{
if (k_fold > s_len)
{
CV_ERROR (CV_StsBadArg,
"Error in parameters of cross-validation ('k_fold' > #samples)!");
}
folds = crVal->folds = (int*) cvAlloc ((k_fold + 1) * sizeof (int));
*folds++ = 0;
for (i = 1; i < k_fold; i++)
{
*folds++ = cvRound (i * s_len * 1. / k_fold);
}
*folds = s_len;
folds = crVal->folds;
crVal->max_fold_size = (s_len - 1) / k_fold + 1;
}
else
{
k = -k_fold;
crVal->max_fold_size = k;
if (k >= s_len)
{
CV_ERROR (CV_StsBadArg,
"Error in parameters of cross-validation (-'k_fold' > #samples)!");
}
crVal->folds_all = k = (s_len - 1) / k + 1;
folds = crVal->folds = (int*) cvAlloc ((k + 1) * sizeof (int));
for (i = 0; i < k; i++)
{
*folds++ = -i * k_fold;
}
*folds = s_len;
folds = crVal->folds;
}
// Prepare other internal fields to working.
CV_CALL (crVal->predict_results = cvCreateMat (1, samples_all, CV_32FC1));
CV_CALL (crVal->sampleIdxEval = cvCreateMatHeader (1, 1, CV_32SC1));
CV_CALL (crVal->sampleIdxTrain = cvCreateMatHeader (1, 1, CV_32SC1));
crVal->sampleIdxEval->cols = 0;
crVal->sampleIdxTrain->cols = 0;
crVal->samples_all = s_len;
crVal->is_checked = 1;
crVal->getTrainIdxMat = cvCrossValGetTrainIdxMatrix;
crVal->getCheckIdxMat = cvCrossValGetCheckIdxMatrix;
crVal->nextStep = cvCrossValNextStep;
crVal->check = cvCrossValCheckClassifier;
crVal->getResult = cvCrossValGetResult;
crVal->reset = cvCrossValReset;
model = (CvStatModel*)crVal;
__END__
if (!model)
{
cvReleaseCrossValidationModel ((CvStatModel**)&crVal);
}
return model;
} // End of cvCreateCrossValidationEstimateModel
void
cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
const CvMat* _distCoeffs,
const CvMat* _R, const CvMat* _P )
{
CV_FUNCNAME( "cvUndistortPoints" );
__BEGIN__;
double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
CvMat _A=cvMat(3, 3, CV_64F, A), _Dk;
CvMat _RR=cvMat(3, 3, CV_64F, RR);
const CvPoint2D32f* srcf;
const CvPoint2D64f* srcd;
CvPoint2D32f* dstf;
CvPoint2D64f* dstd;
int stype, dtype;
int sstep, dstep;
int i, j, n;
CV_ASSERT( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
(_src->rows == 1 || _src->cols == 1) &&
(_dst->rows == 1 || _dst->cols == 1) &&
CV_ARE_SIZES_EQ(_src, _dst) &&
(CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&
(CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));
CV_ASSERT( CV_IS_MAT(_cameraMatrix) && CV_IS_MAT(_distCoeffs) &&
_cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 &&
(_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
(_distCoeffs->rows*_distCoeffs->cols == 4 ||
_distCoeffs->rows*_distCoeffs->cols == 5) );
_Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
cvConvert( _cameraMatrix, &_A );
cvConvert( _distCoeffs, &_Dk );
if( _R )
{
CV_ASSERT( CV_IS_MAT(_R) && _R->rows == 3 && _R->cols == 3 );
cvConvert( _R, &_RR );
}
else
cvSetIdentity(&_RR);
if( _P )
{
double PP[3][3];
CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);
CV_ASSERT( CV_IS_MAT(_P) && _P->rows == 3 && (_P->cols == 3 || _P->cols == 4));
cvConvert( cvGetCols(_P, &_P3x3, 0, 3), &_PP );
cvMatMul( &_PP, &_RR, &_RR );
}
srcf = (const CvPoint2D32f*)_src->data.ptr;
srcd = (const CvPoint2D64f*)_src->data.ptr;
dstf = (CvPoint2D32f*)_dst->data.ptr;
dstd = (CvPoint2D64f*)_dst->data.ptr;
stype = CV_MAT_TYPE(_src->type);
dtype = CV_MAT_TYPE(_dst->type);
sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
n = _src->rows + _src->cols - 1;
fx = A[0][0];
fy = A[1][1];
ifx = 1./fx;
ify = 1./fy;
cx = A[0][2];
cy = A[1][2];
for( i = 0; i < n; i++ )
{
double x, y, x0, y0;
if( stype == CV_32FC2 )
{
x = srcf[i*sstep].x;
y = srcf[i*sstep].y;
}
else
{
x = srcd[i*sstep].x;
y = srcd[i*sstep].y;
}
x0 = x = (x - cx)*ifx;
y0 = y = (y - cy)*ify;
// compensate distortion iteratively
for( j = 0; j < 5; j++ )
{
double r2 = x*x + y*y;
double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - deltaX)*icdist;
y = (y0 - deltaY)*icdist;
}
double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2];
double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]);
x = xx*ww;
y = yy*ww;
if( dtype == CV_32FC2 )
{
dstf[i*dstep].x = (float)x;
dstf[i*dstep].y = (float)y;
}
else
{
dstd[i*dstep].x = x;
dstd[i*dstep].y = y;
}
}
__END__;
}
