QmitkUSNavigationStepMarkerIntervention.cpp

Go to the documentation of this file.

/*============================================================================
 
The Medical Imaging Interaction Toolkit (MITK)
 
Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.
 
Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.
 
============================================================================*/
 
#include "QmitkUSNavigationStepMarkerIntervention.h"
#include "ui_QmitkUSNavigationStepMarkerIntervention.h"
 
#include "mitkBaseRenderer.h"
#include "mitkContourModel.h"
#include "mitkNeedleProjectionFilter.h"
#include "mitkNodeDisplacementFilter.h"
#include "mitkSurface.h"
#include "mitkTextAnnotation2D.h"
#include <mitkBaseGeometry.h>
#include <mitkManualPlacementAnnotationRenderer.h>
 
#include "../Filter/mitkUSNavigationTargetIntersectionFilter.h"
#include "../Filter/mitkUSNavigationTargetOcclusionFilter.h"
#include "../Filter/mitkUSNavigationTargetUpdateFilter.h"
 
#include "../QmitkUSNavigationMarkerPlacement.h"
#include "../Widgets/QmitkZoneProgressBar.h"
#include "../mitkUSTargetPlacementQualityCalculator.h"
 
#include "../Interactors/mitkUSPointMarkInteractor.h"
#include "usModuleRegistry.h"
 
#include "mitkLookupTable.h"
#include "mitkLookupTableProperty.h"
 
#include "mitkSurface.h"
 
// VTK
#include "vtkCellLocator.h"
#include "vtkDataSet.h"
#include "vtkDoubleArray.h"
#include "vtkFloatArray.h"
#include "vtkIdList.h"
#include "vtkLinearTransform.h"
#include "vtkLookupTable.h"
#include "vtkMath.h"
#include "vtkOBBTree.h"
#include "vtkPointData.h"
#include "vtkPointData.h"
#include "vtkPolyData.h"
#include "vtkSelectEnclosedPoints.h"
#include "vtkSmartPointer.h"
#include <vtkSphereSource.h>
#include "vtkTransformPolyDataFilter.h"
#include "vtkWarpScalar.h"
 
 
QmitkUSNavigationStepMarkerIntervention::QmitkUSNavigationStepMarkerIntervention(QWidget *parent)
  : QmitkUSAbstractNavigationStep(parent),
    m_NumberOfTargets(0),
    m_PlannedTargetsNodes(),
    m_ReachedTargetsNodes(),
    m_TargetProgressBar(new QmitkZoneProgressBar(QString::fromStdString("Target: %1 mm"), 200, 0, this)),
    m_PlannedTargetProgressBar(nullptr),
    m_CurrentTargetIndex(0),
    m_CurrentTargetReached(false),
    m_ShowPlanningColors(false),
    m_PointMarkInteractor(mitk::USPointMarkInteractor::New()),
    m_TargetNode(nullptr),
    m_TargetColorLookupTableProperty(nullptr),
    m_TargetSurface(nullptr),
    m_NeedleProjectionFilter(mitk::NeedleProjectionFilter::New()),
    m_NodeDisplacementFilter(mitk::NodeDisplacementFilter::New()),
    m_TargetUpdateFilter(mitk::USNavigationTargetUpdateFilter::New()),
    m_TargetOcclusionFilter(mitk::USNavigationTargetOcclusionFilter::New()),
    m_TargetIntersectionFilter(mitk::USNavigationTargetIntersectionFilter::New()),
    m_PlacementQualityCalculator(mitk::USTargetPlacementQualityCalculator::New()),
    m_TargetStructureWarnOverlay(mitk::TextAnnotation2D::New()),
    m_ReferenceSensorName(),
    m_NeedleSensorName(),
    m_ReferenceSensorIndex(1),
    m_NeedleSensorIndex(0),
    m_ListenerTargetCoordinatesChanged(this, &QmitkUSNavigationStepMarkerIntervention::UpdateTargetCoordinates),
    ui(new Ui::QmitkUSNavigationStepMarkerIntervention)
{
  m_ActiveTargetColor[0] = 1;
  m_ActiveTargetColor[1] = 1;
  m_ActiveTargetColor[2] = 0;
  m_InactiveTargetColor[0] = 1;
  m_InactiveTargetColor[1] = 1;
  m_InactiveTargetColor[2] = 0.5;
  m_ReachedTargetColor[0] = 0.6;
  m_ReachedTargetColor[1] = 1;
  m_ReachedTargetColor[2] = 0.6;
 
  ui->setupUi(this);
 
  connect(ui->freezeImageButton, SIGNAL(SignalFreezed(bool)), this, SLOT(OnFreeze(bool)));
  connect(ui->backToLastTargetButton, SIGNAL(clicked()), this, SLOT(OnBackToLastTargetClicked()));
  connect(ui->targetReachedButton, SIGNAL(clicked()), this, SLOT(OnTargetLeft()));
  connect(this, SIGNAL(TargetReached(int)), this, SLOT(OnTargetReached()));
  connect(this, SIGNAL(TargetLeft(int)), this, SLOT(OnTargetLeft()));
  connect(ui->riskStructuresRangeWidget,
          SIGNAL(SignalZoneViolated(const mitk::DataNode *, mitk::Point3D)),
          this,
          SLOT(OnRiskZoneViolated(const mitk::DataNode *, mitk::Point3D)));
 
  m_PointMarkInteractor->CoordinatesChangedEvent.AddListener(m_ListenerTargetCoordinatesChanged);
 
  this->GenerateTargetColorLookupTable();
 
  m_TargetProgressBar->SetTextFormatInvalid("Target is not on Needle Path");
  ui->targetStructuresRangeLayout->addWidget(m_TargetProgressBar);
 
  m_TargetUpdateFilter->SetScalarArrayIdentifier("USNavigation::ReachedTargetScores");
}
 
