matlab tsc模块,TSCAI算法 MATLAB 执行中的疑问

本人 用网上搜的标准TSCA 算法 输入参数后 确与 实际手算结果不同 求教！！ 代码见附件

参数 世界坐标 5组 X   Y   Z       图像坐标  5组  -0.58    0         Cx Cy 图像中心坐标 0  0    Ncx=Ncy=120        预设 dx=0.0044708;  dy=0.0044708;

0    5    0                                1.73   1

10  7.5  0                                1.73   0

10   5    0                                  0      1

5   10   0                                  0      -1

5    0    0

运行结果  R =0.8650         0       0.5018

0      1.0000         0

-0.5018         0       0.8650

手算结果   R =0.8650         0       -0.5018

0      -1.0000         0

-0.5018         0       0.8650

用中断检查发现  手算的f是小于0的 因此需要将R矩阵中的符号取反，而程序的运行结构f是正的(猜测)因为

% 3) Compute the 3D rotation matrix R, or r1, r2,...,r9

r1 = r1p*Ty;

r2 = r2p*Ty;

r4 = r4p*Ty;

r5 = r5p*Ty;

Tx = Txp*Ty;

s = -sign(r1*r4 + r2*r5);

R=[r1, r2, (1-r1^2-r2^2)^(0.5); r4, r5, s*(1-r4^2-r5^2)^(0.5)];

R = [R(1:2,:); cross(R(1,:), R(2,:))];

r7 = R(3,1);

r8 = R(3,2);

r9 = R(3,3);

y = r4*xw+r5*yw+Ty;

w = r7*xw+r8*yw;

z = [y -dy*Y] \ [dy*(w.*Y)];

f = z(1);                                                       由于可能是正值 直接跳过了 IF语句的判断

if f < 0,

R(1,3) = -R(1,3);

R(2,3) = -R(2,3);

R(3,1) = -R(3,1);

R(3,2) = -R(3,2);

end

r3 = R(1,3);

r6 = R(2,3);

r7 = R(3,1);

r8 = R(3,2);

clear s y w z

% 2) Stage 2 --- Compute Effective Focal Length, Distortion Coefficients, and z Position:

% d) Compute an approximation of f and Tz by ignoring lens distortion:

y = r4*xw+r5*yw+Ty;

w = r7*xw+r8*yw;

z = [y -dy*Y] \ [dy*(w.*Y)];

f = z(1);

Tz = z(2);

% Compute the exactly solution for f, Tz, k1:

params_const = [r4 r5 r6 r7 r8 r9 dx dy sx Ty];

params = [f, Tz, 0];  % add initial guess for k1

[x,fval,exitflag,output] = fminsearch( @Tsai_8b, params, [], params_const, xw, yw, zw, X, Y);

f = x(1);

Tz = x(2);

k1 = x(3);

T=[Tx, Ty, Tz]';

zz=r7*xw+r8*yw+Tz

% fval the value of the objective function fun at the solution x.

fval

% exitflag that describes the exit condition of fminsearch

% >0 Indicates that the function converged to a solution x.

% 0  Indicates that the maximum number of function evaluations was exceeded.

% <0 Indicates that the function did not converge to a solution.

exitflag

% output that contains information about the optimization

% output.algorithmThe algorithm used

% output.funcCountThe number of function evaluations

% output.iterationsThe number of iterations taken

output

但是 即使f 是正的 ，R矩阵中 r3和r7 应该同号的( 原因：计算公式r7=(1-r2^2-r2*r4)/r3 )  为什么会出现 不同号？