From 6b0d92d4e7580b5ba3d316aa90befd9464050d7f Mon Sep 17 00:00:00 2001
From: dsarrut <dsarrut>
Date: Wed, 3 Mar 2010 12:41:27 +0000
Subject: [PATCH] - add clitkSignalApparentMotionTracking

---
 tools/CMakeLists.txt                          |   3 +
 tools/clitkSignalApparentMotionTracking.ggo   |  38 ++
 ...litkSignalApparentMotionTrackingFilter.cxx | 419 ++++++++++++++++++
 .../clitkSignalApparentMotionTrackingFilter.h |  77 ++++
 tools/clitkTrajectory2D.cxx                   | 172 +++++++
 tools/clitkTrajectory2D.h                     |  80 ++++
 6 files changed, 789 insertions(+)
 create mode 100644 tools/clitkSignalApparentMotionTracking.ggo
 create mode 100644 tools/clitkSignalApparentMotionTrackingFilter.cxx
 create mode 100644 tools/clitkSignalApparentMotionTrackingFilter.h
 create mode 100644 tools/clitkTrajectory2D.cxx
 create mode 100644 tools/clitkTrajectory2D.h

diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt
index 0f1962d..c7a8ede 100644
--- a/tools/CMakeLists.txt
+++ b/tools/CMakeLists.txt
@@ -70,6 +70,9 @@ TARGET_LINK_LIBRARIES(clitkAffineTransform clitkCommon ITKIO  clitkFilters)
 ADD_EXECUTABLE(clitkSignalMeanPositionTracking clitkSignalMeanPositionTracking.cxx clitkSignalMeanPositionFilter.cxx clitkEllipse.cxx clitkSignalMeanPositionTracking_ggo.c)
 TARGET_LINK_LIBRARIES(clitkSignalMeanPositionTracking clitkCommon ITKIO )
 
+ADD_EXECUTABLE(clitkSignalApparentMotionTracking clitkSignalApparentMotionTracking.cxx clitkTrajectory2D.cxx clitkSignalApparentMotionTrackingFilter.cxx clitkEllipse.cxx clitkSignalApparentMotionTracking_ggo.c)
+TARGET_LINK_LIBRARIES(clitkSignalApparentMotionTracking clitkCommon ITKIO fftw3)
+
 ADD_EXECUTABLE(clitkSignalFilter clitkSignalFilter.cxx clitkSignalFilter_ggo.c)
 TARGET_LINK_LIBRARIES(clitkSignalFilter clitkCommon ITKIO ) 
 
