diff --git a/Modules/IGT/DataManagement/mitkNavigationTool.cpp b/Modules/IGT/DataManagement/mitkNavigationTool.cpp index 06fec32329..0ef596e00b 100644 --- a/Modules/IGT/DataManagement/mitkNavigationTool.cpp +++ b/Modules/IGT/DataManagement/mitkNavigationTool.cpp @@ -1,335 +1,354 @@ /*=================================================================== 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 "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_ToolLandmarks(mitk::PointSet::New()), m_ToolControlPoints(mitk::PointSet::New()), -m_ToolTipOrientation(mitk::Quaternion(0, 0, 0, 1)) +m_ToolAxisOrientation(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_ToolLandmarks.IsNotNull()) this->m_ToolLandmarks = other.m_ToolLandmarks->Clone(); if (other.m_ToolControlPoints.IsNotNull()) this->m_ToolControlPoints = other.m_ToolControlPoints->Clone(); this->m_ToolTipPosition = other.m_ToolTipPosition; - this->m_ToolTipOrientation = other.m_ToolTipOrientation; - this->m_ToolAxis = other.m_ToolAxis; + this->m_ToolAxisOrientation = other.m_ToolAxisOrientation; } mitk::NavigationTool::~NavigationTool() { } +mitk::Point3D mitk::NavigationTool::GetToolAxis() +{ + // The tool axis in the sensor coordinate system is defined as the negative z-axis + mitk::Vector3D toolAxisSensorCoordinateSystem; + mitk::FillVector3D(toolAxisSensorCoordinateSystem, 0.0, 0.0, -1.0); + // Apply inverse tool axis transform to calculate tool axis + vnl_vector_fixed toolAxisVector = m_ToolAxisOrientation.inverse().rotate(toolAxisSensorCoordinateSystem.GetVnlVector()); + // Transfer to mitk::Point3D + mitk::Point3D toolAxis; + toolAxis[0] = toolAxisVector[0]; + toolAxis[1] = toolAxisVector[1]; + toolAxis[2] = toolAxisVector[2]; + return toolAxis; +} + +void mitk::NavigationTool::SetToolAxis(mitk::Point3D toolAxis) +{ + // The tool axis in the sensor coordinate system is defined as the negative z-axis + mitk::Vector3D toolAxisSensorCoordinateSystem; + mitk::FillVector3D(toolAxisSensorCoordinateSystem, 0.0, 0.0, -1.0); + // Normalize the tool axis as obtained by a tool axis calibration + mitk::Vector3D toolAxisFromCalibration; + mitk::FillVector3D(toolAxisFromCalibration, toolAxis[0], toolAxis[1], toolAxis[2]); + toolAxisFromCalibration.Normalize(); + // Determine rotation angle + mitk::ScalarType rotationAngle = acos(toolAxisSensorCoordinateSystem*toolAxisFromCalibration); + // Determine rotation axis + mitk::Vector3D rotationAxis = itk::CrossProduct(toolAxisSensorCoordinateSystem, toolAxisFromCalibration); + // Calculate transform + itk::AffineTransform::Pointer sensorToToolAxisOrientation = itk::AffineTransform::New(); + sensorToToolAxisOrientation->Rotate3D(rotationAxis, rotationAngle); + // transfer to quaternion notation. Note that the vnl_quaternion expects the matrix in row major format, hence the transpose + mitk::Quaternion toolAxisTransform(sensorToToolAxisOrientation->GetMatrix().GetVnlMatrix().transpose()); + // Update the tool tip orientation + m_ToolAxisOrientation = toolAxisTransform; +} + mitk::AffineTransform3D::Pointer mitk::NavigationTool::GetToolTipTransform() { mitk::NavigationData::Pointer returnValue = mitk::NavigationData::New(); returnValue->SetPosition(this->m_ToolTipPosition); - returnValue->SetOrientation(this->m_ToolTipOrientation); + returnValue->SetOrientation(this->m_ToolAxisOrientation); 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_ToolLandmarks = nd->GetToolLandmarks(); m_ToolControlPoints = nd->GetToolControlPoints(); m_ToolTipPosition = nd->GetToolTipPosition(); - m_ToolTipOrientation = nd->GetToolTipOrientation(); - m_ToolAxis = nd->GetToolAxis(); + m_ToolAxisOrientation = nd->GetToolAxisOrientation(); } 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)) + (m_ToolAxisOrientation.x() == 0) && + (m_ToolAxisOrientation.y() == 0) && + (m_ToolAxisOrientation.z() == 0) && + (m_ToolAxisOrientation.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.; vtkSmartPointer vtkSphere = vtkSmartPointer::New(); vtkSmartPointer vtkCone = vtkSmartPointer::New(); vtkSmartPointer vtkCylinder = vtkSmartPointer::New(); vtkSmartPointer axis = vtkSmartPointer::New(); vtkSmartPointer vtkLine = vtkSmartPointer::New(); vtkSmartPointer vtkLine2 = vtkSmartPointer::New(); vtkSmartPointer vtkLine3 = vtkSmartPointer::New(); vtkSmartPointer appendPolyData = vtkSmartPointer::New(); vtkSmartPointer surface = vtkSmartPointer::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 vtkSmartPointer XTransform = vtkSmartPointer::New(); XTransform->RotateZ(-90); vtkSmartPointer TrafoFilter = vtkSmartPointer::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 vtkSmartPointer ZTransform = vtkSmartPointer::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 vtkSmartPointer ScaleTransform = vtkSmartPointer::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; + << " Tool Axis Orientation: " << m_ToolAxisOrientation << "\n" + << " Tool Axis: " << m_ToolAxisOrientation.inverse().rotate(vnl_vector_fixed(0.0,0.0,-1.0)) + ; return _info.str(); } diff --git a/Modules/IGT/DataManagement/mitkNavigationTool.h b/Modules/IGT/DataManagement/mitkNavigationTool.h index 6c203a97fb..7824ec38a2 100644 --- a/Modules/IGT/DataManagement/mitkNavigationTool.h +++ b/Modules/IGT/DataManagement/mitkNavigationTool.h @@ -1,211 +1,209 @@ /*=================================================================== 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 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); + itkGetConstMacro(ToolAxisOrientation,mitk::Quaternion); + itkSetMacro(ToolAxisOrientation,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 main tool axis which is defined as the z-coordinate of the tool coordinate system. */ + mitk::Point3D GetToolAxis(); + /** Convenience function to define the tool orientation given the main tool axis. As the main tool axis + is defined as the negative z-axis of the tool coordinate system, the tool orientation is calculated as + a rotation of the actual tool axis in tool coordinates as obtained by a calibration to the main axis.*/ + void SetToolAxis(mitk::Point3D toolAxis); /** @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: * * control points: 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 landmarks: These landmarks are designed for representing defined landmarks * on a tools surface. The number of these landmarks might exeed the number of tool control points * 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(ToolLandmarks,mitk::PointSet::Pointer); /** @brief Sets the tool landmarks which represent markers / special points on a * tool that can be used for registration. The landmarks should be given in tool coordinates. */ itkSetMacro(ToolLandmarks,mitk::PointSet::Pointer); /** @return Returns the tool control point in the tool coordinate system, e.g. 2 landmarks for a 5DoF * tool and 3 landmarks for a 6DoF tool. */ itkGetConstMacro(ToolControlPoints,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(ToolControlPoints,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 nullptr 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_ToolLandmarks; /** @brief Holds control 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_ToolControlPoints; /** @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; - //################################################# + /** @brief Holds the transformation of the main tool axis to the negative z-axis (0,0,-1) */ + mitk::Quaternion m_ToolAxisOrientation; }; } // namespace mitk #endif //NAVIGATIONTOOL diff --git a/Modules/IGT/Documentation/doxygen/TrackingTool.png b/Modules/IGT/Documentation/doxygen/TrackingTool.png new file mode 100644 index 0000000000..aabbacf8bd Binary files /dev/null and b/Modules/IGT/Documentation/doxygen/TrackingTool.png differ diff --git a/Modules/IGT/IO/mitkNavigationToolReader.cpp b/Modules/IGT/IO/mitkNavigationToolReader.cpp index 7ed333a0c8..ae1cc66807 100644 --- a/Modules/IGT/IO/mitkNavigationToolReader.cpp +++ b/Modules/IGT/IO/mitkNavigationToolReader.cpp @@ -1,272 +1,272 @@ /*=================================================================== 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. ===================================================================*/ //Poco headers #include #include //mitk headers #include "mitkNavigationToolReader.h" #include "mitkTrackingTypes.h" #include #include //All Tracking devices, which should be available by default #include "mitkNDIAuroraTypeInformation.h" #include "mitkNDIPolarisTypeInformation.h" #include "mitkVirtualTrackerTypeInformation.h" #include "mitkMicronTrackerTypeInformation.h" #include "mitkNPOptitrackTrackingTypeInformation.h" #include "mitkOpenIGTLinkTypeInformation.h" #include "mitkUnspecifiedTrackingTypeInformation.h" mitk::NavigationToolReader::NavigationToolReader() { m_ToolfilePath = mitk::IOUtil::GetTempPath() + Poco::Path::separator() + "IGT_Toolfiles" + Poco::Path::separator(); } mitk::NavigationToolReader::~NavigationToolReader() { } mitk::NavigationTool::Pointer mitk::NavigationToolReader::DoRead(std::string filename) { //decompress all files into a temporary directory std::ifstream file(filename.c_str(), std::ios::binary); if (!file.good()) { m_ErrorMessage = "Cannot open '" + filename + "' for reading"; return nullptr; } std::string tempDirectory = m_ToolfilePath + GetFileWithoutPath(filename); Poco::Zip::Decompress unzipper(file, Poco::Path(tempDirectory)); unzipper.decompressAllFiles(); //use SceneSerialization to load the DataStorage mitk::SceneIO::Pointer mySceneIO = mitk::SceneIO::New(); mitk::DataStorage::Pointer loadedStorage = mySceneIO->LoadScene(tempDirectory + Poco::Path::separator() + GetFileWithoutPath(filename) + ".storage"); if (loadedStorage->GetAll()->size() == 0 || loadedStorage.IsNull()) { m_ErrorMessage = "Invalid file: cannot parse tool data."; return nullptr; } //convert the DataStorage back to a NavigationTool-Object mitk::DataNode::Pointer myNode = loadedStorage->GetAll()->ElementAt(0); mitk::NavigationTool::Pointer returnValue = ConvertDataNodeToNavigationTool(myNode, tempDirectory); //delete the data-storage file which is not needed any more. The toolfile must be left in the temporary directory becauses it is linked in the datatreenode of the tool std::remove((std::string(tempDirectory + Poco::Path::separator() + GetFileWithoutPath(filename) + ".storage")).c_str()); return returnValue; } mitk::NavigationTool::Pointer mitk::NavigationToolReader::ConvertDataNodeToNavigationTool(mitk::DataNode::Pointer node, std::string toolPath) { mitk::NavigationTool::Pointer returnValue = mitk::NavigationTool::New(); //DateTreeNode with Name and Surface mitk::DataNode::Pointer newNode = mitk::DataNode::New(); newNode->SetName(node->GetName()); newNode->SetData(node->GetData()); bool visible = true; node->GetVisibility(visible, NULL); newNode->SetVisibility(visible); returnValue->SetDataNode(newNode); //Identifier std::string identifier; node->GetStringProperty("identifier", identifier); returnValue->SetIdentifier(identifier); //Serial Number std::string serial; node->GetStringProperty("serial number", serial); returnValue->SetSerialNumber(serial); //Tracking Device mitk::TrackingDeviceType device_type; node->GetStringProperty("tracking device type", device_type); //For backward compability with old tool stroages (before 12/2015 device_type was an int value, now it is string) if (device_type.size() == 0) { /* This was the old enum. Numbers inserted for better readibility. Don't delete this if-case to allow loading of ols storages... enum TrackingDeviceType { 0 NDIPolaris, ///< Polaris: optical Tracker from NDI 1 NDIAurora, ///< Aurora: electromagnetic Tracker from NDI 2 ClaronMicron, ///< Micron Tracker: optical Tracker from Claron 3 IntuitiveDaVinci, ///< Intuitive Surgical: DaVinci Telemanipulator API Interface 4 AscensionMicroBird, ///< Ascension microBird / PCIBird family 5 VirtualTracker, ///< Virtual Tracking device class that produces random tracking coordinates 6 TrackingSystemNotSpecified, ///< entry for not specified or initialized tracking system 7 TrackingSystemInvalid, ///< entry for invalid state (mainly for testing) 8 NPOptitrack, ///< NaturalPoint: Optitrack optical Tracking System 9 OpenIGTLinkTrackingDeviceConnection ///< Device which is connected via open igt link }; */ int device_type_old; node->GetIntProperty("tracking device type", device_type_old); switch (device_type_old) { case 0:device_type = mitk::NDIPolarisTypeInformation::GetTrackingDeviceName(); break; case 1:device_type = mitk::NDIAuroraTypeInformation::GetTrackingDeviceName(); break; case 2:device_type = mitk::MicronTrackerTypeInformation::GetTrackingDeviceName(); break; case 3:device_type = "IntuitiveDaVinci"; break; case 4:device_type = "AscensionMicroBird"; break; case 5:device_type = mitk::VirtualTrackerTypeInformation::GetTrackingDeviceName(); break; case 6:device_type = mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName(); break; case 7:device_type = "TrackingSystemInvalid"; break; case 8:device_type = mitk::NPOptitrackTrackingTypeInformation::GetTrackingDeviceName(); break; case 9:device_type = mitk::OpenIGTLinkTypeInformation::GetTrackingDeviceName(); break; default: device_type = mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName(); break; //default... unknown... } } returnValue->SetTrackingDeviceType(static_cast(device_type)); //Tool Type int type; node->GetIntProperty("tracking tool type", type); returnValue->SetType(static_cast(type)); //Calibration File Name std::string calibration_filename; node->GetStringProperty("toolfileName", calibration_filename); if (calibration_filename == "none") { returnValue->SetCalibrationFile("none"); } else { std::string calibration_filename_with_path = toolPath + Poco::Path::separator() + calibration_filename; returnValue->SetCalibrationFile(calibration_filename_with_path); } //Tool Landmarks mitk::PointSet::Pointer ToolRegLandmarks = mitk::PointSet::New(); mitk::PointSet::Pointer ToolCalLandmarks = mitk::PointSet::New(); std::string RegLandmarksString; std::string CalLandmarksString; node->GetStringProperty("ToolRegistrationLandmarks", RegLandmarksString); node->GetStringProperty("ToolCalibrationLandmarks", CalLandmarksString); ToolRegLandmarks = ConvertStringToPointSet(RegLandmarksString); ToolCalLandmarks = ConvertStringToPointSet(CalLandmarksString); returnValue->SetToolLandmarks(ToolRegLandmarks); returnValue->SetToolControlPoints(ToolCalLandmarks); //Tool Tip std::string toolTipPositionString; std::string toolTipOrientationString; bool positionSet = node->GetStringProperty("ToolTipPosition", toolTipPositionString); - bool orientationSet = node->GetStringProperty("ToolTipOrientation", toolTipOrientationString); + bool orientationSet = node->GetStringProperty("ToolAxisOrientation", toolTipOrientationString); if (positionSet && orientationSet) //only define tooltip if it is set { returnValue->SetToolTipPosition(ConvertStringToPoint(toolTipPositionString)); - returnValue->SetToolTipOrientation(ConvertStringToQuaternion(toolTipOrientationString)); + returnValue->SetToolAxisOrientation(ConvertStringToQuaternion(toolTipOrientationString)); } else if (positionSet != orientationSet) { MITK_WARN << "Tooltip definition incomplete: position and orientation have to be set! Skipping tooltip definition."; } //Tool Axis std::string ToolAxisString; node->GetStringProperty("ToolAxis", ToolAxisString); - returnValue->SetToolAxis(ConvertStringToPoint(ToolAxisString)); + //returnValue->SetToolAxis(ConvertStringToPoint(ToolAxisString)); return returnValue; } std::string mitk::NavigationToolReader::GetFileWithoutPath(std::string FileWithPath) { Poco::Path myFile(FileWithPath.c_str()); return myFile.getFileName(); } mitk::PointSet::Pointer mitk::NavigationToolReader::ConvertStringToPointSet(std::string string) { mitk::PointSet::Pointer returnValue = mitk::PointSet::New(); std::string pointSeperator = "|"; std::string valueSeperator = ";"; std::vector points; split(string, pointSeperator, points); for (unsigned int i = 0; i < points.size(); i++) { std::vector values; split(points.at(i), valueSeperator, values); if (values.size() == 4) { double index = atof(values.at(0).c_str()); mitk::Point3D point; point[0] = atof(values.at(1).c_str()); point[1] = atof(values.at(2).c_str()); point[2] = atof(values.at(3).c_str()); returnValue->SetPoint(index, point); } } return returnValue; } mitk::Point3D mitk::NavigationToolReader::ConvertStringToPoint(std::string string) { std::string valueSeperator = ";"; std::vector values; split(string, valueSeperator, values); mitk::Point3D point; if (values.size() == 3) { point[0] = atof(values.at(0).c_str()); point[1] = atof(values.at(1).c_str()); point[2] = atof(values.at(2).c_str()); } return point; } mitk::Quaternion mitk::NavigationToolReader::ConvertStringToQuaternion(std::string string) { std::string valueSeperator = ";"; std::vector values; split(string, valueSeperator, values); mitk::Quaternion quat = mitk::Quaternion(0, 0, 0, 1); if (values.size() == 4) { quat = mitk::Quaternion(atof(values.at(0).c_str()), atof(values.at(1).c_str()), atof(values.at(2).c_str()), atof(values.at(3).c_str())); } return quat; } void mitk::NavigationToolReader::split(std::string& text, std::string& separators, std::vector& words) { int n = text.length(); int start, stop; start = text.find_first_not_of(separators); while ((start >= 0) && (start < n)) { stop = text.find_first_of(separators, start); if ((stop < 0) || (stop > n)) stop = n; words.push_back(text.substr(start, stop - start)); start = text.find_first_not_of(separators, stop + 1); } } diff --git a/Modules/IGT/IO/mitkNavigationToolWriter.cpp b/Modules/IGT/IO/mitkNavigationToolWriter.cpp index 610aff5eda..43b5224b97 100644 --- a/Modules/IGT/IO/mitkNavigationToolWriter.cpp +++ b/Modules/IGT/IO/mitkNavigationToolWriter.cpp @@ -1,179 +1,179 @@ /*=================================================================== 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. ===================================================================*/ //Poco headers #include #include //mitk headers #include "mitkNavigationToolWriter.h" #include #include #include #include #include //std headers #include mitk::NavigationToolWriter::NavigationToolWriter() { } mitk::NavigationToolWriter::~NavigationToolWriter() { } bool mitk::NavigationToolWriter::DoWrite(std::string FileName,mitk::NavigationTool::Pointer Tool) { //some initial validation checks... if ( Tool.IsNull()) { m_ErrorMessage = "Cannot write a navigation tool containing invalid tool data, aborting!"; MITK_ERROR << m_ErrorMessage; return false; } // Workaround for a problem: the geometry might be modified if the tool is tracked. If this // modified geometry is saved the surface representation is moved by this offset. To avoid // this bug, the geometry is set to identity for the saving progress and restored later. mitk::BaseGeometry::Pointer geometryBackup; if ( Tool->GetDataNode().IsNotNull() && (Tool->GetDataNode()->GetData()!=nullptr) && (Tool->GetDataNode()->GetData()->GetGeometry()!=nullptr) ) { geometryBackup = Tool->GetDataNode()->GetData()->GetGeometry()->Clone(); Tool->GetDataNode()->GetData()->GetGeometry()->SetIdentity(); } else {MITK_WARN << "Saving a tool with invalid data node, proceeding but errors might occure!";} //convert whole data to a mitk::DataStorage mitk::StandaloneDataStorage::Pointer saveStorage = mitk::StandaloneDataStorage::New(); mitk::DataNode::Pointer thisTool = ConvertToDataNode(Tool); saveStorage->Add(thisTool); //use SceneSerialization to save the DataStorage std::string DataStorageFileName = mitk::IOUtil::CreateTemporaryDirectory() + Poco::Path::separator() + GetFileWithoutPath(FileName) + ".storage"; mitk::SceneIO::Pointer mySceneIO = mitk::SceneIO::New(); mySceneIO->SaveScene(saveStorage->GetAll(),saveStorage,DataStorageFileName); //now put the DataStorage and the Toolfile in a ZIP-file std::ofstream file( FileName.c_str(), std::ios::binary | std::ios::out); if (!file.good()) { m_ErrorMessage = "Could not open a zip file for writing: '" + FileName + "'"; MITK_ERROR << m_ErrorMessage; return false; } else { Poco::Zip::Compress zipper( file, true ); zipper.addFile(DataStorageFileName,GetFileWithoutPath(DataStorageFileName)); if (Tool->GetCalibrationFile()!="none") zipper.addFile(Tool->GetCalibrationFile(),GetFileWithoutPath(Tool->GetCalibrationFile())); zipper.close(); } //delete the data storage std::remove(DataStorageFileName.c_str()); //restore original geometry if (geometryBackup.IsNotNull()) {Tool->GetDataNode()->GetData()->SetGeometry(geometryBackup);} return true; } mitk::DataNode::Pointer mitk::NavigationToolWriter::ConvertToDataNode(mitk::NavigationTool::Pointer Tool) { mitk::DataNode::Pointer thisTool = mitk::DataNode::New(); //Name if (Tool->GetDataNode().IsNull()) thisTool->SetName("none"); else thisTool->SetName(Tool->GetDataNode()->GetName().c_str()); //Identifier thisTool->AddProperty("identifier",mitk::StringProperty::New(Tool->GetIdentifier().c_str())); //Serial Number thisTool->AddProperty("serial number",mitk::StringProperty::New(Tool->GetSerialNumber().c_str())); //Tracking Device thisTool->AddProperty("tracking device type",mitk::StringProperty::New(Tool->GetTrackingDeviceType())); //Tool Type thisTool->AddProperty("tracking tool type",mitk::IntProperty::New(Tool->GetType())); //Calibration File Name thisTool->AddProperty("toolfileName",mitk::StringProperty::New(GetFileWithoutPath(Tool->GetCalibrationFile()))); //Surface if (Tool->GetDataNode().IsNotNull()) if (Tool->GetDataNode()->GetData() != NULL) { thisTool->SetData(Tool->GetDataNode()->GetData()); //Visibility bool visible = true; Tool->GetDataNode()->GetVisibility(visible, NULL); thisTool->SetVisibility(visible); } //Tool Landmarks thisTool->AddProperty("ToolRegistrationLandmarks",mitk::StringProperty::New(ConvertPointSetToString(Tool->GetToolLandmarks()))); thisTool->AddProperty("ToolCalibrationLandmarks",mitk::StringProperty::New(ConvertPointSetToString(Tool->GetToolControlPoints()))); //Tool Tip if (Tool->IsToolTipSet()) { thisTool->AddProperty("ToolTipPosition",mitk::StringProperty::New(ConvertPointToString(Tool->GetToolTipPosition()))); - thisTool->AddProperty("ToolTipOrientation",mitk::StringProperty::New(ConvertQuaternionToString(Tool->GetToolTipOrientation()))); + thisTool->AddProperty("ToolAxisOrientation",mitk::StringProperty::New(ConvertQuaternionToString(Tool->GetToolAxisOrientation()))); } //Tool Axis thisTool->AddProperty("ToolAxis", mitk::StringProperty::New(ConvertPointToString(Tool->GetToolAxis()))); - //Material is not needed, to avoid errors in scene serialization we have to do this: + ////Material is not needed, to avoid errors in scene serialization we have to do this: thisTool->ReplaceProperty("material",nullptr); return thisTool; } std::string mitk::NavigationToolWriter::GetFileWithoutPath(std::string FileWithPath) { Poco::Path myFile(FileWithPath.c_str()); return myFile.getFileName(); } std::string mitk::NavigationToolWriter::ConvertPointSetToString(mitk::PointSet::Pointer pointSet) { std::stringstream returnValue; mitk::PointSet::PointDataIterator it; for ( it = pointSet->GetPointSet()->GetPointData()->Begin();it != pointSet->GetPointSet()->GetPointData()->End();it++ ) { mitk::Point3D thisPoint = pointSet->GetPoint(it->Index()); returnValue << it->Index() << ";" << ConvertPointToString(thisPoint) << "|"; } return returnValue.str(); } std::string mitk::NavigationToolWriter::ConvertPointToString(mitk::Point3D point) { std::stringstream returnValue; returnValue << point[0] << ";" << point[1] << ";" << point[2]; return returnValue.str(); } std::string mitk::NavigationToolWriter::ConvertQuaternionToString(mitk::Quaternion quat) { std::stringstream returnValue; returnValue << quat.x() << ";" << quat.y() << ";" << quat.z() << ";" << quat.r(); return returnValue.str(); } diff --git a/Modules/IGT/Testing/mitkNavigationToolStorageSerializerAndDeserializerIntegrationTest.cpp b/Modules/IGT/Testing/mitkNavigationToolStorageSerializerAndDeserializerIntegrationTest.cpp index fe16ae88f9..cfdca9a993 100644 --- a/Modules/IGT/Testing/mitkNavigationToolStorageSerializerAndDeserializerIntegrationTest.cpp +++ b/Modules/IGT/Testing/mitkNavigationToolStorageSerializerAndDeserializerIntegrationTest.cpp @@ -1,139 +1,139 @@ /*=================================================================== 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. ===================================================================*/ //testing headers #include #include #include #include //some general mitk headers #include #include #include //headers of IGT classes releated to the tested class #include #include #include #include #include #include //POCO headers for file handling #include #include #include class mitkNavigationToolStorageSerializerAndDeserializerIntegrationTestSuite : public mitk::TestFixture { CPPUNIT_TEST_SUITE(mitkNavigationToolStorageSerializerAndDeserializerIntegrationTestSuite); MITK_TEST(TestInstantiationSerializer); MITK_TEST(TestInstantiationDeserializer); MITK_TEST(TestWriteAndReadSimpleToolStorageWithToolLandmarks); CPPUNIT_TEST_SUITE_END(); private: /** Members used inside the different test methods. All members are initialized via setUp().*/ mitk::NavigationToolStorageSerializer::Pointer m_Serializer; mitk::NavigationToolStorageDeserializer::Pointer m_Deserializer; std::string m_FileName1; public: /**@brief Setup Always call this method before each Test-case to ensure correct and new intialization of the used members for a new test case. (If the members are not used in a test, the method does not need to be called).*/ void setUp() override { try { m_FileName1 = mitk::IOUtil::CreateTemporaryFile(); std::ofstream file; file.open(m_FileName1.c_str()); if (!file.good()) {MITK_ERROR <<"Could not create a valid file during setUp() method.";} file.close(); } catch (std::exception& e) { MITK_ERROR << "File access Exception: " << e.what(); MITK_ERROR <<"Could not create filename during setUp() method."; } m_Serializer = mitk::NavigationToolStorageSerializer::New(); mitk::DataStorage::Pointer DataStorage = dynamic_cast(mitk::StandaloneDataStorage::New().GetPointer()); //needed for deserializer! m_Deserializer = mitk::NavigationToolStorageDeserializer::New(DataStorage); } void tearDown() override { m_Serializer = nullptr; m_Deserializer = nullptr; try { std::remove(m_FileName1.c_str()); } catch(...) { MITK_ERROR << "Warning: Error occured when deleting test file!"; } } void TestInstantiationSerializer() { // let's create objects of our classes mitk::NavigationToolStorageSerializer::Pointer testSerializer = mitk::NavigationToolStorageSerializer::New(); CPPUNIT_ASSERT_MESSAGE("Testing instantiation of NavigationToolStorageSerializer",testSerializer.IsNotNull()); } void TestInstantiationDeserializer() { mitk::DataStorage::Pointer tempStorage = dynamic_cast(mitk::StandaloneDataStorage::New().GetPointer()); //needed for deserializer! mitk::NavigationToolStorageDeserializer::Pointer testDeserializer = mitk::NavigationToolStorageDeserializer::New(tempStorage); CPPUNIT_ASSERT_MESSAGE("Testing instantiation of NavigationToolStorageDeserializer",testDeserializer.IsNotNull()); } void TestWriteAndReadSimpleToolStorageWithToolLandmarks() { //create Tool Storage mitk::NavigationToolStorage::Pointer storage = mitk::NavigationToolStorageTestHelper::CreateTestData_StorageWithOneTool(); //test serialization bool success = m_Serializer->Serialize(m_FileName1,storage); CPPUNIT_ASSERT_MESSAGE("Testing serialization of tool storage with tool registrations",success); //test deserialization of the same file mitk::NavigationToolStorage::Pointer readStorage = m_Deserializer->Deserialize(m_FileName1); CPPUNIT_ASSERT_MESSAGE("Testing deserialization of tool storage with tool registrations",readStorage.IsNotNull()); CPPUNIT_ASSERT_MESSAGE(" ..Testing number of tools in storage",readStorage->GetToolCount()==1); mitk::PointSet::Pointer readRegLandmarks = readStorage->GetTool(0)->GetToolLandmarks(); mitk::PointSet::Pointer readCalLandmarks = readStorage->GetTool(0)->GetToolControlPoints(); CPPUNIT_ASSERT_MESSAGE("..Testing if tool registration landmarks have been stored and loaded correctly.",((readRegLandmarks->GetPoint(5)[0] == 4)&&(readRegLandmarks->GetPoint(5)[1] == 5)&&(readRegLandmarks->GetPoint(5)[2] == 6))); CPPUNIT_ASSERT_MESSAGE("..Testing if tool calibration landmarks have been stored and loaded correctly.",((readCalLandmarks->GetPoint(0)[0] == 1)&&(readCalLandmarks->GetPoint(0)[1] == 2)&&(readCalLandmarks->GetPoint(0)[2] == 3))); mitk::Point3D readToolTipPos = readStorage->GetTool(0)->GetToolTipPosition(); - mitk::Quaternion readToolTipRot = readStorage->GetTool(0)->GetToolTipOrientation(); + mitk::Quaternion readToolTipRot = readStorage->GetTool(0)->GetToolAxisOrientation(); CPPUNIT_ASSERT_MESSAGE("..Testing if tool tip position has been stored and loaded correctly.", ((float(readToolTipPos[0]) == float(1.3423))&& (float(readToolTipPos[1]) == float(2.323))&& (float(readToolTipPos[2]) == float(4.332)))); CPPUNIT_ASSERT_MESSAGE("..Testing if tool tip orientation has been stored and loaded correctly.", ((float(readToolTipRot.x()) == float(0.1))&& (float(readToolTipRot.y()) == float(0.2))&& (float(readToolTipRot.z()) == float(0.3))&& (float(readToolTipRot.r()) == float(0.4)))); } }; MITK_TEST_SUITE_REGISTRATION(mitkNavigationToolStorageSerializerAndDeserializerIntegration) diff --git a/Modules/IGT/Testing/mitkNavigationToolTest.cpp b/Modules/IGT/Testing/mitkNavigationToolTest.cpp index 58f1f72020..361426b542 100644 --- a/Modules/IGT/Testing/mitkNavigationToolTest.cpp +++ b/Modules/IGT/Testing/mitkNavigationToolTest.cpp @@ -1,96 +1,124 @@ /*=================================================================== 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 "mitkCommon.h" #include "mitkTestingMacros.h" #include "mitkDataNode.h" #include "mitkPointSet.h" #include "mitkTrackingTool.h" #include #include "mitkNDIAuroraTypeInformation.h" class mitkNavigationToolTestClass { public: + static bool CompareQuaternions(mitk::Quaternion q1, mitk::Quaternion q2) + { + if ((q1.as_vector() - q2.as_vector()).one_norm() < 10e-3) + return true; + else + return false; + } static void TestInstantiation() { // let's create an object of our class mitk::NavigationTool::Pointer myNavigationTool = mitk::NavigationTool::New(); MITK_TEST_CONDITION_REQUIRED(myNavigationTool.IsNotNull(),"Testing instantiation") } static void TestGetterAndSetter() { mitk::NavigationTool::Pointer myNavigationTool = mitk::NavigationTool::New(); //initialize a few things mitk::DataNode::Pointer myNode = mitk::DataNode::New(); myNode->SetName("TestNodeName"); itk::SpatialObject<3>::Pointer mySpatialObject = itk::SpatialObject<3>::New(); //set everything myNavigationTool->SetType(mitk::NavigationTool::Instrument); myNavigationTool->SetIdentifier("Tool#15"); myNavigationTool->SetDataNode(myNode); myNavigationTool->SetSpatialObject(mySpatialObject); //notice: cannot test Get/SetTrackingTool because this class cannot be instantiated alone myNavigationTool->SetCalibrationFile("Test.srom"); myNavigationTool->SetSerialNumber("0815"); myNavigationTool->SetTrackingDeviceType(mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()); mitk::PointSet::Pointer CalLandmarks = mitk::PointSet::New(); mitk::Point3D testPt1; mitk::FillVector3D(testPt1,1,2,3); CalLandmarks->SetPoint(0,testPt1); mitk::PointSet::Pointer RegLandmarks = mitk::PointSet::New(); mitk::Point3D testPt2; mitk::FillVector3D(testPt2,4,5,6); RegLandmarks->SetPoint(0,testPt2); myNavigationTool->SetToolControlPoints(CalLandmarks); myNavigationTool->SetToolLandmarks(RegLandmarks); //test getter MITK_TEST_CONDITION(myNavigationTool->GetType()==mitk::NavigationTool::Instrument,"Testing getter and setter of type."); MITK_TEST_CONDITION(myNavigationTool->GetIdentifier()=="Tool#15","Testing getter and setter of identifier."); MITK_TEST_CONDITION(myNavigationTool->GetDataNode()==myNode,"Testing getter and setter of dataNode."); MITK_TEST_CONDITION(myNavigationTool->GetSpatialObject()==mySpatialObject,"Testing getter and setter of itk spatial object."); MITK_TEST_CONDITION(myNavigationTool->GetCalibrationFile()=="none","Testing getter and setter of calibration file."); //should be none, because file does not exist MITK_TEST_CONDITION(myNavigationTool->GetSerialNumber()=="0815","Testing getter and setter of serial number."); MITK_TEST_CONDITION(myNavigationTool->GetTrackingDeviceType() == mitk::NDIAuroraTypeInformation::GetTrackingDeviceName(), "Testing getter and setter of tracking device type."); MITK_TEST_CONDITION(myNavigationTool->GetToolName()=="TestNodeName","Testing method GetToolName()."); MITK_TEST_CONDITION(myNavigationTool->GetToolControlPoints()->GetPoint(0)[0] == 1.0,"Testing method GetToolControlPoints()"); MITK_TEST_CONDITION(myNavigationTool->GetToolLandmarks()->GetPoint(0)[0] == 4.0,"Testing method GetToolLandmarks()"); } + static void TestToolTipAndAxis() + { + // let's create an object of our class + mitk::NavigationTool::Pointer myNavigationTool = mitk::NavigationTool::New(); + MITK_TEST_CONDITION_REQUIRED(myNavigationTool.IsNotNull(), "Testing instantiation"); + // define tool tip + mitk::Point3D toolTip; + mitk::FillVector3D(toolTip, 1.0, 3.4, 5.6); + // test Set / GetToolTipPosition + myNavigationTool->SetToolTipPosition(toolTip); + MITK_TEST_CONDITION(myNavigationTool->GetToolTipPosition() == toolTip, "Testing Set / GetToolTipPosition"); + // define tool axis + mitk::Point3D toolAxis; + mitk::FillVector3D(toolAxis, 1.0, 0.0, 0.0); + // This tool axis will result in the following transformation + mitk::Quaternion resultingToolAxisOrientation(0, -sqrt(2)/2.0, 0, sqrt(2)/2.0); + // test calculation of ToolAxisOrientation + myNavigationTool->SetToolAxis(toolAxis); + MITK_TEST_CONDITION(CompareQuaternions(myNavigationTool->GetToolAxisOrientation(),resultingToolAxisOrientation), "Testing caluclation of ToolAxisOrientation"); + } + }; /** This function is testing the TrackingVolume class. */ int mitkNavigationToolTest(int /* argc */, char* /*argv*/[]) { MITK_TEST_BEGIN("NavigationTool") mitkNavigationToolTestClass::TestInstantiation(); mitkNavigationToolTestClass::TestGetterAndSetter(); - + mitkNavigationToolTestClass::TestToolTipAndAxis(); MITK_TEST_END() } diff --git a/Modules/IGT/Testing/mitkPolhemusTrackingDeviceHardwareTest.cpp b/Modules/IGT/Testing/mitkPolhemusTrackingDeviceHardwareTest.cpp index 5538870444..1a8328106c 100644 --- a/Modules/IGT/Testing/mitkPolhemusTrackingDeviceHardwareTest.cpp +++ b/Modules/IGT/Testing/mitkPolhemusTrackingDeviceHardwareTest.cpp @@ -1,65 +1,65 @@ /*=================================================================== 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 "mitkPolhemusInterface.h" #include "mitkTestingMacros.h" #include "mitkStandardFileLocations.h" #include // Testing #include "mitkTestingMacros.h" #include "mitkTestFixture.h" class mitkPolhemusTrackingDeviceHardwareTestSuite : public mitk::TestFixture { CPPUNIT_TEST_SUITE(mitkPolhemusTrackingDeviceHardwareTestSuite); // Test the append method MITK_TEST(testInterface); CPPUNIT_TEST_SUITE_END(); public: void setUp() override { } void tearDown() override { } void testInterface() { mitk::PolhemusInterface::Pointer myInterface = mitk::PolhemusInterface::New(); CPPUNIT_ASSERT_MESSAGE("Testing connection.", myInterface->Connect()); CPPUNIT_ASSERT_MESSAGE("Start tracking.", myInterface->StartTracking()); CPPUNIT_ASSERT_MESSAGE("Tracking 20 frames ...", true); for (int i = 0; i < 20; i++) { std::vector lastFrame = myInterface->GetLastFrame(); MITK_INFO << "Frame " << i; - for (int j = 0; j < lastFrame.size(); j++) + for (size_t j = 0; j < lastFrame.size(); j++) { MITK_INFO << "[" << j << "]" << " Pos:" << lastFrame.at(j).pos << " Rot:" << lastFrame.at(j).rot; } } } }; MITK_TEST_SUITE_REGISTRATION(mitkPolhemusTrackingDeviceHardware) diff --git a/Modules/IGT/Testing/mitkTrackingToolTest.cpp b/Modules/IGT/Testing/mitkTrackingToolTest.cpp index 865048f6f2..2ac84e90d3 100644 --- a/Modules/IGT/Testing/mitkTrackingToolTest.cpp +++ b/Modules/IGT/Testing/mitkTrackingToolTest.cpp @@ -1,275 +1,275 @@ /*=================================================================== 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. ===================================================================*/ // Testing #include "mitkTestFixture.h" #include "mitkTestingMacros.h" #include "mitkTrackingTool.h" // VTK includes #include class mitkTrackingToolTestSuite : public mitk::TestFixture { CPPUNIT_TEST_SUITE(mitkTrackingToolTestSuite); MITK_TEST(GetToolName_Default_ReturnsEmptyString); MITK_TEST(GetErrorMessage_Default_ReturnsEmptyString); MITK_TEST(IsEnabled_Default_ReturnsTrue); MITK_TEST(IsEnabled_Enable_ReturnsTrue); MITK_TEST(IsEnabled_Disable_ReturnsFalse); MITK_TEST(GetPosition_Default_ReturnsAllValuesZero); MITK_TEST(GetPosition_SetValidPosition_ReturnsValidPosition); MITK_TEST(GetOrientation_Default_ReturnsAllValuesZero); MITK_TEST(GetOrientation_SetValidOrientation_ReturnsValidOrientation); MITK_TEST(GetOrientation_SetToolTip_ReturnsTransformedOrientation); MITK_TEST(GetTrackingError_Default_ReturnsZero); MITK_TEST(GetTrackingError_SetValidTrackingError_ReturnsValidTrackingError); MITK_TEST(IsDataValid_Default_ReturnsFalse); MITK_TEST(IsDataValid_SetTrue_ReturnsTrue); MITK_TEST(IsDataValid_SetFalse_ReturnsFalse); MITK_TEST(GetToolTipPosition_Default_ReturnsAllValuesZero); MITK_TEST(GetToolTipOrientation_Default_ReturnsDefaultOrientation); MITK_TEST(IsToolTipSet_Default_ReturnsFalse); MITK_TEST(GetToolTipX_SetValidToolTip_ReturnsValidToolTip); MITK_TEST(GetMTime_SetPosition_ReturnsModifiedTime); MITK_TEST(GetMTime_SetOrientation_ReturnsModifiedTime); CPPUNIT_TEST_SUITE_END(); private: mitk::TrackingTool::Pointer m_TrackingTool; public: void setUp() override { m_TrackingTool = mitk::TrackingTool::New(); } void tearDown() override { m_TrackingTool = nullptr; } void GetToolName_Default_ReturnsEmptyString() { CPPUNIT_ASSERT_MESSAGE("Tool name should be empty", !strcmp(m_TrackingTool->GetToolName(), "")); } void GetErrorMessage_Default_ReturnsEmptyString() { CPPUNIT_ASSERT_MESSAGE("Error message should be empty", !strcmp(m_TrackingTool->GetErrorMessage(), "")); } void IsEnabled_Default_ReturnsTrue() { CPPUNIT_ASSERT_EQUAL(true, m_TrackingTool->IsEnabled()); } void IsEnabled_Enable_ReturnsTrue() { m_TrackingTool->Enable(); CPPUNIT_ASSERT_EQUAL(true, m_TrackingTool->IsEnabled()); } void IsEnabled_Disable_ReturnsFalse() { m_TrackingTool->Disable(); CPPUNIT_ASSERT_EQUAL(false, m_TrackingTool->IsEnabled()); } void GetPosition_Default_ReturnsAllValuesZero() { mitk::Point3D expectedPosition; expectedPosition.Fill(0); mitk::Point3D actualPosition; m_TrackingTool->GetPosition(actualPosition); CPPUNIT_ASSERT_EQUAL(expectedPosition, actualPosition); } void GetPosition_SetValidPosition_ReturnsValidPosition() { mitk::Point3D expectedPosition; expectedPosition[0] = 100; expectedPosition[1] = 200; expectedPosition[2] = 300; m_TrackingTool->SetPosition(expectedPosition); mitk::Point3D actualPosition; m_TrackingTool->GetPosition(actualPosition); CPPUNIT_ASSERT_EQUAL(expectedPosition, actualPosition); } void GetOrientation_Default_ReturnsAllValuesZero() { mitk::Quaternion expectedOrientation(0, 0, 0, 0); mitk::Quaternion actualOrientation; m_TrackingTool->GetOrientation(actualOrientation); CPPUNIT_ASSERT_EQUAL(expectedOrientation, actualOrientation); } void GetOrientation_SetValidOrientation_ReturnsValidOrientation() { mitk::Quaternion expectedOrientation(0.344, 0.625, 0.999, 0.574); m_TrackingTool->SetOrientation(expectedOrientation); mitk::Quaternion actualOrientation; m_TrackingTool->GetOrientation(actualOrientation); CPPUNIT_ASSERT_EQUAL(expectedOrientation, actualOrientation); } void GetOrientation_SetToolTip_ReturnsTransformedOrientation() { mitk::Point3D toolTipPosition; mitk::FillVector3D(toolTipPosition, 1, 1, 1); mitk::Quaternion toolTipOrientation = mitk::Quaternion(0.5, 0, 0, 1); - m_TrackingTool->SetToolTip(toolTipPosition, toolTipOrientation); + m_TrackingTool->SetToolTipPosition(toolTipPosition, toolTipOrientation); mitk::Point3D toolPosition; mitk::FillVector3D(toolPosition, 5, 6, 7); mitk::Quaternion toolOrientation = mitk::Quaternion(0, 0.5, 0, 1); m_TrackingTool->SetPosition(toolPosition); m_TrackingTool->SetOrientation(toolOrientation); mitk::Quaternion expectedToolOrientation = mitk::Quaternion(0.5, 0.5, -0.25, 1); mitk::Quaternion actualToolOrientation; m_TrackingTool->GetOrientation(actualToolOrientation); CPPUNIT_ASSERT_EQUAL(expectedToolOrientation, actualToolOrientation); } void GetTrackingError_Default_ReturnsZero() { CPPUNIT_ASSERT_DOUBLES_EQUAL(0.0f, m_TrackingTool->GetTrackingError(), mitk::eps); } void GetTrackingError_SetValidTrackingError_ReturnsValidTrackingError() { m_TrackingTool->SetTrackingError(0.2f); CPPUNIT_ASSERT_DOUBLES_EQUAL(0.2f, m_TrackingTool->GetTrackingError(), mitk::eps); } void IsDataValid_Default_ReturnsFalse() { CPPUNIT_ASSERT_EQUAL(false, m_TrackingTool->IsDataValid()); } void IsDataValid_SetTrue_ReturnsTrue() { m_TrackingTool->SetDataValid(true); CPPUNIT_ASSERT_EQUAL(true, m_TrackingTool->IsDataValid()); } void IsDataValid_SetFalse_ReturnsFalse() { m_TrackingTool->SetDataValid(false); CPPUNIT_ASSERT_EQUAL(false, m_TrackingTool->IsDataValid()); } void GetToolTipPosition_Default_ReturnsAllValuesZero() { mitk::Point3D expectedPosition; expectedPosition.Fill(0); CPPUNIT_ASSERT_EQUAL(expectedPosition, m_TrackingTool->GetToolTipPosition()); } void GetToolTipOrientation_Default_ReturnsDefaultOrientation() { mitk::Quaternion expectedOrientation(0, 0, 0, 1); - CPPUNIT_ASSERT_EQUAL(expectedOrientation, m_TrackingTool->GetToolTipOrientation()); + CPPUNIT_ASSERT_EQUAL(expectedOrientation, m_TrackingTool->GetToolAxisOrientation()); } void IsToolTipSet_Default_ReturnsFalse() { CPPUNIT_ASSERT_EQUAL(false, m_TrackingTool->IsToolTipSet()); } void GetToolTipX_SetValidToolTip_ReturnsValidToolTip() { mitk::Point3D expectedPosition; expectedPosition[0] = 100; expectedPosition[1] = 200; expectedPosition[2] = 300; mitk::Quaternion expectedOrientation(0.344, 0.625, 0.999, 0.574); - m_TrackingTool->SetToolTip(expectedPosition, expectedOrientation); + m_TrackingTool->SetToolTipPosition(expectedPosition, expectedOrientation); CPPUNIT_ASSERT_EQUAL(expectedPosition, m_TrackingTool->GetToolTipPosition()); - CPPUNIT_ASSERT_EQUAL(expectedOrientation, m_TrackingTool->GetToolTipOrientation()); + CPPUNIT_ASSERT_EQUAL(expectedOrientation, m_TrackingTool->GetToolAxisOrientation()); CPPUNIT_ASSERT_EQUAL(true, m_TrackingTool->IsToolTipSet()); } void GetMTime_SetPosition_ReturnsModifiedTime() { itk::ModifiedTimeType time = m_TrackingTool->GetMTime(); mitk::Point3D position1; mitk::FillVector3D(position1, 1.1, 2.2, 3.3); m_TrackingTool->SetPosition(position1); CPPUNIT_ASSERT(time < m_TrackingTool->GetMTime()); time = m_TrackingTool->GetMTime(); m_TrackingTool->SetPosition(position1); CPPUNIT_ASSERT(time == m_TrackingTool->GetMTime()); mitk::Point3D position2; mitk::FillVector3D(position2, 1, 2, 3); m_TrackingTool->SetPosition(position2); CPPUNIT_ASSERT(time < m_TrackingTool->GetMTime()); } void GetMTime_SetOrientation_ReturnsModifiedTime() { itk::ModifiedTimeType time = m_TrackingTool->GetMTime(); mitk::Quaternion orientation1 = mitk::Quaternion(0, 0, 0.70710678118654757, 0.70710678118654757); m_TrackingTool->SetOrientation(orientation1); CPPUNIT_ASSERT(time < m_TrackingTool->GetMTime()); time = m_TrackingTool->GetMTime(); m_TrackingTool->SetOrientation(orientation1); CPPUNIT_ASSERT(time == m_TrackingTool->GetMTime()); mitk::Quaternion orientation2 = mitk::Quaternion(0, 0, 0.70710678118654757, 0.70710678118654757 + 0.00001); m_TrackingTool->SetOrientation(orientation2); CPPUNIT_ASSERT(time < m_TrackingTool->GetMTime()); } }; MITK_TEST_SUITE_REGISTRATION(mitkTrackingTool) diff --git a/Modules/IGT/TestingHelper/mitkNavigationToolStorageTestHelper.cpp b/Modules/IGT/TestingHelper/mitkNavigationToolStorageTestHelper.cpp index 3dcf39c49d..fc5bd5613c 100644 --- a/Modules/IGT/TestingHelper/mitkNavigationToolStorageTestHelper.cpp +++ b/Modules/IGT/TestingHelper/mitkNavigationToolStorageTestHelper.cpp @@ -1,108 +1,108 @@ /*=================================================================== 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 #include #include "mitkNavigationToolStorageTestHelper.h" #include "mitkNDIAuroraTypeInformation.h" #include "mitkMicronTrackerTypeInformation.h" mitk::NavigationToolStorage::Pointer mitk::NavigationToolStorageTestHelper::CreateTestData_SimpleStorage() { //create Tool Storage mitk::NavigationToolStorage::Pointer myStorage = mitk::NavigationToolStorage::New(); //first tool mitk::NavigationTool::Pointer myTool1 = mitk::NavigationTool::New(); myTool1->SetIdentifier("001"); myStorage->AddTool(myTool1); //second tool mitk::NavigationTool::Pointer myTool2 = mitk::NavigationTool::New(); myTool2->SetIdentifier("002"); myStorage->AddTool(myTool2); //third tool mitk::NavigationTool::Pointer myTool3 = mitk::NavigationTool::New(); myTool3->SetIdentifier("003"); myStorage->AddTool(myTool3); return myStorage; } mitk::NavigationToolStorage::Pointer mitk::NavigationToolStorageTestHelper::CreateTestData_StorageWithOneTool() { //create Tool Storage mitk::NavigationToolStorage::Pointer myStorage = mitk::NavigationToolStorage::New(); //first tool mitk::NavigationTool::Pointer myTool1 = mitk::NavigationTool::New(); myTool1->SetIdentifier("001"); mitk::PointSet::Pointer CalLandmarks1 = mitk::PointSet::New(); mitk::Point3D testPt1; mitk::FillVector3D(testPt1,1,2,3); CalLandmarks1->SetPoint(0,testPt1); mitk::PointSet::Pointer RegLandmarks1 = mitk::PointSet::New(); mitk::Point3D testPt2; mitk::FillVector3D(testPt2,4,5,6); RegLandmarks1->SetPoint(5,testPt2); myTool1->SetToolControlPoints(CalLandmarks1); myTool1->SetToolLandmarks(RegLandmarks1); mitk::Point3D toolTipPos; mitk::FillVector3D(toolTipPos,1.3423,2.323,4.332); mitk::Quaternion toolTipRot = mitk::Quaternion(0.1,0.2,0.3,0.4); myTool1->SetToolTipPosition(toolTipPos); - myTool1->SetToolTipOrientation(toolTipRot); + myTool1->SetToolAxisOrientation(toolTipRot); myStorage->AddTool(myTool1); return myStorage; } mitk::NavigationToolStorage::Pointer mitk::NavigationToolStorageTestHelper::CreateTestData_ComplexStorage(std::string toolFilePath, std::string toolSurfacePath1, std::string toolSurfacePath2) { //create first tool mitk::NavigationTool::Pointer myNavigationTool = mitk::NavigationTool::New(); myNavigationTool->SetCalibrationFile(toolFilePath); mitk::DataNode::Pointer myNode = mitk::DataNode::New(); myNode->SetName("ClaronTool"); myNode->SetData(dynamic_cast(mitk::IOUtil::Load(toolSurfacePath1)[0].GetPointer())); //load an stl File myNavigationTool->SetDataNode(myNode); myNavigationTool->SetIdentifier("ClaronTool#1"); myNavigationTool->SetSerialNumber("0815"); myNavigationTool->SetTrackingDeviceType(mitk::MicronTrackerTypeInformation::GetTrackingDeviceName()); myNavigationTool->SetType(mitk::NavigationTool::Fiducial); //create second tool mitk::NavigationTool::Pointer myNavigationTool2 = mitk::NavigationTool::New(); mitk::Surface::Pointer testSurface2; mitk::DataNode::Pointer myNode2 = mitk::DataNode::New(); myNode2->SetName("AuroraTool"); //load an stl File testSurface2 = dynamic_cast(mitk::IOUtil::Load(toolSurfacePath2)[0].GetPointer()); myNode2->SetData(testSurface2); myNavigationTool2->SetDataNode(myNode2); myNavigationTool2->SetIdentifier("AuroraTool#1"); myNavigationTool2->SetSerialNumber("0816"); myNavigationTool2->SetTrackingDeviceType(mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()); myNavigationTool2->SetType(mitk::NavigationTool::Instrument); //create navigation tool storage mitk::NavigationToolStorage::Pointer myStorage = mitk::NavigationToolStorage::New(); myStorage->AddTool(myNavigationTool); myStorage->AddTool(myNavigationTool2); return myStorage; } diff --git a/Modules/IGT/TrackingDevices/mitkMicronTrackerTypeInformation.cpp b/Modules/IGT/TrackingDevices/mitkMicronTrackerTypeInformation.cpp index 3edfdf7dcd..451511c20a 100644 --- a/Modules/IGT/TrackingDevices/mitkMicronTrackerTypeInformation.cpp +++ b/Modules/IGT/TrackingDevices/mitkMicronTrackerTypeInformation.cpp @@ -1,70 +1,70 @@ /*=================================================================== 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 "mitkMicronTrackerTypeInformation.h" #include "mitkClaronTrackingDevice.h" namespace mitk { std::string MicronTrackerTypeInformation::GetTrackingDeviceName() { return "Claron Micron"; } TrackingDeviceData MicronTrackerTypeInformation::GetDeviceDataMicronTrackerH40() { TrackingDeviceData data = { MicronTrackerTypeInformation::GetTrackingDeviceName(), "Micron Tracker H40", "ClaronMicron.stl", "X" }; return data; } MicronTrackerTypeInformation::MicronTrackerTypeInformation() { m_DeviceName = MicronTrackerTypeInformation::GetTrackingDeviceName(); m_TrackingDeviceData.push_back(GetDeviceDataMicronTrackerH40()); } MicronTrackerTypeInformation::~MicronTrackerTypeInformation() { } mitk::TrackingDeviceSource::Pointer MicronTrackerTypeInformation::CreateTrackingDeviceSource( mitk::TrackingDevice::Pointer trackingDevice, mitk::NavigationToolStorage::Pointer navigationTools, std::string* errorMessage, std::vector* toolCorrespondencesInToolStorage) { mitk::TrackingDeviceSource::Pointer returnValue = mitk::TrackingDeviceSource::New(); mitk::ClaronTrackingDevice::Pointer thisDevice = dynamic_cast(trackingDevice.GetPointer()); *toolCorrespondencesInToolStorage = std::vector(); //add the tools to the tracking device for (unsigned int i = 0; i < navigationTools->GetToolCount(); i++) { mitk::NavigationTool::Pointer thisNavigationTool = navigationTools->GetTool(i); toolCorrespondencesInToolStorage->push_back(i); bool toolAddSuccess = thisDevice->AddTool(thisNavigationTool->GetToolName().c_str(), thisNavigationTool->GetCalibrationFile().c_str()); if (!toolAddSuccess) { //todo error handling errorMessage->append("Can't add tool, is the toolfile valid?"); return nullptr; } - thisDevice->GetTool(i)->SetToolTip(thisNavigationTool->GetToolTipPosition(), thisNavigationTool->GetToolTipOrientation()); + thisDevice->GetTool(i)->SetToolTipPosition(thisNavigationTool->GetToolTipPosition(), thisNavigationTool->GetToolAxisOrientation()); } returnValue->SetTrackingDevice(thisDevice); return returnValue; } } diff --git a/Modules/IGT/TrackingDevices/mitkNDIAuroraTypeInformation.cpp b/Modules/IGT/TrackingDevices/mitkNDIAuroraTypeInformation.cpp index 77b0e70b23..916fea69b1 100644 --- a/Modules/IGT/TrackingDevices/mitkNDIAuroraTypeInformation.cpp +++ b/Modules/IGT/TrackingDevices/mitkNDIAuroraTypeInformation.cpp @@ -1,153 +1,153 @@ /*=================================================================== 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 "mitkNDIAuroraTypeInformation.h" #include "mitkIGTHardwareException.h" #include "mitkNDITrackingDevice.h" namespace mitk { std::string NDIAuroraTypeInformation::GetTrackingDeviceName() { return "NDI Aurora"; } TrackingDeviceData NDIAuroraTypeInformation::GetDeviceDataAuroraCompact() { TrackingDeviceData data = { NDIAuroraTypeInformation::GetTrackingDeviceName(), "Aurora Compact", "NDIAuroraCompactFG_Dome.stl", "A" }; return data; } TrackingDeviceData NDIAuroraTypeInformation::GetDeviceDataAuroraPlanarCube() { TrackingDeviceData data = { NDIAuroraTypeInformation::GetTrackingDeviceName(), "Aurora Planar (Cube)", "NDIAurora.stl", "9" }; return data; } TrackingDeviceData NDIAuroraTypeInformation::GetDeviceDataAuroraPlanarDome() { TrackingDeviceData data = { NDIAuroraTypeInformation::GetTrackingDeviceName(), "Aurora Planar (Dome)", "NDIAuroraPlanarFG_Dome.stl", "A" }; return data; } TrackingDeviceData NDIAuroraTypeInformation::GetDeviceDataAuroraTabletop() { TrackingDeviceData data = { NDIAuroraTypeInformation::GetTrackingDeviceName(), "Aurora Tabletop", "NDIAuroraTabletopFG_Dome.stl", "A" }; return data; } NDIAuroraTypeInformation::NDIAuroraTypeInformation() { m_DeviceName = NDIAuroraTypeInformation::GetTrackingDeviceName(); m_TrackingDeviceData.push_back(GetDeviceDataAuroraCompact()); m_TrackingDeviceData.push_back(GetDeviceDataAuroraPlanarCube()); m_TrackingDeviceData.push_back(GetDeviceDataAuroraPlanarDome()); m_TrackingDeviceData.push_back(GetDeviceDataAuroraTabletop()); } NDIAuroraTypeInformation::~NDIAuroraTypeInformation() { } mitk::TrackingDeviceSource::Pointer NDIAuroraTypeInformation::CreateTrackingDeviceSource( mitk::TrackingDevice::Pointer trackingDevice, mitk::NavigationToolStorage::Pointer navigationTools, std::string* errorMessage, std::vector* toolCorrespondencesInToolStorage) { MITK_DEBUG << "Creating Aurora tracking device."; mitk::TrackingDeviceSource::Pointer returnValue = mitk::TrackingDeviceSource::New(); mitk::NDITrackingDevice::Pointer thisDevice = dynamic_cast(trackingDevice.GetPointer()); try { //connect to aurora to dectect tools automatically thisDevice->OpenConnection(); } catch (mitk::IGTHardwareException& e) { errorMessage->append("Hardware error on opening the connection ("); errorMessage->append(e.GetDescription()); errorMessage->append(")"); return nullptr; } catch (mitk::IGTException& e) { errorMessage->append("Error on opening the connection ("); errorMessage->append(e.GetDescription()); errorMessage->append(")"); return nullptr; } //now search for automatically detected tools in the tool storage and save them mitk::NavigationToolStorage::Pointer newToolStorageInRightOrder = mitk::NavigationToolStorage::New(); std::vector alreadyFoundTools = std::vector(); *toolCorrespondencesInToolStorage = std::vector(); for (unsigned int i = 0; i < thisDevice->GetToolCount(); i++) { bool toolFound = false; for (unsigned int j = 0; j < navigationTools->GetToolCount(); j++) { //check if the serial number is the same to identify the tool if ((dynamic_cast(thisDevice->GetTool(i)))->GetSerialNumber() == navigationTools->GetTool(j)->GetSerialNumber()) { //check if this tool was already added to make sure that every tool is only added once (in case of same serial numbers) bool toolAlreadyAdded = false; for (unsigned int k = 0; k < alreadyFoundTools.size(); k++) { if (alreadyFoundTools.at(k) == j) { toolAlreadyAdded = true; } } if (!toolAlreadyAdded) { //add tool in right order newToolStorageInRightOrder->AddTool(navigationTools->GetTool(j)); toolCorrespondencesInToolStorage->push_back(j); //adapt name of tool dynamic_cast(thisDevice->GetTool(i))->SetToolName(navigationTools->GetTool(j)->GetToolName()); //set tip of tool - dynamic_cast(thisDevice->GetTool(i))->SetToolTip(navigationTools->GetTool(j)->GetToolTipPosition(), navigationTools->GetTool(j)->GetToolTipOrientation()); + dynamic_cast(thisDevice->GetTool(i))->SetToolTipPosition(navigationTools->GetTool(j)->GetToolTipPosition(), navigationTools->GetTool(j)->GetToolAxisOrientation()); //rember that this tool was already found alreadyFoundTools.push_back(j); toolFound = true; break; } } } if (!toolFound) { errorMessage->append("Error: did not find every automatically detected tool in the loaded tool storage: aborting initialization."); return nullptr; } } //And resort them (this was done in TrackingToolBoxWorker before). for (unsigned int i = 0; i < newToolStorageInRightOrder->GetToolCount(); i++) { navigationTools->AssignToolNumber(newToolStorageInRightOrder->GetTool(i)->GetIdentifier(), i); } returnValue->SetTrackingDevice(thisDevice); MITK_DEBUG << "Number of tools of created tracking device: " << thisDevice->GetToolCount(); MITK_DEBUG << "Number of outputs of created source: " << returnValue->GetNumberOfOutputs(); return returnValue; } } diff --git a/Modules/IGT/TrackingDevices/mitkNDIPolarisTypeInformation.cpp b/Modules/IGT/TrackingDevices/mitkNDIPolarisTypeInformation.cpp index 025f26d535..1c67e848d3 100644 --- a/Modules/IGT/TrackingDevices/mitkNDIPolarisTypeInformation.cpp +++ b/Modules/IGT/TrackingDevices/mitkNDIPolarisTypeInformation.cpp @@ -1,90 +1,90 @@ /*=================================================================== 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 "mitkNDIPolarisTypeInformation.h" #include "mitkNDITrackingDevice.h" namespace mitk { std::string NDIPolarisTypeInformation::GetTrackingDeviceName() { return "NDI Polaris"; ///< Polaris: optical Tracker from NDI; } TrackingDeviceData NDIPolarisTypeInformation::GetDeviceDataPolarisOldModel() { TrackingDeviceData data = { NDIPolarisTypeInformation::GetTrackingDeviceName(), "Polaris (Old Model)", "NDIPolarisOldModel.stl", "0" }; return data; } TrackingDeviceData NDIPolarisTypeInformation::GetDeviceDataPolarisSpectra() { //full hardware code of polaris spectra: 5-240000-153200-095000+057200+039800+056946+024303+029773+999999+99999924 TrackingDeviceData data = { NDIPolarisTypeInformation::GetTrackingDeviceName(), "Polaris Spectra", "NDIPolarisSpectra.stl", "5-2" }; return data; } TrackingDeviceData NDIPolarisTypeInformation::GetDeviceDataSpectraExtendedPyramid() { //full hardware code of polaris spectra (extended pyramid): 5-300000-153200-095000+057200+039800+056946+024303+029773+999999+07350024 TrackingDeviceData data = { NDIPolarisTypeInformation::GetTrackingDeviceName(), "Polaris Spectra (Extended Pyramid)", "NDIPolarisSpectraExtendedPyramid.stl", "5-3" }; return data; } TrackingDeviceData NDIPolarisTypeInformation::GetDeviceDataPolarisVicra() { TrackingDeviceData data = { NDIPolarisTypeInformation::GetTrackingDeviceName(), "Polaris Vicra", "NDIPolarisVicra.stl", "7" }; return data; } NDIPolarisTypeInformation::NDIPolarisTypeInformation() { m_DeviceName = NDIPolarisTypeInformation::GetTrackingDeviceName(); m_TrackingDeviceData.push_back(GetDeviceDataPolarisOldModel()); m_TrackingDeviceData.push_back(GetDeviceDataSpectraExtendedPyramid()); m_TrackingDeviceData.push_back(GetDeviceDataPolarisSpectra()); m_TrackingDeviceData.push_back(GetDeviceDataPolarisVicra()); } NDIPolarisTypeInformation::~NDIPolarisTypeInformation() { } mitk::TrackingDeviceSource::Pointer NDIPolarisTypeInformation::CreateTrackingDeviceSource( mitk::TrackingDevice::Pointer trackingDevice, mitk::NavigationToolStorage::Pointer navigationTools, std::string* errorMessage, std::vector* toolCorrespondencesInToolStorage) { mitk::TrackingDeviceSource::Pointer returnValue = mitk::TrackingDeviceSource::New(); mitk::NDITrackingDevice::Pointer thisDevice = dynamic_cast(trackingDevice.GetPointer()); *toolCorrespondencesInToolStorage = std::vector(); //add the tools to the tracking device for (unsigned int i = 0; i < navigationTools->GetToolCount(); i++) { mitk::NavigationTool::Pointer thisNavigationTool = navigationTools->GetTool(i); toolCorrespondencesInToolStorage->push_back(i); bool toolAddSuccess = thisDevice->AddTool(thisNavigationTool->GetToolName().c_str(), thisNavigationTool->GetCalibrationFile().c_str()); if (!toolAddSuccess) { //todo: error handling errorMessage->append("Can't add tool, is the SROM-file valid?"); return nullptr; } - thisDevice->GetTool(i)->SetToolTip(thisNavigationTool->GetToolTipPosition(), thisNavigationTool->GetToolTipOrientation()); + thisDevice->GetTool(i)->SetToolTipPosition(thisNavigationTool->GetToolTipPosition(), thisNavigationTool->GetToolAxisOrientation()); } returnValue->SetTrackingDevice(thisDevice); return returnValue; } } diff --git a/Modules/IGT/TrackingDevices/mitkNPOptitrackTrackingTypeInformation.cpp b/Modules/IGT/TrackingDevices/mitkNPOptitrackTrackingTypeInformation.cpp index 7b56482453..d96093ace6 100644 --- a/Modules/IGT/TrackingDevices/mitkNPOptitrackTrackingTypeInformation.cpp +++ b/Modules/IGT/TrackingDevices/mitkNPOptitrackTrackingTypeInformation.cpp @@ -1,75 +1,75 @@ /*=================================================================== 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 "mitkNPOptitrackTrackingTypeInformation.h" #include "mitkOptitrackTrackingDevice.h" namespace mitk { std::string NPOptitrackTrackingTypeInformation::GetTrackingDeviceName() { return "NP Optitrack"; } TrackingDeviceData NPOptitrackTrackingTypeInformation::GetDeviceDataNPOptitrack() { TrackingDeviceData data = { NPOptitrackTrackingTypeInformation::GetTrackingDeviceName(), "Optitrack", "cube", "X" }; return data; } NPOptitrackTrackingTypeInformation::NPOptitrackTrackingTypeInformation() { m_DeviceName = NPOptitrackTrackingTypeInformation::GetTrackingDeviceName(); m_TrackingDeviceData.push_back(GetDeviceDataNPOptitrack()); } NPOptitrackTrackingTypeInformation::~NPOptitrackTrackingTypeInformation() { } TrackingDeviceSource::Pointer NPOptitrackTrackingTypeInformation::CreateTrackingDeviceSource( TrackingDevice::Pointer trackingDevice, NavigationToolStorage::Pointer navigationTools, std::string* errorMessage, std::vector* toolCorrespondencesInToolStorage) { mitk::TrackingDeviceSource::Pointer returnValue = mitk::TrackingDeviceSource::New(); mitk::OptitrackTrackingDevice::Pointer thisDevice = dynamic_cast(trackingDevice.GetPointer()); *toolCorrespondencesInToolStorage = std::vector(); //OpenConnection with Optitrack thisDevice->OpenConnection(); //add the tools to the tracking device for (unsigned int i = 0; i < navigationTools->GetToolCount(); i++) { mitk::NavigationTool::Pointer thisNavigationTool = navigationTools->GetTool(i); toolCorrespondencesInToolStorage->push_back(i); bool toolAddSuccess = thisDevice->AddToolByDefinitionFile(thisNavigationTool->GetCalibrationFile()); thisDevice->GetOptitrackTool(i)->SetToolName(thisNavigationTool->GetToolName().c_str()); if (!toolAddSuccess) { //todo error handling errorMessage->append("Can't add tool, is the toolfile valid?"); return nullptr; } - //thisDevice->GetTool(i)->SetToolTip(thisNavigationTool->GetToolTipPosition(),thisNavigationTool->GetToolTipOrientation()); + //thisDevice->GetTool(i)->SetToolTip(thisNavigationTool->GetToolTipPosition(),thisNavigationTool->GetToolAxisOrientation()); } returnValue->SetTrackingDevice(thisDevice); return returnValue; } } diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp index b7b8f0cf85..3f772b99bd 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp @@ -1,469 +1,469 @@ /*=================================================================== 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 #define _USE_MATH_DEFINES #include #include BYTE MotionBuf[0x1FA400]; mitk::PolhemusInterface::PolhemusInterface() : m_continousTracking(false) { m_pdiDev = new CPDIdev(); m_numberOfTools = 0; } mitk::PolhemusInterface::~PolhemusInterface() { delete m_pdiDev; } bool mitk::PolhemusInterface::InitializeDevice() { m_pdiDev->ResetTracker(); m_pdiDev->ResetSAlignment(-1); m_pdiDev->Trace(TRUE, 7); m_continousTracking = false; return true; } bool mitk::PolhemusInterface::SetupDevice() { m_pdiDev->SetPnoBuffer(MotionBuf, 0x1FA400); m_pdiDev->SetMetric(true); //use cm instead of inches m_pdiDev->StartPipeExport(); CPDImdat pdiMDat; pdiMDat.Empty(); pdiMDat.Append(PDI_MODATA_FRAMECOUNT); pdiMDat.Append(PDI_MODATA_POS); pdiMDat.Append(PDI_MODATA_ORI); m_pdiDev->SetSDataList(-1, pdiMDat); CPDIbiterr cBE; m_pdiDev->GetBITErrs(cBE); if (!(cBE.IsClear())) { m_pdiDev->ClearBITErrs(); } return true; } bool mitk::PolhemusInterface::StartTracking() { m_continousTracking = true; return m_pdiDev->StartContPno(0); } bool mitk::PolhemusInterface::StopTracking() { m_continousTracking = false; m_pdiDev->StopContPno(); return true; } bool mitk::PolhemusInterface::OpenConnection() { bool returnValue; //Initialize, and if it is not successful, return false. if (!InitializeDevice()) { returnValue = false; } //Connect else if (m_pdiDev->CnxReady()) { returnValue = true; } //If it is not successful, search for connections. else { CPDIser pdiSer; m_pdiDev->SetSerialIF(&pdiSer); ePiCommType eType = m_pdiDev->DiscoverCnx(); switch (eType) { case PI_CNX_USB: MITK_INFO << "USB Connection: " << m_pdiDev->GetLastResultStr(); break; case PI_CNX_SERIAL: MITK_INFO << "Serial Connection: " << m_pdiDev->GetLastResultStr(); break; default: MITK_INFO << "DiscoverCnx result: " << m_pdiDev->GetLastResultStr(); break; } //Setup device if (!SetupDevice()) { returnValue = false; } else { returnValue = m_pdiDev->CnxReady(); } } return returnValue; } bool mitk::PolhemusInterface::Connect() { bool returnValue = OpenConnection(); if (!returnValue) { return returnValue; } m_numberOfTools = this->GetNumberOfTools(); //Get the tracking data to find out which tools are available. std::vector _trackingData = GetFrame(); //if we have more/less tools than before, reset all data. //check with toolStorage changes is nto enough, 'cause a sensor could just have been unplugged. if (m_ToolPorts.size() != _trackingData.size()) { m_ToolPorts.clear(); m_Hemispheres.clear(); m_HemisphereTracking.clear(); } //if we have the same number of tools as before, check if they are still the same. if (m_ToolPorts.size() == _trackingData.size()) { - for (int i = 0; i < _trackingData.size(); ++i) + for (size_t i = 0; i < _trackingData.size(); ++i) { //if they are not the same, clear hemispheres and toolNames and break. if (m_ToolPorts[i] != _trackingData.at(i).id) { m_ToolPorts.clear(); m_Hemispheres.clear(); m_HemisphereTracking.clear(); break; } } } //if we don't have old tool names or if the old ones don't match any more, assign them again. if (m_ToolPorts.size() == 0) { - for (int i = 0; i < _trackingData.size(); ++i) + for (size_t i = 0; i < _trackingData.size(); ++i) { m_ToolPorts.push_back(_trackingData.at(i).id); } //and reset the hemisphere parameters m_Hemispheres.clear(); m_HemisphereTracking.clear(); mitk::Vector3D temp; mitk::FillVector3D(temp, 1, 0, 0); m_Hemispheres.assign(m_numberOfTools, temp); m_HemisphereTracking.assign(m_numberOfTools, false); } return returnValue; } bool mitk::PolhemusInterface::Disconnect() { bool returnValue = true; //If Tracking is running, stop tracking first if (m_continousTracking) { this->StopTracking(); } returnValue = m_pdiDev->Disconnect(); MITK_INFO << "Disconnect: " << m_pdiDev->GetLastResultStr(); return returnValue; } std::vector mitk::PolhemusInterface::AutoDetectTools() { OpenConnection(); std::vector frame = GetSingleFrame(); m_pdiDev->Disconnect(); return frame; } unsigned int mitk::PolhemusInterface::GetNumberOfTools() { std::vector _trackingData = GetFrame(); return _trackingData.size(); } std::vector mitk::PolhemusInterface::GetFrame() { if (m_continousTracking) return this->GetLastFrame(); else return this->GetSingleFrame(); } std::vector mitk::PolhemusInterface::GetLastFrame() { PBYTE pBuf; DWORD dwSize; //read one frame if (!m_pdiDev->LastPnoPtr(pBuf, dwSize)) { MITK_WARN << m_pdiDev->GetLastResultStr(); } std::vector returnValue = ParsePolhemusRawData(pBuf, dwSize); if (returnValue.empty()) { MITK_WARN << "Cannot parse data / no tools present"; } return returnValue; } std::vector mitk::PolhemusInterface::GetSingleFrame() { if (m_continousTracking) { MITK_WARN << "Cannot get a single frame when continuous tracking is on!"; return std::vector(); } PBYTE pBuf; DWORD dwSize; //read one frame if (!m_pdiDev->ReadSinglePnoBuf(pBuf, dwSize)) { MITK_WARN << m_pdiDev->GetLastResultStr(); return std::vector(); } return ParsePolhemusRawData(pBuf, dwSize); } std::vector mitk::PolhemusInterface::ParsePolhemusRawData(PBYTE pBuf, DWORD dwSize) { std::vector returnValue; DWORD i = 0; while (i < dwSize) { BYTE ucSensor = pBuf[i + 2]; SHORT shSize = pBuf[i + 6]; // skip rest of header i += 8; PDWORD pFC = (PDWORD)(&pBuf[i]); PFLOAT pPno = (PFLOAT)(&pBuf[i + 4]); mitk::PolhemusInterface::trackingData currentTrackingData; currentTrackingData.id = ucSensor; currentTrackingData.pos[0] = pPno[0] * 10; //from cm to mm currentTrackingData.pos[1] = pPno[1] * 10; currentTrackingData.pos[2] = pPno[2] * 10; double azimuthAngle = pPno[3] / 180 * M_PI; //from degree to rad double elevationAngle = pPno[4] / 180 * M_PI; double rollAngle = pPno[5] / 180 * M_PI; vnl_quaternion eulerQuat(rollAngle, elevationAngle, azimuthAngle); currentTrackingData.rot = eulerQuat; returnValue.push_back(currentTrackingData); i += shSize; } return returnValue; } void mitk::PolhemusInterface::SetHemisphereTrackingEnabled(bool _HeisphereTrackingEnabeled, int _tool) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return; if (m_Hemispheres.empty()) { MITK_ERROR << "No Hemispheres. This should never happen when connected. Check your code!"; } //HemisphereTracking is switched on by SetSHemiTrack(-1). "-1" means for all sensors. //To switch heisphere tracking of, you need to set a hemisphere vector e.g. by calling SetSHemisphere(-1, { (float)1,0,0 }) if (_HeisphereTrackingEnabeled) { m_pdiDev->SetSHemiTrack(_tool); if (_tool != -1) { m_HemisphereTracking.at(GetToolIndex(_tool)) = true; } else { m_HemisphereTracking.assign(m_numberOfTools, true); } } //switch HemiTracking OFF else { //Get Tool Position. ToDo, this should not be the tool tip but the sensor position. Any chance, to get that from Polhemus interface?! std::vector _position = GetFrame(); for (int index : GetToolIterator(_tool)) { //Scalar product between mitk::point and mitk::vector double _scalarProduct = _position.at(index).pos.GetVectorFromOrigin() * m_Hemispheres.at(index); //if scalar product is negative, then the tool is in the opposite sphere then when we started to track. //Hence, we have to set the inverted hemisphere. //For default (1|0|0) this means, if x is negative, we have to set (-1|0|0). But we want to keep it generic if user sets different hemisphere... if (_scalarProduct < 0) { m_Hemispheres.at(index) = -1. * m_Hemispheres.at(index); } else if (_scalarProduct == 0) MITK_ERROR << "Something went wrong. Hemisphere or Position should not be zero."; SetHemisphere(m_ToolPorts[index], m_Hemispheres.at(index)); } } } void mitk::PolhemusInterface::ToggleHemisphere(int _tool) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return; //toggle. for (int index : GetToolIterator(_tool)) { if (m_HemisphereTracking.at(index)) { SetHemisphereTrackingEnabled(false, m_ToolPorts[index]); this->SetHemisphere(m_ToolPorts[index], -1.*m_Hemispheres.at(index)); SetHemisphereTrackingEnabled(true, m_ToolPorts[index]); } else { this->SetHemisphere(m_ToolPorts[index], -1.*m_Hemispheres.at(index)); } } } void mitk::PolhemusInterface::AdjustHemisphere(int _tool) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return; mitk::Vector3D _hemisphere; mitk::FillVector3D(_hemisphere, 1, 0, 0); for (int index : GetToolIterator(_tool)) { if (m_HemisphereTracking.at(index)) { SetHemisphereTrackingEnabled(false, m_ToolPorts[index]); this->SetHemisphere(m_ToolPorts[index], _hemisphere); SetHemisphereTrackingEnabled(true, m_ToolPorts[index]); } else { this->SetHemisphere(m_ToolPorts[index], _hemisphere); } } } void mitk::PolhemusInterface::SetHemisphere(int _tool, mitk::Vector3D _hemisphere) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return; m_pdiDev->SetSHemisphere(_tool, { (float)_hemisphere[0], (float)_hemisphere[1], (float)_hemisphere[2] }); for (int index : GetToolIterator(_tool)) { if (_hemisphere.GetNorm() != 0) { m_HemisphereTracking.at(index) = false; m_Hemispheres.at(index) = _hemisphere; } else { m_HemisphereTracking.at(index) = true; //don't set the Hemisphere to (0|0|0), as we want to remember the old one. } } } mitk::Vector3D mitk::PolhemusInterface::GetHemisphere(int _tool) { if (_tool == -1) { MITK_WARN << "Can't return hemisphere for all tools. Returning Hemisphere of first tool " << m_ToolPorts[0]; return m_Hemispheres.at(0); } return m_Hemispheres.at(GetToolIndex(_tool)); } bool mitk::PolhemusInterface::GetHemisphereTrackingEnabled(int _tool) { //if tool is -1, this means "All Tools". We return true if HemiTracking is enabled for all tools, and false if it is off for at least one tool. if (_tool == -1) { bool _returnValue = true; for (bool currentValue : m_HemisphereTracking) _returnValue = _returnValue && currentValue; return _returnValue; } else return m_HemisphereTracking.at(GetToolIndex(_tool)); } std::vector mitk::PolhemusInterface::GetToolPorts() { return m_ToolPorts; } int mitk::PolhemusInterface::GetToolIndex(int _tool) { if (_tool == -1) return -1; else return std::find(m_ToolPorts.begin(), m_ToolPorts.end(), _tool) - m_ToolPorts.begin(); } std::vector mitk::PolhemusInterface::GetToolIterator(int _tool) { std::vector _iterator; if (_tool == -1) { for (int i = 0; i < static_cast(m_numberOfTools); ++i) _iterator.push_back(i); } else { _iterator.push_back(GetToolIndex(_tool)); } return _iterator; } void mitk::PolhemusInterface::PrintStatus() { MITK_INFO << "Polhemus status: " << this->m_pdiDev->CnxReady(); } \ No newline at end of file diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp index e040fa025b..5d709a05dd 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp @@ -1,70 +1,70 @@ /*=================================================================== 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 "mitkPolhemusTrackerTypeInformation.h" #include "mitkPolhemusTrackingDevice.h" namespace mitk { std::string PolhemusTrackerTypeInformation::GetTrackingDeviceName() { return "Polhemus tracker"; } TrackingDeviceData PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty() { TrackingDeviceData data = { PolhemusTrackerTypeInformation::GetTrackingDeviceName(), "Polhemus Liberty Tracker", "cube", "X" }; return data; } PolhemusTrackerTypeInformation::PolhemusTrackerTypeInformation() { m_DeviceName = PolhemusTrackerTypeInformation::GetTrackingDeviceName(); m_TrackingDeviceData.push_back(GetDeviceDataPolhemusTrackerLiberty()); } PolhemusTrackerTypeInformation::~PolhemusTrackerTypeInformation() { } mitk::TrackingDeviceSource::Pointer PolhemusTrackerTypeInformation::CreateTrackingDeviceSource( mitk::TrackingDevice::Pointer trackingDevice, mitk::NavigationToolStorage::Pointer navigationTools, std::string* errorMessage, std::vector* toolCorrespondencesInToolStorage) { mitk::TrackingDeviceSource::Pointer returnValue = mitk::TrackingDeviceSource::New(); mitk::PolhemusTrackingDevice::Pointer thisDevice = dynamic_cast(trackingDevice.GetPointer()); *toolCorrespondencesInToolStorage = std::vector(); //add the tools to the tracking device for (unsigned int i = 0; i < navigationTools->GetToolCount(); i++) { mitk::NavigationTool::Pointer thisNavigationTool = navigationTools->GetTool(i); toolCorrespondencesInToolStorage->push_back(i); bool toolAddSuccess = thisDevice->AddTool(thisNavigationTool->GetToolName().c_str(), std::stoi(thisNavigationTool->GetIdentifier())); if (!toolAddSuccess) { //todo error handling errorMessage->append("Can't add tool, is the toolfile valid?"); return NULL; } - thisDevice->GetTool(i)->SetToolTip(thisNavigationTool->GetToolTipPosition(), thisNavigationTool->GetToolTipOrientation()); + thisDevice->GetTool(i)->SetToolTipPosition(thisNavigationTool->GetToolTipPosition(), thisNavigationTool->GetToolAxisOrientation()); } returnValue->SetTrackingDevice(thisDevice); return returnValue; } } diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp index 7121391bfe..96d4f6fd63 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp @@ -1,317 +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" 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++) + for (size_t 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::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); std::stringstream name; name << "Sensor-" << ((int)t.id); 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/IGT/TrackingDevices/mitkTrackingTool.cpp b/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp index eba0257187..b86830db48 100644 --- a/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp +++ b/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp @@ -1,293 +1,293 @@ /*=================================================================== 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 "mitkTrackingTool.h" #include typedef itk::MutexLockHolder MutexLockHolder; mitk::TrackingTool::TrackingTool() : itk::Object(), m_ToolName(""), m_ErrorMessage(""), m_IGTTimeStamp(0), m_MyMutex(itk::FastMutexLock::New()), m_TrackingError(0.0f), m_Enabled(true), m_DataValid(false), m_ToolTipSet(false) { m_Position[0] = 0.0f; m_Position[1] = 0.0f; m_Position[2] = 0.0f; m_Orientation[0] = 0.0f; m_Orientation[1] = 0.0f; m_Orientation[2] = 0.0f; m_Orientation[3] = 0.0f; // this should not be necessary as the tools bring their own tooltip transformation - m_ToolTip[0] = 0.0f; - m_ToolTip[1] = 0.0f; - m_ToolTip[2] = 0.0f; - m_ToolTipRotation[0] = 0.0f; - m_ToolTipRotation[1] = 0.0f; - m_ToolTipRotation[2] = 0.0f; - m_ToolTipRotation[3] = 1.0f; + m_ToolTipPosition[0] = 0.0f; + m_ToolTipPosition[1] = 0.0f; + m_ToolTipPosition[2] = 0.0f; + m_ToolAxisOrientation[0] = 0.0f; + m_ToolAxisOrientation[1] = 0.0f; + m_ToolAxisOrientation[2] = 0.0f; + m_ToolAxisOrientation[3] = 1.0f; } mitk::TrackingTool::~TrackingTool() { m_MyMutex->Unlock(); m_MyMutex = nullptr; } void mitk::TrackingTool::PrintSelf(std::ostream& os, itk::Indent indent) const { Superclass::PrintSelf(os, indent); os << indent << "ToolName: " << m_ToolName << std::endl; os << indent << "ErrorMesage: " << m_ErrorMessage << std::endl; os << indent << "Position: " << m_Position << std::endl; os << indent << "Orientation: " << m_Orientation << std::endl; os << indent << "TrackingError: " << m_TrackingError << std::endl; os << indent << "Enabled: " << m_Enabled << std::endl; os << indent << "DataValid: " << m_DataValid << std::endl; - os << indent << "ToolTip: " << m_ToolTip << std::endl; - os << indent << "ToolTipRotation: " << m_ToolTipRotation << std::endl; + os << indent << "ToolTip: " << m_ToolTipPosition << std::endl; + os << indent << "ToolTipRotation: " << m_ToolAxisOrientation << std::endl; os << indent << "ToolTipSet: " << m_ToolTipSet << std::endl; } const char* mitk::TrackingTool::GetToolName() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return this->m_ToolName.c_str(); } void mitk::TrackingTool::SetToolName(const char* _arg) { itkDebugMacro("setting m_ToolName to " << _arg); MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if ( _arg && (_arg == this->m_ToolName) ) { return; } if (_arg) { this->m_ToolName= _arg; } else { this->m_ToolName= ""; } this->Modified(); } void mitk::TrackingTool::SetToolName( const std::string _arg ) { this->SetToolName(_arg.c_str()); } mitk::Point3D mitk::TrackingTool::GetToolTipPosition() const { MutexLockHolder lock(*m_MyMutex); - return m_ToolTip; + return m_ToolTipPosition; } -mitk::Quaternion mitk::TrackingTool::GetToolTipOrientation() const +mitk::Quaternion mitk::TrackingTool::GetToolAxisOrientation() const { MutexLockHolder lock(*m_MyMutex); - return m_ToolTipRotation; + return m_ToolAxisOrientation; } -void mitk::TrackingTool::SetToolTip(mitk::Point3D toolTipPosition, +void mitk::TrackingTool::SetToolTipPosition(mitk::Point3D toolTipPosition, mitk::Quaternion orientation, mitk::ScalarType eps) { - if ( !Equal(m_ToolTip, toolTipPosition, eps) || - !Equal(m_ToolTipRotation, orientation, eps) ) + if ( !Equal(m_ToolTipPosition, toolTipPosition, eps) || + !Equal(m_ToolAxisOrientation, orientation, eps) ) { if( (toolTipPosition[0] == 0) && (toolTipPosition[1] == 0) && (toolTipPosition[2] == 0) && (orientation.x() == 0) && (orientation.y() == 0) && (orientation.z() == 0) && (orientation.r() == 1)) { m_ToolTipSet = false; } else { m_ToolTipSet = true; } - m_ToolTip = toolTipPosition; - m_ToolTipRotation = orientation; + m_ToolTipPosition = toolTipPosition; + m_ToolAxisOrientation = orientation; this->Modified(); } } bool mitk::TrackingTool::IsToolTipSet() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return m_ToolTipSet; } void mitk::TrackingTool::GetPosition(mitk::Point3D& position) const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if (m_ToolTipSet) { // Compute the position of tool tip in the coordinate frame of the // tracking device: Rotate the position of the tip into the tracking // device coordinate frame then add to the position of the tracking // sensor - vnl_vector pos_vnl = m_Position.GetVnlVector() + m_Orientation.rotate( m_ToolTip.GetVnlVector() ) ; + vnl_vector pos_vnl = m_Position.GetVnlVector() + m_Orientation.rotate( m_ToolTipPosition.GetVnlVector() ) ; position[0] = pos_vnl[0]; position[1] = pos_vnl[1]; position[2] = pos_vnl[2]; } else { position[0] = m_Position[0]; position[1] = m_Position[1]; position[2] = m_Position[2]; } this->Modified(); } void mitk::TrackingTool::SetPosition(mitk::Point3D position) { itkDebugMacro("setting m_Position to " << position); MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if (m_Position != position) { m_Position = position; this->Modified(); } } void mitk::TrackingTool::GetOrientation(mitk::Quaternion& orientation) const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if (m_ToolTipSet) { // Compute the orientation of the tool tip in the coordinate frame of // the tracking device. // // * m_Orientation is the orientation of the sensor relative to the transmitter - // * m_ToolTipRotation is the orientation of the tool tip relative to the sensor - orientation = m_Orientation * m_ToolTipRotation; + // * m_ToolAxisOrientation is the orientation of the tool tip relative to the sensor + orientation = m_Orientation * m_ToolAxisOrientation; } else { orientation = m_Orientation; } } void mitk::TrackingTool::SetOrientation(mitk::Quaternion orientation) { itkDebugMacro("setting m_Orientation to " << orientation); MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if (m_Orientation != orientation) { m_Orientation = orientation; this->Modified(); } } bool mitk::TrackingTool::Enable() { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if (m_Enabled == false) { this->m_Enabled = true; this->Modified(); } return true; } bool mitk::TrackingTool::Disable() { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if (m_Enabled == true) { this->m_Enabled = false; this->Modified(); } return true; } bool mitk::TrackingTool::IsEnabled() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return m_Enabled; } void mitk::TrackingTool::SetDataValid(bool isDataValid) { itkDebugMacro("setting m_DataValid to " << isDataValid); if (this->m_DataValid != isDataValid) { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex this->m_DataValid = isDataValid; this->Modified(); } } bool mitk::TrackingTool::IsDataValid() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return m_DataValid; } float mitk::TrackingTool::GetTrackingError() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return m_TrackingError; } void mitk::TrackingTool::SetTrackingError(float error) { itkDebugMacro("setting m_TrackingError to " << error); MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if (m_TrackingError != error) { m_TrackingError = error; this->Modified(); } } const char* mitk::TrackingTool::GetErrorMessage() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return this->m_ErrorMessage.c_str(); } void mitk::TrackingTool::SetErrorMessage(const char* _arg) { itkDebugMacro("setting m_ErrorMessage to " << _arg); MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if ((_arg == nullptr) || (_arg == this->m_ErrorMessage)) return; if (_arg != nullptr) this->m_ErrorMessage = _arg; else this->m_ErrorMessage = ""; this->Modified(); } diff --git a/Modules/IGT/TrackingDevices/mitkTrackingTool.h b/Modules/IGT/TrackingDevices/mitkTrackingTool.h index dab83ad3d0..f5c887e081 100644 --- a/Modules/IGT/TrackingDevices/mitkTrackingTool.h +++ b/Modules/IGT/TrackingDevices/mitkTrackingTool.h @@ -1,98 +1,105 @@ /*=================================================================== 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 MITKTRACKINGTOOL_H_HEADER_INCLUDED_ #define MITKTRACKINGTOOL_H_HEADER_INCLUDED_ #include #include #include #include #include namespace mitk { /**Documentation * \brief Interface for all Tracking Tools * * This class is a complete TrackingTool implementation. It can either be used directly by * TrackingDevices, or be subclassed for more specific implementations. * mitk::MicroBirdTrackingDevice uses this class to manage its tools. Other tracking devices * uses specialized versions of this class (e.g. mitk::NDITrackingTool) + * + * The TrackingTool class holds all coordinate transforms associated with tracking of a tool. + * The sensor attached to the tool is localized in the global tracking coordinate system (m_Position, m_Orientation). + * A tool tip (m_ToolTipPosition) can be defined in sensor coordinates. + * The tool axis defines the main axis of the tool and is defined as the negative z-axis of the tool tip coordinate system + * The main axis of the representation object of the tool (e.g. a surface) has to be defined along the negative z-axis + * \imageMacro{TrackingTool.png,"Coordinate transforms associated to the tracking tool.",5.00} * * \ingroup IGT */ class MITKIGT_EXPORT TrackingTool : public itk::Object { public: mitkClassMacroItkParent(TrackingTool, itk::Object) itkFactorylessNewMacro(Self) itkCloneMacro(Self) virtual void PrintSelf(std::ostream& os, itk::Indent indent) const override; - virtual const char* GetToolName() const; ///< every tool has a name that can be used to identify it. + virtual const char* GetToolName() const; ///< every tool has a name thatgit can be used to identify it. virtual void SetToolName(const std::string _arg); ///< Sets the name of the tool virtual void SetToolName(const char* _arg); ///< Sets the name of the tool Point3D GetToolTipPosition() const; ///< returns the tool tip in tool coordinates, which where set by SetToolTip - Quaternion GetToolTipOrientation() const; ///< returns the tool tip orientation in tool coordinates, which where set by SetToolTip - virtual void SetToolTip(Point3D toolTipPosition, Quaternion orientation, ScalarType eps=0.0); ///< defines a tool tip for this tool in tool coordinates. GetPosition() and GetOrientation() return the data of the tool tip if it is defined. By default no tooltip is defined. + Quaternion GetToolAxisOrientation() const; ///< returns the transformation of the tool axis with respect to the MITK-IGT main tool axis (0,0,-1) + virtual void SetToolTipPosition(Point3D toolTipPosition, Quaternion orientation, ScalarType eps=0.0); ///< defines a tool tip for this tool in tool coordinates. GetPosition() and GetOrientation() return the data of the tool tip if it is defined. By default no tooltip is defined. virtual bool IsToolTipSet() const; ///< returns true if a tool tip is set, false if not virtual void GetPosition(Point3D& position) const; ///< returns the current position of the tool as an array of three floats (in the tracking device coordinate system) virtual void SetPosition(Point3D position); ///< sets the position virtual void GetOrientation(Quaternion& orientation) const; ///< returns the current orientation of the tool as a quaternion in a mitk::Point4D (in the tracking device coordinate system) virtual void SetOrientation(Quaternion orientation); ///< sets the orientation as a quaternion virtual bool Enable(); ///< enables the tool, so that it will be tracked virtual bool Disable(); ///< disables the tool, so that it will not be tracked anymore virtual bool IsEnabled() const; ///< returns whether the tool is enabled or disabled virtual void SetDataValid(bool isDataValid); ///< sets if the tracking data (position & orientation) is valid virtual bool IsDataValid() const; ///< returns true if the current position data is valid (no error during tracking, tracking error below threshold, ...) virtual float GetTrackingError() const; ///< returns one value that corresponds to the overall tracking error. virtual void SetTrackingError(float error); ///< sets the tracking error virtual const char* GetErrorMessage() const; ///< if the data is not valid, ErrorMessage should contain a string explaining why it is invalid (the Set-method should be implemented in subclasses, it should not be accessible by the user) virtual void SetErrorMessage(const char* _arg); ///< sets the error message itkSetMacro(IGTTimeStamp, double) ///< Sets the IGT timestamp of the tracking tool object (time in milliseconds) itkGetConstMacro(IGTTimeStamp, double) ///< Gets the IGT timestamp of the tracking tool object (time in milliseconds). Returns 0 if the timestamp was not set. protected: TrackingTool(); virtual ~TrackingTool(); std::string m_ToolName; ///< every tool has a name that can be used to identify it. std::string m_ErrorMessage; ///< if a tool is invalid, this member should contain a human readable explanation of why it is invalid double m_IGTTimeStamp; ///< contains the time at which the tracking data was recorded itk::FastMutexLock::Pointer m_MyMutex; ///< mutex to control concurrent access to the tool - Point3D m_Position; ///< holds the position of the tool - Quaternion m_Orientation; ///< holds the orientation of the tool + Point3D m_Position; ///< holds the position of the tool in global tracking coordinates + Quaternion m_Orientation; ///< holds the orientation of the tool´in global tracking coordinates float m_TrackingError; ///< holds the tracking error of the tool bool m_Enabled; ///< if true, tool is enabled and should receive tracking updates from the tracking device bool m_DataValid; ///< if true, data in m_Position and m_Orientation is valid, e.g. true tracking data - Point3D m_ToolTip; - Quaternion m_ToolTipRotation; + Point3D m_ToolTipPosition; ///< holds the position of the tool tip in the coordinate system of the tracking sensor + Quaternion m_ToolAxisOrientation; ///< holds the rotation of the sensor coordinate system such that the z-axis coincides with the main tool axis e.g. obtained by a tool calibration bool m_ToolTipSet; }; } // namespace mitk #endif /* MITKTRACKINGTOOL_H_HEADER_INCLUDED_ */ diff --git a/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.cpp b/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.cpp index 609b8bc600..c5bf3b10ea 100644 --- a/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.cpp +++ b/Modules/IGTUI/Qmitk/QmitkNavigationToolCreationWidget.cpp @@ -1,400 +1,397 @@ /*=================================================================== 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 const std::string QmitkNavigationToolCreationWidget::VIEW_ID = "org.mitk.views.navigationtoolcreationwizardwidget"; QmitkNavigationToolCreationWidget::QmitkNavigationToolCreationWidget(QWidget* parent, Qt::WindowFlags f) : QWidget(parent, f) { m_Controls = NULL; m_ToolToBeEdited = mitk::NavigationTool::New(); m_FinalTool = mitk::NavigationTool::New(); m_ToolTransformationWidget = new QmitkInteractiveTransformationWidget(); m_Controls = nullptr; 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(); OnSurfaceUseToggled(); } QmitkNavigationToolCreationWidget::~QmitkNavigationToolCreationWidget() { m_Controls->m_CalibrationLandmarksList->SetPointSetNode(nullptr); m_Controls->m_RegistrationLandmarksList->SetPointSetNode(nullptr); 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(OnSurfaceUseToggled())); connect(m_Controls->m_Surface_Load_File, SIGNAL(toggled(bool)), this, SLOT(OnSurfaceUseToggled())); 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); 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::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())); FillUIToolLandmarkLists(m_ToolToBeEdited->GetToolControlPoints(), m_ToolToBeEdited->GetToolLandmarks()); 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) + "]"; + QString::number(m_ToolToBeEdited->GetToolAxisOrientation()[0], 'f', 2) + ", " + + QString::number(m_ToolToBeEdited->GetToolAxisOrientation()[1], 'f', 2) + ", " + + QString::number(m_ToolToBeEdited->GetToolAxisOrientation()[2], 'f', 2) + ", " + + QString::number(m_ToolToBeEdited->GetToolAxisOrientation()[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::OnSurfaceUseToggled() { if (m_Controls->m_Surface_Use_Sphere->isChecked()) m_ToolToBeEdited->SetDefaultSurface(); m_Controls->m_SurfaceChooser->setEnabled(m_Controls->m_Surface_Use_Other->isChecked()); m_Controls->m_LoadSurface->setEnabled(m_Controls->m_Surface_Load_File->isChecked()); //Global Reinit to show tool surface preview mitk::RenderingManager::GetInstance()->InitializeViewsByBoundingObjects(m_DataStorage); } void QmitkNavigationToolCreationWidget::OnLoadSurface() { std::string filename = QFileDialog::getOpenFileName(nullptr, 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(nullptr, 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->SetToolControlPoints(toolCalLandmarks); m_ToolToBeEdited->SetToolLandmarks(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); + ////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() { if (m_Controls->m_Surface_Use_Other->isChecked()) m_ToolToBeEdited->GetDataNode()->SetData(m_Controls->m_SurfaceChooser->GetSelectedNode()->GetData()); //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->SetDefaultRotation(m_ToolToBeEdited->GetToolAxisOrientation()); 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()); + m_ToolToBeEdited->SetToolAxisOrientation(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) + "]"; + QString::number(m_ToolToBeEdited->GetToolAxisOrientation()[0], 'f', 2) + ", " + + QString::number(m_ToolToBeEdited->GetToolAxisOrientation()[1], 'f', 2) + ", " + + QString::number(m_ToolToBeEdited->GetToolAxisOrientation()[2], 'f', 2) + ", " + + QString::number(m_ToolToBeEdited->GetToolAxisOrientation()[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/QmitkPolhemusTrackerWidget.cpp b/Modules/IGTUI/Qmitk/QmitkPolhemusTrackerWidget.cpp index 18bdc54b65..47de34a0b7 100644 --- a/Modules/IGTUI/Qmitk/QmitkPolhemusTrackerWidget.cpp +++ b/Modules/IGTUI/Qmitk/QmitkPolhemusTrackerWidget.cpp @@ -1,299 +1,299 @@ /*=================================================================== 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 "QmitkPolhemusTrackerWidget.h" #include #include #include #include #include #include #include #include #include "vtkRenderer.h" #include "vtkCamera.h" const std::string QmitkPolhemusTrackerWidget::VIEW_ID = "org.mitk.views.PolhemusTrackerWidget"; QmitkPolhemusTrackerWidget::QmitkPolhemusTrackerWidget(QWidget* parent, Qt::WindowFlags f) : QmitkAbstractTrackingDeviceWidget(parent, f) , m_Controls(nullptr) { } void QmitkPolhemusTrackerWidget::Initialize() { InitializeSuperclassWidget(); CreateQtPartControl(this); SetAdvancedSettingsEnabled(false); on_m_AdvancedSettings_clicked(); //hide advanced settings on setup } QmitkPolhemusTrackerWidget::~QmitkPolhemusTrackerWidget() { delete m_Controls; } void QmitkPolhemusTrackerWidget::CreateQtPartControl(QWidget *parent) { if (!m_Controls) { // create GUI widgets m_Controls = new Ui::QmitkPolhemusTrackerWidget; m_Controls->setupUi(parent); } } void QmitkPolhemusTrackerWidget::OnToolStorageChanged() { this->m_TrackingDevice = nullptr; MITK_DEBUG<<"Resetting Polhemus Tracking Device, because tool storage changed."; } void QmitkPolhemusTrackerWidget::CreateConnections() { if (m_Controls) { connect((QObject*)(m_Controls->m_hemisphereTracking), SIGNAL(clicked()), this, SLOT(on_m_hemisphereTracking_clicked())); connect((QObject*)(m_Controls->m_ToggleHemisphere), SIGNAL(clicked()), this, SLOT(on_m_ToggleHemisphere_clicked())); connect((QObject*)(m_Controls->m_SetHemisphere), SIGNAL(clicked()), this, SLOT(on_m_SetHemisphere_clicked())); connect((QObject*)(m_Controls->m_GetHemisphere), SIGNAL(clicked()), this, SLOT(on_m_GetHemisphere_clicked())); connect((QObject*)(m_Controls->m_AdjustHemisphere), SIGNAL(clicked()), this, SLOT(on_m_AdjustHemisphere_clicked())); connect((QObject*)(m_Controls->m_AdvancedSettings), SIGNAL(clicked()), this, SLOT(on_m_AdvancedSettings_clicked())); connect((QObject*)(m_Controls->m_ToggleToolTipCalibration), SIGNAL(clicked()), this, SLOT(on_m_ToggleToolTipCalibration_clicked())); } } mitk::TrackingDevice::Pointer QmitkPolhemusTrackerWidget::GetTrackingDevice() { if (m_TrackingDevice.IsNull()) { m_TrackingDevice = mitk::PolhemusTrackingDevice::New(); m_TrackingDevice->SetHemisphereTrackingEnabled(m_Controls->m_hemisphereTracking->isChecked()); } return static_cast(m_TrackingDevice); } QmitkPolhemusTrackerWidget* QmitkPolhemusTrackerWidget::Clone(QWidget* parent) const { QmitkPolhemusTrackerWidget* clonedWidget = new QmitkPolhemusTrackerWidget(parent); clonedWidget->Initialize(); return clonedWidget; } void QmitkPolhemusTrackerWidget::on_m_hemisphereTracking_clicked() { m_TrackingDevice->SetHemisphereTrackingEnabled(m_Controls->m_hemisphereTracking->isChecked()); } void QmitkPolhemusTrackerWidget::on_m_ToggleHemisphere_clicked() { // Index 0 == All Tools == -1 for Polhemus interface; Index 2 == Tool 2 == 1 for Polhemus; etc... m_TrackingDevice->ToggleHemisphere(GetSelectedToolIndex()); MITK_INFO << "Toggle Hemisphere for tool " << m_Controls->m_ToolSelection->currentText().toStdString(); } void QmitkPolhemusTrackerWidget::on_m_SetHemisphere_clicked() { mitk::Vector3D _hemisphere; mitk::FillVector3D(_hemisphere, m_Controls->m_Hemisphere_X->value(), m_Controls->m_Hemisphere_Y->value(), m_Controls->m_Hemisphere_Z->value()); m_TrackingDevice->SetHemisphere(GetSelectedToolIndex(), _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 checkbox in case it was on before, so that it can be reactivated... if (_hemisphere.GetNorm() != 0) m_Controls->m_hemisphereTracking->setChecked(false); MITK_INFO << "Hemisphere set for tool " << m_Controls->m_ToolSelection->currentText().toStdString(); } void QmitkPolhemusTrackerWidget::on_m_GetHemisphere_clicked() { mitk::Vector3D _hemisphere = m_TrackingDevice->GetHemisphere(GetSelectedToolIndex()); m_Controls->m_Hemisphere_X->setValue(_hemisphere[0]); m_Controls->m_Hemisphere_Y->setValue(_hemisphere[1]); m_Controls->m_Hemisphere_Z->setValue(_hemisphere[2]); QString label; if (m_TrackingDevice->GetHemisphereTrackingEnabled(GetSelectedToolIndex())) { label = "HemisphereTracking is ON for tool "; label.append(m_Controls->m_ToolSelection->currentText()); } else if (GetSelectedToolIndex() == -1) { label = "HemisphereTracking is OFF for at least one tool."; } else { label = "HemisphereTracking is OFF for tool "; label.append(m_Controls->m_ToolSelection->currentText()); } m_Controls->m_StatusLabelHemisphereTracking->setText(label); MITK_INFO << "Updated SpinBox for Hemisphere of tool " << m_Controls->m_ToolSelection->currentText().toStdString(); } void QmitkPolhemusTrackerWidget::on_m_AdjustHemisphere_clicked() { int _tool = GetSelectedToolIndex(); QMessageBox msgBox; QString _text; if (_tool == -1) { _text.append("Adjusting hemisphere for all tools."); msgBox.setText(_text); _text.clear(); _text = tr("Please make sure, that the entire tools (including tool tip AND sensor) are placed in the positive x hemisphere. Press 'Adjust hemisphere' if you are ready."); msgBox.setInformativeText(_text); } else { _text.append("Adjusting hemisphere for tool '"); _text.append(m_Controls->m_ToolSelection->currentText()); _text.append(tr("' at port %2.").arg(_tool)); msgBox.setText(_text); _text.clear(); _text = tr("Please make sure, that the entire tool (including tool tip AND sensor) is placed in the positive x hemisphere. Press 'Adjust hemisphere' if you are ready."); msgBox.setInformativeText(_text); } QPushButton *adjustButton = msgBox.addButton(tr("Adjust hemisphere"), QMessageBox::ActionRole); QPushButton *cancelButton = msgBox.addButton(QMessageBox::Cancel); msgBox.exec(); if (msgBox.clickedButton() == adjustButton) { // adjust m_TrackingDevice->AdjustHemisphere(_tool); MITK_INFO << "Adjusting Hemisphere for tool " << m_Controls->m_ToolSelection->currentText().toStdString(); } else if (msgBox.clickedButton() == cancelButton) { // abort MITK_INFO << "Cancel 'Adjust hemisphere'. No harm done..."; } } void QmitkPolhemusTrackerWidget::on_m_ToggleToolTipCalibration_clicked() { if (m_Controls->m_ToolSelection->currentIndex() != 0) { mitk::PolhemusTool* _tool = dynamic_cast (this->m_TrackingDevice->GetToolByName(m_Controls->m_ToolSelection->currentText().toStdString())); mitk::Point3D tip = _tool->GetToolTipPosition().GetVectorFromOrigin()*(-1.); - mitk::Quaternion quat = _tool->GetToolTipOrientation().inverse(); - _tool->SetToolTip(tip, quat); + mitk::Quaternion quat = _tool->GetToolAxisOrientation().inverse(); + _tool->SetToolTipPosition(tip, quat); } else { for (int i = 0; i < m_TrackingDevice->GetToolCount(); ++i) { mitk::PolhemusTool* _tool = dynamic_cast (this->m_TrackingDevice->GetTool(i)); mitk::Point3D tip = _tool->GetToolTipPosition().GetVectorFromOrigin()*(-1.); - mitk::Quaternion quat = _tool->GetToolTipOrientation().inverse(); - _tool->SetToolTip(tip, quat); + mitk::Quaternion quat = _tool->GetToolAxisOrientation().inverse(); + _tool->SetToolTipPosition(tip, quat); } } } void QmitkPolhemusTrackerWidget::OnConnected(bool _success) { if (!_success) { this->m_TrackingDevice = nullptr; return; } SetAdvancedSettingsEnabled(true); if (m_TrackingDevice->GetToolCount() != m_Controls->m_ToolSelection->count()) { m_Controls->m_ToolSelection->clear(); m_Controls->m_ToolSelection->addItem("All Tools"); for (int i = 0; i < m_TrackingDevice->GetToolCount(); ++i) { m_Controls->m_ToolSelection->addItem(m_TrackingDevice->GetTool(i)->GetToolName()); } } } void QmitkPolhemusTrackerWidget::OnStartTracking(bool _success) { if (!_success) return; //Rotate mitk standard multi widget, so that the view matches the sensor. Positive x == right, y == front, z == down; mitk::BaseRenderer::GetInstance(mitk::BaseRenderer::GetRenderWindowByName("stdmulti.widget4"))->GetCameraController()->SetViewToPosterior(); mitk::BaseRenderer::GetInstance(mitk::BaseRenderer::GetRenderWindowByName("stdmulti.widget4"))->GetVtkRenderer()->GetActiveCamera()->SetViewUp(0, 0, -1); } void QmitkPolhemusTrackerWidget::OnDisconnected(bool _success) { if (!_success) return; SetAdvancedSettingsEnabled(false); } void QmitkPolhemusTrackerWidget::SetAdvancedSettingsEnabled(bool _enable) { m_Controls->m_ToolSelection->setEnabled(_enable); m_Controls->label_toolsToChange->setEnabled(_enable); m_Controls->label_UpdateOnRequest->setEnabled(_enable); m_Controls->m_GetHemisphere->setEnabled(_enable); m_Controls->m_Hemisphere_X->setEnabled(_enable); m_Controls->m_Hemisphere_Y->setEnabled(_enable); m_Controls->m_Hemisphere_Z->setEnabled(_enable); m_Controls->m_SetHemisphere->setEnabled(_enable); m_Controls->m_ToggleHemisphere->setEnabled(_enable); m_Controls->m_AdjustHemisphere->setEnabled(_enable); m_Controls->m_ToggleToolTipCalibration->setEnabled(_enable); } void QmitkPolhemusTrackerWidget::on_m_AdvancedSettings_clicked() { bool _enable = m_Controls->m_AdvancedSettings->isChecked(); m_Controls->m_ToolSelection->setVisible(_enable); m_Controls->label_toolsToChange->setVisible(_enable); m_Controls->label_UpdateOnRequest->setVisible(_enable); m_Controls->m_GetHemisphere->setVisible(_enable); m_Controls->m_Hemisphere_X->setVisible(_enable); m_Controls->m_Hemisphere_Y->setVisible(_enable); m_Controls->m_Hemisphere_Z->setVisible(_enable); m_Controls->m_SetHemisphere->setVisible(_enable); m_Controls->m_ToggleHemisphere->setVisible(_enable); m_Controls->m_AdjustHemisphere->setVisible(_enable); m_Controls->m_ToggleToolTipCalibration->setVisible(_enable); m_Controls->m_StatusLabelHemisphereTracking->setVisible(_enable); } int QmitkPolhemusTrackerWidget::GetSelectedToolIndex() { // Index 0 == All Tools == -1 for Polhemus interface; Index 1 == Tool 1 == 1 for Polhemus Interface; etc... int _index = m_Controls->m_ToolSelection->currentIndex() - 1; if (_index != -1) { //we need to find the internal Polhemus index for this tool. This is stored in the identifier of a navigation tool or as Port in PolhemusTool. mitk::PolhemusTool* _tool = dynamic_cast(m_TrackingDevice->GetToolByName(m_Controls->m_ToolSelection->currentText().toStdString())); _index = _tool->GetToolPort(); } return _index; -} \ No newline at end of file +} diff --git a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibration.cpp b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibration.cpp index cf2205033a..f58fe6f3ff 100644 --- a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibration.cpp +++ b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibration.cpp @@ -1,734 +1,730 @@ /*=================================================================== 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 // Blueberry #include #include // Qmitk #include "QmitkIGTNavigationToolCalibration.h" // mitk #include #include #include #include #include #include #include // Qt #include #include //vtk #include const std::string QmitkIGTNavigationToolCalibration::VIEW_ID = "org.mitk.views.igtnavigationtoolcalibration"; QmitkIGTNavigationToolCalibration::QmitkIGTNavigationToolCalibration() { m_ToolTransformationWidget = new QmitkInteractiveTransformationWidget(); } QmitkIGTNavigationToolCalibration::~QmitkIGTNavigationToolCalibration() { //The following code is required due to a bug in the point list widget. //If this is removed, MITK crashes when closing the view: m_Controls.m_RegistrationLandmarkWidget->SetPointSetNode(nullptr); m_Controls.m_CalibrationLandmarkWidget->SetPointSetNode(nullptr); //clean up data storage this->GetDataStorage()->Remove(m_ToolTipPointPreview); delete m_ToolTransformationWidget; } void QmitkIGTNavigationToolCalibration::SetFocus() { } void QmitkIGTNavigationToolCalibration::OnToolCalibrationMethodChanged(int index) { //if pivot calibration (3) or manual(0) is chosen only calibration pointer is needed if (index == 0 || index == 3) { if (!CheckInitialization(false)) { return; } } else{ if (!CheckInitialization()) { return; } } UpdateManualToolTipCalibrationView(); m_Controls.m_CalibrationMethodsWidget->setCurrentIndex(index); m_IndexCurrentCalibrationMethod = index; } void QmitkIGTNavigationToolCalibration::CreateQtPartControl(QWidget *parent) { m_TrackingTimer = new QTimer(this); // create GUI widgets from the Qt Designer's .ui file m_Controls.setupUi(parent); connect(m_Controls.m_SetToolToCalibrate, SIGNAL(clicked()), this, SLOT(SetToolToCalibrate())); connect(m_Controls.m_SetPointer, SIGNAL(clicked()), this, SLOT(SetCalibrationPointer())); connect(m_TrackingTimer, SIGNAL(timeout()), this, SLOT(UpdateTrackingTimer())); connect(m_Controls.m_AddLandmark, SIGNAL(clicked()), this, SLOT(AddLandmark())); connect(m_Controls.m_SaveCalibratedTool, SIGNAL(clicked()), this, SLOT(SaveCalibratedTool())); connect(m_Controls.m_AddPivotPose, SIGNAL(clicked()), this, SLOT(OnAddPivotPose())); connect(m_Controls.m_ComputePivot, SIGNAL(clicked()), this, SLOT(OnComputePivot())); connect(m_Controls.m_UseComputedPivotPoint, SIGNAL(clicked()), this, SLOT(OnUseComputedPivotPoint())); connect(m_Controls.m_StartEditTooltipManually, SIGNAL(clicked()), this, SLOT(OnStartManualToolTipCalibration())); connect(m_Controls.m_GetPositions, SIGNAL(clicked()), this, SLOT(OnGetPositions())); connect(m_Controls.m_ToolAxis_X, SIGNAL(valueChanged(double)), this, SLOT(OnToolAxisSpinboxChanged())); connect(m_Controls.m_ToolAxis_Y, SIGNAL(valueChanged(double)), this, SLOT(OnToolAxisSpinboxChanged())); connect(m_Controls.m_ToolAxis_Z, SIGNAL(valueChanged(double)), this, SLOT(OnToolAxisSpinboxChanged())); connect(m_Controls.m_CalibrateToolAxis, SIGNAL(clicked()), this, SLOT(OnCalibrateToolAxis())); connect((QObject*)(m_ToolTransformationWidget), SIGNAL(EditToolTipFinished(mitk::AffineTransform3D::Pointer)), this, SLOT(OnManualEditToolTipFinished(mitk::AffineTransform3D::Pointer))); connect(m_Controls.m_CalibrationMethodComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(OnToolCalibrationMethodChanged(int))); connect((QObject*)(m_Controls.m_RunCalibrationButton), SIGNAL(clicked()), (QObject*) this, SLOT(OnRunSingleRefToolCalibrationClicked())); connect((QObject*)(m_Controls.m_CollectNavigationDataButton), SIGNAL(clicked()), (QObject*) this, SLOT(OnLoginSingleRefToolNavigationDataClicked())); connect((QObject*)(m_Controls.m_SetNewToolTipPosButton), SIGNAL(clicked()), (QObject*) this, SLOT(OnSetNewToolTipPosButtonClicked())); m_IDToolToCalibrate = -1; m_IDCalibrationPointer = -1; m_IndexCurrentCalibrationMethod = -1; m_OnLoginSingleRefToolNavigationDataClicked = false; m_NumberOfNavigationDataCounter = 0; m_NumberOfNavigationData = -1; //for pivot calibration m_OnAddPivotPoseClicked = false; PivotCount = 0; m_PivotPoses = std::vector(); m_CalibrationLandmarks = mitk::PointSet::New(); m_CalibrationLandmarksNode = mitk::DataNode::New(); m_CalibrationLandmarksNode->SetData(m_CalibrationLandmarks); m_Controls.m_CalibrationLandmarkWidget->SetPointSetNode(m_CalibrationLandmarksNode); m_RegistrationLandmarks = mitk::PointSet::New(); m_RegistrationLandmarksNode = mitk::DataNode::New(); m_RegistrationLandmarksNode->SetData(m_RegistrationLandmarks); m_Controls.m_RegistrationLandmarkWidget->SetPointSetNode(m_RegistrationLandmarksNode); m_ToolSurfaceInToolCoordinatesDataNode = mitk::DataNode::New(); m_ToolSurfaceInToolCoordinatesDataNode->SetName("ToolSurface(ToolCoordinates)"); m_LoggedNavigationDataDifferences = std::vector< mitk::NavigationData::Pointer >(); } void QmitkIGTNavigationToolCalibration::OnRunSingleRefToolCalibrationClicked() { if (!CheckInitialization()) { return; } mitk::NavigationData::Pointer ToolTipTransform = mitk::NavigationData::New(); if (m_Controls.m_CalibratePosition->isChecked()) { //1: Compute mean translational offset vector m_ResultOffsetVector.Fill(0); for (std::vector::iterator vecIter = m_LoggedNavigationDataOffsets.begin(); vecIter != m_LoggedNavigationDataOffsets.end(); vecIter++) { m_ResultOffsetVector[0] = m_ResultOffsetVector[0] + (*vecIter)[0]; m_ResultOffsetVector[1] = m_ResultOffsetVector[1] + (*vecIter)[1]; m_ResultOffsetVector[2] = m_ResultOffsetVector[2] + (*vecIter)[2]; } m_ResultOffsetVector[0] = m_ResultOffsetVector[0] / m_LoggedNavigationDataOffsets.size(); m_ResultOffsetVector[1] = m_ResultOffsetVector[1] / m_LoggedNavigationDataOffsets.size(); m_ResultOffsetVector[2] = m_ResultOffsetVector[2] / m_LoggedNavigationDataOffsets.size(); this->m_Controls.m_ResultOfCalibration->setText( QString("x: ") + QString(QString::number(m_ResultOffsetVector[0], 103, 3)) + QString("; y: ") + (QString::number(m_ResultOffsetVector[1], 103, 3)) + QString("; z: ") + (QString::number(m_ResultOffsetVector[2], 103, 3))); ToolTipTransform->SetPosition(m_ResultOffsetVector); } if (m_Controls.m_CalibrateOrientation->isChecked()) { //2: Compute mean orientation mitk::Quaternion meanOrientation; std::vector allOrientations = std::vector (); for (std::size_t i = 0; i < m_LoggedNavigationDataDifferences.size(); i++) { allOrientations.push_back(m_LoggedNavigationDataDifferences.at(i)->GetOrientation()); } meanOrientation = mitk::QuaternionAveraging::CalcAverage(allOrientations); this->m_Controls.m_ResultOfCalibrationOrientation->setText( QString("qx: ") + QString(QString::number(meanOrientation.x(), 103, 3)) + QString("; qy: ") + (QString::number(meanOrientation.y(), 103, 3)) + QString("; qz: ") + (QString::number(meanOrientation.z(), 103, 3)) + QString("; qr: ") + (QString::number(meanOrientation.r(), 103, 3))); ToolTipTransform->SetOrientation(meanOrientation); } MITK_INFO << "Computed calibration: "; MITK_INFO << "Translation Vector: " << ToolTipTransform->GetPosition(); MITK_INFO << "Quaternion: (" << ToolTipTransform->GetOrientation() << ")"; MITK_INFO << "Euler Angles [rad]: (" << ToolTipTransform->GetOrientation().rotation_euler_angles() << ")"; MITK_INFO << "Matrix:"; vnl_matrix_fixed rotMatrix = ToolTipTransform->GetOrientation().rotation_matrix_transpose(); MITK_INFO << rotMatrix[0][0] << " " << rotMatrix[0][1] << " " << rotMatrix[0][2] << std::endl; MITK_INFO << rotMatrix[1][0] << " " << rotMatrix[1][1] << " " << rotMatrix[1][2] << std::endl; MITK_INFO << rotMatrix[2][0] << " " << rotMatrix[2][1] << " " << rotMatrix[2][2] << std::endl; //3: write everything into the final tool tip transform and save it as member (it will be written to the tool later on) mitk::NavigationData::Pointer ToolTipInTrackingCoordinates = mitk::NavigationData::New(); ToolTipInTrackingCoordinates->Compose(ToolTipTransform); ToolTipInTrackingCoordinates->Compose(m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate)); ShowToolTipPreview(ToolTipInTrackingCoordinates); m_Controls.m_SetNewToolTipPosButton->setEnabled(true); m_ComputedToolTipTransformation = ToolTipTransform; } void QmitkIGTNavigationToolCalibration::OnLoginSingleRefToolNavigationDataClicked() { if (!CheckInitialization()) { return; } //reset old data m_LoggedNavigationDataOffsets.clear(); m_LoggedNavigationDataDifferences.clear(); m_OnLoginSingleRefToolNavigationDataClicked = true; m_Controls.m_CollectNavigationDataButton->setEnabled(false); m_NumberOfNavigationData = m_Controls.m_NumberOfNavigationDataToCollect->value(); MITK_INFO << "Collecting " << m_NumberOfNavigationData << " NavigationData ... " << endl; } void QmitkIGTNavigationToolCalibration::LoginSingleRefToolNavigationData() { if (!CheckInitialization()) { return; } if (m_NumberOfNavigationDataCounter < m_NumberOfNavigationData) { //update label text QString labelText = "Collecting Data: " + QString::number(m_NumberOfNavigationDataCounter); m_Controls.m_CollectionStatus->setText(labelText); mitk::NavigationData::Pointer referenceTool = m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer); mitk::NavigationData::Pointer toolToCalibrate = m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate); //compute difference: // differenceND = toolToCalibrate^-1 * referenceTool mitk::NavigationData::Pointer differenceND = mitk::NavigationData::New(); differenceND->Compose(referenceTool); differenceND->Compose(toolToCalibrate->GetInverse()); //inverse mode... if (m_Controls.m_InvertQuaternions->isChecked()) { // negate identity matrix to directly show parameters that will set up in NDI 6D Software Architect differenceND = differenceND->GetInverse(); } //save difference in member m_LoggedNavigationDataOffsets.push_back(differenceND->GetPosition()); m_LoggedNavigationDataDifferences.push_back(differenceND); m_NumberOfNavigationDataCounter++; } if (m_NumberOfNavigationDataCounter == m_NumberOfNavigationData) { m_NumberOfNavigationDataCounter = 0; m_OnLoginSingleRefToolNavigationDataClicked = false; m_Controls.m_CollectNavigationDataButton->setEnabled(true); m_Controls.m_RunCalibrationButton->setEnabled(true); MITK_INFO << "Collecting " << m_NumberOfNavigationData << " NavigationData ... Finished" << endl; QString labelText = "Collected " + QString::number(m_NumberOfNavigationData) + " data samples!"; m_Controls.m_CollectionStatus->setText(labelText); } } void QmitkIGTNavigationToolCalibration::OnSetNewToolTipPosButtonClicked() { ApplyToolTipTransform(m_ComputedToolTipTransformation); RemoveToolTipPreview(); } void QmitkIGTNavigationToolCalibration::ClearOldPivot() { mitk::NavigationData::Pointer tempND = mitk::NavigationData::New(); this->ApplyToolTipTransform(tempND); UpdateManualToolTipCalibrationView(); //m_ManualToolTipEditWidget->hide(); //TODO this->GetDataStorage()->Remove(m_ToolSurfaceInToolCoordinatesDataNode); } void QmitkIGTNavigationToolCalibration::OnAddPivotPose() { ClearOldPivot(); //When the collect Poses Button is Clicked m_OnAddPivotPoseClicked = true; m_NumberOfNavigationData = m_Controls.m_PosesToCollect->value(); } void QmitkIGTNavigationToolCalibration::AddPivotPose() { //Save the poses to be used in computation if (PivotCount < m_NumberOfNavigationData) { mitk::NavigationData::Pointer currentPose = mitk::NavigationData::New(); currentPose->Graft(m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource()->GetOutput(m_IDToolToCalibrate)); m_PivotPoses.push_back(currentPose); m_Controls.m_PoseNumber->setText(QString::number(m_PivotPoses.size())); PivotCount++; } if (PivotCount == m_NumberOfNavigationData) { m_OnAddPivotPoseClicked = false; } } void QmitkIGTNavigationToolCalibration::OnComputePivot() { mitk::PivotCalibration::Pointer myPivotCalibration = mitk::PivotCalibration::New(); for (std::size_t i = 0; i < this->m_PivotPoses.size(); i++) { myPivotCalibration->AddNavigationData(m_PivotPoses.at(i)); } QString resultString; if (myPivotCalibration->ComputePivotResult()) { mitk::NavigationData::Pointer markerTransformationTrackingCoordinates = m_PivotPoses.at(0); //Get computed pivot transfromation in tool coordinates mitk::NavigationData::Pointer ToolTipToTool = mitk::NavigationData::New(); ToolTipToTool->SetPosition(myPivotCalibration->GetResultPivotPoint()); ToolTipToTool->SetOrientation(myPivotCalibration->GetResultPivotRotation()); mitk::NavigationData::Pointer TrackerToTool = mitk::NavigationData::New(); TrackerToTool->SetOrientation(markerTransformationTrackingCoordinates->GetOrientation()); TrackerToTool->SetPosition(markerTransformationTrackingCoordinates->GetPosition()); TrackerToTool->Compose(ToolTipToTool); // Compute pivot point in relation to marker transformation for preview mitk::NavigationData::Pointer ToolTipToTracker = mitk::NavigationData::New(); ToolTipToTracker->Compose(ToolTipToTool); ToolTipToTracker->Compose(markerTransformationTrackingCoordinates); //add the preview node to the data storage ShowToolTipPreview(ToolTipToTracker); //parse result string resultString = QString("Pivot computation succeeded!\n") + QString("RMS Error: ") + QString::number(myPivotCalibration->GetResultRMSError()) + QString("\n") + QString("Pivot Point: ") + QString::number(myPivotCalibration->GetResultPivotPoint()[0]) + ";" + QString::number(myPivotCalibration->GetResultPivotPoint()[1]) + ";" + QString::number(myPivotCalibration->GetResultPivotPoint()[2]) + QString("\n") + QString("Pivot Rotation: ") + QString::number(myPivotCalibration->GetResultPivotRotation()[0]) + ";" + QString::number(myPivotCalibration->GetResultPivotRotation()[1]) + ";" + QString::number(myPivotCalibration->GetResultPivotRotation()[2]) + ";" + QString::number(myPivotCalibration->GetResultPivotRotation()[3]) + QString("\n"); //finally: save results to member variable m_ComputedToolTipTransformation = ToolTipToTool; //enable button to use the computed point with the tool m_Controls.m_UseComputedPivotPoint->setEnabled(true); } else { resultString = "Pivot computation failed!"; } MITK_INFO << resultString.toStdString().c_str(); m_Controls.m_ResultText->setText(resultString); } void QmitkIGTNavigationToolCalibration::UpdatePivotCount() { PivotCount = 0; while (!m_PivotPoses.empty()) { m_PivotPoses.pop_back(); } m_Controls.m_PoseNumber->setText(QString::number(PivotCount)); } void QmitkIGTNavigationToolCalibration::OnUseComputedPivotPoint() { RemoveToolTipPreview(); QString resultString = QString("Pivoted tool tip transformation was written to the tool ") + m_ToolToCalibrate->GetToolName().c_str(); ApplyToolTipTransform(m_ComputedToolTipTransformation, resultString.toStdString()); m_Controls.m_ResultText->setText(resultString); UpdatePivotCount(); } void QmitkIGTNavigationToolCalibration::ApplyToolTipTransform(mitk::NavigationData::Pointer ToolTipTransformInToolCoordinates, std::string message) { if (!CheckInitialization(false)) { return; } //Update tool in tool storage m_ToolToCalibrate->SetToolTipPosition(ToolTipTransformInToolCoordinates->GetPosition()); - m_ToolToCalibrate->SetToolTipOrientation(ToolTipTransformInToolCoordinates->GetOrientation()); + m_ToolToCalibrate->SetToolAxisOrientation(ToolTipTransformInToolCoordinates->GetOrientation()); //And also update tracking device, so the transform is directly used mitk::TrackingDeviceSource::Pointer trackingDeviceSource; try { trackingDeviceSource = dynamic_cast(m_NavigationDataSourceOfToolToCalibrate.GetPointer()); mitk::TrackingTool::Pointer TrackingToolToCalibrate = trackingDeviceSource->GetTrackingDevice()->GetTool(m_IDToolToCalibrate); - TrackingToolToCalibrate->SetToolTip(ToolTipTransformInToolCoordinates->GetPosition(), ToolTipTransformInToolCoordinates->GetOrientation()); + TrackingToolToCalibrate->SetToolTipPosition(ToolTipTransformInToolCoordinates->GetPosition(), ToolTipTransformInToolCoordinates->GetOrientation()); } catch (std::exception& e) { MITK_ERROR << "Error while trying to set the tool tip to the running tracking device. Aborting! (" << e.what() << ")"; } MITK_INFO << message; } void QmitkIGTNavigationToolCalibration::ShowToolTipPreview(mitk::NavigationData::Pointer ToolTipInTrackingCoordinates) { if(m_ToolTipPointPreview.IsNull()) { m_ToolTipPointPreview = mitk::DataNode::New(); m_ToolTipPointPreview->SetName("Modified Tool Tip Preview"); mitk::Color blue; blue.SetBlue(1); m_ToolTipPointPreview->SetColor(blue); mitk::Surface::Pointer mySphere = mitk::Surface::New(); vtkSmartPointer vtkData = vtkSmartPointer::New(); vtkData->SetRadius(3.0f); vtkData->SetCenter(0.0, 0.0, 0.0); vtkData->Update(); mySphere->SetVtkPolyData(vtkData->GetOutput()); m_ToolTipPointPreview->SetData(mySphere); this->GetDataStorage()->Add(m_ToolTipPointPreview); } m_ToolTipPointPreview->GetData()->GetGeometry()->SetIndexToWorldTransform(ToolTipInTrackingCoordinates->GetAffineTransform3D()); } void QmitkIGTNavigationToolCalibration::RemoveToolTipPreview() { this->GetDataStorage()->Remove(m_ToolTipPointPreview.GetPointer()); } void QmitkIGTNavigationToolCalibration::UpdateManualToolTipCalibrationView() { if (m_ToolToCalibrate.IsNull()) { return; } //parse human readable transformation data and display it std::stringstream translation; std::stringstream orientation; translation << m_ToolToCalibrate->GetToolTipPosition(); - orientation << "Quaternion: (" << m_ToolToCalibrate->GetToolTipOrientation() << ")" << std::endl; + orientation << "Quaternion: (" << m_ToolToCalibrate->GetToolAxisOrientation() << ")" << std::endl; orientation << std::endl; - orientation << "Euler Angles [rad]: (" << m_ToolToCalibrate->GetToolTipOrientation().rotation_euler_angles() << ")" << std::endl; + orientation << "Euler Angles [rad]: (" << m_ToolToCalibrate->GetToolAxisOrientation().rotation_euler_angles() << ")" << std::endl; orientation << std::endl; orientation << "Matrix:" << std::endl; - vnl_matrix_fixed rotMatrix = m_ToolToCalibrate->GetToolTipOrientation().rotation_matrix_transpose(); + vnl_matrix_fixed rotMatrix = m_ToolToCalibrate->GetToolAxisOrientation().rotation_matrix_transpose(); orientation << rotMatrix[0][0] << " " << rotMatrix[0][1] << " " << rotMatrix[0][2] << std::endl; orientation << rotMatrix[1][0] << " " << rotMatrix[1][1] << " " << rotMatrix[1][2] << std::endl; orientation << rotMatrix[2][0] << " " << rotMatrix[2][1] << " " << rotMatrix[2][2] << std::endl; m_Controls.m_ManualCurrentTranslation->setText(translation.str().c_str()); m_Controls.m_ManualCurrentOrientation->setPlainText(orientation.str().c_str()); } void QmitkIGTNavigationToolCalibration::OnStartManualToolTipCalibration() { if (!CheckInitialization(false)) { return; } m_ToolTransformationWidget->SetToolToEdit(m_ToolToCalibrate); m_ToolTransformationWidget->SetDefaultOffset(m_ToolToCalibrate->GetToolTipPosition()); - m_ToolTransformationWidget->SetDefaultRotation(m_ToolToCalibrate->GetToolTipOrientation()); + m_ToolTransformationWidget->SetDefaultRotation(m_ToolToCalibrate->GetToolAxisOrientation()); m_ToolTransformationWidget->open(); } void QmitkIGTNavigationToolCalibration::OnManualEditToolTipFinished(mitk::AffineTransform3D::Pointer toolTip) { //This function is called, when the toolTipEdit view is closed. //if user pressed cancle, nullptr is returned. Do nothing. Else, set values. if (toolTip) { mitk::NavigationData::Pointer tempND = mitk::NavigationData::New(toolTip);//Convert to Navigation data for simple transversion to quaternion QString resultString = QString("Manual edited values are written to ") + m_ToolToCalibrate->GetToolName().c_str(); ApplyToolTipTransform(tempND, resultString.toStdString()); m_Controls.m_ResultText->setText(resultString); } UpdateManualToolTipCalibrationView(); } void QmitkIGTNavigationToolCalibration::OnGetPositions() { if (!CheckInitialization(true)) { return; } //Navigation Data from Tool which should be calibrated if (!m_AxisCalibration_ToolToCalibrate) m_AxisCalibration_ToolToCalibrate = mitk::NavigationData::New(); m_AxisCalibration_ToolToCalibrate->Graft(m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource()->GetOutput(m_IDToolToCalibrate)); //Navigation Data from calibration pointer tool if (!m_AxisCalibration_NavDataCalibratingTool) m_AxisCalibration_NavDataCalibratingTool = mitk::NavigationData::New(); m_AxisCalibration_NavDataCalibratingTool->Graft(m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource()->GetOutput(m_IDCalibrationPointer)); MITK_DEBUG << "Positions for tool axis calibration:"; MITK_DEBUG << " ToolTip: " << m_AxisCalibration_ToolToCalibrate->GetPosition() << ","; MITK_DEBUG << " Rotation: \n" << m_AxisCalibration_ToolToCalibrate->GetRotationMatrix(); MITK_DEBUG << " End of the tool: " << m_AxisCalibration_NavDataCalibratingTool->GetPosition(); QString _label = "Position recorded: " + QString::number(m_AxisCalibration_NavDataCalibratingTool->GetPosition()[0], 'f', 1) + ", " + QString::number(m_AxisCalibration_NavDataCalibratingTool->GetPosition()[1], 'f', 1) + ", " + QString::number(m_AxisCalibration_NavDataCalibratingTool->GetPosition()[2], 'f', 1); m_Controls.m_ToolAxisPositionLabel->setText(_label); } void QmitkIGTNavigationToolCalibration::OnCalibrateToolAxis() { if (!m_AxisCalibration_ToolToCalibrate || !m_AxisCalibration_NavDataCalibratingTool) { MITK_ERROR << "Please record position first."; return; } //Calculate the tool tip //here is an explanation, what is happening here: /* The axis is equal to the (tool tip) minus the (end of the tool) in tool coordinates of the tool which should be calibrated. - The tool tip in tool coordinates is zero (definition of the tip). - The end of the tool is recorded by the calibration pointer's position and is transformed using the inverse of the tool which should be calibrated. + The tool tip is defined as the origin of the tool coordinate system. + The end of the tool is recorded by the calibration pointer's position and is transformed into the coordinate system of the tool to be calibrated Normalize it. */ + //m_CalibratedToolAxis = -m_AxisCalibration_ToolToCalibrate->TransformPoint(m_AxisCalibration_NavDataCalibratingTool->GetPosition()).GetVectorFromOrigin(); m_CalibratedToolAxis = -m_AxisCalibration_ToolToCalibrate->GetInverse()->TransformPoint(m_AxisCalibration_NavDataCalibratingTool->GetPosition()).GetVectorFromOrigin(); MITK_DEBUG << "Tool Endpoint in Tool coordinates: " << m_CalibratedToolAxis; m_CalibratedToolAxis.Normalize(); MITK_DEBUG << "Tool Axis: " << m_CalibratedToolAxis; m_ToolToCalibrate->SetToolAxis(m_CalibratedToolAxis); + // Update TrackingTool + m_ComputedToolTipTransformation->SetPosition(m_ToolToCalibrate->GetToolTipPosition()); + m_ComputedToolTipTransformation->SetOrientation(m_ToolToCalibrate->GetToolAxisOrientation()); + ApplyToolTipTransform(m_ComputedToolTipTransformation); + //Update GUI QString calibratedToolAxisString = "Tool Axis: " + QString::number(m_CalibratedToolAxis.GetElement(0), 'f', 3) + ", " + QString::number(m_CalibratedToolAxis.GetElement(1), 'f', 3) + ", " + QString::number(m_CalibratedToolAxis.GetElement(2), 'f', 3); m_Controls.m_ToolAxisCalibrationLabel->setText(calibratedToolAxisString); //Block QT signals, we don't want to emit SpinboxChanged on the first value to overwrite the next ones m_Controls.m_ToolAxis_X->blockSignals(true); m_Controls.m_ToolAxis_Y->blockSignals(true); m_Controls.m_ToolAxis_Z->blockSignals(true); m_Controls.m_ToolAxis_X->setValue(m_CalibratedToolAxis[0]); m_Controls.m_ToolAxis_Y->setValue(m_CalibratedToolAxis[1]); m_Controls.m_ToolAxis_Z->setValue(m_CalibratedToolAxis[2]); m_Controls.m_ToolAxis_X->blockSignals(false); m_Controls.m_ToolAxis_Y->blockSignals(false); m_Controls.m_ToolAxis_Z->blockSignals(false); } void QmitkIGTNavigationToolCalibration::OnToolAxisSpinboxChanged() { m_CalibratedToolAxis.SetElement(0, m_Controls.m_ToolAxis_X->value()); m_CalibratedToolAxis.SetElement(1, m_Controls.m_ToolAxis_Y->value()); m_CalibratedToolAxis.SetElement(2, m_Controls.m_ToolAxis_Z->value()); m_ToolToCalibrate->SetToolAxis(m_CalibratedToolAxis); MITK_INFO << "Tool axis changed to " << m_CalibratedToolAxis; } void QmitkIGTNavigationToolCalibration::SetToolToCalibrate() { m_IDToolToCalibrate = m_Controls.m_SelectionWidget->GetSelectedToolID(); if (m_IDToolToCalibrate == -1) //no valid tool to calibrate { m_Controls.m_CalToolLabel->setText(""); m_Controls.m_StatusWidgetToolToCalibrate->RemoveStatusLabels(); m_TrackingTimer->stop(); } else { m_ToolToCalibrate = m_Controls.m_SelectionWidget->GetSelectedNavigationTool(); m_NavigationDataSourceOfToolToCalibrate = m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource(); m_Controls.m_CalToolLabel->setText(m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate)->GetName()); //initialize widget m_Controls.m_StatusWidgetToolToCalibrate->RemoveStatusLabels(); m_Controls.m_StatusWidgetToolToCalibrate->SetShowPositions(true); m_Controls.m_StatusWidgetToolToCalibrate->SetTextAlignment(Qt::AlignLeft); m_Controls.m_StatusWidgetToolToCalibrate->AddNavigationData(m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate)); m_Controls.m_StatusWidgetToolToCalibrate->ShowStatusLabels(); //initialize manual tool tip calibration view UpdateManualToolTipCalibrationView(); //save tool surface in tool coordinates for further editing mitk::Surface::Pointer ToolSurface = dynamic_cast(m_ToolToCalibrate->GetDataNode()->GetData())->Clone(); m_ToolSurfaceInToolCoordinatesDataNode->SetData(ToolSurface); m_ToolSurfaceInToolCoordinatesDataNode->GetData()->GetGeometry()->SetIdentity(); - //Set the default needle axis - m_CalibratedToolAxis = m_ToolToCalibrate->GetToolAxis().GetVectorFromOrigin(); - //Block QT signals, we don't want to emit SpinboxChanged on the first value to overwrite the next ones - m_Controls.m_ToolAxis_X->blockSignals(true); m_Controls.m_ToolAxis_Y->blockSignals(true); m_Controls.m_ToolAxis_Z->blockSignals(true); - m_Controls.m_ToolAxis_X->setValue(m_CalibratedToolAxis[0]); - m_Controls.m_ToolAxis_Y->setValue(m_CalibratedToolAxis[1]); - m_Controls.m_ToolAxis_Z->setValue(m_CalibratedToolAxis[2]); - m_Controls.m_ToolAxis_X->blockSignals(false); m_Controls.m_ToolAxis_Y->blockSignals(false); m_Controls.m_ToolAxis_Z->blockSignals(false); - UpdateManualToolTipCalibrationView(); //start updating timer for status widgets, etc. if (!m_TrackingTimer->isActive()) m_TrackingTimer->start(100); } } void QmitkIGTNavigationToolCalibration::SetCalibrationPointer() { m_IDCalibrationPointer = m_Controls.m_SelectionWidget->GetSelectedToolID(); if (m_IDCalibrationPointer == -1) { m_Controls.m_PointerLabel->setText(""); m_Controls.m_StatusWidgetCalibrationPointer->RemoveStatusLabels(); m_TrackingTimer->stop(); } else { m_NavigationDataSourceOfCalibrationPointer = m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource(); m_Controls.m_PointerLabel->setText(m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer)->GetName()); //initialize widget m_Controls.m_StatusWidgetCalibrationPointer->RemoveStatusLabels(); m_Controls.m_StatusWidgetCalibrationPointer->SetShowPositions(true); m_Controls.m_StatusWidgetCalibrationPointer->SetTextAlignment(Qt::AlignLeft); m_Controls.m_StatusWidgetCalibrationPointer->AddNavigationData(m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer)); m_Controls.m_StatusWidgetCalibrationPointer->ShowStatusLabels(); if (!m_TrackingTimer->isActive()) m_TrackingTimer->start(100); } } void QmitkIGTNavigationToolCalibration::UpdateOffsetCoordinates() { if (m_NavigationDataSourceOfCalibrationPointer.IsNull() || m_NavigationDataSourceOfToolToCalibrate.IsNull()) { return; } mitk::NavigationData::Pointer referenceToolND = m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer); mitk::NavigationData::Pointer toolToCalibrateND = m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate); if (referenceToolND->IsDataValid() && toolToCalibrateND->IsDataValid()) { //computation: difference between both tools (in tool coordinates) //differenceND = toolToCalibrateND^-1 * referenceToolND mitk::NavigationData::Pointer differenceND = mitk::NavigationData::New(); differenceND->Compose(referenceToolND); differenceND->Compose(toolToCalibrateND->GetInverse()); //display this orientation in the UI m_Controls.m_OffsetCoordinates->setText( QString("x: ") + QString(QString::number(differenceND->GetPosition()[0], 103, 3)) + QString("; y: ") + (QString::number(differenceND->GetPosition()[1], 103, 3)) + QString("; z: ") + (QString::number(differenceND->GetPosition()[2], 103, 3))); m_Controls.m_OrientationOffsetCoordinates->setText( QString("qx: ") + QString(QString::number(differenceND->GetOrientation().x(), 103, 3)) + QString("; qy: ") + (QString::number(differenceND->GetOrientation().y(), 103, 3)) + QString("; qz: ") + (QString::number(differenceND->GetOrientation().z(), 103, 3)) + QString("; qr: ") + (QString::number(differenceND->GetOrientation().r(), 103, 3))); //also update preview if active if (m_ToolTipPointPreview.IsNotNull()) //NOT WORKING! TODO: fix or remove! { mitk::NavigationData::Pointer ToolTipTransform = mitk::NavigationData::New(); ToolTipTransform->SetPosition(m_ResultOffsetVector); mitk::NavigationData::Pointer ToolTipInTrackingCoordinates = mitk::NavigationData::New(); //maybe store as for better peformance... ToolTipInTrackingCoordinates->Compose(m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate)); ToolTipInTrackingCoordinates->Compose(ToolTipTransform); m_ToolTipPointPreview->GetData()->GetGeometry()->SetIndexToWorldTransform(ToolTipInTrackingCoordinates->GetAffineTransform3D()); } } } void QmitkIGTNavigationToolCalibration::UpdateTrackingTimer() { m_Controls.m_StatusWidgetToolToCalibrate->Refresh(); m_Controls.m_StatusWidgetCalibrationPointer->Refresh(); if (m_OnLoginSingleRefToolNavigationDataClicked) LoginSingleRefToolNavigationData(); if (m_OnAddPivotPoseClicked) AddPivotPose(); // 1 == Single Reference Calibration Method if (m_IndexCurrentCalibrationMethod == 1) UpdateOffsetCoordinates(); } void QmitkIGTNavigationToolCalibration::AddLandmark() { if (!CheckInitialization()) { return; } mitk::NavigationData::Pointer navDataTool = m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate); mitk::Point3D landmark = m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer)->GetPosition(); //convert to itk transform itk::Vector translation; for (int k = 0; k < 3; k++) translation[k] = navDataTool->GetPosition()[k]; itk::Matrix rotation; for (int k = 0; k < 3; k++) for (int l = 0; l < 3; l++) rotation[k][l] = navDataTool->GetOrientation().rotation_matrix_transpose()[k][l]; rotation = rotation.GetTranspose(); itk::Vector landmarkItk; landmarkItk[0] = landmark[0]; landmarkItk[1] = landmark[1]; landmarkItk[2] = landmark[2]; //compute landmark in tool coordinates itk::Matrix rotationInverse; for (int k = 0; k < 3; k++) for (int l = 0; l < 3; l++) rotationInverse[k][l] = rotation.GetInverse()[k][l]; landmarkItk = rotationInverse * (landmarkItk - translation); //convert back and add landmark to pointset landmark[0] = landmarkItk[0]; landmark[1] = landmarkItk[1]; landmark[2] = landmarkItk[2]; m_RegistrationLandmarks->InsertPoint(m_RegistrationLandmarks->GetSize(), landmark); } void QmitkIGTNavigationToolCalibration::SaveCalibratedTool() { if (m_ToolToCalibrate.IsNotNull()) { mitk::NavigationTool::Pointer calibratedTool = m_ToolToCalibrate; calibratedTool->SetToolControlPoints(this->m_CalibrationLandmarks); calibratedTool->SetToolLandmarks(this->m_RegistrationLandmarks); mitk::NavigationToolWriter::Pointer myWriter = mitk::NavigationToolWriter::New(); std::string filename = QFileDialog::getSaveFileName(nullptr,tr("Save Navigation Tool"), "/", "*.IGTTool").toUtf8().data(); - filename.append(".IGTTool"); if (filename == "") return; if (myWriter->DoWrite(filename, calibratedTool)) MITK_INFO << "Saved calibrated tool to file " << filename; else MITK_WARN << "Can't write tool to file " << filename; } else { MITK_ERROR << "Did not find navigation tool storage of calibrated tool, aborting!"; } } bool QmitkIGTNavigationToolCalibration::CheckInitialization(bool CalibrationPointerRequired) { if ((m_IDToolToCalibrate == -1) || ((CalibrationPointerRequired) && (m_IDCalibrationPointer == -1) ) ) { QMessageBox msgBox; msgBox.setText("Tool to calibrate and/or calibration pointer not initialized, cannot proceed!"); msgBox.exec(); return false; } else { return true; } }