AddNoise2Trajectory

 Adds noise to a trajectory.

 Corrupts a trajectory, T with a given noise. The parameter Noise is
 only the covariance of the noise to be added since it is assumed to
 have zero mean.

 The trajectory is corrupted, adding noise to the relative displacements
 between consecutive poses. In this way the error accumulates, which is
 what happens with real noisy sensors.
   
 The output is in the form of a (noisy) Trajectory

0001 function Tn=AddNoise2Trajectory(T,Noise)
0002 % Adds noise to a trajectory.
0003 %
0004 % Corrupts a trajectory, T with a given noise. The parameter Noise is
0005 % only the covariance of the noise to be added since it is assumed to
0006 % have zero mean.
0007 %
0008 % The trajectory is corrupted, adding noise to the relative displacements
0009 % between consecutive poses. In this way the error accumulates, which is
0010 % what happens with real noisy sensors.
0011 %
0012 % The output is in the form of a (noisy) Trajectory
0013 
0014   n=size(T);
0015   R=cell(1,n);
0016   
0017   d=det(Noise);
0018   if d==0
0019     % a noissless sensor
0020     R=T.data;
0021   else
0022     % T is a proper trajectory (already defined). Its not clear why
0023     % we can not use T{1} here....
0024     R{1}=T.data{1}; % First pose is not affected by noise since
0025     % noise is applied to displacements
0026     for i=2:n
0027       % Relative displacement between the steps in the 'ideal' trajectory
0028       D=Absolute2Relative(T.data{i-1},T.data{i});
0029       % Corrupt the relative displacement  with noise
0030       Dn=AddNoise2Relative(D,Noise);
0031       % Add the corrupted displacement to the last pose in the
0032       % corrupted trajectory
0033       R{i}=Relative2Absolute(R{i-1},Dn); % R is not a trajectory yet -> do
0034                                          % not use trajectory methods
0035     end
0036   end
0037   Tn=Trajectory(R);
0038   
0039