A = [  1     0     0     0;
       0     2     0     0;
       0     0     3     0 ];

v = [  1     2     3     4     1     3     4     2];
plot3(A(1,v),A(2,v),A(3,v),'-.');
hold on;
view(115,20);
axis([-2 2 0 5 0 4]);

x = [ 0     0.5   0.5   0     0     0     1;
      0     0     1     1.5   0.5   0     0;
      0     1.5   0     0.75  2.25  2     0 ];
plot3(x(1,:),x(2,:),x(3,:),'*');

%thetar = rand(1,1:3)
thetar = [pi/4 pi/15 -pi/6];
Qr = ang2orth(thetar);

%tr = rand(3,1)*3;
tr = [1;3;2];

B = Qr * A + tr * ones(1, size(A,2));
plot3(B(1,v),B(2,v),B(3,v),':');
pause

% (randomly distorted) measured points:
% xi = (Qr*x+tr*ones(1,length(x))+randn(size(x))/10);

xi = [ 0.8314 0.9821 1.0211 0.1425 0.2572 0.5229 1.7713
       3.0358 4.5232 3.8075 4.4826 5.0120 4.5364 3.3987
       1.9328 2.8703 1.0573 1.5803 3.1471 3.5394 1.9054];
plot3(xi(1,:),xi(2,:),xi(3,:),'o');
pause

[t,Q]= pointfit(xi,x);
% the fitted pyramid:
C = Q * A + t * ones(1, size(A,2));
plot3(C(1,v),C(2,v),C(3,v),'-');
hold off;

theta = rotangle(Q)

thetar


S = ang2orth(theta);    %S.B > removed
norm(Q - S)


% Define nominal points.
A = [1 0 0 0;0 2 0 0;3 3 3 0];
v = [1 2 3 4 1 3 2 4];
plot3(A(1,v),A(2,v),A(3,v),'-.');
hold on;
view(110,20)
axis([-1 2 -1 3 -3 3])

B = [1 0 0 0;0 2 0 0;-3 -3 -3 0];
plot3(B(1,v),B(2,v),B(3,v),':')

[t,Q] = pointfit(B,A);
C = Q * A + t * ones(1, size(A,2))
plot3(C(1,v),C(2,v),C(3,v),'-')
text(1,-.25,3,'x1'); text(0,2.1,3,'x2')
text(0,.1,3.3,'x3'); text(0,-1,-0.1,'x4, xi4, c4')
text(1,-.25,-3,'xi1'); text(0,2.1,-3,'xi2, c2')
text(0,.1,-2.65,'xi3'); text(0.7,-0.3,-2.65,'c1')
text(1.75,.6,-2.65,'c3')
theta = 180/pi*rotangle(Q)
hold off;

