diff --git a/Modules/IGT/TrackingDevices/mitkVirtualTrackerTypeInformation.cpp b/Modules/IGT/TrackingDevices/mitkVirtualTrackerTypeInformation.cpp index eae7f126b7..f66258d2db 100644 --- a/Modules/IGT/TrackingDevices/mitkVirtualTrackerTypeInformation.cpp +++ b/Modules/IGT/TrackingDevices/mitkVirtualTrackerTypeInformation.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 "mitkVirtualTrackerTypeInformation.h" #include "mitkVirtualTrackingDevice.h" namespace mitk { std::string VirtualTrackerTypeInformation::GetTrackingDeviceName() { return "Virtual Tracker"; } TrackingDeviceData VirtualTrackerTypeInformation::GetDeviceDataVirtualTracker() { TrackingDeviceData data = { VirtualTrackerTypeInformation::GetTrackingDeviceName(), "Virtual Tracker", "cube", "X" }; return data; } VirtualTrackerTypeInformation::VirtualTrackerTypeInformation() { m_DeviceName = VirtualTrackerTypeInformation::GetTrackingDeviceName(); m_TrackingDeviceData.push_back(GetDeviceDataVirtualTracker()); } VirtualTrackerTypeInformation::~VirtualTrackerTypeInformation() { } mitk::TrackingDeviceSource::Pointer VirtualTrackerTypeInformation::CreateTrackingDeviceSource( mitk::TrackingDevice::Pointer trackingDevice, mitk::NavigationToolStorage::Pointer navigationTools, std::string* errorMessage, std::vector* toolCorrespondencesInToolStorage) { mitk::TrackingDeviceSource::Pointer returnValue = mitk::TrackingDeviceSource::New(); mitk::VirtualTrackingDevice::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()); + bool toolAddSuccess = thisDevice->AddTool(navigationTools->GetTool(i)); if (!toolAddSuccess) { //todo error handling errorMessage->append("Can't add tool, is the toolfile valid?"); return nullptr; } } returnValue->SetTrackingDevice(thisDevice); return returnValue; } } diff --git a/Modules/IGT/TrackingDevices/mitkVirtualTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkVirtualTrackingDevice.cpp index bccb360090..bf03a949e2 100644 --- a/Modules/IGT/TrackingDevices/mitkVirtualTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkVirtualTrackingDevice.cpp @@ -1,334 +1,347 @@ /*=================================================================== 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 "mitkVirtualTrackingDevice.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTException.h" #include #include #include #include #include #include #include typedef itk::MutexLockHolder MutexLockHolder; mitk::VirtualTrackingDevice::VirtualTrackingDevice() : mitk::TrackingDevice(), m_AllTools(), m_ToolsMutex(nullptr), m_MultiThreader(nullptr), m_ThreadID(-1), m_RefreshRate(100), m_NumberOfControlPoints(20), m_GaussianNoiseEnabled(false), m_MeanDistributionParam(0.0), m_DeviationDistributionParam(1.0) { m_Data = mitk::VirtualTrackerTypeInformation::GetDeviceDataVirtualTracker(); m_Bounds[0] = m_Bounds[2] = m_Bounds[4] = -400.0; // initialize bounds to -400 ... +400 (mm) cube m_Bounds[1] = m_Bounds[3] = m_Bounds[5] = 400.0; m_ToolsMutex = itk::FastMutexLock::New(); } mitk::VirtualTrackingDevice::~VirtualTrackingDevice() { if (this->GetState() == Tracking) { this->StopTracking(); } if (this->GetState() == Ready) { this->CloseConnection(); } /* cleanup tracking thread */ if (m_MultiThreader.IsNotNull() && (m_ThreadID != -1)) { m_MultiThreader->TerminateThread(m_ThreadID); m_MultiThreader = nullptr; } m_AllTools.clear(); } mitk::TrackingTool* mitk::VirtualTrackingDevice::AddTool(const char* toolName) { //if (this->GetState() == Tracking) //{ // return nullptr; //} mitk::VirtualTrackingTool::Pointer t = mitk::VirtualTrackingTool::New(); t->SetToolName(toolName); t->SetVelocity(0.1); this->InitializeSpline(t); MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex m_AllTools.push_back(t); return t; } +mitk::TrackingTool *mitk::VirtualTrackingDevice::AddTool(mitk::NavigationTool::Pointer tool) +{ + mitk::VirtualTrackingTool::Pointer t = mitk::VirtualTrackingTool::New(); + t->SetToolName(tool->GetToolName()); + t->SetVelocity(0.1); + t->SetToolTipPosition(tool->GetToolTipPosition(), tool->GetToolAxisOrientation()); + t->SetOrientation(tool->GetToolAxisOrientation()); + + this->InitializeSpline(t); + MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex + m_AllTools.push_back(t); + return t; +} bool mitk::VirtualTrackingDevice::StartTracking() { if (this->GetState() != Ready) return false; this->SetState(Tracking); // go to mode Tracking this->m_StopTrackingMutex->Lock(); this->m_StopTracking = false; this->m_StopTrackingMutex->Unlock(); mitk::IGTTimeStamp::GetInstance()->Start(this); if (m_MultiThreader.IsNotNull() && (m_ThreadID != -1)) m_MultiThreader->TerminateThread(m_ThreadID); if (m_MultiThreader.IsNull()) m_MultiThreader = itk::MultiThreader::New(); m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method return true; } bool mitk::VirtualTrackingDevice::StopTracking() { if (this->GetState() == Tracking) // Only if the object is in the correct state { m_StopTrackingMutex->Lock(); // m_StopTracking is used by two threads, so we have to ensure correct thread handling m_StopTracking = true; m_StopTrackingMutex->Unlock(); m_TrackingFinishedMutex->Lock(); this->SetState(Ready); m_TrackingFinishedMutex->Unlock(); } mitk::IGTTimeStamp::GetInstance()->Stop(this); return true; } unsigned int mitk::VirtualTrackingDevice::GetToolCount() const { MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex return static_cast(this->m_AllTools.size()); } mitk::TrackingTool* mitk::VirtualTrackingDevice::GetTool(unsigned int toolNumber) const { MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex if (toolNumber < m_AllTools.size()) return this->m_AllTools.at(toolNumber); return nullptr; } bool mitk::VirtualTrackingDevice::OpenConnection() { if (m_NumberOfControlPoints < 1) { mitkThrowException(mitk::IGTException) << "to few control points for spline interpolation"; } srand(time(nullptr)); //Init random number generator this->SetState(Ready); return true; } void mitk::VirtualTrackingDevice::InitializeSpline(mitk::VirtualTrackingTool* t) { if (t == nullptr) return; typedef mitk::VirtualTrackingTool::SplineType SplineType; /* create random control points */ SplineType::ControlPointListType controlPoints; controlPoints.reserve(m_NumberOfControlPoints + 1); controlPoints.push_back(this->GetRandomPoint()); // insert point 0 double length = 0.0; // estimate spline length by calculating line segments lengths for (unsigned int i = 1; i < m_NumberOfControlPoints - 1; ++i) // set points 1..n-2 { SplineType::ControlPointType pos; pos = this->GetRandomPoint(); length += controlPoints.at(i - 1).EuclideanDistanceTo(pos); controlPoints.push_back(pos); } controlPoints.push_back(controlPoints.at(0)); // close spline --> insert point last control point with same value as first control point length += controlPoints.at(controlPoints.size() - 2).EuclideanDistanceTo(controlPoints.at(controlPoints.size() - 1)); /* Create knot list. TODO: rethink knot list values and list size. Is there a better solution? */ SplineType::KnotListType knotList; knotList.push_back(0.0); for (unsigned int i = 1; i < controlPoints.size() + t->GetSpline()->GetSplineOrder() + 1; ++i) knotList.push_back(i); knotList.push_back(controlPoints.size() + t->GetSpline()->GetSplineOrder() + 1); t->GetSpline()->SetControlPoints(controlPoints); t->GetSpline()->SetKnots(knotList); t->SetSplineLength(length); } bool mitk::VirtualTrackingDevice::CloseConnection() { bool returnValue = true; if (this->GetState() == Setup) return true; this->SetState(Setup); return returnValue; } mitk::ScalarType mitk::VirtualTrackingDevice::GetSplineChordLength(unsigned int idx) { mitk::VirtualTrackingTool* t = this->GetInternalTool(idx); if (t != nullptr) return t->GetSplineLength(); else throw std::invalid_argument("invalid index"); } void mitk::VirtualTrackingDevice::SetToolSpeed(unsigned int idx, mitk::ScalarType roundsPerSecond) { if (roundsPerSecond < 0.0001) throw std::invalid_argument("Minimum tool speed is 0.0001 rounds per second"); mitk::VirtualTrackingTool* t = this->GetInternalTool(idx); if (t != nullptr) t->SetVelocity(roundsPerSecond); else throw std::invalid_argument("invalid index"); } mitk::VirtualTrackingTool* mitk::VirtualTrackingDevice::GetInternalTool(unsigned int idx) { MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex if (idx < m_AllTools.size()) return m_AllTools.at(idx); else return nullptr; } void mitk::VirtualTrackingDevice::TrackTools() { /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope if (this->GetState() != Tracking) return; bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; this->m_StopTrackingMutex->Unlock(); mitk::ScalarType t = 0.0; while ((this->GetState() == Tracking) && (localStopTracking == false)) { //for (ToolContainer::iterator itAllTools = m_AllTools.begin(); itAllTools != m_AllTools.end(); itAllTools++) for (unsigned int i = 0; i < this->GetToolCount(); ++i) // use mutexed methods to access tool container { mitk::VirtualTrackingTool::Pointer currentTool = this->GetInternalTool(i); mitk::VirtualTrackingTool::SplineType::PointType pos; /* calculate tool position with spline interpolation */ pos = currentTool->GetSpline()->EvaluateSpline(t); mitk::Point3D mp; mitk::itk2vtk(pos, mp); // convert from SplineType::PointType to mitk::Point3D //Add Gaussian Noise to Tracking Coordinates if enabled if (this->m_GaussianNoiseEnabled) { std::random_device rd; std::mt19937 generator(rd()); std::normal_distribution dist(this->m_MeanDistributionParam, this->m_DeviationDistributionParam); double noise = dist(generator); mp = mp + noise; } currentTool->SetPosition(mp); // Currently, a constant speed is used. TODO: use tool velocity setting t += 0.001; if (t >= 1.0) t = 0.0; mitk::Quaternion quat; /* fix quaternion rotation */ quat.x() = 0.0; quat.y() = 0.0; quat.z() = 0.0; quat.r() = 1.0; quat.normalize(); currentTool->SetOrientation(quat); // TODO: rotate once per cycle around a fixed rotation vector currentTool->SetTrackingError(2 * (rand() / (RAND_MAX + 1.0))); // tracking error in 0 .. 2 Range currentTool->SetDataValid(true); currentTool->Modified(); } itksys::SystemTools::Delay(m_RefreshRate); /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } // tracking ends if we pass this line } ITK_THREAD_RETURN_TYPE mitk::VirtualTrackingDevice::ThreadStartTracking(void* pInfoStruct) { /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == nullptr) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == nullptr) { return ITK_THREAD_RETURN_VALUE; } VirtualTrackingDevice *trackingDevice = static_cast(pInfo->UserData); if (trackingDevice != nullptr) trackingDevice->TrackTools(); trackingDevice->m_ThreadID = -1; // reset thread ID because we end the thread here return ITK_THREAD_RETURN_VALUE; } mitk::VirtualTrackingDevice::ControlPointType mitk::VirtualTrackingDevice::GetRandomPoint() { ControlPointType pos; pos[0] = m_Bounds[0] + (m_Bounds[1] - m_Bounds[0]) * (rand() / (RAND_MAX + 1.0)); // X = xMin + xRange * (random number between 0 and 1) pos[1] = m_Bounds[2] + (m_Bounds[3] - m_Bounds[2]) * (rand() / (RAND_MAX + 1.0)); // Y pos[2] = m_Bounds[4] + (m_Bounds[5] - m_Bounds[4]) * (rand() / (RAND_MAX + 1.0)); // Z return pos; } void mitk::VirtualTrackingDevice::EnableGaussianNoise() { this->m_GaussianNoiseEnabled = true; } void mitk::VirtualTrackingDevice::DisableGaussianNoise() { this->m_GaussianNoiseEnabled = false; } void mitk::VirtualTrackingDevice::SetParamsForGaussianNoise(double meanDistribution, double deviationDistribution) { this->m_MeanDistributionParam = meanDistribution; this->m_DeviationDistributionParam = deviationDistribution; } double mitk::VirtualTrackingDevice::GetDeviationDistribution() { return m_DeviationDistributionParam; } double mitk::VirtualTrackingDevice::GetMeanDistribution() { return m_MeanDistributionParam; } diff --git a/Modules/IGT/TrackingDevices/mitkVirtualTrackingDevice.h b/Modules/IGT/TrackingDevices/mitkVirtualTrackingDevice.h index 814cbdb81a..f6a7decbd1 100644 --- a/Modules/IGT/TrackingDevices/mitkVirtualTrackingDevice.h +++ b/Modules/IGT/TrackingDevices/mitkVirtualTrackingDevice.h @@ -1,220 +1,220 @@ /*=================================================================== 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 MITKVIRTUALTRACKINGDEVICE_H_HEADER_INCLUDED_ #define MITKVIRTUALTRACKINGDEVICE_H_HEADER_INCLUDED_ #include #include #include #include #include "itkFastMutexLock.h" #include namespace mitk { /** Documentation * \brief Class representing a tracking device which generates random positions / orientations. * No hardware is needed for tracking device. * * This TrackingDevice class does not interface with a physical tracking device. It simulates * a tracking device by moving the tools on a randomly generated spline path. * * \ingroup IGT */ class MITKIGT_EXPORT VirtualTrackingDevice : public TrackingDevice { public: mitkClassMacro(VirtualTrackingDevice, TrackingDevice); itkFactorylessNewMacro(Self) itkCloneMacro(Self) /** * \brief Sets the refresh rate of the virtual tracking device in ms * \warning This refresh rate is not guaranteed. A thread is used to refresh the positions * of the virtual tools. However, this thread may not run at all during this refresh time period. * \return Sets the refresh rate of the virtual tracking device in ms */ itkSetMacro(RefreshRate, unsigned int) /** * \brief Returns the refresh rate in ms. * \return Returns the refresh rate in ms. */ itkGetConstMacro(RefreshRate, unsigned int) /** * \brief Starts the tracking. * * After StartTracking() is called, * the tools will move on their spline paths with a constant velocity that can be set with * SetToolSpeed(). The standard velocity is 10 seconds for one complete cycle along the spline path. * \warning tool speed is not yet used in the current version * \return Returns true if the tracking is started. Returns false if there was an error. */ bool StartTracking() override; /** * \brief Stops the tracking. * \return Returns true if the tracking is stopped. Returns false if there was an error. */ bool StopTracking() override; /** * \brief Opens the connection to the device. This have to be done before the tracking is started. * @throw mitk::IGTException Throws an exception if there are two less control points to start the the virtual device. */ bool OpenConnection() override; /** * \brief Closes the connection and clears all resources. */ bool CloseConnection() override; /** * \return Returns the number of tools which have been added to the device. */ unsigned int GetToolCount() const override; /** * \param toolNumber The number of the tool which should be given back. * \return Returns the tool which the number "toolNumber". Returns nullptr, if there is * no tool with this number. */ TrackingTool* GetTool(unsigned int toolNumber) const override; /** * \brief Adds a tool to the tracking device. * * The tool will have a random path on which it will move around. The path is created with a * spline function and random control points inside the tracking volume. * * \param toolName The tool which will be added. * \return Returns true if the tool has been added, false otherwise. */ TrackingTool* AddTool(const char* toolName); - + TrackingTool *AddTool(mitk::NavigationTool::Pointer tool); /** * \brief Set the tracking volume bounds * * This will set the tracking volume as an axis aligned bounding box * defined by the six bounds values xMin, xMax, yMin, yMax, zMin, zMax. * Note that the random path of existing tools will not be updated with the new * tracking volume. Tools that are created after calling SetBounds() will use the * new tracking volume */ itkSetVectorMacro(Bounds, mitk::ScalarType, 6); /** * \brief return the tracking volume bounds * * This will return the tracking volume as an axis aligned bounding box * defined by the six bounds values xMin, xMax, yMin, yMax, zMin, zMax */ const mitk::ScalarType* GetBounds() const { return m_Bounds; }; /** * \brief return the approximate length of the spline for tool with index idx in millimeter * * if the index idx is not a * valid tool index, a std::invalid_argument exception is thrown. * GetSplineChordLength() returns the distance between all control points of the * spline in millimeter. This can be used as an approximation for the length of the spline path. */ mitk::ScalarType GetSplineChordLength(unsigned int idx); /** * \brief sets the speed of the tool idx in rounds per second * * The virtual tools will travel along a closed spline path. * This method sets the speed of a tool as a factor of how many rounds per second * the tool should move. A setting of 1.0 will indicate one complete round per second. * Together with GetSplineChordLength(), the speed in millimeter per second can be estimated. * roundsPerSecond must be positive and larger than 0.0001. * \warning Tool speed is currently not used. * \TODO: use tool speed */ void SetToolSpeed(unsigned int idx, mitk::ScalarType roundsPerSecond); /** * \brief enable addition of Gaussian Noise to tracking coordinates */ void EnableGaussianNoise(); /** * \brief disable addition of Gaussian Noise to Trackin coordinates */ void DisableGaussianNoise(); /** * \brief sets the mean distribution and the standard deviation for the Gaussian Noise * */ void SetParamsForGaussianNoise(double meanDistribution, double deviationDistribution); /** * \brief returns the mean distribution for the Gaussian Noise */ double GetMeanDistribution(); /** * \brief returns the deviation distribution for the Gaussian Noise */ double GetDeviationDistribution(); protected: VirtualTrackingDevice(); ~VirtualTrackingDevice() override; /** * \brief This method tracks tools as long as the variable m_Mode is set to "Tracking". * Tracking tools means generating random numbers for the tool position and orientation. * @throw mitk::IGTException Throws an mitk::IGTException if there is an error during virtual tool tracking. */ void TrackTools(); void InitializeSpline(mitk::VirtualTrackingTool* t); ///< initializes the spline path of the tool t with random control points inside the current tracking volume static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void* data); ///< static start method for tracking thread typedef mitk::VirtualTrackingTool::SplineType::ControlPointType ControlPointType; ControlPointType GetRandomPoint(); ///< returns a random position inside the tracking volume (defined by m_Bounds) mitk::VirtualTrackingTool* GetInternalTool(unsigned int idx); typedef std::vector ToolContainer; ///< container type for tracking tools ToolContainer m_AllTools; ///< container for all tracking tools itk::FastMutexLock::Pointer m_ToolsMutex; ///< mutex for coordinated access of tool container itk::MultiThreader::Pointer m_MultiThreader; ///< MultiThreader that starts continuous tracking update int m_ThreadID; unsigned int m_RefreshRate; ///< refresh rate of the internal tracking thread in milliseconds (NOT refreshs per second!) unsigned int m_NumberOfControlPoints; ///< number of control points for the random path generation mitk::ScalarType m_Bounds[6]; ///< bounding box of the tracking volume stored as {xMin, xMax, yMin, yMax, zMin, zMax} bool m_GaussianNoiseEnabled; ///< adding Gaussian Noise to tracking coordinates or not, false by default double m_MeanDistributionParam; /// mean distribution for Gaussion Noise, 0.0 by default double m_DeviationDistributionParam; ///< deviation distribution for Gaussian Noise, 1.0 by default }; }//mitk #endif /* MITKVIRTUALTRACKINGDEVICE_H_HEADER_INCLUDED_ */