Triangulation & Direct linear transform

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

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