opencv转换录像视频3D到2D！！！

void IPMPoint3DTO2D(const Point3D& p3D, Point2D& p2D,CameraInfo cam)
{
 double AX=cam.ax;
 double AY=cam.ay;
 double AxisX=cam.u0;
 double AxisY=cam.v0;
 double Hight=cam.Height;

 CvMat *CoorW=cvCreateMat(3,1,CV_32FC1);
 double CW[3]={
  p3D.X,p3D.Y,p3D.Z
 };
 cvSetData(CoorW,CW,CoorW->step);

 CvMat *CoorC=cvCreateMat(3,1,CV_32FC1);
 cvmMul(cam.R,CoorW,CoorC);
 
 p2D.X=AxisX+cvmGet(CoorC,0,0)/cvmGet(CoorC,2,0)*AX;
 p2D.Y=AxisY+cvmGet(CoorC,1,0)/cvmGet(CoorC,2,0)*AY;

}

void IPMPoint2DTO3D(const Point2D& p2D, Point3D& p3D,CameraInfo cam)
{
 double AX=cam.ax;
 double AY=cam.ay;
 double AxisX=cam.u0;
 double AxisY=cam.v0;
 double Hight=cam.Height;

 CvMat *CoorC=cvCreateMat(3,1,CV_32FC1);
 double CC[3]={
  (p2D.X-AxisX)/AX,
  (p2D.Y-AxisY)/AY,
  1
 };
 cvSetData(CoorC,CC,CoorC->step);
 CvMat *CoorW=cvCreateMat(3,1,CV_32FC1);
 CvMat *TR=cvCreateMat(cam.R->cols,cam.R->rows,CV_32FC1);
 cvTranspose(cam.R, TR );
 cvmMul(TR,CoorC,CoorW);
 
 p3D.X=cvmGet(CoorW,0,0)/cvmGet(CoorW,1,0)*Hight;
 p3D.Z=cvmGet(CoorW,2,0)/cvmGet(CoorW,1,0)*Hight;
 p3D.Y=Hight;
}