灭点法摄像机标定

clearclose all;
clc;
points = [245508117592238455335;410386321275230214161];
%%
%compute the rate from pixel to distances
P1 = [points（11） points（21）];
P2 = [points（12） points（22）];

Pixel_D = norm（P1 - P2‘fro‘）;
Real_D = 300*sqrt（2）;
global rate;
rate = Real_D / Pixel_D; %mm

%%
%get the vanishing points
vanishPoint1_x = （points（11）*points（23）-points（21）*points（13））*（points（14）-points（17）） - （points（14）*points（27） - points（17）*points（24））*（points（11） - points（13））;
tmp1 = （（points（24） - points（27））* （points（11） - points（13）） - （points（21） - points（23））*（points（14） - points（17）））;
vanishPoint1_x = vanishPoint1_x / tmp1;

vanishPoint1_y = vanishPoint1_x * （points（24） - points（27）） + points（14）*points（27） - points（17）*points（24）;
vanishPoint1_y = vanishPoint1_y / （points（14） - points（17））;

vanishPoint2_x = （points（17）*points（23）-points（27）*points（13））*（points（14）-points（12）） - （points（14）*points（22） - points（12）*points（24））*（points（17） - points（13））;
tmp2 = （（points（24） - points（22））* （points（17） - points（13）） - （points（27） - points（23））*（points（14） - points（12）））;
vanishPoint2_x = vanishPoint2_x / tmp2;

vanishPoint2_y = vanishPoint2_x * （points（24） - points（22）） + points（14）*points（22） - points（12）*points（24）;
vanishPoint2_y = vanishPoint2_y / （points（14） - points（12））;

v_V1 = [vanishPoint1_x vanishPoint1_y];
v_V2 = [vanishPoint2_x vanishPoint2_y];

%%
%compute the focus distance
point_P_x = 352;
point_P_y = 288;
v_P = [point_P_xpoint_P_y];
[v_real_P_xv_real_P_y] = pixel2space（point_P_xpoint_P_y）;


k = （vanishPoint2_y - vanishPoint1_y） / （vanishPoint2_x - vanishPoint1_x）;
Puv_x = （point_P_x +  k * point_P_y - vanishPoint2_y * k） / （k^2 +1-vanishPoint2_x * k^2）;
Puv_y = vanishPoint2_y -vanishPoint2_x * k + Puv_x * k;

v_Puv = [Puv_x Puv_y];
[v_Real_Puv_xv_Real_Puv_y] = pixel2space（Puv_xPuv_y）;

v1toPuv = v_V1 - v_Puv;
v2toPuv = v_V2 - v_Puv;

Dv12Puv = norm（v1toPuv‘f