1 /*=========================================================================
4 Module: $RCSfile: clitkSignalApparentMotionTrackingFilter.cxx,v $
6 Date: $Date: 2010/03/03 12:41:27 $
7 Version: $Revision: 1.1 $
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.
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.
17 =========================================================================*/
19 #include "clitkSignalApparentMotionTrackingFilter.h"
23 //---------------------------------------------------------------------
24 void clitk::SignalApparentMotionTrackingFilter::SetParameters(args_info_clitkSignalApparentMotionTracking & a) {
27 //---------------------------------------------------------------------
30 //---------------------------------------------------------------------
31 void clitk::SignalApparentMotionTrackingFilter::ComputeMeanAndIsoPhase(clitk::Signal & s,
36 clitk::Signal & isoPhase) {
37 int mMaxIteration = 1;
39 // Compute augmented signal
42 s.ComputeAugmentedSpace(sx, sy, delay);
44 // DEBUG augmented space
47 if (tt==0) os.open("augmentedU.sig");
48 else os.open("augmentedV.sig");
50 for(uint i=0; i<sx.size(); i++) {
51 os << i << " " << sx[i] << " " << sy[i] << std::endl;
55 // Compute starting ellipse
57 An.InitialiseEllipseFitting(-1, L, sx, sy); // -1 is auto eta
58 DD(An.ComputeSemiAxeLengths());
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);
68 // DD(An.ComputeSemiAxeLengths());
70 while (nn<mMaxIteration) {
71 An.EllipseFittingNextIteration();
74 // DD(An.ComputeSemiAxeLengths());
77 clitk::Ellipse * B = new clitk::Ellipse(An);
80 DD(B->ComputeCenter());
81 // DD(B->ComputeSemiAxeLengths());
84 // Get mean and isoPhase
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());
93 DD("------------------------");
95 //---------------------------------------------------------------------
98 //---------------------------------------------------------------------
99 void clitk::SignalApparentMotionTrackingFilter::ComputeMeanAndIsoPhase(clitk::Trajectory2D & s,
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());
109 clitk::Signal & meanU = mean.GetU();
110 clitk::Signal & meanV = mean.GetV();
112 meanU.resize(s.size());
113 meanV.resize(s.size());
115 clitk::Signal isoPhaseU;
116 clitk::Signal isoPhaseV;
121 // Compute mean and iso independently
122 ComputeMeanAndIsoPhase(U, delay, L, nbIsoPhase, meanU, isoPhaseU);
123 ComputeMeanAndIsoPhase(V, delay, L, nbIsoPhase, meanV, isoPhaseV);
125 // Check isphase ??? ???
126 isoPhase.resize(s.size());
127 for(unsigned int i=0; i<isoPhaseU.size(); i++) {
129 // ********************* U OR V ??????????
131 isoPhase[i+delay+L] = isoPhaseV[i];
132 //isoPhase[i+delay+L] = isoPhaseU[i];
134 // mean[i+delay+L] = mean[i];
138 //---------------------------------------------------------------------
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());
148 std::getline(is, line);
150 // Compute nb of column in this first line
151 std::istringstream iss(line);
160 std::cout << "ERROR in the file '" << filename << "', I read " << nb
161 << " columns, which is not odd." << std::endl;
169 std::istringstream iss(line);
173 double u = atof(s.c_str());
175 double v = atof(s.c_str());
176 bd[i].push_back(u,v);
179 std::getline(is, line);
183 //---------------------------------------------------------------------
186 //---------------------------------------------------------------------
187 void clitk::SignalApparentMotionTrackingFilter::Update() {
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);
195 DD(mReferencePhase.GetSamplingPeriod());
196 DD(mReferencePhase.size());
197 DD(mReferenceTrajectoriesBD.size());
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]);
205 for(uint i=0; i<mReferenceTrajectoriesBD.size(); i++)
206 mReferenceTrajectoriesBD[i].SetSamplingPeriod(mReferencePhase.GetSamplingPeriod());
208 // Relative reference motion
209 for(uint i=0; i<mReferenceTrajectoriesBD.size(); i++) {
211 // mReferenceTrajectoriesBD[i].Print();
212 mReferenceTrajectoriesBD[i].Substract(mReferenceMean[i]);
213 // mReferenceTrajectoriesBD[i].Print();
216 // -----------------------------------------------------
218 mInput.Read(args_info.input_arg);
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());
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);
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++) {
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));
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;
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());
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];
272 DDV(isophaseIndex, isophaseIndex.size());
274 for(uint ips=0; ips<isophaseIndex.size()-1; ips++) {
276 int begin = isophaseIndex[ips];
277 int end = isophaseIndex[ips+1];
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);
289 // Get current mean position
290 Vector2d currentMeanPosition;
291 mInputMean.GetPoint(begin, currentMeanPosition);
292 DD(currentMeanPosition);
294 // Find closest ref trajectory
295 int index = FindClosestTrajectory(mReferenceMean, currentMeanPosition);
297 clitk::Trajectory2D & currentClosestReferenceTrajectory = mReferenceTrajectoriesBD[index];
299 DD(mReferenceMean[index]);
300 currentClosestReferenceTrajectory.Print("currentClosestReferenceTrajectory");
302 // Estimate phase delta
303 clitk::Signal tempPhase;
304 EstimatePhase(mInputRelative, begin, end,
305 currentClosestReferenceTrajectory,
306 mReferencePhase, tempPhase);
310 for(int i=begin; i<end; i++) {
311 mEstimatedPhase[i] = tempPhase[n];
316 // -----------------------------------------------------
317 // Output time - phase
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;
326 //---------------------------------------------------------------------
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) {
336 // Create time-warped resampled trajectory
337 clitk::Trajectory2D T;
338 T.SetSamplingPeriod(ref.GetSamplingPeriod());
340 input.ResampleWithTimeWarpTo(begin, end, T);
341 DD(input.GetSamplingPeriod());
342 DD(ref.GetSamplingPeriod());
344 input.Print("input", begin, end);
347 // Find optimal delta
348 int delta = FindPhaseDelta(T, ref);
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());
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());
368 temp.ResampleWithTimeWarpTo(phase, true); // linear
369 DDV(phase, phase.size());
371 //---------------------------------------------------------------------
374 //---------------------------------------------------------------------
375 int clitk::SignalApparentMotionTrackingFilter::FindPhaseDelta(const clitk::Trajectory2D & A,
376 const clitk::Trajectory2D & B) const {
377 assert(A.GetSamplingPeriod() == B.GetSamplingPeriod());
385 double min=numeric_limits<double>::max();
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;
399 //---------------------------------------------------------------------
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();
408 for(uint i=0; i<meanlist.size(); i++) {
409 double d = (meanlist[i]-mean).GetNorm();
419 //---------------------------------------------------------------------