NorMIT-nav  16.5
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
cxTemporalCalibration.cpp
Go to the documentation of this file.
1 /*=========================================================================
2 This file is part of CustusX, an Image Guided Therapy Application.
3 
4 Copyright (c) 2008-2014, SINTEF Department of Medical Technology
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10 1. Redistributions of source code must retain the above copyright notice,
11  this list of conditions and the following disclaimer.
12 
13 2. Redistributions in binary form must reproduce the above copyright notice,
14  this list of conditions and the following disclaimer in the documentation
15  and/or other materials provided with the distribution.
16 
17 3. Neither the name of the copyright holder nor the names of its contributors
18  may be used to endorse or promote products derived from this software
19  without specific prior written permission.
20 
21 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 =========================================================================*/
32 #include <cxTemporalCalibration.h>
33 
34 
35 #include <QtWidgets>
36 
37 #include <QVBoxLayout>
38 #include "boost/bind.hpp"
39 #include "cxTrackingService.h"
40 #include <vtkPiecewiseFunction.h>
41 #include <vtkPointData.h>
42 #include <vtkImageData.h>
43 #include "cxTypeConversions.h"
44 #include "cxSettings.h"
45 #include "cxUtilHelpers.h"
46 #include "cxVolumeHelpers.h"
47 #include "vtkImageCorrelation.h"
48 #include "cxUSFrameData.h"
49 #include "cxImage.h"
51 #include "cxLogger.h"
52 #include "cxTime.h"
53 #include <vtkImageMask.h>
54 
55 typedef vtkSmartPointer<vtkImageMask> vtkImageMaskPtr;
56 typedef vtkSmartPointer<vtkImageCorrelation> vtkImageCorrelationPtr;
57 
58 namespace cx
59 {
60 
61 typedef unsigned char uchar; // for removing eclipse warnings
62 
63 
64 
74 void correlate(double* x, double* y, double* corr, int maxdelay, int n)
75 {
76  int i, j;
77  double mx, my, sx, sy, sxy, denom, r;
78  int delay;
79 
80  /* Calculate the mean of the two series x[], y[] */
81  mx = 0;
82  my = 0;
83  for (i = 0; i < n; i++)
84  {
85  mx += x[i];
86  my += y[i];
87  }
88  mx /= n;
89  my /= n;
90 
91  /* Calculate the denominator */
92  sx = 0;
93  sy = 0;
94  for (i = 0; i < n; i++)
95  {
96  sx += (x[i] - mx) * (x[i] - mx);
97  sy += (y[i] - my) * (y[i] - my);
98  }
99  denom = sqrt(sx * sy);
100 
101  /* Calculate the correlation series */
102  for (delay = -maxdelay; delay < maxdelay; delay++)
103  {
104  sxy = 0;
105  for (i = 0; i < n; i++)
106  {
107  j = i + delay;
108  if (j < 0 || j >= n)
109  continue;
110  else
111  sxy += (x[i] - mx) * (y[j] - my);
112  }
113  r = sxy / denom;
114  corr[delay+maxdelay] = r;//(sxy/denom+1) * 128;
115 
116  /* r is the correlation coefficient at "delay" */
117 
118  }
119 
120 }
121 
123 {
124  mAddRawToDebug = false;
125  mMask = vtkImageDataPtr();
126 }
127 
128 void TemporalCalibration::selectData(QString filename)
129 {
130  mFilename = filename;
131  mFileData = USReconstructInputData();
132 
133  if (!QFileInfo(filename).exists())
134  return;
135 
136  UsReconstructionFileReader fileReader;
137  mFileData = fileReader.readAllFiles(filename);
138 
139  if (!mFileData.mUsRaw)
140  {
141  reportWarning("Failed to load data from " + filename);
142  return;
143  }
144 
145  report("Temporal Calibration: Successfully loaded data from " + filename);
146 }
147 
149 {
150  mDebugFolder = filename;
151 }
152 
153 void TemporalCalibration::saveDebugFile()
154 {
155  if (mDebugFolder.isEmpty())
156  return;
157 
158  QString dbFilename = mDebugFolder +"/"+ QFileInfo(mFilename).baseName() + "_temporal_calib.txt";
159  QFile file(dbFilename);
160  file.remove();
161 
162  if (!file.open(QIODevice::ReadWrite))
163  {
164  reportError("Failed to write file " + file.fileName() + ".");
165  return;
166  }
167 
168  file.write(qstring_cast(mDebugStream.str()).toLatin1());
169  report("Saved temporal calibration details to " + file.fileName());
170  file.close();
171 }
172 
173 
188 double TemporalCalibration::calibrate(bool* success)
189 {
190  mDebugStream.str("");
191  mDebugStream.clear();
192 
193  if (!mFileData.mUsRaw)
194  {
195  reportWarning("Temporal calib: No data loaded");
196  return 0;
197  }
198  if (mFileData.mPositions.empty())
199  {
200  reportWarning("Temporal calib: Missing tracking data.");
201  return 0;
202  }
203 
204  mDebugStream << "Temporal Calibration " << QDateTime::currentDateTime().toString(timestampSecondsFormatNice()) << std::endl;
205  mDebugStream << "Loaded data: " << mFilename << std::endl;
206  mDebugStream << "=======================================" << std::endl;
207 
208  mProcessedFrames = mFileData.mUsRaw->initializeFrames(std::vector<bool>(1, false)).front();
209 
210  std::vector<double> frameMovement = this->computeProbeMovement();
211 
212  if (!this->checkFrameMovementQuality(frameMovement))
213  {
214  reportError("Failed to detect movement in images. Make sure that the first image is clear and visible.");
215  *success = false;
216  return 0;
217  }
218 
219 
220  std::vector<double> trackingMovement = this->computeTrackingMovement();
221 
222  // set a resolution, resample both tracking and frames to that
223  double resolution = 5; // ms
224 
225  double offset = mFileData.mFrames.front().mTime - mFileData.mPositions.front().mTime;
226  std::vector<double> frameMovementRegular = this->resample(frameMovement, mFileData.mFrames, resolution);
227  std::vector<double> trackingMovementRegular = this->resample(trackingMovement, mFileData.mPositions, resolution);
228 
229  double shift = this->findLSShift(frameMovementRegular, trackingMovementRegular, resolution);
230 
231  double totalShift = offset + shift;
232 
233  mDebugStream << "=======================================" << std::endl;
234  mDebugStream << "Performed temporal calibration:" << std::endl;
235  mDebugStream << "offset = " << offset << ", shift = " << shift << std::endl;
236  mDebugStream << "Total temporal shift tf-tt = " << offset+shift << " ms" << std::endl;
237  mDebugStream << "=======================================" << std::endl;
238 
239  double startTime = mFileData.mPositions.front().mTime;
240  this->writePositions("Tracking", trackingMovement, mFileData.mPositions, startTime);
241  this->writePositions("Frames", frameMovement, mFileData.mFrames, startTime);
242  this->writePositions("Shifted Frames", frameMovement, mFileData.mFrames, startTime + totalShift);
243 
244 
245  this->saveDebugFile();
246  *success = true;
247  return totalShift;
248 }
249 
250 bool TemporalCalibration::checkFrameMovementQuality(std::vector<double> pos)
251 {
252  int count = 0;
253  for (unsigned i=0; i<pos.size(); ++i)
254  if (similar(pos[i], 0))
255  count++;
256 
257  // accept if less than 20% zeros.
258  double error = double(count)/pos.size();
259  if (error > 0.05)
260  reportWarning(QString("Found %1 % zeros in frame movement").arg(error*100));
261  return error < 0.2;
262 }
263 
267 double TemporalCalibration::findLeastSquares(std::vector<double> frames, std::vector<double> tracking, int shift) const
268 {
269  size_t r0 = 0;
270  size_t r1 = frames.size();
271 
272  r0 = std::max<int>(r0, - shift);
273  r1 = std::min<int>(r1, tracking.size() - shift);
274 
275  double value = 0;
276 
277  for (int i=r0; i<r1; ++i)
278  {
279  double core = pow(frames[i] - tracking[i+shift], 2.0);
280  value += core;
281  }
282  value /= (r1-r0);
283  value = sqrt(value);
284  return value;
285 }
286 
292 double TemporalCalibration::findLSShift(std::vector<double> frames, std::vector<double> tracking, double resolution) const
293 {
294  double maxShift = 1000;
295  size_t N = std::min(tracking.size(), frames.size());
296  N = std::min<int>(N, 2*maxShift/resolution); // constrain search to 1 second in each direction
297  std::vector<double> result(N, 0);
298  int W = N/2;
299 
300  for (int i=-W; i<W; ++i)
301  {
302  double rms = this->findLeastSquares(frames, tracking, i);
303  result[i+W] = rms;
304  }
305 
306  int top = std::distance(result.begin(), std::min_element(result.begin(), result.end()));
307  double shift = (W-top) * resolution; // convert to shift in ms.
308 
309  mDebugStream << "=======================================" << std::endl;
310  mDebugStream << "tracking vs frames fit using least squares:" << std::endl;
311  mDebugStream << "Temporal resolution " << resolution << " ms" << std::endl;
312  mDebugStream << "Max shift " << maxShift << " ms" << std::endl;
313  mDebugStream << "#frames=" << frames.size() << ", #tracks=" << tracking.size() << std::endl;
314  mDebugStream << std::endl;
315  mDebugStream << "Frame pos" << "\t" << "Track pos" << "\t" << "RMS(center=" << W << ")" << std::endl;
316  for (size_t x = 0; x < std::min<int>(tracking.size(), frames.size()); ++x)
317  {
318  mDebugStream << frames[x] << "\t" << tracking[x];
319  if (x<N)
320  mDebugStream << "\t" << result[x];
321  mDebugStream << std::endl;
322  }
323 
324  mDebugStream << std::endl;
325  mDebugStream << "minimal index: " << top << ", = shift in ms: " << shift << std::endl;
326  mDebugStream << "=======================================" << std::endl;
327 
328  return shift; // shift frames-tracking: frame = tracking + shift
329 }
330 
336 double TemporalCalibration::findCorrelationShift(std::vector<double> frames, std::vector<double> tracking, double resolution) const
337 {
338  size_t N = std::min(tracking.size(), frames.size());
339  std::vector<double> result(N, 0);
340 
341  correlate(&*frames.begin(), &*tracking.begin(), &*result.begin(), N / 2, N);
342 
343  int top = std::distance(result.begin(), std::max_element(result.begin(), result.end()));
344  double shift = (N/2-top) * resolution; // convert to shift in ms.
345 
346  mDebugStream << "=======================================" << std::endl;
347  mDebugStream << "tracking vs frames correlation:" << std::endl;
348  mDebugStream << "Temporal resolution " << resolution << " ms" << std::endl;
349  mDebugStream << "#frames=" << frames.size() << ", #tracks=" << tracking.size() << std::endl;
350  mDebugStream << std::endl;
351  mDebugStream << "Frame pos" << "\t" << "Track pos" << "\t" << "correlation" << std::endl;
352  for (int x = 0; x < N; ++x)
353  {
354  mDebugStream << frames[x] << "\t" << tracking[x] << "\t" << result[x] << std::endl;
355  }
356 
357  mDebugStream << std::endl;
358  mDebugStream << "corr top: " << top << ", = shift in ms: " << shift << std::endl;
359  mDebugStream << "=======================================" << std::endl;
360 
361  return shift; // shift frames-tracking: frame = tracking + shift
362 }
363 
364 
365 void TemporalCalibration::writePositions(QString title, std::vector<double> pos, std::vector<TimedPosition> time, double shift)
366 {
367  if (pos.size()!=time.size())
368  {
369  std::cout << "size mismatch" << std::endl;
370  return;
371  }
372 
373  mDebugStream << title << std::endl;
374  mDebugStream << "time\t" << "pos\t" << std::endl;
375  for (unsigned i=0; i<time.size(); ++i)
376  {
377  mDebugStream << time[i].mTime - shift << "\t" << pos[i] << std::endl;
378  }
379 }
380 
384 std::vector<double> TemporalCalibration::resample(std::vector<double> shift, std::vector<TimedPosition> time, double resolution)
385 {
386  if (shift.size()!=time.size())
387  {
388  reportError("Assert failure, shift and time different sizes");
389  }
390 
391  vtkPiecewiseFunctionPtr frames = vtkPiecewiseFunctionPtr::New();
392  for (unsigned i=0; i<shift.size(); ++i)
393  {
394  frames->AddPoint(time[i].mTime, shift[i]);
395  }
396  double r0, r1;
397  frames->GetRange(r0, r1);
398  double range = r1-r0;
399  int N_r = range/resolution;
400 
401  std::vector<double> framesRegular;
402  for (int i=0; i<N_r; ++i)
403  framesRegular.push_back(frames->GetValue(r0 + i*resolution));
404 
405  return framesRegular;
406 }
407 
408 std::vector<double> TemporalCalibration::computeTrackingMovement()
409 {
410  std::vector<double> retval;
411  Vector3D e_z(0,0,1);
412  Vector3D origin(0,0,0);
413  double zero = 0;
414  Transform3D prM0t = mFileData.mPositions[0].mPos;
415  Vector3D ez_pr = prM0t.vector(e_z);
416 
417  for (unsigned i=0; i<mFileData.mPositions.size(); ++i)
418  {
419  Transform3D prMt = mFileData.mPositions[i].mPos;
420  Vector3D p_pr = prMt.coord(origin);
421 
422  double val = dot(ez_pr, p_pr);
423 
424  if (retval.empty())
425  zero = val;
426 
427  retval.push_back(val-zero);
428  }
429 
430  if (mAddRawToDebug)
431  {
432  mDebugStream << "=======================================" << std::endl;
433  mDebugStream << "tracking raw data:" << std::endl;
434  mDebugStream << std::endl;
435  mDebugStream << "timestamp" << "\t" << "pos" << std::endl;
436  for (unsigned x = 0; x < mFileData.mPositions.size(); ++x)
437  {
438  mDebugStream << mFileData.mPositions[x].mTime << "\t" << retval[x] << std::endl;
439  }
440  mDebugStream << std::endl;
441  mDebugStream << "=======================================" << std::endl;
442  }
443 
444  return retval;
445 }
446 
450 std::vector<double> TemporalCalibration::computeProbeMovement()
451 {
452  int N_frames = mFileData.mUsRaw->getDimensions()[2];
453 
454  std::vector<double> retval;
455 
456  double maxSingleStep = 5; // assume max 5mm movement per frame
457 // double currentMaxShift;
458  double lastVal = 0;
459 
460  mMask = mFileData.getMask();
461  for (int i=0; i<N_frames; ++i)
462  {
463  double val = this->findCorrelation(mFileData.mUsRaw, 0, i, maxSingleStep, lastVal);
464 // currentMaxShift = fabs(val) + maxSingleStep;
465  lastVal = val;
466  retval.push_back(val);
467  }
468 
469  if (mAddRawToDebug)
470  {
471  mDebugStream << "=======================================" << std::endl;
472  mDebugStream << "frames raw data:" << std::endl;
473  mDebugStream << std::endl;
474  mDebugStream << "timestamp" << "\t" << "shift" << std::endl;
475  for (int x = 0; x < N_frames; ++x)
476  {
477  mDebugStream << mFileData.mFrames[x].mTime << "\t" << retval[x] << std::endl;
478  }
479  mDebugStream << std::endl;
480  mDebugStream << "=======================================" << std::endl;
481  }
482 
483  return retval;
484 }
485 
486 double TemporalCalibration::findCorrelation(USFrameDataPtr data, int frame_a, int frame_b, double maxShift, double lastVal)
487 {
488  int maxShift_pix = maxShift / mFileData.mUsRaw->getSpacing()[1];
489  int lastVal_pix = lastVal / mFileData.mUsRaw->getSpacing()[1];
490 
491  int line_index_x = mFileData.mProbeDefinition.mData.getOrigin_p()[0];
492 
493  int dimY = mFileData.mUsRaw->getDimensions()[1];
494 
495  vtkImageDataPtr line1 = this->extractLine_y(mFileData.mUsRaw, line_index_x, frame_a);
496  vtkImageDataPtr line2 = this->extractLine_y(mFileData.mUsRaw, line_index_x, frame_b);
497 
498  int N = 2*dimY; //result vector allocate space on both sides of zero
499  std::vector<double> result(N, 0);
500  double* line_a = static_cast<double*>(line1->GetScalarPointer());
501  double* line_b = static_cast<double*>(line2->GetScalarPointer());
502  double* line_c = &*result.begin();
503 
504  correlate(line_a, line_b, line_c, N/2, dimY);
505 
506  // use the last found hit as a seed for looking for a local maximum
507  int lastTop = N/2 - lastVal_pix;
508  std::pair<int,int> range(lastTop - maxShift_pix, lastTop+maxShift_pix);
509  range.first = std::max(0, range.first);
510  range.second = std::min(N, range.second);
511 
512  // look for a max in the vicinity of the last hit
513  int top = std::distance(result.begin(), std::max_element(result.begin()+range.first, result.begin()+range.second));
514 
515  double hit = (N/2-top) * mFileData.mUsRaw->getSpacing()[1]; // convert to downwards movement in mm.
516 
517  return hit;
518 }
519 
523 vtkImageDataPtr TemporalCalibration::extractLine_y(USFrameDataPtr data, int line_index_x, int frame)
524 {
525  int dimX = data->getDimensions()[0];
526  int dimY = data->getDimensions()[1];
527 
528  vtkImageDataPtr retval = generateVtkImageDataDouble(Eigen::Array3i(dimY, 1, 1), Vector3D(1,1,1), 1);
529 
530  vtkImageDataPtr base = mProcessedFrames[frame];
531 
532  // run the base frame through the mask. Result is source.
533  vtkImageMaskPtr maskFilter = vtkImageMaskPtr::New();
534  maskFilter->SetMaskInputData(mMask);
535  maskFilter->SetImageInputData(base);
536  maskFilter->SetMaskedOutputValue(0.0);
537  maskFilter->Update();
538  uchar* source = static_cast<uchar*>(maskFilter->GetOutput()->GetScalarPointer());
539 
540  double* dest = static_cast<double*>(retval->GetScalarPointer());
541 
542  for (int y=0; y<dimY; ++y)
543  {
544  dest[y] = source[y*dimX + line_index_x];
545  }
546 
547  return retval;
548 }
549 
550 
551 
552 
553 
554 
555 
556 }//namespace cx
557 
558 
QString qstring_cast(const T &val)
Reader class for the US Acquisition files.
vtkSmartPointer< vtkImageCorrelation > vtkImageCorrelationPtr
void reportError(QString msg)
Definition: cxLogger.cpp:92
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
double calibrate(bool *success)
boost::shared_ptr< class USFrameData > USFrameDataPtr
QString timestampSecondsFormatNice()
Definition: cxTime.cpp:47
std::vector< TimedPosition > mFrames
vtkSmartPointer< class vtkPiecewiseFunction > vtkPiecewiseFunctionPtr
bool similar(const DoubleBoundingBox3D &a, const DoubleBoundingBox3D &b, double tol)
Vector3D getOrigin_p() const
void correlate(double *x, double *y, double *corr, int maxdelay, int n)
USReconstructInputData readAllFiles(QString fileName, QString calFilesPath="")
void reportWarning(QString msg)
Definition: cxLogger.cpp:91
void selectData(QString filename)
double dot(const Vector3D &a, const Vector3D &b)
compute inner product (or dot product) of a and b.
Definition: cxVector3D.cpp:67
std::vector< TimedPosition > mPositions
ProbeDefinition mData
Definition: cxProbeSector.h:75
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:63
void report(QString msg)
Definition: cxLogger.cpp:90
vtkSmartPointer< vtkImageMask > vtkImageMaskPtr
unsigned char uchar
vtkSmartPointer< class vtkImageData > vtkImageDataPtr
USFrameDataPtr mUsRaw
All imported US data frames with pointers to each frame.
vtkImageDataPtr generateVtkImageDataDouble(Eigen::Array3i dim, Vector3D spacing, double initValue)