直接线性变换 matlab,matlab – 三角剖分和直接线性变换

按照Hartley / Zisserman的Multiview Geometery,算法12：最优三角测量方法(p318),我得到了相应的图像点xhat1和xhat2(步骤10).在步骤11中,需要计算3D点Xhat.一种这样的方法是直接线性变换(DLT),在12.2(p312)和4.1(p88)中提到.

同质方法(DLT),p312-313,指出它找到一个解作为对应于A的最小奇异值的单位奇异向量,因此,

A = [xhat1(1) * P1(3,:)' - P1(1,:)' ;

xhat1(2) * P1(3,:)' - P1(2,:)' ;

xhat2(1) * P2(3,:)' - P2(1,:)' ;

xhat2(2) * P2(3,:)' - P2(2,:)' ];

[Ua Ea Va] = svd(A);

Xhat = Va(:,end);

plot3(Xhat(1),Xhat(2),Xhat(3), 'r.');

但是,A是16×1矩阵,导致Va为1×1.

在获取3D点时我做错了什么(以及修复)？

对于它值得的样本数据：

xhat1 =

1.0e+009 *

4.9973

-0.2024

0.0027

xhat2 =

1.0e+011 *

2.0729

2.6624

0.0098

P1 =

699.6674 0 392.1170 0

0 701.6136 304.0275 0

0 0 1.0000 0

P2 =

1.0e+003 *

-0.7845 0.0508 -0.1592 1.8619

-0.1379 0.7338 0.1649 0.6825

-0.0006 0.0001 0.0008 0.0010

A =

1.0e+011 *

-0.0000

0

0.0500

0

0

-0.0000

-0.0020

0

-1.3369

0.2563

1.5634

2.0729

-1.7170

0.3292

2.0079

2.6624

更新算法中xi节的工作代码

% xi

A = [xhat1(1) * P1(3,:) - P1(1,:) ;

xhat1(2) * P1(3,:) - P1(2,:) ;

xhat2(1) * P2(3,:) - P2(1,:) ;

xhat2(2) * P2(3,:) - P2(2,:) ];

A(1,:) = A(1,:)/norm(A(1,:));

A(2,:) = A(2,:)/norm(A(2,:));

A(3,:) = A(3,:)/norm(A(3,:));

A(4,:) = A(4,:)/norm(A(4,:));

[Ua Ea Va] = svd(A);

X = Va(:,end);

X = X / X(4); % 3D Point