diff --git a/Modules/IGT/DataManagement/mitkNavigationTool.cpp b/Modules/IGT/DataManagement/mitkNavigationTool.cpp index 14bd49e8b1..1763aeecfe 100644 --- a/Modules/IGT/DataManagement/mitkNavigationTool.cpp +++ b/Modules/IGT/DataManagement/mitkNavigationTool.cpp @@ -1,183 +1,336 @@ /*=================================================================== 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 "mitkNavigationTool.h" #include "mitkIGTException.h" #include "mitkNavigationData.h" #include "Poco/File.h" #include "mitkUnspecifiedTrackingTypeInformation.h" #include "mitkInternalTrackingTool.h" +#include "vtkSphereSource.h" +#include "vtkConeSource.h" +#include "vtkLineSource.h" +#include "vtkCylinderSource.h" +#include "vtkTransformPolyDataFilter.h" +#include +#include "mitkTextAnnotation3D.h" +#include "mitkManualPlacementAnnotationRenderer.h" +#include "mitkBaseRenderer.h" + mitk::NavigationTool::NavigationTool() : m_Identifier("None"), m_Type(mitk::NavigationTool::Unknown), m_CalibrationFile("none"), m_SerialNumber(""), m_TrackingDeviceType(mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName()), m_ToolRegistrationLandmarks(mitk::PointSet::New()), m_ToolCalibrationLandmarks(mitk::PointSet::New()), m_ToolTipOrientation(mitk::Quaternion(0, 0, 0, 1)) { m_ToolTipPosition[0] = 0; m_ToolTipPosition[1] = 0; m_ToolTipPosition[2] = 0; m_ToolAxis[0] = 1; m_ToolAxis[1] = 0; m_ToolAxis[2] = 0; + + SetDefaultSurface(); } itk::LightObject::Pointer mitk::NavigationTool::InternalClone() const { Self::Pointer tool = new Self(*this); tool->UnRegister(); return tool.GetPointer(); } mitk::NavigationTool::NavigationTool(const NavigationTool &other) : Superclass() { this->m_Identifier = other.m_Identifier; this->m_Type = other.m_Type; if (other.m_DataNode.IsNotNull()) { this->m_DataNode = other.m_DataNode->Clone(); this->m_DataNode->SetName(other.m_DataNode->GetName()); if (other.m_DataNode->GetData()) { this->m_DataNode->SetData(dynamic_cast(other.m_DataNode->GetData()->Clone().GetPointer())); } } if (other.m_SpatialObject.IsNotNull()) this->m_SpatialObject = other.m_SpatialObject->Clone(); this->m_CalibrationFile = other.m_CalibrationFile; this->m_SerialNumber = other.m_SerialNumber; this->m_TrackingDeviceType = other.m_TrackingDeviceType; if (other.m_ToolRegistrationLandmarks.IsNotNull()) this->m_ToolRegistrationLandmarks = other.m_ToolRegistrationLandmarks->Clone(); if (other.m_ToolCalibrationLandmarks.IsNotNull()) this->m_ToolCalibrationLandmarks = other.m_ToolCalibrationLandmarks->Clone(); this->m_ToolTipPosition = other.m_ToolTipPosition; this->m_ToolTipOrientation = other.m_ToolTipOrientation; this->m_ToolAxis = other.m_ToolAxis; } mitk::NavigationTool::~NavigationTool() { } mitk::AffineTransform3D::Pointer mitk::NavigationTool::GetToolTipTransform() { mitk::NavigationData::Pointer returnValue = mitk::NavigationData::New(); returnValue->SetPosition(this->m_ToolTipPosition); returnValue->SetOrientation(this->m_ToolTipOrientation); return returnValue->GetAffineTransform3D(); } void mitk::NavigationTool::Graft(const DataObject *data) { // Attempt to cast data to an NavigationData const Self* nd; try { nd = dynamic_cast(data); } catch (...) { mitkThrowException(mitk::IGTException) << "mitk::NavigationData::Graft cannot cast " << typeid(data).name() << " to " << typeid(const Self *).name(); } if (!nd) { // pointer could not be cast back down mitkThrowException(mitk::IGTException) << "mitk::NavigationData::Graft cannot cast " << typeid(data).name() << " to " << typeid(const Self *).name(); } // Now copy anything that is needed m_Identifier = nd->GetIdentifier(); m_Type = nd->GetType(); m_DataNode->SetName(nd->GetDataNode()->GetName()); m_DataNode->SetData(nd->GetDataNode()->GetData()); m_SpatialObject = nd->GetSpatialObject(); m_CalibrationFile = nd->GetCalibrationFile(); m_SerialNumber = nd->GetSerialNumber(); m_TrackingDeviceType = nd->GetTrackingDeviceType(); m_ToolRegistrationLandmarks = nd->GetToolRegistrationLandmarks(); m_ToolCalibrationLandmarks = nd->GetToolCalibrationLandmarks(); m_ToolTipPosition = nd->GetToolTipPosition(); m_ToolTipOrientation = nd->GetToolTipOrientation(); m_ToolAxis = nd->GetToolAxis(); } bool mitk::NavigationTool::IsToolTipSet() { if ((m_ToolTipPosition[0] == 0) && (m_ToolTipPosition[1] == 0) && (m_ToolTipPosition[2] == 0) && (m_ToolTipOrientation.x() == 0) && (m_ToolTipOrientation.y() == 0) && (m_ToolTipOrientation.z() == 0) && (m_ToolTipOrientation.r() == 1)) return false; else return true; } void mitk::NavigationTool::SetCalibrationFile(const std::string filename) { //check if file does exist: if (filename == "") { m_CalibrationFile = "none"; } else { Poco::File myFile(filename); if (myFile.exists()) m_CalibrationFile = filename; else m_CalibrationFile = "none"; } } std::string mitk::NavigationTool::GetToolName() { if (this->m_DataNode.IsNull()) { return ""; } else { return m_DataNode->GetName(); } } mitk::Surface::Pointer mitk::NavigationTool::GetToolSurface() { if (this->m_DataNode.IsNull()) { return nullptr; } else if (this->m_DataNode->GetData() == nullptr) { return nullptr; } else { return dynamic_cast(m_DataNode->GetData()); } } +void mitk::NavigationTool::SetDefaultSurface() +{ + if (m_DataNode.IsNull()) + m_DataNode = mitk::DataNode::New(); + + mitk::Surface::Pointer mySphere = mitk::Surface::New(); + + double axisLength = 5.; + + vtkSphereSource *vtkSphere = vtkSphereSource::New(); + vtkConeSource *vtkCone = vtkConeSource::New(); + vtkCylinderSource *vtkCylinder = vtkCylinderSource::New(); + vtkPolyData* axis = vtkPolyData::New(); + vtkLineSource *vtkLine = vtkLineSource::New(); + vtkLineSource *vtkLine2 = vtkLineSource::New(); + vtkLineSource *vtkLine3 = vtkLineSource::New(); + + vtkAppendPolyData* appendPolyData = vtkAppendPolyData::New(); + vtkPolyData* surface = vtkPolyData::New(); + + //Y-Axis (start with y, cause cylinder is oriented in y by vtk default...) + vtkCone->SetDirection(0, 1, 0); + vtkCone->SetHeight(1.0); + vtkCone->SetRadius(0.4f); + vtkCone->SetResolution(16); + vtkCone->SetCenter(0.0, axisLength, 0.0); + vtkCone->Update(); + + vtkCylinder->SetRadius(0.05); + vtkCylinder->SetHeight(axisLength); + vtkCylinder->SetCenter(0.0, 0.5*axisLength, 0.0); + vtkCylinder->Update(); + + appendPolyData->AddInputData(vtkCone->GetOutput()); + appendPolyData->AddInputData(vtkCylinder->GetOutput()); + appendPolyData->Update(); + axis->DeepCopy(appendPolyData->GetOutput()); + + //y symbol + vtkLine->SetPoint1(-0.5, axisLength + 2., 0.0); + vtkLine->SetPoint2(0.0, axisLength + 1.5, 0.0); + vtkLine->Update(); + + vtkLine2->SetPoint1(0.5, axisLength + 2., 0.0); + vtkLine2->SetPoint2(-0.5, axisLength + 1., 0.0); + vtkLine2->Update(); + + appendPolyData->AddInputData(vtkLine->GetOutput()); + appendPolyData->AddInputData(vtkLine2->GetOutput()); + appendPolyData->AddInputData(axis); + appendPolyData->Update(); + surface->DeepCopy(appendPolyData->GetOutput()); + + //X-axis + vtkTransform *XTransform = vtkTransform::New(); + XTransform->RotateZ(-90); + vtkTransformPolyDataFilter *TrafoFilter = vtkTransformPolyDataFilter::New(); + TrafoFilter->SetTransform(XTransform); + TrafoFilter->SetInputData(axis); + TrafoFilter->Update(); + + //x symbol + vtkLine->SetPoint1(axisLength + 2., -0.5, 0.0); + vtkLine->SetPoint2(axisLength + 1., 0.5, 0.0); + vtkLine->Update(); + + vtkLine2->SetPoint1(axisLength + 2., 0.5, 0.0); + vtkLine2->SetPoint2(axisLength + 1., -0.5, 0.0); + vtkLine2->Update(); + + appendPolyData->AddInputData(vtkLine->GetOutput()); + appendPolyData->AddInputData(vtkLine2->GetOutput()); + appendPolyData->AddInputData(TrafoFilter->GetOutput()); + appendPolyData->AddInputData(surface); + appendPolyData->Update(); + surface->DeepCopy(appendPolyData->GetOutput()); + + //Z-axis + vtkTransform *ZTransform = vtkTransform::New(); + ZTransform->RotateX(90); + TrafoFilter->SetTransform(ZTransform); + TrafoFilter->SetInputData(axis); + TrafoFilter->Update(); + + //z symbol + vtkLine->SetPoint1(-0.5, 0.0, axisLength + 2.); + vtkLine->SetPoint2(0.5, 0.0, axisLength + 2.); + vtkLine->Update(); + + vtkLine2->SetPoint1(-0.5, 0.0, axisLength + 2.); + vtkLine2->SetPoint2(0.5, 0.0, axisLength + 1.); + vtkLine2->Update(); + + vtkLine3->SetPoint1(0.5, 0.0, axisLength + 1.); + vtkLine3->SetPoint2(-0.5, 0.0, axisLength + 1.); + vtkLine3->Update(); + + appendPolyData->AddInputData(vtkLine->GetOutput()); + appendPolyData->AddInputData(vtkLine2->GetOutput()); + appendPolyData->AddInputData(vtkLine3->GetOutput()); + appendPolyData->AddInputData(TrafoFilter->GetOutput()); + appendPolyData->AddInputData(surface); + appendPolyData->Update(); + surface->DeepCopy(appendPolyData->GetOutput()); + + //Center + vtkSphere->SetRadius(0.5f); + vtkSphere->SetCenter(0.0, 0.0, 0.0); + vtkSphere->Update(); + + appendPolyData->AddInputData(vtkSphere->GetOutput()); + appendPolyData->AddInputData(surface); + appendPolyData->Update(); + surface->DeepCopy(appendPolyData->GetOutput()); + + //Scale + vtkTransform *ScaleTransform = vtkTransform::New(); + ScaleTransform->Scale(20., 20., 20.); + + TrafoFilter->SetTransform(ScaleTransform); + TrafoFilter->SetInputData(surface); + TrafoFilter->Update(); + + mySphere->SetVtkPolyData(TrafoFilter->GetOutput()); + + vtkCone->Delete(); + vtkSphere->Delete(); + vtkLine->Delete(); + vtkLine2->Delete(); + vtkLine3->Delete(); + vtkCylinder->Delete(); + ZTransform->Delete(); + XTransform->Delete(); + ScaleTransform->Delete(); + TrafoFilter->Delete(); + appendPolyData->Delete(); + surface->Delete(); + + this->GetDataNode()->SetData(mySphere); +} + std::string mitk::NavigationTool::GetStringWithAllToolInformation() const { std::stringstream _info; _info << " Identifier: " << this->m_Identifier << "\n" << " NavigationToolType: " << m_Type << "\n" << " Calibration file: " << m_CalibrationFile << "\n" << " Serial number: " << m_SerialNumber << "\n" << " TrackingDeviceType: " << m_TrackingDeviceType << "\n" << " ToolTip Position: " << m_ToolTipPosition << "\n" << " ToolTip Orientation: " << m_ToolTipOrientation << "\n" << " ToolTip Axis: " << m_ToolAxis; return _info.str(); } \ No newline at end of file diff --git a/Modules/IGT/DataManagement/mitkNavigationTool.h b/Modules/IGT/DataManagement/mitkNavigationTool.h index 0cc12c29fb..1e3c9212f9 100644 --- a/Modules/IGT/DataManagement/mitkNavigationTool.h +++ b/Modules/IGT/DataManagement/mitkNavigationTool.h @@ -1,209 +1,212 @@ /*=================================================================== 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 NAVIGATIONTOOL_H_INCLUDED #define NAVIGATIONTOOL_H_INCLUDED //itk headers #include #include #include //mitk headers #include #include #include #include #include #include #include namespace mitk { /**Documentation * \brief An object of this class represents a navigation tool in the view of the software. * A few informations like an identifier, a toolname, a surface and a itk spatial * object are stored in such an object. The classes NavigationToolReader and * are availiable to write/read tools to/from the harddisc. If you need a collection * of navigation tools the class NavigationToolStorage could be used. * * \ingroup IGT */ class MITKIGT_EXPORT NavigationTool : public itk::DataObject { public: mitkClassMacroItkParent(NavigationTool,itk::DataObject); itkFactorylessNewMacro(Self) itkCloneMacro(Self) enum NavigationToolType {Instrument, Fiducial, Skinmarker, Unknown}; //## getter and setter ## //NavigationToolType: itkGetConstMacro(Type,NavigationToolType); itkSetMacro(Type,NavigationToolType); //Identifier: itkGetConstMacro(Identifier,std::string); itkSetMacro(Identifier,std::string); //Datatreenode: itkGetConstMacro(DataNode,mitk::DataNode::Pointer); itkSetMacro(DataNode,mitk::DataNode::Pointer); //SpatialObject: itkGetConstMacro(SpatialObject,itk::SpatialObject<3>::Pointer); itkSetMacro(SpatialObject,itk::SpatialObject<3>::Pointer); //CalibrationFile: itkGetConstMacro(CalibrationFile,std::string); void SetCalibrationFile(const std::string filename); //Tool tip definition: itkGetConstMacro(ToolTipPosition,mitk::Point3D); itkSetMacro(ToolTipPosition,mitk::Point3D); itkGetConstMacro(ToolTipOrientation,mitk::Quaternion); itkSetMacro(ToolTipOrientation,mitk::Quaternion); //Tool Axis definition: //default tool axis is along x axis, the tool axis must be normalized itkGetConstMacro(ToolAxis, mitk::Point3D); itkSetMacro(ToolAxis, mitk::Point3D); /** @return Returns the tooltip as transform object. */ mitk::AffineTransform3D::Pointer GetToolTipTransform(); /** @return Returns true if a tooltip is set, false if not. */ bool IsToolTipSet(); //Tool Landmarks: /** For overview, here are descriptons of the two types of tool landmarks: * * tool calibration landmarks: These landmarks may be used clearly define the tools pose only by * using landmarks in the tool coordinate system. E.g., two landmarks for a 5DoF tool and three * landmarks for a 6DoF tool. These landmarks may be used, e.g., for a point based registration * of a tool from image space to tracking space. * * tool registration landmarks: These landmarks are designed for representing defined landmarks * on a tools surface. The number of these landmarks might exeed the number of tool calibration * landmarks for reasons of redundancy and averaging. They are used for, e.g., manually registering * the pose of a tool by visual markers in a CT scan. If you would use these landmarks to do a * point based registration from image space to tracking space later, you might overweight the * tool because of two many landmarks compared to other markers. * * @return Returns the tool registration landmarks which represent markers / special points on a * tool that can be used for registration. The landmarks should be given in tool coordinates. * If there are no landmarks defined for this tool the method returns an empty point set. */ itkGetConstMacro(ToolRegistrationLandmarks,mitk::PointSet::Pointer); /** @brief Sets the tool registration landmarks which represent markers / special points on a * tool that can be used for registration. The landmarks should be given in tool coordinates. */ itkSetMacro(ToolRegistrationLandmarks,mitk::PointSet::Pointer); /** @return Returns the tool calibration landmarks for calibration of the defined points in the * tool coordinate system, e.g. 2 landmarks for a 5DoF tool and 3 landmarks for a 6DoF tool. */ itkGetConstMacro(ToolCalibrationLandmarks,mitk::PointSet::Pointer); /** @brief Sets the tool calibration landmarks for calibration of defined points in the * tool coordinate system, e.g. 2 landmarks for a 5DoF tool and 3 landmarks for a 6DoF tool. */ itkSetMacro(ToolCalibrationLandmarks,mitk::PointSet::Pointer); //SerialNumber: itkGetConstMacro(SerialNumber,std::string); itkSetMacro(SerialNumber,std::string); //TrackingDeviceType: itkGetConstMacro(TrackingDeviceType,mitk::TrackingDeviceType); itkSetMacro(TrackingDeviceType,mitk::TrackingDeviceType); //ToolName (only getter): /** @return Returns the name of this navigation tool. Returns an empty string if there is * no name (for example because the data node has not been set yet). * * Note: There is no setter for the name, * because the name of the corresponding data node is used as tool name. So if you * want to modify the name of this navigation tool only get the data node and modify * its name. */ std::string GetToolName(); //ToolSurface (only getter): /** @return Returns the surface of this navigation tool. Returns NULL if there is * no surface (for example because the data node has not been set yet). * * Note: There is no setter for the surface, * because the surface is the data of the corresponding data node. So if you * want to set a new surface only get the data node and modify its data. */ mitk::Surface::Pointer GetToolSurface(); /** * \brief Graft the data and information from one NavigationTool to another. * * Copies the content of data into this object. * This is a convenience method to setup a second NavigationTool object with all the meta * information of another NavigationTool object. * Note that this method is different than just using two * SmartPointers to the same NavigationTool object since separate DataObjects are * still maintained. */ virtual void Graft(const DataObject *data) override; /** * Return all relevant information as string, e.g. to display all tool information */ std::string GetStringWithAllToolInformation() const; + + void SetDefaultSurface(); + //####################### protected: NavigationTool(); NavigationTool(const NavigationTool &other); ~NavigationTool(); virtual itk::LightObject::Pointer InternalClone() const override; //## data structure of a navigation tool object ## std::string m_Identifier; NavigationToolType m_Type; /** @brief This DataNode holds a toolname and a tool surface */ mitk::DataNode::Pointer m_DataNode; /** @brief This member variable holds a mathamatical description of the tool */ itk::SpatialObject<3>::Pointer m_SpatialObject; /** @brief The path to the calibration file of the tool. */ std::string m_CalibrationFile; /** @brief A unique serial number of the tool which is needed to identify the tool correctly. This is very important * in case of the NDI Aurora System. */ std::string m_SerialNumber; /** @brief This member holds the tracking device type of the tool. */ mitk::TrackingDeviceType m_TrackingDeviceType; /** @brief Holds landmarks for tool registration. */ mitk::PointSet::Pointer m_ToolRegistrationLandmarks; /** @brief Holds landmarks for calibration of the defined points in the tool coordinate system, * e.g. 2 landmarks for a 5DoF tool and 3 landmarks for a 6DoF tool. */ mitk::PointSet::Pointer m_ToolCalibrationLandmarks; /** @brief Holds the position of the tool tip. */ mitk::Point3D m_ToolTipPosition; /** @brief Holds the orientation of the tool tip. */ mitk::Quaternion m_ToolTipOrientation; /** @brief Holds the axis of the tool. */ mitk::Point3D m_ToolAxis; //################################################# }; } // namespace mitk #endif //NAVIGATIONTOOL diff --git a/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp index f274dbee55..9f3e895a38 100644 --- a/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp @@ -1,1324 +1,1314 @@ /*=================================================================== 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 "mitkNDITrackingDevice.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include #include #include #include #include #include // vtk #include typedef itk::MutexLockHolder MutexLockHolder; const unsigned char CR = 0xD; // == '\r' - carriage return const unsigned char LF = 0xA; // == '\n' - line feed mitk::NDITrackingDevice::NDITrackingDevice() : TrackingDevice(), m_DeviceName(""), m_PortNumber(mitk::SerialCommunication::COM5), m_BaudRate(mitk::SerialCommunication::BaudRate9600), m_DataBits(mitk::SerialCommunication::DataBits8), m_Parity(mitk::SerialCommunication::None), m_StopBits(mitk::SerialCommunication::StopBits1), m_HardwareHandshake(mitk::SerialCommunication::HardwareHandshakeOff), m_IlluminationActivationRate(Hz20), m_DataTransferMode(TX), m_6DTools(), m_ToolsMutex(nullptr), m_SerialCommunication(nullptr), m_SerialCommunicationMutex(nullptr), m_DeviceProtocol(nullptr), m_MultiThreader(nullptr), m_ThreadID(0), m_OperationMode(ToolTracking6D), m_MarkerPointsMutex(nullptr), m_MarkerPoints() { m_Data = mitk::UnspecifiedTrackingTypeInformation::GetDeviceDataUnspecified(); m_6DTools.clear(); m_SerialCommunicationMutex = itk::FastMutexLock::New(); m_DeviceProtocol = NDIProtocol::New(); m_DeviceProtocol->SetTrackingDevice(this); m_DeviceProtocol->UseCRCOn(); m_MultiThreader = itk::MultiThreader::New(); m_ToolsMutex = itk::FastMutexLock::New(); m_MarkerPointsMutex = itk::FastMutexLock::New(); m_MarkerPoints.reserve(50); // a maximum of 50 marker positions can be reported by the tracking device } bool mitk::NDITrackingDevice::UpdateTool(mitk::TrackingTool* tool) { if (this->GetState() != Setup) { mitk::NDIPassiveTool* ndiTool = dynamic_cast(tool); if (ndiTool == nullptr) return false; std::string portHandle = ndiTool->GetPortHandle(); //return false if the SROM Data has not been set if (ndiTool->GetSROMData() == nullptr) return false; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->PVWR(&portHandle, ndiTool->GetSROMData(), ndiTool->GetSROMDataLength()); if (returnvalue != NDIOKAY) return false; returnvalue = m_DeviceProtocol->PINIT(&portHandle); if (returnvalue != NDIOKAY) return false; returnvalue = m_DeviceProtocol->PENA(&portHandle, ndiTool->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) return false; return true; } else { return false; } } void mitk::NDITrackingDevice::SetRotationMode(RotationMode r) { m_RotationMode = r; } mitk::NDITrackingDevice::~NDITrackingDevice() { /* stop tracking and disconnect from tracking device */ if (GetState() == Tracking) { this->StopTracking(); } if (GetState() == Ready) { this->CloseConnection(); } /* cleanup tracking thread */ if ((m_ThreadID != 0) && (m_MultiThreader.IsNotNull())) { m_MultiThreader->TerminateThread(m_ThreadID); } m_MultiThreader = nullptr; /* free serial communication interface */ if (m_SerialCommunication.IsNotNull()) { m_SerialCommunication->ClearReceiveBuffer(); m_SerialCommunication->ClearSendBuffer(); m_SerialCommunication->CloseConnection(); m_SerialCommunication = nullptr; } } void mitk::NDITrackingDevice::SetPortNumber(const PortNumber _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting PortNumber to " << _arg); if (this->m_PortNumber != _arg) { this->m_PortNumber = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetDeviceName(std::string _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting eviceName to " << _arg); if (this->m_DeviceName != _arg) { this->m_DeviceName = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetBaudRate(const BaudRate _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting BaudRate to " << _arg); if (this->m_BaudRate != _arg) { this->m_BaudRate = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetDataBits(const DataBits _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting DataBits to " << _arg); if (this->m_DataBits != _arg) { this->m_DataBits = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetParity(const Parity _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting Parity to " << _arg); if (this->m_Parity != _arg) { this->m_Parity = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetStopBits(const StopBits _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting StopBits to " << _arg); if (this->m_StopBits != _arg) { this->m_StopBits = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetHardwareHandshake(const HardwareHandshake _arg) { if (this->GetState() != Setup) return; itkDebugMacro("setting HardwareHandshake to " << _arg); if (this->m_HardwareHandshake != _arg) { this->m_HardwareHandshake = _arg; this->Modified(); } } void mitk::NDITrackingDevice::SetIlluminationActivationRate(const IlluminationActivationRate _arg) { if (this->GetState() == Tracking) return; itkDebugMacro("setting IlluminationActivationRate to " << _arg); if (this->m_IlluminationActivationRate != _arg) { this->m_IlluminationActivationRate = _arg; this->Modified(); if (this->GetState() == Ready) // if the connection to the tracking system is established, send the new rate to the tracking device too m_DeviceProtocol->IRATE(this->m_IlluminationActivationRate); } } void mitk::NDITrackingDevice::SetDataTransferMode(const DataTransferMode _arg) { itkDebugMacro("setting DataTransferMode to " << _arg); if (this->m_DataTransferMode != _arg) { this->m_DataTransferMode = _arg; this->Modified(); } } mitk::NDIErrorCode mitk::NDITrackingDevice::Send(const std::string* input, bool addCRC) { if (input == nullptr) return SERIALSENDERROR; std::string message; if (addCRC == true) message = *input + CalcCRC(input) + std::string(1, CR); else message = *input + std::string(1, CR); //unsigned int messageLength = message.length() + 1; // +1 for CR // Clear send buffer this->ClearSendBuffer(); // Send the date to the device MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex long returnvalue = m_SerialCommunication->Send(message); if (returnvalue == 0) return SERIALSENDERROR; else return NDIOKAY; } mitk::NDIErrorCode mitk::NDITrackingDevice::Receive(std::string* answer, unsigned int numberOfBytes) { if (answer == nullptr) return SERIALRECEIVEERROR; MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex long returnvalue = m_SerialCommunication->Receive(*answer, numberOfBytes); // never read more bytes than the device has send, the function will block until enough bytes are send... if (returnvalue == 0) return SERIALRECEIVEERROR; else return NDIOKAY; } mitk::NDIErrorCode mitk::NDITrackingDevice::ReceiveByte(char* answer) { if (answer == nullptr) return SERIALRECEIVEERROR; std::string m; MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex long returnvalue = m_SerialCommunication->Receive(m, 1); if ((returnvalue == 0) || (m.size() != 1)) return SERIALRECEIVEERROR; *answer = m.at(0); return NDIOKAY; } mitk::NDIErrorCode mitk::NDITrackingDevice::ReceiveLine(std::string* answer) { if (answer == nullptr) return SERIALRECEIVEERROR; std::string m; MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex do { long returnvalue = m_SerialCommunication->Receive(m, 1); if ((returnvalue == 0) || (m.size() != 1)) return SERIALRECEIVEERROR; *answer += m; } while (m.at(0) != LF); return NDIOKAY; } void mitk::NDITrackingDevice::ClearSendBuffer() { MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex m_SerialCommunication->ClearSendBuffer(); } void mitk::NDITrackingDevice::ClearReceiveBuffer() { MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex m_SerialCommunication->ClearReceiveBuffer(); } const std::string mitk::NDITrackingDevice::CalcCRC(const std::string* input) { if (input == nullptr) return ""; /* the crc16 calculation code is taken from the NDI API guide example code section */ static int oddparity[16] = { 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0 }; unsigned int data; // copy of the input string's current character unsigned int crcValue = 0; // the crc value is stored here unsigned int* puCRC16 = &crcValue; // the algorithm uses a pointer to crcValue, so it's easier to provide that than to change the algorithm for (unsigned int i = 0; i < input->length(); i++) { data = (*input)[i]; data = (data ^ (*(puCRC16)& 0xff)) & 0xff; *puCRC16 >>= 8; if (oddparity[data & 0x0f] ^ oddparity[data >> 4]) { *(puCRC16) ^= 0xc001; } data <<= 6; *puCRC16 ^= data; data <<= 1; *puCRC16 ^= data; } // crcValue contains now the CRC16 value. Convert it to a string and return it char returnvalue[13]; sprintf(returnvalue, "%04X", crcValue); // 4 hexadecimal digit with uppercase format return std::string(returnvalue); } bool mitk::NDITrackingDevice::OpenConnection() { //this->m_ModeMutex->Lock(); if (this->GetState() != Setup) { mitkThrowException(mitk::IGTException) << "Can only try to open the connection if in setup mode"; } m_SerialCommunication = mitk::SerialCommunication::New(); /* init local com port to standard com settings for a NDI tracking device: 9600 baud, 8 data bits, no parity, 1 stop bit, no hardware handshake */ if (m_DeviceName.empty()) m_SerialCommunication->SetPortNumber(m_PortNumber); else m_SerialCommunication->SetDeviceName(m_DeviceName); m_SerialCommunication->SetBaudRate(mitk::SerialCommunication::BaudRate9600); m_SerialCommunication->SetDataBits(mitk::SerialCommunication::DataBits8); m_SerialCommunication->SetParity(mitk::SerialCommunication::None); m_SerialCommunication->SetStopBits(mitk::SerialCommunication::StopBits1); m_SerialCommunication->SetSendTimeout(5000); m_SerialCommunication->SetReceiveTimeout(5000); if (m_SerialCommunication->OpenConnection() == 0) // 0 == ERROR_VALUE { m_SerialCommunication->CloseConnection(); m_SerialCommunication = nullptr; mitkThrowException(mitk::IGTHardwareException) << "Can not open serial port"; } /* Reset Tracking device by sending a serial break for 500ms */ m_SerialCommunication->SendBreak(400); /* Read answer from tracking device (RESETBE6F) */ static const std::string reset("RESETBE6F\r"); std::string answer = ""; this->Receive(&answer, reset.length()); // read answer (should be RESETBE6F) this->ClearReceiveBuffer(); // flush the receive buffer of all remaining data (carriage return, strings other than reset if (reset.compare(answer) != 0) // check for RESETBE6F { if (m_SerialCommunication.IsNotNull()) { m_SerialCommunication->CloseConnection(); m_SerialCommunication = nullptr; } mitkThrowException(mitk::IGTHardwareException) << "Hardware Reset of tracking device did not work"; } /* Now the tracking device isSetData reset, start initialization */ NDIErrorCode returnvalue; /* set device com settings to new values and wait for the device to change them */ returnvalue = m_DeviceProtocol->COMM(m_BaudRate, m_DataBits, m_Parity, m_StopBits, m_HardwareHandshake); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << "Could not set comm settings in trackingdevice"; } //after changing COMM wait at least 100ms according to NDI Api documentation page 31 itksys::SystemTools::Delay(500); /* now change local com settings accordingly */ m_SerialCommunication->CloseConnection(); m_SerialCommunication->SetBaudRate(m_BaudRate); m_SerialCommunication->SetDataBits(m_DataBits); m_SerialCommunication->SetParity(m_Parity); m_SerialCommunication->SetStopBits(m_StopBits); m_SerialCommunication->SetHardwareHandshake(m_HardwareHandshake); m_SerialCommunication->SetSendTimeout(5000); m_SerialCommunication->SetReceiveTimeout(5000); m_SerialCommunication->OpenConnection(); /* initialize the tracking device */ returnvalue = m_DeviceProtocol->INIT(); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << "Could not initialize the tracking device"; } if (this->GetType() == mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName()) // if the type of tracking device is not specified, try to query the connected device { mitk::TrackingDeviceType deviceType; returnvalue = m_DeviceProtocol->VER(deviceType); if ((returnvalue != NDIOKAY) || (deviceType == mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName())) { mitkThrowException(mitk::IGTHardwareException) << "Could not determine tracking device type. Please set manually and try again."; } this->SetType(deviceType); } /**** Optional Polaris specific code, Work in progress // start diagnostic mode returnvalue = m_DeviceProtocol->DSTART(); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not start diagnostic mode"); return false; } else // we are in diagnostic mode { // initialize extensive IR checking returnvalue = m_DeviceProtocol->IRINIT(); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not initialize intense infrared light checking"); return false; } bool intenseIR = false; returnvalue = m_DeviceProtocol->IRCHK(&intenseIR); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not execute intense infrared light checking"); return false; } if (intenseIR == true) // do something - warn the user, raise exception, write to protocol or similar std::cout << "Warning: Intense infrared light detected. Accurate tracking will probably not be possible.\n"; // stop diagnictic mode returnvalue = m_DeviceProtocol->DSTOP(); if (returnvalue != NDIOKAY) { this->SetErrorMessage("Could not stop diagnostic mode"); return false; } } *** end of optional polaris code ***/ /** * now add tools to the tracking system **/ /* First, check if the tracking device has port handles that need to be freed and free them */ returnvalue = FreePortHandles(); // non-critical, therefore no error handling /** * POLARIS: initialize the tools that were added manually **/ { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex std::string portHandle; auto endIt = m_6DTools.end(); for (auto it = m_6DTools.begin(); it != endIt; ++it) { /* get a port handle for the tool */ returnvalue = m_DeviceProtocol->PHRQ(&portHandle); if (returnvalue == NDIOKAY) { (*it)->SetPortHandle(portHandle.c_str()); /* now write the SROM file of the tool to the tracking system using PVWR */ if (this->m_Data.Line == mitk::NDIPolarisTypeInformation::GetTrackingDeviceName()) { returnvalue = m_DeviceProtocol->PVWR(&portHandle, (*it)->GetSROMData(), (*it)->GetSROMDataLength()); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not write SROM file for tool '") + (*it)->GetToolName() + std::string("' to tracking device")).c_str(); } returnvalue = m_DeviceProtocol->PINIT(&portHandle); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize tool '") + (*it)->GetToolName()).c_str(); } if ((*it)->IsEnabled() == true) { returnvalue = m_DeviceProtocol->PENA(&portHandle, (*it)->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + portHandle + std::string("' for tool '") + (*it)->GetToolName() + std::string("'")).c_str(); } } } } } } // end of toolsmutexlockholder scope /* check for wired tools and add them too */ if (this->DiscoverWiredTools() == false) // query the tracking device for wired tools and add them to our tool list return false; // \TODO: could we continue anyways? /*POLARIS: set the illuminator activation rate */ if (this->m_Data.Line == mitk::NDIPolarisTypeInformation::GetTrackingDeviceName()) { returnvalue = m_DeviceProtocol->IRATE(this->m_IlluminationActivationRate); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << "Could not set the illuminator activation rate"; } } /* finish - now all tools should be added, initialized and enabled, so that tracking can be started */ this->SetState(Ready); try { SetVolume(this->m_Data); } catch (mitk::IGTHardwareException e) { MITK_WARN << e.GetDescription(); } return true; } bool mitk::NDITrackingDevice::InitializeWiredTools() { NDIErrorCode returnvalue; std::string portHandle; returnvalue = m_DeviceProtocol->PHSR(OCCUPIED, &portHandle); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that are connected"; } /* if there are port handles that need to be initialized, initialize them. Furthermore instantiate tools for each handle that has no tool yet. */ std::string ph; for (unsigned int i = 0; i < portHandle.size(); i += 2) { ph = portHandle.substr(i, 2); mitk::NDIPassiveTool* pt = this->GetInternalTool(ph); if (pt == nullptr) // if we don't have a tool, something is wrong. Tools should be discovered first by calling DiscoverWiredTools() continue; if (pt->GetSROMData() == nullptr) continue; returnvalue = m_DeviceProtocol->PVWR(&ph, pt->GetSROMData(), pt->GetSROMDataLength()); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not write SROM file for tool '") + pt->GetToolName() + std::string("' to tracking device")).c_str(); } returnvalue = m_DeviceProtocol->PINIT(&ph); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize tool '") + pt->GetToolName()).c_str(); } if (pt->IsEnabled() == true) { returnvalue = m_DeviceProtocol->PENA(&ph, pt->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + portHandle + std::string("' for tool '") + pt->GetToolName() + std::string("'")).c_str(); } } } return true; } mitk::TrackingDeviceType mitk::NDITrackingDevice::TestConnection() { if (this->GetState() != Setup) { return mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName(); } m_SerialCommunication = mitk::SerialCommunication::New(); //m_DeviceProtocol = mitk::NDIProtocol::New(); //m_DeviceProtocol->SetTrackingDevice(this); //m_DeviceProtocol->UseCRCOn(); /* init local com port to standard com settings for a NDI tracking device: 9600 baud, 8 data bits, no parity, 1 stop bit, no hardware handshake */ if (m_DeviceName.empty()) m_SerialCommunication->SetPortNumber(m_PortNumber); else m_SerialCommunication->SetDeviceName(m_DeviceName); m_SerialCommunication->SetBaudRate(mitk::SerialCommunication::BaudRate9600); m_SerialCommunication->SetDataBits(mitk::SerialCommunication::DataBits8); m_SerialCommunication->SetParity(mitk::SerialCommunication::None); m_SerialCommunication->SetStopBits(mitk::SerialCommunication::StopBits1); m_SerialCommunication->SetSendTimeout(5000); m_SerialCommunication->SetReceiveTimeout(5000); if (m_SerialCommunication->OpenConnection() == 0) // error { m_SerialCommunication = nullptr; return mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName(); } /* Reset Tracking device by sending a serial break for 500ms */ m_SerialCommunication->SendBreak(400); /* Read answer from tracking device (RESETBE6F) */ static const std::string reset("RESETBE6F\r"); std::string answer = ""; this->Receive(&answer, reset.length()); // read answer (should be RESETBE6F) this->ClearReceiveBuffer(); // flush the receive buffer of all remaining data (carriage return, strings other than reset if (reset.compare(answer) != 0) // check for RESETBE6F { m_SerialCommunication->CloseConnection(); m_SerialCommunication = nullptr; mitkThrowException(mitk::IGTHardwareException) << "Hardware Reset of tracking device did not work"; } /* Now the tracking device is reset, start initialization */ NDIErrorCode returnvalue; /* initialize the tracking device */ //returnvalue = m_DeviceProtocol->INIT(); //if (returnvalue != NDIOKAY) //{ // this->SetErrorMessage("Could not initialize the tracking device"); // return mitk::TrackingSystemNotSpecified; //} mitk::TrackingDeviceType deviceType; returnvalue = m_DeviceProtocol->VER(deviceType); if ((returnvalue != NDIOKAY) || (deviceType == mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName())) { m_SerialCommunication = nullptr; return mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName(); } m_SerialCommunication = nullptr; return deviceType; } bool mitk::NDITrackingDevice::CloseConnection() { if (this->GetState() != Setup) { //init before closing to force the field generator from aurora to switch itself off m_DeviceProtocol->INIT(); /* close the serial connection */ m_SerialCommunication->CloseConnection(); /* invalidate all tools */ this->InvalidateAll(); /* return to setup mode */ this->SetState(Setup); m_SerialCommunication = nullptr; } return true; } ITK_THREAD_RETURN_TYPE mitk::NDITrackingDevice::ThreadStartTracking(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == nullptr) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == nullptr) { return ITK_THREAD_RETURN_VALUE; } NDITrackingDevice *trackingDevice = (NDITrackingDevice*)pInfo->UserData; if (trackingDevice != nullptr) { if (trackingDevice->GetOperationMode() == ToolTracking6D) trackingDevice->TrackTools(); // call TrackTools() from the original object else if (trackingDevice->GetOperationMode() == MarkerTracking3D) trackingDevice->TrackMarkerPositions(); // call TrackMarkerPositions() from the original object else if (trackingDevice->GetOperationMode() == ToolTracking5D) trackingDevice->TrackMarkerPositions(); // call TrackMarkerPositions() from the original object else if (trackingDevice->GetOperationMode() == HybridTracking) { trackingDevice->TrackToolsAndMarkers(); } } trackingDevice->m_ThreadID = 0; // erase thread id, now that this thread will end. return ITK_THREAD_RETURN_VALUE; } bool mitk::NDITrackingDevice::StartTracking() { if (this->GetState() != Ready) return false; this->SetState(Tracking); // go to mode Tracking this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking this->m_StopTracking = false; this->m_StopTrackingMutex->Unlock(); m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method mitk::IGTTimeStamp::GetInstance()->Start(this); return true; } void mitk::NDITrackingDevice::TrackTools() { /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope if (this->GetState() != Tracking) return; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->TSTART(); if (returnvalue != NDIOKAY) return; bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; this->m_StopTrackingMutex->Unlock(); while ((this->GetState() == Tracking) && (localStopTracking == false)) { if (this->m_DataTransferMode == TX) { returnvalue = this->m_DeviceProtocol->TX(); if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors break; } else { returnvalue = this->m_DeviceProtocol->BX(); if (returnvalue != NDIOKAY) break; } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */ returnvalue = m_DeviceProtocol->TSTOP(); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << "An error occured while tracking tools."; } return; // returning from this function (and ThreadStartTracking()) this will end the thread and transfer control back to main thread by releasing trackingFinishedLockHolder } void mitk::NDITrackingDevice::TrackMarkerPositions() { MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope if (m_OperationMode == ToolTracking6D) return; if (this->GetState() != Tracking) return; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->DSTART(); // Start Diagnostic Mode if (returnvalue != NDIOKAY) return; bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; this->m_StopTrackingMutex->Unlock(); while ((this->GetState() == Tracking) && (localStopTracking == false)) { m_MarkerPointsMutex->Lock(); // lock points data structure returnvalue = this->m_DeviceProtocol->POS3D(&m_MarkerPoints); // update points data structure with new position data from tracking device m_MarkerPointsMutex->Unlock(); if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors { std::cout << "Error in POS3D: could not read data. Possibly no markers present." << std::endl; } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); itksys::SystemTools::Delay(1); } /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */ returnvalue = m_DeviceProtocol->DSTOP(); if (returnvalue != NDIOKAY) return; // how can this thread tell the application, that an error has occured? this->SetState(Ready); return; // returning from this function (and ThreadStartTracking()) this will end the thread } void mitk::NDITrackingDevice::TrackToolsAndMarkers() { MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope if (m_OperationMode != HybridTracking) return; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->TSTART(); // Start Diagnostic Mode if (returnvalue != NDIOKAY) return; bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; this->m_StopTrackingMutex->Unlock(); while ((this->GetState() == Tracking) && (localStopTracking == false)) { m_MarkerPointsMutex->Lock(); // lock points data structure returnvalue = this->m_DeviceProtocol->TX(true, &m_MarkerPoints); // update points data structure with new position data from tracking device m_MarkerPointsMutex->Unlock(); if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors { std::cout << "Error in TX: could not read data. Possibly no markers present." << std::endl; } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */ returnvalue = m_DeviceProtocol->TSTOP(); if (returnvalue != NDIOKAY) return; // how can this thread tell the application, that an error has occurred? this->SetState(Ready); return; // returning from this function (and ThreadStartTracking()) this will end the thread } mitk::TrackingTool* mitk::NDITrackingDevice::GetTool(unsigned int toolNumber) const { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex if (toolNumber < m_6DTools.size()) return m_6DTools.at(toolNumber); return nullptr; } mitk::TrackingTool* mitk::NDITrackingDevice::GetToolByName(std::string name) const { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex auto end = m_6DTools.end(); for (auto iterator = m_6DTools.begin(); iterator != end; ++iterator) if (name.compare((*iterator)->GetToolName()) == 0) return *iterator; return nullptr; } mitk::NDIPassiveTool* mitk::NDITrackingDevice::GetInternalTool(std::string portHandle) { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex auto end = m_6DTools.end(); for (auto iterator = m_6DTools.begin(); iterator != end; ++iterator) if (portHandle.compare((*iterator)->GetPortHandle()) == 0) return *iterator; return nullptr; } unsigned int mitk::NDITrackingDevice::GetToolCount() const { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex return m_6DTools.size(); } bool mitk::NDITrackingDevice::Beep(unsigned char count) { if (this->GetState() != Setup) { return (m_DeviceProtocol->BEEP(count) == NDIOKAY); } else { return false; } } mitk::TrackingTool* mitk::NDITrackingDevice::AddTool(const char* toolName, const char* fileName, TrackingPriority p /*= NDIPassiveTool::Dynamic*/) { mitk::NDIPassiveTool::Pointer t = mitk::NDIPassiveTool::New(); if (t->LoadSROMFile(fileName) == false) return nullptr; t->SetToolName(toolName); t->SetTrackingPriority(p); if (this->InternalAddTool(t) == false) return nullptr; return t.GetPointer(); } bool mitk::NDITrackingDevice::InternalAddTool(mitk::NDIPassiveTool* tool) { if (tool == nullptr) return false; NDIPassiveTool::Pointer p = tool; /* if the connection to the tracking device is already established, add the new tool to the device now */ if (this->GetState() == Ready) { /* get a port handle for the tool */ std::string newPortHandle; NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->PHRQ(&newPortHandle); if (returnvalue == NDIOKAY) { p->SetPortHandle(newPortHandle.c_str()); /* now write the SROM file of the tool to the tracking system using PVWR */ returnvalue = m_DeviceProtocol->PVWR(&newPortHandle, p->GetSROMData(), p->GetSROMDataLength()); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not write SROM file for tool '") + p->GetToolName() + std::string("' to tracking device")).c_str(); } /* initialize the port handle */ returnvalue = m_DeviceProtocol->PINIT(&newPortHandle); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize port '") + newPortHandle + std::string("' for tool '") + p->GetToolName() + std::string("'")).c_str(); } /* enable the port handle */ if (p->IsEnabled() == true) { returnvalue = m_DeviceProtocol->PENA(&newPortHandle, p->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + newPortHandle + std::string("' for tool '") + p->GetToolName() + std::string("'")).c_str(); } } } /* now that the tool is added to the device, add it to list too */ m_ToolsMutex->Lock(); this->m_6DTools.push_back(p); m_ToolsMutex->Unlock(); this->Modified(); return true; } else if (this->GetState() == Setup) { /* In Setup mode, we only add it to the list, so that OpenConnection() can add it later */ m_ToolsMutex->Lock(); this->m_6DTools.push_back(p); m_ToolsMutex->Unlock(); this->Modified(); return true; } else // in Tracking mode, no tools can be added return false; } bool mitk::NDITrackingDevice::RemoveTool(mitk::TrackingTool* tool) { mitk::NDIPassiveTool* ndiTool = dynamic_cast(tool); if (ndiTool == nullptr) return false; std::string portHandle = ndiTool->GetPortHandle(); /* a valid portHandle has length 2. If a valid handle exists, the tool is already added to the tracking device, so we have to remove it there if the connection to the tracking device has already been established. */ if ((portHandle.length() == 2) && (this->GetState() == Ready)) // do not remove a tool in tracking mode { NDIErrorCode returnvalue; returnvalue = m_DeviceProtocol->PHF(&portHandle); if (returnvalue != NDIOKAY) return false; /* Now that the tool is removed from the tracking device, remove it from our tool list too */ MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex (scope is inside the if-block auto end = m_6DTools.end(); for (auto iterator = m_6DTools.begin(); iterator != end; ++iterator) { if (iterator->GetPointer() == ndiTool) { m_6DTools.erase(iterator); this->Modified(); return true; } } return false; } else if (this->GetState() == Setup) // in Setup Mode, we are not connected to the tracking device, so we can just remove the tool from the tool list { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex auto end = m_6DTools.end(); for (auto iterator = m_6DTools.begin(); iterator != end; ++iterator) { if ((*iterator).GetPointer() == ndiTool) { m_6DTools.erase(iterator); this->Modified(); return true; } } return false; } return false; } void mitk::NDITrackingDevice::InvalidateAll() { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex auto end = m_6DTools.end(); for (auto iterator = m_6DTools.begin(); iterator != end; ++iterator) (*iterator)->SetDataValid(false); } bool mitk::NDITrackingDevice::SetOperationMode(OperationMode mode) { if (GetState() == Tracking) return false; m_OperationMode = mode; return true; } mitk::OperationMode mitk::NDITrackingDevice::GetOperationMode() { return m_OperationMode; } bool mitk::NDITrackingDevice::GetMarkerPositions(MarkerPointContainerType* markerpositions) { m_MarkerPointsMutex->Lock(); *markerpositions = m_MarkerPoints; // copy the internal vector to the one provided m_MarkerPointsMutex->Unlock(); return (markerpositions->size() != 0); } bool mitk::NDITrackingDevice::DiscoverWiredTools() { /* First, check for disconnected tools and remove them */ this->FreePortHandles(); /* check for new tools, add and initialize them */ NDIErrorCode returnvalue; std::string portHandle; returnvalue = m_DeviceProtocol->PHSR(OCCUPIED, &portHandle); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that are connected"; } /* if there are port handles that need to be initialized, initialize them. Furthermore instantiate tools for each handle that has no tool yet. */ std::string ph; /* we need to remember the ports which are occupied to be able to readout the serial numbers of the connected tools later */ std::vector occupiedPorts = std::vector(); int numberOfToolsAtStart = this->GetToolCount(); //also remember the number of tools at start to identify the automatically detected tools later for (unsigned int i = 0; i < portHandle.size(); i += 2) { ph = portHandle.substr(i, 2); if (this->GetInternalTool(ph) != nullptr) // if we already have a tool with this handle continue; // then skip the initialization //instantiate an object for each tool that is connected mitk::NDIPassiveTool::Pointer newTool = mitk::NDIPassiveTool::New(); newTool->SetPortHandle(ph.c_str()); newTool->SetTrackingPriority(mitk::NDIPassiveTool::Dynamic); //set a name for identification newTool->SetToolName((std::string("Port ") + ph).c_str()); returnvalue = m_DeviceProtocol->PINIT(&ph); if (returnvalue != NDIINITIALIZATIONFAILED) //if the initialization failed (AURORA) it can not be enabled. A srom file will have to be specified manually first. Still return true to be able to continue { if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize port '") + ph + std::string("' for tool '") + newTool->GetToolName() + std::string("'")).c_str(); } /* enable the port handle */ returnvalue = m_DeviceProtocol->PENA(&ph, newTool->GetTrackingPriority()); // Enable tool if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + ph + std::string("' for tool '") + newTool->GetToolName() + std::string("'")).c_str(); } } //we have to temporarily unlock m_ModeMutex here to avoid a deadlock with another lock inside InternalAddTool() if (this->InternalAddTool(newTool) == false) { mitkThrowException(mitk::IGTException) << "Error while adding new tool"; } else occupiedPorts.push_back(i); } // after initialization readout serial numbers of automatically detected tools for (unsigned int i = 0; i < occupiedPorts.size(); i++) { ph = portHandle.substr(occupiedPorts.at(i), 2); std::string portInfo; NDIErrorCode returnvaluePort = m_DeviceProtocol->PHINF(ph, &portInfo); if ((returnvaluePort == NDIOKAY) && (portInfo.size()>31)) dynamic_cast(this->GetTool(i + numberOfToolsAtStart))->SetSerialNumber(portInfo.substr(23, 8)); itksys::SystemTools::Delay(10); } return true; } mitk::NDIErrorCode mitk::NDITrackingDevice::FreePortHandles() { /* first search for port handles that need to be freed: e.g. because of a reset of the tracking system */ NDIErrorCode returnvalue = NDIOKAY; std::string portHandle; returnvalue = m_DeviceProtocol->PHSR(FREED, &portHandle); if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that need to be freed"; } /* if there are port handles that need to be freed, free them */ if (portHandle.empty() == true) return returnvalue; std::string ph; for (unsigned int i = 0; i < portHandle.size(); i += 2) { ph = portHandle.substr(i, 2); mitk::NDIPassiveTool* t = this->GetInternalTool(ph); if (t != nullptr) // if we have a tool for the port handle that needs to be freed { if (this->RemoveTool(t) == false) // remove it (this will free the port too) returnvalue = NDIERROR; } else // we don't have a tool, the port handle exists only in the tracking device { returnvalue = m_DeviceProtocol->PHF(&ph); // free it there // What to do if port handle could not be freed? This seems to be a non critical error if (returnvalue != NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << "Could not free all port handles"; } } } return returnvalue; } int mitk::NDITrackingDevice::GetMajorFirmwareRevisionNumber() { std::string revision; if (m_DeviceProtocol->APIREV(&revision) != mitk::NDIOKAY || revision.empty() || (revision.size() != 9)) { MITK_ERROR << "Could not receive firmware revision number!"; return 0; } const std::string majrevno = revision.substr(2, 3); //cut out "004" from "D.004.001" return std::atoi(majrevno.c_str()); } const char* mitk::NDITrackingDevice::GetFirmwareRevisionNumber() { static std::string revision; if (m_DeviceProtocol->APIREV(&revision) != mitk::NDIOKAY || revision.empty() || (revision.size() != 9)) { MITK_ERROR << "Could not receive firmware revision number!"; revision = ""; return revision.c_str(); } return revision.c_str(); } bool mitk::NDITrackingDevice::AutoDetectToolsAvailable() { if (this->GetType() == mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()) { return true; } else { return false; } } bool mitk::NDITrackingDevice::AddSingleToolIsAvailable() { //For Aurora, only AutoDetecion or loading of toolStorage should be used. It is not possible to add a single tool. if (this->GetType() == mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()) { return false; } //For Polaris, a single tool can be added, there is no autoDetection. else { return true; } } mitk::NavigationToolStorage::Pointer mitk::NDITrackingDevice::AutoDetectTools() { mitk::NavigationToolStorage::Pointer autoDetectedStorage = mitk::NavigationToolStorage::New(); if (this->GetType() == mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()) { try { this->OpenConnection(); this->StartTracking(); } catch (mitk::Exception& e) { MITK_WARN << "Warning, can not auto-detect tools! (" << e.GetDescription() << ")"; return autoDetectedStorage; } for (unsigned int i = 0; i < this->GetToolCount(); i++) { //create a navigation tool with sphere as surface std::stringstream toolname; toolname << "AutoDetectedTool" << i; mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); newTool->SetSerialNumber(dynamic_cast(this->GetTool(i))->GetSerialNumber()); newTool->SetIdentifier(toolname.str()); newTool->SetTrackingDeviceType(mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()); - mitk::DataNode::Pointer newNode = mitk::DataNode::New(); - mitk::Surface::Pointer mySphere = mitk::Surface::New(); - vtkSphereSource *vtkData = vtkSphereSource::New(); - vtkData->SetRadius(3.0f); - vtkData->SetCenter(0.0, 0.0, 0.0); - vtkData->Update(); - mySphere->SetVtkPolyData(vtkData->GetOutput()); - vtkData->Delete(); - newNode->SetData(mySphere); - newNode->SetName(toolname.str()); - newTool->SetDataNode(newNode); + newTool->GetDataNode()->SetName(toolname.str()); autoDetectedStorage->AddTool(newTool); } this->StopTracking(); this->CloseConnection(); } return autoDetectedStorage; } bool mitk::NDITrackingDevice::GetSupportedVolumes(unsigned int* numberOfVolumes, mitk::NDITrackingDevice::NDITrackingVolumeContainerType* volumes, mitk::NDITrackingDevice::TrackingVolumeDimensionType* volumesDimensions) { if (numberOfVolumes == nullptr || volumes == nullptr || volumesDimensions == nullptr) return false; static std::string info; if (m_DeviceProtocol->SFLIST(&info) != mitk::NDIOKAY || info.empty()) { MITK_ERROR << "Could not receive tracking volume information of tracking system!"; return false; } /*info contains the following: (+n times:) */ (*numberOfVolumes) = (unsigned int)std::atoi(info.substr(0, 1).c_str()); for (unsigned int i = 0; i < (*numberOfVolumes); i++) { //e.g. for cube: "9-025000+025000-025000+025000-055000-005000+000000+000000+000000+00000011" //for dome: "A+005000+048000+005000+066000+000000+000000+000000+000000+000000+00000011" std::string::size_type offset, end; offset = (i * 73) + 1; end = 73 + (i * 73); std::string currentVolume = info.substr(offset, end);//i=0: from 1 to 73 characters; i=1: from 75 to 148 char; // if i>0 then we have a return statement infront if (i > 0) currentVolume = currentVolume.substr(1, currentVolume.size()); if (currentVolume.compare(0, 1, NDIPolarisTypeInformation::GetDeviceDataPolarisOldModel().HardwareCode) == 0) volumes->push_back(NDIPolarisTypeInformation::GetDeviceDataPolarisOldModel().Model); if (currentVolume.compare(0, 3, NDIPolarisTypeInformation::GetDeviceDataPolarisSpectra().HardwareCode) == 0) volumes->push_back(NDIPolarisTypeInformation::GetDeviceDataPolarisSpectra().Model); if (currentVolume.compare(1, 3, NDIPolarisTypeInformation::GetDeviceDataSpectraExtendedPyramid().HardwareCode) == 0) { currentVolume = currentVolume.substr(1, currentVolume.size()); volumes->push_back(NDIPolarisTypeInformation::GetDeviceDataSpectraExtendedPyramid().Model); } if (currentVolume.compare(0, 1, NDIPolarisTypeInformation::GetDeviceDataPolarisVicra().HardwareCode) == 0) volumes->push_back(NDIPolarisTypeInformation::GetDeviceDataPolarisVicra().Model); else if (currentVolume.compare(0, 1, mitk::NDIAuroraTypeInformation::GetDeviceDataAuroraPlanarCube().HardwareCode) == 0) volumes->push_back(mitk::NDIAuroraTypeInformation::GetDeviceDataAuroraPlanarCube().Model);//alias cube else if (currentVolume.compare(0, 1, mitk::NDIAuroraTypeInformation::GetDeviceDataAuroraPlanarDome().HardwareCode) == 0) volumes->push_back(mitk::NDIAuroraTypeInformation::GetDeviceDataAuroraPlanarDome().Model); //fill volumesDimensions for (unsigned int index = 0; index < 10; index++) { std::string::size_type offD, endD; offD = 1 + (index * 7); //7 digits per dimension and the first is the type of volume endD = offD + 7; int dimension = std::atoi(currentVolume.substr(offD, endD).c_str()); dimension /= 100; //given in mm. 7 digits are xxxx.xx according to NDI //strange, the last two digits (11) also for the metal flag get read also... volumesDimensions->push_back(dimension); } } return true; } bool mitk::NDITrackingDevice::SetVolume(mitk::TrackingDeviceData volume) { if (m_DeviceProtocol->VSEL(volume) != mitk::NDIOKAY) { mitkThrowException(mitk::IGTHardwareException) << "Could not set volume!"; } return true; } \ No newline at end of file diff --git a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp index 3a208eceb9..6c9248d390 100644 --- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp @@ -1,524 +1,508 @@ /*=================================================================== 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 "mitkOpenIGTLinkTrackingDevice.h" #include "mitkOpenIGTLinkTrackingTool.h" #include "mitkIGTConfig.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include "mitkTrackingTypes.h" #include #include #include #include #include #include //sleep headers #include #include typedef itk::MutexLockHolder MutexLockHolder; mitk::OpenIGTLinkTrackingDevice::OpenIGTLinkTrackingDevice() : mitk::TrackingDevice(), m_UpdateRate(60) { //set the type of this tracking device this->m_Data = mitk::OpenIGTLinkTypeInformation::GetDeviceDataOpenIGTLinkTrackingDeviceConnection(); m_OpenIGTLinkClient = mitk::IGTLClient::New(true); m_OpenIGTLinkClient->SetName("OpenIGTLink Tracking Device"); m_OpenIGTLinkClient->EnableNoBufferingMode(false); m_IGTLDeviceSource = mitk::IGTLTrackingDataDeviceSource::New(); m_IGTLDeviceSource->SetIGTLDevice(m_OpenIGTLinkClient); } mitk::OpenIGTLinkTrackingDevice::~OpenIGTLinkTrackingDevice() { } int mitk::OpenIGTLinkTrackingDevice::GetPortNumber() { return m_OpenIGTLinkClient->GetPortNumber(); } bool mitk::OpenIGTLinkTrackingDevice::AutoDetectToolsAvailable() { return true; } mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::AutoDetectTools() { mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); if (m_OpenIGTLinkClient->GetPortNumber() == -1) { MITK_WARN << "Connection not initialized, aborting (invalid port number)."; return mitk::NavigationToolStorage::New(); } //open connection try { m_IGTLDeviceSource->Connect(); m_IGTLDeviceSource->StartCommunication(); } catch (std::runtime_error &e) { MITK_WARN << "AutoDetection: Open IGT Link device retruned an error while trying to connect: " << e.what(); return mitk::NavigationToolStorage::New(); } //get a message to find out type m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::UNKNOWN); mitk::IGTLMessage::Pointer receivedMessage = ReceiveMessage(100); const char* msgType = receivedMessage->GetIGTLMessageType(); if (std::string(msgType).empty()) { MITK_INFO << "Did not receive a message. Do you have to start the stream manually at the server?"; MITK_INFO << "Waiting for 10 seconds ..."; receivedMessage = ReceiveMessage(10000); msgType = receivedMessage->GetIGTLMessageType(); } MITK_INFO << "################# got message type: " << msgType; mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type = GetMessageTypeFromString(msgType); switch (type) { case UNKNOWN: m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::UNKNOWN); break; case TDATA: m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::TDATA); break; case QTDATA: m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::QTDATA); break; case TRANSFORM: m_IGTLDeviceSource->SettrackingDataType(mitk::IGTLTrackingDataDeviceSource::TRANSFORM); break; } returnValue = DiscoverToolsAndConvertToNavigationTools(type); //close connection try { m_IGTLDeviceSource->StopCommunication(); m_IGTLDeviceSource->Disconnect(); } catch (std::runtime_error &e) { MITK_WARN << "AutoDetection: Open IGT Link device retruned an error while trying to disconnect: " << e.what(); return mitk::NavigationToolStorage::New(); } return returnValue; } mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::DiscoverToolsAndConvertToNavigationTools(mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type, int NumberOfMessagesToWait) { MITK_INFO << "Start discovering tools by " << type << " messages"; mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); std::map toolNameMap; for (int j = 0; jUpdate(); switch (type) { case TRANSFORM: { igtl::TransformMessage::Pointer msg = dynamic_cast(m_IGTLDeviceSource->GetOutput()->GetMessage().GetPointer()); if (msg == nullptr || msg.IsNull()) { MITK_INFO << "Received message is invalid / null. Skipping.."; continue; } int count = toolNameMap[msg->GetDeviceName()]; if (count == 0) { toolNameMap[msg->GetDeviceName()] = 1; } else { toolNameMap[msg->GetDeviceName()]++; } } break; case TDATA: { igtl::TrackingDataMessage::Pointer msg = dynamic_cast(m_IGTLDeviceSource->GetOutput()->GetMessage().GetPointer()); if (msg == nullptr || msg.IsNull()) { MITK_INFO << "Received message is invalid / null. Skipping.."; continue; } for (int k = 0; k < msg->GetNumberOfTrackingDataElements(); k++) { igtl::TrackingDataElement::Pointer tde; msg->GetTrackingDataElement(k, tde); if (tde.IsNotNull()) { int count = toolNameMap[tde->GetName()]; if (count == 0) { toolNameMap[tde->GetName()] = 1; } else { toolNameMap[tde->GetName()]++; } } } } break; default: MITK_WARN << "Only TRANSFORM and TDATA is currently supported, skipping!"; break; } } int i = 0; for (std::map::iterator it = toolNameMap.begin(); it != toolNameMap.end(); ++it) { MITK_INFO << "Found tool: " << it->first; std::stringstream name; name << it->first; std::stringstream identifier; identifier << "AutoDetectedTool-" << i; i++; mitk::NavigationTool::Pointer newTool = ConstructDefaultOpenIGTLinkTool(name.str(), identifier.str()); returnValue->AddTool(newTool); } return returnValue; } std::string mitk::OpenIGTLinkTrackingDevice::GetHostname() { return m_OpenIGTLinkClient->GetHostname(); } void mitk::OpenIGTLinkTrackingDevice::SetPortNumber(int portNumber) { m_OpenIGTLinkClient->SetPortNumber(portNumber); } void mitk::OpenIGTLinkTrackingDevice::SetHostname(std::string hostname) { m_OpenIGTLinkClient->SetHostname(hostname); } bool mitk::OpenIGTLinkTrackingDevice::IsDeviceInstalled() { return true; } mitk::TrackingTool* mitk::OpenIGTLinkTrackingDevice::AddTool(const char* toolName, const char* fileName) { mitk::OpenIGTLinkTrackingTool::Pointer t;// = mitk::OpenIGTLinkTrackingTool::New(); //TODO: Implement if (this->InternalAddTool(t) == false) return NULL; return t.GetPointer(); } bool mitk::OpenIGTLinkTrackingDevice::InternalAddTool(OpenIGTLinkTrackingTool::Pointer tool) { m_AllTools.push_back(tool); return true; } bool mitk::OpenIGTLinkTrackingDevice::DiscoverTools(int waitingTime) { if (m_OpenIGTLinkClient->GetPortNumber() == -1) { MITK_WARN << "Connection not initialized, aborting (invalid port number)."; return false; } try { m_IGTLDeviceSource->Connect(); m_IGTLDeviceSource->StartCommunication(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what(); return false; } mitk::IGTLMessage::Pointer receivedMessage = ReceiveMessage(waitingTime); //check the tracking stream for the number and type of tools //igtl::MessageBase::Pointer receivedMessage = m_OpenIGTLinkClient->GetNextMessage(); if (receivedMessage.IsNull()) { MITK_WARN << "No message was received. Is there really a server?"; return false; } else if (!receivedMessage->IsDataValid()) { MITK_WARN << "Received invalid message."; return false; } const char* msgType = receivedMessage->GetIGTLMessageType(); mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type = GetMessageTypeFromString(msgType); mitk::NavigationToolStorage::Pointer foundTools = this->DiscoverToolsAndConvertToNavigationTools(type); if (foundTools.IsNull() || (foundTools->GetToolCount() == 0)) { return false; } for (int i = 0; i < foundTools->GetToolCount(); i++) { AddNewToolForName(foundTools->GetTool(i)->GetToolName(), i); } MITK_INFO << "Found tools: " << foundTools->GetToolCount(); return true; } mitk::IGTLMessage::Pointer mitk::OpenIGTLinkTrackingDevice::ReceiveMessage(int waitingTime) { mitk::IGTLMessage::Pointer receivedMessage; //send a message to the server: start tracking stream mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory(); std::string message[2] = {"STT_QTDATA","STT_TDATA"}; for (int i = 0; i < 2; i++) { igtl::MessageBase::Pointer sttMsg = msgFactory->CreateInstance(message[i]); //TODO: Fix this to dynamically get this from GUI ((igtl::StartTrackingDataMessage*)sttMsg.GetPointer())->SetResolution(m_UpdateRate); m_OpenIGTLinkClient->SendMessage(mitk::IGTLMessage::New(sttMsg)); } std::chrono::high_resolution_clock::time_point time = std::chrono::high_resolution_clock::now(); std::chrono::milliseconds d = std::chrono::milliseconds(waitingTime); while (!(receivedMessage.IsNotNull() && receivedMessage->IsDataValid())) { m_IGTLDeviceSource->Update(); receivedMessage = m_IGTLDeviceSource->GetOutput(); if ((time + d) < std::chrono::high_resolution_clock::now()) break; std::this_thread::sleep_for(std::chrono::milliseconds(100)); } return receivedMessage; } void mitk::OpenIGTLinkTrackingDevice::AddNewToolForName(std::string name, int i) { mitk::OpenIGTLinkTrackingTool::Pointer newTool = mitk::OpenIGTLinkTrackingTool::New(); if (name == "") //if no name was given create a default name { std::stringstream defaultName; defaultName << "OpenIGTLinkTool#" << i; name = defaultName.str(); } MITK_INFO << "Added tool " << name << " to tracking device."; newTool->SetToolName(name); InternalAddTool(newTool); } mitk::NavigationTool::Pointer mitk::OpenIGTLinkTrackingDevice::ConstructDefaultOpenIGTLinkTool(std::string name, std::string identifier) { - mitk::DataNode::Pointer newNode = mitk::DataNode::New(); - - newNode->SetName(name); - - mitk::Surface::Pointer myCone = mitk::Surface::New(); - vtkConeSource *vtkData = vtkConeSource::New(); - vtkData->SetAngle(5.0); - vtkData->SetResolution(50); - vtkData->SetHeight(6.0f); - vtkData->SetRadius(2.0f); - vtkData->SetCenter(0.0, 0.0, 0.0); - vtkData->Update(); - myCone->SetVtkPolyData(vtkData->GetOutput()); - vtkData->Delete(); - newNode->SetData(myCone); - mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); - newTool->SetDataNode(newNode); + newTool->GetDataNode()->SetName(name); newTool->SetIdentifier(identifier); newTool->SetTrackingDeviceType(mitk::OpenIGTLinkTypeInformation::GetDeviceDataOpenIGTLinkTrackingDeviceConnection().Line); return newTool; } void mitk::OpenIGTLinkTrackingDevice::UpdateTools() { if (this->GetState() != Tracking) { MITK_ERROR << "Method was called in the wrong state, something went wrong!"; return; } m_IGTLMsgToNavDataFilter->Update(); for (int j = 0; j < m_IGTLMsgToNavDataFilter->GetNumberOfIndexedOutputs(); j++) { mitk::NavigationData::Pointer currentNavData = m_IGTLMsgToNavDataFilter->GetOutput(j); const char* name = currentNavData->GetName(); for (int i = 0; i < m_AllTools.size(); i++) { if (strcmp(m_AllTools.at(i)->GetToolName(), name) == 0) { m_AllTools.at(i)->SetDataValid(currentNavData->IsDataValid()); m_AllTools.at(i)->SetPosition(currentNavData->GetPosition()); m_AllTools.at(i)->SetOrientation(currentNavData->GetOrientation()); m_AllTools.at(i)->SetIGTTimeStamp(currentNavData->GetIGTTimeStamp()); } } } } bool mitk::OpenIGTLinkTrackingDevice::StartTracking() { //check tracking state if (this->GetState() != Ready) { MITK_WARN << "Cannot start tracking, device is not ready!"; return false; } try { m_IGTLDeviceSource->StartCommunication(); //send a message to the server: start tracking stream mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory(); std::string message = "STT_TDATA"; //m_OpenIGTLinkClient->SendMessage(msgFactory->CreateInstance(message)); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while starting communication: " << e.what(); return false; } //create internal igtl pipeline m_IGTLMsgToNavDataFilter = mitk::IGTLMessageToNavigationDataFilter::New(); m_IGTLMsgToNavDataFilter->SetNumberOfExpectedOutputs(this->GetToolCount()); m_IGTLMsgToNavDataFilter->ConnectTo(m_IGTLDeviceSource); //connect itk events typedef itk::SimpleMemberCommand< mitk::OpenIGTLinkTrackingDevice > CurCommandType; CurCommandType::Pointer messageReceivedCommand = CurCommandType::New(); messageReceivedCommand->SetCallbackFunction(this, &mitk::OpenIGTLinkTrackingDevice::UpdateTools); m_MessageReceivedObserverTag = m_OpenIGTLinkClient->AddObserver(mitk::MessageReceivedEvent(), messageReceivedCommand); m_OpenIGTLinkClient->EnableNoBufferingMode(true); this->SetState(Tracking); return true; } bool mitk::OpenIGTLinkTrackingDevice::StopTracking() { //check tracking state if (this->GetState() != Tracking) { MITK_WARN << "Cannot open connection, device is already connected!"; return false; } m_OpenIGTLinkClient->RemoveObserver(m_MessageReceivedObserverTag); //disconnect itk events try { m_IGTLDeviceSource->StopCommunication(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while stopping communication: " << e.what(); return false; } m_OpenIGTLinkClient->EnableNoBufferingMode(false); this->SetState(Ready); return true; } unsigned int mitk::OpenIGTLinkTrackingDevice::GetToolCount() const { return (unsigned int)this->m_AllTools.size(); } mitk::TrackingTool* mitk::OpenIGTLinkTrackingDevice::GetTool(unsigned int toolNumber) const { if (toolNumber >= this->GetToolCount()) return NULL; else return this->m_AllTools[toolNumber]; } bool mitk::OpenIGTLinkTrackingDevice::OpenConnection() { //check tracking state if (this->GetState() != Setup) { MITK_WARN << "Cannot open connection, device is already connected!"; return false; } try { m_IGTLDeviceSource->Connect(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what(); return false; } this->SetState(Ready); return true; } bool mitk::OpenIGTLinkTrackingDevice::CloseConnection() { //check tracking state if (this->GetState() != Ready) { MITK_WARN << "Cannot close connection, device is in the wrong state!"; return false; } try { m_IGTLDeviceSource->Disconnect(); } catch (std::runtime_error &e) { MITK_WARN << "Open IGT Link device retruned an error while trying to disconnect: " << e.what(); return false; } this->SetState(Setup); return true; } std::vector mitk::OpenIGTLinkTrackingDevice::GetAllTools() { return this->m_AllTools; } mitk::OpenIGTLinkTrackingDevice::TrackingMessageType mitk::OpenIGTLinkTrackingDevice::GetMessageTypeFromString(const char* messageTypeString) { if (strcmp(messageTypeString, "TDATA") == 0) { return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::TDATA; } else if (strcmp(messageTypeString, "QTDATA") == 0) { return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::QTDATA; } else if (strcmp(messageTypeString, "TRANSFORM") == 0) { return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::TRANSFORM; } else { return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::UNKNOWN; } } diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp index c05d70f7dd..7121391bfe 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp @@ -1,332 +1,317 @@ /*=================================================================== 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 "mitkPolhemusTrackingDevice.h" #include "mitkPolhemusTool.h" #include "mitkIGTConfig.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include #include #include #include "mitkPolhemusTrackerTypeInformation.h" -#include typedef itk::MutexLockHolder MutexLockHolder; mitk::PolhemusTrackingDevice::PolhemusTrackingDevice() : mitk::TrackingDevice() { //set the type of this tracking device this->m_Data = mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty(); this->m_MultiThreader = itk::MultiThreader::New(); m_ThreadID = 0; m_Device = mitk::PolhemusInterface::New(); } mitk::PolhemusTrackingDevice::~PolhemusTrackingDevice() { } bool mitk::PolhemusTrackingDevice::IsDeviceInstalled() { return true; } mitk::TrackingTool* mitk::PolhemusTrackingDevice::AddTool(const char* toolName, int toolPort) { //Only add tool if port isn't already used. for (auto _tool : m_AllTools) { if (_tool->GetToolPort() == toolPort) { MITK_DEBUG << "There is already a tool connected to this port. Returning existing tool"; return _tool; } } mitk::PolhemusTool::Pointer t = mitk::PolhemusTool::New(); t->SetToolName(toolName); t->SetToolPort(toolPort); if (this->InternalAddTool(t) == false) return nullptr; return t.GetPointer(); } bool mitk::PolhemusTrackingDevice::InternalAddTool(PolhemusTool::Pointer tool) { m_AllTools.push_back(tool); return true; } bool mitk::PolhemusTrackingDevice::StartTracking() { bool success = m_Device->StartTracking(); if (success) { mitk::IGTTimeStamp::GetInstance()->Start(this); this->SetState(Tracking); this->m_StopTrackingMutex->Lock(); this->m_StopTracking = false; this->m_StopTrackingMutex->Unlock(); m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method return true; } else { this->SetState(Ready); mitkThrowException(mitk::IGTHardwareException) << "Error while trying to start the device!"; } return success; } bool mitk::PolhemusTrackingDevice::StopTracking() { m_Device->StopTracking(); return Superclass::StopTracking(); } unsigned int mitk::PolhemusTrackingDevice::GetToolCount() const { return (unsigned int)this->m_AllTools.size(); } mitk::TrackingTool* mitk::PolhemusTrackingDevice::GetTool(unsigned int toolNumber) const { if (toolNumber >= this->GetToolCount()) return nullptr; else return this->m_AllTools[toolNumber]; } bool mitk::PolhemusTrackingDevice::OpenConnection() { //reset everything if (m_Device.IsNull()) { m_Device = mitk::PolhemusInterface::New(); } if (!m_Device->Connect()) //Connect the device, if it fails, throw an error. { MITK_ERROR << "Cannot connect Polhemus device!"; CloseConnection(); return false; } //Ready must be set here, 'cause if tools don't match we need to be able to disconnect. this->SetState(Ready); //check if connected ports of Polhemus matches the tools in the toolStorage. std::vector toolPorts = m_Device->GetToolPorts(); //first, check size. if (this->GetToolCount() != toolPorts.size()) { MITK_ERROR << "Cannot connect device, number of tools in toolstorage doesn't match the number of tools connected to Polhemus device!"; CloseConnection(); return false; } //second, check if toolStorage identifier is included in this port. for (auto _tool : m_AllTools) { if (std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort()) == toolPorts.end()) { MITK_ERROR << "Cannot connect device, tool " << _tool->GetToolPort() << " is not connected to its port."; CloseConnection(); return false; } else { //erase this port to avoid that two tools want to connect to the same port. toolPorts.erase(std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort())); } } m_Device->SetHemisphereTrackingEnabled(m_HemisphereTrackingEnabled); return true; } bool mitk::PolhemusTrackingDevice::CloseConnection() { bool returnValue = true; if (this->GetState() == Setup) return true; returnValue = m_Device->Disconnect(); this->SetState(Setup); return returnValue; } mitk::PolhemusInterface* mitk::PolhemusTrackingDevice::GetDevice() { return m_Device; } std::vector mitk::PolhemusTrackingDevice::GetAllTools() { return this->m_AllTools; } void mitk::PolhemusTrackingDevice::TrackTools() { try { /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; this->m_StopTrackingMutex->Unlock(); Sleep(100);//Wait a bit until the tracker is ready... while ((this->GetState() == Tracking) && (localStopTracking == false)) { std::vector lastData = this->GetDevice()->GetLastFrame(); if (lastData.size() != m_AllTools.size()) { MITK_WARN << "Tool count is corrupt. Hardware gives " << lastData.size() << " tools, MITK expects " << m_AllTools.size() << " tools. Aborting!"; } else { std::vector allTools = this->GetAllTools(); for (int i = 0; i < allTools.size(); i++) { mitk::PolhemusTool::Pointer currentTool = allTools.at(i); currentTool->SetDataValid(true); currentTool->SetPosition(lastData.at(i).pos); currentTool->SetOrientation(lastData.at(i).rot); currentTool->SetIGTTimeStamp(mitk::IGTTimeStamp::GetInstance()->GetElapsed()); } } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } } catch (...) { this->StopTracking(); mitkThrowException(mitk::IGTHardwareException) << "Error while trying to track tools. Thread stopped."; } } ITK_THREAD_RETURN_TYPE mitk::PolhemusTrackingDevice::ThreadStartTracking(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == nullptr) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == nullptr) { return ITK_THREAD_RETURN_VALUE; } PolhemusTrackingDevice *trackingDevice = (PolhemusTrackingDevice*)pInfo->UserData; if (trackingDevice != nullptr) trackingDevice->TrackTools(); return ITK_THREAD_RETURN_VALUE; } bool mitk::PolhemusTrackingDevice::AutoDetectToolsAvailable() { return true; } mitk::NavigationToolStorage::Pointer mitk::PolhemusTrackingDevice::AutoDetectTools() { std::vector singeFrameData = this->m_Device->AutoDetectTools(); MITK_INFO << "Found " << singeFrameData.size() << " tools."; mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); for each (mitk::PolhemusInterface::trackingData t in singeFrameData) { - mitk::DataNode::Pointer newNode = mitk::DataNode::New(); + mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); + std::stringstream name; name << "Sensor-" << ((int)t.id); - newNode->SetName(name.str()); - - mitk::Surface::Pointer myCone = mitk::Surface::New(); - vtkConeSource *vtkData = vtkConeSource::New(); - vtkData->SetAngle(5.0); - vtkData->SetResolution(50); - vtkData->SetHeight(6.0f); - vtkData->SetRadius(2.0f); - vtkData->SetCenter(0.0, 0.0, 0.0); - vtkData->Update(); - myCone->SetVtkPolyData(vtkData->GetOutput()); - vtkData->Delete(); - newNode->SetData(myCone); - - mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); - newTool->SetDataNode(newNode); + newTool->GetDataNode()->SetName(name.str()); //The identifier defines, which plug is used (e.g. "Sens 2" --> 2). std::stringstream identifier; identifier << ((int)t.id); newTool->SetIdentifier(identifier.str()); newTool->SetTrackingDeviceType(mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty().Line); returnValue->AddTool(newTool); } return returnValue; } void mitk::PolhemusTrackingDevice::SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled) { //We need to remember if HemisphereTracking is switch on for this reason: /* m_Device->SetHemi works only if the device is connected. However, GUI can also change if it is not connected. In this case, we remember it in the m_HemisphereTrackingEnabled variable. And when connecting, we know, which status is wanted from the user by GUI. */ m_HemisphereTrackingEnabled = _HemisphereTrackingEnabled; this->m_Device->SetHemisphereTrackingEnabled(_HemisphereTrackingEnabled); } void mitk::PolhemusTrackingDevice::ToggleHemisphere(int _tool) { this->m_Device->ToggleHemisphere(_tool); } void mitk::PolhemusTrackingDevice::SetHemisphere(int _tool, mitk::Vector3D _hemisphere) { //If you set a hemisphere vector which is unequal (0|0|0), this means, that there is no hemisphere tracking any more //disable the option, so that it can be reactivated... Also if it is just a single tool. if (_hemisphere.GetNorm() != 0) m_HemisphereTrackingEnabled = false; this->m_Device->SetHemisphere(_tool, _hemisphere); } mitk::Vector3D mitk::PolhemusTrackingDevice::GetHemisphere(int _tool) { return this->m_Device->GetHemisphere(_tool); } bool mitk::PolhemusTrackingDevice::GetHemisphereTrackingEnabled(int _tool) { return this->m_Device->GetHemisphereTrackingEnabled(_tool); } void mitk::PolhemusTrackingDevice::AdjustHemisphere(int _tool) { return this->m_Device->AdjustHemisphere(_tool); } \ No newline at end of file diff --git a/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.cpp b/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.cpp index aa1817928d..be33438ebc 100644 --- a/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.cpp +++ b/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.cpp @@ -1,420 +1,398 @@ /*=================================================================== 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 "QmitkNavigationToolCreationWidget.h" //mitk headers #include #include #include #include #include "mitkTrackingDeviceTypeCollection.h" //qt headers #include #include #include #include #include //poco headers #include // vtk #include #include const std::string QmitkNavigationToolCreationWidget::VIEW_ID = "org.mitk.views.navigationtoolcreationwizardwidget"; QmitkNavigationToolCreationWidget::QmitkNavigationToolCreationWidget(QWidget* parent, Qt::WindowFlags f) : QWidget(parent, f) { m_ToolToBeEdited = mitk::NavigationTool::New(); m_FinalTool = mitk::NavigationTool::New(); m_ToolTransformationWidget = new QmitkInteractiveTransformationWidget(); m_Controls = NULL; CreateQtPartControl(this); CreateConnections(); this->InitializeUIToolLandmarkLists(); Initialize(nullptr, ""); //Default values, which are not stored in tool m_Controls->m_CalibrationFileName->setText("none"); m_Controls->m_Surface_Use_Sphere->setChecked(true); m_Controls->m_CalibrationLandmarksList->EnableEditButton(false); m_Controls->m_RegistrationLandmarksList->EnableEditButton(false); RefreshTrackingDeviceCollection(); } QmitkNavigationToolCreationWidget::~QmitkNavigationToolCreationWidget() { m_Controls->m_CalibrationLandmarksList->SetPointSetNode(NULL); m_Controls->m_RegistrationLandmarksList->SetPointSetNode(NULL); delete m_ToolTransformationWidget; } void QmitkNavigationToolCreationWidget::CreateQtPartControl(QWidget *parent) { if (!m_Controls) { // create GUI widgets m_Controls = new Ui::QmitkNavigationToolCreationWidgetControls; m_Controls->setupUi(parent); } } void QmitkNavigationToolCreationWidget::CreateConnections() { if (m_Controls) { connect((QObject*)(m_Controls->m_TrackingDeviceTypeChooser), SIGNAL(currentIndexChanged(int)), this, SLOT(GetValuesFromGuiElements())); connect((QObject*)(m_Controls->m_ToolNameEdit), SIGNAL(textChanged(const QString)), this, SLOT(GetValuesFromGuiElements())); connect((QObject*)(m_Controls->m_ToolTypeChooser), SIGNAL(currentIndexChanged(int)), this, SLOT(GetValuesFromGuiElements())); connect((QObject*)(m_Controls->m_IdentifierEdit), SIGNAL(textChanged(const QString)), this, SLOT(GetValuesFromGuiElements())); connect((QObject*)(m_Controls->m_SerialNumberEdit), SIGNAL(textChanged(const QString)), this, SLOT(GetValuesFromGuiElements())); connect((QObject*)(m_Controls->m_ToolAxisX), SIGNAL(valueChanged(int)), this, SLOT(GetValuesFromGuiElements())); connect((QObject*)(m_Controls->m_ToolAxisY), SIGNAL(valueChanged(int)), this, SLOT(GetValuesFromGuiElements())); connect((QObject*)(m_Controls->m_ToolAxisZ), SIGNAL(valueChanged(int)), this, SLOT(GetValuesFromGuiElements())); //Buttons connect((QObject*)(m_Controls->m_LoadCalibrationFile), SIGNAL(clicked()), this, SLOT(OnLoadCalibrationFile())); connect(m_Controls->m_Surface_Use_Other, SIGNAL(toggled(bool)), this, SLOT(OnSurfaceUseOtherToggled())); connect((QObject*)(m_Controls->m_LoadSurface), SIGNAL(clicked()), this, SLOT(OnLoadSurface())); connect((QObject*)(m_Controls->m_EditToolTip), SIGNAL(clicked()), this, SLOT(OnEditToolTip())); connect((QObject*)(m_ToolTransformationWidget), SIGNAL(EditToolTipFinished(mitk::AffineTransform3D::Pointer)), this, SLOT(OnEditToolTipFinished(mitk::AffineTransform3D::Pointer))); connect((QObject*)(m_Controls->m_cancel), SIGNAL(clicked()), this, SLOT(OnCancel())); connect((QObject*)(m_Controls->m_finished), SIGNAL(clicked()), this, SLOT(OnFinished())); } } void QmitkNavigationToolCreationWidget::Initialize(mitk::DataStorage* dataStorage, const std::string& supposedIdentifier, const std::string& supposedName) { m_DataStorage = dataStorage; //initialize UI components m_Controls->m_SurfaceChooser->SetDataStorage(m_DataStorage); m_Controls->m_SurfaceChooser->SetAutoSelectNewItems(true); m_Controls->m_SurfaceChooser->SetPredicate(mitk::NodePredicateDataType::New("Surface")); //Create new tool, which should be edited/created m_ToolToBeEdited = nullptr;//Reset m_ToolToBeEdited = mitk::NavigationTool::New();//Reinitialize m_ToolToBeEdited->SetIdentifier(supposedIdentifier); - //Create the default cone surface. Can be changed later on... - //create DataNode... - mitk::DataNode::Pointer newNode = mitk::DataNode::New(); - newNode->SetName(supposedName); - m_ToolToBeEdited->SetDataNode(newNode); - - SetConeAsToolSurface(); + m_ToolToBeEdited->GetDataNode()->SetName(supposedName); this->SetDefaultData(m_ToolToBeEdited); } void QmitkNavigationToolCreationWidget::ShowToolPreview(std::string _name) { m_DataStorage->Add(m_ToolToBeEdited->GetDataNode()); m_ToolToBeEdited->GetDataNode()->SetName(_name); //change color to blue m_ToolToBeEdited->GetDataNode()->SetProperty("color", mitk::ColorProperty::New(0, 0, 1)); //Global Reinit to show new tool mitk::RenderingManager::GetInstance()->InitializeViewsByBoundingObjects(m_DataStorage); } -void QmitkNavigationToolCreationWidget::SetConeAsToolSurface() -{ - //create small cone and use it as surface - mitk::Surface::Pointer mySphere = mitk::Surface::New(); - vtkConeSource *vtkData = vtkConeSource::New(); - vtkData->SetAngle(5.0); - vtkData->SetResolution(50); - vtkData->SetHeight(6.0f); - vtkData->SetRadius(2.0f); - vtkData->SetCenter(0.0, 0.0, 0.0); - vtkData->Update(); - mySphere->SetVtkPolyData(vtkData->GetOutput()); - vtkData->Delete(); - m_ToolToBeEdited->GetDataNode()->SetData(mySphere); -} - void QmitkNavigationToolCreationWidget::SetDefaultData(mitk::NavigationTool::Pointer DefaultTool) { //Set Members. This can either be the new initialized tool from call of Initialize() or a tool which already exists in the toolStorage m_ToolToBeEdited = DefaultTool->Clone(); //Set all gui variables SetGuiElements(); } void QmitkNavigationToolCreationWidget::SetGuiElements() { //Block signals, so that we don't call SetGuiElements again. This is undone at the end of this function! m_Controls->m_TrackingDeviceTypeChooser->blockSignals(true); m_Controls->m_ToolNameEdit->blockSignals(true); m_Controls->m_ToolTypeChooser->blockSignals(true); m_Controls->m_IdentifierEdit->blockSignals(true); m_Controls->m_SerialNumberEdit->blockSignals(true); m_Controls->m_ToolAxisX->blockSignals(true); m_Controls->m_ToolAxisY->blockSignals(true); m_Controls->m_ToolAxisZ->blockSignals(true); //DeviceType int index = m_Controls->m_TrackingDeviceTypeChooser->findText(QString::fromStdString(m_ToolToBeEdited->GetTrackingDeviceType())); if (index >= 0) { m_Controls->m_TrackingDeviceTypeChooser->setCurrentIndex(index); } m_Controls->m_ToolNameEdit->setText(QString(m_ToolToBeEdited->GetToolName().c_str())); m_Controls->m_CalibrationFileName->setText(QString(m_ToolToBeEdited->GetCalibrationFile().c_str())); m_Controls->m_SurfaceChooser->SetSelectedNode(m_ToolToBeEdited->GetDataNode()); FillUIToolLandmarkLists(m_ToolToBeEdited->GetToolCalibrationLandmarks(), m_ToolToBeEdited->GetToolRegistrationLandmarks()); switch (m_ToolToBeEdited->GetType()) { case mitk::NavigationTool::Instrument: m_Controls->m_ToolTypeChooser->setCurrentIndex(0); break; case mitk::NavigationTool::Fiducial: m_Controls->m_ToolTypeChooser->setCurrentIndex(1); break; case mitk::NavigationTool::Skinmarker: m_Controls->m_ToolTypeChooser->setCurrentIndex(2); break; case mitk::NavigationTool::Unknown: m_Controls->m_ToolTypeChooser->setCurrentIndex(3); break; } m_Controls->m_IdentifierEdit->setText(QString(m_ToolToBeEdited->GetIdentifier().c_str())); m_Controls->m_SerialNumberEdit->setText(QString(m_ToolToBeEdited->GetSerialNumber().c_str())); m_Controls->m_ToolAxisX->setValue(m_ToolToBeEdited->GetToolAxis()[0]); m_Controls->m_ToolAxisY->setValue(m_ToolToBeEdited->GetToolAxis()[1]); m_Controls->m_ToolAxisZ->setValue(m_ToolToBeEdited->GetToolAxis()[2]); QString _label = "(" + QString::number(m_ToolToBeEdited->GetToolTipPosition()[0], 'f', 1) + ", " + QString::number(m_ToolToBeEdited->GetToolTipPosition()[1], 'f', 1) + ", " + QString::number(m_ToolToBeEdited->GetToolTipPosition()[2], 'f', 1) + "), quat: [" + QString::number(m_ToolToBeEdited->GetToolTipOrientation()[0], 'f', 2) + ", " + QString::number(m_ToolToBeEdited->GetToolTipOrientation()[1], 'f', 2) + ", " + QString::number(m_ToolToBeEdited->GetToolTipOrientation()[2], 'f', 2) + ", " + QString::number(m_ToolToBeEdited->GetToolTipOrientation()[3], 'f', 2) + "]"; m_Controls->m_ToolTipLabel->setText(_label); //Undo block signals. Don't remove it, if signals are still blocked at the beginning of this function! m_Controls->m_TrackingDeviceTypeChooser->blockSignals(false); m_Controls->m_ToolNameEdit->blockSignals(false); m_Controls->m_ToolTypeChooser->blockSignals(false); m_Controls->m_IdentifierEdit->blockSignals(false); m_Controls->m_SerialNumberEdit->blockSignals(false); m_Controls->m_ToolAxisX->blockSignals(false); m_Controls->m_ToolAxisY->blockSignals(false); m_Controls->m_ToolAxisZ->blockSignals(false); } void QmitkNavigationToolCreationWidget::OnSurfaceUseOtherToggled() { m_Controls->m_LoadSurface->setEnabled(m_Controls->m_Surface_Use_Other->isChecked()); if (m_Controls->m_Surface_Use_Sphere->isChecked()) - SetConeAsToolSurface(); + m_ToolToBeEdited->SetDefaultSurface(); //Global Reinit to show tool surface preview mitk::RenderingManager::GetInstance()->InitializeViewsByBoundingObjects(m_DataStorage); } void QmitkNavigationToolCreationWidget::OnLoadSurface() { std::string filename = QFileDialog::getOpenFileName(NULL, tr("Open Surface"), QmitkIGTCommonHelper::GetLastFileLoadPath(), tr("STL (*.stl)")).toLatin1().data(); QmitkIGTCommonHelper::SetLastFileLoadPathByFileName(QString::fromStdString(filename)); mitk::Surface::Pointer surface; try { surface = mitk::IOUtil::LoadSurface(filename.c_str()); } catch (mitk::Exception &e) { MITK_ERROR << "Exception occured: " << e.what(); return; } m_ToolToBeEdited->GetDataNode()->SetData(surface); //Global Reinit to show tool surface or preview mitk::RenderingManager::GetInstance()->InitializeViewsByBoundingObjects(m_DataStorage); } void QmitkNavigationToolCreationWidget::OnLoadCalibrationFile() { QString fileName = QFileDialog::getOpenFileName(NULL, tr("Open Calibration File"), QmitkIGTCommonHelper::GetLastFileLoadPath(), "*.*"); QmitkIGTCommonHelper::SetLastFileLoadPathByFileName(fileName); m_Controls->m_CalibrationFileName->setText(fileName); m_ToolToBeEdited->SetCalibrationFile(fileName.toStdString()); } void QmitkNavigationToolCreationWidget::GetValuesFromGuiElements() { //Tracking Device m_ToolToBeEdited->SetTrackingDeviceType(m_Controls->m_TrackingDeviceTypeChooser->currentText().toStdString()); m_ToolToBeEdited->GetDataNode()->SetName(m_Controls->m_ToolNameEdit->text().toStdString()); //Tool Landmarks mitk::PointSet::Pointer toolCalLandmarks, toolRegLandmarks; GetUIToolLandmarksLists(toolCalLandmarks, toolRegLandmarks); m_ToolToBeEdited->SetToolCalibrationLandmarks(toolCalLandmarks); m_ToolToBeEdited->SetToolRegistrationLandmarks(toolRegLandmarks); //Advanced if (m_Controls->m_ToolTypeChooser->currentText() == "Instrument") m_ToolToBeEdited->SetType(mitk::NavigationTool::Instrument); else if (m_Controls->m_ToolTypeChooser->currentText() == "Fiducial") m_ToolToBeEdited->SetType(mitk::NavigationTool::Fiducial); else if (m_Controls->m_ToolTypeChooser->currentText() == "Skinmarker") m_ToolToBeEdited->SetType(mitk::NavigationTool::Skinmarker); else m_FinalTool->SetType(mitk::NavigationTool::Unknown); m_ToolToBeEdited->SetIdentifier(m_Controls->m_IdentifierEdit->text().toLatin1().data()); m_ToolToBeEdited->SetSerialNumber(m_Controls->m_SerialNumberEdit->text().toLatin1().data()); //Tool Axis mitk::Point3D toolAxis; toolAxis.SetElement(0, (m_Controls->m_ToolAxisX->value())); toolAxis.SetElement(1, (m_Controls->m_ToolAxisY->value())); toolAxis.SetElement(2, (m_Controls->m_ToolAxisZ->value())); m_ToolToBeEdited->SetToolAxis(toolAxis); } mitk::NavigationTool::Pointer QmitkNavigationToolCreationWidget::GetCreatedTool() { return m_FinalTool; } void QmitkNavigationToolCreationWidget::OnFinished() { //here we create a new tool m_FinalTool = m_ToolToBeEdited->Clone(); //Set the correct name of data node, cause the m_ToolToBeEdited was called "Tool preview" m_FinalTool->GetDataNode()->SetName(m_Controls->m_ToolNameEdit->text().toStdString()); emit NavigationToolFinished(); } void QmitkNavigationToolCreationWidget::OnCancel() { Initialize(nullptr, "");//Reset everything to a fresh tool, like it was done in the constructor emit Canceled(); } void QmitkNavigationToolCreationWidget::SetTrackingDeviceType(mitk::TrackingDeviceType type, bool changeable /*= true*/) { //Adapt Gui int index = m_Controls->m_TrackingDeviceTypeChooser->findText(QString::fromStdString(type)); if (index >= 0) { m_Controls->m_TrackingDeviceTypeChooser->setCurrentIndex(index); } m_Controls->m_TrackingDeviceTypeChooser->setEditable(changeable); //Set data to member m_ToolToBeEdited->SetTrackingDeviceType(type); } //################################################################################## //############################## internal help methods ############################# //################################################################################## void QmitkNavigationToolCreationWidget::MessageBox(std::string s) { QMessageBox msgBox; msgBox.setText(s.c_str()); msgBox.exec(); } void QmitkNavigationToolCreationWidget::OnEditToolTip() { m_ToolTransformationWidget->SetToolToEdit(m_ToolToBeEdited); m_ToolTransformationWidget->SetDefaultRotation(m_ToolToBeEdited->GetToolTipOrientation()); m_ToolTransformationWidget->SetDefaultOffset(m_ToolToBeEdited->GetToolTipPosition()); m_ToolTransformationWidget->open(); } void QmitkNavigationToolCreationWidget::OnEditToolTipFinished(mitk::AffineTransform3D::Pointer toolTip) { //if user pressed cancle, nullptr is returned. Do nothing. Else, set values. if (toolTip) { m_ToolToBeEdited->SetToolTipPosition(toolTip->GetOffset()); mitk::NavigationData::Pointer tempND = mitk::NavigationData::New(toolTip);//Convert to Navigation data for simple transversion to quaternion m_ToolToBeEdited->SetToolTipOrientation(tempND->GetOrientation()); //Update Label QString _label = "(" + QString::number(m_ToolToBeEdited->GetToolTipPosition()[0], 'f', 1) + ", " + QString::number(m_ToolToBeEdited->GetToolTipPosition()[1], 'f', 1) + ", " + QString::number(m_ToolToBeEdited->GetToolTipPosition()[2], 'f', 1) + "), quat: [" + QString::number(m_ToolToBeEdited->GetToolTipOrientation()[0], 'f', 2) + ", " + QString::number(m_ToolToBeEdited->GetToolTipOrientation()[1], 'f', 2) + ", " + QString::number(m_ToolToBeEdited->GetToolTipOrientation()[2], 'f', 2) + ", " + QString::number(m_ToolToBeEdited->GetToolTipOrientation()[3], 'f', 2) + "]"; m_Controls->m_ToolTipLabel->setText(_label); } } void QmitkNavigationToolCreationWidget::FillUIToolLandmarkLists(mitk::PointSet::Pointer calLandmarks, mitk::PointSet::Pointer regLandmarks) { m_calLandmarkNode->SetData(calLandmarks); m_regLandmarkNode->SetData(regLandmarks); m_Controls->m_CalibrationLandmarksList->SetPointSetNode(m_calLandmarkNode); m_Controls->m_RegistrationLandmarksList->SetPointSetNode(m_regLandmarkNode); } void QmitkNavigationToolCreationWidget::GetUIToolLandmarksLists(mitk::PointSet::Pointer& calLandmarks, mitk::PointSet::Pointer& regLandmarks) { calLandmarks = dynamic_cast(m_calLandmarkNode->GetData()); regLandmarks = dynamic_cast(m_regLandmarkNode->GetData()); } void QmitkNavigationToolCreationWidget::InitializeUIToolLandmarkLists() { m_calLandmarkNode = mitk::DataNode::New(); m_regLandmarkNode = mitk::DataNode::New(); FillUIToolLandmarkLists(mitk::PointSet::New(), mitk::PointSet::New()); } void QmitkNavigationToolCreationWidget::RefreshTrackingDeviceCollection() { us::ModuleContext* context = us::GetModuleContext(); std::vector > refs = context->GetServiceReferences(); if (refs.empty()) { MITK_WARN << "No tracking device service found!"; return; } mitk::TrackingDeviceTypeCollection* _DeviceTypeCollection = context->GetService(refs.front()); for (auto name : _DeviceTypeCollection->GetTrackingDeviceTypeNames()) { //if the device is not included yet, add name to comboBox and widget to stackedWidget if (m_Controls->m_TrackingDeviceTypeChooser->findText(QString::fromStdString(name)) == -1) { m_Controls->m_TrackingDeviceTypeChooser->addItem(QString::fromStdString(name)); } } } \ No newline at end of file diff --git a/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.h b/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.h index b468fb4598..963e633b5c 100644 --- a/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.h +++ b/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.h @@ -1,147 +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 QmitkNavigationToolCreationWidget_H #define QmitkNavigationToolCreationWidget_H //QT headers #include //mitk headers #include "MitkIGTUIExports.h" #include #include #include #include "QmitkInteractiveTransformationWidget.h" #include //Microservices #include #include #include #include //ui header #include "ui_QmitkNavigationToolCreationWidget.h" /** Documentation: * \brief An object of this class offers an UI to create or modify NavigationTools. * * Be sure to call the initialize method before you start the widget * otherwise some errors might occure. * * \ingroup IGTUI */ class MITKIGTUI_EXPORT QmitkNavigationToolCreationWidget : public QWidget { Q_OBJECT public: static const std::string VIEW_ID; /** @brief Initializes the widget. * @param dataStorage The data storage is needed to offer the possibility to choose surfaces from the data storage for tool visualization. * @param supposedIdentifier This Identifier is supposed for the user. It is needed because every identifier in a navigation tool storage must be unique and we don't know the others. */ void Initialize(mitk::DataStorage* dataStorage, const std::string &supposedIdentifier, const std::string &supposedName = "NewTool"); /** @brief This Function will add a new node to the Data Manager with given name to enable a preview of the m_ToolToBeEdited */ void ShowToolPreview(std::string _name); /** @brief Sets the default tracking device type. You may also define if it is changeable or not.*/ void SetTrackingDeviceType(mitk::TrackingDeviceType type, bool changeable = true); /** @brief Sets the default data of all input fields. The default data is used from the default tool which is given as parameter. */ void SetDefaultData(mitk::NavigationTool::Pointer DefaultTool); QmitkNavigationToolCreationWidget(QWidget* parent = 0, Qt::WindowFlags f = 0); ~QmitkNavigationToolCreationWidget(); /** @return Returns the created tool. Returns NULL if no tool was created yet. */ mitk::NavigationTool::Pointer GetCreatedTool(); signals: /** @brief This signal is emitted if the user finished the creation of the tool. */ void NavigationToolFinished(); /** @brief This signal is emitted if the user canceled the creation of the tool. */ void Canceled(); protected slots: void OnLoadCalibrationFile(); void OnSurfaceUseOtherToggled(); void OnLoadSurface(); void OnEditToolTip(); void OnEditToolTipFinished(mitk::AffineTransform3D::Pointer toolTip); void OnCancel(); void OnFinished(); void GetValuesFromGuiElements(); private: //############## private help methods ####################### /** Shows a message box with the given message s. */ void MessageBox(std::string s); /** Set the tool landmark lists in the UI.*/ void FillUIToolLandmarkLists(mitk::PointSet::Pointer calLandmarks, mitk::PointSet::Pointer regLandmarks); /** Returns the tool landmark lists from the UI. * @param[out] calLandmarks Returns a pointer to the calibration landmarks point set. * @param[out] regLandmarks Returns a pointer to the registration landmarks point set. */ void GetUIToolLandmarksLists(mitk::PointSet::Pointer& calLandmarks, mitk::PointSet::Pointer& regLandmarks); /** Initializes the tool landmark lists in the UI. */ void InitializeUIToolLandmarkLists(); void RefreshTrackingDeviceCollection(); void SetGuiElements(); - void SetConeAsToolSurface(); - protected: /// \brief Creation of the connections virtual void CreateConnections(); virtual void CreateQtPartControl(QWidget *parent); Ui::QmitkNavigationToolCreationWidgetControls* m_Controls; /** @brief this pointer holds the tool which is edited. If finished is clicked, it will be copied to the final tool, if it is cancled, it is reseted and not used. This can be regarded as the clipboard for all changes. */ mitk::NavigationTool::Pointer m_ToolToBeEdited; /** @brief this pointer holds the tool which is created and returned */ mitk::NavigationTool::Pointer m_FinalTool; QmitkInteractiveTransformationWidget* m_ToolTransformationWidget; /** @brief holds the DataStorage */ mitk::DataStorage* m_DataStorage; /** Hold the data nodes which are needed for the landmark widgets. */ mitk::DataNode::Pointer m_calLandmarkNode, m_regLandmarkNode; }; #endif diff --git a/Plugins/org.mitk.gui.qt.igtexamples/src/internal/QmitkIGTTrackingLabView.cpp b/Plugins/org.mitk.gui.qt.igtexamples/src/internal/QmitkIGTTrackingLabView.cpp index 2ad530037d..52b353a50d 100644 --- a/Plugins/org.mitk.gui.qt.igtexamples/src/internal/QmitkIGTTrackingLabView.cpp +++ b/Plugins/org.mitk.gui.qt.igtexamples/src/internal/QmitkIGTTrackingLabView.cpp @@ -1,737 +1,734 @@ /*=================================================================== 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. ===================================================================*/ // Blueberry #include #include // Qmitk #include "QmitkIGTTrackingLabView.h" #include "QmitkStdMultiWidget.h" #include #include #include #include #include #include - -#include #include #include #include #include #include -#include #include #include #include #include // Qt #include #include #include // vtk #include const std::string QmitkIGTTrackingLabView::VIEW_ID = "org.mitk.views.igttrackinglab"; QmitkIGTTrackingLabView::QmitkIGTTrackingLabView() : QmitkAbstractView() ,m_Source(NULL) ,m_PermanentRegistrationFilter(NULL) ,m_Visualizer(NULL) ,m_VirtualView(NULL) ,m_PSRecordingPointSet(NULL) ,m_PointSetRecording(false) ,m_PermanentRegistration(false) ,m_CameraView(false) ,m_ImageFiducialsDataNode(NULL) ,m_TrackerFiducialsDataNode(NULL) ,m_PermanentRegistrationSourcePoints(NULL) { } //############################################################################################### //############################################################################################### //############################## Timer method for IGT pipeline updating ######################### //############################################################################################### //############################################################################################### void QmitkIGTTrackingLabView::UpdateTimer() { if (m_PermanentRegistration && m_PermanentRegistrationFilter.IsNotNull()) { if(IsTransformDifferenceHigh(m_ObjectmarkerNavigationData, m_ObjectmarkerNavigationDataLastUpdate)) { m_ObjectmarkerNavigationDataLastUpdate->Graft(m_ObjectmarkerNavigationData); m_PermanentRegistrationFilter->Update(); } } if (m_CameraView && m_VirtualView.IsNotNull()) {m_VirtualView->Update();} if(m_PointSetRecording && m_PSRecordingPointSet.IsNotNull()) { int size = m_PSRecordingPointSet->GetSize(); mitk::NavigationData::Pointer nd = m_PointSetRecordingNavigationData; if(size > 0) { mitk::Point3D p = m_PSRecordingPointSet->GetPoint(size-1); if(p.EuclideanDistanceTo(nd->GetPosition()) > (double) m_Controls.m_PSRecordingSpinBox->value()) m_PSRecordingPointSet->InsertPoint(size, nd->GetPosition()); } else m_PSRecordingPointSet->InsertPoint(size, nd->GetPosition()); } } //############################################################################################### //############################################################################################### //############################## Slots of CONFIGURATION step #################################### //############################################################################################### //############################################################################################### void QmitkIGTTrackingLabView::OnSetupNavigation() { if(m_Source.IsNotNull()) if(m_Source->IsTracking()) return; mitk::DataStorage* ds = this->GetDataStorage(); if(ds == NULL) { MITK_WARN << "IGTSurfaceTracker: Error", "can not access DataStorage. Navigation not possible"; return; } //Building up the filter pipeline try { this->InitializeRegistration(); } catch(mitk::IGTException& e) { MITK_WARN << "Error while building the IGT-Pipeline: " << e.GetDescription(); this->DestroyIGTPipeline(); // destroy the pipeline if building is incomplete return; } catch(...) { MITK_WARN << "Unexpected error while building the IGT-Pipeline"; this->DestroyIGTPipeline(); return; } } void QmitkIGTTrackingLabView::OnInstrumentSelected() { if (m_Controls.m_TrackingDeviceSelectionWidget->GetSelectedNavigationDataSource().IsNotNull()) { m_InstrumentNavigationData = m_Controls.m_TrackingDeviceSelectionWidget->GetSelectedNavigationDataSource()->GetOutput(m_Controls.m_TrackingDeviceSelectionWidget->GetSelectedToolID()); } else { m_Controls.m_PointerNameLabel->setText(""); return; } if (m_InstrumentNavigationData.IsNotNull()) { m_Controls.m_PointerNameLabel->setText(m_InstrumentNavigationData->GetName()); } else { m_Controls.m_PointerNameLabel->setText(""); } } void QmitkIGTTrackingLabView::OnObjectmarkerSelected() { if (m_Controls.m_TrackingDeviceSelectionWidget->GetSelectedNavigationDataSource().IsNotNull()) { m_ObjectmarkerNavigationData = m_Controls.m_TrackingDeviceSelectionWidget->GetSelectedNavigationDataSource()->GetOutput(m_Controls.m_TrackingDeviceSelectionWidget->GetSelectedToolID()); MITK_INFO << "Objectmarker rotation: " << m_ObjectmarkerNavigationData->GetOrientation(); } else { m_Controls.m_ObjectmarkerNameLabel->setText(""); return; } if (m_ObjectmarkerNavigationData.IsNotNull()) { m_Controls.m_ObjectmarkerNameLabel->setText(m_ObjectmarkerNavigationData->GetName()); } else { m_Controls.m_ObjectmarkerNameLabel->setText(""); } } //############################################################################################### //############################################################################################### //####################### Slots of INITIAL REGISTRATION step #################################### //############################################################################################### //############################################################################################### void QmitkIGTTrackingLabView::OnInitialRegistration() { //Check for initialization if (!CheckRegistrationInitialization()) return; /* retrieve fiducials from data storage */ mitk::DataStorage* ds = this->GetDataStorage(); mitk::PointSet::Pointer imageFiducials = dynamic_cast(m_ImageFiducialsDataNode->GetData()); mitk::PointSet::Pointer trackerFiducials = dynamic_cast(m_TrackerFiducialsDataNode->GetData()); //############### conversion to vtk data types (we will use the vtk landmark based transform) ########################## //convert point sets to vtk poly data vtkSmartPointer sourcePoints = vtkSmartPointer::New(); vtkSmartPointer targetPoints = vtkSmartPointer::New(); for (int i=0; iGetSize(); i++) { double point[3] = {imageFiducials->GetPoint(i)[0],imageFiducials->GetPoint(i)[1],imageFiducials->GetPoint(i)[2]}; sourcePoints->InsertNextPoint(point); double point_targets[3] = {trackerFiducials->GetPoint(i)[0],trackerFiducials->GetPoint(i)[1],trackerFiducials->GetPoint(i)[2]}; targetPoints->InsertNextPoint(point_targets); } //########################### here, the actual transform is computed ########################## //compute transform vtkSmartPointer transform = vtkSmartPointer::New(); transform->SetSourceLandmarks(sourcePoints); transform->SetTargetLandmarks(targetPoints); transform->SetModeToRigidBody(); transform->Modified(); transform->Update(); //compute FRE of transform double FRE = mitk::StaticIGTHelperFunctions::ComputeFRE(imageFiducials, trackerFiducials, transform); m_Controls.m_RegistrationWidget->SetQualityDisplayText("FRE: " + QString::number(FRE) + " mm"); //############################################################################################# //############### conversion back to itk/mitk data types ########################## //convert from vtk to itk data types itk::Matrix rotationFloat = itk::Matrix(); itk::Vector translationFloat = itk::Vector(); itk::Matrix rotationDouble = itk::Matrix(); itk::Vector translationDouble = itk::Vector(); vtkSmartPointer m = transform->GetMatrix(); for(int k=0; k<3; k++) for(int l=0; l<3; l++) { rotationFloat[k][l] = m->GetElement(k,l); rotationDouble[k][l] = m->GetElement(k,l); } for(int k=0; k<3; k++) { translationFloat[k] = m->GetElement(k,3); translationDouble[k] = m->GetElement(k,3); } //create affine transform 3D surface mitk::AffineTransform3D::Pointer mitkTransform = mitk::AffineTransform3D::New(); mitkTransform->SetMatrix(rotationDouble); mitkTransform->SetOffset(translationDouble); //############################################################################################# //############### object is transformed ########################## //save transform m_T_ObjectReg = mitk::NavigationData::New(mitkTransform); // this is stored in a member because it is needed for permanent registration later on //transform surface if(m_Controls.m_SurfaceActive->isChecked() && m_Controls.m_ObjectComboBox->GetSelectedNode().IsNotNull()) { m_Controls.m_ObjectComboBox->GetSelectedNode()->GetData()->GetGeometry()->SetIndexToWorldTransform(mitkTransform); } //################################################################ //############### if activated: ct image is also transformed ########################## //transform ct image //todo: Erklären, dass hier AffineTransform3D verwendet wird, weil NavigationData kein Spacing unterstützt! if(m_Controls.m_ImageActive->isChecked() && m_Controls.m_ImageComboBox->GetSelectedNode().IsNotNull()) { //first we have to store the original ct image transform to compose it with the new transform later mitk::AffineTransform3D::Pointer imageTransform = m_Controls.m_ImageComboBox->GetSelectedNode()->GetData()->GetGeometry()->GetIndexToWorldTransform(); m_T_ImageGeo = mitk::AffineTransform3D::New(); // this is also stored in a member because it is needed for permanent registration later on //now the new transform of the ct image is computed m_T_ImageGeo->Compose(imageTransform); imageTransform->Compose(mitkTransform); mitk::AffineTransform3D::Pointer newImageTransform = mitk::AffineTransform3D::New(); //create new image transform... setting the composed directly leads to an error itk::Matrix rotationFloatNew = imageTransform->GetMatrix(); itk::Vector translationFloatNew = imageTransform->GetOffset(); newImageTransform->SetMatrix(rotationFloatNew); newImageTransform->SetOffset(translationFloatNew); m_Controls.m_ImageComboBox->GetSelectedNode()->GetData()->GetGeometry()->SetIndexToWorldTransform(newImageTransform); m_T_ImageReg = m_Controls.m_ImageComboBox->GetSelectedNode()->GetData()->GetGeometry()->GetIndexToWorldTransform(); } //################################################################ } void QmitkIGTTrackingLabView::OnAddRegistrationTrackingFiducial() { mitk::NavigationData::Pointer nd = m_InstrumentNavigationData; if( nd.IsNull() || !nd->IsDataValid()) { QMessageBox::warning( 0, "Invalid tracking data", "Navigation data is not available or invalid!", QMessageBox::Ok ); return; } if(m_TrackerFiducialsDataNode.IsNotNull() && m_TrackerFiducialsDataNode->GetData() != NULL) { mitk::PointSet::Pointer ps = dynamic_cast(m_TrackerFiducialsDataNode->GetData()); ps->InsertPoint(ps->GetSize(), nd->GetPosition()); } else QMessageBox::warning(NULL, "IGTSurfaceTracker: Error", "Can not access Tracker Fiducials. Adding fiducial not possible!"); } void QmitkIGTTrackingLabView::InitializeRegistration() { mitk::DataStorage* ds = this->GetDataStorage(); if( ds == NULL ) return; // let the registration widget know about the slice navigation controllers // in the active render window part (crosshair updates) foreach(QmitkRenderWindow* renderWindow, this->GetRenderWindowPart()->GetQmitkRenderWindows().values()) { m_Controls.m_RegistrationWidget->AddSliceNavigationController(renderWindow->GetSliceNavigationController()); } if(m_ImageFiducialsDataNode.IsNull()) { m_ImageFiducialsDataNode = mitk::DataNode::New(); mitk::PointSet::Pointer ifPS = mitk::PointSet::New(); m_ImageFiducialsDataNode->SetData(ifPS); mitk::Color color; color.Set(1.0f, 0.0f, 0.0f); m_ImageFiducialsDataNode->SetName("Image Fiducials"); m_ImageFiducialsDataNode->SetColor(color); m_ImageFiducialsDataNode->SetBoolProperty( "updateDataOnRender", false ); ds->Add(m_ImageFiducialsDataNode); } m_Controls.m_RegistrationWidget->SetImageFiducialsNode(m_ImageFiducialsDataNode); if(m_TrackerFiducialsDataNode.IsNull()) { m_TrackerFiducialsDataNode = mitk::DataNode::New(); mitk::PointSet::Pointer tfPS = mitk::PointSet::New(); m_TrackerFiducialsDataNode->SetData(tfPS); mitk::Color color; color.Set(0.0f, 1.0f, 0.0f); m_TrackerFiducialsDataNode->SetName("Tracking Fiducials"); m_TrackerFiducialsDataNode->SetColor(color); m_TrackerFiducialsDataNode->SetBoolProperty( "updateDataOnRender", false ); ds->Add(m_TrackerFiducialsDataNode); } m_Controls.m_RegistrationWidget->SetTrackerFiducialsNode(m_TrackerFiducialsDataNode); } //############################################################################################### //############################################################################################### //####################### Slots of PERMANENT REGISTRATION step ################################## //############################################################################################### //############################################################################################### void QmitkIGTTrackingLabView::OnPermanentRegistration(bool on) { if(on) { //###################################################################### //######################## inititalization ############################# //###################################################################### //some initial checks if(!CheckRegistrationInitialization()) { m_Controls.m_UsePermanentRegistrationToggle->setChecked(false); return; } //remember initial object transform to calculate the object to marker transform later on and convert it to navigation data mitk::AffineTransform3D::Pointer transform = this->m_Controls.m_ObjectComboBox->GetSelectedNode()->GetData()->GetGeometry()->GetIndexToWorldTransform(); mitk::NavigationData::Pointer T_Object = mitk::NavigationData::New(transform,false); //TODO: catch exception during conversion? //then reset the transform because we will now start to calculate the permanent registration this->m_Controls.m_ObjectComboBox->GetSelectedNode()->GetData()->GetGeometry()->SetIdentity(); if(m_Controls.m_ImageActive->isChecked()) {this->m_Controls.m_ImageComboBox->GetSelectedNode()->GetData()->GetGeometry()->SetIndexToWorldTransform(m_T_ImageGeo);} //create the permanent registration filter m_PermanentRegistrationFilter = mitk::NavigationDataObjectVisualizationFilter::New(); //###################################################################### //first: initialize permanent registration of surface (always activated) //###################################################################### //connect filter to source m_PermanentRegistrationFilter->SetInput(0,this->m_ObjectmarkerNavigationData); //set representation object m_PermanentRegistrationFilter->SetRepresentationObject(0,this->m_Controls.m_ObjectComboBox->GetSelectedNode()->GetData()); //get the marker transform out of the navigation data mitk::NavigationData::Pointer T_Marker = m_ObjectmarkerNavigationData; //compute transform from object to marker (T_MarkerRel = T_Object * T_Marker^-1) mitk::NavigationData::Pointer T_MarkerRel = mitk::NavigationData::New(); T_MarkerRel->Compose(T_Object); T_MarkerRel->Compose(T_Marker->GetInverse()); m_T_MarkerRel = T_MarkerRel; m_PermanentRegistrationFilter->SetOffset(0,m_T_MarkerRel->GetAffineTransform3D()); //###################################################################### //second: initialize permanent registration of image (if activated) //###################################################################### if (m_Controls.m_ImageActive->isChecked() && (m_Controls.m_ImageComboBox->GetSelectedNode().IsNotNull())) { mitk::DataNode::Pointer imageNode = this->m_Controls.m_ImageComboBox->GetSelectedNode(); imageNode->AddProperty( "reslice interpolation", mitk::VtkResliceInterpolationProperty::New(VTK_RESLICE_LINEAR) ); m_PermanentRegistrationFilter->SetInput(1,this->m_ObjectmarkerNavigationData); m_PermanentRegistrationFilter->SetRepresentationObject(1,imageNode->GetData()); //for the image we can't use NavigationData objects as transforms because an image needs additional geometry information, e.g., spacing //thus we use mitk::AffineTransform3D objects //computer transform from image to marker (T_ImageRel = T_ImageGeo * T_MarkerRel) mitk::AffineTransform3D::Pointer T_ImageRel = mitk::AffineTransform3D::New(); T_ImageRel->SetIdentity(); T_ImageRel->Compose(m_T_ImageGeo); T_ImageRel->Compose(m_T_MarkerRel->GetAffineTransform3D()); m_PermanentRegistrationFilter->SetOffset(1,T_ImageRel); } //some general stuff m_PermanentRegistration = true; m_ObjectmarkerNavigationDataLastUpdate = mitk::NavigationData::New(); } else //if off = disable the permanent registration { //stop permanent registration m_PermanentRegistration = false; //restore old registration if(m_T_ObjectReg.IsNotNull()) {this->m_Controls.m_ObjectComboBox->GetSelectedNode()->GetData()->GetGeometry()->SetIndexToWorldTransform(m_T_ObjectReg->GetAffineTransform3D());} if(m_T_ImageReg.IsNotNull()) {this->m_Controls.m_ImageComboBox->GetSelectedNode()->GetData()->GetGeometry()->SetIndexToWorldTransform(m_T_ImageReg);} //delete filter m_PermanentRegistrationFilter = NULL; } } //############################################################################################### //############################################################################################### //####################### Slots of POINT SET RECORDING step ##################################### //############################################################################################### //############################################################################################### void QmitkIGTTrackingLabView::OnPointSetRecording(bool record) { mitk::DataStorage* ds = this->GetDataStorage(); if(record) { if (m_Controls.m_PointSetRecordingToolSelectionWidget->GetSelectedToolID() == -1) { QMessageBox::warning(NULL, "Error", "No tool selected for point set recording!"); m_Controls.m_PointSetRecordCheckBox->setChecked(false); return; } m_PointSetRecordingNavigationData = m_Controls.m_PointSetRecordingToolSelectionWidget->GetSelectedNavigationDataSource()->GetOutput(m_Controls.m_PointSetRecordingToolSelectionWidget->GetSelectedToolID()); //initialize point set mitk::DataNode::Pointer psRecND = ds->GetNamedNode("Recorded Points"); if(m_PSRecordingPointSet.IsNull() || psRecND.IsNull()) { m_PSRecordingPointSet = NULL; m_PSRecordingPointSet = mitk::PointSet::New(); mitk::DataNode::Pointer dn = mitk::DataNode::New(); dn->SetName("Recorded Points"); dn->SetColor(0.,1.,0.); dn->SetData(m_PSRecordingPointSet); ds->Add(dn); } else { m_PSRecordingPointSet->Clear(); } m_PointSetRecording = true; } else { m_PointSetRecording = false; } } //############################################################################################### //############################################################################################### //####################### Slots of VIRTUAL CAMERA VIEW step ##################################### //############################################################################################### //############################################################################################### void QmitkIGTTrackingLabView::OnVirtualCamera(bool on) { if (m_Controls.m_CameraViewSelection->GetSelectedToolID() == -1) { m_Controls.m_ActivateNeedleView->setChecked(false); QMessageBox::warning(NULL, "Error", "No tool selected for camera view!"); return; } if(on) { m_VirtualView = mitk::CameraVisualization::New(); m_VirtualView->SetInput(m_Controls.m_CameraViewSelection->GetSelectedNavigationDataSource()->GetOutput(m_Controls.m_CameraViewSelection->GetSelectedToolID())); mitk::Vector3D viewDirection; viewDirection[0] = (int)(m_Controls.m_NeedleViewX->isChecked()); viewDirection[1] = (int)(m_Controls.m_NeedleViewY->isChecked()); viewDirection[2] = (int)(m_Controls.m_NeedleViewZ->isChecked()); if (m_Controls.m_NeedleViewInvert->isChecked()) viewDirection *= -1; m_VirtualView->SetDirectionOfProjectionInToolCoordinates(viewDirection); mitk::Vector3D viewUpVector; viewUpVector[0] = (int)(m_Controls.m_NeedleUpX->isChecked()); viewUpVector[1] = (int)(m_Controls.m_NeedleUpY->isChecked()); viewUpVector[2] = (int)(m_Controls.m_NeedleUpZ->isChecked()); if (m_Controls.m_NeedleUpInvert->isChecked()) viewUpVector *= -1; m_VirtualView->SetViewUpInToolCoordinates(viewUpVector); m_VirtualView->SetRenderer(this->GetRenderWindowPart()->GetQmitkRenderWindow("3d")->GetRenderer()); //next line: better code when this plugin is migrated to mitk::abstractview //m_VirtualView->SetRenderer(mitk::BaseRenderer::GetInstance(this->GetRenderWindowPart()->GetRenderWindow("3d")->GetRenderWindow())); m_CameraView = true; //make pointer itself invisible m_Controls.m_CameraViewSelection->GetSelectedNavigationTool()->GetDataNode()->SetBoolProperty("visible",false); //disable UI elements m_Controls.m_ViewDirectionBox->setEnabled(false); m_Controls.m_ViewUpBox->setEnabled(false); } else { m_VirtualView = NULL; m_CameraView = false; m_Controls.m_CameraViewSelection->GetSelectedNavigationTool()->GetDataNode()->SetBoolProperty("visible",true); m_Controls.m_ViewDirectionBox->setEnabled(true); m_Controls.m_ViewUpBox->setEnabled(true); } } //############################################################################################### //############################################################################################### //############################## some general UI methods, always needed ######################### //############################################################################################### //############################################################################################### QmitkIGTTrackingLabView::~QmitkIGTTrackingLabView() { if (m_Timer->isActive()) m_Timer->stop(); } void QmitkIGTTrackingLabView::CreateQtPartControl( QWidget *parent ) { // create GUI widgets from the Qt Designer's .ui file m_Controls.setupUi( parent ); this->CreateBundleWidgets( parent ); this->CreateConnections(); } void QmitkIGTTrackingLabView::CreateBundleWidgets( QWidget* parent ) { //initialize registration widget m_Controls.m_RegistrationWidget->HideStaticRegistrationRadioButton(true); m_Controls.m_RegistrationWidget->HideContinousRegistrationRadioButton(true); m_Controls.m_RegistrationWidget->HideUseICPRegistrationCheckbox(true); } void QmitkIGTTrackingLabView::CreateConnections() { //initialize timer m_Timer = new QTimer(this); //create connections connect(m_Timer, SIGNAL(timeout()), this, SLOT(UpdateTimer())); connect( m_Controls.m_UsePermanentRegistrationToggle, SIGNAL(toggled(bool)), this, SLOT(OnPermanentRegistration(bool)) ); connect( m_Controls.m_TrackingDeviceSelectionWidget, SIGNAL(NavigationDataSourceSelected(mitk::NavigationDataSource::Pointer)), this, SLOT(OnSetupNavigation()) ); connect( m_Controls.m_UseAsPointerButton, SIGNAL(clicked()), this, SLOT(OnInstrumentSelected()) ); connect( m_Controls.m_UseAsObjectmarkerButton, SIGNAL(clicked()), this, SLOT(OnObjectmarkerSelected()) ); connect( m_Controls.m_RegistrationWidget, SIGNAL(AddedTrackingFiducial()), this, SLOT(OnAddRegistrationTrackingFiducial()) ); connect( m_Controls.m_RegistrationWidget, SIGNAL(PerformFiducialRegistration()), this, SLOT(OnInitialRegistration()) ); connect( m_Controls.m_PointSetRecordCheckBox, SIGNAL(toggled(bool)), this, SLOT(OnPointSetRecording(bool)) ); connect( m_Controls.m_ActivateNeedleView, SIGNAL(toggled(bool)), this, SLOT(OnVirtualCamera(bool)) ); //start timer m_Timer->start(30); //initialize Combo Boxes m_Controls.m_ObjectComboBox->SetDataStorage(this->GetDataStorage()); m_Controls.m_ObjectComboBox->SetAutoSelectNewItems(false); m_Controls.m_ObjectComboBox->SetPredicate(mitk::NodePredicateDataType::New("Surface")); m_Controls.m_ImageComboBox->SetDataStorage(this->GetDataStorage()); m_Controls.m_ImageComboBox->SetAutoSelectNewItems(false); m_Controls.m_ImageComboBox->SetPredicate(mitk::NodePredicateDataType::New("Image")); } void QmitkIGTTrackingLabView::SetFocus() { m_Controls.m_UseAsPointerButton->setFocus(); } //############################################################################################### //############################################################################################### //####################### some additional slots and help methods ################################ //####################### for cleaner code - not that important ################################ //####################### to understand the basic functions ################################ //############################################################################################### //############################################################################################### void QmitkIGTTrackingLabView::DestroyIGTPipeline() { if(m_Source.IsNotNull()) { m_Source->StopTracking(); m_Source->Disconnect(); m_Source = NULL; } m_PermanentRegistrationFilter = NULL; m_Visualizer = NULL; m_VirtualView = NULL; } bool QmitkIGTTrackingLabView::CheckRegistrationInitialization() { // a couple of variables which we need in this method std::string warningMessage = ""; bool initializationErrorDetected = false; mitk::PointSet::Pointer imageFiducials,trackerFiducials; // check some initialization stuff if (m_ImageFiducialsDataNode.IsNull() || m_TrackerFiducialsDataNode.IsNull()) { warningMessage = "Initialization not finished!"; MITK_WARN << warningMessage; QMessageBox::warning(NULL, "Registration not possible", warningMessage.c_str()); return false; } else { imageFiducials = dynamic_cast(m_ImageFiducialsDataNode->GetData()); trackerFiducials = dynamic_cast(m_TrackerFiducialsDataNode->GetData()); } // now, do a lot of other checks... if (m_Controls.m_SurfaceActive->isChecked() && m_Controls.m_ObjectComboBox->GetSelectedNode().IsNull()) { warningMessage = "No surface selected for registration.\nRegistration is not possible"; initializationErrorDetected = true; } else if (m_Controls.m_ImageActive->isChecked() && m_Controls.m_ImageComboBox->GetSelectedNode().IsNull()) { warningMessage = "No image selected for registration.\nRegistration is not possible"; initializationErrorDetected = true; } else if (imageFiducials.IsNull() || trackerFiducials.IsNull()) { warningMessage = "Fiducial data objects not found. \n" "Please set 3 or more fiducials in the image and with the tracking system.\n\n" "Registration is not possible"; initializationErrorDetected = true; } else if ((imageFiducials->GetSize() < 3) || (trackerFiducials->GetSize() < 3) || (imageFiducials->GetSize() != trackerFiducials->GetSize())) { warningMessage = "Not enough fiducial pairs found. At least 3 fiducial must exist for the image and the tracking system respectively."; initializationErrorDetected = true; } // finaly: if an err was detected, give a warning and an error popup, then return false if(initializationErrorDetected) { MITK_WARN << warningMessage; QMessageBox::warning(NULL, "Registration not possible", warningMessage.c_str()); return false; } //if no error was detected simply return true else {return true;} } bool QmitkIGTTrackingLabView::IsTransformDifferenceHigh(mitk::NavigationData::Pointer transformA, mitk::NavigationData::Pointer transformB, double euclideanDistanceThreshold, double angularDifferenceThreshold) { if(transformA.IsNull() || transformA.IsNull()) {return false;} mitk::Point3D posA,posB; posA = transformA->GetPosition(); posB = transformB->GetPosition(); if(posA.EuclideanDistanceTo(posB) > euclideanDistanceThreshold) {return true;} double returnValue; mitk::Quaternion rotA,rotB; rotA = transformA->GetOrientation(); rotB = transformB->GetOrientation(); itk::Vector point; //caution 5D-Tools: Vector must lie in the YZ-plane for a correct result. point[0] = 0.0; point[1] = 0.0; point[2] = 100000.0; rotA.normalize(); rotB.normalize(); itk::Matrix rotMatrixA; for(int i=0; i<3; i++) for(int j=0; j<3; j++) rotMatrixA[i][j] = rotA.rotation_matrix_transpose().transpose()[i][j]; itk::Matrix rotMatrixB; for(int i=0; i<3; i++) for(int j=0; j<3; j++) rotMatrixB[i][j] = rotB.rotation_matrix_transpose().transpose()[i][j]; itk::Vector pt1 = rotMatrixA * point; itk::Vector pt2 = rotMatrixB * point; returnValue = (pt1[0]*pt2[0]+pt1[1]*pt2[1]+pt1[2]*pt2[2]) / ( sqrt(pow(pt1[0],2.0)+pow(pt1[1],2.0)+pow(pt1[2],2.0)) * sqrt(pow(pt2[0],2.0)+pow(pt2[1],2.0)+pow(pt2[2],2.0))); returnValue = acos(returnValue); if(returnValue*57.3 > angularDifferenceThreshold){return true;} return false; }