QmitkUSNavigationStepMarkerIntervention::~QmitkUSNavigationStepMarkerIntervention()
{
  mitk::DataStorage::Pointer dataStorage = this->GetDataStorage(false);
  if (dataStorage.IsNotNull())
  {
    // remove the node for the needle path
    mitk::DataNode::Pointer node =
      this->GetNamedDerivedNode("Needle Path", QmitkUSAbstractNavigationStep::DATANAME_BASENODE);
    if (node.IsNotNull())
    {
      dataStorage->Remove(node);
    }
  }
 
  delete ui;
 
  m_PointMarkInteractor->CoordinatesChangedEvent.RemoveListener(m_ListenerTargetCoordinatesChanged);
}
 
bool QmitkUSNavigationStepMarkerIntervention::OnStartStep()
{
  m_NeedleProjectionFilter->SelectInput(m_NeedleSensorIndex);
 
  // create node for Needle Projection
  mitk::DataNode::Pointer node =
    this->GetNamedDerivedNodeAndCreate("Needle Path", QmitkUSAbstractNavigationStep::DATANAME_BASENODE);
  node->SetData(m_NeedleProjectionFilter->GetProjection());
  node->SetBoolProperty("show contour", true);
 
  // initialize warning overlay (and do not display it, yet)
  m_TargetStructureWarnOverlay->SetText("Warning: Needle is Inside the Target Structure.");
  m_TargetStructureWarnOverlay->SetVisibility(false);
 
  // set position and font size for the text overlay
  mitk::Point2D overlayPosition;
  overlayPosition.SetElement(0, 10.0f);
  overlayPosition.SetElement(1, 10.0f);
  m_TargetStructureWarnOverlay->SetPosition2D(overlayPosition);
  m_TargetStructureWarnOverlay->SetFontSize(18);
 
  // overlay should be red
  mitk::Color color;
  color[0] = 1;
  color[1] = 0;
  color[2] = 0;
  m_TargetStructureWarnOverlay->SetColor(color);
 
  auto allRenderWindows = mitk::BaseRenderer::GetAll3DRenderWindows();
  for (auto mapit = allRenderWindows.begin(); mapit != allRenderWindows.end(); ++mapit)
  {
    mitk::ManualPlacementAnnotationRenderer::AddAnnotation(m_TargetStructureWarnOverlay.GetPointer(), mapit->second);
  }
 
  return true;
}
 
bool QmitkUSNavigationStepMarkerIntervention::OnStopStep()
{
  mitk::DataStorage::Pointer dataStorage = this->GetDataStorage();
 
  // remove all reached nodes from the data storage
  for (QVector<itk::SmartPointer<mitk::DataNode>>::iterator it = m_ReachedTargetsNodes.begin();
       it != m_ReachedTargetsNodes.end();
       ++it)
  {
    dataStorage->Remove(*it);
  }
  m_ReachedTargetsNodes.clear();
  m_CurrentTargetIndex = 0;
 
  // reset button states
  ui->freezeImageButton->setEnabled(false);
  ui->backToLastTargetButton->setEnabled(false);
  ui->targetReachedButton->setEnabled(true);
 
  // make sure that it is unfreezed after stopping the step
  ui->freezeImageButton->Unfreeze();
 
  // remove base node for reached targets from the data storage
  mitk::DataNode::Pointer reachedTargetsNode = this->GetNamedDerivedNode(
    QmitkUSAbstractNavigationStep::DATANAME_BASENODE, QmitkUSNavigationMarkerPlacement::DATANAME_REACHED_TARGETS);
  if (reachedTargetsNode.IsNotNull())
  {
    dataStorage->Remove(reachedTargetsNode);
  }
 
  return true;
}
 
bool QmitkUSNavigationStepMarkerIntervention::OnFinishStep()
{
  return true;
}
 
