diff --git a/Modules/US/USNavigation/Filter/mitkNodeDisplacementFilter.cpp b/Modules/US/USNavigation/Filter/mitkNodeDisplacementFilter.cpp index 590fe816f1..8e0b92baa9 100644 --- a/Modules/US/USNavigation/Filter/mitkNodeDisplacementFilter.cpp +++ b/Modules/US/USNavigation/Filter/mitkNodeDisplacementFilter.cpp @@ -1,185 +1,189 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include "mitkNodeDisplacementFilter.h" mitk::NodeDisplacementFilter::NodeDisplacementFilter() : m_SelectedInput(-1) { } mitk::NodeDisplacementFilter::~NodeDisplacementFilter() { } bool mitk::NodeDisplacementFilter::AddNode( mitk::DataNode::Pointer node ) { // Consistency Checks if (node.IsNull()) { MITK_WARN("NodeDisplacementFilter") << "Null Node passed to NodeDisplacementFilter. Ignoring Node...."; return false; } if (node->GetData() == 0) { MITK_WARN("NodeDisplacementFilter") << "Empty Node passed to NodeDisplacementFilter. Ignoring Node...."; return false; } if(m_SelectedInput == -1) { MITK_ERROR("NodeDisplacementFilter") << "Cannot add nodes before input Stream was selected"; mitkThrow() << "Cannot add nodes before input Stream was selected"; } this->Update(); // make sure we are working on current data - mitk::NavigationData::Pointer reference = this->GetOutput(m_SelectedInput); + mitk::NavigationData::Pointer reference; + if (m_InitialReferencePose.IsNotNull()) //if there is a given reference pose use it + {reference = m_InitialReferencePose;} + else //else use the current pose of the given input + {reference = this->GetOutput(m_SelectedInput);} if (! reference->IsDataValid()) { MITK_WARN("NodeDisplacementFilter") << "Cannot add node while selected tool is not tracked. Ignoring Node...."; return false; } // find transformation and add node mitk::AffineTransform3D::Pointer inverseAffineTransform = mitk::AffineTransform3D::New(); if ( ! reference->GetAffineTransform3D()->GetInverse(inverseAffineTransform) ) { MITK_ERROR("NodeDisplacementFilter") << "Could not get the inverse transformation of the navigation data transformation."; mitkThrow() << "Could not get the inverse transformation of the navigation data transformation."; } inverseAffineTransform->Compose(node->GetData()->GetGeometry()->GetIndexToWorldTransform(), true); m_Transforms.push_back(inverseAffineTransform); m_Nodes.push_back(node); return true; } bool mitk::NodeDisplacementFilter::RemoveNode(unsigned int i) { if ( i >= m_Nodes.size() ) { return false; } m_Nodes.erase(m_Nodes.begin()+i); m_Transforms.erase(m_Transforms.begin()+i); return true; } int mitk::NodeDisplacementFilter::GetNumberOfNodes() { return m_Nodes.size(); } mitk::DataNode::Pointer mitk::NodeDisplacementFilter::GetNode (unsigned int i) { if (i < m_Nodes.size() ) { return m_Nodes.at(i); } else { return NULL; } } std::vector< mitk::DataNode::Pointer > mitk::NodeDisplacementFilter::GetNodes() { return m_Nodes; } void mitk::NodeDisplacementFilter::SelectInput(int i) { if (i < 0) { mitkThrow() << "Negative Input selected in NodeDisplacementFilter"; } if (! (static_cast(i) < this->GetInputs().size())) { MITK_ERROR("NodeDisplacementFilter") << "Selected input index is larger than actual number of inputs."; mitkThrow() << "Selected input index is larger than actual number of inputs in NodeDisplacementFilter"; } m_SelectedInput = i; } mitk::NavigationData::Pointer mitk::NodeDisplacementFilter::GetRawDisplacementNavigationData(unsigned int i) { mitk::NavigationData::Pointer returnValue = mitk::NavigationData::New(); if((m_Nodes.size()>i) && (m_Nodes.at(i).IsNotNull())) { try { returnValue = mitk::NavigationData::New(m_Nodes.at(i)->GetData()->GetGeometry()->GetIndexToWorldTransform()); } catch (mitk::Exception& e) { returnValue->SetDataValid(false); MITK_WARN << "Excetion while returning navigation data: " << e.GetDescription(); } } else { returnValue->SetDataValid(false); MITK_WARN << "Node Nr. " << i << " does not exist!"; } return returnValue; } void mitk::NodeDisplacementFilter::GenerateData() { // copy the navigation data from the inputs to the outputs mitk::NavigationDataPassThroughFilter::GenerateData(); // if no reference has been set yet, warn and abort if (m_SelectedInput == -1) { MITK_INFO("NodeDisplacementFilter") << "No input has been selected. Only forwarding NavigationData..."; return; } // cancel, if selected tool is currently not being tracked if ( ! this->GetInput(m_SelectedInput)->IsDataValid() ) { return; } // outputs have been updated, now to transform the nodes // 1) Generate Pseudo-Geometry for Reference mitk::Geometry3D::Pointer refGeom = this->TransformToGeometry( this->GetInput(m_SelectedInput)->GetAffineTransform3D()); // 2) For each node, calculate new position for (unsigned int index=0; index < m_Nodes.size(); index++) { mitk::Geometry3D::Pointer transformGeometry = refGeom->Clone(); // create transformation to the reference position and from there to // the node position (node has fixed transformation from reference position) transformGeometry->Compose(m_Transforms.at(index), true); m_Nodes.at(index)->GetData()->SetGeometry(transformGeometry); } } void mitk::NodeDisplacementFilter::ResetNodes() { m_Nodes.clear(); m_Transforms.clear(); } mitk::Geometry3D::Pointer mitk::NodeDisplacementFilter::TransformToGeometry(mitk::AffineTransform3D::Pointer transform){ mitk::Geometry3D::Pointer g3d = mitk::Geometry3D::New(); g3d->SetIndexToWorldTransform(transform); g3d->TransferItkToVtkTransform(); // update VTK Transform for rendering too g3d->Modified(); return g3d; } diff --git a/Modules/US/USNavigation/Filter/mitkNodeDisplacementFilter.h b/Modules/US/USNavigation/Filter/mitkNodeDisplacementFilter.h index dc170a6476..03048daa52 100644 --- a/Modules/US/USNavigation/Filter/mitkNodeDisplacementFilter.h +++ b/Modules/US/USNavigation/Filter/mitkNodeDisplacementFilter.h @@ -1,133 +1,145 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #ifndef NODEDISPLACEMENTFILTER_H_INCLUDED #define NODEDISPLACEMENTFILTER_H_INCLUDED #include "MitkUSNavigationExports.h" #include #include "mitkBaseRenderer.h" #include "mitkCommon.h" #include "mitkNavigationData.h" #include "mitkNavigationDataPassThroughFilter.h" //MITK namespace mitk { /**Documentation * \brief This filter moves DataNodes relatively to tracking Data from a 6-DoF Sensor. * * This behaviour can for example be used for rigid tracking of risk structures relative to a skin marker. * To use it, connect the Filter and select the input that delivers tracking data from the reference marker * via SelectInput(). * Make sure tracking is started before proceeding any further: The filter requires tracking data from the * sensor to calculate the relative position of the added node. * * One can then add Nodes to the filter via AddNode(). Make sure that the node has a geometry and position set * in the tracking coordinate system of the reference input. The Filter will then calculate the offset between * Node and reference marker and continously update the node position accordign to the tracking data. * * \ingroup US */ class MitkUSNavigation_EXPORT NodeDisplacementFilter : public NavigationDataPassThroughFilter { public: mitkClassMacro(NodeDisplacementFilter, NavigationDataPassThroughFilter); itkNewMacro(Self); /** * \brief Adds a node to the filter. * The position of which will then be continously update relatively to the selected input stream. * * The node should have a geometry and position set in the coordinate system of the selected input stream */ bool AddNode(mitk::DataNode::Pointer node); /** * \brief Removes a node from the filter. * \param i index of the node, the index corresponds to the order in which the nodes where added by AddNode() * \return true if a node with the given index was removed, false if the index was greater or equal the number of nodes in the filter */ bool RemoveNode(unsigned int i); /** * \brief Returns the number of nodes that were added to this filter. */ virtual int GetNumberOfNodes(); /** * \brief Returns the nth node that was added to this filter. */ virtual mitk::DataNode::Pointer GetNode (unsigned int i = 0); /** @return Returns the current pose in world coordinates of node i as raw navigation data. */ virtual mitk::NavigationData::Pointer GetRawDisplacementNavigationData(unsigned int i = 0); /** * \brief Returns a vector containing all nodes that have been added to this filter. * * Indexes in this vector correspond to indexes in the vector provided by GetOffsets(). */ virtual std::vector< mitk::DataNode::Pointer > GetNodes(); /** * \brief Selects an input stream as the reference stream. * * Position and orientation of all Nodes will be Updated according to information from the selected stream. * Make sure to select the input before adding nodes. The input should deliver 6DoF Data. Behaviour is undefined * for 5-Dof Data. The selected input can be changed during intervention if both old and new reference input * Lie in the same coordinate system. Be aware however that the offsets will not be recalculated, just moved * to the new stream. */ virtual void SelectInput(int i); + /** Manually sets the initial marker pose which is used for computing the offset. By default this + * option is disabled and the current pose of the selected input is used to compute the offset. + * However, sometimes it is needed to give a defined marker pose, then this method can be used. + * To disable the option you can set MarkerPosition to NULL. + */ + itkSetMacro(InitialReferencePose,mitk::NavigationData::Pointer); + /**Documentation * \brief Removes all added Nodes from the Filter but leaves all other configuration intact. */ virtual void ResetNodes(); protected: NodeDisplacementFilter(); virtual ~NodeDisplacementFilter(); virtual void GenerateData(); /** * \brief Creates an Geometry 3D Object from an AffineTransformation. */ mitk::Geometry3D::Pointer TransformToGeometry(mitk::AffineTransform3D::Pointer transform); /** * \brief All Nodes that are being managed by this Filter. */ std::vector m_Nodes; /** * \brief The transformation that each node has to be reached from the selected navigation tool. * The indexes correspond to indexes in the node vector. */ std::vector m_Transforms; /** * \brief The Input that is used as a reference to orient the managed nodes. */ int m_SelectedInput; + + /** + * Reference pose of the selected input to compute the offset. If this is NULL the current pose of the selected input is used instead. + */ + mitk::NavigationData::Pointer m_InitialReferencePose; }; } // namespace mitk #endif