CvMat * LKInverseComp::currentEstimate()
{
static float* ptemp1 = (float *)malloc(sizeof(float)*(nS+4));
static float* ptemp2 = (float *)malloc(sizeof(float)*(nS+4));
for (int i=0;i<(nS+4);i++)
{
ptemp1[i]=0;
ptemp2[i]=0;
}
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);
newDelaunayShapeCompute->calculateAffineWarpParameters(affShapeVec);
affShapeVec = computeShape(ptemp1);
newDelaunayShapeCompute->calculateAffineWarpParametersComposeWithCurrent(affShapeVec);
CvMat * mat = cvCreateMat(1,totalnumberofpoints,CV_64FC1);
for (int i=0;i<numberofpoints;i++)
{
double x,y;
double finalx,finaly;
finalx=0;
finaly=0;
int t=newDelaunayShapeCompute->ithNodeCode[i].count;
if (t>0)
{
int xx=newDelaunayShapeCompute->ithNodeCode[i].triangles[0].xi;
int yy=newDelaunayShapeCompute->ithNodeCode[i].triangles[0].yi;
x = xx* newDelaunayShapeCompute->ithNodeCode[i].triangles[0].affine[0] + yy* newDelaunayShapeCompute->ithNodeCode[i].triangles[0].affine[1] + newDelaunayShapeCompute->ithNodeCode[i].triangles[0].affine[2];
y = xx* newDelaunayShapeCompute->ithNodeCode[i].triangles[0].affine[3] + yy* newDelaunayShapeCompute->ithNodeCode[i].triangles[0].affine[4] + newDelaunayShapeCompute->ithNodeCode[i].triangles[0].affine[5];
CvScalar s1,s2;
s1.val[0]=x;
s2.val[0]=y;
cvSet2D(mat,0,2*i,s1);
cvSet2D(mat,0,2*i+1,s2);
}
}
return mat;
}
InventorShape::InventorShape(Modeling::Shape::SharedPtr aShape) : shape(aShape) {
group.reset(new SoGroup());
separator.reset(new SoSeparator());
group->addChild(separator.get());
computeShape();
}
void Ellipse::computeEllipseFromRegion(Region* const _region, const double _scale)
{
m_dData1 = _region->getMeanData1();
m_dData2 = _region->getMeanData2();
m_dData3 = _region->getMeanData3();
// compute inertia matrix
double m00 = computeMoment(_region, 0, 0, _scale);
double m01 = computeMoment(_region, 0, 1, _scale);
double m10 = computeMoment(_region, 1, 0, _scale);
double m11 = computeMoment(_region, 1, 1, _scale);
double m20 = computeMoment(_region, 2, 0, _scale);
double m02 = computeMoment(_region, 0, 2, _scale);
m_dPosX = m10 / m00;
m_dPosY = m01 / m00;
m_dIM00 = m20 / m00 - m_dPosX * m_dPosX;
m_dIM01 = m11 / m00 - m_dPosX * m_dPosY;
m_dIM10 = m_dIM01;
m_dIM11 = m02 / m00 - m_dPosY * m_dPosY;
// compute ellipse matrix and its properties
double temp1 = m_dIM00 + m_dIM11;
double temp2 = sqrt(pow(m_dIM00 - m_dIM11, 2) + 4 * pow(m_dIM01, 2));
double temp3 = 0.0;
m_dEValue1 = (temp1 + temp2) / 2.0; // eigenvalues
m_dEValue2 = (temp1 - temp2) / 2.0;
temp1 = m_dIM11 + m_dIM01 - m_dEValue1; // eigenvector 1
temp2 = m_dEValue1 - m_dIM00 - m_dIM01;
temp3 = sqrt(temp1 * temp1 + temp2 * temp2);
m_dEVector11 = temp1 / temp3;
m_dEVector12 = temp2 / temp3;
temp1 = m_dIM11 + m_dIM01 - m_dEValue2; // eigenvector 2
temp2 = m_dEValue2 - m_dIM00 - m_dIM01;
temp3 = sqrt(temp1 * temp1 + temp2 * temp2);
m_dEVector21 = temp1 / temp3;
m_dEVector22 = temp2 / temp3;
m_dLengthPA1 = 2 * sqrt(m_dEValue1); // lenght of principal axis 1
m_dLengthPA2 = 2 * sqrt(m_dEValue2); // s.a. 2
m_dAnglePA1 = - atan(m_dEVector12 / m_dEVector11); // angle of pricipla axis 1 (circular measure)
m_dAnglePA2 = - atan(m_dEVector22 / m_dEVector21);
const double pi = 3.141592653589;
m_dArea = 4.0 * pi * sqrt(m_dIM00 * m_dIM11 - m_dIM01 * m_dIM01);
m_dShape[0] = min(m_dLengthPA1, m_dLengthPA2) / max(m_dLengthPA1, m_dLengthPA2); // shape, near 1 = more circle like, near 0 = more line like
m_dEM00 = 0.25 * (m_dEVector11 * m_dEVector11 / m_dEValue1 + m_dEVector21 * m_dEVector21 / m_dEValue2); // ellipse matrix
m_dEM01 = 0.25 * (m_dEVector12 * m_dEVector11 / m_dEValue1 + m_dEVector21 * m_dEVector22 / m_dEValue2);
m_dEM10 = m_dEM01;
m_dEM11 = 0.25 * (m_dEVector12 * m_dEVector12 / m_dEValue1 + m_dEVector22 * m_dEVector22 / m_dEValue2);
computeShape(_region, _scale);
}
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;
}