bool QmitkUSNavigationStepMarkerIntervention::OnActivateStep()
{
  this->ClearZones(); // clear risk zones before adding new ones
 
  // get target node from data storage and make sure that it contains data
  m_TargetNode = this->GetNamedDerivedNode(QmitkUSNavigationMarkerPlacement::DATANAME_TARGETSURFACE,
    QmitkUSNavigationMarkerPlacement::DATANAME_TUMOUR);
  if (m_TargetNode.IsNull() || m_TargetNode->GetData() == nullptr)
  {
    mitkThrow() << "Target node (" << QmitkUSNavigationMarkerPlacement::DATANAME_TARGETSURFACE << ") must not be null.";
  }
 
  // get target data and make sure that it is a surface
  m_TargetSurface = dynamic_cast<mitk::Surface *>(m_TargetNode->GetData());
  if (m_TargetSurface.IsNull())
  {
    mitkThrow() << "Target node (" << QmitkUSNavigationMarkerPlacement::DATANAME_TARGETSURFACE
                << ") data must be of type mitk::Surface";
  }
 
  // delete progress bars for reinitializing them again afterwards
  if (m_PlannedTargetProgressBar)
  {
    ui->targetStructuresRangeLayout->removeWidget(m_PlannedTargetProgressBar);
    delete m_PlannedTargetProgressBar;
    m_PlannedTargetProgressBar = nullptr;
  }
 
  m_NodeDisplacementFilter->SelectInput(m_ReferenceSensorIndex);
 
  this->UpdateTargetProgressDisplay();
 
  mitk::DataNode::Pointer tumourNode = this->GetNamedDerivedNode(QmitkUSNavigationMarkerPlacement::DATANAME_TUMOUR,
                                                                 QmitkUSAbstractNavigationStep::DATANAME_BASENODE);
  if (tumourNode.IsNotNull())
  {
    // do not show tumour node during intervention (target surface is shown)
    tumourNode->SetBoolProperty("visible", false);
 
    // add tumour as a risk structure
    ui->riskStructuresRangeWidget->AddZone(tumourNode);
  }
 
  // set target structure for target update filter
  m_TargetUpdateFilter->SetTargetStructure(m_TargetNode);
  m_TargetOcclusionFilter->SetTargetStructure(m_TargetNode);
 
  // set lookup table of tumour node
  m_TargetNode->SetProperty("LookupTable", m_TargetColorLookupTableProperty);
 
  //
  mitk::DataNode::Pointer targetsBaseNode = this->GetNamedDerivedNode(QmitkUSNavigationMarkerPlacement::DATANAME_TARGETS,
                                                                      QmitkUSAbstractNavigationStep::DATANAME_BASENODE);
  mitk::DataStorage::SetOfObjects::ConstPointer plannedTargetNodes;
  if (targetsBaseNode.IsNotNull())
  {
    plannedTargetNodes = this->GetDataStorage()->GetDerivations(targetsBaseNode);
  }
  if (plannedTargetNodes.IsNotNull() && plannedTargetNodes->Size() > 0)
  {
    for (mitk::DataStorage::SetOfObjects::ConstIterator it = plannedTargetNodes->Begin();
         it != plannedTargetNodes->End();
         ++it)
    {
      m_PlannedTargetsNodes.push_back(it->Value());
    }
 
    m_PlannedTargetProgressBar = new QmitkZoneProgressBar(QString::fromStdString("Planned Target"), 200, 0);
    ui->targetStructuresRangeLayout->addWidget(m_PlannedTargetProgressBar);
  }
 
  // add progress bars for risk zone nodes
  mitk::DataNode::Pointer zonesBaseNode = this->GetNamedDerivedNode(QmitkUSNavigationMarkerPlacement::DATANAME_ZONES,
                                                                    QmitkUSAbstractNavigationStep::DATANAME_BASENODE);
  // only add progress bars if the base node for zones was created
  if (zonesBaseNode.IsNotNull())
  {
    mitk::DataStorage::SetOfObjects::ConstPointer zoneNodes = this->GetDataStorage()->GetDerivations(zonesBaseNode);
 
    for (mitk::DataStorage::SetOfObjects::ConstIterator it = zoneNodes->Begin(); it != zoneNodes->End(); ++it)
    {
      ui->riskStructuresRangeWidget->AddZone(it->Value());
    }
 
    m_TargetOcclusionFilter->SelectStartPositionInput(m_NeedleSensorIndex);
    m_TargetOcclusionFilter->SetObstacleStructures(zoneNodes);
  }
 
  return true;
}
 
bool QmitkUSNavigationStepMarkerIntervention::OnDeactivateStep()
{
  ui->freezeImageButton->Unfreeze();
 
  return true;
}
 
void QmitkUSNavigationStepMarkerIntervention::OnUpdate()
{
  // get navigation data source and make sure that it is not null
  mitk::NavigationDataSource::Pointer navigationDataSource = this->GetCombinedModality()->GetNavigationDataSource();
  if (navigationDataSource.IsNull())
  {
    MITK_ERROR("QmitkUSAbstractNavigationStep")
    ("QmitkUSNavigationStepMarkerIntervention") << "Navigation Data Source of Combined Modality must not be null.";
    mitkThrow() << "Navigation Data Source of Combined Modality must not be null.";
  }
 
  ui->riskStructuresRangeWidget->UpdateDistancesToNeedlePosition(navigationDataSource->GetOutput(m_NeedleSensorIndex));
  this->UpdateBodyMarkerStatus(navigationDataSource->GetOutput(m_ReferenceSensorIndex));
 
  this->UpdateTargetColors();
  this->UpdateTargetScore();
 
  this->UpdateTargetViolationStatus();
}
 
void QmitkUSNavigationStepMarkerIntervention::OnSettingsChanged(const itk::SmartPointer<mitk::DataNode> settingsNode)
{
  if (settingsNode.IsNull())
  {
    return;
  }
 
  int numberOfTargets;
  if (settingsNode->GetIntProperty("settings.number-of-targets", numberOfTargets))
  {
    m_NumberOfTargets = numberOfTargets;
    m_TargetUpdateFilter->SetNumberOfTargets(numberOfTargets);
 
    m_PlacementQualityCalculator->SetOptimalAngle(m_TargetUpdateFilter->GetOptimalAngle());
  }
 
  std::string referenceSensorName;
  if (settingsNode->GetStringProperty("settings.reference-name-selected", referenceSensorName))
  {
    m_ReferenceSensorName = referenceSensorName;
  }
 
  std::string needleSensorName;
  if (settingsNode->GetStringProperty("settings.needle-name-selected", needleSensorName))
  {
    m_NeedleSensorName = needleSensorName;
  }
 
  this->UpdateSensorsNames();
}
 
