Triangulation & Direct linear transform

Question

Following Hartley/Zisserman's Multiview Geometery, Algorithm 12: The optimal triangulation method (p318), I got the corresponding image points xhat1 and xhat2 (step 10). In step 11, one needs to compute the 3D point Xhat. One such method is Direct Linear Transform (DLT), mentioned in 12.2 (p312) and 4.1 (p88).

The homogenous method (DLT), p312-313, states that it finds a solution as the unit singular vector corresponding to the smallest singular value of A, thus,

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.');

However, A is a 16x1 matrix, resulting in a Va that is 1x1.

What am I doing wrong (and a fix) in getting the 3D point?

For what its worth sample data:

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 =    <- my computation

  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

Update Working code for section xi in algorithm

% 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

Answer 1

As is mentioned in the book (sec 12.2), p^{i T} are the rows of P. Therefore, you don't need to transpose P1(k,:) (i.e. the right formulation is A = [xhat1(1) * P1(3,:) - P1(1,:) ; ...).

I hope that was just a typo.

Additionally, it is recommended to normalize each row of A with its L2 norm, i.e. for all i

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

And if you want to plot the triangulated 3D points, you have to normalize Xhat before plotting (its meaningless otherwise), i.e.

Xhat = Xhat/Xhat(4);

Answer 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,:));

Could be simplified as A = normr(A).