37 #include <QVBoxLayout>
38 #include "boost/bind.hpp"
40 #include <vtkPiecewiseFunction.h>
41 #include <vtkPointData.h>
42 #include <vtkImageData.h>
47 #include "vtkImageCorrelation.h"
53 #include <vtkImageMask.h>
61 typedef unsigned char uchar;
74 void correlate(
double* x,
double* y,
double* corr,
int maxdelay,
int n)
77 double mx, my, sx, sy, sxy, denom, r;
83 for (i = 0; i < n; i++)
94 for (i = 0; i < n; i++)
96 sx += (x[i] - mx) * (x[i] - mx);
97 sy += (y[i] - my) * (y[i] - my);
99 denom = sqrt(sx * sy);
102 for (delay = -maxdelay; delay < maxdelay; delay++)
105 for (i = 0; i < n; i++)
111 sxy += (x[i] - mx) * (y[j] - my);
114 corr[delay+maxdelay] = r;
124 mAddRawToDebug =
false;
130 mFilename = filename;
133 if (!QFileInfo(filename).exists())
145 report(
"Temporal Calibration: Successfully loaded data from " + filename);
150 mDebugFolder = filename;
153 void TemporalCalibration::saveDebugFile()
155 if (mDebugFolder.isEmpty())
158 QString dbFilename = mDebugFolder +
"/"+ QFileInfo(mFilename).baseName() +
"_temporal_calib.txt";
159 QFile file(dbFilename);
162 if (!file.open(QIODevice::ReadWrite))
164 reportError(
"Failed to write file " + file.fileName() +
".");
168 file.write(
qstring_cast(mDebugStream.str()).toLatin1());
169 report(
"Saved temporal calibration details to " + file.fileName());
190 mDebugStream.str(
"");
191 mDebugStream.clear();
205 mDebugStream <<
"Loaded data: " << mFilename << std::endl;
206 mDebugStream <<
"=======================================" << std::endl;
208 mProcessedFrames = mFileData.
mUsRaw->initializeFrames(std::vector<bool>(1,
false)).front();
210 std::vector<double> frameMovement = this->computeProbeMovement();
212 if (!this->checkFrameMovementQuality(frameMovement))
214 reportError(
"Failed to detect movement in images. Make sure that the first image is clear and visible.");
220 std::vector<double> trackingMovement = this->computeTrackingMovement();
223 double resolution = 5;
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);
229 double shift = this->findLSShift(frameMovementRegular, trackingMovementRegular, resolution);
231 double totalShift = offset + shift;
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;
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);
245 this->saveDebugFile();
250 bool TemporalCalibration::checkFrameMovementQuality(std::vector<double> pos)
253 for (
unsigned i=0; i<pos.size(); ++i)
258 double error = double(count)/pos.size();
260 reportWarning(QString(
"Found %1 % zeros in frame movement").arg(error*100));
267 double TemporalCalibration::findLeastSquares(std::vector<double> frames, std::vector<double> tracking,
int shift)
const
270 size_t r1 = frames.size();
272 r0 = std::max<int>(r0, - shift);
273 r1 = std::min<int>(r1, tracking.size() - shift);
277 for (
int i=r0; i<r1; ++i)
279 double core = pow(frames[i] - tracking[i+shift], 2.0);
292 double TemporalCalibration::findLSShift(std::vector<double> frames, std::vector<double> tracking,
double resolution)
const
294 double maxShift = 1000;
295 size_t N = std::min(tracking.size(), frames.size());
296 N = std::min<int>(N, 2*maxShift/resolution);
297 std::vector<double> result(N, 0);
300 for (
int i=-W; i<W; ++i)
302 double rms = this->findLeastSquares(frames, tracking, i);
306 int top = std::distance(result.begin(), std::min_element(result.begin(), result.end()));
307 double shift = (W-top) * resolution;
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)
318 mDebugStream << frames[x] <<
"\t" << tracking[x];
320 mDebugStream <<
"\t" << result[x];
321 mDebugStream << std::endl;
324 mDebugStream << std::endl;
325 mDebugStream <<
"minimal index: " << top <<
", = shift in ms: " << shift << std::endl;
326 mDebugStream <<
"=======================================" << std::endl;
336 double TemporalCalibration::findCorrelationShift(std::vector<double> frames, std::vector<double> tracking,
double resolution)
const
338 size_t N = std::min(tracking.size(), frames.size());
339 std::vector<double> result(N, 0);
341 correlate(&*frames.begin(), &*tracking.begin(), &*result.begin(), N / 2, N);
343 int top = std::distance(result.begin(), std::max_element(result.begin(), result.end()));
344 double shift = (N/2-top) * resolution;
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)
354 mDebugStream << frames[x] <<
"\t" << tracking[x] <<
"\t" << result[x] << std::endl;
357 mDebugStream << std::endl;
358 mDebugStream <<
"corr top: " << top <<
", = shift in ms: " << shift << std::endl;
359 mDebugStream <<
"=======================================" << std::endl;
365 void TemporalCalibration::writePositions(QString title, std::vector<double> pos, std::vector<TimedPosition> time,
double shift)
367 if (pos.size()!=time.size())
369 std::cout <<
"size mismatch" << std::endl;
373 mDebugStream << title << std::endl;
374 mDebugStream <<
"time\t" <<
"pos\t" << std::endl;
375 for (
unsigned i=0; i<time.size(); ++i)
377 mDebugStream << time[i].mTime - shift <<
"\t" << pos[i] << std::endl;
384 std::vector<double> TemporalCalibration::resample(std::vector<double> shift, std::vector<TimedPosition> time,
double resolution)
386 if (shift.size()!=time.size())
388 reportError(
"Assert failure, shift and time different sizes");
392 for (
unsigned i=0; i<shift.size(); ++i)
394 frames->AddPoint(time[i].mTime, shift[i]);
397 frames->GetRange(r0, r1);
398 double range = r1-r0;
399 int N_r = range/resolution;
401 std::vector<double> framesRegular;
402 for (
int i=0; i<N_r; ++i)
403 framesRegular.push_back(frames->GetValue(r0 + i*resolution));
405 return framesRegular;
408 std::vector<double> TemporalCalibration::computeTrackingMovement()
410 std::vector<double> retval;
417 for (
unsigned i=0; i<mFileData.
mPositions.size(); ++i)
422 double val =
dot(ez_pr, p_pr);
427 retval.push_back(val-zero);
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)
438 mDebugStream << mFileData.
mPositions[x].mTime <<
"\t" << retval[x] << std::endl;
440 mDebugStream << std::endl;
441 mDebugStream <<
"=======================================" << std::endl;
450 std::vector<double> TemporalCalibration::computeProbeMovement()
452 int N_frames = mFileData.
mUsRaw->getDimensions()[2];
454 std::vector<double> retval;
456 double maxSingleStep = 5;
461 for (
int i=0; i<N_frames; ++i)
463 double val = this->findCorrelation(mFileData.
mUsRaw, 0, i, maxSingleStep, lastVal);
466 retval.push_back(val);
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)
477 mDebugStream << mFileData.
mFrames[x].mTime <<
"\t" << retval[x] << std::endl;
479 mDebugStream << std::endl;
480 mDebugStream <<
"=======================================" << std::endl;
486 double TemporalCalibration::findCorrelation(
USFrameDataPtr data,
int frame_a,
int frame_b,
double maxShift,
double lastVal)
488 int maxShift_pix = maxShift / mFileData.
mUsRaw->getSpacing()[1];
489 int lastVal_pix = lastVal / mFileData.
mUsRaw->getSpacing()[1];
493 int dimY = mFileData.
mUsRaw->getDimensions()[1];
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();
504 correlate(line_a, line_b, line_c, N/2, dimY);
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);
513 int top = std::distance(result.begin(), std::max_element(result.begin()+range.first, result.begin()+range.second));
515 double hit = (N/2-top) * mFileData.
mUsRaw->getSpacing()[1];
525 int dimX = data->getDimensions()[0];
526 int dimY = data->getDimensions()[1];
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());
540 double* dest =
static_cast<double*
>(retval->GetScalarPointer());
542 for (
int y=0; y<dimY; ++y)
544 dest[y] = source[y*dimX + line_index_x];
QString qstring_cast(const T &val)
Reader class for the US Acquisition files.
vtkSmartPointer< vtkImageCorrelation > vtkImageCorrelationPtr
void reportError(QString msg)
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
double calibrate(bool *success)
boost::shared_ptr< class USFrameData > USFrameDataPtr
QString timestampSecondsFormatNice()
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)
void selectData(QString filename)
double dot(const Vector3D &a, const Vector3D &b)
compute inner product (or dot product) of a and b.
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
void setDebugFolder(QString path)
vtkSmartPointer< vtkImageMask > vtkImageMaskPtr
vtkImageDataPtr generateVtkImageDataDouble(Eigen::Array3i dim, Vector3D spacing, double initValue)