diff --git a/Modules/US/USNavigation/mitkAbstractUltrasoundTrackerDevice.cpp b/Modules/US/USNavigation/mitkAbstractUltrasoundTrackerDevice.cpp index 0f9e821238..b6ced66df9 100644 --- a/Modules/US/USNavigation/mitkAbstractUltrasoundTrackerDevice.cpp +++ b/Modules/US/USNavigation/mitkAbstractUltrasoundTrackerDevice.cpp @@ -1,495 +1,496 @@ /*============================================================================ The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center (DKFZ) All rights reserved. Use of this source code is governed by a 3-clause BSD license that can be found in the LICENSE file. ============================================================================*/ #include "mitkAbstractUltrasoundTrackerDevice.h" #include "mitkImageReadAccessor.h" #include "mitkNavigationDataDelayFilter.h" #include "mitkNavigationDataDisplacementFilter.h" #include "mitkNavigationDataSmoothingFilter.h" #include "mitkTrackingDeviceSource.h" +#include <mitkUIDGenerator.h> // US Control Interfaces #include "mitkUSControlInterfaceBMode.h" #include "mitkUSControlInterfaceDoppler.h" #include "mitkUSControlInterfaceProbes.h" // Microservices #include <usGetModuleContext.h> #include <usModule.h> #include <usModuleContext.h> #include <usServiceProperties.h> #include <algorithm> // TempIncludes #include <tinyxml2.h> const std::string mitk::AbstractUltrasoundTrackerDevice::DeviceClassIdentifier = "org.mitk.modules.us.AbstractUltrasoundTrackerDevice"; const char *mitk::AbstractUltrasoundTrackerDevice::DefaultProbeIdentifier = "default"; const char *mitk::AbstractUltrasoundTrackerDevice::ProbeAndDepthSeperator = "_"; const std::string mitk::AbstractUltrasoundTrackerDevice::US_INTERFACE_NAME = "org.mitk.services.AbstractUltrasoundTrackerDevice"; const std::string mitk::AbstractUltrasoundTrackerDevice::US_PROPKEY_DEVICENAME = US_INTERFACE_NAME + ".devicename"; const std::string mitk::AbstractUltrasoundTrackerDevice::US_PROPKEY_CLASS = US_INTERFACE_NAME + ".class"; const std::string mitk::AbstractUltrasoundTrackerDevice::US_PROPKEY_ID = US_INTERFACE_NAME + ".id"; //____ mitk::AbstractUltrasoundTrackerDevice::AbstractUltrasoundTrackerDevice(USDevice::Pointer usDevice, NavigationDataSource::Pointer trackingDevice, bool trackedUltrasoundActive) : m_UltrasoundDevice(usDevice), m_TrackingDeviceDataSource(trackingDevice), m_SmoothingFilter(mitk::NavigationDataSmoothingFilter::New()), m_DelayFilter(mitk::NavigationDataDelayFilter::New(0)), m_DisplacementFilter(mitk::NavigationDataDisplacementFilter::New()), m_LastFilterOfIGTPipeline(nullptr), m_NumberOfSmoothingValues(0), m_DelayCount(0), m_IsTrackedUltrasoundActive(trackedUltrasoundActive) { m_DisplacementFilter->SetTransform6DOF(true); this->RebuildFilterPipeline(); // create a new output (for the image data) //___ mitk::Image::Pointer newOutput = mitk::Image::New(); //___ this->SetNthOutput(0, newOutput); // Combined Modality should not spawn an own acquire thread, because // image acquiring is done by the included us device //___ m_UltrasoundDevice->SetSpawnAcquireThread(false); } mitk::AffineTransform3D::Pointer mitk::AbstractUltrasoundTrackerDevice::GetUSPlaneTransform() { return mitk::AffineTransform3D::New(); } void mitk::AbstractUltrasoundTrackerDevice::SetIsFreezed(bool freeze) { if (m_UltrasoundDevice.IsNull() || m_TrackingDeviceDataSource.IsNull()) { MITK_WARN << "Combined modality not correctly initialized, aborting!"; return; } if (!m_UltrasoundDevice->GetIsActive()) { MITK_WARN("mitkUSDevice") << "Cannot freeze or unfreeze if device is not active."; return; } this->OnFreeze(freeze); if (freeze) { m_IsFreezed = true; } else { m_IsFreezed = false; } } bool mitk::AbstractUltrasoundTrackerDevice::GetIsFreezed() { return m_IsFreezed; } mitk::AbstractUltrasoundTrackerDevice::~AbstractUltrasoundTrackerDevice() { if (m_ServiceRegistration != nullptr) { m_ServiceRegistration.Unregister(); } m_ServiceRegistration = 0; } mitk::AffineTransform3D::Pointer mitk::AbstractUltrasoundTrackerDevice::GetCalibration() { return this->GetCalibration(this->GetCurrentDepthValue(), this->GetIdentifierForCurrentProbe()); } mitk::AffineTransform3D::Pointer mitk::AbstractUltrasoundTrackerDevice::GetCalibration(std::string depth) { return this->GetCalibration(depth, this->GetIdentifierForCurrentProbe()); } mitk::AffineTransform3D::Pointer mitk::AbstractUltrasoundTrackerDevice::GetCalibration(std::string depth, std::string probe) { // make sure that there is no '/' which would cause problems for TinyXML std::replace(probe.begin(), probe.end(), '/', '-'); // create identifier for calibration from probe and depth std::string calibrationKey = probe + mitk::AbstractUltrasoundTrackerDevice::ProbeAndDepthSeperator + depth; // find calibration for combination of probe identifier and depth std::map<std::string, mitk::AffineTransform3D::Pointer>::iterator calibrationIterator = m_Calibrations.find(calibrationKey); if (calibrationIterator == m_Calibrations.end()) { return nullptr; } return calibrationIterator->second; } void mitk::AbstractUltrasoundTrackerDevice::SetCalibration(mitk::AffineTransform3D::Pointer calibration) { if (calibration.IsNull()) { MITK_WARN << "Null pointer passed to SetCalibration of mitk::USDevice. Ignoring call."; return; } std::string calibrationKey = this->GetIdentifierForCurrentCalibration(); if (calibrationKey.empty()) { MITK_WARN << "Could not get a key for the calibration -> Calibration cannot be set."; return; } m_Calibrations[calibrationKey] = calibration; } bool mitk::AbstractUltrasoundTrackerDevice::RemoveCalibration() { return this->RemoveCalibration(this->GetCurrentDepthValue(), this->GetIdentifierForCurrentProbe()); } bool mitk::AbstractUltrasoundTrackerDevice::RemoveCalibration(std::string depth) { return this->RemoveCalibration(depth, this->GetIdentifierForCurrentProbe()); } bool mitk::AbstractUltrasoundTrackerDevice::RemoveCalibration(std::string depth, std::string probe) { // make sure that there is no '/' which would cause problems for TinyXML std::replace(probe.begin(), probe.end(), '/', '-'); // create identifier for calibration from probe and depth std::string calibrationKey = probe + mitk::AbstractUltrasoundTrackerDevice::ProbeAndDepthSeperator + depth; return m_Calibrations.erase(calibrationKey) > 0; } std::string mitk::AbstractUltrasoundTrackerDevice::GetDeviceClass() { return DeviceClassIdentifier; } mitk::USImageSource::Pointer mitk::AbstractUltrasoundTrackerDevice::GetUSImageSource() { if (m_UltrasoundDevice.IsNull()) { MITK_ERROR("AbstractUltrasoundTrackerDevice")("USDevice") << "UltrasoundDevice must not be null."; mitkThrow() << "UltrasoundDevice must not be null."; } return m_UltrasoundDevice->GetUSImageSource(); } mitk::NavigationDataSource::Pointer mitk::AbstractUltrasoundTrackerDevice::GetNavigationDataSource() { if (m_LastFilterOfIGTPipeline.IsNull()) { this->RebuildFilterPipeline(); } m_LastFilterOfIGTPipeline->SetToolMetaDataCollection(this->m_TrackingDeviceDataSource->GetToolMetaDataCollection()); return m_LastFilterOfIGTPipeline; } bool mitk::AbstractUltrasoundTrackerDevice::GetIsCalibratedForCurrentStatus() { return m_Calibrations.find(this->GetIdentifierForCurrentCalibration()) != m_Calibrations.end(); } bool mitk::AbstractUltrasoundTrackerDevice::GetContainsAtLeastOneCalibration() { return !m_Calibrations.empty(); } std::string mitk::AbstractUltrasoundTrackerDevice::SerializeCalibration() { tinyxml2::XMLDocument doc; auto* calibrations = doc.NewElement("calibrations"); doc.InsertEndChild(calibrations); // For each calibration in the set for (std::map<std::string, mitk::AffineTransform3D::Pointer>::iterator it = m_Calibrations.begin(); it != m_Calibrations.end(); it++) { mitk::AffineTransform3D::MatrixType matrix = it->second->GetMatrix(); mitk::AffineTransform3D::TranslationType translation = it->second->GetTranslation(); auto *elem = doc.NewElement(it->first.c_str()); // Serialize Matrix elem->SetAttribute("M00", matrix[0][0]); elem->SetAttribute("M01", matrix[0][1]); elem->SetAttribute("M02", matrix[0][2]); elem->SetAttribute("M10", matrix[1][0]); elem->SetAttribute("M11", matrix[1][1]); elem->SetAttribute("M12", matrix[1][2]); elem->SetAttribute("M20", matrix[2][0]); elem->SetAttribute("M21", matrix[2][1]); elem->SetAttribute("M22", matrix[2][2]); // Serialize Offset elem->SetAttribute("T0", translation[0]); elem->SetAttribute("T1", translation[1]); elem->SetAttribute("T2", translation[2]); calibrations->InsertEndChild(elem); } tinyxml2::XMLPrinter printer; doc.Print(&printer); return printer.CStr(); } void mitk::AbstractUltrasoundTrackerDevice::DeserializeCalibration(const std::string &xmlString, bool clearPreviousCalibrations) { // Sanitize Input if (xmlString == "") { MITK_ERROR << "Empty string passed to Deserialize() method of CombinedModality. Aborting..."; mitkThrow() << "Empty string passed to Deserialize() method of CombinedModality. Aborting..."; return; } // Clear previous calibrations if necessary if (clearPreviousCalibrations) m_Calibrations.clear(); // Parse Input tinyxml2::XMLDocument doc; if (tinyxml2::XML_SUCCESS != doc.Parse(xmlString.c_str())) { MITK_ERROR << "Unable to deserialize calibrations in CombinedModality. Error was: " << doc.ErrorStr(); mitkThrow() << "Unable to deserialize calibrations in CombinedModality. Error was: " << doc.ErrorStr(); return; } auto *root = doc.FirstChildElement(); if (root == nullptr) { MITK_ERROR << "Unable to deserialize calibrations in CombinedModality. String contained no root element."; mitkThrow() << "Unable to deserialize calibrations in CombinedModality. String contained no root element."; return; } // Read Calibrations for (auto *elem = root->FirstChildElement(); elem != nullptr; elem = elem->NextSiblingElement()) { mitk::AffineTransform3D::MatrixType matrix; mitk::AffineTransform3D::OffsetType translation; std::string calibName = elem->Value(); // Deserialize Matrix elem->QueryDoubleAttribute("M00", &matrix[0][0]); elem->QueryDoubleAttribute("M01", &matrix[0][1]); elem->QueryDoubleAttribute("M02", &matrix[0][2]); elem->QueryDoubleAttribute("M10", &matrix[1][0]); elem->QueryDoubleAttribute("M11", &matrix[1][1]); elem->QueryDoubleAttribute("M12", &matrix[1][2]); elem->QueryDoubleAttribute("M20", &matrix[2][0]); elem->QueryDoubleAttribute("M21", &matrix[2][1]); elem->QueryDoubleAttribute("M22", &matrix[2][2]); // Deserialize Offset elem->QueryDoubleAttribute("T0", &translation[0]); elem->QueryDoubleAttribute("T1", &translation[1]); elem->QueryDoubleAttribute("T2", &translation[2]); mitk::AffineTransform3D::Pointer calibration = mitk::AffineTransform3D::New(); calibration->SetMatrix(matrix); calibration->SetTranslation(translation); m_Calibrations[calibName] = calibration; } } void mitk::AbstractUltrasoundTrackerDevice::SetNumberOfSmoothingValues(unsigned int numberOfSmoothingValues) { unsigned int oldNumber = m_NumberOfSmoothingValues; m_NumberOfSmoothingValues = numberOfSmoothingValues; // if filter should be activated or deactivated if ((oldNumber == 0 && numberOfSmoothingValues != 0) || (oldNumber != 0 && numberOfSmoothingValues == 0)) { this->RebuildFilterPipeline(); } m_SmoothingFilter->SetNumerOfValues(numberOfSmoothingValues); } void mitk::AbstractUltrasoundTrackerDevice::SetDelayCount(unsigned int delayCount) { unsigned int oldCount = m_DelayCount; m_DelayCount = delayCount; // if filter should be activated or deactivated if ((oldCount == 0 && delayCount != 0) || (oldCount != 0 && delayCount == 0)) { this->RebuildFilterPipeline(); } m_DelayFilter->SetDelay(delayCount); } void mitk::AbstractUltrasoundTrackerDevice::GenerateData() {} std::string mitk::AbstractUltrasoundTrackerDevice::GetIdentifierForCurrentCalibration() { return this->GetIdentifierForCurrentProbe() + mitk::AbstractUltrasoundTrackerDevice::ProbeAndDepthSeperator + this->GetCurrentDepthValue(); } std::string mitk::AbstractUltrasoundTrackerDevice::GetIdentifierForCurrentProbe() { us::ServiceProperties usdeviceProperties = m_UltrasoundDevice->GetServiceProperties(); us::ServiceProperties::const_iterator probeIt = usdeviceProperties.find(mitk::USDevice::GetPropertyKeys().US_PROPKEY_PROBES_SELECTED); // get probe identifier from control interface for probes std::string probeName = mitk::AbstractUltrasoundTrackerDevice::DefaultProbeIdentifier; if (probeIt != usdeviceProperties.end()) { probeName = (probeIt->second).ToString(); } // make sure that there is no '/' which would cause problems for TinyXML std::replace(probeName.begin(), probeName.end(), '/', '-'); return probeName; } std::string mitk::AbstractUltrasoundTrackerDevice::GetCurrentDepthValue() { us::ServiceProperties usdeviceProperties = m_UltrasoundDevice->GetServiceProperties(); // get string for depth value from the micro service properties std::string depth; us::ServiceProperties::iterator depthIterator = usdeviceProperties.find(mitk::USDevice::GetPropertyKeys().US_PROPKEY_BMODE_DEPTH); if (depthIterator != usdeviceProperties.end()) { depth = depthIterator->second.ToString(); } else { depth = "0"; } return depth; } void mitk::AbstractUltrasoundTrackerDevice::RebuildFilterPipeline() { m_LastFilterOfIGTPipeline = m_TrackingDeviceDataSource; if (m_NumberOfSmoothingValues > 0) { m_SmoothingFilter->ConnectTo(m_LastFilterOfIGTPipeline.GetPointer()); m_LastFilterOfIGTPipeline = m_SmoothingFilter; } if (m_DelayCount > 0) { m_DelayFilter->ConnectTo(m_LastFilterOfIGTPipeline.GetPointer()); m_LastFilterOfIGTPipeline = m_DelayFilter; } if (m_IsTrackedUltrasoundActive) { m_DisplacementFilter->ConnectTo(m_LastFilterOfIGTPipeline.GetPointer()); m_LastFilterOfIGTPipeline = m_DisplacementFilter; } } void mitk::AbstractUltrasoundTrackerDevice::UnregisterOnService() { if (m_UltrasoundDevice->GetDeviceState() == USDevice::State_Activated) { m_UltrasoundDevice->Deactivate(); } if (m_UltrasoundDevice->GetDeviceState() == USDevice::State_Connected) { m_UltrasoundDevice->Disconnect(); } if (m_ServiceRegistration != nullptr) m_ServiceRegistration.Unregister(); m_ServiceRegistration = 0; } void mitk::AbstractUltrasoundTrackerDevice::RegisterAsMicroservice() { // Get Context us::ModuleContext *context = us::GetModuleContext(); // Define ServiceProps // us::ServiceProperties props; mitk::UIDGenerator uidGen = mitk::UIDGenerator("org.mitk.services.AbstractUltrasoundTrackerDevice"); m_ServiceProperties[US_PROPKEY_ID] = uidGen.GetUID(); m_ServiceProperties[US_PROPKEY_DEVICENAME] = m_UltrasoundDevice->GetName(); m_ServiceProperties[US_PROPKEY_CLASS] = mitk::AbstractUltrasoundTrackerDevice::DeviceClassIdentifier; m_ServiceRegistration = context->RegisterService(this, m_ServiceProperties); } mitk::USAbstractControlInterface::Pointer mitk::AbstractUltrasoundTrackerDevice::GetControlInterfaceCustom() { if (m_UltrasoundDevice.IsNull()) { MITK_ERROR("USCombinedModality")("USDevice") << "UltrasoundDevice must not be null."; mitkThrow() << "UltrasoundDevice must not be null."; } return m_UltrasoundDevice->GetControlInterfaceCustom(); } mitk::USControlInterfaceBMode::Pointer mitk::AbstractUltrasoundTrackerDevice::GetControlInterfaceBMode() { if (m_UltrasoundDevice.IsNull()) { MITK_ERROR("USCombinedModality")("USDevice") << "UltrasoundDevice must not be null."; mitkThrow() << "UltrasoundDevice must not be null."; } return m_UltrasoundDevice->GetControlInterfaceBMode(); } mitk::USControlInterfaceProbes::Pointer mitk::AbstractUltrasoundTrackerDevice::GetControlInterfaceProbes() { if (m_UltrasoundDevice.IsNull()) { MITK_ERROR("USCombinedModality")("USDevice") << "UltrasoundDevice must not be null."; mitkThrow() << "UltrasoundDevice must not be null."; } return m_UltrasoundDevice->GetControlInterfaceProbes(); } mitk::USControlInterfaceDoppler::Pointer mitk::AbstractUltrasoundTrackerDevice::GetControlInterfaceDoppler() { if (m_UltrasoundDevice.IsNull()) { MITK_ERROR("USCombinedModality")("USDevice") << "UltrasoundDevice must not be null."; mitkThrow() << "UltrasoundDevice must not be null."; } return m_UltrasoundDevice->GetControlInterfaceDoppler(); }