rt.cpp

用于系统跟踪的程序

#include "stdafx.h"
#include "RT.h"
#include <stdio.h>  
#include "resource.h"   
#include "math.h"
#include "windows.h"
#include "process.h"

 

 

//根据RT矩阵计算三维空间中一个点在图像上的投影
void PerspectiveProjection (CvMat rotation,CvMat translation,float flFocalLength,CvPoint3D32f Obj_point,CvPoint2D32f &img_point)
{
	//CvMat translation = cvMat( 3, 1, CV_MAT32F, NULL );
	//CvMat true_rotation = cvMat( 3, 3, CV_MAT32F, NULL );
	//CvPoint3D32f obj_points;
	//CvPoint2D32f img_points;
	//float flFocalLength;

	float vec[3];
    CvMat Vec = cvMat( 3, 1, CV_MAT32F, vec );
    CvMat Obj_pointMat = cvMat( 3, 1, CV_MAT32F, &Obj_point.x );
	cvmMul( &rotation, &Obj_pointMat, &Vec );

    vec[0] += translation.data.fl[0];
    vec[1] += translation.data.fl[1];
    vec[2] += translation.data.fl[2];

    img_point.x = flFocalLength * vec[0] / vec[2];
    img_point.y = flFocalLength * vec[1] / vec[2];

}


void ComputeRTbyPOSIT(CvPoint2D32f* img_points,CvPoint3D32f* obj_points,int PointCount, float flFocalLength,CvMat rotation,CvMat translation)
{

	
	//CvPoint3D32f* obj_points;
	//CvPoint2D32f* img_points;
	//float flFocalLength;
	//CvMat rotation = cvMat( 3, 3, CV_MAT32F, NULL );
	//cvmAlloc( &rotation );
	//CvMat translation = cvMat( 3, 1, CV_MAT32F, NULL );
	//cvmAlloc( &translation );

	
	CvPOSITObject* object;
    CvTermCriteria criteria;
    /* Initilization */
	float flEpsilon=0.1f;
    criteria.type = CV_TERMCRIT_EPS|CV_TERMCRIT_ITER;
    criteria.epsilon = flEpsilon;
    criteria.max_iter = 10000;

	//obj_points = (CvPoint3D32f*)cvAlloc( 6 * sizeof(CvPoint3D32f) );


	//obj_points[0].x = 0;  obj_points[0].y = 0;  obj_points[0].z = 10;
 //   obj_points[1].x = 0; obj_points[1].y = 0;  obj_points[1].z = 40;
 //   obj_points[2].x = -10; obj_points[2].y = 10; obj_points[2].z = 0;
 //   obj_points[3].x = 10;  obj_points[3].y = 10; obj_points[3].z = 0;
 //   obj_points[4].x = 10;  obj_points[4].y = -10;  obj_points[4].z = 0;
 //   obj_points[5].x = -10; obj_points[5].y = -10;  obj_points[5].z = 0;

	object = cvCreatePOSITObject( obj_points, PointCount);
    cvPOSIT( object, img_points, flFocalLength, criteria,rotation.data.fl, translation.data.fl );
	cvReleasePOSITObject( &object );

 

	//cvFree( (void**)&obj_points );

}