cxTemporalCalibration.cpp

Go to the documentation of this file.

/*=========================================================================
This file is part of CustusX, an Image Guided Therapy Application.

Copyright (c) 2008-2014, SINTEF Department of Medical Technology
All rights reserved.

Redistribution and use in source and binary forms, with or without 
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, 
   this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, 
   this list of conditions and the following disclaimer in the documentation 
   and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors 
   may be used to endorse or promote products derived from this software 
   without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
=========================================================================*/
#include <cxTemporalCalibration.h>


#include <QtWidgets>

#include <QVBoxLayout>
#include "boost/bind.hpp"
#include "cxTrackingService.h"
#include <vtkPiecewiseFunction.h>
#include <vtkPointData.h>
#include <vtkImageData.h>
#include "cxTypeConversions.h"
#include "cxSettings.h"
#include "cxUtilHelpers.h"
#include "cxVolumeHelpers.h"
#include "vtkImageCorrelation.h"
#include "cxUSFrameData.h"
#include "cxImage.h"
#include "cxUsReconstructionFileReader.h"
#include "cxLogger.h"
#include "cxTime.h"
#include <vtkImageMask.h>

typedef vtkSmartPointer<vtkImageMask> vtkImageMaskPtr;
typedef vtkSmartPointer<vtkImageCorrelation> vtkImageCorrelationPtr;

