void cvmPerspectiveProject( const CvMat* mat, const CvArr* src, CvArr* dst )
{
CvMat tsrc, tdst;
cvReshape( src, &tsrc, 3, 0 );
cvReshape( dst, &tdst, 3, 0 );
cvPerspectiveTransform( &tsrc, &tdst, mat );
}
float basicOCR::classify(IplImage* img, int showResult)//第二个参数主要用来控制是测试训练样本还是手写识别
{
IplImage prs_image;
CvMat data;
CvMat* nearest=cvCreateMat(1,K,CV_32FC1);
float result;
//处理输入的图像
prs_image = preprocessing(img, size, size);
//Set data
IplImage* img32 = cvCreateImage( cvSize( size, size ), IPL_DEPTH_32F, 1 );
cvConvertScale(&prs_image, img32, 0.0039215, 0);
cvGetSubRect(img32, &data, cvRect(0,0, size,size));
CvMat row_header, *row1;
row1 = cvReshape( &data, &row_header, 0, 1 );
result=knn->find_nearest(row1,K,0,0,nearest,0);
int accuracy=0;
for(int i=0;i<K;i++)
{
if( (nearest->data.fl[i]) == result)
accuracy++;
}
float pre=100*((float)accuracy/(float)K);
char out = result;
if(showResult==1)
{
printf("|\t %c \t| \t %.2f%% \t| \t %d of %d \t| \n",out,pre,accuracy,K);
printf(" ------------------------------------------------------------------------\n");
}
return result;
}
/// <summary>
/// Classifies the given prs_image by running k-nearest algorithm and prints the result.
/// </summary>
/// <param name="prs_image">
/// IplImage to be classified.
/// </param>
/// <param name="showResult">
/// If its 1, then prints result after classifying.
/// </param>
/// <returns> Result after classifying given image. </returns>
float OCR::print(IplImage prs_image, int showResult)
{
float result;
CvMat data;
CvMat* nearest=cvCreateMat(1,K,CV_32FC1);
//Set data
IplImage* img32 = cvCreateImage( cvSize( size, size ), IPL_DEPTH_32F, 1 );
cvConvertScale(&prs_image, img32, 0.0039215, 0);
cvGetSubRect(img32, &data, cvRect(0,0, size,size));
CvMat row_header, *row1;
row1 = cvReshape( &data, &row_header, 0, 1 );
result=knn->find_nearest(row1,K,0,0,nearest,0);
if(showResult == 1)
{
char r = result;
int accuracy=0;
for(int i=0;i<K;i++)
{
if( nearest->data.fl[i] == result)
accuracy++;
}
printf("%c ",r);
// float pre=100*((float)accuracy/(float)K);
// printf("|\t%c\t| \t%.2f%% \t| \t%d of %d \t",r,pre,accuracy,K);
// printf(" \n---------------------------------------------------------------\n");
}
return result;
}
/// <summary>
/// Reads the sample images and associated charaters into trainClasses and trainData respectively.
/// </summary>
/// <returns> Nothing. </returns>
void OCR::getData()
{
IplImage* src_image;
IplImage prs_image;
CvMat row,data;
char file[255];
char dataFile[255];
std::ifstream labelStream;
std::ostringstream outStringStream;
char ch;
int i,j;
for(i = 0; i < classes; i++)
{ //26
//Read the corresponding character for current sample being processed into ch.
sprintf(dataFile,"%s%d/data.txt",file_path, i);
labelStream.open(dataFile);
labelStream >> ch;
labelStream.close();
for( j = 0; j< train_samples; j++)
{ //3
//Load file
//get the path of image for training into file.
if(j<10)
sprintf(file,"%s%d/%d0%d.pbm",file_path, i, i, j);
else
sprintf(file,"%s%d/%d%d.pbm",file_path, i, i, j);
src_image = cvLoadImage(file,0);
if(!src_image)
{
printf("Error: Cant load image %s\n", file);
//exit(-1);
}
//process file
prs_image = preprocessing(src_image, size, size);
//Set class label
cvGetRow(trainClasses, &row, i*train_samples + j);
cvSet(&row, cvRealScalar(ch));
//Set data
cvGetRow(trainData, &row, i*train_samples + j);
IplImage* img = cvCreateImage( cvSize( size, size ), IPL_DEPTH_32F, 1 );
//convert 8 bits image to 32 float image
cvConvertScale(&prs_image, img, 0.0039215, 0);
cvGetSubRect(img, &data, cvRect(0,0, size,size));
CvMat row_header, *row1;
//convert data matrix sizexsize to vecor
row1 = cvReshape( &data, &row_header, 0, 1 );
cvCopy(row1, &row, NULL);
}
}
}
/*
mask should be a symmetric filter and its size should be an odd number
*/
int STBuffer::TemporalConvolve(IplImage* dst,std::vector<double> mask)
{
int tfsz=(int)mask.size();
assert(tfsz<=BufferSize);
int i;
if(BufferSize)
{
assert(dst->widthStep * dst->height == Buffer->step);
assert(BufferSize>=3);
}
assert(tfsz%2); //the size of filter should be odd
int tstampres=FrameIndices.Middle(tfsz);
if((int)mask.size()<BufferSize)
for(i=(int)mask.size();i<BufferSize;i++)
mask.push_back(0);
std::vector<int> Sorted =FrameIndices.GetSortedIndices();
CvMat *fil=cvCreateMat(1,BufferSize,DATATYPE);
assert(BufferSize==(int)mask.size()); //filter is too big (it could be cut)
IMG_ELEM_TYPE* filter=new IMG_ELEM_TYPE[BufferSize];
for(i=0;i<BufferSize;i++)
filter[Sorted[i]]=(IMG_ELEM_TYPE)mask[i];
for(i=0;i<BufferSize;i++)
cvmSet(fil,0,i, filter[i]);
CvMat *rdst, dsthdr;
rdst = cvReshape(dst,&dsthdr,0,1);
cvMatMul(fil,Buffer,rdst);
delete[] filter;
cvReleaseMat(&fil);
return tstampres;
}
void basicOCR::getData()
{
IplImage* src_image;
IplImage prs_image;
CvMat row,data;
char file[255];
int i,j;
//for(i =0; i<classes; i++)
for (i = 32; i < 32 + classes; i++)
{
for ( j = 0; j < train_samples; j++)
{
//加载pbm格式图像，作为训练
/*if(j < 10)
sprintf(file,"%s%d/%d0%d.pbm",file_path, i - 48, i - 48 , j);
else
sprintf(file,"%s%d/%d%d.pbm",file_path, i - 48, i - 48 , j);*/
if (i >= 48 && i <= 57)
sprintf(file,"%s%d/%d.pbm",file_path, i, j);
else
sprintf(file,"%s%d/%d.bmp",file_path, i, j);
src_image = cvLoadImage(file,0);
if(!src_image)
{
//printf("Error: Cant load image %s\n", file);
continue;
//exit(-1);
}
//process file
prs_image = preprocessing(src_image, size, size);
//Set class label
cvGetRow(trainClasses, &row, (i - 32)*train_samples + j);
cvSet(&row, cvRealScalar(i));
//Set data
cvGetRow(trainData, &row, (i - 32)*train_samples + j);
IplImage* img = cvCreateImage( cvSize( size, size ), IPL_DEPTH_32F, 1 );
//convert 8 bits image to 32 float image
cvConvertScale(&prs_image, img, 0.0039215, 0);
cvGetSubRect(img, &data, cvRect(0,0, size,size));
CvMat row_header, *row1;
//convert data matrix sizexsize to vecor
row1 = cvReshape( &data, &row_header, 0, 1 );
cvCopy(row1, &row, NULL);
}
}
}
int STBuffer::ExponentialSmooth(IplImage* dst, std::vector<double> mask) {
int tfsz = (int)mask.size();
assert(tfsz<=BufferSize);
int i;
if(BufferSize) {
assert(dst->widthStep * dst->height == Buffer->step);
}
int tstampres = FrameIndices.Last();
if ((int)mask.size() < BufferSize)
for(i=(int)mask.size();i<BufferSize;i++)
mask.push_back(0);
// If current frame is t,
// Sorted[i] = the index of (t - i)th frame in FrameIndices.
std::vector<int> Sorted = FrameIndices.GetSortedIndices();
CvMat *fil = cvCreateMat(1, BufferSize, DATATYPE);
assert(BufferSize == (int)mask.size()); //filter is too big (it could be cut)
IMG_ELEM_TYPE* filter = new IMG_ELEM_TYPE[BufferSize];
for (i = 0; i < BufferSize; i++)
filter[Sorted[i]] = (IMG_ELEM_TYPE)mask[i];
for(i=0;i<BufferSize;i++)
cvmSet(fil,0,i, filter[i]);
CvMat *rdst, dsthdr;
rdst = cvReshape(dst, &dsthdr, 0, 1);
cvMatMul(fil, Buffer, rdst);
//memcpy((void*)(Buffer->data.ptr + Buffer->step * FrameIndices.LastIndex),
//dst->imageData, Buffer->step);
delete[] filter;
cvReleaseMat(&fil);
return tstampres;
}
void CvEM::init_em( const CvVectors& train_data )
{
CvMat *w = 0, *u = 0, *tcov = 0;
CV_FUNCNAME( "CvEM::init_em" );
__BEGIN__;
double maxval = 0;
int i, force_symm_plus = 0;
int nclusters = params.nclusters, nsamples = train_data.count, dims = train_data.dims;
if( params.start_step == START_AUTO_STEP || nclusters == 1 || nclusters == nsamples )
init_auto( train_data );
else if( params.start_step == START_M_STEP )
{
for( i = 0; i < nsamples; i++ )
{
CvMat prob;
cvGetRow( params.probs, &prob, i );
cvMaxS( &prob, 0., &prob );
cvMinMaxLoc( &prob, 0, &maxval );
if( maxval < FLT_EPSILON )
cvSet( &prob, cvScalar(1./nclusters) );
else
cvNormalize( &prob, &prob, 1., 0, CV_L1 );
}
EXIT; // do not preprocess covariation matrices,
// as in this case they are initialized at the first iteration of EM
}
else
{
CV_ASSERT( params.start_step == START_E_STEP && params.means );
if( params.weights && params.covs )
{
cvConvert( params.means, means );
cvReshape( weights, weights, 1, params.weights->rows );
cvConvert( params.weights, weights );
cvReshape( weights, weights, 1, 1 );
cvMaxS( weights, 0., weights );
cvMinMaxLoc( weights, 0, &maxval );
if( maxval < FLT_EPSILON )
cvSet( weights, cvScalar(1./nclusters) );
cvNormalize( weights, weights, 1., 0, CV_L1 );
for( i = 0; i < nclusters; i++ )
CV_CALL( cvConvert( params.covs[i], covs[i] ));
force_symm_plus = 1;
}
else
init_auto( train_data );
}
CV_CALL( tcov = cvCreateMat( dims, dims, CV_64FC1 ));
CV_CALL( w = cvCreateMat( dims, dims, CV_64FC1 ));
if( params.cov_mat_type == COV_MAT_GENERIC )
CV_CALL( u = cvCreateMat( dims, dims, CV_64FC1 ));
for( i = 0; i < nclusters; i++ )
{
if( force_symm_plus )
{
cvTranspose( covs[i], tcov );
cvAddWeighted( covs[i], 0.5, tcov, 0.5, 0, tcov );
}
else
cvCopy( covs[i], tcov );
cvSVD( tcov, w, u, 0, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
if( params.cov_mat_type == COV_MAT_SPHERICAL )
cvSetIdentity( covs[i], cvScalar(cvTrace(w).val[0]/dims) );
else if( params.cov_mat_type == COV_MAT_DIAGONAL )
cvCopy( w, covs[i] );
else
{
// generic case: covs[i] = (u')'*max(w,0)*u'
cvGEMM( u, w, 1, 0, 0, tcov, CV_GEMM_A_T );
cvGEMM( tcov, u, 1, 0, 0, covs[i], 0 );
}
}
__END__;
cvReleaseMat( &w );
cvReleaseMat( &u );
cvReleaseMat( &tcov );
}
float
CvEM::predict( const CvMat* _sample, CvMat* _probs ) const
{
float* sample_data = 0;
void* buffer = 0;
int allocated_buffer = 0;
int cls = 0;
CV_FUNCNAME( "CvEM::predict" );
__BEGIN__;
int i, k, dims;
int nclusters;
int cov_mat_type = params.cov_mat_type;
double opt = FLT_MAX;
size_t size;
CvMat diff, expo;
dims = means->cols;
nclusters = params.nclusters;
CV_CALL( cvPreparePredictData( _sample, dims, 0, params.nclusters, _probs, &sample_data ));
// allocate memory and initializing headers for calculating
size = sizeof(double) * (nclusters + dims);
if( size <= CV_MAX_LOCAL_SIZE )
buffer = cvStackAlloc( size );
else
{
CV_CALL( buffer = cvAlloc( size ));
allocated_buffer = 1;
}
expo = cvMat( 1, nclusters, CV_64FC1, buffer );
diff = cvMat( 1, dims, CV_64FC1, (double*)buffer + nclusters );
// calculate the probabilities
for( k = 0; k < nclusters; k++ )
{
const double* mean_k = (const double*)(means->data.ptr + means->step*k);
const double* w = (const double*)(inv_eigen_values->data.ptr + inv_eigen_values->step*k);
double cur = log_weight_div_det->data.db[k];
CvMat* u = cov_rotate_mats[k];
// cov = u w u' --> cov^(-1) = u w^(-1) u'
if( cov_mat_type == COV_MAT_SPHERICAL )
{
double w0 = w[0];
for( i = 0; i < dims; i++ )
{
double val = sample_data[i] - mean_k[i];
cur += val*val*w0;
}
}
else
{
for( i = 0; i < dims; i++ )
diff.data.db[i] = sample_data[i] - mean_k[i];
if( cov_mat_type == COV_MAT_GENERIC )
cvGEMM( &diff, u, 1, 0, 0, &diff, CV_GEMM_B_T );
for( i = 0; i < dims; i++ )
{
double val = diff.data.db[i];
cur += val*val*w[i];
}
}
expo.data.db[k] = cur;
if( cur < opt )
{
cls = k;
opt = cur;
}
/* probability = (2*pi)^(-dims/2)*exp( -0.5 * cur ) */
}
if( _probs )
{
CV_CALL( cvConvertScale( &expo, &expo, -0.5 ));
CV_CALL( cvExp( &expo, &expo ));
if( _probs->cols == 1 )
CV_CALL( cvReshape( &expo, &expo, 0, nclusters ));
CV_CALL( cvConvertScale( &expo, _probs, 1./cvSum( &expo ).val[0] ));
}
__END__;
if( sample_data != _sample->data.fl )
cvFree( &sample_data );
if( allocated_buffer )
cvFree( &buffer );
return (float)cls;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
CvMat *tgso (CvMat &tmap, int ntex, double sigma, double theta, CvMat &tsim, int useChi2) {
CvMat *roundTmap=cvCreateMat(tmap.rows,tmap.cols,CV_32FC1);
CvMat *comp=cvCreateMat(tmap.rows,tmap.cols,CV_32FC1);
for (int i=0;i<tmap.rows;i++)
for (int j=0;j<tmap.cols;j++)
cvSetReal2D(roundTmap,i,j,cvRound(cvGetReal2D(&tmap,i,j)));
cvSub(&tmap,roundTmap,comp);
if (cvCountNonZero(comp)) {
printf("texton labels not integral");
cvReleaseMat(&roundTmap);
cvReleaseMat(&comp);
exit(1);
}
double min,max;
cvMinMaxLoc(&tmap,&min,&max);
if (min<1 && max>ntex) {
char *msg=new char[50];
printf(msg,"texton labels out of range [1,%d]",ntex);
cvReleaseMat(&roundTmap);
cvReleaseMat(&comp);
exit(1);
}
cvReleaseMat(&roundTmap);
cvReleaseMat(&comp);
double wr=floor(sigma); //sigma=radius (Leo)
CvMat *x=cvCreateMat(1,wr-(-wr)+1, CV_64FC1);
CvMat *y=cvCreateMat(wr-(-wr)+1,1, CV_64FC1);
CvMat *u=cvCreateMat(wr-(-wr)+1,wr-(-wr)+1, CV_64FC1);
CvMat *v=cvCreateMat(wr-(-wr)+1,wr-(-wr)+1, CV_64FC1);
CvMat *gamma=cvCreateMat(u->rows,v->rows, CV_64FC1);
// Set x,y directions
for (int j=-wr;j<=wr;j++) {
cvSetReal2D(x,0,(j+wr),j);
cvSetReal2D(y,(j+wr),0,j);
}
// Set u,v, meshgrids
for (int i=0;i<u->rows;i++) {
cvRepeat(x,u);
cvRepeat(y,v);
}
// Compute the gamma matrix from the grid
for (int i=0;i<u->rows;i++)
for (int j=0;j<u->cols;j++)
cvSetReal2D(gamma,i,j,atan2(cvGetReal2D(v,i,j),cvGetReal2D(u,i,j)));
cvReleaseMat(&x);
cvReleaseMat(&y);
CvMat *sum=cvCreateMat(u->rows,u->cols, CV_64FC1);
cvMul(u,u,u);
cvMul(v,v,v);
cvAdd(u,v,sum);
CvMat *mask=cvCreateMat(u->rows,u->cols, CV_8UC1);
cvCmpS(sum,sigma*sigma,mask,CV_CMP_LE);
cvConvertScale(mask,mask,1.0/255);
cvSetReal2D(mask,wr,wr,0);
int count=cvCountNonZero(mask);
cvReleaseMat(&u);
cvReleaseMat(&v);
cvReleaseMat(&sum);
CvMat *sub=cvCreateMat(mask->rows,mask->cols, CV_64FC1);
CvMat *side=cvCreateMat(mask->rows,mask->cols, CV_8UC1);
cvSubS(gamma,cvScalar(theta),sub);
cvReleaseMat(&gamma);
for (int i=0;i<mask->rows;i++){
for (int j=0;j<mask->cols;j++) {
double n=cvmGet(sub,i,j);
double n_mod = n-floor(n/(2*M_PI))*2*M_PI;
cvSetReal2D(side,i,j, 1 + int(n_mod < M_PI));
}
}
cvMul(side,mask,side);
cvReleaseMat(&sub);
cvReleaseMat(&mask);
CvMat *lmask=cvCreateMat(side->rows,side->cols, CV_8UC1);
CvMat *rmask=cvCreateMat(side->rows,side->cols, CV_8UC1);
cvCmpS(side,1,lmask,CV_CMP_EQ);
cvCmpS(side,2,rmask,CV_CMP_EQ);
int count1=cvCountNonZero(lmask), count2=cvCountNonZero(rmask);
if (count1 != count2) {
printf("Bug: imbalance\n");
}
CvMat *rlmask=cvCreateMat(side->rows,side->cols, CV_32FC1);
CvMat *rrmask=cvCreateMat(side->rows,side->cols, CV_32FC1);
cvConvertScale(lmask,rlmask,1.0/(255*count)*2);
cvConvertScale(rmask,rrmask,1.0/(255*count)*2);
cvReleaseMat(&lmask);
cvReleaseMat(&rmask);
cvReleaseMat(&side);
int h=tmap.rows;
int w=tmap.cols;
CvMat *d = cvCreateMat(h*w,ntex,CV_32FC1);
CvMat *coltemp = cvCreateMat(h*w,1,CV_32FC1);
CvMat *tgL = cvCreateMat(h,w, CV_32FC1);
CvMat *tgR = cvCreateMat(h,w, CV_32FC1);
CvMat *temp = cvCreateMat(h,w,CV_8UC1);
CvMat *im = cvCreateMat(h,w, CV_32FC1);
CvMat *sub2 = cvCreateMat(h,w,CV_32FC1);
CvMat *sub2t = cvCreateMat(w,h,CV_32FC1);
CvMat *prod = cvCreateMat(h*w,ntex,CV_32FC1);
CvMat reshapehdr,*reshape;
CvMat* tgL_pad = cvCreateMat(h+rlmask->rows-1,w+rlmask->cols-1,CV_32FC1);
CvMat* tgR_pad = cvCreateMat(h+rlmask->rows-1,w+rlmask->cols-1,CV_32FC1);
CvMat* im_pad = cvCreateMat(h+rlmask->rows-1,w+rlmask->cols-1,CV_32FC1);
CvMat *tg=cvCreateMat(h,w,CV_32FC1);
cvZero(tg);
if (useChi2 == 1){
CvMat* temp_add1 = cvCreateMat(h,w,CV_32FC1);
for (int i=0;i<ntex;i++) {
cvCmpS(&tmap,i+1,temp,CV_CMP_EQ);
cvConvertScale(temp,im,1.0/255);
cvCopyMakeBorder(tgL,tgL_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(tgR,tgR_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(im,im_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvFilter2D(im_pad,tgL_pad,rlmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvFilter2D(im_pad,tgR_pad,rrmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvGetSubRect(tgL_pad,tgL,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgL->cols,tgL->rows));
cvGetSubRect(tgR_pad,tgR,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgR->cols,tgR->rows));
cvSub(tgL,tgR,sub2);
cvPow(sub2,sub2,2.0);
cvAdd(tgL,tgR,temp_add1);
cvAddS(temp_add1,cvScalar(0.0000000001),temp_add1);
cvDiv(sub2,temp_add1,sub2);
cvAdd(tg,sub2,tg);
}
cvScale(tg,tg,0.5);
cvReleaseMat(&temp_add1);
}
else{// if not chi^2
for (int i=0;i<ntex;i++) {
cvCmpS(&tmap,i+1,temp,CV_CMP_EQ);
cvConvertScale(temp,im,1.0/255);
cvCopyMakeBorder(tgL,tgL_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(tgR,tgR_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvCopyMakeBorder(im,im_pad,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2),IPL_BORDER_CONSTANT);
cvFilter2D(im_pad,tgL_pad,rlmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvFilter2D(im_pad,tgR_pad,rrmask,cvPoint((rlmask->cols-1)/2,(rlmask->rows-1)/2));
cvGetSubRect(tgL_pad,tgL,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgL->cols,tgL->rows));
cvGetSubRect(tgR_pad,tgR,cvRect((rlmask->cols-1)/2,(rlmask->rows-1)/2,tgR->cols,tgR->rows));
cvSub(tgL,tgR,sub2);
cvAbs(sub2,sub2);
cvTranspose(sub2,sub2t);
reshape=cvReshape(sub2t,&reshapehdr,0,h*w);
cvGetCol(d,coltemp,i);
cvCopy(reshape,coltemp);
}
cvMatMul(d,&tsim,prod);
cvMul(prod,d,prod);
CvMat *sumcols=cvCreateMat(h*w,1,CV_32FC1);
cvSetZero(sumcols);
for (int i=0;i<prod->cols;i++) {
cvGetCol(prod,coltemp,i);
cvAdd(sumcols,coltemp,sumcols);
}
reshape=cvReshape(sumcols,&reshapehdr,0,w);
cvTranspose(reshape,tg);
cvReleaseMat(&sumcols);
}
//Smooth the gradient now!!
tg=fitparab(*tg,sigma,sigma/4,theta);
cvMaxS(tg,0,tg);
cvReleaseMat(&im_pad);
cvReleaseMat(&tgL_pad);
cvReleaseMat(&tgR_pad);
cvReleaseMat(&rlmask);
cvReleaseMat(&rrmask);
cvReleaseMat(&im);
cvReleaseMat(&tgL);
cvReleaseMat(&tgR);
cvReleaseMat(&temp);
cvReleaseMat(&coltemp);
cvReleaseMat(&sub2);
cvReleaseMat(&sub2t);
cvReleaseMat(&d);
cvReleaseMat(&prod);
return tg;
}
/**
* Euclidean distance transform of a binary image,
* Computation performed upon 'short' values
*
* @param src IN: positive pixels are treated as background
* @param distxArr OUT:
* @param distyArr OUT:
*/
void cvCalcEuclideanDistance(
const CvArr * src, // positive pixels are treated as background
CvArr * distxArr, CvArr * distyArr)
{
CV_FUNCNAME("cvEuclideanDistance");
int x, y, i;
int offset_u, offset_ur, offset_r, offset_rd,
offset_d, offset_dl, offset_l, offset_lu;
double olddist2, newdist2, newdistx, newdisty;
int changed;
__BEGIN__;
// CV_ASSERT(cvGetElemType(src)==CV_16S);
// CV_ASSERT(cvGetElemType(distxArr)==CV_16S);
// CV_ASSERT(cvGetElemType(distyArr)==CV_16S);
CvMat srcHeader, distHeaderX, distHeaderY;
CvMat * srcImage = cvGetMat(src, &srcHeader);
CvMat * distImageX = cvGetMat(distxArr, &distHeaderX);
CvMat * distImageY = cvGetMat(distyArr, &distHeaderY);
CvMat * srcImage2 =
cvCreateMat(srcImage->rows, srcImage->cols, CV_16S);
CvMat * distImageX2 =
cvCreateMat(srcImage->rows, srcImage->cols, CV_16S);
CvMat * distImageY2 =
cvCreateMat(srcImage->rows, srcImage->cols, CV_16S);
if (cvGetElemType(src)!=CV_16S) cvConvert(srcImage, srcImage2);
else cvCopy(srcImage, srcImage2);
CvMat distImageXheader, distImageYheader;
CvMat * distImageXreshaped =
cvReshape(distImageX2, &distImageXheader, 0, 1);
CvMat * distImageYreshaped =
cvReshape(distImageY2, &distImageYheader, 0, 1);
int w = srcImage->cols;
int h = srcImage->rows;
short * distx = (short*)distImageXreshaped->data.ptr;
short * disty = (short*)distImageYreshaped->data.ptr;
// Initialize index offsets for the current image width
offset_u = -w;
offset_ur = -w+1;
offset_r = 1;
offset_rd = w+1;
offset_d = w;
offset_dl = w-1;
offset_l = -1;
offset_lu = -w-1;
/* Initialize the distance images to be all large values */
for(i=0; i<w*h; i++)
// if(img(i) <= 0.0)
if(CV_MAT_ELEM(*srcImage2, short, i/w, i%w) <= 0.0)
{
distx[i] = 32000; // Large but still representable in a short, and
disty[i] = 32000; // 32000^2 + 32000^2 does not overflow an int
}
else
{
void icvConvertPointsHomogenious( const CvMat* src, CvMat* dst )
{
CvMat* temp = 0;
CvMat* denom = 0;
CV_FUNCNAME( "cvConvertPointsHomogenious" );
__BEGIN__;
int i, s_count, s_dims, d_count, d_dims;
CvMat _src, _dst, _ones;
CvMat* ones = 0;
if( !CV_IS_MAT(src) )
CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg,
"The input parameter is not a valid matrix" );
if( !CV_IS_MAT(dst) )
CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg,
"The output parameter is not a valid matrix" );
if( src == dst || src->data.ptr == dst->data.ptr )
{
if( src != dst && (!CV_ARE_TYPES_EQ(src, dst) ||
!CV_ARE_SIZES_EQ(src,dst)) )
CV_ERROR( CV_StsBadArg, "Invalid inplace operation" );
EXIT;
}
if( src->rows > src->cols )
{
if( !((src->cols > 1) ^ (CV_MAT_CN(src->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows must be =1" );
s_dims = CV_MAT_CN(src->type)*src->cols;
s_count = src->rows;
}
else
{
if( !((src->rows > 1) ^ (CV_MAT_CN(src->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows must be =1" );
s_dims = CV_MAT_CN(src->type)*src->rows;
s_count = src->cols;
}
if( src->rows == 1 || src->cols == 1 )
src = cvReshape( src, &_src, 1, s_count );
if( dst->rows > dst->cols )
{
if( !((dst->cols > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows in the input matrix must be =1" );
d_dims = CV_MAT_CN(dst->type)*dst->cols;
d_count = dst->rows;
}
else
{
if( !((dst->rows > 1) ^ (CV_MAT_CN(dst->type) > 1)) )
CV_ERROR( CV_StsBadSize,
"Either the number of channels or columns or "
"rows in the output matrix must be =1" );
d_dims = CV_MAT_CN(dst->type)*dst->rows;
d_count = dst->cols;
}
if( dst->rows == 1 || dst->cols == 1 )
dst = cvReshape( dst, &_dst, 1, d_count );
if( s_count != d_count )
CV_ERROR( CV_StsUnmatchedSizes, "Both matrices must have the "
"same number of points" );
if( CV_MAT_DEPTH(src->type) < CV_32F || CV_MAT_DEPTH(dst->type) < CV_32F )
CV_ERROR( CV_StsUnsupportedFormat,
"Both matrices must be floating-point "
"(single or double precision)" );
if( s_dims < 2 || s_dims > 4 || d_dims < 2 || d_dims > 4 )
CV_ERROR( CV_StsOutOfRange,
"Both input and output point dimensionality "
"must be 2, 3 or 4" );
if( s_dims < d_dims - 1 || s_dims > d_dims + 1 )
CV_ERROR( CV_StsUnmatchedSizes,
"The dimensionalities of input and output "
"point sets differ too much" );
if( s_dims == d_dims - 1 )
{
if( d_count == dst->rows )
{
ones = cvGetSubRect( dst, &_ones, cvRect( s_dims, 0, 1, d_count ));
dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, s_dims, d_count ));
}
else
{
ones = cvGetSubRect( dst, &_ones, cvRect( 0, s_dims, d_count, 1 ));
dst = cvGetSubRect( dst, &_dst, cvRect( 0, 0, d_count, s_dims ));
}
}
if( s_dims <= d_dims )
{
if( src->rows == dst->rows && src->cols == dst->cols )
{
if( CV_ARE_TYPES_EQ( src, dst ) )
cvCopy( src, dst );
else
cvConvert( src, dst );
}
else
{
if( !CV_ARE_TYPES_EQ( src, dst ))
{
CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
cvConvert( src, temp );
src = temp;
}
cvTranspose( src, dst );
}
if( ones )
cvSet( ones, cvRealScalar(1.) );
}
else
{
int s_plane_stride, s_stride, d_plane_stride, d_stride, elem_size;
if( !CV_ARE_TYPES_EQ( src, dst ))
{
CV_CALL( temp = cvCreateMat( src->rows, src->cols, dst->type ));
cvConvert( src, temp );
src = temp;
}
elem_size = CV_ELEM_SIZE(src->type);
if( s_count == src->cols )
s_plane_stride = src->step / elem_size, s_stride = 1;
else
s_stride = src->step / elem_size, s_plane_stride = 1;
if( d_count == dst->cols )
d_plane_stride = dst->step / elem_size, d_stride = 1;
else
d_stride = dst->step / elem_size, d_plane_stride = 1;
CV_CALL( denom = cvCreateMat( 1, d_count, dst->type ));
if( CV_MAT_DEPTH(dst->type) == CV_32F )
{
const float* xs = src->data.fl;
const float* ys = xs + s_plane_stride;
const float* zs = 0;
const float* ws = xs + (s_dims - 1)*s_plane_stride;
float* iw = denom->data.fl;
float* xd = dst->data.fl;
float* yd = xd + d_plane_stride;
float* zd = 0;
if( d_dims == 3 )
{
zs = ys + s_plane_stride;
zd = yd + d_plane_stride;
}
for( i = 0; i < d_count; i++, ws += s_stride )
{
float t = *ws;
iw[i] = t ? t : 1.f;
}
cvDiv( 0, denom, denom );
if( d_dims == 3 )
for( i = 0; i < d_count; i++ )
{
float w = iw[i];
float x = *xs * w, y = *ys * w, z = *zs * w;
xs += s_stride; ys += s_stride; zs += s_stride;
*xd = x; *yd = y; *zd = z;
xd += d_stride; yd += d_stride; zd += d_stride;
}
else
for( i = 0; i < d_count; i++ )
{
float w = iw[i];
float x = *xs * w, y = *ys * w;
xs += s_stride; ys += s_stride;
*xd = x; *yd = y;
xd += d_stride; yd += d_stride;
}
}
else
{
const double* xs = src->data.db;
const double* ys = xs + s_plane_stride;
const double* zs = 0;
const double* ws = xs + (s_dims - 1)*s_plane_stride;
double* iw = denom->data.db;
double* xd = dst->data.db;
double* yd = xd + d_plane_stride;
double* zd = 0;
if( d_dims == 3 )
{
zs = ys + s_plane_stride;
zd = yd + d_plane_stride;
}
for( i = 0; i < d_count; i++, ws += s_stride )
{
double t = *ws;
iw[i] = t ? t : 1.;
}
cvDiv( 0, denom, denom );
if( d_dims == 3 )
for( i = 0; i < d_count; i++ )
{
double w = iw[i];
double x = *xs * w, y = *ys * w, z = *zs * w;
xs += s_stride; ys += s_stride; zs += s_stride;
*xd = x; *yd = y; *zd = z;
xd += d_stride; yd += d_stride; zd += d_stride;
}
else
for( i = 0; i < d_count; i++ )
{
double w = iw[i];
double x = *xs * w, y = *ys * w;
xs += s_stride; ys += s_stride;
*xd = x; *yd = y;
xd += d_stride; yd += d_stride;
}
}
}
__END__;
cvReleaseMat( &denom );
cvReleaseMat( &temp );
}
bool
cvFindExtrinsicCameraParams3( const CvMat* obj_points,
const CvMat* img_points, const CvMat* A,
const CvMat* dist_coeffs,
CvMat* r_vec, CvMat* t_vec )
{
bool fGood = true;
const int max_iter = 20;
CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0;
CV_FUNCNAME( "cvFindExtrinsicCameraParams3" );
__BEGIN__;
int i, j, count;
double a[9], k[4] = { 0, 0, 0, 0 }, R[9], ifx, ify, cx, cy;
double Mc[3] = {0, 0, 0}, MM[9], U[9], V[9], W[3];
double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6];
CvPoint3D64f* M = 0;
CvPoint2D64f *m = 0, *mn = 0;
CvMat _a = cvMat( 3, 3, CV_64F, a );
CvMat _R = cvMat( 3, 3, CV_64F, R );
CvMat _r = cvMat( 3, 1, CV_64F, param );
CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
CvMat _Mc = cvMat( 1, 3, CV_64F, Mc );
CvMat _MM = cvMat( 3, 3, CV_64F, MM );
CvMat _U = cvMat( 3, 3, CV_64F, U );
CvMat _V = cvMat( 3, 3, CV_64F, V );
CvMat _W = cvMat( 3, 1, CV_64F, W );
CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ );
CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr );
CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW );
CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV );
CvMat _delta = cvMat( 6, 1, CV_64F, delta );
CvMat _param = cvMat( 6, 1, CV_64F, param );
CvMat _dpdr, _dpdt;
if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(img_points) ||
!CV_IS_MAT(A) || !CV_IS_MAT(r_vec) || !CV_IS_MAT(t_vec) )
CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
count = MAX(obj_points->cols, obj_points->rows);
CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 ));
M = (CvPoint3D64f*)_M->data.db;
m = (CvPoint2D64f*)_m->data.db;
mn = (CvPoint2D64f*)_mn->data.db;
CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
CV_CALL( cvConvertPointsHomogenious( img_points, _m ));
CV_CALL( cvConvert( A, &_a ));
if( dist_coeffs )
{
CvMat _k;
if( !CV_IS_MAT(dist_coeffs) ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F ||
dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(dist_coeffs->type)), k );
CV_CALL( cvConvert( dist_coeffs, &_k ));
}
if( CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F ||
r_vec->rows != 1 && r_vec->cols != 1 ||
r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3 )
CV_ERROR( CV_StsBadArg, "Rotation vector must be 1x3 or 3x1 floating-point vector" );
if( CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F ||
t_vec->rows != 1 && t_vec->cols != 1 ||
t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
CV_ERROR( CV_StsBadArg,
"Translation vector must be 1x3 or 3x1 floating-point vector" );
ifx = 1./a[0]; ify = 1./a[4];
cx = a[2]; cy = a[5];
// normalize image points
// (unapply the intrinsic matrix transformation and distortion)
for( i = 0; i < count; i++ )
{
double x = (m[i].x - cx)*ifx, y = (m[i].y - cy)*ify, x0 = x, y0 = y;
// compensate distortion iteratively
if( dist_coeffs )
for( j = 0; j < 5; j++ )
{
double r2 = x*x + y*y;
double icdist = 1./(1 + k[0]*r2 + k[1]*r2*r2);
double delta_x = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
double delta_y = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
x = (x0 - delta_x)*icdist;
y = (y0 - delta_y)*icdist;
}
mn[i].x = x; mn[i].y = y;
// calc mean(M)
Mc[0] += M[i].x;
Mc[1] += M[i].y;
Mc[2] += M[i].z;
}
Mc[0] /= count;
Mc[1] /= count;
Mc[2] /= count;
cvReshape( _M, _M, 1, count );
cvMulTransposed( _M, &_MM, 1, &_Mc );
cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T );
// initialize extrinsic parameters
if( W[2]/W[1] < 1e-3 || count < 4 )
{
// a planar structure case (all M's lie in the same plane)
double tt[3], h[9], h1_norm, h2_norm;
CvMat* R_transform = &_V;
CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
CvMat _H = cvMat( 3, 3, CV_64F, h );
CvMat _h1, _h2, _h3;
if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
cvSetIdentity( R_transform );
if( cvDet(R_transform) < 0 )
cvScale( R_transform, R_transform, -1 );
cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
for( i = 0; i < count; i++ )
{
const double* Rp = R_transform->data.db;
const double* Tp = T_transform.data.db;
const double* src = _M->data.db + i*3;
double* dst = _Mxy->data.db + i*2;
dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
}
cvFindHomography( _Mxy, _mn, &_H );
cvGetCol( &_H, &_h1, 0 );
_h2 = _h1; _h2.data.db++;
_h3 = _h2; _h3.data.db++;
h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
cvScale( &_h1, &_h1, 1./h1_norm );
cvScale( &_h2, &_h2, 1./h2_norm );
cvScale( &_h3, &_t, 2./(h1_norm + h2_norm));
cvCrossProduct( &_h1, &_h2, &_h3 );
cvRodrigues2( &_H, &_r );
cvRodrigues2( &_r, &_H );
cvMatMulAdd( &_H, &T_transform, &_t, &_t );
cvMatMul( &_H, R_transform, &_R );
cvRodrigues2( &_R, &_r );
}
else
{
// non-planar structure. Use DLT method
double* L;
double LL[12*12], LW[12], LV[12*12], sc;
CvMat _LL = cvMat( 12, 12, CV_64F, LL );
CvMat _LW = cvMat( 12, 1, CV_64F, LW );
CvMat _LV = cvMat( 12, 12, CV_64F, LV );
CvMat _RRt, _RR, _tt;
CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F ));
L = _L->data.db;
for( i = 0; i < count; i++, L += 24 )
{
double x = -mn[i].x, y = -mn[i].y;
L[0] = L[16] = M[i].x;
L[1] = L[17] = M[i].y;
L[2] = L[18] = M[i].z;
L[3] = L[19] = 1.;
L[4] = L[5] = L[6] = L[7] = 0.;
L[12] = L[13] = L[14] = L[15] = 0.;
L[8] = x*M[i].x;
L[9] = x*M[i].y;
L[10] = x*M[i].z;
L[11] = x;
L[20] = y*M[i].x;
L[21] = y*M[i].y;
L[22] = y*M[i].z;
L[23] = y;
}
cvMulTransposed( _L, &_LL, 1 );
cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
_RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
cvGetCols( &_RRt, &_RR, 0, 3 );
cvGetCol( &_RRt, &_tt, 3 );
if( cvDet(&_RR) < 0 )
cvScale( &_RRt, &_RRt, -1 );
sc = cvNorm(&_RR);
cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
cvScale( &_tt, &_t, cvNorm(&_R)/sc );
cvRodrigues2( &_R, &_r );
cvReleaseMat( &_L );
}
//
// Check if new r and t are good
//
if ( cvGetReal1D( r_vec, 0 ) ||
cvGetReal1D( r_vec, 1 ) ||
cvGetReal1D( r_vec, 2 ) ||
cvGetReal1D( t_vec, 0 ) ||
cvGetReal1D( t_vec, 1 ) ||
cvGetReal1D( t_vec, 2 ) )
{
//
// perfom this only when the old r and t exist.
//
CvMat * R_inv = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * r_old = cvCreateMat( 3, 1, CV_64FC1 );
CvMat * R_old = cvCreateMat( 3, 3, CV_64FC1 );
CvMat * t_old = cvCreateMat( 3, 1, CV_64FC1 );
// get new center
cvInvert( &_R, R_inv );
double new_center[3];
CvMat newCenter = cvMat( 3, 1, CV_64FC1, new_center );
cvMatMul( R_inv, &_t, &newCenter );
cvScale( &newCenter, &newCenter, -1 );
// get old center
cvConvert( r_vec, r_old );
cvConvert( t_vec, t_old );
cvRodrigues2( r_old, R_old );
cvInvert( R_old, R_inv );
double old_center[3];
CvMat oldCenter = cvMat( 3, 1, CV_64FC1, old_center );
cvMatMul( R_inv, t_old, &oldCenter );
cvScale( &oldCenter, &oldCenter, -1 );
// get difference
double diff_center = 0;
for ( int i = 0; i < 3 ; i ++ )
diff_center += pow( new_center[i] - old_center[i], 2 );
diff_center = sqrt( diff_center );
if ( diff_center > 300 )
{
printf("diff_center = %.2f --> set initial r and t as same as the previous\n", diff_center);
cvConvert(r_vec, &_r);
cvConvert(t_vec, &_t);
fGood = false;
}
// else printf("diff_center = %.2f\n", diff_center );
cvReleaseMat( &R_inv );
cvReleaseMat( &r_old );
cvReleaseMat( &R_old );
cvReleaseMat( &t_old );
}
if ( fGood )
{
CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 ));
cvGetCols( _J, &_dpdr, 0, 3 );
cvGetCols( _J, &_dpdt, 3, 6 );
// refine extrinsic parameters using iterative algorithm
for( i = 0; i < max_iter; i++ )
{
double n1, n2;
cvReshape( _mn, _mn, 2, 1 );
cvProjectPoints2( _M, &_r, &_t, &_a, dist_coeffs,
_mn, &_dpdr, &_dpdt, 0, 0, 0 );
cvSub( _m, _mn, _mn );
cvReshape( _mn, _mn, 1, 2*count );
cvMulTransposed( _J, &_JtJ, 1 );
cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T );
cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T );
if( JtJW[5]/JtJW[0] < 1e-12 )
break;
cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr,
&_delta, CV_SVD_U_T + CV_SVD_V_T );
cvAdd( &_delta, &_param, &_param );
n1 = cvNorm( &_delta );
n2 = cvNorm( &_param );
if( n1/n2 < 1e-10 )
break;
}
_r = cvMat( r_vec->rows, r_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), param );
_t = cvMat( t_vec->rows, t_vec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), param + 3 );
cvConvert( &_r, r_vec );
cvConvert( &_t, t_vec );
}
__END__;
cvReleaseMat( &_M );
cvReleaseMat( &_Mxy );
cvReleaseMat( &_m );
cvReleaseMat( &_mn );
cvReleaseMat( &_L );
cvReleaseMat( &_J );
return fGood;
}
CvMat* cveReshape(CvArr* arr, CvMat* header, int newCn, int newRows)
{
return cvReshape(arr, header, newCn, newRows);
}
int main()
{
FILE * fp = fopen("markingData_body_frontal_flipped.txt", "r");
char fn[1024]; int no;
int x, y;
const int N = 12;
const int M = 23*2;
CvMat * data = cvCreateMat(N, M, CV_64F);
CvMat * mean = cvCreateMat(1, M, CV_64F);
// get data matrix for training
for (int ff = 0; ff < N; ff++){
fscanf(fp, "%s %d\n", fn, &no);
for (int i = 0; i < 23; i++){
fscanf(fp, "%d %d\n", &x, &y);
cvmSet(data, ff, i*2+0, x);
cvmSet(data, ff, i*2+1, y);
}fscanf(fp, "\n");
}
const int ncols = 320, nrows = 240;
IplImage * dispImage = cvCreateImage(cvSize(320,240), IPL_DEPTH_8U, 3);
cvSet(dispImage,cvScalarAll(255));
CvPrincipleComponentAnalysis pca;
CV_TIMER_START();
pca.set_data(data);
pca.back_project();
CV_TIMER_SHOW();
cvCopy(pca.mean, mean);
CvMat subdatahdr;
CvMat * subdata = cvGetSubRect(data, &subdatahdr, cvRect(0,0,M,1));
CvMat subdata2hdr, meanhdr;
CvMat * subdata2 = cvReshape(subdata, &subdata2hdr, 2);
CvMat * mean2 = cvReshape(mean, &meanhdr, 2);
double scale, theta, tx,ty;
cvShapeFitting(mean2, subdata2, scale, theta, tx, ty);
CvMat * subdata3 = cvCreateMat(2, 23, CV_64F);
for (int i = 0; i < 23; i++){
cvmSet(subdata3, 0, i, cvmGet(subdata, 0, i*2+0));
cvmSet(subdata3, 1, i, cvmGet(subdata, 0, i*2+1));
}
cvShapeTransform2(subdata3,scale, theta, tx, ty);
for (int i = 0; i < 23; i++){
cvmSet(subdata, 0, i*2+0, cvmGet(subdata3, 0, i));
cvmSet(subdata, 0, i*2+1, cvmGet(subdata3, 1, i));
}
cvReleaseMatEx(subdata3);
// compare landmarks
if (0)
for (int i = 0; i < 12; i++){
for (int j = 0; j < 23; j++){
cvCircle(dispImage, cvPoint(cvmGet(data, i, j*2), cvmGet(data, i, j*2+1)), 2, cvScalar(255,0,0), -1);
//cvCircle(dispImage, cvPoint(cvmGet(orig, i, j*2), cvmGet(data, i, j*2+1)), 2, cvScalar(0,0,255), -1);
}
}
for (int j = 0; j < 23; j++){
cvCircle(
dispImage, cvPoint(cvmGet(mean, 0, j*2), cvmGet(mean, 0, j*2+1)),
1, cvScalar(255,0,0), -1);
}
for (int j = 0; j < 23; j++){
cvCircle(
dispImage, cvPoint(cvmGet(subdata, 0, j*2), cvmGet(subdata, 0, j*2+1)),
1, cvScalar(0,0,255), -1);
}
cvNamedWindow("Test");
cvShowImageEx("Test", dispImage); cvWaitKey(0);
fclose(fp);
cvReleaseImageEx(dispImage);
cvReleaseMatEx(data);
cvReleaseMatEx(mean);
return 0;
}
CV_IMPL double
cvThreshold( const void* srcarr, void* dstarr, double thresh, double maxval, int type )
{
CvHistogram* hist = 0;
CV_FUNCNAME( "cvThreshold" );
__BEGIN__;
CvSize roi;
int src_step, dst_step;
CvMat src_stub, *src = (CvMat*)srcarr;
CvMat dst_stub, *dst = (CvMat*)dstarr;
CvMat src0, dst0;
int coi1 = 0, coi2 = 0;
int ithresh, imaxval, cn;
bool use_otsu;
CV_CALL( src = cvGetMat( src, &src_stub, &coi1 ));
CV_CALL( dst = cvGetMat( dst, &dst_stub, &coi2 ));
if( coi1 + coi2 )
CV_ERROR( CV_BadCOI, "COI is not supported by the function" );
if( !CV_ARE_CNS_EQ( src, dst ) )
CV_ERROR( CV_StsUnmatchedFormats, "Both arrays must have equal number of channels" );
cn = CV_MAT_CN(src->type);
if( cn > 1 )
{
src = cvReshape( src, &src0, 1 );
dst = cvReshape( dst, &dst0, 1 );
}
use_otsu = (type & ~CV_THRESH_MASK) == CV_THRESH_OTSU;
type &= CV_THRESH_MASK;
if( use_otsu )
{
float _ranges[] = { 0, 256 };
float* ranges = _ranges;
int hist_size = 256;
void* srcarr0 = src;
if( CV_MAT_TYPE(src->type) != CV_8UC1 )
CV_ERROR( CV_StsNotImplemented, "Otsu method can only be used with 8uC1 images" );
CV_CALL( hist = cvCreateHist( 1, &hist_size, CV_HIST_ARRAY, &ranges ));
cvCalcArrHist( &srcarr0, hist );
thresh = cvFloor(icvGetThreshVal_Otsu( hist ));
}
if( !CV_ARE_DEPTHS_EQ( src, dst ) )
{
if( CV_MAT_TYPE(dst->type) != CV_8UC1 )
CV_ERROR( CV_StsUnsupportedFormat, "In case of different types destination should be 8uC1" );
if( type != CV_THRESH_BINARY && type != CV_THRESH_BINARY_INV )
CV_ERROR( CV_StsBadArg,
"In case of different types only CV_THRESH_BINARY "
"and CV_THRESH_BINARY_INV thresholding types are supported" );
if( maxval < 0 )
{
CV_CALL( cvSetZero( dst ));
}
else
{
CV_CALL( cvCmpS( src, thresh, dst, type == CV_THRESH_BINARY ? CV_CMP_GT : CV_CMP_LE ));
if( maxval < 255 )
CV_CALL( cvAndS( dst, cvScalarAll( maxval ), dst ));
}
EXIT;
}
if( !CV_ARE_SIZES_EQ( src, dst ) )
CV_ERROR( CV_StsUnmatchedSizes, "" );
roi = cvGetMatSize( src );
if( CV_IS_MAT_CONT( src->type & dst->type ))
{
roi.width *= roi.height;
roi.height = 1;
src_step = dst_step = CV_STUB_STEP;
}
else
{
src_step = src->step;
dst_step = dst->step;
}
switch( CV_MAT_DEPTH(src->type) )
{
case CV_8U:
ithresh = cvFloor(thresh);
imaxval = cvRound(maxval);
if( type == CV_THRESH_TRUNC )
imaxval = ithresh;
imaxval = CV_CAST_8U(imaxval);
if( ithresh < 0 || ithresh >= 255 )
{
if( type == CV_THRESH_BINARY || type == CV_THRESH_BINARY_INV ||
((type == CV_THRESH_TRUNC || type == CV_THRESH_TOZERO_INV) && ithresh < 0) ||
(type == CV_THRESH_TOZERO && ithresh >= 255) )
{
int v = type == CV_THRESH_BINARY ? (ithresh >= 255 ? 0 : imaxval) :
type == CV_THRESH_BINARY_INV ? (ithresh >= 255 ? imaxval : 0) :
type == CV_THRESH_TRUNC ? imaxval : 0;
cvSet( dst, cvScalarAll(v) );
EXIT;
}
else
{
cvCopy( src, dst );
EXIT;
}
}
if( type == CV_THRESH_BINARY || type == CV_THRESH_BINARY_INV )
{
if( icvCompareC_8u_C1R_cv_p && icvAndC_8u_C1R_p )
{
IPPI_CALL( icvCompareC_8u_C1R_cv_p( src->data.ptr, src_step,
(uchar)ithresh, dst->data.ptr, dst_step, roi,
type == CV_THRESH_BINARY ? cvCmpGreater : cvCmpLessEq ));
if( imaxval < 255 )
IPPI_CALL( icvAndC_8u_C1R_p( dst->data.ptr, dst_step,
(uchar)imaxval, dst->data.ptr, dst_step, roi ));
EXIT;
}
}
else if( type == CV_THRESH_TRUNC || type == CV_THRESH_TOZERO_INV )
{
if( icvThreshold_GTVal_8u_C1R_p )
{
IPPI_CALL( icvThreshold_GTVal_8u_C1R_p( src->data.ptr, src_step,
dst->data.ptr, dst_step, roi, (uchar)ithresh,
(uchar)(type == CV_THRESH_TRUNC ? ithresh : 0) ));
EXIT;
}
}
else
{
assert( type == CV_THRESH_TOZERO );
if( icvThreshold_LTVal_8u_C1R_p )
{
ithresh = cvFloor(thresh+1.);
ithresh = CV_CAST_8U(ithresh);
IPPI_CALL( icvThreshold_LTVal_8u_C1R_p( src->data.ptr, src_step,
dst->data.ptr, dst_step, roi, (uchar)ithresh, 0 ));
EXIT;
}
}
icvThresh_8u_C1R( src->data.ptr, src_step,
dst->data.ptr, dst_step, roi,
(uchar)ithresh, (uchar)imaxval, type );
break;
case CV_32F:
if( type == CV_THRESH_TRUNC || type == CV_THRESH_TOZERO_INV )
{
if( icvThreshold_GTVal_32f_C1R_p )
{
IPPI_CALL( icvThreshold_GTVal_32f_C1R_p( src->data.fl, src_step,
dst->data.fl, dst_step, roi, (float)thresh,
type == CV_THRESH_TRUNC ? (float)thresh : 0 ));
EXIT;
}
}
else if( type == CV_THRESH_TOZERO )
{
if( icvThreshold_LTVal_32f_C1R_p )
{
IPPI_CALL( icvThreshold_LTVal_32f_C1R_p( src->data.fl, src_step,
dst->data.fl, dst_step, roi, (float)(thresh*(1 + FLT_EPSILON)), 0 ));
EXIT;
}
}
icvThresh_32f_C1R( src->data.fl, src_step, dst->data.fl, dst_step, roi,
(float)thresh, (float)maxval, type );
break;
default:
CV_ERROR( CV_BadDepth, cvUnsupportedFormat );
}
__END__;
if( hist )
cvReleaseHist( &hist );
return thresh;
}
