非Opencv函數把3D座標轉成2D座標

原創

2020-02-22 09:40

#include "stdafx.h"
#include <cv.h>
#include <highgui.h>

class Point2D
{
public:
double X,Y;
};
class Point3D
{
public:
double X,Y,Z;
};
class CameraInfo
{
public:
CameraInfo():ax(1),ay(1),u0(1),v0(1),f(0),Alph(0),Beta(0),Gama(0),Height(2){}
double ax,ay,u0,v0,f,Alph,Beta,Gama,Height;
double R[3][3],K[3][3];

};
void InitCameraInfo(CameraInfo &cam)
{
int i,j,k;
int num=0;
double number=0;
double R_x[3][3] = {
  {1,0,0},
  {0,cos(cam.Alph),sin(cam.Alph)},
  {0,-sin(cam.Alph),cos(cam.Alph)}
};
double R_y[3][3] = {
  {cos(cam.Beta),0,-sin(cam.Alph)},
  {0,1,0},
  {sin(cam.Beta),0,cos(cam.Beta)}
};
double R_z[3][3] = {
  {cos(cam.Gama),sin(cam.Gama),0},
  {-sin(cam.Gama),cos(cam.Gama),0},
  {0,0,1}
};
double temp[9] = {
  cam.ax,0,cam.u0,
  0,cam.ay,cam.v0,
  0,0,1
};
for(i=0;i<3;i++)
  for(j=0;j<3;j++)
   cam.K[i][j]=temp[num++];
for(i=0;i<3;i++)
{
  for(j=0;j<3;j++)
  {
   cam.R[i][j]=0;
   for(k=0;k<3;k++)
    cam.R[i][j]+=R_x[i][k]*R_y[k][j];
   //printf("%lf ",cam.R[i][j]);
  }
  //printf("/n");
}
for(i=0;i<3;i++)
{
  for(j=0;j<3;j++)
  {
   for(k=0;k<3;k++)
   {
    number+=cam.R[i][k]*R_z[k][j];
   }
   cam.R[i][j]=number;
   //printf("%lf ",cam.R[i][j]);
   number=0;
  }
  //printf("/n");
}

}
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;
int i,j,k;
double CoorW[3][1] = {p3D.X,p3D.Y,p3D.Z};//錯誤，應該把p3D.Y改成cam.Height
double CoorC[3][1];
for(i=0;i<3;i++)
{
  for(j=0;j<1;j++)
  {
   CoorC[i][j]=0;
   for(k=0;k<3;k++)
    CoorC[i][j]+=cam.R[i][k]*CoorW[k][j];
  }
}

p2D.X=AxisX+CoorC[0][0]/CoorC[2][0]*AX;
p2D.Y=AxisY+CoorC[1][0]/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;

int i,j,k;
double temp;
double CoorC[3][1]={
  (p2D.X-AxisX)/AX,
  (p2D.Y-AxisY)/AY,
  1
};
//for(i=0;i<3;i++)printf("%lf ",CoorC[i][0]);
double CoorW[3][1];
double TransR[3][3];

for(i=0;i<3;i++)
{
  for(j=0;j<3;j++)
  {
   TransR[i][j]=cam.R[j][i];
   printf("%lf ",TransR[i][j]);
  }
  printf("/n");
}
for(i=0;i<3;i++)
{

  CoorW[i][0]=0.0;
  for(k=0;k<3;k++)
   CoorW[i][0]+=TransR[i][k]*CoorC[k][0];
  printf("%lf",CoorW[i][0]);
  printf("/n");
}

p3D.X=CoorW[0][0]/CoorW[1][0]*Hight;
p3D.Z=CoorW[2][0]/CoorW[1][0]*Hight;
p3D.Y=Hight;
}
int _tmain(int argc, _TCHAR* argv[])
{
CameraInfo cam;
InitCameraInfo(cam);
Point2D p2D;
p2D.X=1;
p2D.Y=1;
Point3D p3D;
IPMPoint2DTO3D(p2D,p3D,cam);
printf("%lf %lf %lf /n",p3D.X,p3D.Y,p3D.Z);
return 0;
}

function InitCameraInfo()
global CameraInfo;

CameraInfo.ax = 115.88;
CameraInfo.ay = 115.88;
CameraInfo.u0 = 180;
CameraInfo.v0 = 120;
CameraInfo.f = 2.85;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% C100 %%%%%%%%%%%%%%%%%%%%%
CameraInfo.Alph = -30.05 * pi / 180;%老參數
CameraInfo.Beta = -1.31 * pi / 180;
CameraInfo.Gama = 0.09 * pi / 180;
CameraInfo.Height = 881;

%     CameraInfo.Alph =- 37.56 * pi / 180;%E組新參數
%     CameraInfo.Beta = -1.85 * pi / 180;
%     CameraInfo.Gama = 0.72* pi / 180;
%     CameraInfo.Height = 838;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 松下 %%%%%%%%%%%%%%%%%%%%%
%     CameraInfo.ax = 956 / 2;
%     CameraInfo.ay = 868 / 2;
%     CameraInfo.u0 = 180;
%     CameraInfo.v0 = 121;
%     CameraInfo.f = 2.85;
%     CameraInfo.Alph = 0 * pi / 180;
%     CameraInfo.Beta = 0 * pi / 180;
%     CameraInfo.Gama = 0 * pi / 180;
%     CameraInfo.Height = 1220;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    R_x = [ 1       0        0;
           0    cos(CameraInfo.Alph) sin(CameraInfo.Alph);
           0    -sin(CameraInfo.Alph)   cos(CameraInfo.Alph) ];

    R_y = [ cos(CameraInfo.Beta)    0    -sin(CameraInfo.Beta);
                  0       1      0;
            sin(CameraInfo.Beta)    0    cos(CameraInfo.Beta) ];

    R_z = [ cos(CameraInfo.Gama)   sin(CameraInfo.Gama)    0;
            -sin(CameraInfo.Gama)     cos(CameraInfo.Gama)    0;
                  0           0        1 ];

    CameraInfo.R = R_z * R_y * R_x;

    CameraInfo.K = [ CameraInfo.ax 0 CameraInfo.u0; 0 CameraInfo.ay CameraInfo.v0; 0 0 1 ];
% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%