diff --git a/tools/clitkSignalApparentMotionTracking.ggo b/tools/clitkSignalApparentMotionTracking.ggo
new file mode 100644
index 0000000..0608d04
--- /dev/null
+++ b/tools/clitkSignalApparentMotionTracking.ggo
@@ -0,0 +1,38 @@
+#File clitkSignalApparentMotionTrackingFilter.ggo
+Package "clitkSignalMeanPositionTracking"
+version "1.0"
+purpose "See Ruan 2008, compute mean position from a signal"
+
+option "config"	  	-	"Config file"                            string  	no
+option "input"		i	"Input trajectory filename"		 string  	yes
+option "ref"		r	"Input reference trajectory filename"	 string  	yes
+
+option "refsampling"  	-  	"Sampling between ref samples"	         double         yes
+option "inputsampling"  -  	"Sampling between input samples"         double         yes
+
+option "refphase"    	p  	"Input phase file"	                 string 	yes
+option "delay"    	d  	"Delay for augmented space"	         int            no default="5"
+option "windowLength"   L  	"Sliding window length for ellipse fit"  int            no default="25"
+option "nbiso"    	n       "Number of iso phase"	                  int           no default="4"
+
+option "output"    	o  	"Output phase (debug)"	  string 	no
+
+
+
+
+option "eta"    	e  	"Convergence param (auto if not given)"	          double no
+option "iter"    	-  	"Max # of iterations"	          int no default="100"
+option "t"    	        t  	"DEBUG"	                          int no default="1000"
+
+option "phase"    	-  	"Input phase file for validation"	  string 	no
+option "delta"    	-  	"Input learn delta phase file"	          string 	no
+
+
+section "Output"
+option "residual"    	-  	"Output optimisation residual filemane"	  string 	no
+option "augmented"    	a  	"Output augmented signal filemane"	  string 	no
+
+section "Verbose"
+option "verbose"   	        v   	"Verbose"			  flag		off
+option "verbose_iteration"   	-   	"Verbose each iteration"	  flag		off
+
diff --git a/tools/clitkSignalApparentMotionTrackingFilter.cxx b/tools/clitkSignalApparentMotionTrackingFilter.cxx
new file mode 100644
index 0000000..24a0b1c
--- /dev/null
+++ b/tools/clitkSignalApparentMotionTrackingFilter.cxx
@@ -0,0 +1,419 @@
+/*=========================================================================
+                                                                                
+  Program:   clitk
+  Module:    $RCSfile: clitkSignalApparentMotionTrackingFilter.cxx,v $
+  Language:  C++
+  Date:      $Date: 2010/03/03 12:41:27 $
+  Version:   $Revision: 1.1 $
+                                                                                
+  Copyright (c) CREATIS (Centre de Recherche et d'Applications en Traitement de
+  l'Image). All rights reserved. See Doc/License.txt or
+  http://www.creatis.insa-lyon.fr/Public/Gdcm/License.html for details.
+                                                                                
+     This software is distributed WITHOUT ANY WARRANTY; without even
+     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+     PURPOSE.  See the above copyright notices for more information.
+                                                                             
+=========================================================================*/
+
+#include "clitkSignalApparentMotionTrackingFilter.h"
+#include <limits>
+#include <fstream>
+
+//---------------------------------------------------------------------
+void clitk::SignalApparentMotionTrackingFilter::SetParameters(args_info_clitkSignalApparentMotionTracking & a) {
+  args_info = a;
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+void clitk::SignalApparentMotionTrackingFilter::ComputeMeanAndIsoPhase(clitk::Signal & s, 
+                                                                       int delay, 
+                                                                       int L,
+                                                                       int nbIsoPhase, 
+                                                                       clitk::Signal & mean, 
+                                                                       clitk::Signal & isoPhase) {
+  int mMaxIteration = 1;
+
+  // Compute augmented signal
+  clitk::Signal sx;
+  clitk::Signal sy;
+  s.ComputeAugmentedSpace(sx, sy, delay);
+  
+  // DEBUG augmented space
+  static int tt=0;
+  ofstream os;
+  if (tt==0) os.open("augmentedU.sig");
+  else os.open("augmentedV.sig");
+  tt++;
+  for(uint i=0; i<sx.size(); i++) {
+    os << i << " " << sx[i] << " " << sy[i] << std::endl;
+  }
+  os.close();
+
+  // Compute starting ellipse
+  Ellipse An;
+  An.InitialiseEllipseFitting(-1, L, sx, sy); // -1 is auto eta
+  DD(An.ComputeSemiAxeLengths());
+  
+  // Fit successive ellipses
+  std::vector<clitk::Ellipse*> e;
+  int n = s.size()-L-delay;
+  for(int t=0; t<n; t++) {
+    // An.InitialiseEllipseFitting(-1, L, sx, sy); // -1 is auto eta
+    An.UpdateSMatrix(t, L, sx, sy);
+    // DD(An.GetEta());
+//     DD(An);
+//     DD(An.ComputeSemiAxeLengths());
+    int nn = 0;
+    while (nn<mMaxIteration) {
+      An.EllipseFittingNextIteration();
+      // DD(nn);
+//       DD(An);
+//       DD(An.ComputeSemiAxeLengths());
+      nn++;
+    }
+    clitk::Ellipse * B = new clitk::Ellipse(An);
+    e.push_back(B);
+    DD(t);
+    DD(B->ComputeCenter());
+//      DD(B->ComputeSemiAxeLengths());
+  }
+
+  // Get mean and isoPhase
+  //mean.resize(n);
+  isoPhase.resize(n);
+  ComputeIsoPhaseFromListOfEllipses(e, sx, sy, nbIsoPhase, delay, L, isoPhase);
+  for(unsigned int i=0; i<e.size(); i++) {
+    clitk::Ellipse & An = *e[i];
+    mean[i+delay+L] = An.ComputeCenter()[0];
+    //    DD(An.ComputeCenter());
+  }
+  DD("------------------------");
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+void clitk::SignalApparentMotionTrackingFilter::ComputeMeanAndIsoPhase(clitk::Trajectory2D & s, 
+                                                                       int delay, 
+                                                                       int L,
+                                                                       int nbIsoPhase, 
+                                                                       clitk::Trajectory2D & mean, 
+                                                                       std::vector<int> & isoPhase) {
+  // Extract signal (slow I know)
+  clitk::Signal U(s.GetU());
+  clitk::Signal V(s.GetV());
+  
+  clitk::Signal & meanU = mean.GetU();
+  clitk::Signal & meanV = mean.GetV();
+
+  meanU.resize(s.size());
+  meanV.resize(s.size());
+
+  clitk::Signal isoPhaseU;
+  clitk::Signal isoPhaseV;
+  
+  DD(U.size());
+  DD(V.size());
+  
+  // Compute mean and iso independently
+  ComputeMeanAndIsoPhase(U, delay, L, nbIsoPhase, meanU, isoPhaseU);
+  ComputeMeanAndIsoPhase(V, delay, L, nbIsoPhase, meanV, isoPhaseV);
+
+  // Check isphase ??? ???
+  isoPhase.resize(s.size());
+  for(unsigned int i=0; i<isoPhaseU.size(); i++) {
+
+    // ********************* U OR V ??????????
+
+    isoPhase[i+delay+L] = isoPhaseV[i]; 
+    //isoPhase[i+delay+L] = isoPhaseU[i]; 
+
+    //    mean[i+delay+L] = mean[i];
+    //DD(isoPhase[i]);
+  }
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+void clitk::SignalApparentMotionTrackingFilter::ReadDB(std::string filename, 
+                                                       std::vector<Trajectory2D> & bd) {
+  // File format : 2 columns by point (U,V), time by line
+  std::ifstream is(filename.c_str());
+  skipComment(is);
+  std::string line;
+  std::getline(is, line);
+  
+  // Compute nb of column in this first line
+  std::istringstream iss(line);
+  std::string s;
+  int nb = 0;
+  while (iss>>s) {
+    nb++;
+  }
+  DD(nb);
+
+  if (nb%2 == 1) {
+    std::cout << "ERROR in the file '" << filename << "', I read " << nb 
+              << " columns, which is not odd." << std::endl;
+  }
+  nb = nb/2;
+  DD(nb);
+
+  // Read trajectories
+  bd.resize(nb);
+  while (is) {
+    std::istringstream iss(line);
+    std::string s;
+    int i=0;
+    while (iss>>s) {
+      double u = atof(s.c_str());
+      iss >> s;
+      double v = atof(s.c_str());
+      bd[i].push_back(u,v);
+      i++;
+    }
+    std::getline(is, line);
+  }
+  DD(bd[0].size());
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+void clitk::SignalApparentMotionTrackingFilter::Update() {
+
+  // -----------------------------------------------------
+  // Read and build initial DB
+  ReadDB(args_info.ref_arg, mReferenceTrajectoriesBD);
+  mReferencePhase.Read(args_info.refphase_arg);
+  mReferencePhase.SetSamplingPeriod(args_info.refsampling_arg);
+
+  DD(mReferencePhase.GetSamplingPeriod());
+  DD(mReferencePhase.size());  
+  DD(mReferenceTrajectoriesBD.size());
+
+  mReferenceMean.resize(mReferenceTrajectoriesBD.size());
+  for(unsigned int i=0; i<mReferenceMean.size(); i++) {
+    mReferenceTrajectoriesBD[i].GetMean(mReferenceMean[i]);
+    // DD(mReferenceMean[i]);
+  }
+  
+  for(uint i=0; i<mReferenceTrajectoriesBD.size(); i++)
+    mReferenceTrajectoriesBD[i].SetSamplingPeriod(mReferencePhase.GetSamplingPeriod());
+
+  // Relative reference motion
+  for(uint i=0; i<mReferenceTrajectoriesBD.size(); i++) {
+    // DD(i);
+    // mReferenceTrajectoriesBD[i].Print();
+    mReferenceTrajectoriesBD[i].Substract(mReferenceMean[i]);
+    // mReferenceTrajectoriesBD[i].Print();
+  }
+
+  // -----------------------------------------------------
+  // Read trajectory
+  mInput.Read(args_info.input_arg);
+  DD(mInput.size());
+  mInput.SetSamplingPeriod(args_info.inputsampling_arg);
+  mInputMean.SetSamplingPeriod(args_info.inputsampling_arg);
+  mInputRelative.SetSamplingPeriod(args_info.inputsampling_arg);
+  DD(mInput.GetSamplingPeriod());
+
+  // -----------------------------------------------------
+  // Compute input mean and isophase
+  std::cout << "Compute Mean and IsoPhase" << std::endl;
+  std::vector<int> isophase;
+  double d = args_info.delay_arg;
+  int L = args_info.windowLength_arg;
+  int nbiso = args_info.nbiso_arg;
+  isophase.resize(mInput.size());
+  ComputeMeanAndIsoPhase(mInput, d, L, nbiso, mInputMean, isophase);
+  
+  // -----------------------------------------------------
+  // Substract mean to trajectory
+  std::cout << "Build relative traj" << std::endl;
+  mInputRelative.resize(mInput.size());
+  for(int i=0; i<mInput.size(); i++) {
+     // DD(i);
+//      DD(mInput.GetU(i));
+//      DD(mInput.GetV(i));
+//      DD(mInputMean.GetU(i));
+//      DD(mInputMean.GetV(i));
+     mInputRelative.SetPoint(i, mInput.GetU(i) - mInputMean.GetU(i),
+                            mInput.GetV(i) - mInputMean.GetV(i));
+    // DD(mInputRelative.GetU(i));
+//     DD(mInputRelative.GetV(i));
+  }
+
+  // DEBUG : output mean
+  ofstream osm("mean-phase.sig");
+  for(int i=0; i<mInput.size(); i++) {
+    osm << mInputMean.GetU(i) << " " << mInputMean.GetV(i) << " " << isophase[i] << std::endl;
+  }
+  osm.close();
+
+  // -----------------------------------------------------
+  // Loop on each iso-phase segment
+  std::cout << "Loop on each isophase" << std::endl;
+  clitk::Signal mEstimatedPhase;
+  mEstimatedPhase.resize(mInput.size());
+
+  std::vector<int> isophaseIndex;
+  int previous = isophase[0];
+  for(uint i=0; i<isophase.size(); i++) {
+    if (isophase[i] != previous) {
+      isophaseIndex.push_back(i);
+      previous = isophase[i];
+    }
+  }
+  DDV(isophaseIndex, isophaseIndex.size());
+
+  for(uint ips=0; ips<isophaseIndex.size()-1; ips++) {  
+    DD(ips);
+    int begin = isophaseIndex[ips];
+    int end = isophaseIndex[ips+1];
+    DD(begin);
+    DD(end);
+
+
+    std::cout << "-----------------------------------------------" << std::endl;
+    std::cout << "-----------------------------------------------" << std::endl;
+    std::cout << "Find traj from " << begin << " to " << end << std::endl;
+    mInput.Print("mInput", begin, end);
+    mInputRelative.Print("mInputRelativ", begin, end);
+    
+
+    // Get current mean position
+    Vector2d currentMeanPosition;
+    mInputMean.GetPoint(begin, currentMeanPosition);
+    DD(currentMeanPosition);
+
+    // Find closest ref trajectory
+    int index = FindClosestTrajectory(mReferenceMean, currentMeanPosition);
+    DD(index);
+    clitk::Trajectory2D & currentClosestReferenceTrajectory = mReferenceTrajectoriesBD[index];
+    
+    DD(mReferenceMean[index]);
+    currentClosestReferenceTrajectory.Print("currentClosestReferenceTrajectory");
+
+    // Estimate phase delta
+    clitk::Signal tempPhase;
+    EstimatePhase(mInputRelative, begin, end, 
+                  currentClosestReferenceTrajectory, 
+                  mReferencePhase, tempPhase);
+
+    // Cat current phase
+    int n=0;
+    for(int i=begin; i<end; i++) {
+      mEstimatedPhase[i] = tempPhase[n];
+      n++;
+    }
+  }
+  
+  // -----------------------------------------------------
+  // Output time - phase
+  // DEBUG OUTPUT
+  ofstream os(args_info.output_arg);
+  for(int t=0; t<mInput.size(); t++) {
+    os << t << " " << t*mInput.GetSamplingPeriod() << " " << mEstimatedPhase[t] << std::endl;
+  }
+  os.close();
+
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+void clitk::SignalApparentMotionTrackingFilter::EstimatePhase(const clitk::Trajectory2D & input, 
+                                                              const int begin, const int end, 
+                                                              const clitk::Trajectory2D & ref, 
+                                                              const clitk::Signal & phaseref, 
+                                                              clitk::Signal & phase) {
+
+  // Create time-warped resampled trajectory
+  clitk::Trajectory2D T;
+  T.SetSamplingPeriod(ref.GetSamplingPeriod());
+  DD(input.size());
+  input.ResampleWithTimeWarpTo(begin, end, T);
+  DD(input.GetSamplingPeriod());
+  DD(ref.GetSamplingPeriod());
+  ref.Print("ref");
+  input.Print("input", begin, end);
+  T.Print("T");
+
+  // Find optimal delta
+  int delta = FindPhaseDelta(T, ref);
+  DD(delta);
+
+  // Shift phase
+  clitk::Signal temp(phaseref);
+  DD(temp.GetSamplingPeriod());
+  DDV(temp, temp.size());
+  temp.Shift(delta, (int)lrint((end-begin)*input.GetSamplingPeriod()/phaseref.GetSamplingPeriod()));
+  DD(temp.GetSamplingPeriod());
+  DDV(temp, temp.size());
+  
+
+  // Output result with time-unwarp
+  phase.resize(end-begin);
+  phase.SetSamplingPeriod(input.GetSamplingPeriod());
+  DD(phase.GetSamplingPeriod());
+  DD(phaseref.GetSamplingPeriod());
+  DD(temp.GetSamplingPeriod());
+  DD(temp.size());
+  DD(phase.size());
+  temp.ResampleWithTimeWarpTo(phase, true); // linear
+  DDV(phase, phase.size());
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+int clitk::SignalApparentMotionTrackingFilter::FindPhaseDelta(const clitk::Trajectory2D & A, 
+                                                                 const clitk::Trajectory2D & B) const {
+  assert(A.GetSamplingPeriod() == B.GetSamplingPeriod());
+  
+  DD("A");
+  A.Print("A");
+  DD("B");
+  B.Print("B");
+
+  // exhaustive search
+  double min=numeric_limits<double>::max();
+  int imin =0;
+  for(int i=0; i<B.size(); i++) {
+    double d = A.DistanceTo(i, B);
+    std::cout.precision(10);
+    std::cout << "i=" << i << " d=" << d << " (min=" << min << " imin=" << imin << ")" << std::endl;
+    if (d < min) {
+      imin = i;
+      min = d;
+    }
+  }
+  DD(imin);
+  return imin;
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+int clitk::SignalApparentMotionTrackingFilter::FindClosestTrajectory(const std::vector<Vector2d> & meanlist, 
+                                                                     const Vector2d & mean) {
+  // Brute force for the moment
+  double dmin = numeric_limits<double>::max();
+  double imin=0;
+  for(uint i=0; i<meanlist.size(); i++) {
+    double d = (meanlist[i]-mean).GetNorm();
+    if (d< dmin) {
+      dmin = d;
+      imin = i;
+    }
+  }
+  DD(dmin);
+  DD(imin);
+  return imin;
+}
+//---------------------------------------------------------------------
diff --git a/tools/clitkSignalApparentMotionTrackingFilter.h b/tools/clitkSignalApparentMotionTrackingFilter.h
new file mode 100644
index 0000000..0d9cb30
--- /dev/null
+++ b/tools/clitkSignalApparentMotionTrackingFilter.h
@@ -0,0 +1,77 @@
+/*=========================================================================
+                                                                                
+  Program:   clitk
+  Module:    $RCSfile: clitkSignalApparentMotionTrackingFilter.h,v $
+  Language:  C++
+  Date:      $Date: 2010/03/03 12:41:27 $
+  Version:   $Revision: 1.1 $
+                                                                                
+  Copyright (c) CREATIS (Centre de Recherche et d'Applications en Traitement de
+  l'Image). All rights reserved. See Doc/License.txt or
+  http://www.creatis.insa-lyon.fr/Public/Gdcm/License.html for details.
+                                                                                
+     This software is distributed WITHOUT ANY WARRANTY; without even
+     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+     PURPOSE.  See the above copyright notices for more information.
+                                                                             
+=========================================================================*/
+
+#ifndef SIGNALAPPARENTMOTIONTRACKINGFILTER_H
+#define SIGNALAPPARENTMOTIONTRACKINGFILTER_H
+
+#include "clitkSignalApparentMotionTracking_ggo.h"
+#include "clitkSignal.h"
+#include "clitkEllipse.h"
+#include "clitkTrajectory2D.h"
+#include "itkVector.h"
+#include <math.h>
+
+namespace clitk {
+
+  //---------------------------------------------------------------------
+  class SignalApparentMotionTrackingFilter {
+  public:
+    typedef itk::Vector<double,2> Vector2d;
+
+    void SetParameters(args_info_clitkSignalApparentMotionTracking & args_info);
+    void Update();
+    
+  protected:    
+    args_info_clitkSignalApparentMotionTracking args_info;
+    clitk::Signal mRefU;
+    clitk::Signal mRefV;
+    clitk::Signal mRefPhase;
+    clitk::Signal mInputU;
+    clitk::Signal mInputV;
+    clitk::Signal mMeanU;
+    clitk::Signal mMeanV;
+    clitk::Signal mIsoPhaseU;
+    clitk::Signal mIsoPhaseV;
+    double mRefSampling;
+
+    std::vector<Trajectory2D> mReferenceTrajectoriesBD;
+    clitk::Signal mReferencePhase;
+    clitk::Trajectory2D mInput;
+    clitk::Trajectory2D mInputRelative;
+    clitk::Trajectory2D mInputMean;
+    std::vector<Vector2d> mReferenceMean;
+
+    void ComputeMeanAndIsoPhase(clitk::Signal & s, int delay, int L, int nbIso,
+                                clitk::Signal & mean, clitk::Signal & isoPhase);
+    void ComputeMeanAndIsoPhase(clitk::Trajectory2D & s, int delay, int L,
+                                int nbIsoPhase, clitk::Trajectory2D & mean, 
+                                std::vector<int> & isoPhase);
+    void EstimatePhase(const clitk::Trajectory2D & input, 
+                       const int begin, const int end, 
+                       const clitk::Trajectory2D & ref, 
+                       const clitk::Signal & phaseref, 
+                       clitk::Signal & phase);
+    int FindPhaseDelta(const clitk::Trajectory2D & A, const clitk::Trajectory2D & B) const;
+    int FindClosestTrajectory(const std::vector<Vector2d> & meanlist, const Vector2d & mean);
+    void ReadDB(std::string filename, std::vector<Trajectory2D> & bd);
+  };
+  //---------------------------------------------------------------------
+  
+} // end namespace
+
+#endif
diff --git a/tools/clitkTrajectory2D.cxx b/tools/clitkTrajectory2D.cxx
new file mode 100644
index 0000000..8ee6a71
--- /dev/null
+++ b/tools/clitkTrajectory2D.cxx
@@ -0,0 +1,172 @@
+/*=========================================================================
+                                                                                
+  Program:   clitk
+  Module:    $RCSfile: clitkTrajectory2D.cxx,v $
+  Language:  C++
+  Date:      $Date: 2010/03/03 12:41:27 $
+  Version:   $Revision: 1.1 $
+                                                                                
+  Copyright (c) CREATIS (Centre de Recherche et d'Applications en Traitement de
+  l'Image). All rights reserved. See Doc/License.txt or
+  http://www.creatis.insa-lyon.fr/Public/Gdcm/License.html for details.
+                                                                                
+     This software is distributed WITHOUT ANY WARRANTY; without even
+     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+     PURPOSE.  See the above copyright notices for more information.
+                                                                             
+=========================================================================*/
+
+#include "clitkTrajectory2D.h"
+
+//---------------------------------------------------------------------
+void clitk::Trajectory2D::ResampleWithTimeWarpTo(int begin, int end, Trajectory2D & output) const {
+  DD(begin);
+  DD(end);
+  int n = (int)lrint((end-begin)*GetSamplingPeriod()/output.GetSamplingPeriod());
+  output.resize(n);
+  DD(n);
+  double duration = (end-begin)*GetSamplingPeriod();
+  DD(duration);
+  double sp = output.GetSamplingPeriod();
+  DD(output.GetTotalTimeDuration());
+  //   DD(sp);
+  for(int i=0; i<n; i++) {
+    Vector2d p;
+    // DD(i);
+    GetValueAtLin((i*sp)+(begin*GetSamplingPeriod()), p);
+    // DD(p);
+    output.SetPoint(i, p);
+  }
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+bool clitk::Trajectory2D::Read(std::string filename) {
+  bool b = mU.Read(filename, 0);
+  if (!b) return false;
+  b = mV.Read(filename, 1);
+  return b;
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+void clitk::Trajectory2D::GetValueAtLin(double t, Vector2d & p) const {
+  // Can be faster, I know ...
+  p[0] = mU.GetValueAtLin(t);
+  p[1] = mV.GetValueAtLin(t);
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+void clitk::Trajectory2D::ResampleWithTimeWarpTo(Trajectory2D & output) const {
+  ResampleWithTimeWarpTo(0, size(), output);
+}
+//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------        
+void clitk::Trajectory2D::Shift(double s) {
+  std::cout << "Shift s=" << s << std::endl;
+  int d = (int)lrint(s/GetSamplingPeriod()); // closest integer delta
+  DD(d);
+  clitk::Signal tempU((uint)size());
+  clitk::Signal tempV((uint)size());
+  for(int i=0; i<size(); i++) {
+    tempU[0] = mU[(i+d)%size()];
+    tempV[0] = mV[(i+d)%size()];
+  }
+  for(int i=0; i<size(); i++) {
+    mU[i] = tempU[i];
+    mV[i] = tempV[i];
+  }
+}
+//---------------------------------------------------------------------        
+
+
+//---------------------------------------------------------------------        
+void clitk::Trajectory2D::GetPoint(const int index, Vector2d & p) {
+  p[0] = mU[index];
+  p[1] = mV[index];
+}
+//---------------------------------------------------------------------        
+
+
+//---------------------------------------------------------------------        
+clitk::Trajectory2D & clitk::Trajectory2D::operator+(Trajectory2D & d) {
+  for(int i=0; i<size(); i++) {
+    mU[i] += d.GetU(i);
+    mV[i] += d.GetV(i);
+  }
+  return  *this;
+}
+//---------------------------------------------------------------------        
+
+
+//---------------------------------------------------------------------        
+clitk::Trajectory2D & clitk::Trajectory2D::operator-(Trajectory2D & d) {
+  for(int i=0; i<size(); i++) {
+    mU[i] -= d.GetU(i);
+    mV[i] -= d.GetV(i);
+  }
+  return  *this;
+}
+//---------------------------------------------------------------------        
+
+
+//---------------------------------------------------------------------        
+void clitk::Trajectory2D::GetMean(Vector2d & m) {
+  m[0] = 0.0;
+  m[1] = 0.0;
+  for(int i=0; i<size(); i++) {
+    m[0] += mU[i];
+    m[1] += mV[i];
+  }
+  m[0] /= size();
+  m[1] /= size();
+}
+//---------------------------------------------------------------------        
+
+
+//---------------------------------------------------------------------        
+double clitk::Trajectory2D::DistanceTo(int delta, const clitk::Trajectory2D & B) const {
+  double d = 0.0;
+  for(int n=0; n<size(); n++) {
+    int i = n;//(n)%size();
+    int j = (n+delta)%B.size();
+    // DD(j);
+//     DD(i);
+//     DD(n);
+    d += pow(mU[i] - B.GetU(j), 2) + pow(mV[i] - B.GetV(j), 2);
+  }
+  return d;
+}
+//---------------------------------------------------------------------        
+
+
+//---------------------------------------------------------------------        
+void clitk::Trajectory2D::Print(const std::string & name) const {
+  Print(name, 0, size());
+}
+//---------------------------------------------------------------------        
+
+
+//---------------------------------------------------------------------        
+void clitk::Trajectory2D::Print(const std::string & name, int begin, int end) const {
+  std::cout << "Traj " << name << " size = " << size() 
+            << " from " << begin << " to " << end << std ::endl;
+  for(int i=begin; i<end; i++) {
+    std::cout << mU[i] << " " << mV[i] << std::endl;
+  }
+}
+//---------------------------------------------------------------------        
+
+
+//---------------------------------------------------------------------        
+void clitk::Trajectory2D::Substract(const Vector2d & m) {
+  mU.AddValue(-m[0]);
+  mV.AddValue(-m[1]);
+}
+//---------------------------------------------------------------------        
diff --git a/tools/clitkTrajectory2D.h b/tools/clitkTrajectory2D.h
new file mode 100644
index 0000000..f5fdef9
--- /dev/null
+++ b/tools/clitkTrajectory2D.h
@@ -0,0 +1,80 @@
+/*=========================================================================
+                                                                                
+  Program:   clitk
+  Module:    $RCSfile: clitkTrajectory2D.h,v $
+  Language:  C++
+  Date:      $Date: 2010/03/03 12:41:27 $
+  Version:   $Revision: 1.1 $
+                                                                                
+  Copyright (c) CREATIS (Centre de Recherche et d'Applications en Traitement de
+  l'Image). All rights reserved. See Doc/License.txt or
+  http://www.creatis.insa-lyon.fr/Public/Gdcm/License.html for details.
+                                                                                
+     This software is distributed WITHOUT ANY WARRANTY; without even
+     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+     PURPOSE.  See the above copyright notices for more information.
+                                                                             
+=========================================================================*/
+
+#ifndef CLITKTRAJECTORY2D_H
+#define CLITKTRAJECTORY2D_H
+
+#include "clitkCommon.h"
+#include "clitkSignal.h"
+#include "itkVector.h"
+#include <vnl/algo/vnl_generalized_eigensystem.h>
+#include <vnl/algo/vnl_symmetric_eigensystem.h>
+#include <vnl/algo/vnl_real_eigensystem.h>
+
+namespace clitk {
+
+  //---------------------------------------------------------------------
+  class Trajectory2D {
+  public:
+    typedef itk::Vector<double,2> Vector2d;
+
+    void ResampleWithTimeWarpTo(int begin, int end, Trajectory2D & output) const;
+    void ResampleWithTimeWarpTo(Trajectory2D & output) const; 
+    void Shift(double s);
+    
+    bool Read(std::string filename);
+
+    void GetPoint(const int index, Vector2d & p);
+    void GetMean(Vector2d & m);
+    double GetU(const int index) const { return mU[index]; }
+    double GetV(const int index) const { return mV[index]; }
+    clitk::Signal & GetU() { return mU; }
+    clitk::Signal & GetV() { return mV; }
+    
+    int size() const { return mU.size(); }
+    void resize(int n) { mU.resize(n); mV.resize(n); }
+
+    void SetPoint(const int i, Vector2d & p) { mU[i] = p[0]; mV[i] = p[1]; }
+    void SetPoint(const int i, double u, double v) { mU[i] = u; mV[i] = v; }
+    void GetValueAtLin(double t, Vector2d & p) const;
+
+    double GetSamplingPeriod() const { return mU.GetSamplingPeriod(); }
+    void SetSamplingPeriod(double sp){ mU.SetSamplingPeriod(sp); mV.SetSamplingPeriod(sp); }
+    double DistanceTo(int delta, const clitk::Trajectory2D & B) const;
+
+    Trajectory2D & operator+(Trajectory2D & d);
+    Trajectory2D & operator-(Trajectory2D & d);
+    
+    void Print(const std::string & name) const;
+    void Print(const std::string & name, int begin, int end) const;
+
+    void push_back(double u, double v) { mU.push_back(u); mV.push_back(v); }
+    void Substract(const Vector2d & m);
+    double GetTotalTimeDuration() const { return mU.GetTotalTimeDuration(); }
+
+  protected:
+    clitk::Signal mU;
+    clitk::Signal mV;
+    
+  };
+  //---------------------------------------------------------------------
+
+
+} // end namespace
+
+#endif
-- 
2.45.1