namespace cx
{

typedef unsigned char uchar; // for removing eclipse warnings



void correlate(double* x, double* y, double* corr, int maxdelay, int n)
{
  int i, j;
  double mx, my, sx, sy, sxy, denom, r;
  int delay;

  /* Calculate the mean of the two series x[], y[] */
  mx = 0;
  my = 0;
  for (i = 0; i < n; i++)
  {
    mx += x[i];
    my += y[i];
  }
  mx /= n;
  my /= n;

  /* Calculate the denominator */
  sx = 0;
  sy = 0;
  for (i = 0; i < n; i++)
  {
    sx += (x[i] - mx) * (x[i] - mx);
    sy += (y[i] - my) * (y[i] - my);
  }
  denom = sqrt(sx * sy);

  /* Calculate the correlation series */
  for (delay = -maxdelay; delay < maxdelay; delay++)
  {
    sxy = 0;
    for (i = 0; i < n; i++)
    {
      j = i + delay;
      if (j < 0 || j >= n)
        continue;
      else
        sxy += (x[i] - mx) * (y[j] - my);
    }
    r = sxy / denom;
    corr[delay+maxdelay] = r;//(sxy/denom+1) * 128;

    /* r is the correlation coefficient at "delay" */

  }

}

TemporalCalibration::TemporalCalibration()
{
        mAddRawToDebug = false;
        mMask = vtkImageDataPtr();
}

void TemporalCalibration::selectData(QString filename)
{
  mFilename = filename;
  mFileData = USReconstructInputData();

  if (!QFileInfo(filename).exists())
    return;

  UsReconstructionFileReader fileReader;
  mFileData = fileReader.readAllFiles(filename);

  if (!mFileData.mUsRaw)
  {
    reportWarning("Failed to load data from " + filename);
    return;
  }

  report("Temporal Calibration: Successfully loaded data from " + filename);
}

void TemporalCalibration::setDebugFolder(QString filename)
{
        mDebugFolder = filename;
}

void TemporalCalibration::saveDebugFile()
{
  if (mDebugFolder.isEmpty())
    return;

  QString dbFilename = mDebugFolder +"/"+ QFileInfo(mFilename).baseName() + "_temporal_calib.txt";
  QFile file(dbFilename);
  file.remove();

  if (!file.open(QIODevice::ReadWrite))
  {
    reportError("Failed to write file " + file.fileName() + ".");
    return;
  }

  file.write(qstring_cast(mDebugStream.str()).toLatin1());
  report("Saved temporal calibration details to " + file.fileName());
  file.close();
}


double TemporalCalibration::calibrate(bool* success)
{
  mDebugStream.str("");
  mDebugStream.clear();

  if (!mFileData.mUsRaw)
  {
    reportWarning("Temporal calib: No data loaded");
    return 0;
  }
  if (mFileData.mPositions.empty())
  {
    reportWarning("Temporal calib: Missing tracking data.");
    return 0;
  }

  mDebugStream << "Temporal Calibration " << QDateTime::currentDateTime().toString(timestampSecondsFormatNice()) << std::endl;
  mDebugStream << "Loaded data: " << mFilename << std::endl;
  mDebugStream << "=======================================" << std::endl;

  mProcessedFrames = mFileData.mUsRaw->initializeFrames(std::vector<bool>(1, false)).front();

  std::vector<double> frameMovement = this->computeProbeMovement();

  if (!this->checkFrameMovementQuality(frameMovement))
  {
          reportError("Failed to detect movement in images. Make sure that the first image is clear and visible.");
          *success = false;
          return 0;
  }


  std::vector<double> trackingMovement = this->computeTrackingMovement();

  // set a resolution, resample both tracking and frames to that
  double resolution = 5; // ms

  double offset = mFileData.mFrames.front().mTime - mFileData.mPositions.front().mTime;
  std::vector<double> frameMovementRegular = this->resample(frameMovement, mFileData.mFrames, resolution);
  std::vector<double> trackingMovementRegular = this->resample(trackingMovement, mFileData.mPositions, resolution);

  double shift = this->findLSShift(frameMovementRegular, trackingMovementRegular, resolution);

  double totalShift = offset + shift;

  mDebugStream << "=======================================" << std::endl;
  mDebugStream << "Performed temporal calibration:" << std::endl;
  mDebugStream << "offset = " << offset << ", shift = " << shift << std::endl;
  mDebugStream << "Total temporal shift tf-tt = " << offset+shift << " ms" << std::endl;
  mDebugStream << "=======================================" << std::endl;

  double startTime = mFileData.mPositions.front().mTime;
  this->writePositions("Tracking", trackingMovement, mFileData.mPositions, startTime);
  this->writePositions("Frames", frameMovement, mFileData.mFrames, startTime);
  this->writePositions("Shifted Frames", frameMovement, mFileData.mFrames, startTime + totalShift);


  this->saveDebugFile();
  *success = true;
  return totalShift;
}

bool TemporalCalibration::checkFrameMovementQuality(std::vector<double> pos)
{
        int count = 0;
        for (unsigned i=0; i<pos.size(); ++i)
                if (similar(pos[i], 0))
                        count++;

        // accept if less than 20% zeros.
        double error = double(count)/pos.size();
        if (error > 0.05)
                reportWarning(QString("Found %1 % zeros in frame movement").arg(error*100));
        return error < 0.2;
}

double TemporalCalibration::findLeastSquares(std::vector<double> frames, std::vector<double> tracking, int shift) const
{
        size_t r0 = 0;
        size_t r1 = frames.size();

        r0 = std::max<int>(r0, - shift);
        r1 = std::min<int>(r1, tracking.size() - shift);

        double value = 0;

        for (int i=r0; i<r1; ++i)
        {
                double core = pow(frames[i] - tracking[i+shift], 2.0);
                value += core;
        }
        value /= (r1-r0);
        value = sqrt(value);
        return value;
}

double TemporalCalibration::findLSShift(std::vector<double> frames, std::vector<double> tracking, double resolution) const
{
        double maxShift = 1000;
        size_t N = std::min(tracking.size(), frames.size());
  N = std::min<int>(N, 2*maxShift/resolution); // constrain search to 1 second in each direction
  std::vector<double> result(N, 0);
  int W = N/2;

  for (int i=-W; i<W; ++i)
  {
        double rms = this->findLeastSquares(frames, tracking, i);
        result[i+W] = rms;
  }

  int top = std::distance(result.begin(), std::min_element(result.begin(), result.end()));
  double shift = (W-top) * resolution; // convert to shift in ms.

  mDebugStream << "=======================================" << std::endl;
  mDebugStream << "tracking vs frames fit using least squares:" << std::endl;
  mDebugStream << "Temporal resolution " << resolution << " ms" << std::endl;
  mDebugStream << "Max shift " << maxShift << " ms" << std::endl;
  mDebugStream << "#frames=" << frames.size() << ", #tracks=" << tracking.size() << std::endl;
  mDebugStream << std::endl;
  mDebugStream << "Frame pos" << "\t" << "Track pos" << "\t" << "RMS(center=" << W << ")"       << std::endl;
        for (size_t x = 0; x < std::min<int>(tracking.size(), frames.size()); ++x)
  {
    mDebugStream << frames[x] << "\t" << tracking[x];
    if (x<N)
        mDebugStream << "\t" << result[x];
        mDebugStream << std::endl;
  }

  mDebugStream << std::endl;
  mDebugStream << "minimal index: " << top << ", = shift in ms: " << shift << std::endl;
  mDebugStream << "=======================================" << std::endl;

  return shift; // shift frames-tracking: frame = tracking + shift
}

double TemporalCalibration::findCorrelationShift(std::vector<double> frames, std::vector<double> tracking, double resolution) const
{
        size_t N = std::min(tracking.size(), frames.size());
  std::vector<double> result(N, 0);

  correlate(&*frames.begin(), &*tracking.begin(), &*result.begin(), N / 2, N);

  int top = std::distance(result.begin(), std::max_element(result.begin(), result.end()));
  double shift = (N/2-top) * resolution; // convert to shift in ms.

  mDebugStream << "=======================================" << std::endl;
  mDebugStream << "tracking vs frames correlation:" << std::endl;
  mDebugStream << "Temporal resolution " << resolution << " ms" << std::endl;
  mDebugStream << "#frames=" << frames.size() << ", #tracks=" << tracking.size() << std::endl;
  mDebugStream << std::endl;
  mDebugStream << "Frame pos" << "\t" << "Track pos" << "\t" << "correlation" << std::endl;
  for (int x = 0; x < N; ++x)
  {
    mDebugStream << frames[x] << "\t" << tracking[x] << "\t" << result[x] << std::endl;
  }

  mDebugStream << std::endl;
  mDebugStream << "corr top: " << top << ", = shift in ms: " << shift << std::endl;
  mDebugStream << "=======================================" << std::endl;

  return shift; // shift frames-tracking: frame = tracking + shift
}


void TemporalCalibration::writePositions(QString title, std::vector<double> pos, std::vector<TimedPosition> time, double shift)
{
        if (pos.size()!=time.size())
        {
                std::cout << "size mismatch" << std::endl;
                return;
        }

        mDebugStream << title << std::endl;
        mDebugStream << "time\t" << "pos\t" << std::endl;
        for (unsigned i=0; i<time.size(); ++i)
        {
                mDebugStream << time[i].mTime - shift << "\t" << pos[i] << std::endl;
        }
}

std::vector<double> TemporalCalibration::resample(std::vector<double> shift, std::vector<TimedPosition> time, double resolution)
{
  if (shift.size()!=time.size())
  {
    reportError("Assert failure, shift and time different sizes");
  }

  vtkPiecewiseFunctionPtr frames = vtkPiecewiseFunctionPtr::New();
  for (unsigned i=0; i<shift.size(); ++i)
  {
    frames->AddPoint(time[i].mTime, shift[i]);
  }
  double r0, r1;
  frames->GetRange(r0, r1);
  double range = r1-r0;
  int N_r = range/resolution;

  std::vector<double> framesRegular;
  for (int i=0; i<N_r; ++i)
    framesRegular.push_back(frames->GetValue(r0 + i*resolution));

  return framesRegular;
}

std::vector<double> TemporalCalibration::computeTrackingMovement()
{
  std::vector<double> retval;
  Vector3D e_z(0,0,1);
  Vector3D origin(0,0,0);
  double zero = 0;
  Transform3D prM0t = mFileData.mPositions[0].mPos;
  Vector3D ez_pr = prM0t.vector(e_z);

  for (unsigned i=0; i<mFileData.mPositions.size(); ++i)
  {
    Transform3D prMt = mFileData.mPositions[i].mPos;
    Vector3D p_pr = prMt.coord(origin);

    double val = dot(ez_pr, p_pr);

    if (retval.empty())
      zero = val;

    retval.push_back(val-zero);
  }

  if (mAddRawToDebug)
  {
                mDebugStream << "=======================================" << std::endl;
                mDebugStream << "tracking raw data:" << std::endl;
                mDebugStream << std::endl;
                mDebugStream << "timestamp" << "\t" << "pos" << std::endl;
                for (unsigned x = 0; x < mFileData.mPositions.size(); ++x)
                {
                        mDebugStream << mFileData.mPositions[x].mTime << "\t" << retval[x] << std::endl;
                }
                mDebugStream << std::endl;
                mDebugStream << "=======================================" << std::endl;
  }

  return retval;
}

std::vector<double> TemporalCalibration::computeProbeMovement()
{
  int N_frames = mFileData.mUsRaw->getDimensions()[2];

  std::vector<double> retval;

  double maxSingleStep = 5; // assume max 5mm movement per frame
//      double currentMaxShift;
  double lastVal = 0;

        mMask = mFileData.getMask();
  for (int i=0; i<N_frames; ++i)
  {
    double val = this->findCorrelation(mFileData.mUsRaw, 0, i, maxSingleStep, lastVal);
//    currentMaxShift =  fabs(val) + maxSingleStep;
    lastVal = val;
    retval.push_back(val);
  }

  if (mAddRawToDebug)
  {
                mDebugStream << "=======================================" << std::endl;
                mDebugStream << "frames raw data:" << std::endl;
                mDebugStream << std::endl;
                mDebugStream << "timestamp" << "\t" << "shift" << std::endl;
                for (int x = 0; x < N_frames; ++x)
                {
                        mDebugStream << mFileData.mFrames[x].mTime << "\t" << retval[x] << std::endl;
                }
                mDebugStream << std::endl;
                mDebugStream << "=======================================" << std::endl;
  }

  return retval;
}

double TemporalCalibration::findCorrelation(USFrameDataPtr data, int frame_a, int frame_b, double maxShift, double lastVal)
{
        int maxShift_pix = maxShift / mFileData.mUsRaw->getSpacing()[1];
        int lastVal_pix = lastVal / mFileData.mUsRaw->getSpacing()[1];

        int line_index_x = mFileData.mProbeDefinition.mData.getOrigin_p()[0];

  int dimY = mFileData.mUsRaw->getDimensions()[1];

        vtkImageDataPtr line1 = this->extractLine_y(mFileData.mUsRaw, line_index_x, frame_a);
        vtkImageDataPtr line2 = this->extractLine_y(mFileData.mUsRaw, line_index_x, frame_b);

  int N = 2*dimY; //result vector allocate space on both sides of zero
  std::vector<double> result(N, 0);
  double* line_a = static_cast<double*>(line1->GetScalarPointer());
  double* line_b = static_cast<double*>(line2->GetScalarPointer());
  double* line_c = &*result.begin();

  correlate(line_a, line_b, line_c, N/2, dimY);

  // use the last found hit as a seed for looking for a local maximum
  int lastTop = N/2 - lastVal_pix;
  std::pair<int,int> range(lastTop - maxShift_pix, lastTop+maxShift_pix);
  range.first = std::max(0, range.first);
  range.second = std::min(N, range.second);

  // look for a max in the vicinity of the last hit
  int top = std::distance(result.begin(), std::max_element(result.begin()+range.first, result.begin()+range.second));

  double hit = (N/2-top) * mFileData.mUsRaw->getSpacing()[1]; // convert to downwards movement in mm.

  return hit;
}

vtkImageDataPtr TemporalCalibration::extractLine_y(USFrameDataPtr data, int line_index_x, int frame)
{
  int dimX = data->getDimensions()[0];
  int dimY = data->getDimensions()[1];

  vtkImageDataPtr retval = generateVtkImageDataDouble(Eigen::Array3i(dimY, 1, 1), Vector3D(1,1,1), 1);

  vtkImageDataPtr base = mProcessedFrames[frame];

  // run the base frame through the mask. Result is source.
  vtkImageMaskPtr maskFilter = vtkImageMaskPtr::New();
        maskFilter->SetMaskInputData(mMask);
  maskFilter->SetImageInputData(base);
  maskFilter->SetMaskedOutputValue(0.0);
  maskFilter->Update();
  uchar* source = static_cast<uchar*>(maskFilter->GetOutput()->GetScalarPointer());

  double* dest = static_cast<double*>(retval->GetScalarPointer());

  for (int y=0; y<dimY; ++y)
  {
    dest[y] = source[y*dimX + line_index_x];
  }

  return retval;
}







}//namespace cx