4point.cpp

用于系统跟踪的程序

#include "stdafx.h"
#include "4Point.h"


double norm(double r1,double r2,double r3)
{
	double n=0;
	n=sqrt(r1*r1+r2*r2+r3*r3);
	return n;
}

//A x B=(y1*z2-z1*y2,z1*x2-x1*z2,x1*y2-y1*x2);
void cross(double x1,double y1,double z1,double x2,double y2,double z2,double& x3,double& y3,double& z3)
{
	x3=y1*z2-z1*y2;
	y3=z1*x2-x1*z2;
	z3=x1*y2-y1*x2;
}

void test()
{

}

vnl_matrix<double> ComputePMatrixFrom4Points(vcl_vector<HomgPoint2D> PointsInImage,vcl_vector<HomgPoint2D> PointsInWorld,vnl_matrix<double> K,HMatrix2D & HMatrixFor2D)
 {

	HMatrix2DCompute4Point	 H4p;
	//HMatrix2D H2d;

	 

	H4p.compute(PointsInWorld,PointsInImage,&HMatrixFor2D);

	//vcl_cerr<<H2d<<vcl_endl;

	//pHMatrixFor2D=&H2d;

	//vcl_cerr<<"Test Point [0.5,0.5,1]";
	//vcl_cerr<<H2d.transform_to_plane2(HomgPoint2D(0.5,0.5,1))<<vcl_endl;


	//------------------------------------------------------
	//compute for K invse (Kinv) and P 3*3 ([r1,r2,t] P33)  and  P Matrix (4*4) P44

	vnl_matrix<double> P33;
	vnl_matrix<double> P34(3,4);
	vnl_matrix<double> P44(4,4);
	vnl_matrix<double> H;


	//Get H From H2d
	HMatrixFor2D.get(&H);

	//vcl_cerr<<"K Matrix ="<<vcl_endl;
	//vcl_cerr<<K<<vcl_endl;

	//vcl_cerr<<"H Matrix ="<<vcl_endl;
	//vcl_cerr<<H<<vcl_endl;

	//Comput P = Kinv * H
	P33=vnl_matrix_inverse<double>(K);
	P33=P33* H;
	
	
	//vcl_cerr<<"P Matrix ="<<vcl_endl;
	//vcl_cerr<<P33<<vcl_endl; 

	double norm1=norm(P33(0,0),P33(1,0),P33(2,0));
	//r1
	P34(0,0)=P33(0,0);
	P34(1,0)=P33(1,0);
	P34(2,0)=P33(2,0); 

	double norm2=norm(P33(0,1),P33(1,1),P33(2,1));
	//r2=r2/||r2||
	P34(0,1)=P33(0,1)/norm2;
	P34(1,1)=P33(1,1)/norm2;
	P34(2,1)=P33(2,1)/norm2; 

	
	//r3=r1**r2/||r1**r2||
	cross(P34(0,0),P34(1,0),P34(2,0),P34(0,1),P34(1,1),P34(2,1),P34(0,2),P34(1,2),P34(2,2));
	double norm3=norm(P34(0,2),P34(1,2),P34(2,2));
	P34(0,2)=P34(0,2)/norm3;
	P34(1,2)=P34(1,2)/norm3;
	P34(2,2)=P34(2,2)/norm3; 
	
	//P34(0,2)=P33(1,0)*P33(2,1)-P33(2,0)*P33(1,1);
	//P34(1,2)=P33(2,0)*P33(0,1)-P33(0,0)*P33(2,1);
	//P34(2,2)=P33(0,0)*P33(1,1)-P33(1,0)*P33(0,1); 

	//r1=r2**r3
	cross(P34(0,1),P34(1,1),P34(2,1),P34(0,2),P34(1,2),P34(2,2),P34(0,0),P34(1,0),P34(2,0));

	//t=t*(1/s)
	double s=(norm1+norm2+norm3)/3;
	P34(0,3)=P33(0,2)*(1/s);
	P34(1,3)=P33(1,2)*(1/s);
	P34(2,3)=P33(2,2)*(1/s); 

	//vcl_cerr<<"P (3*4) Matrix ="<<vcl_endl;
	//vcl_cerr<<P34<<vcl_endl;

	//vcl_cerr<<"K * P   Matrix ="<<vcl_endl;
	//vcl_cerr<<K * P34<<vcl_endl;



	vnl_matrix<double> Ptmp=K*P34;

	P44(0,0)=Ptmp(0,0);
	P44(0,1)=Ptmp(0,1);
	P44(0,2)=Ptmp(0,2);
	P44(0,3)=Ptmp(0,3);

	P44(1,0)=Ptmp(1,0);
	P44(1,1)=Ptmp(1,1);
	P44(1,2)=Ptmp(1,2);
	P44(1,3)=Ptmp(1,3);

	
	P44(2,0)=Ptmp(2,0);
	P44(2,1)=Ptmp(2,1);
	P44(2,2)=Ptmp(2,2);
	P44(2,3)=Ptmp(2,3);


	P44(3,0)=0;
	P44(3,1)=0;
	P44(3,2)=0;
	P44(3,3)=1;

	//vcl_cerr<<"P 4*4   Matrix ="<<vcl_endl;
	//vcl_cerr<<P44<<vcl_endl;

    
	
	return P34;

	 
}