QString QmitkUSNavigationStepMarkerIntervention::GetTitle()
{
  return "Computer-assisted Intervention";
}
 
bool QmitkUSNavigationStepMarkerIntervention::GetIsRestartable()
{
  return true;
}
 
QmitkUSAbstractNavigationStep::FilterVector QmitkUSNavigationStepMarkerIntervention::GetFilter()
{
  FilterVector filter;
  filter.push_back(m_NeedleProjectionFilter.GetPointer());
  filter.push_back(m_NodeDisplacementFilter.GetPointer());
  filter.push_back(m_TargetOcclusionFilter.GetPointer());
  return filter;
}
 
void QmitkUSNavigationStepMarkerIntervention::OnSetCombinedModality()
{
  mitk::AbstractUltrasoundTrackerDevice::Pointer combinedModality = this->GetCombinedModality(false);
  if (combinedModality.IsNotNull())
  {
    mitk::AffineTransform3D::Pointer calibration = combinedModality->GetCalibration();
    if (calibration.IsNotNull())
    {
      m_NeedleProjectionFilter->SetTargetPlane(calibration);
    }
  }
 
  ui->freezeImageButton->SetCombinedModality(combinedModality, m_ReferenceSensorIndex);
 
  this->UpdateSensorsNames();
}
 
void QmitkUSNavigationStepMarkerIntervention::OnTargetReached()
{
  m_CurrentTargetReached = true;
}
 
void QmitkUSNavigationStepMarkerIntervention::OnTargetLeft()
{
  m_CurrentTargetReached = false;
 
  m_CurrentTargetIndex++;
 
  if (m_CurrentTargetIndex >= 0 && static_cast<unsigned int>(m_CurrentTargetIndex) >= m_NumberOfTargets)
  {
    ui->targetReachedButton->setDisabled(true);
  }
 
  ui->backToLastTargetButton->setEnabled(true);
  ui->freezeImageButton->setEnabled(true);
 
  this->UpdateTargetProgressDisplay();
 
  if (m_ReachedTargetsNodes.size() < m_CurrentTargetIndex)
  {
    mitk::DataNode::Pointer node = mitk::DataNode::New();
    node->SetName(
      (QString("Target ") + QString("%1").arg(m_CurrentTargetIndex, 2, 10, QLatin1Char('0'))).toStdString());
    this->GetDataStorage()->Add(
      node,
      this->GetNamedDerivedNodeAndCreate(QmitkUSNavigationMarkerPlacement::DATANAME_REACHED_TARGETS,
                                         QmitkUSAbstractNavigationStep::DATANAME_BASENODE));
    m_ReachedTargetsNodes.push_back(node);
  }
 
  mitk::DataNode::Pointer node = m_ReachedTargetsNodes.at(m_CurrentTargetIndex - 1);
  mitk::Surface::Pointer zone = mitk::Surface::New();
 
  // create a vtk sphere with given radius
  vtkSmartPointer<vtkSphereSource> vtkSphere = vtkSmartPointer<vtkSphereSource>::New();
  vtkSphere->SetRadius(5);
  vtkSphere->SetCenter(0, 0, 0);
  vtkSphere->Update();
  zone->SetVtkPolyData(vtkSphere->GetOutput());
 
  // set vtk sphere and origin to data node
  node->SetData(zone);
  node->GetData()->GetGeometry()->SetOrigin(
    this->GetCombinedModality()->GetNavigationDataSource()->GetOutput(m_NeedleSensorIndex)->GetPosition());
  node->SetColor(0.2, 0.9, 0.2);
  this->UpdateTargetCoordinates(node);
}
 
void QmitkUSNavigationStepMarkerIntervention::OnBackToLastTargetClicked()
{
  if (m_CurrentTargetIndex < 1)
  {
    MITK_WARN << "Cannot go back to last target as there is no last target.";
    return;
  }
 
  m_CurrentTargetIndex--;
 
  if (m_ReachedTargetsNodes.size() > m_CurrentTargetIndex)
  {
    this->GetDataStorage()->Remove(m_ReachedTargetsNodes.last());
    MITK_INFO("QmitkUSAbstractNavigationStep")
    ("QmitkUSNavigationStepMarkerIntervention") << "Removed Target " << m_ReachedTargetsNodes.size();
    m_ReachedTargetsNodes.pop_back();
  }
 
  if (m_CurrentTargetIndex == 0)
  {
    ui->backToLastTargetButton->setDisabled(true);
  }
  if (m_CurrentTargetIndex >= 0 && static_cast<unsigned int>(m_CurrentTargetIndex) < m_NumberOfTargets)
  {
    ui->targetReachedButton->setEnabled(true);
  }
 
  ui->freezeImageButton->setEnabled(false);
  ui->freezeImageButton->Unfreeze();
 
  this->UpdateTargetProgressDisplay();
 
  m_TargetUpdateFilter->RemovePositionOfTarget(m_CurrentTargetIndex);
}
 
