Exemplo n.º 1
0
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;
}
Exemplo n.º 3
0
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);
}