static int getDeltaS( float H[8], float dU[], float J_U_H[][8], int n )
{
ARMatf matH, matU, matJ;
ARMatf *matJt, *matJtJ, *matJtU;
int ret = 0;
matH.row = 8;
matH.clm = 1;
matH.m = H;
matU.row = n;
matU.clm = 1;
matU.m = dU;
matJ.row = n;
matJ.clm = 8;
matJ.m = &J_U_H[0][0];
matJt = arMatrixAllocTransf( &matJ );
if( matJt == NULL ) {
ret = -1;
goto bail;
}
matJtJ = arMatrixAllocMulf( matJt, &matJ );
if( matJtJ == NULL ) {
ret = -1;
goto bail1;
}
matJtU = arMatrixAllocMulf( matJt, &matU );
if( matJtU == NULL ) {
ret = -1;
goto bail2;
}
if( arMatrixSelfInvf(matJtJ) < 0 ) {
ret = -1;
goto bail3;
}
arMatrixMulf( &matH, matJtJ, matJtU );
bail3:
arMatrixFreef( matJtU );
bail2:
arMatrixFreef( matJtJ );
bail1:
arMatrixFreef( matJt );
bail:
return (ret);
}
static int ComputeHomography(Point2f* pt1, Point2f* pt2, float h[3][3])
{
ARMatf A, B, C;
float m1[8][8], m2[8], *p1, *p2;
A.clm = 8;
A.row = 8;
A.m = p1 = &m1[0][0];
B.clm = 1;
B.row = 8;
B.m = p2 = &m2[0];
C.clm = 1;
C.row = 8;
C.m = &h[0][0];
for(int i = 0; i < 4; i++ ) {
*(p1++) = pt2[i].x;
*(p1++) = pt2[i].y;
*(p1++) = 1.0F;
*(p1++) = 0.0F;
*(p1++) = 0.0F;
*(p1++) = 0.0F;
*(p1++) = -pt2[i].x * pt1[i].x;
*(p1++) = -pt2[i].y * pt1[i].x;
*(p1++) = 0.0F;
*(p1++) = 0.0F;
*(p1++) = 0.0F;
*(p1++) = pt2[i].x;
*(p1++) = pt2[i].y;
*(p1++) = 1.0F;
*(p1++) = -pt2[i].x * pt1[i].y;
*(p1++) = -pt2[i].y * pt1[i].y;
*(p2++) = pt1[i].x;
*(p2++) = pt1[i].y;
}
if( arMatrixSelfInvf(&A) < 0 ) return -1;
if( arMatrixMulf(&C, &A, &B) < 0 ) return -1;
h[2][2] = 1.0F;
return 0;
}