void QmitkUSNavigationStepMarkerIntervention::OnFreeze(bool freezed)
{
  if (freezed)
  {
    this->GetCombinedModality()->SetIsFreezed(true);
    // load state machine and event config for data interactor
    m_PointMarkInteractor->LoadStateMachine("USPointMarkInteractions.xml", us::ModuleRegistry::GetModule("MitkUS"));
    m_PointMarkInteractor->SetEventConfig("globalConfig.xml");
 
    if (m_CurrentTargetIndex < 1)
    {
      mitkThrow() << "Current target index has to be greater zero when freeze button is clicked.";
    }
 
    if (m_ReachedTargetsNodes.size() < m_CurrentTargetIndex)
    {
      mitk::DataNode::Pointer node = mitk::DataNode::New();
      node->SetName(
        (QString("Target ") + QString("%1").arg(m_CurrentTargetIndex, 2, 10, QLatin1Char('0'))).toStdString());
      this->GetDataStorage()->Add(
        node,
        this->GetNamedDerivedNodeAndCreate(QmitkUSNavigationMarkerPlacement::DATANAME_REACHED_TARGETS,
                                           QmitkUSAbstractNavigationStep::DATANAME_BASENODE));
      m_ReachedTargetsNodes.push_back(node);
    }
 
    m_PointMarkInteractor->SetDataNode(m_ReachedTargetsNodes.last());
  }
  else
  {
    m_PointMarkInteractor->SetDataNode(nullptr);
    this->GetCombinedModality()->SetIsFreezed(false);
  }
}
 
void QmitkUSNavigationStepMarkerIntervention::OnShowPlanningView(bool show)
{
  m_ShowPlanningColors = show;
}
 
void QmitkUSNavigationStepMarkerIntervention::OnRiskZoneViolated(const mitk::DataNode *node, mitk::Point3D position)
{
  MITK_INFO << "Risk zone (" << node->GetName() << ") violated at position " << position << ".";
}
 
void QmitkUSNavigationStepMarkerIntervention::ClearZones()
{
  ui->riskStructuresRangeWidget->ClearZones();
 
  // remove all reached target nodes from the data storage and clear the list
  mitk::DataStorage::Pointer dataStorage = this->GetDataStorage();
  for (QVector<mitk::DataNode::Pointer>::iterator it = m_ReachedTargetsNodes.begin(); it != m_ReachedTargetsNodes.end();
       ++it)
  {
    if (it->IsNotNull())
    {
      dataStorage->Remove(*it);
    }
  }
  m_ReachedTargetsNodes.clear();
}
 
void QmitkUSNavigationStepMarkerIntervention::UpdateTargetCoordinates(mitk::DataNode *dataNode)
{
  m_NodeDisplacementFilter->ResetNodes();
  for (QVector<itk::SmartPointer<mitk::DataNode>>::iterator it = m_ReachedTargetsNodes.begin();
       it != m_ReachedTargetsNodes.end();
       ++it)
  {
    if (it->IsNotNull() && (*it)->GetData() != nullptr)
    {
      m_NodeDisplacementFilter->AddNode(*it);
    }
  }
 
  mitk::BaseData *baseData = dataNode->GetData();
  if (!baseData)
  {
    mitkThrow() << "Data of the data node must not be null.";
  }
 
  mitk::BaseGeometry::Pointer geometry = baseData->GetGeometry();
  if (geometry.IsNull())
  {
    mitkThrow() << "Geometry of the data node must not be null.";
  }
 
  m_TargetUpdateFilter->SetControlNode(m_CurrentTargetIndex - 1, dataNode);
 
  if (m_PlannedTargetsNodes.size() > m_CurrentTargetIndex - 1)
  {
    m_PlannedTargetsNodes.at(m_CurrentTargetIndex - 1)->SetVisibility(false);
  }
 
  MITK_INFO("QmitkUSAbstractNavigationStep")
  ("QmitkUSNavigationStepMarkerIntervention") << "Target " << m_CurrentTargetIndex << " reached at position "
                                              << geometry->GetOrigin();
 
  this->CalculateTargetPlacementQuality();
}
 
void QmitkUSNavigationStepMarkerIntervention::UpdateBodyMarkerStatus(mitk::NavigationData::Pointer bodyMarker)
{
  if (bodyMarker.IsNull())
  {
    MITK_ERROR("QmitkUSAbstractNavigationStep")
    ("QmitkUSNavigationStepMarkerIntervention")
      << "Current Navigation Data for body marker of Combined Modality must not be null.";
    mitkThrow() << "Current Navigation Data for body marker of Combined Modality must not be null.";
  }
 
  bool valid = bodyMarker->IsDataValid();
 
  // update body marker status label
  if (valid)
  {
    ui->bodyMarkerTrackingStatusLabel->setStyleSheet(
      "background-color: #8bff8b; margin-right: 1em; margin-left: 1em; border: 1px solid grey");
    ui->bodyMarkerTrackingStatusLabel->setText("Body marker is inside the tracking volume.");
  }
  else
  {
    ui->bodyMarkerTrackingStatusLabel->setStyleSheet(
      "background-color: #ff7878; margin-right: 1em; margin-left: 1em; border: 1px solid grey");
    ui->bodyMarkerTrackingStatusLabel->setText("Body marker is not inside the tracking volume.");
  }
 
  ui->targetStructuresRangeGroupBox->setEnabled(valid);
  ui->riskStructuresRangeGroupBox->setEnabled(valid);
}
 
