tools/clitkSignalMeanPositionFilter.cxx

   1 /*=========================================================================
   2
   3   Program:   clitk
   4   Module:    $RCSfile: clitkSignalMeanPositionFilter.cxx,v $
   5   Language:  C++
   6   Date:      $Date: 2010/02/10 14:55:00 $
   7   Version:   $Revision: 1.4 $
   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 "clitkSignalMeanPositionFilter.h"
  20
  21 //---------------------------------------------------------------------
  22 void clitk::SignalMeanPositionFilter::SetParameters(args_info_clitkSignalMeanPositionTracking & args) {
  23   args_info = args;
  24   mEtaIsSet = false;
  25   mOutputFilenameIsSet = false;
  26   mOutputResidualFilenameIsSet = false;
  27   mInput.Read(args_info.input_arg);
  28   mAugmentationDelay = args_info.delay_arg;
  29   mIsAdaptiveMethod = false;
  30   mWindowLength = -1;
  31   mValidationWithRealPhase = false;
  32   mUseLearnedDeltaPhase = false;
  33   if (args_info.eta_given) {
  34     mEta = args_info.eta_arg;
  35     mEtaIsSet = true;
  36   }
  37   mMaxIteration = args_info.iter_arg;
  38   if (args_info.output_given) {
  39     mOutputFilenameIsSet = true;
  40     mOutputFilename = args_info.output_arg;
  41   }
  42   if (args_info.residual_given) {
  43     mOutputResidualFilenameIsSet = true;
  44     mOutputResidualFilename = args_info.residual_arg;
  45   }
  46   if (args_info.augmented_given) {
  47     mOutputAugmentedFilenameIsSet = true;
  48     mOutputAugmentedFilename = args_info.augmented_arg;
  49   }
  50   mVerbose = args_info.verbose_flag;
  51   mVerboseIteration = args_info.verbose_iteration_flag;
  52   if (args_info.L_given) {
  53     mIsAdaptiveMethod = true;
  54     mWindowLength = args_info.L_arg;
  55   }
  56   if (args_info.phase_given) {
  57     mValidationWithRealPhase = true;
  58     mInputPhaseFilename = args_info.phase_arg;
  59     mInputPhase.Read(mInputPhaseFilename);
  60   }
  61   mNumberOfIsoPhase = args_info.nbiso_arg;
  62   if (args_info.delta_given) {
  63     mUseLearnedDeltaPhase = true;
  64     mLearnIsoPhaseDelta.Read(args_info.delta_arg);
  65     mNumberOfIsoPhase = mLearnIsoPhaseDelta.size();
  66   }
  67
  68   // DEBUG
  69   if (args_info.phase_given) {
  70     std::cout << "PLEASE DO NO USE THIS OPTION --phase YET ..." << std::endl;
  71     exit(0);
  72   }
  73   if (args_info.delta_given) {
  74     std::cout << "PLEASE DO NO USE THIS OPTION --delta YET ..." << std::endl;
  75     exit(0);
  76   }
  77
  78 }
  79 //---------------------------------------------------------------------
  80
  81
  82 //---------------------------------------------------------------------
  83 void clitk::SignalMeanPositionFilter::Update() {
  84
  85   // Compute augmented space
  86   if (mVerbose) {
  87     std::cout << "Computing augmented space with delay = " << mAugmentationDelay
  88               << ", number of samples is " << mInput.size()-mAugmentationDelay
  89               << std::endl;
  90   }
  91   mInput.ComputeAugmentedSpace(mAugmentedInputX, mAugmentedInputY, mAugmentationDelay);
  92
  93   // Output augmented state
  94   if (mOutputAugmentedFilenameIsSet) {
  95     std::ofstream os;
  96     openFileForWriting(os, mOutputAugmentedFilename);
  97     for(unsigned int i=0; i<(unsigned int)mAugmentedInputX.size(); i++) {
  98       os << mAugmentedInputX[i] << " " << mAugmentedInputY[i] << std::endl;
  99     }
 100     os.close();
 101   }
 102
 103   // Initial starting ellipse
 104   Ellipse An; // starting point
 105   if (!mEtaIsSet) mEta = -1;
 106   DD(mWindowLength);
 107   if (mWindowLength == -1) {
 108     mWindowLength = mAugmentedInputY.size();
 109   }
 110   An.InitialiseEllipseFitting(mEta, mWindowLength, mAugmentedInputX, mAugmentedInputY);
 111   mEta = An.GetEta();
 112   if (mVerbose) {
 113     std::cout << "Eta is " << mEta << std::endl;
 114   }
 115   Ellipse AnInitial(An);
 116   if (mVerbose) {
 117     std::cout << "Set initial ellipse to c=" << An.ComputeCenter()
 118               << " and axes=" << An.ComputeSemiAxeLengths() << std::endl;
 119   }
 120
 121   // Fitting method
 122   if (mIsAdaptiveMethod) {
 123     AdaptiveFitEllipse(An);
 124   }
 125   else FitEllipse(An);
 126
 127   // Output
 128   if (mVerbose) {
 129     std::cout << "Result    is "  << An << std::endl;
 130     std::cout << "Center    is "  << An.ComputeCenter() << std::endl;
 131     std::cout << "SemiAxis are " << An.ComputeSemiAxeLengths() << std::endl;
 132     std::cout << "Angle     is " << rad2deg(An.ComputeAngleInRad()) << " deg" << std::endl;
 133   }
 134   if (mOutputFilenameIsSet) {
 135     std::ofstream os;
 136     openFileForWriting(os, mOutputFilename);
 137
 138     std::vector<double> phase;
 139     ComputeIsoPhase(mListOfEllipses, phase, mCycles);
 140
 141     //int currentCycle = 0;
 142     for (unsigned int i=0; i<mListOfEllipses.size(); i++) {
 143       clitk::Ellipse & An = *mListOfEllipses[i];
 144       int time = i+mAugmentationDelay+mWindowLength;
 145       os << time << " " // current 'time' in input
 146          << mInput[time] << " " // current value in input
 147          << An.ComputeCenter()[0] << " " << An.ComputeCenter()[1] << " "
 148          << An.ComputeSemiAxeLengths()[0] << " " << An.ComputeSemiAxeLengths()[1] << " "
 149          << An.ComputeAngleInRad() << " "
 150          << rad2deg(An.ComputeAngleInRad()) << " _Phase_ "
 151          << mIsoPhaseIndex[i] << " " ;
 152
 153       /*
 154         if (mUseLearnedDeltaPhase) {
 155         os << mIsoPhaseDelta[mIsoPhaseIndex[i]] << " " << phase[i] << " ";
 156         }
 157       */
 158
 159       os << " _Ellipse_ ";
 160       for(int j=0; j<6; j++) os << An[j] << " ";
 161       os << std::endl;
 162     }
 163     os.close();
 164   }
 165
 166   if (mOutputResidualFilenameIsSet) {
 167     std::ofstream os;
 168     openFileForWriting(os, mOutputResidualFilename);
 169     for(unsigned int i=0; i<mCurrentResidual.size(); i++) {
 170       os.precision(10);
 171       os << mCurrentResidual[i] << std::endl;
 172     }
 173   os.close();
 174   }
 175 }
 176 //---------------------------------------------------------------------
 177
 178
 179 //---------------------------------------------------------------------
 180 void clitk::SignalMeanPositionFilter::FitEllipse(clitk::Ellipse & An) {
 181   int n = 0;
 182   mCurrentResidual.clear();
 183   while (n<mMaxIteration) {
 184     double r = An.EllipseFittingNextIteration();
 185     mCurrentResidual.push_back(r);
 186     n++;
 187     if (mVerboseIteration) {
 188       std::cout.precision(3);
 189       std::cout << "A(" << n << ")=" << An
 190                 << " c="  << An.ComputeCenter()
 191                 << " ax=" << An.ComputeSemiAxeLengths()
 192                 << " theta=" << rad2deg(An.ComputeAngleInRad());
 193       std::cout.precision(10);
 194       std::cout << " R=" << r << std::endl;
 195     }
 196   }
 197 }
 198 //---------------------------------------------------------------------
 199
 200
 201 //---------------------------------------------------------------------
 202 void clitk::SignalMeanPositionFilter::AdaptiveFitEllipse(clitk::Ellipse & An) {
 203   // Compute the total number of iteration :
 204   //     = nb of inputs minus the delay, minus the windowL, minus the first one
 205   int maxN = std::min((unsigned int)args_info.t_arg,
 206                       mInput.size()-mWindowLength-mAugmentationDelay);
 207   for(int t=0; t<maxN; t++) {
 208     An.UpdateSMatrix(t, mWindowLength, mAugmentedInputX, mAugmentedInputY);
 209     FitEllipse(An);
 210     clitk::Ellipse * B = new clitk::Ellipse(An);
 211     mListOfEllipses.push_back(B);
 212   }
 213 }
 214 //---------------------------------------------------------------------
 215
 216
 217 //---------------------------------------------------------------------
 218 void clitk::SignalMeanPositionFilter::ComputeIsoPhase(std::vector<clitk::Ellipse*> & l,
 219                                                       std::vector<double> & phase,
 220                                                       std::vector<int> & cycles) {
 221   double refphaseangle=0;
 222   double previousangle=0;
 223   phase.resize(mListOfEllipses.size());
 224
 225   // DD(mListOfEllipses.size());
 226   mIsoPhaseIndex.resize(mListOfEllipses.size());
 227   // mIsoPhaseDelta.resize(mNumberOfIsoPhase);
 228   //   mIsoPhaseDeltaNb.resize(mNumberOfIsoPhase);
 229   mIsoPhaseRefAngle.resize(mNumberOfIsoPhase);
 230
 231   for (unsigned int i=0; i<mListOfEllipses.size(); i++) {
 232     clitk::Ellipse & An = *mListOfEllipses[i];
 233
 234     // Current time accordint to initial input signal
 235     // int time = i+mAugmentationDelay+mWindowLength;   // not use yet
 236
 237     // Compute current signed angle
 238     Vector2d x1(An.ComputeCenter());
 239     double a = mListOfEllipses[0]->ComputeSemiAxeLengths()[0];
 240     double theta = mListOfEllipses[0]->ComputeAngleInRad();
 241     Vector2d x2; x2[0] = x1[0]+a * cos(theta); x2[1] = x1[1]+a * sin(theta);
 242     Vector2d x3(x1);
 243     Vector2d x4; x4[0] = mAugmentedInputX[i+mWindowLength]; x4[1] = mAugmentedInputY[i+mWindowLength];
 244     Vector2d A(x2-x1);
 245     Vector2d B(x4-x3);
 246     double signed_angle = atan2(B[1], B[0]) - atan2(A[1], A[0]);
 247     // double signed_angle = atan2(B[1], B[0]) - atan2(0, 1);
 248     if (signed_angle<0) signed_angle = 2*M_PI+signed_angle;
 249     // http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm
 250
 251     // First time : set the angle
 252     if (i==0) {
 253       refphaseangle = signed_angle;
 254       for(int a=0; a<mNumberOfIsoPhase; a++) {
 255         if (a==0) mIsoPhaseRefAngle[a] = signed_angle;
 256         else mIsoPhaseRefAngle[a] = (signed_angle
 257                                      + (2*M_PI)*a/mNumberOfIsoPhase);
 258         if (mIsoPhaseRefAngle[a] > 2*M_PI) mIsoPhaseRefAngle[a] -= 2*M_PI;
 259         if (mIsoPhaseRefAngle[a] < 0) mIsoPhaseRefAngle[a] = 2*M_PI-mIsoPhaseRefAngle[a];
 260         // DD(rad2deg(mIsoPhaseRefAngle[a]));
 261         // mIsoPhaseDelta[a] = 0.0;
 262         // mIsoPhaseDeltaNb[a] = 0;
 263       }
 264       int a=0;
 265       // DD(rad2deg(signed_angle));
 266       while ((a<mNumberOfIsoPhase) && (signed_angle>=mIsoPhaseRefAngle[a])) { a++; }
 267       // DD(a);
 268       mIsoPhaseIndex[i] = a-1;
 269       // DD(mIsoPhaseIndex[0]);
 270       cycles.push_back(0);
 271     }
 272     else {
 273       mIsoPhaseIndex[i] = mIsoPhaseIndex[i-1];
 274
 275       // Check if angle cross a ref angle
 276       for(int a=0; a<mNumberOfIsoPhase; a++) {
 277         std::cout << "a=" << rad2deg(signed_angle) << " p=" << rad2deg(previousangle)
 278                   << " ref=" << rad2deg(mIsoPhaseRefAngle[a]) << std::endl;
 279         if (
 280             (((signed_angle > mIsoPhaseRefAngle[a]) && (previousangle < mIsoPhaseRefAngle[a]))) ||
 281             ((mIsoPhaseRefAngle[a]==0) && (signed_angle < previousangle)))
 282           {
 283             // if (mValidationWithRealPhase) {
 284             //             mIsoPhaseDelta[a] += mInputPhase[time];
 285             //             mIsoPhaseDeltaNb[a]++;
 286             //           }
 287             mIsoPhaseIndex[i] = a;
 288             // DD(a);
 289             cycles.push_back(i);
 290           }
 291       }
 292       // DD(mIsoPhaseIndex[i]);
 293     }
 294
 295     previousangle = signed_angle;
 296   }
 297
 298   /*
 299   if (mValidationWithRealPhase) {
 300     // Mean of all deltas
 301     for(unsigned int a=0; a<mIsoPhaseDelta.size(); a++) {
 302       if (mIsoPhaseDelta[a] == 0) {
 303         std::cerr << "Error : less than one cyle ?" << std::endl;
 304         exit(0);
 305       }
 306       mIsoPhaseDelta[a] /= mIsoPhaseDeltaNb[a];
 307       DD(mIsoPhaseDelta[a]);
 308     }
 309     std::ofstream os;
 310     openFileForWriting(os, "delta.sig");
 311     for(unsigned int a=0; a<mIsoPhaseDelta.size(); a++) {
 312       os << mIsoPhaseDelta[a] << std::endl;
 313     }
 314     os.close();
 315   }
 316   */
 317
 318   /*
 319   if (mUseLearnedDeltaPhase) {
 320     for(unsigned int a=0; a<mIsoPhaseDelta.size(); a++) {
 321       mIsoPhaseDelta[a] = mLearnIsoPhaseDelta[a];
 322       DD(mIsoPhaseDelta[a]);
 323     }
 324   }
 325   */
 326
 327   // DEBUG UNUSED
 328   /*
 329   int j=0;
 330   for (unsigned int i=0; i<cycles.size(); i++) {
 331     DD(cycles[i]);
 332     int n;
 333     if (i!=0) {
 334       n=cycles[i]-cycles[i-1];
 335       for(int index=0; index<n; index++) {
 336
 337         int indexOfPhase1 = mIsoPhaseIndex[cycles[i-1]];
 338         int indexOfPhase2 = mIsoPhaseIndex[cycles[i]];
 339         DD(indexOfPhase1);
 340         DD(indexOfPhase2);
 341         double ph1 = mIsoPhaseDelta[indexOfPhase1];
 342         double ph2 = mIsoPhaseDelta[indexOfPhase2];
 343         DD(ph1);
 344         DD(ph2);
 345
 346         phase[j] = (ph1+(double)index*(ph2-ph1)/(double)n);
 347         if (phase[j]>1.0) phase[j] = 1.0-phase[j];
 348         DD(phase[j]);
 349         j++;
 350       }
 351     }
 352     else j+=cycles[0];
 353   }
 354   */
 355   for (unsigned int i=0; i<cycles.size(); i++) {
 356     DD(cycles[i]);
 357   }
 358
 359 }
 360 //---------------------------------------------------------------------
 361