int multiplyTwo3x3Matrices(double * result , double * matrixA , double * matrixB)
{
if ( (matrixA==0) || (matrixB==0) || (result==0) ) { return 0; }
#if PRINT_MATRIX_DEBUGGING
fprintf(stderr,"Multiplying 3x3 A and B \n");
print3x3DMatrix("A", matrixA);
print3x3DMatrix("B", matrixB);
#endif
//MULTIPLICATION_RESULT FIRST ROW
// 0 1 2
// 3 4 5
// 6 7 8
result[0]=matrixA[0] * matrixB[0] + matrixA[1] * matrixB[3] + matrixA[2] * matrixB[6];
result[1]=matrixA[0] * matrixB[1] + matrixA[1] * matrixB[4] + matrixA[2] * matrixB[7];
result[2]=matrixA[0] * matrixB[2] + matrixA[1] * matrixB[5] + matrixA[2] * matrixB[8];
result[3]=matrixA[3] * matrixB[0] + matrixA[4] * matrixB[3] + matrixA[5] * matrixB[6];
result[4]=matrixA[3] * matrixB[1] + matrixA[4] * matrixB[4] + matrixA[5] * matrixB[7];
result[5]=matrixA[3] * matrixB[2] + matrixA[4] * matrixB[5] + matrixA[5] * matrixB[8];
result[6]=matrixA[6] * matrixB[0] + matrixA[7] * matrixB[3] + matrixA[8] * matrixB[6];
result[7]=matrixA[6] * matrixB[1] + matrixA[7] * matrixB[4] + matrixA[8] * matrixB[7];
result[8]=matrixA[6] * matrixB[2] + matrixA[7] * matrixB[5] + matrixA[8] * matrixB[8];
print3x3DMatrix("AxB", result);
return 1;
}
int invert3x3MatrixD(double * mat,double * result)
{
double * a = mat;
double * b = result;
double detA = det3x3Matrix(mat);
if (detA==0.0)
{
copy3x3Matrix(result,mat);
fprintf(stderr,"Matrix 3x3 cannot be inverted (det = 0)\n");
return 0;
}
double one_div_detA = (double) 1 / detA;
//FIRST LINE
b[I11] = a[I22] * a[I33] - a[I23]*a[I32] ;
b[I11] *= one_div_detA;
b[I12] = a[I13] * a[I32] - a[I12]*a[I33] ;
b[I12] *= one_div_detA;
b[I13] = a[I12] * a[I23] - a[I13]*a[I22] ;
b[I13] *= one_div_detA;
//SECOND LINE
b[I21] = a[I23] * a[I31] - a[I21]*a[I33] ;
b[I21] *= one_div_detA;
b[I22] = a[I11] * a[I33] - a[I13]*a[I31] ;
b[I22] *= one_div_detA;
b[I23] = a[I13] * a[I21] - a[I11]*a[I23] ;
b[I23] *= one_div_detA;
//THIRD LINE
b[I31] = a[I21] * a[I32] - a[I22]*a[I31] ;
b[I31] *= one_div_detA;
b[I32] = a[I12] * a[I31] - a[I11]*a[I32] ;
b[I32] *= one_div_detA;
b[I33] = a[I11] * a[I22] - a[I12]*a[I21] ;
b[I33] *= one_div_detA;
print3x3DMatrix("Inverted Matrix From Source",a);
print3x3DMatrix("Inverted Matrix To Target",b);
return 1;
}
int convertRodriguezTo3x3(double * result,double * matrix)
{
if ( (matrix==0) || (result==0) ) { return 0; }
double x = matrix[0] , y = matrix[1] , z = matrix[2];
double th = sqrt( x*x + y*y + z*z );
double cosTh = cos(th);
x = x / th; y = y / th; z = z / th;
if ( th < 0.00001 )
{
result[0]=1.0f; result[1]=0.0f; result[2]=0.0f;
result[3]=0.0f; result[4]=1.0f; result[5]=0.0f;
result[6]=0.0f; result[7]=0.0f; result[8]=1.0f;
//create3x3IdentityMatrix(result);
return 1;
}
//NORMAL RESULT
result[0]=x*x * (1 - cosTh) + cosTh; result[1]=x*y*(1 - cosTh) - z*sin(th); result[2]=x*z*(1 - cosTh) + y*sin(th);
result[3]=x*y*(1 - cosTh) + z*sin(th); result[4]=y*y*(1 - cosTh) + cosTh; result[5]=y*z*(1 - cosTh) - x*sin(th);
result[6]=x*z*(1 - cosTh) - y*sin(th); result[7]=y*z*(1 - cosTh) + x*sin(th); result[8]=z*z*(1 - cosTh) + cosTh;
#if PRINT_MATRIX_DEBUGGING
fprintf(stderr,"rodriguez %f %f %f\n ",matrix[0],matrix[1],matrix[2]);
print3x3DMatrix("Rodriguez Initial", result);
#endif // PRINT_MATRIX_DEBUGGING
return 1;
}
int calculateFundamentalMatrix8PointMultipleView(double * result3x3Matrix , unsigned int pointsNum , double * pointsA, double * pointsB )
{
if (pointsNum<8) { fprintf(stderr,"calculateFundamentalMatrix8Point requires at least 8 points\n"); return 0; }
//http://en.wikipedia.org/wiki/Eight-point_algorithm#Step_1:_Formulating_a_homogeneous_linear_equation
//
// ( Xa ) ( Xb ) ( e11 e12 e13 )
// pointsA ( Ya ) pointsB ( Yb ) E ( e21 e22 e23 )
// ( 1 ) ( 1 ) ( e31 e32 e33 )
//
//So what we do is convert this to a matrix for solving a linear system of 8 equations
// Xb*Xa*e11 + Xb*Ya*e12 + Xb*e13 + yB*xA*e21 + yB*yA*e22 + yB*e23 + xA*e31 + yA*e32 + 1*e33 = 0
double * pxA , * pyA , * pxB , * pyB ;
int elements=10;
double * compiledPoints = (double * ) malloc(pointsNum * elements * sizeof(double));
if (compiledPoints==0) { return 0; }
unsigned int i=0;
for (i=0; i< pointsNum; i++)
{
//Shortcut our vars
pxA = &pointsA[i*2 + 0]; pyA = &pointsA[i*2 + 1];
pxB = &pointsB[i*2 + 0]; pyB = &pointsB[i*2 + 1];
fprintf(stderr,"Pair %u : Point A %f,%f Point B %f,%f \n",i,*pxA,*pyA,*pxB,*pyB);
//Make the precalculations for each of the elements
compiledPoints[i*elements + xBxA] = (double) (*pxB) * (*pxA);
compiledPoints[i*elements + xByA] = (double) (*pxB) * (*pyA);
compiledPoints[i*elements + xB] = (double) (*pxB) ;
compiledPoints[i*elements + yBxA] = (double) (*pyB) * (*pxA);
compiledPoints[i*elements + yByA] = (double) (*pyB) * (*pyA);
compiledPoints[i*elements + yB] = (double) (*pyB);
compiledPoints[i*elements + xA] = (double) (*pxA);
compiledPoints[i*elements + yA] = (double) (*pyA);
compiledPoints[i*elements + One] = 1.0;
compiledPoints[i*elements + Result] = 0.0;
}
fprintf(stderr,"\n\n");
printSystemPlain(compiledPoints,"Original",pointsNum);
//fprintf(stderr,"\n\n");
printSystemMathematica(compiledPoints, pointsNum);
//printSystemScilab(compiledPoints, pointsNum);
solveLinearSystemGJ(result3x3Matrix,compiledPoints,elements,pointsNum);
print3x3DMatrix("Result 3x3 Matrix",result3x3Matrix);
free(compiledPoints);
return 1;
}
void testGJSolver()
{
double * F3x3 = alloc3x3Matrix(); if (F3x3 ==0) { return; }
double * pointsA = (double *) malloc(sizeof(double) * 2 * 8);
memset(pointsA , 0 ,sizeof(double) * 2 * 8 );
double * pointsB = (double *) malloc(sizeof(double) * 2 * 8);
memset(pointsB , 0 ,sizeof(double) * 2 * 8 );
// SOURCE FRAME POINTS TARGET FRAME POINTS
//---------------------------------------------------------------------------------------------
unsigned int i=0;
pointsA[i*2+0]=34; pointsA[i*2+1]=379; /* | | */ pointsB[i*2+0]=33; pointsB[i*2+1]=358; /* | | */ ++i;
pointsA[i*2+0]=178; pointsA[i*2+1]=379; /* | | */ pointsB[i*2+0]=84; pointsB[i*2+1]=374; /* | | */ ++i;
pointsA[i*2+0]=320; pointsA[i*2+1]=379; /* | | */ pointsB[i*2+0]=139; pointsB[i*2+1]=392; /* | | */ ++i;
pointsA[i*2+0]=461; pointsA[i*2+1]=379; /* | | */ pointsB[i*2+0]=202; pointsB[i*2+1]=410; /* | | */ ++i;
pointsA[i*2+0]=605; pointsA[i*2+1]=379; /* | | */ pointsB[i*2+0]=271; pointsB[i*2+1]=432; /* | | */ ++i;
pointsA[i*2+0]=120; pointsA[i*2+1]=312; /* | | */ pointsB[i*2+0]=88; pointsB[i*2+1]=318; /* | | */ ++i;
pointsA[i*2+0]=219; pointsA[i*2+1]=312; /* | | */ pointsB[i*2+0]=134; pointsB[i*2+1]=329; /* | | */ ++i;
pointsA[i*2+0]=319; pointsA[i*2+1]=312; /* | | */ pointsB[i*2+0]=184; pointsB[i*2+1]=342; /* | | */ ++i;
//---------------------------------------------------------------------------------------------
calculateFundamentalMatrix8Point(F3x3 , i /*Number of points*/ , pointsA, pointsB );
print3x3DMatrix("Calculated matrix using 8 points", F3x3);
print3x3DScilabMatrix("M",F3x3);
/*
double err = testHomographyError(F3x3 , i , pointsA , pointsB);
printf("result homography brings error %0.2f \n",err);
F3x3[0] = 3.369783338522472; F3x3[1] = -1.637685275601417; F3x3[2] = 851.0036476001653;
F3x3[3] = -0.2783636300638685; F3x3[4] = 15.54534472903452; F3x3[5] = -2133.959529863233;
F3x3[6] = -0.003793213664419078; F3x3[7] = 0.02530490689886264; F3x3[8] = 1;
print3x3DMatrix("3x3 known good homography", F3x3);
err = testHomographyError(F3x3 , i , pointsA , pointsB);
printf("known good homography brings error %0.2f \n",err);
*/
free3x3Matrix(&F3x3);
free(pointsA);
free(pointsB);
return ;
}