void QmitkUSNavigationStepMarkerIntervention::GenerateTargetColorLookupTable()
{
  vtkSmartPointer<vtkLookupTable> lookupTable = vtkSmartPointer<vtkLookupTable>::New();
  lookupTable->SetHueRange(0.0, 0.33);
  lookupTable->SetSaturationRange(1.0, 1.0);
  lookupTable->SetValueRange(1.0, 1.0);
  lookupTable->SetTableRange(0.0, 1.0);
  lookupTable->Build();
 
  mitk::LookupTable::Pointer lut = mitk::LookupTable::New();
  lut->SetVtkLookupTable(lookupTable);
 
  m_TargetColorLookupTableProperty = mitk::LookupTableProperty::New(lut);
}
 
void QmitkUSNavigationStepMarkerIntervention::UpdateTargetColors()
{
  if (m_TargetNode.IsNull())
  {
    return;
  }
 
  m_TargetNode->SetColor(1, 1, 1);
 
  mitk::BaseData *targetNodeData = m_TargetNode->GetData();
  if (targetNodeData == nullptr)
  {
    return;
  }
 
  mitk::Surface::Pointer targetNodeSurface = dynamic_cast<mitk::Surface *>(targetNodeData);
  vtkSmartPointer<vtkPolyData> targetNodeSurfaceVtk = targetNodeSurface->GetVtkPolyData();
 
  vtkPointData *targetPointData = targetNodeSurface->GetVtkPolyData()->GetPointData();
 
  vtkFloatArray *scalars = dynamic_cast<vtkFloatArray *>(targetPointData->GetScalars("USNavigation::Occlusion"));
  vtkFloatArray *targetScoreScalars;
 
  if (m_ShowPlanningColors)
  {
    targetScoreScalars = dynamic_cast<vtkFloatArray *>(targetPointData->GetScalars("USNavigation::PlanningScalars"));
  }
  else
  {
    targetScoreScalars =
      dynamic_cast<vtkFloatArray *>(targetPointData->GetScalars("USNavigation::ReachedTargetScores"));
  }
 
  if (!scalars || !targetScoreScalars)
  {
    return;
  }
 
  unsigned int numberOfTupels = scalars->GetNumberOfTuples();
 
  vtkSmartPointer<vtkFloatArray> colors = vtkSmartPointer<vtkFloatArray>::New();
  colors->SetNumberOfComponents(1);
  colors->SetNumberOfTuples(numberOfTupels);
  colors->SetName("Colors");
 
  double color, intersection, markerScore;
 
  for (unsigned int n = 0; n < numberOfTupels; n++)
  {
    scalars->GetTuple(n, &intersection);
    targetScoreScalars->GetTuple(n, &markerScore);
 
    if (intersection > 0)
    {
      color = 0;
    }
    else
    {
      color = markerScore;
    }
 
    colors->SetTuple(n, &color);
  }
 
  if (numberOfTupels > 0)
  {
    targetNodeSurfaceVtk->GetPointData()->SetScalars(colors);
    targetNodeSurfaceVtk->GetPointData()->Update();
  }
}
 
void QmitkUSNavigationStepMarkerIntervention::UpdateTargetScore()
{
  if (m_NeedleProjectionFilter->GetProjection()->GetSize() != 2)
  {
    return;
  }
 
  vtkSmartPointer<vtkPolyData> targetSurfaceVtk = m_TargetSurface->GetVtkPolyData();
 
  m_TargetIntersectionFilter->SetTargetSurface(m_TargetSurface);
  m_TargetIntersectionFilter->SetLine(m_NeedleProjectionFilter->GetProjection());
 
  m_TargetIntersectionFilter->CalculateIntersection();
 
  if (m_TargetIntersectionFilter->GetIsIntersecting())
  {
    vtkFloatArray *scalars =
      dynamic_cast<vtkFloatArray *>(targetSurfaceVtk->GetPointData()->GetScalars("USNavigation::ReachedTargetScores"));
    double score;
    scalars->GetTuple(m_TargetIntersectionFilter->GetIntersectionNearestSurfacePointId(), &score);
 
    double color[3];
    m_TargetColorLookupTableProperty->GetLookupTable()->GetVtkLookupTable()->GetColor(score, color);
 
    float colorF[3];
    colorF[0] = color[0];
    colorF[1] = color[1];
    colorF[2] = color[2];
    m_TargetProgressBar->SetColor(colorF);
    m_TargetProgressBar->SetBorderColor(colorF);
    m_TargetProgressBar->setValue(m_TargetIntersectionFilter->GetDistanceToIntersection());
 
    if (m_PlannedTargetProgressBar)
    {
      vtkFloatArray *scalars =
        dynamic_cast<vtkFloatArray *>(targetSurfaceVtk->GetPointData()->GetScalars("USNavigation::PlanningScalars"));
      if (scalars)
      {
        double score;
        scalars->GetTuple(m_TargetIntersectionFilter->GetIntersectionNearestSurfacePointId(), &score);
 
        double color[3];
        m_TargetColorLookupTableProperty->GetLookupTable()->GetVtkLookupTable()->GetColor(score, color);
 
        float colorF[3];
        colorF[0] = color[0];
        colorF[1] = color[1];
        colorF[2] = color[2];
        m_PlannedTargetProgressBar->SetColor(colorF);
        m_PlannedTargetProgressBar->SetBorderColor(colorF);
        m_PlannedTargetProgressBar->SetTextFormatValid("Planned Target: %1 mm");
 
        mitk::Point3D intersectionPoint = m_TargetIntersectionFilter->GetIntersectionPoint();
 
        mitk::ScalarType minDistance = -1;
        for (QVector<itk::SmartPointer<mitk::DataNode>>::iterator it = m_PlannedTargetsNodes.begin();
             it != m_PlannedTargetsNodes.end();
             ++it)
        {
          mitk::ScalarType distance =
            intersectionPoint.EuclideanDistanceTo((*it)->GetData()->GetGeometry()->GetOrigin());
          if (minDistance < 0 || distance < minDistance)
          {
            minDistance = distance;
          }
        }
 
        m_PlannedTargetProgressBar->setValue(minDistance);
      }
    }
  }
  else
  {
    m_TargetProgressBar->setValueInvalid();
  }
}
 
