imppoint2dto3d.m

从2维空间向三维空间的转换 matlab版的

function [ Point3D ] = IMPPoint2DTo3D( Point2D )
    
    global CAMERA_INFO;
%     Point2D.X=1;
%     Point2D.Y=1;

%     AX = 249.646271;
%     AY = 246.940048;
%     AxisX = 359.440552;
%     AxisY = 249.259109;

%      AX = 124.731781;
%     AY = 123.380569;   
%     AxisX = 180.917313;
%     AxisY = 124.262062;
      AX = 115.88;
    AY = 115.88;
      
    AxisX = 180;
    AxisY = 120;
    Hight = 0.874;
 
    Pitch = 30.16*pi/180 ;
    Yaw = -0.81*pi/180;
    Roll = 1.09*pi/180 ;

    if Point2D.Y ~= AxisY
        A = AY / ( Point2D.Y - AxisY );
        B = ( Point2D.X - AxisX ) * AY / ( AX * ( Point2D.Y - AxisY ) );
    else
        A = AY;
        B = ( Point2D.X - AxisX ) * AY / AX;
    end

    a_11 = cos(Roll)*cos(Yaw);
    a_12 = (cos(Roll)*sin(Yaw)*sin(Pitch) - sin(Roll)*cos(Pitch));
    a_13 = (cos(Roll)*sin(Yaw)*cos(Pitch) + sin(Roll)*sin(Pitch));
    a_21 = sin(Roll)*cos(Yaw);
    a_22 = (cos(Roll)*cos(Pitch) + sin(Roll)*sin(Yaw)*sin(Pitch));
    a_23 = (sin(Roll)*sin(Yaw)*cos(Pitch) - cos(Roll)*sin(Pitch));
    a_31 = -sin(Yaw);
    a_32 = cos(Yaw)*sin(Pitch);
    a_33 = cos(Yaw)*cos(Pitch);

    deta_1 = a_31 - A*a_21;
    deta_2 = a_33 - A*a_23;
    deta_3 = A*a_22*Hight - a_32*Hight;
    deta_4 = a_11 - B*a_21;
    deta_5 = a_13 - B*a_23;
    deta_6 = (B*a_22 - a_12)*Hight;

    X = (deta_3*deta_5 - deta_6*deta_2)/(deta_1*deta_5 - deta_4*deta_2);
    Z = (deta_3*deta_4 - deta_6*deta_1)/(deta_2*deta_4 - deta_1*deta_5);
    
    Point3D.X = X;
    Point3D.Y = Z;

return;