%  This is the workpage for synthesizing trajectories and projecting
%  sets of feature points.

%  Load in some feature sets from tape

load feature0;   % Feature0 points are in two planes in depth
load feature1;   % Feature1 points are more scattered in depth 

%  Load in the motbas data from file.
%  These are the motion parameters that are referenced by the
%  below cmd lists.

motbas;

%  The following section generates the camera rotations and translations
%  for each frame in a sequence.
%
%  The format of the command list is :
%  cmds = [ num_frames next_index num_frames next_index ... ]
%
%  The trajectory is initialized with the first three rows of
%  the motion parameters (motbas in all these cases) providing
%  x, dx/dt, and d2x/dt2.  The cmds list is then
%  traversed, starting at elem 0, not changing the motion
%  parameters (dx/dt and d2x/dt2) for num_frames,
%  at which point the next_index provides a new d2x/dt2.

  %  StillSeq
  %  This trajectory isn't one.  It is for testing the conv. of the filter

% stillseq_cmds = [ 240 ];
% stillseq = synth( stillseq_cmds, motbas, 240 );

  %  Trajectory 0
  %  Dull and boring, this one only has X and Y translations

t0_bas = [ 0 0	0	0	0	0	120 % start at origin
0	0	0	0	0	0	0   % with vel
0	0	0	0	0	0	0   % and no acc
0.001	0	0	0	0	0	0   % 4 - slight acc in +X
0	0.001	0	0	0	0	0   % 5 - slight acc in Y
-0.001	0	0	0	0	0	0   % 6 - slight acc in -X
0	-0.001	0	0	0	0	0 ];  % 7 - slight acc in -Y

traj0_cmds = [ 60  5 5  4 5  3 8  7 2  6 2  3 16  7 2  3 8  6 4  7 2  3 240 ];
traj0 = synth( traj0_cmds, t0_bas, 480 );

  %  Trajectory 1
  %  This one is much longer, with still periods to test convergence !

%            0   20   24    40   44    60   62   78    84   102  112  128 136
% traj1_cmds = [ 20  4 4  3 16  5 4  3 16  7 2  3 16  6 4  3 16  5 4  3 16  8 8  3 120 ];
% traj1 = synth( traj1_cmds, motbas, 240 );

  %  Trajectory 2
  %  This one has translation and rotation - separately

% traj2_cmds = [ 2  19 1  4 8  8 14  7 4  8 8  7 4  3 8  12 8  11 8  15 8  14 8];
% traj2 = synth( traj2_cmds, motbas, 90 );

  %  Trajectory 3  - very simple, for fooling around with  motion prediction

t3_bas = [ 0 0	0	0	0	0	120 % start at origin
0.005	0	0	0	0	0	0   % with vel
0	0	0	0	0	0	0   % and no acc
0.005	0	0	0	0	0	0 ]; % slight acc

% traj3_cmds = [ 120 4 4 3 10 ];
% traj3 = synth( traj3_cmds, t3_bas, 240 );

  %  Wild Trajectory
  %  This one has translation and rotation - seperately and quite a lot

% traj_wild_cmds = [ 10  5 3  4 4  3 40  7 2  8 4  3 60  5 1  7 1  3 40  12 3  3 30  15 3 ];
% traj_wild = synth( traj_wild_cmds, motbas, 200 );

%  Now use the trajectories computed above, along with the feature points,
%  to generate a sample sequence.

test0 = do_proj( feature1, traj0 );
% test1 = do_proj( feature1, traj1 );
% test2 = do_proj( feature1, traj2 );
% wild = do_proj( feature1, traj_wild );

%  We also need a covariance of the measurement noise.
%  Here is a real simple one :

[ m, n ] = size( test0 );
test0_cov = zeros([ n, n ]);
for m = 1:n
  test0_cov( m, m ) = 2.0e-5;
end

% To save the results to file in an ascii format such as that used
% by my estimation applications, use the following lines
% Format: save file_name variable_name -ascii
%
% save test0 test0 -ascii
% save test1 test1 -ascii
% save test1_cov test1_cov -ascii
% save test2 test2 -ascii

% After running the filter, a file containing the results (estimated
% state vectors for each frame in the sequence) should be available.
% This expression will calculate the error in the sequence :

% load test1_out
% error1 = calc_e( feature1, traj1, test1_out );