2D到3D背投无法正常工作

Question

我目前正在尝试在matlab的this paper (p5-8, p17-20)中实现的第二种算法的核心，作为需要对象高度检测的较大项目的原型。该算法已编码，但是对于我提供的任何数据，给定的输出都远不正确。

失败之后，我尝试使用3.2.2节中描述的方法派生多个参数，并多次遵循派生。我可能会错过对坐标系统的不正确理解之类的东西。如果有人可以告诉我我犯下的任何误解或错误，我将永远感激不已。

数据，对象和代码如下。请注意，我使用的相机是Fujifilm FinePix S4000，所有使用的度量单位均为米或度。大多数参数在主算法中给出。

主要算法：

clear
clc

R = [3072 2304];    %x,y dimensions of image
c0 = [1472, 1616];  %base point (floor, Z point = 0)
c1 = [1472, 728];   %top of object (should be ~14.5cm or 0.145m)
f = 0.0043;         %focal length
T = [0;0;0.07175];   %Camera position (start of co-ord system except height)
cmos_x = 0.00616;   %CCD x dimension = 6.16mm (I think)
cmos_y = 0.00462;   %CCD y dimension = 4.62mm (I think)
tilt = 0;           %perpendicular to floor

gamma_x = 2*atand(cmos_x/(2*f));
gamma_y = 2*atand(cmos_y/(2*f));

[Rx_theta, r] = r_calc(c0, R, gamma_x, gamma_y, tilt); %Rx_theta is unused - was for a test

t = T(3,1)/-r(3,1);    %t=z0/-c, gotten from Z

X = T(1,1) + r(1,1)*t; %should be common between points 1 and 2 - typically not?
Y = T(2,1) + r(2,1)*t; %""
Z = T(3,1) + r(3,1)*t; %check is 0

[Rx_theta1, r1] = r_calc(c1, R, gamma_x, gamma_y, tilt);

Z1 = T(3,1) + r1(3,1)*t;   %height
vpa(Z1-Z, 10)              %Z = 0

r_calc的功能

function [Rx_theta,r] = r_calc(p,R,gamma_x,gamma_y,tilt)

syms theta

beta = -(p(1,1)/R(1,1))*gamma_x;
alpha = -(p(1,2)/R(1,2))*gamma_y+tilt;

Rx = [1 0 0; 0 cosd(theta) -sin(theta); 0 sin(theta) cos(theta)];
Rz = [cosd(theta) -sind(theta) 0; sind(theta) cosd(theta) 0; 0 0 1];

rs = [0; 1; 0];

Rx_theta = subs(Rx, theta, alpha);
Rz_theta = subs(Rz, theta, beta);
r = Rx_theta*Rz_theta*rs;

end

14.5厘米的物体，距离相机30厘米，相机抬高4厘米，镜头直径6.35厘米：