void mitk::ClaronTrackingDevice::TrackTools()
{
try
{
/* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */
MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope
bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here
this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking
localStopTracking = this->m_StopTracking;
this->m_StopTrackingMutex->Unlock();
while ((this->GetState() == Tracking) && (localStopTracking == false))
{
this->GetDevice()->GrabFrame();
std::vector<mitk::ClaronTool::Pointer> detectedTools = this->DetectTools();
std::vector<mitk::ClaronTool::Pointer> allTools = this->GetAllTools();
std::vector<mitk::ClaronTool::Pointer>::iterator itAllTools;
for(itAllTools = allTools.begin(); itAllTools != allTools.end(); itAllTools++)
{
mitk::ClaronTool::Pointer currentTool = *itAllTools;
//test if current tool was detected
std::vector<mitk::ClaronTool::Pointer>::iterator itDetectedTools;
bool foundTool = false;
for(itDetectedTools = detectedTools.begin(); itDetectedTools != detectedTools.end(); itDetectedTools++)
{
mitk::ClaronTool::Pointer aktuDet = *itDetectedTools;
std::string tempString(currentTool->GetCalibrationName());
if (tempString.compare(aktuDet->GetCalibrationName())==0)
{
currentTool->SetToolHandle(aktuDet->GetToolHandle());
foundTool = true;
}
}
if (!foundTool)
{
currentTool->SetToolHandle(0);
}
if (currentTool->GetToolHandle() != 0)
{
currentTool->SetDataValid(true);
//get tip position of tool:
std::vector<double> pos_vector = this->GetDevice()->GetTipPosition(currentTool->GetToolHandle());
//write tip position into tool:
mitk::Point3D pos;
pos[0] = pos_vector[0];
pos[1] = pos_vector[1];
pos[2] = pos_vector[2];
currentTool->SetPosition(pos);
//get tip quaternion of tool
std::vector<double> quat = this->GetDevice()->GetTipQuaternions(currentTool->GetToolHandle());
//write tip quaternion into tool
mitk::Quaternion orientation(quat[1], quat[2], quat[3], quat[0]);
currentTool->SetOrientation(orientation);
}
else
{
mitk::Point3D origin;
origin.Fill(0);
currentTool->SetPosition(origin);
currentTool->SetOrientation(mitk::Quaternion(0,0,0,0));
currentTool->SetDataValid(false);
}
}
/* Update the local copy of m_StopTracking */
this->m_StopTrackingMutex->Lock();
localStopTracking = m_StopTracking;
this->m_StopTrackingMutex->Unlock();
}
}
catch(...)
{
this->StopTracking();
this->SetErrorMessage("Error while trying to track tools. Thread stopped.");
}
}
void mitk::MicroBirdTrackingDevice::TrackTools()
{
if (this->GetState() != Tracking)
return;
/* Frequency configuration */
double updateRate = 1000.0 / m_SystemConfig.measurementRate;
double measurementDuration = 0.0;
// 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
// 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
bool localStopTracking;
/* update the local copy of m_StopTracking */
this->m_StopTrackingMutex->Lock();
localStopTracking = this->m_StopTracking;
this->m_StopTrackingMutex->Unlock();
/* Tracking loop */
while ((this->GetState() == Tracking) && (localStopTracking == false))
{
int errorCode;
unsigned int nOfAttachedSensors = 0;
double timeStamp = 0.0;
int toolNumber = 0; // Numbers for attached sensors only
for (int sensorID = 0; sensorID < m_SystemConfig.numberSensors; sensorID++) // for each sensor grep data
{
if (!m_SensorConfig[sensorID].attached)
continue;
// sensor attached so get record
errorCode = GetAsynchronousRecord(sensorID, pRecord, sizeof(record));
if (CompareError(errorCode, BIRD_ERROR_SUCCESS))
{ // On SUCCESS, parse sensor information
nOfAttachedSensors++;
timeStamp += record.time; // Get timestamp from record
ToolType* tool = GetMicroBirdTool(toolNumber); /// Get tool (current sensor)
if (tool != NULL)
{
tool->SetTrackingError(record.quality); // Set tracking error (quality) from record
mitk::Point3D position;
position[0] = record.x;
position[1] = record.y;
position[2] = record.z;
tool->SetPosition(position); // Set position
mitk::Quaternion orientation(record.q[1], record.q[2], record.q[3],record.q[0]);
tool->SetOrientation(orientation); // Set orientation as quaternion \todo : verify quaternion order q(r,x,y,z)
tool->SetDataValid(true); // Set data state to valid
}
toolNumber++; // Increment tool number
}
else
{ // ERROR while reading sensor information
HandleError(errorCode);
}
}
/// @todo : is there any synchronisation?
// Average timestamp: timeStamp/nOfAttachedSensors
// Compute sleep time
double sleepTime = updateRate - measurementDuration;
// Sleep
if (sleepTime > 0.0 && sleepTime < 500.0)
{
// Note: we only have to approximately sleep one measurement cycle,
// since the tracker keeps track of the measurement rate itself
itksys::SystemTools::Delay(sleepTime)
//Sleep(static_cast<DWORD>(sleepTime));
}
// Update the local copy of m_StopTracking
this->m_StopTrackingMutex->Lock();
localStopTracking = m_StopTracking;
this->m_StopTrackingMutex->Unlock();
}
// @bug (#1813) : maybe we need to check for localStopTracking=true here?
// m_StopTracking should only ever be updated by StopTracking(), so
// maybe we should not unlock a mutex that nobody is waiting for?
return; // returning from this function (and ThreadStartTracking()) this will end the thread
}
