MATRIX* showMotion (MATRIX AffineMatrix, FILE *output, FILE *logFile)
{
MATRIX_CONTENT_TYPE** AffineMatrixData = AffineMatrix.getDataPtr();
double rawCosX = (AffineMatrixData[0][0]+AffineMatrixData[1][1]);
double rawSinX = (AffineMatrixData[1][0]-AffineMatrixData[0][1]);
double Normalizer = sqrt ( pow (rawCosX,2) + pow (rawSinX,2) );
double cosX = rawCosX / Normalizer;
double sinX = rawSinX / Normalizer;
double theta = atan2 ( sinX, cosX );//acos (cosX) //asin (sinX),
//fprintf (logFile,"\t\tEstimated: %6.3lf\n\n", theta);
fprintf (logFile,"\t\tEstimated: %6.3lf Translation: ( x: %8.3lf , y: %8.3lf )\n\n", theta, AffineMatrixData[2][0], AffineMatrixData[2][1]);
if (output) {
fwrite (&theta, sizeof (double), 1, output);
fflush (output);
}
MATRIX *roTrans = new MATRIX (3,2);
MATRIX_CONTENT_TYPE **rotData = roTrans->getDataPtr ();
rotData[0][0] = cosX; rotData[1][0] = -sinX; rotData[2][0] = AffineMatrixData[2][0];
rotData[0][1] = sinX; rotData[1][1] = cosX; rotData[2][1] = AffineMatrixData[2][1];
return roTrans;
}
MATRIX fitAffineMatrix (COORDS initialPosition, COORDS finalPosition ) {
AMATRIX initialAMatrix = AMATRIX (3,initialPosition.Number_of_Coordinates);
MATRIX initialMatrix = initialAMatrix;
MATRIX_CONTENT_TYPE **initialData = initialMatrix.getDataPtr ();
for (int row = 0; row < initialPosition.Number_of_Coordinates; row++) {
initialData[0][row] = initialPosition.x[row];
initialData[1][row] = initialPosition.y[row];
initialData[2][row] = 1;
}
MATRIX TransposedInitMat = transpose (initialMatrix);
MATRIX finalX = MATRIX (finalPosition.x, finalPosition.Number_of_Coordinates);
MATRIX finalY = MATRIX (finalPosition.y, finalPosition.Number_of_Coordinates);
MATRIX ProjectionX = TransposedInitMat * finalX;
MATRIX ProjectionY = TransposedInitMat * finalY;
AMATRIX ProjectionMatrix = AMATRIX (3,3);
MATRIX tmpMatrix = TransposedInitMat * initialMatrix;
MATRIX_CONTENT_TYPE **ProjectMatrixData = ProjectionMatrix.getDataPtr ();
MATRIX_CONTENT_TYPE **tmpMatrixData = tmpMatrix.getDataPtr ();
for (int row = 0; row < 3; row++)
for (int col = 0; col < 3; col++) {
ProjectMatrixData[col][row] = tmpMatrixData[col][row];
}
double *affineX = ProjectionMatrix.invert (ProjectionX.getDataPtr()[0]);
double *affineY = ProjectionMatrix.invert (ProjectionY.getDataPtr()[0]);
MATRIX AffineMatrix = MATRIX (3,2);
MATRIX_CONTENT_TYPE **AffineMatrixData = AffineMatrix.getDataPtr ();
for (int col = 0; col < 3; col++) {
AffineMatrixData[col][0] = affineX[col];
AffineMatrixData[col][1] = affineY[col];
}
return AffineMatrix;
}
void showMotion (MATRIX AffineMatrix) {
MATRIX_CONTENT_TYPE** AffineMatrixData = AffineMatrix.getDataPtr();
double rawCosX = (AffineMatrixData[0][0]+AffineMatrixData[1][1]);
double rawSinX = (AffineMatrixData[1][0]-AffineMatrixData[0][1]);
double Normalizer = sqrt ( pow (rawCosX,2) + pow (rawSinX,2) );
double cosX = rawCosX / Normalizer;
double sinX = rawSinX / Normalizer;
cout << "Rotation: " << acos (cosX) << " = " << asin (sinX) << " " ;
cout << "Translation: " << "( x: " << AffineMatrixData[2][0] << " , y: " << AffineMatrixData[2][1] << " )" << endl;
}
void testMotionExtract (void) {
MATRIX AffineMatrix (3,2);
MATRIX_CONTENT_TYPE** AffineData = AffineMatrix.getDataPtr ();
double theta = 3.14 / 6;
cout << "theta: " << theta << endl;
// row 1
AffineData[0][0] = cos (theta);
AffineData[1][0] = sin (theta);
AffineData[2][0] = 1;
// row 2
AffineData[0][1] = - sin (theta);
AffineData[1][1] = cos (theta);
AffineData[2][1] = 2;
showMotion (AffineMatrix);
}