NorMIT-nav  18.04
An IGT application
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) SINTEF Department of Medical Technology.
5 All rights reserved.
6 
7 CustusX is released under a BSD 3-Clause license.
8 
9 See Lisence.txt (https://github.com/SINTEFMedtek/CustusX/blob/master/License.txt) for details.
10 =========================================================================*/
11 #include <cxTemporalCalibration.h>
12 
13 
14 #include <QtWidgets>
15 
16 #include <QVBoxLayout>
17 #include "boost/bind.hpp"
18 #include "cxTrackingService.h"
19 #include <vtkPiecewiseFunction.h>
20 #include <vtkPointData.h>
21 #include <vtkImageData.h>
22 #include "cxTypeConversions.h"
23 #include "cxSettings.h"
24 #include "cxUtilHelpers.h"
25 #include "cxVolumeHelpers.h"
26 #include "vtkImageCorrelation.h"
27 #include "cxUSFrameData.h"
28 #include "cxImage.h"
30 #include "cxLogger.h"
31 #include "cxTime.h"
32 #include <vtkImageMask.h>
33 
34 typedef vtkSmartPointer<vtkImageMask> vtkImageMaskPtr;
35 typedef vtkSmartPointer<vtkImageCorrelation> vtkImageCorrelationPtr;
36 
37 namespace cx
38 {
39 
40 typedef unsigned char uchar; // for removing eclipse warnings
41 
42 
43 
53 void correlate(double* x, double* y, double* corr, int maxdelay, int n)
54 {
55  int i, j;
56  double mx, my, sx, sy, sxy, denom, r;
57  int delay;
58 
59  /* Calculate the mean of the two series x[], y[] */
60  mx = 0;
61  my = 0;
62  for (i = 0; i < n; i++)
63  {
64  mx += x[i];
65  my += y[i];
66  }
67  mx /= n;
68  my /= n;
69 
70  /* Calculate the denominator */
71  sx = 0;
72  sy = 0;
73  for (i = 0; i < n; i++)
74  {
75  sx += (x[i] - mx) * (x[i] - mx);
76  sy += (y[i] - my) * (y[i] - my);
77  }
78  denom = sqrt(sx * sy);
79 
80  /* Calculate the correlation series */
81  for (delay = -maxdelay; delay < maxdelay; delay++)
82  {
83  sxy = 0;
84  for (i = 0; i < n; i++)
85  {
86  j = i + delay;
87  if (j < 0 || j >= n)
88  continue;
89  else
90  sxy += (x[i] - mx) * (y[j] - my);
91  }
92  r = sxy / denom;
93  corr[delay+maxdelay] = r;//(sxy/denom+1) * 128;
94 
95  /* r is the correlation coefficient at "delay" */
96 
97  }
98 
99 }
100 
102 {
103  mAddRawToDebug = false;
104  mMask = vtkImageDataPtr();
105 }
106 
107 void TemporalCalibration::selectData(QString filename)
108 {
109  mFilename = filename;
110  mFileData = USReconstructInputData();
111 
112  if (!QFileInfo(filename).exists())
113  return;
114 
115  UsReconstructionFileReader fileReader;
116  mFileData = fileReader.readAllFiles(filename);
117 
118  if (!mFileData.mUsRaw)
119  {
120  reportWarning("Failed to load data from " + filename);
121  return;
122  }
123 
124  report("Temporal Calibration: Successfully loaded data from " + filename);
125 }
126 
128 {
129  mDebugFolder = filename;
130 }
131 
132 void TemporalCalibration::saveDebugFile()
133 {
134  if (mDebugFolder.isEmpty())
135  return;
136 
137  QString dbFilename = mDebugFolder +"/"+ QFileInfo(mFilename).baseName() + "_temporal_calib.txt";
138  QFile file(dbFilename);
139  file.remove();
140 
141  if (!file.open(QIODevice::ReadWrite))
142  {
143  reportError("Failed to write file " + file.fileName() + ".");
144  return;
145  }
146 
147  file.write(qstring_cast(mDebugStream.str()).toLatin1());
148  report("Saved temporal calibration details to " + file.fileName());
149  file.close();
150 }
151 
152 
167 double TemporalCalibration::calibrate(bool* success)
168 {
169  mDebugStream.str("");
170  mDebugStream.clear();
171 
172  if (!mFileData.mUsRaw)
173  {
174  reportWarning("Temporal calib: No data loaded");
175  return 0;
176  }
177  if (mFileData.mPositions.empty())
178  {
179  reportWarning("Temporal calib: Missing tracking data.");
180  return 0;
181  }
182 
183  mDebugStream << "Temporal Calibration " << QDateTime::currentDateTime().toString(timestampSecondsFormatNice()) << std::endl;
184  mDebugStream << "Loaded data: " << mFilename << std::endl;
185  mDebugStream << "=======================================" << std::endl;
186 
187  mProcessedFrames = mFileData.mUsRaw->initializeFrames(std::vector<bool>(1, false)).front();
188 
189  std::vector<double> frameMovement = this->computeProbeMovement();
190 
191  if (!this->checkFrameMovementQuality(frameMovement))
192  {
193  reportError("Failed to detect movement in images. Make sure that the first image is clear and visible.");
194  *success = false;
195  return 0;
196  }
197 
198 
199  std::vector<double> trackingMovement = this->computeTrackingMovement();
200 
201  // set a resolution, resample both tracking and frames to that
202  double resolution = 5; // ms
203 
204  double offset = mFileData.mFrames.front().mTime - mFileData.mPositions.front().mTime;
205  std::vector<double> frameMovementRegular = this->resample(frameMovement, mFileData.mFrames, resolution);
206  std::vector<double> trackingMovementRegular = this->resample(trackingMovement, mFileData.mPositions, resolution);
207 
208  double shift = this->findLSShift(frameMovementRegular, trackingMovementRegular, resolution);
209 
210  double totalShift = offset + shift;
211 
212  mDebugStream << "=======================================" << std::endl;
213  mDebugStream << "Performed temporal calibration:" << std::endl;
214  mDebugStream << "offset = " << offset << ", shift = " << shift << std::endl;
215  mDebugStream << "Total temporal shift tf-tt = " << offset+shift << " ms" << std::endl;
216  mDebugStream << "=======================================" << std::endl;
217 
218  double startTime = mFileData.mPositions.front().mTime;
219  this->writePositions("Tracking", trackingMovement, mFileData.mPositions, startTime);
220  this->writePositions("Frames", frameMovement, mFileData.mFrames, startTime);
221  this->writePositions("Shifted Frames", frameMovement, mFileData.mFrames, startTime + totalShift);
222 
223 
224  this->saveDebugFile();
225  *success = true;
226  return totalShift;
227 }
228 
229 bool TemporalCalibration::checkFrameMovementQuality(std::vector<double> pos)
230 {
231  int count = 0;
232  for (unsigned i=0; i<pos.size(); ++i)
233  if (similar(pos[i], 0))
234  count++;
235 
236  // accept if less than 20% zeros.
237  double error = double(count)/pos.size();
238  if (error > 0.05)
239  reportWarning(QString("Found %1 % zeros in frame movement").arg(error*100));
240  return error < 0.2;
241 }
242 
246 double TemporalCalibration::findLeastSquares(std::vector<double> frames, std::vector<double> tracking, int shift) const
247 {
248  size_t r0 = 0;
249  size_t r1 = frames.size();
250 
251  r0 = std::max<int>(r0, - shift);
252  r1 = std::min<int>(r1, tracking.size() - shift);
253 
254  double value = 0;
255 
256  for (int i=r0; i<r1; ++i)
257  {
258  double core = pow(frames[i] - tracking[i+shift], 2.0);
259  value += core;
260  }
261  value /= (r1-r0);
262  value = sqrt(value);
263  return value;
264 }
265 
271 double TemporalCalibration::findLSShift(std::vector<double> frames, std::vector<double> tracking, double resolution) const
272 {
273  double maxShift = 1000;
274  size_t N = std::min(tracking.size(), frames.size());
275  N = std::min<int>(N, 2*maxShift/resolution); // constrain search to 1 second in each direction
276  std::vector<double> result(N, 0);
277  int W = N/2;
278 
279  for (int i=-W; i<W; ++i)
280  {
281  double rms = this->findLeastSquares(frames, tracking, i);
282  result[i+W] = rms;
283  }
284 
285  int top = std::distance(result.begin(), std::min_element(result.begin(), result.end()));
286  double shift = (W-top) * resolution; // convert to shift in ms.
287 
288  mDebugStream << "=======================================" << std::endl;
289  mDebugStream << "tracking vs frames fit using least squares:" << std::endl;
290  mDebugStream << "Temporal resolution " << resolution << " ms" << std::endl;
291  mDebugStream << "Max shift " << maxShift << " ms" << std::endl;
292  mDebugStream << "#frames=" << frames.size() << ", #tracks=" << tracking.size() << std::endl;
293  mDebugStream << std::endl;
294  mDebugStream << "Frame pos" << "\t" << "Track pos" << "\t" << "RMS(center=" << W << ")" << std::endl;
295  for (size_t x = 0; x < std::min<int>(tracking.size(), frames.size()); ++x)
296  {
297  mDebugStream << frames[x] << "\t" << tracking[x];
298  if (x<N)
299  mDebugStream << "\t" << result[x];
300  mDebugStream << std::endl;
301  }
302 
303  mDebugStream << std::endl;
304  mDebugStream << "minimal index: " << top << ", = shift in ms: " << shift << std::endl;
305  mDebugStream << "=======================================" << std::endl;
306 
307  return shift; // shift frames-tracking: frame = tracking + shift
308 }
309 
315 double TemporalCalibration::findCorrelationShift(std::vector<double> frames, std::vector<double> tracking, double resolution) const
316 {
317  size_t N = std::min(tracking.size(), frames.size());
318  std::vector<double> result(N, 0);
319 
320  correlate(&*frames.begin(), &*tracking.begin(), &*result.begin(), N / 2, N);
321 
322  int top = std::distance(result.begin(), std::max_element(result.begin(), result.end()));
323  double shift = (N/2-top) * resolution; // convert to shift in ms.
324 
325  mDebugStream << "=======================================" << std::endl;
326  mDebugStream << "tracking vs frames correlation:" << std::endl;
327  mDebugStream << "Temporal resolution " << resolution << " ms" << std::endl;
328  mDebugStream << "#frames=" << frames.size() << ", #tracks=" << tracking.size() << std::endl;
329  mDebugStream << std::endl;
330  mDebugStream << "Frame pos" << "\t" << "Track pos" << "\t" << "correlation" << std::endl;
331  for (int x = 0; x < N; ++x)
332  {
333  mDebugStream << frames[x] << "\t" << tracking[x] << "\t" << result[x] << std::endl;
334  }
335 
336  mDebugStream << std::endl;
337  mDebugStream << "corr top: " << top << ", = shift in ms: " << shift << std::endl;
338  mDebugStream << "=======================================" << std::endl;
339 
340  return shift; // shift frames-tracking: frame = tracking + shift
341 }
342 
343 
344 void TemporalCalibration::writePositions(QString title, std::vector<double> pos, std::vector<TimedPosition> time, double shift)
345 {
346  if (pos.size()!=time.size())
347  {
348  std::cout << "size mismatch" << std::endl;
349  return;
350  }
351 
352  mDebugStream << title << std::endl;
353  mDebugStream << "time\t" << "pos\t" << std::endl;
354  for (unsigned i=0; i<time.size(); ++i)
355  {
356  mDebugStream << time[i].mTime - shift << "\t" << pos[i] << std::endl;
357  }
358 }
359 
363 std::vector<double> TemporalCalibration::resample(std::vector<double> shift, std::vector<TimedPosition> time, double resolution)
364 {
365  if (shift.size()!=time.size())
366  {
367  reportError("Assert failure, shift and time different sizes");
368  }
369 
370  vtkPiecewiseFunctionPtr frames = vtkPiecewiseFunctionPtr::New();
371  for (unsigned i=0; i<shift.size(); ++i)
372  {
373  frames->AddPoint(time[i].mTime, shift[i]);
374  }
375  double r0, r1;
376  frames->GetRange(r0, r1);
377  double range = r1-r0;
378  int N_r = range/resolution;
379 
380  std::vector<double> framesRegular;
381  for (int i=0; i<N_r; ++i)
382  framesRegular.push_back(frames->GetValue(r0 + i*resolution));
383 
384  return framesRegular;
385 }
386 
387 std::vector<double> TemporalCalibration::computeTrackingMovement()
388 {
389  std::vector<double> retval;
390  Vector3D e_z(0,0,1);
391  Vector3D origin(0,0,0);
392  double zero = 0;
393  Transform3D prM0t = mFileData.mPositions[0].mPos;
394  Vector3D ez_pr = prM0t.vector(e_z);
395 
396  for (unsigned i=0; i<mFileData.mPositions.size(); ++i)
397  {
398  Transform3D prMt = mFileData.mPositions[i].mPos;
399  Vector3D p_pr = prMt.coord(origin);
400 
401  double val = dot(ez_pr, p_pr);
402 
403  if (retval.empty())
404  zero = val;
405 
406  retval.push_back(val-zero);
407  }
408 
409  if (mAddRawToDebug)
410  {
411  mDebugStream << "=======================================" << std::endl;
412  mDebugStream << "tracking raw data:" << std::endl;
413  mDebugStream << std::endl;
414  mDebugStream << "timestamp" << "\t" << "pos" << std::endl;
415  for (unsigned x = 0; x < mFileData.mPositions.size(); ++x)
416  {
417  mDebugStream << mFileData.mPositions[x].mTime << "\t" << retval[x] << std::endl;
418  }
419  mDebugStream << std::endl;
420  mDebugStream << "=======================================" << std::endl;
421  }
422 
423  return retval;
424 }
425 
429 std::vector<double> TemporalCalibration::computeProbeMovement()
430 {
431  int N_frames = mFileData.mUsRaw->getDimensions()[2];
432 
433  std::vector<double> retval;
434 
435  double maxSingleStep = 5; // assume max 5mm movement per frame
436 // double currentMaxShift;
437  double lastVal = 0;
438 
439  mMask = mFileData.getMask();
440  for (int i=0; i<N_frames; ++i)
441  {
442  double val = this->findCorrelation(mFileData.mUsRaw, 0, i, maxSingleStep, lastVal);
443 // currentMaxShift = fabs(val) + maxSingleStep;
444  lastVal = val;
445  retval.push_back(val);
446  }
447 
448  if (mAddRawToDebug)
449  {
450  mDebugStream << "=======================================" << std::endl;
451  mDebugStream << "frames raw data:" << std::endl;
452  mDebugStream << std::endl;
453  mDebugStream << "timestamp" << "\t" << "shift" << std::endl;
454  for (int x = 0; x < N_frames; ++x)
455  {
456  mDebugStream << mFileData.mFrames[x].mTime << "\t" << retval[x] << std::endl;
457  }
458  mDebugStream << std::endl;
459  mDebugStream << "=======================================" << std::endl;
460  }
461 
462  return retval;
463 }
464 
465 double TemporalCalibration::findCorrelation(USFrameDataPtr data, int frame_a, int frame_b, double maxShift, double lastVal)
466 {
467  int maxShift_pix = maxShift / mFileData.mUsRaw->getSpacing()[1];
468  int lastVal_pix = lastVal / mFileData.mUsRaw->getSpacing()[1];
469 
470  int line_index_x = mFileData.mProbeDefinition.mData.getOrigin_p()[0];
471 
472  int dimY = mFileData.mUsRaw->getDimensions()[1];
473 
474  vtkImageDataPtr line1 = this->extractLine_y(mFileData.mUsRaw, line_index_x, frame_a);
475  vtkImageDataPtr line2 = this->extractLine_y(mFileData.mUsRaw, line_index_x, frame_b);
476 
477  int N = 2*dimY; //result vector allocate space on both sides of zero
478  std::vector<double> result(N, 0);
479  double* line_a = static_cast<double*>(line1->GetScalarPointer());
480  double* line_b = static_cast<double*>(line2->GetScalarPointer());
481  double* line_c = &*result.begin();
482 
483  correlate(line_a, line_b, line_c, N/2, dimY);
484 
485  // use the last found hit as a seed for looking for a local maximum
486  int lastTop = N/2 - lastVal_pix;
487  std::pair<int,int> range(lastTop - maxShift_pix, lastTop+maxShift_pix);
488  range.first = std::max(0, range.first);
489  range.second = std::min(N, range.second);
490 
491  // look for a max in the vicinity of the last hit
492  int top = std::distance(result.begin(), std::max_element(result.begin()+range.first, result.begin()+range.second));
493 
494  double hit = (N/2-top) * mFileData.mUsRaw->getSpacing()[1]; // convert to downwards movement in mm.
495 
496  return hit;
497 }
498 
502 vtkImageDataPtr TemporalCalibration::extractLine_y(USFrameDataPtr data, int line_index_x, int frame)
503 {
504  int dimX = data->getDimensions()[0];
505  int dimY = data->getDimensions()[1];
506 
507  vtkImageDataPtr retval = generateVtkImageDataDouble(Eigen::Array3i(dimY, 1, 1), Vector3D(1,1,1), 1);
508 
509  vtkImageDataPtr base = mProcessedFrames[frame];
510 
511  // run the base frame through the mask. Result is source.
512  vtkImageMaskPtr maskFilter = vtkImageMaskPtr::New();
513  maskFilter->SetMaskInputData(mMask);
514  maskFilter->SetImageInputData(base);
515  maskFilter->SetMaskedOutputValue(0.0);
516  maskFilter->Update();
517  uchar* source = static_cast<uchar*>(maskFilter->GetOutput()->GetScalarPointer());
518 
519  double* dest = static_cast<double*>(retval->GetScalarPointer());
520 
521  for (int y=0; y<dimY; ++y)
522  {
523  dest[y] = source[y*dimX + line_index_x];
524  }
525 
526  return retval;
527 }
528 
529 
530 
531 
532 
533 
534 
535 }//namespace cx
536 
537 
QString qstring_cast(const T &val)
Reader class for the US Acquisition files.
vtkSmartPointer< vtkImageCorrelation > vtkImageCorrelationPtr
void reportError(QString msg)
Definition: cxLogger.cpp:71
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:26
std::vector< TimedPosition > mFrames
vtkSmartPointer< class vtkPiecewiseFunction > vtkPiecewiseFunctionPtr
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:70
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:46
std::vector< TimedPosition > mPositions
ProbeDefinition mData
Definition: cxProbeSector.h:54
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:42
void report(QString msg)
Definition: cxLogger.cpp:69
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
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)
Namespace for all CustusX production code.