diff --git a/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp b/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp index 2ba10e96fa..74af99e2cb 100644 --- a/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp +++ b/Modules/IGT/TrackingDevices/mitkTrackingTool.cpp @@ -1,288 +1,289 @@ /*=================================================================== 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; } 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 << "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::GetToolTip() const +mitk::Point3D mitk::TrackingTool::GetToolTipPosition() const { + MutexLockHolder lock(*m_MyMutex); return m_ToolTip; } +mitk::Quaternion mitk::TrackingTool::GetToolTipOrientation() const +{ + MutexLockHolder lock(*m_MyMutex); + return m_ToolTipRotation; +} + void mitk::TrackingTool::SetToolTip(mitk::Point3D toolTipPosition, mitk::Quaternion orientation, mitk::ScalarType eps) { if ( !Equal(m_ToolTip, 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_ToolTipRotation = orientation; this->Modified(); } } bool mitk::TrackingTool::IsToolTipSet() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return m_ToolTipSet; } -mitk::Quaternion mitk::TrackingTool::GetToolTipOrientation() const -{ - MutexLockHolder lock(*m_MyMutex); - return m_ToolTipRotation; -} - 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() ) ; 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 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 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 _arg) { itkDebugMacro("setting m_DataValid to " << _arg); if (this->m_DataValid != _arg) { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex this->m_DataValid = _arg; 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 float r = m_TrackingError; return r; } void mitk::TrackingTool::SetTrackingError(float error) { itkDebugMacro("setting m_TrackingError to " << error); MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if (error == m_TrackingError) { return; } 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 4aff1058eb..c8818804f5 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 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 GetToolTip() const; ///< returns the tool tip in tool coordinates, which where set by SetToolTip + 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. virtual bool IsToolTipSet() const; ///< returns true if a tool tip is set, false if not - Quaternion GetToolTipOrientation() const; ///< returns the tool tip orientation in tool coordinates, which where set by SetToolTip 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 _arg); ///< 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; Quaternion m_ToolTipRotation; bool m_ToolTipSet; }; } // namespace mitk #endif /* MITKTRACKINGTOOL_H_HEADER_INCLUDED_ */