diff --git a/Modules/IGT/TrackingDevices/mitkMicronTrackerTypeInformation.cpp b/Modules/IGT/TrackingDevices/mitkMicronTrackerTypeInformation.cpp index d7b0dba5e7..a62d339590 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->GetToolAxisTransform()); + thisDevice->GetTool(i)->SetToolTipPosition(thisNavigationTool->GetToolTipPosition(), thisNavigationTool->GetToolAxisTransform()); } returnValue->SetTrackingDevice(thisDevice); return returnValue; } } diff --git a/Modules/IGT/TrackingDevices/mitkNDIAuroraTypeInformation.cpp b/Modules/IGT/TrackingDevices/mitkNDIAuroraTypeInformation.cpp index 129f6c44df..1aac45aa4b 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)->GetToolAxisTransform()); + dynamic_cast(thisDevice->GetTool(i))->SetToolTipPosition(navigationTools->GetTool(j)->GetToolTipPosition(), navigationTools->GetTool(j)->GetToolAxisTransform()); //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 ec5dffd442..1eb02438f9 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->GetToolAxisTransform()); + thisDevice->GetTool(i)->SetToolTipPosition(thisNavigationTool->GetToolTipPosition(), thisNavigationTool->GetToolAxisTransform()); } returnValue->SetTrackingDevice(thisDevice); return returnValue; } } diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp index 91eb6af98d..5fe89eacbf 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->GetToolAxisTransform()); + thisDevice->GetTool(i)->SetToolTipPosition(thisNavigationTool->GetToolTipPosition(), thisNavigationTool->GetToolAxisTransform()); } returnValue->SetTrackingDevice(thisDevice); return returnValue; } } diff --git a/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp b/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp index 39f4f80979..4d4d18129d 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_ToolTipPosition[0] = 0.0f; + m_ToolTipPosition[1] = 0.0f; + m_ToolTipPosition[2] = 0.0f; m_ToolTipRotation[0] = 0.0f; m_ToolTipRotation[1] = 0.0f; m_ToolTipRotation[2] = 0.0f; m_ToolTipRotation[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 << "ToolTip: " << m_ToolTipPosition << std::endl; os << indent << "ToolTipRotation: " << m_ToolTipRotation << 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::GetToolAxisTransform() const { MutexLockHolder lock(*m_MyMutex); return m_ToolTipRotation; } -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) || + if ( !Equal(m_ToolTipPosition, toolTipPosition, eps) || !Equal(m_ToolTipRotation, 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_ToolTipPosition = toolTipPosition; m_ToolTipRotation = 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; } 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 cab84c83de..ae0284abe7 100644 --- a/Modules/IGT/TrackingDevices/mitkTrackingTool.h +++ b/Modules/IGT/TrackingDevices/mitkTrackingTool.h @@ -1,98 +1,98 @@ /*=================================================================== 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) * * \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 GetToolAxisTransform() const; ///< returns the transformation of the tool axis with respect to the MITK-IGT main tool axis (0,0,-1) - 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. + 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 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; + Point3D m_ToolTipPosition; Quaternion m_ToolTipRotation; bool m_ToolTipSet; }; } // namespace mitk #endif /* MITKTRACKINGTOOL_H_HEADER_INCLUDED_ */ diff --git a/Modules/IGTUI/Qmitk/QmitkPolhemusTrackerWidget.cpp b/Modules/IGTUI/Qmitk/QmitkPolhemusTrackerWidget.cpp index 4c6fc0d0cc..49fb51d473 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->GetToolAxisTransform().inverse(); - _tool->SetToolTip(tip, quat); + _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->GetToolAxisTransform().inverse(); - _tool->SetToolTip(tip, quat); + _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; } 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 e4b36e6407..094d726b29 100644 --- a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibration.cpp +++ b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibration.cpp @@ -1,726 +1,726 @@ /*=================================================================== 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->SetToolAxisTransform(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->GetToolAxisTransform() << ")" << std::endl; orientation << std::endl; orientation << "Euler Angles [rad]: (" << m_ToolToCalibrate->GetToolAxisTransform().rotation_euler_angles() << ")" << std::endl; orientation << std::endl; orientation << "Matrix:" << std::endl; vnl_matrix_fixed rotMatrix = m_ToolToCalibrate->GetToolAxisTransform().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->GetToolAxisTransform()); 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 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 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(); 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->SetToolCalibrationLandmarks(this->m_CalibrationLandmarks); calibratedTool->SetToolRegistrationLandmarks(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; } }