//============================================================================
void AAM_TDM::AlignTextureToRef(const CvMat* refTextrure, CvMat* Texture)
{
AAM_TDM::ZeroMeanUnitLength(Texture);
double alpha = cvDotProduct(Texture, refTextrure);
if(fabs(alpha) > DBL_EPSILON)
cvConvertScale(Texture, Texture, 1.0 / alpha, 0.0);
}
static void icvH11Ops( CvMat* X, CvMat* Y, void* userdata )
{
CvH11OpsData* h11 = (CvH11OpsData*)userdata;
h11->AOps( X, h11->AR, h11->userdata );
h11->AtOps( h11->AR, h11->AtR, h11->userdata );
double rc = h11->fe_inv_2 * cvDotProduct( h11->atr, X );
cvAddWeighted( h11->AtR, -h11->fe_inv, h11->atr, rc, 0, h11->AtR );
cvMul( h11->sigx, X, h11->tX );
cvAdd( h11->tX, h11->AtR, Y );
}
double cvCGSolve( CvMatOps AOps, void* userdata, CvMat* B, CvMat* X, CvTermCriteria term_crit )
{
cvZero( X );
CvMat* R = cvCloneMat( B );
double delta = cvDotProduct( R, R );
CvMat* D = cvCloneMat( R );
double delta0 = delta;
double bestres = 1.;
CvMat* BX = cvCloneMat( X );
CvMat* Q = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
for ( int i = 0; i < term_crit.max_iter; i++ )
{
AOps( D, Q, userdata );
double alpha = delta / cvDotProduct( D, Q );
cvScaleAdd( D, cvScalar(alpha), X, X );
if ( (i + 1) % 50 == 0 )
{
AOps( X, R, userdata );
cvSub( B, R, R );
} else {
cvScaleAdd( Q, cvScalar(-alpha), R, R );
}
double deltaold = delta;
delta = cvDotProduct( R, R );
double beta = delta / deltaold;
cvScaleAdd( D, cvScalar(beta), R, D );
double res = delta / delta0;
if ( res < bestres )
{
cvCopy( X, BX );
bestres = res;
}
if ( delta < delta0 * term_crit.epsilon )
break;
}
cvCopy( BX, X );
cvReleaseMat( &R );
cvReleaseMat( &D );
cvReleaseMat( &BX );
cvReleaseMat( &Q );
return sqrt( bestres );
}
CvMat* Conv_Weighted_Gaussian(IplImage *inp_img,CvMat *kernel)
{
//IplImage *result;
int i,j;
CvPoint start;
CvMat *tmp;
CvMat *ddd;
CvMat *w_gauss;
//result=cvCloneImage(inp_img);
double val;
ddd=cvCreateMat(inp_img->height,inp_img->width,CV_64FC1);
for(i=0;i<inp_img->height;i++)
{
for(j=0;j<inp_img->width;j++)
{
start.x=j-(kernel->cols/2);
start.y=i-(kernel->rows/2);
tmp=Get_Mat(start,kernel->cols,kernel->rows,inp_img);
w_gauss=Weighted_Gaussian(tmp,kernel);
val=cvDotProduct(w_gauss,tmp);
cvmSet(ddd,i,j,val);
cvReleaseMat(&tmp);
cvReleaseMat(&w_gauss);
}
}
/*
cvNamedWindow("fdf",1);
cvShowImage("fdf",ddd);
cvWaitKey(0);
*/
return ddd;
}
int Recognize(double * Features,CvArr** eigenVects,CvArr** eigenVals,CvArr** Means,int numGestures,int numFeatures,int use)
{
int i,j,z;
double d=10000;
double d2,tmp;
int choice=0;
//double mahal=0;
CvArr* input=cvCreateMat(1,numFeatures,CV_64FC1);
CvArr* tempSub=cvCreateMat(1,numFeatures,CV_64FC1);
CvArr* tempEV=cvCreateMat(1,numFeatures,CV_64FC1);
for(i=0;i<numFeatures;i++)
{
*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)input), 0,i ) ) = Features[i];
}
for(i=0;i<numGestures;i++)
{
for(j=0;j<use;j++)
{
cvSub(input,Means[i],tempSub,NULL);
//cvTranspose(tempSub,tempTran);
for(z=0;z<numFeatures;z++)
{
//tempEV=eigenVects[i]
*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)tempEV), 0,z ) )=CV_MAT_ELEM( *((CvMat *)eigenVects[i]), double, j, z );
}
tmp=(double)(cvDotProduct(tempSub,tempEV));//ut(x-u)/lambda
d2+=(tmp*tmp)/(double)(CV_MAT_ELEM( *((CvMat *)eigenVals[i]), double, 0, j ));
}
d2=sqrt(d2);
fprintf(stderr,"for %d the distance is %lf\n",i,d2);
if (d2<d)
{
//fprintf(stderr,"for %d the distance is %lf\n",i,d2);
d=d2;
choice=i;
}
d2=0;
}
return choice;
}
int plane_intersection(struct plane *pl, struct line *l, CvMat *res)
{
float d = cvDotProduct(&(pl->p), &(pl->norm));
float num = d - (FLOAT_MAT_ELEM(&(pl->norm), 0, 0) * FLOAT_MAT_ELEM(&(l->p0), 0, 0)) -
(FLOAT_MAT_ELEM(&(pl->norm), 1, 0) * FLOAT_MAT_ELEM(&(l->p0), 1, 0)) -
(FLOAT_MAT_ELEM(&(pl->norm), 2, 0) * FLOAT_MAT_ELEM(&(l->p0), 2, 0));
float denom = FLOAT_MAT_ELEM(&(pl->norm), 0, 0) * ( FLOAT_MAT_ELEM(l->p1, 0, 0) - FLOAT_MAT_ELEM( &(l->p0), 0, 0) ) +
FLOAT_MAT_ELEM(&(pl->norm), 1, 0) * ( FLOAT_MAT_ELEM(l->p1, 1, 0) - FLOAT_MAT_ELEM( &(l->p0), 1, 0) ) +
FLOAT_MAT_ELEM(&(pl->norm), 2, 0) * ( FLOAT_MAT_ELEM(l->p1, 2, 0) - FLOAT_MAT_ELEM( &(l->p0), 2, 0) );
if (denom == 0)
return 0;
float t = num/denom;
cvSub(l->p1, &(l->p0), res);
myMulS(res, t, res);
return 1;
}
CV_IMPL void
cvCalcImageHomography( float* line, CvPoint3D32f* _center,
float* _intrinsic, float* _homography )
{
double norm_xy, norm_xz, xy_sina, xy_cosa, xz_sina, xz_cosa, nx1, plane_dist;
float _ry[3], _rz[3], _r_trans[9];
CvMat rx = cvMat( 1, 3, CV_32F, line );
CvMat ry = cvMat( 1, 3, CV_32F, _ry );
CvMat rz = cvMat( 1, 3, CV_32F, _rz );
CvMat r_trans = cvMat( 3, 3, CV_32F, _r_trans );
CvMat center = cvMat( 3, 1, CV_32F, _center );
float _sub[9];
CvMat sub = cvMat( 3, 3, CV_32F, _sub );
float _t_trans[3];
CvMat t_trans = cvMat( 3, 1, CV_32F, _t_trans );
CvMat intrinsic = cvMat( 3, 3, CV_32F, _intrinsic );
CvMat homography = cvMat( 3, 3, CV_32F, _homography );
if( !line || !_center || !_intrinsic || !_homography )
CV_Error( CV_StsNullPtr, "" );
norm_xy = cvSqrt( line[0] * line[0] + line[1] * line[1] );
xy_cosa = line[0] / norm_xy;
xy_sina = line[1] / norm_xy;
norm_xz = cvSqrt( line[0] * line[0] + line[2] * line[2] );
xz_cosa = line[0] / norm_xz;
xz_sina = line[2] / norm_xz;
nx1 = -xz_sina;
_rz[0] = (float)(xy_cosa * nx1);
_rz[1] = (float)(xy_sina * nx1);
_rz[2] = (float)xz_cosa;
cvScale( &rz, &rz, 1./cvNorm(&rz,0,CV_L2) );
/* new axe y */
cvCrossProduct( &rz, &rx, &ry );
cvScale( &ry, &ry, 1./cvNorm( &ry, 0, CV_L2 ) );
/* transpone rotation matrix */
memcpy( &_r_trans[0], line, 3*sizeof(float));
memcpy( &_r_trans[3], _ry, 3*sizeof(float));
memcpy( &_r_trans[6], _rz, 3*sizeof(float));
/* calculate center distanse from arm plane */
plane_dist = cvDotProduct( ¢er, &rz );
/* calculate (I - r_trans)*center */
cvSetIdentity( &sub );
cvSub( &sub, &r_trans, &sub );
cvMatMul( &sub, ¢er, &t_trans );
cvMatMul( &t_trans, &rz, &sub );
cvScaleAdd( &sub, cvRealScalar(1./plane_dist), &r_trans, &sub ); /* ? */
cvMatMul( &intrinsic, &sub, &r_trans );
cvInvert( &intrinsic, &sub, CV_SVD );
cvMatMul( &r_trans, &sub, &homography );
}
//============================================================================
void AAM_TDM::NormalizeTexture(const CvMat* refTextrure, CvMat* Texture)
{
AAM_TDM::ZeroMeanUnitLength(Texture);
double alpha = cvDotProduct(Texture, refTextrure);
if(alpha != 0) cvConvertScale(Texture, Texture, 1.0/alpha, 0);
}
static void
icvSpillTreeDFSearch( CvSpillTree* tr,
CvSpillTreeNode* node,
CvResult* heap,
int* es,
const CvMat* desc,
const int k,
const int emax,
bool * cache)
{
if ((emax > 0)&&( *es >= emax ))
return;
double dist, p=0;
double distance;
while ( node->spill )
{
// defeatist search
if ( !node->leaf )
p = cvDotProduct( node->u, desc );
if ( p < node->lb && node->lc->cc >= k ) // check the number of children larger than k otherwise you'll skip over better neighbor
node = node->lc;
else if ( p > node->ub && node->rc->cc >= k )
node = node->rc;
else
break;
if ( NULL == node )
return;
}
if ( node->leaf )
{
// a leaf, naive search
CvSpillTreeNode* it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
if ( !cache[it->i] )
{
distance = cvNorm( it->center, desc );
cache[it->i] = true;
if (( heap[0].index == -1)||( distance < heap[0].distance ))
{
CvResult current_result;
current_result.index = it->i;
current_result.distance = distance;
heap[0] = current_result;
icvSpillTreeNodeHeapify( heap, 0, k );
(*es)++;
}
}
it = it->rc;
}
return;
}
dist = cvNorm( node->center, desc );
// impossible case, skip
if (( heap[0].index != -1 )&&( dist-node->r > heap[0].distance ))
return;
p = cvDotProduct( node->u, desc );
// guided dfs
if ( p < node->mp )
{
icvSpillTreeDFSearch( tr, node->lc, heap, es, desc, k, emax, cache );
icvSpillTreeDFSearch( tr, node->rc, heap, es, desc, k, emax, cache );
} else {
icvSpillTreeDFSearch( tr, node->rc, heap, es, desc, k, emax, cache );
icvSpillTreeDFSearch( tr, node->lc, heap, es, desc, k, emax, cache );
}
}
static void
icvDFSInitSpillTreeNode( const CvSpillTree* tr,
const int d,
CvSpillTreeNode* node )
{
if ( node->cc <= tr->naive )
{
// already get to a leaf, terminate the recursion.
node->leaf = true;
node->spill = false;
return;
}
// random select a node, then find a farthest node from this one, then find a farthest from that one...
// to approximate the farthest node-pair
static CvRNG rng_state = cvRNG(0xdeadbeef);
int rn = cvRandInt( &rng_state ) % node->cc;
CvSpillTreeNode* lnode = NULL;
CvSpillTreeNode* rnode = node->lc;
for ( int i = 0; i < rn; i++ )
rnode = rnode->rc;
lnode = icvFarthestNode( rnode, node->lc, node->cc );
rnode = icvFarthestNode( lnode, node->lc, node->cc );
// u is the projection vector
node->u = cvCreateMat( 1, d, tr->type );
cvSub( lnode->center, rnode->center, node->u );
cvNormalize( node->u, node->u );
// find the center of node in hyperspace
node->center = cvCreateMat( 1, d, tr->type );
cvZero( node->center );
CvSpillTreeNode* it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
cvAdd( it->center, node->center, node->center );
it = it->rc;
}
cvConvertScale( node->center, node->center, 1./node->cc );
// project every node to "u", and find the mean point "mp"
it = node->lc;
node->r = -1.;
node->mp = 0;
for ( int i = 0; i < node->cc; i++ )
{
node->mp += ( it->p = cvDotProduct( it->center, node->u ) );
double norm = cvNorm( node->center, it->center );
if ( norm > node->r )
node->r = norm;
it = it->rc;
}
node->mp = node->mp / node->cc;
// overlapping buffer and upper bound, lower bound
double ob = (lnode->p-rnode->p)*tr->tau*.5;
node->ub = node->mp+ob;
node->lb = node->mp-ob;
int sl = 0, l = 0;
int sr = 0, r = 0;
it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
if ( it->p <= node->ub )
sl++;
if ( it->p >= node->lb )
sr++;
if ( it->p < node->mp )
l++;
else
r++;
it = it->rc;
}
// precision problem, return the node as it is.
if (( l == 0 )||( r == 0 ))
{
cvReleaseMat( &(node->u) );
cvReleaseMat( &(node->center) );
node->leaf = true;
node->spill = false;
return;
}
CvSpillTreeNode* lc = (CvSpillTreeNode*)cvAlloc( sizeof(CvSpillTreeNode) );
memset(lc, 0, sizeof(CvSpillTreeNode));
CvSpillTreeNode* rc = (CvSpillTreeNode*)cvAlloc( sizeof(CvSpillTreeNode) );
memset(rc, 0, sizeof(CvSpillTreeNode));
lc->lc = lc->rc = rc->lc = rc->rc = NULL;
lc->cc = rc->cc = 0;
int undo = cvRound(node->cc*tr->rho);
if (( sl >= undo )||( sr >= undo ))
{
// it is not a spill point (defeatist search disabled)
it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
CvSpillTreeNode* next = it->rc;
if ( it->p < node->mp )
icvAppendSpillTreeNode( lc, it );
else
icvAppendSpillTreeNode( rc, it );
it = next;
}
node->spill = false;
} else {
// a spill point
it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
CvSpillTreeNode* next = it->rc;
if ( it->p < node->lb )
icvAppendSpillTreeNode( lc, it );
else if ( it->p > node->ub )
icvAppendSpillTreeNode( rc, it );
else {
CvSpillTreeNode* cit = icvCloneSpillTreeNode( it );
icvAppendSpillTreeNode( lc, it );
icvAppendSpillTreeNode( rc, cit );
}
it = next;
}
node->spill = true;
}
node->lc = lc;
node->rc = rc;
// recursion process
icvDFSInitSpillTreeNode( tr, d, node->lc );
icvDFSInitSpillTreeNode( tr, d, node->rc );
}
void ImageProcessing::getDisparity() {
int windowSize = 9;
int DSR = 20;
//char *leftImgPath, *rightImgPath;
//cout<<"Enter full path of Left image ";
//cin>>leftImgPath;
//cout<<"Enter full path of Left image ";
//cin>>rightImgPath;
IplImage *LeftinputImage = cvLoadImage("../outputs/raw/left-0.pgm", 0);
IplImage *RightinputImage = cvLoadImage("../outputs/raw/right-0.pgm", 0);
// int width = LeftinputImage->width;
// int height = LeftinputImage->height;
/****************8U to 32F**********************/
IplImage *LeftinputImage32 = cvCreateImage(cvSize(LeftinputImage->width, LeftinputImage->height), 32, 1);
//IPL_DEPTH_32F
IplImage *RightinputImage32 = cvCreateImage(cvSize(LeftinputImage->width, LeftinputImage->height), 32, 1);
cvConvertScale(LeftinputImage, LeftinputImage32, 1 / 255.);
cvConvertScale(RightinputImage, RightinputImage32, 1 / 255.);
int offset = floor((double) windowSize / 2);
int height = LeftinputImage32->height;
int width = LeftinputImage32->width;
double *localNCC = new double[DSR];
int x = 0, y = 0, d = 0, m = 0;
int N = windowSize;
IplImage *leftWinImg = cvCreateImage(cvSize(N, N), 32, 1);
//mySubImage(LeftinputImage32,cvRect(0,0,N,N));
IplImage *rightWinImg = cvCreateImage(cvSize(N, N), 32, 1);
//mySubImage(RightinputImage32,cvRect(0,0,N,N));
IplImage *disparity = cvCreateImage(cvSize(width, height), 8, 1);
//or IPL_DEPTH_8U
BwImage imgA(disparity);
for (y = 0; y < height; y++) {
for (x = 0; x < width; x++) {
imgA[y][x] = 0;
}
}
CvScalar s1;
CvScalar s2;
for (y = 0; y < height - N; y++) {
//height-N
for (x = 0; x < width - N; x++) {
//width-N
//getWindow(i,j,leftim,wl,N);
cvSetImageROI(LeftinputImage32, cvRect(x, y, N, N));
s1 = cvAvg(LeftinputImage32, NULL);
cvSubS(LeftinputImage32, s1, leftWinImg, NULL); //zero-means
cvNormalize(leftWinImg, leftWinImg, 1, 0, CV_L2, NULL);
d = 0;
//initialise localNCC
for (m = 0; m < DSR; m++) {
localNCC[m] = 0;
}
do {
if (x - d >= 0) {
cvSetImageROI(RightinputImage32, cvRect(x - d, y, N, N));
s2 = cvAvg(RightinputImage32, NULL);
cvSubS(RightinputImage32, s2, rightWinImg, NULL); //zero-means
cvNormalize(rightWinImg, rightWinImg, 1, 0, CV_L2, NULL);
} else {
break;
}
localNCC[d] = cvDotProduct(leftWinImg, rightWinImg);
cvResetImageROI(RightinputImage32);
d++;
} while (d <= DSR);
//to find the best d and store
imgA[y + offset][x + offset] = getMaxMin(localNCC, DSR, 1) *16;
cvResetImageROI(LeftinputImage32);
} //x
if (y % 10 == 0)
cout << "row=" << y << " of " << height << endl;
} //y
cvReleaseImage(&leftWinImg);
cvReleaseImage(&rightWinImg);
cvSaveImage("disparity.pgm", disparity);
waitHere();
//cv::imwrite("disparity.pgm",&disparity);
cout << "Displaying Disparity image" << endl;
// cvShowImage( "Disparity", disparity);
//cv::waitKey(0);
//return disparity;
}
//For the given PCA space, loads a dataset and saves the corresponding UCI format
void CPCAUtils::SaveDatasetinUCIFormat(char *directory)
{
ClassifiedImageDataset* tBC;
int ii;
char DatasetDir[256];
sprintf(DatasetDir,"%s\\Objects\\",directory);
tBC=LoadClassifiedDataset(DatasetDir);
//Projections
DatasetProjections *tPDS=(DatasetProjections*)new unsigned char[sizeof(DatasetProjections)];
tPDS->TotalProjections=tBC->TotalImages;
tPDS->NumCoefs=PCAspace->NumCoefs;
tPDS->P=(float**)new unsigned char[sizeof(float*)*tPDS->TotalProjections];
int clase=1;
int acum=tBC->FramesPerClass[clase-1];
char cName[256];
#ifndef ENCARA2INLINUX
sprintf(cName,"%s\\UCI.txt",directory);
#else
sprintf(cName,"%s/UCI.txt",directory);
#endif
FILE *fUCI=fopen(cName,"w");
fprintf(fUCI,"%d\n%d\n",tPDS->TotalProjections,tPDS->NumCoefs);
for (ii=0;ii<(int)tPDS->TotalProjections;ii++)
{
tPDS->P[ii]=(float*)new unsigned char[sizeof(float)*tPDS->NumCoefs];
//Etiqueta del objetivo
//Assigns a number to each class, depending of the samples for each one
if (ii>=acum)
{
clase++;
acum+=tBC->FramesPerClass[clase-1];
}
//Projections
IplImage *I=cvCreateImage( PCAspace->ImageSize, IPL_DEPTH_32F, 1 );
if (tBC->BC[ii]->nChannels==I->nChannels)
cvConvertScale(tBC->BC[ii],I,1,0); // convierte de 8U a 32F
else
{
//Convierte primero a una imagen de grises
IplImage *Itemp=cvCreateImage( PCAspace->ImageSize, IPL_DEPTH_8U, 1 );
cvCvtColor( tBC->BC[ii], Itemp, CV_BGR2GRAY );
cvConvertScale(Itemp,I,1,0); // convierte de 8U a 32F
cvReleaseImage(&Itemp);
}
IplImage *itemp= cvCreateImage( PCAspace->ImageSize, IPL_DEPTH_32F, 1 );
// restar a I la imagen media...
//iplSubtract(I,PCA->avg, itemp);
cvSub(I,PCAspace->avg, itemp);
// multiplicar itemp por las autocaras...
for (unsigned int j=0;j<tPDS->NumCoefs;j++)
{
tPDS->P[ii][j]=(float)cvDotProduct(itemp,PCAspace->eigenObjects[j]);
fprintf(fUCI,"%f ",tPDS->P[ii][j]);//Escribe en fichero el atributo
}
fprintf(fUCI,"%d\n",clase);//Escribe en fichero la clase
cvReleaseImage(&itemp);
cvReleaseImage(&I);
}
fclose(fUCI);
//Escribir ahora a fichero
//Free projections
if (tPDS!=NULL)
{
for (unsigned int ii=0;ii<(int)tPDS->TotalProjections;ii++)
{
delete [] tPDS->P[ii];
}
delete [] tPDS->P;
delete [] tPDS;
tPDS=NULL;
}
delete [] tBC;
}
double LKInverseComp::iterate()
{
if (iterateImage==NULL)
return 0;
//cvWaitKey(-1);
double timer = (double)cvGetTickCount();
//printf("\n");
for (int i=0;i<(4);i++)
{
ptemp1[i]=param[i];
}
for (int i=4;i<(nS+4);i++)
{
ptemp2[i]=param[i];
}
CvMat * affShapeVec = computeShape(ptemp2);
newDelaunay->calculateAffineWarpParameters(affShapeVec);
affShapeVec = computeShape(ptemp1);
newDelaunay->calculateAffineWarpParametersComposeWithCurrent(affShapeVec);
for (int i=0;i<numberofpoints;i++)
{
double x,y;
x=newDelaunay->ithNodeCode[i].triangles[0].xi;
y=newDelaunay->ithNodeCode[i].triangles[0].yi;
int k=0;
double xx= x* newDelaunay->ithNodeCode[i].triangles[k].affine[0] + y* newDelaunay->ithNodeCode[i].triangles[k].affine[1] + newDelaunay->ithNodeCode[i].triangles[k].affine[2];
double yy= x* newDelaunay->ithNodeCode[i].triangles[k].affine[3] + y* newDelaunay->ithNodeCode[i].triangles[k].affine[4] + newDelaunay->ithNodeCode[i].triangles[k].affine[5];
CvScalar s1,s2;
s1.val[0]=xx;
s2.val[0]=yy;
cvSet2D(srcShape,0,2*i,s1);
cvSet2D(srcShape,0,2*i+1,s2);
}
// cvZero(ErrorImage);
totalerror=0;
bottomStepSize=0;
topStepSize=0;
for (int i=0;i<totalNumberOfPixels;i++)
{
int flag=1;
pixel * pix = newDelaunay->getpixel(i);
pixel *pix2 =newDelaunay->findCorrespondingPixelInImage(pix);
if (((double)pix2->x)<(double)iterateImage->width && ((double)pix2->y)<(double)iterateImage->height )
{
if ((pix2->y)>=0 && (pix2->x)>=0)
{
flag=0;
CvScalar val1,val2,val3;
val1.val[0]=(interpolate<uchar>(iterateImage,double(pix2->x),double(pix2->y)));
val2.val[0]=0;
if ((pix->y+pix->ty)>=0 && (pix->x+pix->tx)>=0)
{
val2.val[0]=interpolateMean[i];
val3.val[0] = val1.val[0]-val2.val[0];
bottomStepSize+=pow(val2.val[0],2);
topStepSize+=val2.val[0]*val1.val[0];
totalerror+=pow(val3.val[0],2);
ErrorImage->data.db[ (int(pix->y+pix->ty)*int(pix->width) + int(pix->x+pix->tx)) *ErrorImage->cols + 0 ] =val3.val[0];
// cvSet2D(ErrorImage, int(pix->y+pix->ty)*int(pix->width+10) + int(pix->x+pix->tx),0,val3);
}
}
}
}
// timer = (double)cvGetTickCount() - timer;
// printf( " Error Estimate = %gms\n", (timer)/((double)cvGetTickFrequency()*1000.) );
//
//
//printf("Error = %e \n",totalerror);
// timer= (double)cvGetTickCount();
static CvMat * parameter = cvCreateMat( nS+4,1, CV_64FC1);
cvMatMul(HessianMatrixInverse_steepestDescentImageTranspose,ErrorImage,parameter);
//
//
// timer = (double)cvGetTickCount() - timer;
// printf( " MULTIPLICATION = %gms\n", timer/((double)cvGetTickFrequency()*1000.) );
//printf("Rat %e\n",(bottomStepSize/topStepSize));
for (int i=0;i<(nS+4);i++)
{
CvScalar s = cvGet2D(parameter,i,0);
negNewParam[i] =-1*(bottomStepSize/topStepSize)*s.val[0];
}
for (int i=0;i<(4);i++)
{
negNewParam4[i]= negNewParam[i];
}
for (int i=4;i<(nS+4);i++)
{
negNewParamnS[i]= negNewParam[i];
}
// timer= (double)cvGetTickCount();
affShapeVec = computeShape(negNewParamnS); //Modified
newDelaunayInverse->calculateAffineWarpParameters(affShapeVec);
affShapeVec = computeShape(negNewParam4);
newDelaunayInverse->calculateAffineWarpParametersComposeWithCurrent(affShapeVec);
// timer = (double)cvGetTickCount() - timer;
// printf( " Compose = %gms\n", timer/((double)cvGetTickFrequency()*1000.) );
for (int i=0;i<numberofpoints;i++)
{
double x,y;
double finalx,finaly;
finalx=0;
finaly=0;
int ind = newDelaunayInverse->ithNodeCode[i].count;
x=newDelaunayInverse->ithNodeCode[i].triangles[0].xi;
y=newDelaunayInverse->ithNodeCode[i].triangles[0].yi;
for (int k=0;k<ind;k++)
{
double xx= x* newDelaunayInverse->ithNodeCode[i].triangles[k].affine[0] + y* newDelaunayInverse->ithNodeCode[i].triangles[k].affine[1] + newDelaunayInverse->ithNodeCode[i].triangles[k].affine[2];
double yy= x* newDelaunayInverse->ithNodeCode[i].triangles[k].affine[3] + y* newDelaunayInverse->ithNodeCode[i].triangles[k].affine[4] + newDelaunayInverse->ithNodeCode[i].triangles[k].affine[5];
finalx+= xx* newDelaunay->ithNodeCode[i].triangles[k].affine[0] + yy* newDelaunay->ithNodeCode[i].triangles[k].affine[1] + newDelaunay->ithNodeCode[i].triangles[k].affine[2];
finaly+= xx* newDelaunay->ithNodeCode[i].triangles[k].affine[3] + yy* newDelaunay->ithNodeCode[i].triangles[k].affine[4] + newDelaunay->ithNodeCode[i].triangles[k].affine[5];
}
finalx/=ind;
finaly/=ind;
CvScalar s1,s2;
s1.val[0]=finalx;
s2.val[0]=finaly;
cvSet2D(destShape,0,2*i,s1);
cvSet2D(destShape,0,2*i+1,s2);
}
cvAddWeighted(srcShape, 0.3, destShape, 0.7, 0, destShape);
cvSub(destShape,meanShape,destShapeTemp);
for (int i=0;i<4;i++)
{
double p=cvDotProduct(combineshapevectors[i],destShapeTemp);
negNewParam4[i]=-1*p;
Param4[i]=p;
}
for (int j=0;j<totalnumberofpoints;j++)
{
CvScalar s2 = cvGet2D(destShape,0,j);
double p=0;
for (int i=0;i<4;i++)
{
CvScalar s1 = cvGet2D(combineshapevectors[i],0,j);
p+= (negNewParam4[i] * s1.val[0]);
}
CvScalar t;
t.val[0] = s2.val[0] + p;
cvSet2D(destShapewithoutQ,0,j,t);
}
cvSub(destShapewithoutQ,meanShape,destShapeTemp);
for (int i=4;i<(nS+4);i++)
{
double p=cvDotProduct(combineshapevectors[i],destShapeTemp);
negNewParamnS[i]=-1*p;
ParamnS[i]=p;
}
for (int i=4;i<nS+4;i++)
{
param[i] =ParamnS[i];
}
for (int i=0;i<4;i++)
{
param[i] =Param4[i];
}
// printf(" -------------- \n");
for (int i=0;i<nS+4;i++)
{
if(i>3)
{
CvScalar s;
s= cvGet2D(eigenVal,0,i-4);
// printf("%e \n",s.val[0]);
if(param[i]>((.7)*sqrt(s.val[0])))
param[i]=((.7)*sqrt(s.val[0]));
if(param[i]<((-.7)*sqrt(s.val[0])))
param[i]=((-.7)*sqrt(s.val[0]));
}
// printf("param %e \n",param[i]);
}
timer = (double)cvGetTickCount() - timer;
printf( " Current Estimate = %gms\n", timer/((double)cvGetTickFrequency()*1000.) );
return totalerror;
}
static int icvL1QCNewton( CvAAtOpsData& AAtData, CvMat* B, CvMat* X, CvMat* U, double epsilon, double tau, CvTermCriteria nt_term_crit, CvTermCriteria cg_term_crit )
{
const double alpha = .01;
const double beta = .5;
CvMat* R = cvCreateMat( B->rows, B->cols, CV_MAT_TYPE(B->type) );
AAtData.AOps( X, AAtData.AR, AAtData.userdata );
cvSub( AAtData.AR, B, R );
CvMat* fu1 = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* fu2 = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* lfu1 = cvCreateMat( fu1->rows, fu1->cols, CV_MAT_TYPE(fu1->type) );
CvMat* lfu2 = cvCreateMat( fu2->rows, fu2->cols, CV_MAT_TYPE(fu2->type) );
cvSub( U, X, lfu1 );
cvAdd( X, U, lfu2 );
cvSubRS( lfu1, cvScalar(0), fu1 );
cvSubRS( lfu2, cvScalar(0), fu2 );
double epsilon2 = epsilon * epsilon;
double tau_inv = 1. / tau;
double fe = .5 * (cvDotProduct( R, R ) - epsilon2);
double fe_inv = 1. / fe;
cvLog( lfu1, lfu1 );
cvLog( lfu2, lfu2 );
CvScalar sumU = cvSum( U );
CvScalar sumfu1 = cvSum( lfu1 );
CvScalar sumfu2 = cvSum( lfu2 );
double f = sumU.val[0] - tau_inv * (sumfu1.val[0] + sumfu2.val[0] + log(-fe));
CvMat* atr = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* ntgx = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* ntgu = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* sig1211 = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* sigx = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* w1 = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* du = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* pX = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* pU = cvCreateMat( U->rows, U->cols, CV_MAT_TYPE(U->type) );
CvMat* pR = cvCreateMat( R->rows, R->cols, CV_MAT_TYPE(R->type) );
CvMat* pfu1 = cvCreateMat( fu1->rows, fu1->cols, CV_MAT_TYPE(fu1->type) );
CvMat* pfu2 = cvCreateMat( fu2->rows, fu2->cols, CV_MAT_TYPE(fu2->type) );
CvMat* Adx = cvCreateMat( B->rows, B->cols, CV_MAT_TYPE(B->type) );
CvMat* dx = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
CvMat* tX = cvCreateMat( X->rows, X->cols, CV_MAT_TYPE(X->type) );
int result = nt_term_crit.max_iter;
CvH11OpsData H11OpsData;
H11OpsData.AOps = AAtData.AOps;
H11OpsData.AtOps = AAtData.AtOps;
H11OpsData.AR = AAtData.AR;
H11OpsData.AtR = AAtData.AtR;
H11OpsData.userdata = AAtData.userdata;
H11OpsData.tX = tX;
H11OpsData.atr = atr;
H11OpsData.sigx = sigx;
int t, i;
for ( t = 0; t < nt_term_crit.max_iter; ++t )
{
AAtData.AtOps( R, atr, AAtData.userdata );
double* atrp = atr->data.db;
double* fu1p = fu1->data.db;
double* fu2p = fu2->data.db;
double* ntgxp = ntgx->data.db;
double* ntgup = ntgu->data.db;
double* sig1211p = sig1211->data.db;
double* sigxp = sigx->data.db;
double* w1p = w1->data.db;
double* dup = du->data.db;
for ( i = 0; i < X->rows; ++i, ++atrp, ++fu1p, ++fu2p, ++ntgxp, ++ntgup, ++sig1211p, ++sigxp, ++w1p, ++dup )
{
double fu1_inv = 1. / (*fu1p);
double fu2_inv = 1. / (*fu2p);
double ntgxv = fu1_inv - fu2_inv + fe_inv * (*atrp);
double ntguv = -tau - fu1_inv - fu2_inv;
double sig11 = fu1_inv * fu1_inv + fu2_inv * fu2_inv;
double sig12 = -fu1_inv * fu1_inv + fu2_inv * fu2_inv;
*sig1211p = sig12 / sig11;
*sigxp = sig11 - sig12 * (*sig1211p);
*w1p = ntgxv - (*sig1211p) * ntguv;
*ntgxp = -tau_inv * ntgxv;
*ntgup = -tau_inv * ntguv;
*dup = ntguv / sig11;
}
H11OpsData.fe_inv = fe_inv;
H11OpsData.fe_inv_2 = fe_inv * fe_inv;
if ( cvCGSolve( icvH11Ops, &H11OpsData, w1, dx, cg_term_crit ) > .5 )
{
result = t;
goto __clean_up__;
}
AAtData.AOps( dx, Adx, AAtData.userdata );
dup = du->data.db;
sig1211p = sig1211->data.db;
double* dxp = dx->data.db;
for ( i = 0; i < X->rows; ++i, ++dup, ++sig1211p, ++dxp )
*dup -= (*sig1211p) * (*dxp);
/* minimum step size that stays in the interior */
double aqe = cvDotProduct( Adx, Adx );
double bqe = 2. * cvDotProduct( R, Adx );
double cqe = cvDotProduct( R, R ) - epsilon2;
double smax = MIN( 1, -bqe + sqrt( bqe * bqe - 4 * aqe * cqe ) / (2 * aqe) );
dup = du->data.db;
dxp = dx->data.db;
fu1p = fu1->data.db;
fu2p = fu2->data.db;
for ( i = 0; i < X->rows; ++i, ++dup, ++dxp, ++fu1p, ++fu2p )
{
if ( (*dxp) - (*dup) > 0 )
smax = MIN( smax, -(*fu1p) / ((*dxp) - (*dup)) );
if ( (*dxp) + (*dup) < 0 )
smax = MIN( smax, (*fu2p) / ((*dxp) + (*dup)) );
}
smax *= .99;
/* backtracking line search */
bool suffdec = 0;
int backiter = 0;
double fep = fe;
double fp = f;
double lambda2;
while (!suffdec)
{
cvAddWeighted( X, 1, dx, smax, 0, pX );
cvAddWeighted( U, 1, du, smax, 0, pU );
cvAddWeighted( R, 1, Adx, smax, 0, pR );
cvSub( pU, pX, lfu1 );
cvAdd( pX, pU, lfu2 );
cvSubRS( lfu1, cvScalar(0), pfu1 );
cvSubRS( lfu2, cvScalar(0), pfu2 );
fep = .5 * (cvDotProduct( pR, pR ) - epsilon2);
cvLog( lfu1, lfu1 );
cvLog( lfu2, lfu2 );
CvScalar sumpU = cvSum( pU );
CvScalar sumpfu1 = cvSum( pfu1 );
CvScalar sumpfu2 = cvSum( pfu2 );
fp = sumpU.val[0] - tau_inv * (sumpfu1.val[0] + sumpfu2.val[0] + log(-fep));
lambda2 = cvDotProduct( ntgx, dx ) + cvDotProduct( ntgu, du );
double flin = f + alpha * smax * lambda2;
suffdec = (fp <= flin);
smax = beta * smax;
++backiter;
if ( backiter > 32 )
{
result = t;
goto __clean_up__;
}
}
/* set up for next iteration */
cvCopy( pX, X );
cvCopy( pU, U );
cvCopy( pR, R );
cvCopy( pfu1, fu1 );
cvCopy( pfu2, fu2 );
fe = fep;
fe_inv = 1. / fe;
f = fp;
lambda2 = -lambda2 * .5;
if ( lambda2 < nt_term_crit.epsilon )
{
result = t + 1;
break;
}
}
__clean_up__:
cvReleaseMat( &pfu2 );
cvReleaseMat( &pfu1 );
cvReleaseMat( &pR );
cvReleaseMat( &pU );
cvReleaseMat( &pX );
cvReleaseMat( &tX );
cvReleaseMat( &dx );
cvReleaseMat( &Adx );
cvReleaseMat( &du );
cvReleaseMat( &w1 );
cvReleaseMat( &sigx );
cvReleaseMat( &sig1211 );
cvReleaseMat( &ntgu );
cvReleaseMat( &ntgx );
cvReleaseMat( &lfu2 );
cvReleaseMat( &lfu1 );
cvReleaseMat( &fu2 );
cvReleaseMat( &fu1 );
cvReleaseMat( &R );
return result;
}
ImageRAII match( IplImage * image1, IplImage * image2, std::pair< CvMat *, CvMat * > image1_keys, std::pair< CvMat *, CvMat * > image2_keys )
{
ImageRAII appended_images = appendimages( image1, image2 );
ImageRAII rgb_appended_images( cvCreateImage( cvGetSize( appended_images.image ), appended_images.image->depth, 3 ) );
cvCvtColor( appended_images.image, rgb_appended_images.image, CV_GRAY2RGB );
CvScalar red;
red.val[2] = 255;
std::vector< std::pair< int, int > > points;
// check for matches with the vectors in image1 and image2
for( int i = 0; i < image1_keys.first->height; i++ )
{
double magnitude1 = 0;
MatrixRAII current_vector( cvCreateMat( 1, image1_keys.first->cols, CV_32FC1 ) );
// keeps track of minimum row found b/t image1 and image2 vectors
MatrixRAII min( cvCreateMat( 1, image2_keys.first->cols, CV_32FC1 ) );
cvGetRow( image1_keys.first, current_vector.matrix, i );
CvPoint point1 = cvPoint( ( int )cvmGet( current_vector.matrix, 0, 1 ), ( int )cvmGet( current_vector.matrix, 0, 0 ) );
std::map< float, int > angles;
for( int k = 0; k < image1_keys.second->width; k++ )
magnitude1 += pow( cvmGet( image1_keys.second, i, k ), 2 );
magnitude1 = cvSqrt( magnitude1 );
// compare a vector in image1 to every vector in image2 by calculating the cosine simularity
for( int j = 0; j < image2_keys.first->height; j++ )
{
MatrixRAII descriptor1( cvCreateMat( 1, image1_keys.second->cols, CV_32FC1 ) );
MatrixRAII descriptor2( cvCreateMat( 1, image2_keys.second->cols, CV_32FC1 ) );
cvGetRow( image1_keys.second, descriptor1.matrix, i );
cvGetRow( image2_keys.second, descriptor2.matrix, j );
double dot_product = cvDotProduct( descriptor1.matrix, descriptor2.matrix );
double magnitude2 = 0;
for( int k = 0; k < image2_keys.second->width; k++ )
magnitude2 += pow( cvmGet( descriptor1.matrix, 0, k ), 2 );
magnitude2 = cvSqrt( magnitude2 );
angles.insert( std::pair< float, int >( acos( dot_product / ( magnitude1 * magnitude2 ) ), j ) );
}
std::map< float, int >::iterator it = angles.begin();
int index = it->second;
float angle = it->first;
it++;
if( angle < THRESHOLD * it->first )
{
points.push_back( std::make_pair( i, index ) );
}
}
std::vector< std::pair< int, int > >::iterator it;
for( it = points.begin(); it < points.end(); it++ )
{
CvPoint point1 = cvPoint( ( int )cvmGet( image1_keys.first, it->first, 1 ), ( int )cvmGet( image1_keys.first, it->first, 0 ) );
CvPoint point2 = cvPoint( ( int )cvmGet( image2_keys.first, it->second, 1 ) + image1->width, ( int )cvmGet( image2_keys.first, it->second, 0 ) );
cvLine( rgb_appended_images.image, point1, point2, red );
}
return rgb_appended_images;
}
