diff --git a/Modules/IGT/IGTTrackingDevices/mitkInternalTrackingTool.cpp b/Modules/IGT/IGTTrackingDevices/mitkInternalTrackingTool.cpp index 70ed507ff6..fca5eceae2 100644 --- a/Modules/IGT/IGTTrackingDevices/mitkInternalTrackingTool.cpp +++ b/Modules/IGT/IGTTrackingDevices/mitkInternalTrackingTool.cpp @@ -1,241 +1,254 @@ /*=================================================================== 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 "mitkInternalTrackingTool.h" #include typedef itk::MutexLockHolder MutexLockHolder; mitk::InternalTrackingTool::InternalTrackingTool() : TrackingTool(), 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::InternalTrackingTool::~InternalTrackingTool() { } void mitk::InternalTrackingTool::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::InternalTrackingTool::SetToolName( const std::string _arg ) { this->SetToolName(_arg.c_str()); } void mitk::InternalTrackingTool::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.Get_vnl_vector() + m_Orientation.rotate( m_ToolTip.Get_vnl_vector() ) ; 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::InternalTrackingTool::SetPosition(mitk::Point3D position) +void mitk::InternalTrackingTool::SetPosition(mitk::Point3D position, mitk::ScalarType eps) { itkDebugMacro("setting m_Position to " << position); - MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex - m_Position = position; - this->Modified(); + if (!Equal(m_Position, position, eps)) + { + MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex + m_Position = position; + this->Modified(); + } } void mitk::InternalTrackingTool::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::InternalTrackingTool::SetToolTip(mitk::Point3D toolTipPosition, mitk::Quaternion orientation) +void mitk::InternalTrackingTool::SetToolTip(mitk::Point3D toolTipPosition, + mitk::Quaternion orientation, + mitk::ScalarType eps) { -if( (toolTipPosition[0] == 0) && - (toolTipPosition[1] == 0) && - (toolTipPosition[2] == 0) && - (orientation.x() == 0) && - (orientation.y() == 0) && - (orientation.z() == 0) && - (orientation.r() == 1)) + 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; + m_ToolTipSet = false; } -else + else { - m_ToolTipSet = true; + m_ToolTipSet = true; } -m_ToolTip = toolTipPosition; -m_ToolTipRotation = orientation; + m_ToolTip = toolTipPosition; + m_ToolTipRotation = orientation; + this->Modified(); + } } -void mitk::InternalTrackingTool::SetOrientation(mitk::Quaternion orientation) +void mitk::InternalTrackingTool::SetOrientation(mitk::Quaternion orientation, mitk::ScalarType eps) { itkDebugMacro("setting m_Orientation to " << orientation); - MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex - m_Orientation = orientation; - this->Modified(); + if (!Equal(m_Orientation, orientation, eps)) + { + MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex + m_Orientation = orientation; + this->Modified(); + } } void mitk::InternalTrackingTool::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(); } float mitk::InternalTrackingTool::GetTrackingError() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex float r = m_TrackingError; return r; } bool mitk::InternalTrackingTool::Enable() { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if (m_Enabled == false) { this->m_Enabled = true; this->Modified(); } return true; } bool mitk::InternalTrackingTool::Disable() { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if (m_Enabled == true) { this->m_Enabled = false; this->Modified(); } return true; } bool mitk::InternalTrackingTool::IsEnabled() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return m_Enabled; } bool mitk::InternalTrackingTool::IsDataValid() const { MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex return m_DataValid; } void mitk::InternalTrackingTool::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(); } } void mitk::InternalTrackingTool::SetErrorMessage(const char* _arg) { itkDebugMacro("setting m_ErrorMessage to " << _arg); MutexLockHolder lock(*m_MyMutex); // lock and unlock the mutex if ((_arg == NULL) || (_arg == this->m_ErrorMessage)) return; if (_arg != NULL) this->m_ErrorMessage = _arg; else this->m_ErrorMessage = ""; this->Modified(); } diff --git a/Modules/IGT/IGTTrackingDevices/mitkInternalTrackingTool.h b/Modules/IGT/IGTTrackingDevices/mitkInternalTrackingTool.h index aab5a232d7..91b3deea4f 100644 --- a/Modules/IGT/IGTTrackingDevices/mitkInternalTrackingTool.h +++ b/Modules/IGT/IGTTrackingDevices/mitkInternalTrackingTool.h @@ -1,76 +1,78 @@ /*=================================================================== 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 MITKINTERNALTRACKINGTOOL_H_HEADER_INCLUDED_ #define MITKINTERNALTRACKINGTOOL_H_HEADER_INCLUDED_ #include #include #include #include namespace mitk { /**Documentation * \brief implements TrackingTool interface * * 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 InternalTrackingTool : public TrackingTool { friend class MicroBirdTrackingDevice; // Add all TrackingDevice subclasses that use InternalTrackingDevice directly public: mitkClassMacro(InternalTrackingTool, TrackingTool); - virtual void GetPosition(mitk::Point3D& position) const; ///< returns the current position of the tool as an array of three floats (in the tracking device coordinate system) - virtual void GetOrientation(mitk::Quaternion& orientation) const; ///< returns the current orientation of the tool as a quaternion (in the tracking device coordinate system) + virtual void PrintSelf(std::ostream& os, itk::Indent indent) const; + + 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 GetOrientation(Quaternion& orientation) const; ///< returns the current orientation of the tool as a quaternion (in the tracking device coordinate system) virtual bool Enable(); ///< enablea the tool, so that it will be tracked. Returns true if enabling was successfull virtual bool Disable(); ///< disables the tool, so that it will not be tracked anymore. Returns true if disabling was successfull virtual bool IsEnabled() const; ///< returns whether the tool is enabled or disabled 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; ///< return one value that corresponds to the overall tracking error. The dimension of this value is specific to each tracking device 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 - virtual void SetPosition(mitk::Point3D position); ///< sets the position - virtual void SetOrientation(mitk::Quaternion orientation); ///< sets the orientation as a quaternion + virtual void SetPosition(Point3D position, ScalarType eps=0.0); ///< sets the position + virtual void SetOrientation(Quaternion orientation, ScalarType eps=0.0); ///< sets the orientation as a quaternion virtual void SetTrackingError(float error); ///< sets the tracking error virtual void SetDataValid(bool _arg); ///< sets if the tracking data (position & Orientation) is valid virtual void SetErrorMessage(const char* _arg); ///< sets the error message - virtual void SetToolTip(mitk::Point3D toolTipPosition, mitk::Quaternion orientation = mitk::Quaternion(0,0,0,1)); ///< 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 SetToolTip(Point3D toolTipPosition, Quaternion orientation = Quaternion(0,0,0,1), 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. protected: itkNewMacro(Self); InternalTrackingTool(); virtual ~InternalTrackingTool(); 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 /* MITKINTERNALTRACKINGTOOL_H_HEADER_INCLUDED_ */ diff --git a/Modules/IGT/Testing/mitkInternalTrackingToolTest.cpp b/Modules/IGT/Testing/mitkInternalTrackingToolTest.cpp index 4a4ae85d97..b014b773e1 100644 --- a/Modules/IGT/Testing/mitkInternalTrackingToolTest.cpp +++ b/Modules/IGT/Testing/mitkInternalTrackingToolTest.cpp @@ -1,143 +1,203 @@ /*=================================================================== 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 "mitkInternalTrackingTool.h" #include "mitkTestingMacros.h" #include #include /**Documentation * NDIPassiveTool has a protected constructor and a protected itkNewMacro * so that only it's friend class NDITrackingDevice is able to instantiate * tool objects. Therefore, we derive from NDIPassiveTool and add a * public itkNewMacro, so that we can instantiate and test the class */ class InternalTrackingToolTestClass : public mitk::InternalTrackingTool { public: mitkClassMacro(InternalTrackingToolTestClass, InternalTrackingTool); /** make a public constructor, so that the test is able * to instantiate NDIPassiveTool */ itkNewMacro(Self); protected: InternalTrackingToolTestClass() : mitk::InternalTrackingTool() { } public: //these static methods are only to structure the test //please see them seperated from the upper part of the class static void TestBasicFunctionality() { // let's create an object of our class mitk::InternalTrackingTool::Pointer internalTrackingTool = InternalTrackingToolTestClass::New().GetPointer(); // first test: did this work? // using MITK_TEST_CONDITION_REQUIRED makes the test stop after failure, since // it makes no sense to continue without an object. MITK_TEST_CONDITION_REQUIRED(internalTrackingTool.IsNotNull(),"Testing instantiation"); // test for Enable() internalTrackingTool->Enable(); MITK_TEST_CONDITION((internalTrackingTool->IsEnabled()==true),"Testing of Enable()"); srand(time(NULL)); // generate a random position to test Set/GetPosition() mitk::Point3D position; position[0] = rand()%1000; position[1] = rand()%1000; position[2] = rand()%1000; internalTrackingTool->SetPosition(position); mitk::Point3D returnedPosition; returnedPosition.Fill(0); internalTrackingTool->GetPosition(returnedPosition); MITK_TEST_CONDITION((position==returnedPosition),"Testing of Set/GetPosition()"); // generate a random orientation to test Set/GetOrientation() mitk::Quaternion orientation; orientation[0] = (rand()%1000)/1000.0; orientation[1] = (rand()%1000)/1000.0; orientation[2] = (rand()%1000)/1000.0; orientation[3] = (rand()%1000)/1000.0; internalTrackingTool->SetOrientation(orientation); mitk::Quaternion returnedOrientation(0,0,0,0); internalTrackingTool->GetOrientation(returnedOrientation); MITK_TEST_CONDITION((orientation==returnedOrientation),"Testing of Set/GetQuaternion()"); // test Set/GetTrackingError() float trackingError = rand()%2; internalTrackingTool->SetTrackingError(trackingError); MITK_TEST_CONDITION((internalTrackingTool->GetTrackingError()==trackingError),"Testing of Set/GetTrackingError()"); // test Set/GetDataValid() internalTrackingTool->SetDataValid(true); MITK_TEST_CONDITION((internalTrackingTool->IsDataValid()==true),"Testing of SetDataValid and IsDataValid() for parameter 'true'"); internalTrackingTool->SetDataValid(false); MITK_TEST_CONDITION((internalTrackingTool->IsDataValid()==false),"Testing of SetDataValid and IsDataValid() for parameter 'false'"); internalTrackingTool->Disable(); MITK_TEST_CONDITION((internalTrackingTool->IsEnabled()==false),"Testing of Disable()"); } static void TestTooltipFunctionality() { mitk::InternalTrackingTool::Pointer internalTrackingTool = InternalTrackingToolTestClass::New().GetPointer(); mitk::Point3D toolTipPos; mitk::FillVector3D(toolTipPos,1,1,1); mitk::Quaternion toolTipQuat = mitk::Quaternion(0,0,0,1); internalTrackingTool->SetToolTip(toolTipPos,toolTipQuat); mitk::Point3D positionInput; mitk::FillVector3D(positionInput,5,6,7); internalTrackingTool->SetPosition(positionInput); mitk::Point3D positionOutput; internalTrackingTool->GetPosition(positionOutput); MITK_TEST_CONDITION(((positionOutput[0] == 6)&& (positionOutput[0] == 6)&& (positionOutput[0] == 6)&& (positionOutput[0] == 6)), "Testing tooltip definition." ); } + +static void TestModiciationTimeCorrectness() +{ + mitk::InternalTrackingTool::Pointer tool = InternalTrackingToolTestClass::New().GetPointer(); + unsigned long mTime1 = tool->GetMTime(); + + mitk::Point3D position1; + mitk::FillVector3D(position1, 1.1, 2.2, 3.3); + tool->SetPosition(position1); + MITK_TEST_CONDITION( mTime1 < tool->GetMTime(), + "Testing MTime updated after initial position set" ); + + mitk::Quaternion quat1 = mitk::Quaternion(0,0,0.70710678118654757,0.70710678118654757); + tool->SetOrientation(quat1); + MITK_TEST_CONDITION( mTime1 < tool->GetMTime(), + "Testing MTime updated after initial orientation set" ); + + unsigned long mTime2 = tool->GetMTime(); + + tool->SetPosition(position1); + MITK_TEST_CONDITION( mTime2 == tool->GetMTime(), + "Testing MTime NOT updated after same initial position set" ); + + tool->SetOrientation(quat1); + MITK_TEST_CONDITION( mTime2 == tool->GetMTime(), + "Testing MTime NOT updated after same initial orientation set" ); + + mitk::Point3D position2; + mitk::FillVector3D(position2, 1.10001, 2.2, 3.3); + tool->SetPosition(position2); + MITK_TEST_CONDITION( mTime2 < tool->GetMTime(), + "Testing MTime updated after new position set" ); + + unsigned long mTime3 = tool->GetMTime(); + + mitk::Quaternion quat2 = mitk::Quaternion(0.0, + 0.0, + 0.70710678118654757, + 0.70710678118654757 + 0.00001); + tool->SetOrientation(quat2); + MITK_TEST_CONDITION( mTime3 < tool->GetMTime(), + "Testing MTime updated after new orientation set" ); + + unsigned long mTime4 = tool->GetMTime(); + + mitk::Point3D position3; + mitk::FillVector3D(position3, 1.10002, 2.2, 3.3); + tool->SetPosition(position3, 0.001); + MITK_TEST_CONDITION( mTime4 == tool->GetMTime(), + "Testing MTime NOT updated after position set within epsilon tolerance" ); + + mitk::Quaternion quat3 = mitk::Quaternion(0.0, + 0.0, + 0.70710678118654757, + 0.70710678118654757 + 0.00002); + tool->SetOrientation(quat3, 0.001); + MITK_TEST_CONDITION( mTime4 == tool->GetMTime(), + "Testing MTime NOT updated after orientation set within epsilon tolerance" ); +} }; /** * Simple example for a test for the class "InternalTrackingTool". * * argc and argv are the command line parameters which were passed to * the ADD_TEST command in the CMakeLists.txt file. For the automatic * tests, argv is either empty for the simple tests or contains the filename * of a test image for the image tests (see CMakeLists.txt). */ int mitkInternalTrackingToolTest(int /* argc */, char* /*argv*/[]) { // always start with this! MITK_TEST_BEGIN("InternalTrackingTool") InternalTrackingToolTestClass::TestBasicFunctionality(); InternalTrackingToolTestClass::TestTooltipFunctionality(); + InternalTrackingToolTestClass::TestModiciationTimeCorrectness(); // always end with this! MITK_TEST_END(); }