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