void QmitkUSNavigationStepMarkerIntervention::UpdateTargetProgressDisplay()
{
  QString description;
  if (m_CurrentTargetIndex >= static_cast<int>(m_NumberOfTargets))
  {
    description = "All Targets Reached";
    if (m_TargetProgressBar)
    {
      m_TargetProgressBar->hide();
    }
  }
  else
  {
    description = QString("Distance to Target ") + QString::number(m_CurrentTargetIndex + 1) + QString(" of ") +
                  QString::number(m_NumberOfTargets);
    if (m_TargetProgressBar)
    {
      m_TargetProgressBar->show();
    }
  }
 
  ui->targetStructuresRangeGroupBox->setTitle(description);
}
 
void QmitkUSNavigationStepMarkerIntervention::UpdatePlannedTargetProgressDisplay()
{
  // make sure that the needle projection consists of two points
  if (m_NeedleProjectionFilter->GetProjection()->GetSize() != 2)
  {
    return;
  }
 
  vtkSmartPointer<vtkPolyData> targetSurfaceVtk = m_TargetSurface->GetVtkPolyData();
 
  m_TargetIntersectionFilter->SetTargetSurface(m_TargetSurface);
  m_TargetIntersectionFilter->SetLine(m_NeedleProjectionFilter->GetProjection());
 
  m_TargetIntersectionFilter->CalculateIntersection();
 
  // update target progress bar according to the color of the intersection
  // point on the target surface and the distance to the intersection
  if (m_TargetIntersectionFilter->GetIsIntersecting())
  {
    vtkFloatArray *scalars = dynamic_cast<vtkFloatArray *>(targetSurfaceVtk->GetPointData()->GetScalars("Colors"));
    double score;
    scalars->GetTuple(m_TargetIntersectionFilter->GetIntersectionNearestSurfacePointId(), &score);
 
    double color[3];
    m_TargetColorLookupTableProperty->GetLookupTable()->GetVtkLookupTable()->GetColor(score, color);
 
    float colorF[3];
    colorF[0] = color[0];
    colorF[1] = color[1];
    colorF[2] = color[2];
    m_TargetProgressBar->SetColor(colorF);
    m_TargetProgressBar->SetBorderColor(colorF);
    m_TargetProgressBar->setValue(m_TargetIntersectionFilter->GetDistanceToIntersection());
  }
  else
  {
    float red[3] = {0.6f, 0.0f, 0.0f};
    m_TargetProgressBar->SetBorderColor(red);
    m_TargetProgressBar->setValueInvalid();
  }
}
 
void QmitkUSNavigationStepMarkerIntervention::UpdateTargetViolationStatus()
{
  // transform vtk polydata according to mitk geometry
  vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
  transformFilter->SetInputData(0, m_TargetSurface->GetVtkPolyData());
  transformFilter->SetTransform(m_TargetSurface->GetGeometry()->GetVtkTransform());
  transformFilter->Update();
 
  vtkSmartPointer<vtkSelectEnclosedPoints> enclosedPoints = vtkSmartPointer<vtkSelectEnclosedPoints>::New();
  enclosedPoints->Initialize(transformFilter->GetOutput());
 
  mitk::Point3D needleTip = m_NeedleProjectionFilter->GetProjection()->GetPoint(0);
 
  // show warning if the needle tip is inside the target surface
  if (enclosedPoints->IsInsideSurface(needleTip[0], needleTip[1], needleTip[2]))
  {
    if (!m_TargetStructureWarnOverlay->IsVisible(nullptr))
    {
      m_TargetStructureWarnOverlay->SetVisibility(true);
 
      mitk::DataNode::Pointer targetViolationResult = mitk::DataNode::New();
      targetViolationResult->SetName("TargetViolation");
      targetViolationResult->SetProperty("USNavigation::TargetViolationPoint", mitk::Point3dProperty::New(needleTip));
 
      emit SignalIntermediateResult(targetViolationResult);
    }
 
    MITK_INFO("QmitkUSAbstractNavigationStep")
    ("QmitkUSNavigationStepMarkerIntervention") << "Target surface violated at " << needleTip << ".";
  }
  else
  {
    m_TargetStructureWarnOverlay->SetVisibility(false);
  }
}
 
