%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Solution to Problem 6 of
%  Achieving a Common Viewpoint: Yaw, Pitch, and Roll
%  Dianne P. O'Leary and David A. Schug
%  Computing in Science and Engineering
%
%  In this problem, we use the singular value decomposition
%  to solve a series of problems
%  specified by the data in the global variables A
%  and B (3x7).
%  We compute the orthogonal matrix Q that
%  minimizes   || B - Q A - t e^T ||_F
%  (where "F" denotes the Frobenius norm).
%  The matrix Q is parameterized by the three Euler angles
%  that give yaw, pitch, and roll.
%  The 3x1 vector t specifies a translation, and the
%  nx1 vector e contains ones.
%
%  problem6.m   06/2004
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

baseplot = 0;
m = 20;
pitch = pi/4;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate the problems one by one and solve them.
% For each, the true yaw is pi/4, the true pitch is pi/4,
% and the true roll is pi/9.
% First we solve 20 problems with different translation
% vectors (sigma = 0).  
% Then we solve 20 more problems in which the entries
% in A are perturbed by random numbers in [-.001,.001]
% (sigma = 1.e-3).
% Plot the resulting errors.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for sigma = [0 1.e-3]
   anglsav = [];
   qerror = [];
   aerror = [];

   for k=1:m,
     angltrue = [pi/4,pitch,pi/9];
     [A,B,Qtrue] = makedata(angltrue);
     ttrue = rand(3,1);
   
     B = Qtrue*(A+sigma*rand(size(A))) + ttrue*ones(1,7);
   
     [rmsd,angl,t,Q,newB] = viewpoint(A,B,1);
   
     anglsav = [anglsav,angl];
     qerror = [qerror,norm(Qtrue-Q,'fro')];
     aerror = [aerror,rmsd];
   end

   subplot(2,2,baseplot+1)
   plot(1:m,anglsav(1,:)-pi/4,'b+',1:m,anglsav(2,:)-pitch,'go', ...
        1:m,anglsav(3,:)-pi/9,'rx')
   %legend('Yaw','Pitch','Roll')
   xlabel('sample number')
   ylabel('angle(radians)')
   title(sprintf('sigma = %5.3f',sigma))
   
   subplot(2,2,baseplot+3)
   semilogy(1:m,qerror,'b+',1:m,aerror,'go')
   %legend('Error in Q','RMSD')
   xlabel('sample number')
   ylabel('error')
   title(sprintf('sigma = %5.3f',sigma))
   
   baseplot = baseplot + 1;
end