tools/clitkSignalApparentMotionTrackingFilter.cxx

   1 /*=========================================================================
   2
   3   Program:   clitk
   4   Module:    $RCSfile: clitkSignalApparentMotionTrackingFilter.cxx,v $
   5   Language:  C++
   6   Date:      $Date: 2010/03/03 12:41:27 $
   7   Version:   $Revision: 1.1 $
   8
   9   Copyright (c) CREATIS (Centre de Recherche et d'Applications en Traitement de
  10   l'Image). All rights reserved. See Doc/License.txt or
  11   http://www.creatis.insa-lyon.fr/Public/Gdcm/License.html for details.
  12
  13      This software is distributed WITHOUT ANY WARRANTY; without even
  14      the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  15      PURPOSE.  See the above copyright notices for more information.
  16
  17 =========================================================================*/
  18
  19 #include "clitkSignalApparentMotionTrackingFilter.h"
  20 #include <limits>
  21 #include <fstream>
  22
  23 //---------------------------------------------------------------------
  24 void clitk::SignalApparentMotionTrackingFilter::SetParameters(args_info_clitkSignalApparentMotionTracking & a) {
  25   args_info = a;
  26 }
  27 //---------------------------------------------------------------------
  28
  29
  30 //---------------------------------------------------------------------
  31 void clitk::SignalApparentMotionTrackingFilter::ComputeMeanAndIsoPhase(clitk::Signal & s,
  32                                                                        int delay,
  33                                                                        int L,
  34                                                                        int nbIsoPhase,
  35                                                                        clitk::Signal & mean,
  36                                                                        clitk::Signal & isoPhase) {
  37   int mMaxIteration = 1;
  38
  39   // Compute augmented signal
  40   clitk::Signal sx;
  41   clitk::Signal sy;
  42   s.ComputeAugmentedSpace(sx, sy, delay);
  43
  44   // DEBUG augmented space
  45   static int tt=0;
  46   ofstream os;
  47   if (tt==0) os.open("augmentedU.sig");
  48   else os.open("augmentedV.sig");
  49   tt++;
  50   for(uint i=0; i<sx.size(); i++) {
  51     os << i << " " << sx[i] << " " << sy[i] << std::endl;
  52   }
  53   os.close();
  54
  55   // Compute starting ellipse
  56   Ellipse An;
  57   An.InitialiseEllipseFitting(-1, L, sx, sy); // -1 is auto eta
  58   DD(An.ComputeSemiAxeLengths());
  59
  60   // Fit successive ellipses
  61   std::vector<clitk::Ellipse*> e;
  62   int n = s.size()-L-delay;
  63   for(int t=0; t<n; t++) {
  64     // An.InitialiseEllipseFitting(-1, L, sx, sy); // -1 is auto eta
  65     An.UpdateSMatrix(t, L, sx, sy);
  66     // DD(An.GetEta());
  67 //     DD(An);
  68 //     DD(An.ComputeSemiAxeLengths());
  69     int nn = 0;
  70     while (nn<mMaxIteration) {
  71       An.EllipseFittingNextIteration();
  72       // DD(nn);
  73 //       DD(An);
  74 //       DD(An.ComputeSemiAxeLengths());
  75       nn++;
  76     }
  77     clitk::Ellipse * B = new clitk::Ellipse(An);
  78     e.push_back(B);
  79     DD(t);
  80     DD(B->ComputeCenter());
  81 //      DD(B->ComputeSemiAxeLengths());
  82   }
  83
  84   // Get mean and isoPhase
  85   //mean.resize(n);
  86   isoPhase.resize(n);
  87   ComputeIsoPhaseFromListOfEllipses(e, sx, sy, nbIsoPhase, delay, L, isoPhase);
  88   for(unsigned int i=0; i<e.size(); i++) {
  89     clitk::Ellipse & An = *e[i];
  90     mean[i+delay+L] = An.ComputeCenter()[0];
  91     //    DD(An.ComputeCenter());
  92   }
  93   DD("------------------------");
  94 }
  95 //---------------------------------------------------------------------
  96
  97
  98 //---------------------------------------------------------------------
  99 void clitk::SignalApparentMotionTrackingFilter::ComputeMeanAndIsoPhase(clitk::Trajectory2D & s,
 100                                                                        int delay,
 101                                                                        int L,
 102                                                                        int nbIsoPhase,
 103                                                                        clitk::Trajectory2D & mean,
 104                                                                        std::vector<int> & isoPhase) {
 105   // Extract signal (slow I know)
 106   clitk::Signal U(s.GetU());
 107   clitk::Signal V(s.GetV());
 108
 109   clitk::Signal & meanU = mean.GetU();
 110   clitk::Signal & meanV = mean.GetV();
 111
 112   meanU.resize(s.size());
 113   meanV.resize(s.size());
 114
 115   clitk::Signal isoPhaseU;
 116   clitk::Signal isoPhaseV;
 117
 118   DD(U.size());
 119   DD(V.size());
 120
 121   // Compute mean and iso independently
 122   ComputeMeanAndIsoPhase(U, delay, L, nbIsoPhase, meanU, isoPhaseU);
 123   ComputeMeanAndIsoPhase(V, delay, L, nbIsoPhase, meanV, isoPhaseV);
 124
 125   // Check isphase ??? ???
 126   isoPhase.resize(s.size());
 127   for(unsigned int i=0; i<isoPhaseU.size(); i++) {
 128
 129     // ********************* U OR V ??????????
 130
 131     isoPhase[i+delay+L] = isoPhaseV[i];
 132     //isoPhase[i+delay+L] = isoPhaseU[i];
 133
 134     //    mean[i+delay+L] = mean[i];
 135     //DD(isoPhase[i]);
 136   }
 137 }
 138 //---------------------------------------------------------------------
 139
 140
 141 //---------------------------------------------------------------------
 142 void clitk::SignalApparentMotionTrackingFilter::ReadDB(std::string filename,
 143                                                        std::vector<Trajectory2D> & bd) {
 144   // File format : 2 columns by point (U,V), time by line
 145   std::ifstream is(filename.c_str());
 146   skipComment(is);
 147   std::string line;
 148   std::getline(is, line);
 149
 150   // Compute nb of column in this first line
 151   std::istringstream iss(line);
 152   std::string s;
 153   int nb = 0;
 154   while (iss>>s) {
 155     nb++;
 156   }
 157   DD(nb);
 158
 159   if (nb%2 == 1) {
 160     std::cout << "ERROR in the file '" << filename << "', I read " << nb
 161               << " columns, which is not odd." << std::endl;
 162   }
 163   nb = nb/2;
 164   DD(nb);
 165
 166   // Read trajectories
 167   bd.resize(nb);
 168   while (is) {
 169     std::istringstream iss(line);
 170     std::string s;
 171     int i=0;
 172     while (iss>>s) {
 173       double u = atof(s.c_str());
 174       iss >> s;
 175       double v = atof(s.c_str());
 176       bd[i].push_back(u,v);
 177       i++;
 178     }
 179     std::getline(is, line);
 180   }
 181   DD(bd[0].size());
 182 }
 183 //---------------------------------------------------------------------
 184
 185
 186 //---------------------------------------------------------------------
 187 void clitk::SignalApparentMotionTrackingFilter::Update() {
 188
 189   // -----------------------------------------------------
 190   // Read and build initial DB
 191   ReadDB(args_info.ref_arg, mReferenceTrajectoriesBD);
 192   mReferencePhase.Read(args_info.refphase_arg);
 193   mReferencePhase.SetSamplingPeriod(args_info.refsampling_arg);
 194
 195   DD(mReferencePhase.GetSamplingPeriod());
 196   DD(mReferencePhase.size());
 197   DD(mReferenceTrajectoriesBD.size());
 198
 199   mReferenceMean.resize(mReferenceTrajectoriesBD.size());
 200   for(unsigned int i=0; i<mReferenceMean.size(); i++) {
 201     mReferenceTrajectoriesBD[i].GetMean(mReferenceMean[i]);
 202     // DD(mReferenceMean[i]);
 203   }
 204
 205   for(uint i=0; i<mReferenceTrajectoriesBD.size(); i++)
 206     mReferenceTrajectoriesBD[i].SetSamplingPeriod(mReferencePhase.GetSamplingPeriod());
 207
 208   // Relative reference motion
 209   for(uint i=0; i<mReferenceTrajectoriesBD.size(); i++) {
 210     // DD(i);
 211     // mReferenceTrajectoriesBD[i].Print();
 212     mReferenceTrajectoriesBD[i].Substract(mReferenceMean[i]);
 213     // mReferenceTrajectoriesBD[i].Print();
 214   }
 215
 216   // -----------------------------------------------------
 217   // Read trajectory
 218   mInput.Read(args_info.input_arg);
 219   DD(mInput.size());
 220   mInput.SetSamplingPeriod(args_info.inputsampling_arg);
 221   mInputMean.SetSamplingPeriod(args_info.inputsampling_arg);
 222   mInputRelative.SetSamplingPeriod(args_info.inputsampling_arg);
 223   DD(mInput.GetSamplingPeriod());
 224
 225   // -----------------------------------------------------
 226   // Compute input mean and isophase
 227   std::cout << "Compute Mean and IsoPhase" << std::endl;
 228   std::vector<int> isophase;
 229   double d = args_info.delay_arg;
 230   int L = args_info.windowLength_arg;
 231   int nbiso = args_info.nbiso_arg;
 232   isophase.resize(mInput.size());
 233   ComputeMeanAndIsoPhase(mInput, d, L, nbiso, mInputMean, isophase);
 234
 235   // -----------------------------------------------------
 236   // Substract mean to trajectory
 237   std::cout << "Build relative traj" << std::endl;
 238   mInputRelative.resize(mInput.size());
 239   for(int i=0; i<mInput.size(); i++) {
 240      // DD(i);
 241 //      DD(mInput.GetU(i));
 242 //      DD(mInput.GetV(i));
 243 //      DD(mInputMean.GetU(i));
 244 //      DD(mInputMean.GetV(i));
 245      mInputRelative.SetPoint(i, mInput.GetU(i) - mInputMean.GetU(i),
 246                             mInput.GetV(i) - mInputMean.GetV(i));
 247     // DD(mInputRelative.GetU(i));
 248 //     DD(mInputRelative.GetV(i));
 249   }
 250
 251   // DEBUG : output mean
 252   ofstream osm("mean-phase.sig");
 253   for(int i=0; i<mInput.size(); i++) {
 254     osm << mInputMean.GetU(i) << " " << mInputMean.GetV(i) << " " << isophase[i] << std::endl;
 255   }
 256   osm.close();
 257
 258   // -----------------------------------------------------
 259   // Loop on each iso-phase segment
 260   std::cout << "Loop on each isophase" << std::endl;
 261   clitk::Signal mEstimatedPhase;
 262   mEstimatedPhase.resize(mInput.size());
 263
 264   std::vector<int> isophaseIndex;
 265   int previous = isophase[0];
 266   for(uint i=0; i<isophase.size(); i++) {
 267     if (isophase[i] != previous) {
 268       isophaseIndex.push_back(i);
 269       previous = isophase[i];
 270     }
 271   }
 272   DDV(isophaseIndex, isophaseIndex.size());
 273
 274   for(uint ips=0; ips<isophaseIndex.size()-1; ips++) {
 275     DD(ips);
 276     int begin = isophaseIndex[ips];
 277     int end = isophaseIndex[ips+1];
 278     DD(begin);
 279     DD(end);
 280
 281
 282     std::cout << "-----------------------------------------------" << std::endl;
 283     std::cout << "-----------------------------------------------" << std::endl;
 284     std::cout << "Find traj from " << begin << " to " << end << std::endl;
 285     mInput.Print("mInput", begin, end);
 286     mInputRelative.Print("mInputRelativ", begin, end);
 287
 288
 289     // Get current mean position
 290     Vector2d currentMeanPosition;
 291     mInputMean.GetPoint(begin, currentMeanPosition);
 292     DD(currentMeanPosition);
 293
 294     // Find closest ref trajectory
 295     int index = FindClosestTrajectory(mReferenceMean, currentMeanPosition);
 296     DD(index);
 297     clitk::Trajectory2D & currentClosestReferenceTrajectory = mReferenceTrajectoriesBD[index];
 298
 299     DD(mReferenceMean[index]);
 300     currentClosestReferenceTrajectory.Print("currentClosestReferenceTrajectory");
 301
 302     // Estimate phase delta
 303     clitk::Signal tempPhase;
 304     EstimatePhase(mInputRelative, begin, end,
 305                   currentClosestReferenceTrajectory,
 306                   mReferencePhase, tempPhase);
 307
 308     // Cat current phase
 309     int n=0;
 310     for(int i=begin; i<end; i++) {
 311       mEstimatedPhase[i] = tempPhase[n];
 312       n++;
 313     }
 314   }
 315
 316   // -----------------------------------------------------
 317   // Output time - phase
 318   // DEBUG OUTPUT
 319   ofstream os(args_info.output_arg);
 320   for(int t=0; t<mInput.size(); t++) {
 321     os << t << " " << t*mInput.GetSamplingPeriod() << " " << mEstimatedPhase[t] << std::endl;
 322   }
 323   os.close();
 324
 325 }
 326 //---------------------------------------------------------------------
 327
 328
 329 //---------------------------------------------------------------------
 330 void clitk::SignalApparentMotionTrackingFilter::EstimatePhase(const clitk::Trajectory2D & input,
 331                                                               const int begin, const int end,
 332                                                               const clitk::Trajectory2D & ref,
 333                                                               const clitk::Signal & phaseref,
 334                                                               clitk::Signal & phase) {
 335
 336   // Create time-warped resampled trajectory
 337   clitk::Trajectory2D T;
 338   T.SetSamplingPeriod(ref.GetSamplingPeriod());
 339   DD(input.size());
 340   input.ResampleWithTimeWarpTo(begin, end, T);
 341   DD(input.GetSamplingPeriod());
 342   DD(ref.GetSamplingPeriod());
 343   ref.Print("ref");
 344   input.Print("input", begin, end);
 345   T.Print("T");
 346
 347   // Find optimal delta
 348   int delta = FindPhaseDelta(T, ref);
 349   DD(delta);
 350
 351   // Shift phase
 352   clitk::Signal temp(phaseref);
 353   DD(temp.GetSamplingPeriod());
 354   DDV(temp, temp.size());
 355   temp.Shift(delta, (int)lrint((end-begin)*input.GetSamplingPeriod()/phaseref.GetSamplingPeriod()));
 356   DD(temp.GetSamplingPeriod());
 357   DDV(temp, temp.size());
 358
 359
 360   // Output result with time-unwarp
 361   phase.resize(end-begin);
 362   phase.SetSamplingPeriod(input.GetSamplingPeriod());
 363   DD(phase.GetSamplingPeriod());
 364   DD(phaseref.GetSamplingPeriod());
 365   DD(temp.GetSamplingPeriod());
 366   DD(temp.size());
 367   DD(phase.size());
 368   temp.ResampleWithTimeWarpTo(phase, true); // linear
 369   DDV(phase, phase.size());
 370 }
 371 //---------------------------------------------------------------------
 372
 373
 374 //---------------------------------------------------------------------
 375 int clitk::SignalApparentMotionTrackingFilter::FindPhaseDelta(const clitk::Trajectory2D & A,
 376                                                                  const clitk::Trajectory2D & B) const {
 377   assert(A.GetSamplingPeriod() == B.GetSamplingPeriod());
 378
 379   DD("A");
 380   A.Print("A");
 381   DD("B");
 382   B.Print("B");
 383
 384   // exhaustive search
 385   double min=numeric_limits<double>::max();
 386   int imin =0;
 387   for(int i=0; i<B.size(); i++) {
 388     double d = A.DistanceTo(i, B);
 389     std::cout.precision(10);
 390     std::cout << "i=" << i << " d=" << d << " (min=" << min << " imin=" << imin << ")" << std::endl;
 391     if (d < min) {
 392       imin = i;
 393       min = d;
 394     }
 395   }
 396   DD(imin);
 397   return imin;
 398 }
 399 //---------------------------------------------------------------------
 400
 401
 402 //---------------------------------------------------------------------
 403 int clitk::SignalApparentMotionTrackingFilter::FindClosestTrajectory(const std::vector<Vector2d> & meanlist,
 404                                                                      const Vector2d & mean) {
 405   // Brute force for the moment
 406   double dmin = numeric_limits<double>::max();
 407   double imin=0;
 408   for(uint i=0; i<meanlist.size(); i++) {
 409     double d = (meanlist[i]-mean).GetNorm();
 410     if (d< dmin) {
 411       dmin = d;
 412       imin = i;
 413     }
 414   }
 415   DD(dmin);
 416   DD(imin);
 417   return imin;
 418 }
 419 //---------------------------------------------------------------------