void QmitkUSNavigationStepMarkerIntervention::CalculateTargetPlacementQuality()
{
  // clear quality display if there aren't all targets reached
  if (m_ReachedTargetsNodes.size() != static_cast<int>(m_NumberOfTargets))
  {
    ui->placementQualityGroupBox->setEnabled(false);
    ui->angleDifferenceValue->setText("");
    ui->centersOfMassValue->setText("");
    return;
  }
 
  ui->placementQualityGroupBox->setEnabled(true);
 
  mitk::Surface::Pointer targetSurface = dynamic_cast<mitk::Surface *>(m_TargetNode->GetData());
  if (targetSurface.IsNull())
  {
    mitkThrow() << "Target surface must not be null.";
  }
 
  m_PlacementQualityCalculator->SetTargetSurface(targetSurface);
  mitk::PointSet::Pointer targetPointSet = mitk::PointSet::New();
 
  // copy the origins of all reached target nodes into a point set
  // for the quality calculator
  mitk::PointSet::PointIdentifier n = 0;
  for (QVector<itk::SmartPointer<mitk::DataNode>>::iterator it = m_ReachedTargetsNodes.begin();
       it != m_ReachedTargetsNodes.end();
       ++it)
  {
    targetPointSet->InsertPoint(n++, (*it)->GetData()->GetGeometry()->GetOrigin());
  }
  m_PlacementQualityCalculator->SetTargetPoints(targetPointSet);
 
  m_PlacementQualityCalculator->Update();
 
  double centersOfMassDistance = m_PlacementQualityCalculator->GetCentersOfMassDistance();
  ui->centersOfMassValue->setText(QString::number(centersOfMassDistance, 103, 2) + " mm");
 
  double meanAnglesDifference = m_PlacementQualityCalculator->GetMeanAngleDifference();
  ui->angleDifferenceValue->setText(QString::number(meanAnglesDifference, 103, 2) + QString::fromLatin1(" °"));
 
  // create an intermediate result of the placement quality
  mitk::DataNode::Pointer placementQualityResult = mitk::DataNode::New();
  placementQualityResult->SetName("PlacementQuality");
  placementQualityResult->SetFloatProperty("USNavigation::CentersOfMassDistance", centersOfMassDistance);
  placementQualityResult->SetFloatProperty("USNavigation::MeanAngleDifference", meanAnglesDifference);
  placementQualityResult->SetProperty(
    "USNavigation::AngleDifferences",
    mitk::GenericProperty<mitk::VnlVector>::New(m_PlacementQualityCalculator->GetAngleDifferences()));
 
  if (m_PlannedTargetsNodes.size() == static_cast<int>(m_NumberOfTargets))
  {
    mitk::VnlVector reachedPlannedDifferences;
    double reachedPlannedDifferencesSum = 0;
    double reachedPlannedDifferencesMax = 0;
    reachedPlannedDifferences.set_size(m_NumberOfTargets);
 
    // get sum and maximum of the planning / reality differences
    for (unsigned int n = 0; n < m_NumberOfTargets; ++n)
    {
      mitk::ScalarType distance =
        m_PlannedTargetsNodes.at(n)->GetData()->GetGeometry()->GetOrigin().EuclideanDistanceTo(
          m_ReachedTargetsNodes.at(n)->GetData()->GetGeometry()->GetOrigin());
 
      reachedPlannedDifferences.put(n, distance);
      reachedPlannedDifferencesSum += distance;
 
      if (distance > reachedPlannedDifferencesMax)
      {
        reachedPlannedDifferencesMax = distance;
      }
    }
 
    // add distances between planning and reality to the quality intermediate result
    placementQualityResult->SetProperty("USNavigation::PlanningRealityDistances",
                                        mitk::GenericProperty<mitk::VnlVector>::New(reachedPlannedDifferences));
    placementQualityResult->SetProperty(
      "USNavigation::MeanPlanningRealityDistance",
      mitk::DoubleProperty::New(reachedPlannedDifferencesSum / static_cast<double>(m_NumberOfTargets)));
    placementQualityResult->SetProperty("USNavigation::MaximumPlanningRealityDistance",
                                        mitk::DoubleProperty::New(reachedPlannedDifferencesMax));
  }
 
  emit SignalIntermediateResult(placementQualityResult);
}
 
void QmitkUSNavigationStepMarkerIntervention::UpdateSensorsNames()
{
  mitk::AbstractUltrasoundTrackerDevice::Pointer combinedModality = this->GetCombinedModality(false);
  if (combinedModality.IsNull())
  {
    return;
  }
 
  mitk::NavigationDataSource::Pointer navigationDataSource = combinedModality->GetNavigationDataSource();
  if (navigationDataSource.IsNull())
  {
    return;
  }
 
  if (!m_NeedleSensorName.empty())
  {
    try
    {
      m_NeedleSensorIndex = navigationDataSource->GetOutputIndex(m_NeedleSensorName);
    }
    catch (const std::exception &e)
    {
      MITK_WARN("QmitkUSAbstractNavigationStep")
      ("QmitkUSNavigationStepPlacementPlanning") << "Cannot get index for needle sensor name: " << e.what();
    }
  }
  if (this->GetNavigationStepState() >= QmitkUSAbstractNavigationStep::State_Active)
  {
    m_NeedleProjectionFilter->SelectInput(m_NeedleSensorIndex);
  }
 
  if (!m_ReferenceSensorName.empty())
  {
    try
    {
      m_ReferenceSensorIndex = navigationDataSource->GetOutputIndex(m_ReferenceSensorName);
    }
    catch (const std::exception &e)
    {
      MITK_WARN("QmitkUSAbstractNavigationStep")
      ("QmitkUSNavigationStepPlacementPlanning") << "Cannot get index for reference sensor name: " << e.what();
    }
  }
  if (this->GetNavigationStepState() >= QmitkUSAbstractNavigationStep::State_Active)
  {
    m_NodeDisplacementFilter->SelectInput(m_ReferenceSensorIndex);
  }
 
  ui->freezeImageButton->SetCombinedModality(combinedModality, m_ReferenceSensorIndex);
}