diff --git a/Modules/IGT/Algorithms/mitkPivotCalibration.cpp b/Modules/IGT/Algorithms/mitkPivotCalibration.cpp new file mode 100644 index 0000000000..bb1094b217 --- /dev/null +++ b/Modules/IGT/Algorithms/mitkPivotCalibration.cpp @@ -0,0 +1,114 @@ +/*=================================================================== + +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 "mitkPivotCalibration.h" +#include "vnl/algo/vnl_svd.h" +#include "vnl/vnl_matrix.h" +#include "vnl/vnl_vector.h" +#include + +mitk::PivotCalibration::PivotCalibration() : m_NavigationDatas(std::vector()), m_ResultPivotRotation(mitk::Quaternion(0, 0, 0, 1)) +{ + + +} + +mitk::PivotCalibration::~PivotCalibration() +{ + +} + +void mitk::PivotCalibration::AddNavigationData(mitk::NavigationData::Pointer data) +{ + m_NavigationDatas.push_back(data); +} + +bool mitk::PivotCalibration::ComputePivotResult() +{ + return ComputePivotPoint(); +} + +bool mitk::PivotCalibration::ComputePivotPoint() +{ + double defaultThreshold = 1e-1; + + std::vector _CheckedTransforms; + for (size_t i = 0; i < m_NavigationDatas.size(); ++i) + { + if (!m_NavigationDatas.at(i)->IsDataValid()) + { + MITK_WARN << "Skipping invalid transform " << i << "."; + continue; + } + _CheckedTransforms.push_back(m_NavigationDatas.at(i)); + } + + if (_CheckedTransforms.empty()) + { + MITK_WARN << "Checked Transforms are empty"; + return false; + } + + unsigned int rows = 3 * _CheckedTransforms.size(); + unsigned int columns = 6; + + vnl_matrix< double > A(rows, columns), minusI(3, 3, 0), R(3, 3); + vnl_vector< double > b(rows), x(columns), t(3); + + minusI(0, 0) = -1; + minusI(1, 1) = -1; + minusI(2, 2) = -1; + + //do the computation and set the internal variables + unsigned int currentRow = 0; + + + for (size_t i = 0; i < _CheckedTransforms.size(); ++i) + { + t = _CheckedTransforms.at(i)->GetPosition().Get_vnl_vector();// t = the current position of the tracked sensor + t *= -1; + b.update(t, currentRow); //b = combines the position for each collected transform in one column vector + R = _CheckedTransforms.at(i)->GetOrientation().rotation_matrix_transpose().transpose(); // R = the current rotation of the tracked sensor, *rotation_matrix_transpose().transpose() is used to obtain original matrix + A.update(R, currentRow, 0); //A = the matrix which stores the rotations for each collected transform and -I + A.update(minusI, currentRow, 3); + currentRow += 3; + } + vnl_svd svdA(A); //The singular value decomposition of matrix A + svdA.zero_out_absolute(defaultThreshold); + + //there is a solution only if rank(A)=6 (columns are linearly + //independent) + if (svdA.rank() < 6) + { + MITK_WARN << "svdA.rank() < 6"; + return false; + } + else + { + x = svdA.solve(b); //x = the resulting pivot point + + m_ResultRMSError = (A * x - b).rms(); //the root mean sqaure error of the computation + + //sets the Pivot Point + m_ResultPivotPoint[0] = x[0]; + m_ResultPivotPoint[1] = x[1]; + m_ResultPivotPoint[2] = x[2]; + + + } + return true; + +} diff --git a/Modules/IGT/Algorithms/mitkPivotCalibration.h b/Modules/IGT/Algorithms/mitkPivotCalibration.h new file mode 100644 index 0000000000..1caabf5358 --- /dev/null +++ b/Modules/IGT/Algorithms/mitkPivotCalibration.h @@ -0,0 +1,67 @@ +/*=================================================================== + +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 PIVOTCALIBRATION_H_HEADER_INCLUDED_ +#define PIVOTCALIBRATION_H_HEADER_INCLUDED_ + +#include "MitkIGTExports.h" +#include +#include +#include +#include +#include +#include + + +namespace mitk { + /**Documentation + * \brief Class for performing a pivot calibration out of a set of navigation datas + * \ingroup IGT + */ + class MITKIGT_EXPORT PivotCalibration : public itk::Object + { + public: + mitkClassMacroItkParent(PivotCalibration, itk::Object); + itkNewMacro(Self); + void AddNavigationData(mitk::NavigationData::Pointer data); + /** @brief Computes the pivot point and rotation/axis on the given + * navigation datas. You can get the results afterwards. + * @return Returns true if the computation was successfull, false if not. + */ + bool ComputePivotResult(); + + itkGetMacro(ResultPivotPoint,mitk::Point3D); + itkGetMacro(ResultPivotRotation,mitk::Quaternion); + itkGetMacro(ResultRMSError,double); + + + protected: + PivotCalibration(); + virtual ~PivotCalibration(); + + std::vector m_NavigationDatas; + + bool ComputePivotPoint(); + bool ComputePivotAxis(); + + mitk::Point3D m_ResultPivotPoint; + mitk::Quaternion m_ResultPivotRotation; + double m_ResultRMSError; + + }; +} // Ende Namespace +#endif diff --git a/Modules/IGT/files.cmake b/Modules/IGT/files.cmake index 3632dac715..8d9ec0bc9e 100644 --- a/Modules/IGT/files.cmake +++ b/Modules/IGT/files.cmake @@ -1,112 +1,113 @@ set(CPP_FILES TestingHelper/mitkNavigationToolStorageTestHelper.cpp Algorithms/mitkNavigationDataDelayFilter.cpp Algorithms/mitkNavigationDataDisplacementFilter.cpp Algorithms/mitkNavigationDataEvaluationFilter.cpp Algorithms/mitkNavigationDataLandmarkTransformFilter.cpp Algorithms/mitkNavigationDataReferenceTransformFilter.cpp Algorithms/mitkNavigationDataSmoothingFilter.cpp Algorithms/mitkNavigationDataToMessageFilter.cpp Algorithms/mitkNavigationDataToNavigationDataFilter.cpp Algorithms/mitkNavigationDataToPointSetFilter.cpp Algorithms/mitkNavigationDataTransformFilter.cpp Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp + Algorithms/mitkPivotCalibration.cpp Common/mitkIGTTimeStamp.cpp Common/mitkSerialCommunication.cpp DataManagement/mitkNavigationDataSource.cpp DataManagement/mitkNavigationTool.cpp DataManagement/mitkNavigationToolStorage.cpp DataManagement/mitkTrackingDeviceSourceConfigurator.cpp DataManagement/mitkTrackingDeviceSource.cpp DataManagement/mitkTrackingDeviceTypeCollection.cpp ExceptionHandling/mitkIGTException.cpp ExceptionHandling/mitkIGTHardwareException.cpp ExceptionHandling/mitkIGTIOException.cpp IO/mitkNavigationDataPlayer.cpp IO/mitkNavigationDataPlayerBase.cpp IO/mitkNavigationDataRecorder.cpp IO/mitkNavigationDataRecorderDeprecated.cpp IO/mitkNavigationDataSequentialPlayer.cpp IO/mitkNavigationToolReader.cpp IO/mitkNavigationToolStorageSerializer.cpp IO/mitkNavigationToolStorageDeserializer.cpp IO/mitkNavigationToolWriter.cpp IO/mitkNavigationDataReaderInterface.cpp Rendering/mitkCameraVisualization.cpp Rendering/mitkNavigationDataObjectVisualizationFilter.cpp Rendering/mitkNavigationDataSliceVisualization.cpp TrackingDevices/mitkClaronTool.cpp TrackingDevices/mitkClaronTrackingDevice.cpp TrackingDevices/mitkInternalTrackingTool.cpp TrackingDevices/mitkNDIPassiveTool.cpp TrackingDevices/mitkNDIProtocol.cpp TrackingDevices/mitkNDITrackingDevice.cpp TrackingDevices/mitkTrackingDevice.cpp TrackingDevices/mitkTrackingTool.cpp TrackingDevices/mitkTrackingVolumeGenerator.cpp TrackingDevices/mitkVirtualTrackingDevice.cpp TrackingDevices/mitkVirtualTrackingTool.cpp TrackingDevices/mitkOptitrackErrorMessages.cpp TrackingDevices/mitkOptitrackTrackingDevice.cpp TrackingDevices/mitkOptitrackTrackingTool.cpp TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp TrackingDevices/mitkOpenIGTLinkTrackingTool.cpp TrackingDevices/mitkNDIAuroraTypeInformation.cpp TrackingDevices/mitkNDIPolarisTypeInformation.cpp TrackingDevices/mitkNPOptitrackTrackingTypeInformation.cpp TrackingDevices/mitkVirtualTrackerTypeInformation.cpp TrackingDevices/mitkMicronTrackerTypeInformation.cpp TrackingDevices/mitkOpenIGTLinkTypeInformation.cpp TrackingDevices/mitkUnspecifiedTrackingTypeInformation.cpp # TrackingDevices/mitkPolhemusTrackingDevice.cpp # TrackingDevices/mitkPolhemusTool.cpp # TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp ) set(H_FILES DataManagement/mitkTrackingDeviceTypeInformation.h Common/mitkTrackingTypes.h ) set(RESOURCE_FILES ClaronMicron.stl IntuitiveDaVinci.stl NDIAurora.stl NDIAurora_Dome.stl NDIAuroraCompactFG_Dome.stl NDIAuroraPlanarFG_Dome.stl NDIAuroraTabletopFG_Dome.stl NDIAuroraTabletopFG_Prototype_Dome.stl NDIPolarisOldModel.stl NDIPolarisSpectra.stl NDIPolarisSpectraExtendedPyramid.stl NDIPolarisVicra.stl ) if(MITK_USE_MICRON_TRACKER) set(CPP_FILES ${CPP_FILES} TrackingDevices/mitkClaronInterface.cpp) else() set(CPP_FILES ${CPP_FILES} TrackingDevices/mitkClaronInterfaceStub.cpp) endif(MITK_USE_MICRON_TRACKER) if(MITK_USE_MICROBIRD_TRACKER) set(CPP_FILES ${CPP_FILES} TrackingDevices/mitkMicroBirdTrackingDevice.cpp) endif(MITK_USE_MICROBIRD_TRACKER) if(MITK_USE_POLHEMUS_TRACKER) set(CPP_FILES ${CPP_FILES} TrackingDevices/mitkPolhemusInterface.cpp TrackingDevices/mitkPolhemusTrackingDevice.cpp TrackingDevices/mitkPolhemusTool.cpp TrackingDevices/mitkPolhemusTrackerTypeInformation.cpp ) endif(MITK_USE_POLHEMUS_TRACKER) diff --git a/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibration.cpp b/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibration.cpp index 9e7db5fa3a..74ee05efaa 100644 --- a/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibration.cpp +++ b/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibration.cpp @@ -1,579 +1,636 @@ /*=================================================================== 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 + // Blueberry #include #include // Qmitk #include "IGTNavigationToolCalibration.h" // mitk #include #include #include #include #include +#include +#include // Qt #include #include //vtk #include + const std::string IGTNavigationToolCalibration::VIEW_ID = "org.mitk.views.igtnavigationtoolcalibration"; IGTNavigationToolCalibration::IGTNavigationToolCalibration() {} IGTNavigationToolCalibration::~IGTNavigationToolCalibration() { -//The following code is required due to a bug in the point list widget. -//If this is removed, MITK crashes when closing the view: -m_Controls.m_RegistrationLandmarkWidget->SetPointSetNode(NULL); -m_Controls.m_CalibrationLandmarkWidget->SetPointSetNode(NULL); + //The following code is required due to a bug in the point list widget. + //If this is removed, MITK crashes when closing the view: + m_Controls.m_RegistrationLandmarkWidget->SetPointSetNode(NULL); + m_Controls.m_CalibrationLandmarkWidget->SetPointSetNode(NULL); } void IGTNavigationToolCalibration::SetFocus() { } void IGTNavigationToolCalibration::OnToolCalibrationMethodChanged(int index) { + //if pivot calibration (3) or manual(0) is chosen only calibration pointer is needed + if (index == 0 || index == 3) { + + if (!CheckInitialization(FALSE)) { + return; + } + } + else{ + if (!CheckInitialization()) { return; } + } + UpdateManualToolTipCalibrationView(); m_Controls.m_CalibrationMethodsWidget->setCurrentIndex(index); m_IndexCurrentCalibrationMethod = index; } -void IGTNavigationToolCalibration::CreateQtPartControl( QWidget *parent ) +void IGTNavigationToolCalibration::CreateQtPartControl(QWidget *parent) { //initialize manual tool editing widget m_ManualToolTipEditWidget = new QmitkNavigationToolCreationAdvancedWidget(parent); m_ManualToolTipEditWidget->setWindowFlags(Qt::Tool | Qt::WindowStaysOnTopHint); m_ManualToolTipEditWidget->setWindowTitle("Edit Tool Tip Manually"); m_ManualToolTipEditWidget->setModal(false); m_ManualToolTipEditWidget->SetDataStorage(this->GetDataStorage()); m_TrackingTimer = new QTimer(this); // create GUI widgets from the Qt Designer's .ui file - m_Controls.setupUi( parent ); - connect( m_Controls.m_SetToolToCalibrate, SIGNAL(clicked()), this, SLOT(SetToolToCalibrate()) ); - connect( m_Controls.m_SetPointer, SIGNAL(clicked()), this, SLOT(SetCalibrationPointer()) ); - connect( m_TrackingTimer, SIGNAL(timeout()), this, SLOT(UpdateTrackingTimer())); - connect( m_Controls.m_AddLandmark, SIGNAL(clicked()), this, SLOT(AddLandmark())); - connect( m_Controls.m_SaveCalibratedTool, SIGNAL(clicked()), this, SLOT(SaveCalibratedTool())); - connect( m_Controls.m_AddPivotPose, SIGNAL(clicked()), this, SLOT(OnAddPivotPose())); - connect( m_Controls.m_ComputePivot, SIGNAL(clicked()), this, SLOT(OnComutePivot())); - connect( m_Controls.m_UseComputedPivotPoint, SIGNAL(clicked()), this, SLOT(OnUseComutedPivotPoint())); - connect( m_Controls.m_StartEditTooltipManually, SIGNAL(clicked()), this, SLOT(OnStartManualToolTipCalibration())); - connect( (QObject*)(m_ManualToolTipEditWidget), SIGNAL(RetrieveDataForManualToolTipManipulation()), this, SLOT(OnRetrieveDataForManualTooltipManipulation()) ); - connect( (QObject*)(m_ManualToolTipEditWidget), SIGNAL(DialogCloseRequested()), this, SLOT(OnProcessManualTooltipEditDialogCloseRequest()) ); - connect( m_Controls.m_CalibrationMethodComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(OnToolCalibrationMethodChanged(int))); - - connect( (QObject*)(m_Controls.m_RunCalibrationButton), SIGNAL(clicked()),(QObject*) this, SLOT(OnRunSingleRefToolCalibrationClicked())); - connect( (QObject*)(m_Controls.m_CollectNavigationDataButton), SIGNAL(clicked()),(QObject*) this, SLOT(OnLoginSingleRefToolNavigationDataClicked())); - connect( (QObject*)(m_Controls.m_SetNewToolTipPosButton), SIGNAL(clicked()),(QObject*) this, SLOT(OnSetNewToolTipPosButtonClicked())); + m_Controls.setupUi(parent); + connect(m_Controls.m_SetToolToCalibrate, SIGNAL(clicked()), this, SLOT(SetToolToCalibrate())); + connect(m_Controls.m_SetPointer, SIGNAL(clicked()), this, SLOT(SetCalibrationPointer())); + connect(m_TrackingTimer, SIGNAL(timeout()), this, SLOT(UpdateTrackingTimer())); + connect(m_Controls.m_AddLandmark, SIGNAL(clicked()), this, SLOT(AddLandmark())); + connect(m_Controls.m_SaveCalibratedTool, SIGNAL(clicked()), this, SLOT(SaveCalibratedTool())); + connect(m_Controls.m_AddPivotPose, SIGNAL(clicked()), this, SLOT(OnAddPivotPose())); + connect(m_Controls.m_ComputePivot, SIGNAL(clicked()), this, SLOT(OnComputePivot())); + connect(m_Controls.m_UseComputedPivotPoint, SIGNAL(clicked()), this, SLOT(OnUseComputedPivotPoint())); + connect(m_Controls.m_StartEditTooltipManually, SIGNAL(clicked()), this, SLOT(OnStartManualToolTipCalibration())); + connect((QObject*)(m_ManualToolTipEditWidget), SIGNAL(RetrieveDataForManualToolTipManipulation()), this, SLOT(OnRetrieveDataForManualTooltipManipulation())); + connect((QObject*)(m_ManualToolTipEditWidget), SIGNAL(DialogCloseRequested()), this, SLOT(OnProcessManualTooltipEditDialogCloseRequest())); + connect(m_Controls.m_CalibrationMethodComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(OnToolCalibrationMethodChanged(int))); + + connect((QObject*)(m_Controls.m_RunCalibrationButton), SIGNAL(clicked()), (QObject*) this, SLOT(OnRunSingleRefToolCalibrationClicked())); + connect((QObject*)(m_Controls.m_CollectNavigationDataButton), SIGNAL(clicked()), (QObject*) this, SLOT(OnLoginSingleRefToolNavigationDataClicked())); + connect((QObject*)(m_Controls.m_SetNewToolTipPosButton), SIGNAL(clicked()), (QObject*) this, SLOT(OnSetNewToolTipPosButtonClicked())); m_IDToolToCalibrate = -1; m_IDCalibrationPointer = -1; m_IndexCurrentCalibrationMethod = -1; m_OnLoginSingleRefToolNavigationDataClicked = false; m_NumberOfNavigationDataCounter = 0; m_NumberOfNavigationData = -1; + //for pivot calibration + m_OnAddPivotPoseClicked = false; + PivotCount = 0; + m_PivotPoses = std::vector(); + m_CalibrationLandmarks = mitk::PointSet::New(); m_CalibrationLandmarksNode = mitk::DataNode::New(); m_CalibrationLandmarksNode->SetData(m_CalibrationLandmarks); m_Controls.m_CalibrationLandmarkWidget->SetPointSetNode(m_CalibrationLandmarksNode); m_RegistrationLandmarks = mitk::PointSet::New(); m_RegistrationLandmarksNode = mitk::DataNode::New(); m_RegistrationLandmarksNode->SetData(m_RegistrationLandmarks); m_Controls.m_RegistrationLandmarkWidget->SetPointSetNode(m_RegistrationLandmarksNode); m_ToolSurfaceInToolCoordinatesDataNode = mitk::DataNode::New(); m_ToolSurfaceInToolCoordinatesDataNode->SetName("ToolSurface(ToolCoordinates)"); - m_PivotPoses = std::vector(); - m_LoggedNavigationDataDifferences = std::vector< mitk::NavigationData::Pointer >(); } void IGTNavigationToolCalibration::OnRunSingleRefToolCalibrationClicked() { - if (!CheckInitialization()) { return; } + if (!CheckInitialization()) { return; } - mitk::NavigationData::Pointer ToolTipTransform = mitk::NavigationData::New(); + mitk::NavigationData::Pointer ToolTipTransform = mitk::NavigationData::New(); - if (m_Controls.m_CalibratePosition->isChecked()) + if (m_Controls.m_CalibratePosition->isChecked()) + { + //1: Compute mean translational offset vector + m_ResultOffsetVector.Fill(0); + for (std::vector::iterator vecIter = m_LoggedNavigationDataOffsets.begin(); vecIter != m_LoggedNavigationDataOffsets.end(); vecIter++) { - //1: Compute mean translational offset vector - m_ResultOffsetVector.Fill(0); - for (std::vector::iterator vecIter = m_LoggedNavigationDataOffsets.begin(); vecIter != m_LoggedNavigationDataOffsets.end(); vecIter++) - { - m_ResultOffsetVector[0] = m_ResultOffsetVector[0] + (*vecIter)[0]; - m_ResultOffsetVector[1] = m_ResultOffsetVector[1] + (*vecIter)[1]; - m_ResultOffsetVector[2] = m_ResultOffsetVector[2] + (*vecIter)[2]; - } - m_ResultOffsetVector[0] = m_ResultOffsetVector[0] / m_LoggedNavigationDataOffsets.size(); - m_ResultOffsetVector[1] = m_ResultOffsetVector[1] / m_LoggedNavigationDataOffsets.size(); - m_ResultOffsetVector[2] = m_ResultOffsetVector[2] / m_LoggedNavigationDataOffsets.size(); - - this->m_Controls.m_ResultOfCalibration->setText( - QString("x: ") + QString(QString::number(m_ResultOffsetVector[0], 103, 3)) + - QString("; y: ") + (QString::number(m_ResultOffsetVector[1], 103, 3)) + - QString("; z: ") + (QString::number(m_ResultOffsetVector[2], 103, 3))); - - - ToolTipTransform->SetPosition(m_ResultOffsetVector); + m_ResultOffsetVector[0] = m_ResultOffsetVector[0] + (*vecIter)[0]; + m_ResultOffsetVector[1] = m_ResultOffsetVector[1] + (*vecIter)[1]; + m_ResultOffsetVector[2] = m_ResultOffsetVector[2] + (*vecIter)[2]; } + m_ResultOffsetVector[0] = m_ResultOffsetVector[0] / m_LoggedNavigationDataOffsets.size(); + m_ResultOffsetVector[1] = m_ResultOffsetVector[1] / m_LoggedNavigationDataOffsets.size(); + m_ResultOffsetVector[2] = m_ResultOffsetVector[2] / m_LoggedNavigationDataOffsets.size(); + this->m_Controls.m_ResultOfCalibration->setText( + QString("x: ") + QString(QString::number(m_ResultOffsetVector[0], 103, 3)) + + QString("; y: ") + (QString::number(m_ResultOffsetVector[1], 103, 3)) + + QString("; z: ") + (QString::number(m_ResultOffsetVector[2], 103, 3))); - if (m_Controls.m_CalibrateOrientation->isChecked()) - { - //2: Compute mean orientation - mitk::Quaternion meanOrientation; - std::vector allOrientations = std::vector (); - for (int i = 0; i < m_LoggedNavigationDataDifferences.size(); i++) { allOrientations.push_back(m_LoggedNavigationDataDifferences.at(i)->GetOrientation()); } - meanOrientation = mitk::QuaternionAveraging::CalcAverage(allOrientations); - this->m_Controls.m_ResultOfCalibrationOrientation->setText( - QString("qx: ") + QString(QString::number(meanOrientation.x(), 103, 3)) + - QString("; qy: ") + (QString::number(meanOrientation.y(), 103, 3)) + - QString("; qz: ") + (QString::number(meanOrientation.z(), 103, 3)) + - QString("; qr: ") + (QString::number(meanOrientation.r(), 103, 3))); - - ToolTipTransform->SetOrientation(meanOrientation); - } - MITK_INFO << "Computed calibration: "; - MITK_INFO << "Translation Vector: " << ToolTipTransform->GetPosition(); - MITK_INFO << "Quaternion: (" << ToolTipTransform->GetOrientation() <<")"; - MITK_INFO<<"Euler Angles [rad]: (" << ToolTipTransform->GetOrientation().rotation_euler_angles() <<")"; - MITK_INFO<<"Matrix:"; - vnl_matrix_fixed rotMatrix =ToolTipTransform->GetOrientation().rotation_matrix_transpose(); - MITK_INFO<Compose(ToolTipTransform); - ToolTipInTrackingCoordinates->Compose(m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate)); - ShowToolTipPreview(ToolTipInTrackingCoordinates); - m_Controls.m_SetNewToolTipPosButton->setEnabled(true); - m_ComputedToolTipTransformation = ToolTipTransform; + ToolTipTransform->SetPosition(m_ResultOffsetVector); + } + + + if (m_Controls.m_CalibrateOrientation->isChecked()) + { + //2: Compute mean orientation + mitk::Quaternion meanOrientation; + std::vector allOrientations = std::vector (); + for (int i = 0; i < m_LoggedNavigationDataDifferences.size(); i++) { allOrientations.push_back(m_LoggedNavigationDataDifferences.at(i)->GetOrientation()); } + meanOrientation = mitk::QuaternionAveraging::CalcAverage(allOrientations); + this->m_Controls.m_ResultOfCalibrationOrientation->setText( + QString("qx: ") + QString(QString::number(meanOrientation.x(), 103, 3)) + + QString("; qy: ") + (QString::number(meanOrientation.y(), 103, 3)) + + QString("; qz: ") + (QString::number(meanOrientation.z(), 103, 3)) + + QString("; qr: ") + (QString::number(meanOrientation.r(), 103, 3))); + + ToolTipTransform->SetOrientation(meanOrientation); + } + + MITK_INFO << "Computed calibration: "; + MITK_INFO << "Translation Vector: " << ToolTipTransform->GetPosition(); + MITK_INFO << "Quaternion: (" << ToolTipTransform->GetOrientation() << ")"; + MITK_INFO << "Euler Angles [rad]: (" << ToolTipTransform->GetOrientation().rotation_euler_angles() << ")"; + MITK_INFO << "Matrix:"; + vnl_matrix_fixed rotMatrix = ToolTipTransform->GetOrientation().rotation_matrix_transpose(); + MITK_INFO << rotMatrix[0][0] << " " << rotMatrix[0][1] << " " << rotMatrix[0][2] << std::endl; + MITK_INFO << rotMatrix[1][0] << " " << rotMatrix[1][1] << " " << rotMatrix[1][2] << std::endl; + MITK_INFO << rotMatrix[2][0] << " " << rotMatrix[2][1] << " " << rotMatrix[2][2] << std::endl; + + //3: write everything into the final tool tip transform and save it as member (it will be written to the tool later on) + mitk::NavigationData::Pointer ToolTipInTrackingCoordinates = mitk::NavigationData::New(); + ToolTipInTrackingCoordinates->Compose(ToolTipTransform); + ToolTipInTrackingCoordinates->Compose(m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate)); + ShowToolTipPreview(ToolTipInTrackingCoordinates); + m_Controls.m_SetNewToolTipPosButton->setEnabled(true); + m_ComputedToolTipTransformation = ToolTipTransform; } void IGTNavigationToolCalibration::OnLoginSingleRefToolNavigationDataClicked() { - if (!CheckInitialization()) {return;} + if (!CheckInitialization()) { return; } m_OnLoginSingleRefToolNavigationDataClicked = true; m_Controls.m_CollectNavigationDataButton->setEnabled(false); m_NumberOfNavigationData = m_Controls.m_NumberOfNavigationDataToCollect->value(); - MITK_INFO << "Collecting " << m_NumberOfNavigationData << " NavigationData ... "<setText(labelText); + //update label text + QString labelText = "Collecting Data: " + QString::number(m_NumberOfNavigationDataCounter); + m_Controls.m_CollectionStatus->setText(labelText); - mitk::NavigationData::Pointer referenceTool = m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer); - mitk::NavigationData::Pointer toolToCalibrate = m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate); + mitk::NavigationData::Pointer referenceTool = m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer); + mitk::NavigationData::Pointer toolToCalibrate = m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate); - //compute difference: - // differenceND = toolToCalibrate^-1 * referenceTool - mitk::NavigationData::Pointer differenceND = mitk::NavigationData::New(); - differenceND->Compose(referenceTool); - differenceND->Compose(toolToCalibrate->GetInverse()); + //compute difference: + // differenceND = toolToCalibrate^-1 * referenceTool + mitk::NavigationData::Pointer differenceND = mitk::NavigationData::New(); + differenceND->Compose(referenceTool); + differenceND->Compose(toolToCalibrate->GetInverse()); - //inverse mode... - if (m_Controls.m_InvertQuaternions->isChecked()) + //inverse mode... + if (m_Controls.m_InvertQuaternions->isChecked()) { - // negate identity matrix to directly show parameters that will set up in NDI 6D Software Architect - differenceND = differenceND->GetInverse(); + // negate identity matrix to directly show parameters that will set up in NDI 6D Software Architect + differenceND = differenceND->GetInverse(); } - //save difference in member - m_LoggedNavigationDataOffsets.push_back(differenceND->GetPosition()); - m_LoggedNavigationDataDifferences.push_back(differenceND); - m_NumberOfNavigationDataCounter++; + //save difference in member + m_LoggedNavigationDataOffsets.push_back(differenceND->GetPosition()); + m_LoggedNavigationDataDifferences.push_back(differenceND); + m_NumberOfNavigationDataCounter++; } - if( m_NumberOfNavigationDataCounter == m_NumberOfNavigationData) - { - m_NumberOfNavigationDataCounter = 0; - m_OnLoginSingleRefToolNavigationDataClicked = false; - m_Controls.m_CollectNavigationDataButton->setEnabled(true); - m_Controls.m_RunCalibrationButton->setEnabled(true); - MITK_INFO << "Collecting " << m_NumberOfNavigationData << " NavigationData ... Finished"<setText(labelText); - } + if (m_NumberOfNavigationDataCounter == m_NumberOfNavigationData) + { + m_NumberOfNavigationDataCounter = 0; + m_OnLoginSingleRefToolNavigationDataClicked = false; + m_Controls.m_CollectNavigationDataButton->setEnabled(true); + m_Controls.m_RunCalibrationButton->setEnabled(true); + MITK_INFO << "Collecting " << m_NumberOfNavigationData << " NavigationData ... Finished" << endl; + QString labelText = "Collected " + QString::number(m_NumberOfNavigationData) + " data samples!"; + m_Controls.m_CollectionStatus->setText(labelText); + } } void IGTNavigationToolCalibration::OnSetNewToolTipPosButtonClicked() { ApplyToolTipTransform(m_ComputedToolTipTransformation); RemoveToolTipPreview(); } +void IGTNavigationToolCalibration::ClearOldPivot() +{ + mitk::NavigationData::Pointer tempND = mitk::NavigationData::New(); + this->ApplyToolTipTransform(tempND); + UpdateManualToolTipCalibrationView(); + m_ManualToolTipEditWidget->hide(); + this->GetDataStorage()->Remove(m_ToolSurfaceInToolCoordinatesDataNode); +} void IGTNavigationToolCalibration::OnAddPivotPose() { - if (!CheckInitialization()) {return;} - mitk::NavigationData::Pointer currentPose = mitk::NavigationData::New(); - currentPose->Graft(m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource()->GetOutput(m_IDToolToCalibrate)); - m_PivotPoses.push_back(currentPose); - m_Controls.m_PoseNumber->setText(QString::number(m_PivotPoses.size())); + ClearOldPivot(); + //When the collect Poses Button is Clicked + m_OnAddPivotPoseClicked = true; + m_NumberOfNavigationData = m_Controls.m_PosesToCollect->value(); + } -void IGTNavigationToolCalibration::OnComutePivot() +void IGTNavigationToolCalibration::AddPivotPose() { - MITK_WARN << "Not implemented yet!"; - /* + //Save the poses to be used in computation + if (PivotCount < m_NumberOfNavigationData) + { + mitk::NavigationData::Pointer currentPose = mitk::NavigationData::New(); + currentPose->Graft(m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource()->GetOutput(m_IDToolToCalibrate)); + m_PivotPoses.push_back(currentPose); + m_Controls.m_PoseNumber->setText(QString::number(m_PivotPoses.size())); + PivotCount++; + } + if (PivotCount == m_NumberOfNavigationData) + { + m_OnAddPivotPoseClicked = false; + } +} + +void IGTNavigationToolCalibration::OnComputePivot() +{ + mitk::PivotCalibration::Pointer myPivotCalibration = mitk::PivotCalibration::New(); - for (int i=0; im_PivotPoses.size(); i++) + for (int i = 0; i < this->m_PivotPoses.size(); i++) { myPivotCalibration->AddNavigationData(m_PivotPoses.at(i)); } QString resultString; if (myPivotCalibration->ComputePivotResult()) { - //Get first marker transformation (TODO: compute for each pivot pose and average laster... this would be more accurate!) - mitk::NavigationData::Pointer markerTransformationTrackingCoordinates = m_PivotPoses.at(0); - - //Get computed pivot transfromation in tool coordinates - mitk::NavigationData::Pointer pivotPointToolCoordinates = mitk::NavigationData::New(); - pivotPointToolCoordinates->SetPosition(myPivotCalibration->GetResultPivotPoint()); - pivotPointToolCoordinates->SetOrientation(myPivotCalibration->GetResultPivotRotation()); - mitk::NavigationData::Pointer toolRotation = mitk::NavigationData::New(); - toolRotation->SetOrientation(markerTransformationTrackingCoordinates->GetOrientation()); - pivotPointToolCoordinates->Compose(toolRotation); - - //Compute pivot point in relation to marker transformation for preview - mitk::NavigationData::Pointer pivotInTrackingCoordinates = mitk::NavigationData::New(); - pivotInTrackingCoordinates->Compose(pivotPointToolCoordinates); - pivotInTrackingCoordinates->Compose(markerTransformationTrackingCoordinates); - - //add the preview node to the data storage - ShowToolTipPreview(pivotInTrackingCoordinates); - - //parse result string - resultString = QString("Pivot comutation succeeded!\n") - + QString("RMS Error: ") + QString::number(myPivotCalibration->GetResultRMSError()) + QString("\n") - + QString("Pivot Point: ") + QString::number(myPivotCalibration->GetResultPivotPoint()[0]) + ";" + QString::number(myPivotCalibration->GetResultPivotPoint()[1]) + ";" + QString::number(myPivotCalibration->GetResultPivotPoint()[2]) + QString("\n") - + QString("Pivot Rotation: ") + QString::number(myPivotCalibration->GetResultPivotRotation()[0]) + ";" + QString::number(myPivotCalibration->GetResultPivotRotation()[1]) + ";" + QString::number(myPivotCalibration->GetResultPivotRotation()[2]) + ";" + QString::number(myPivotCalibration->GetResultPivotRotation()[3]) + QString("\n"); - - //finally: save results to member variable - m_ComputedToolTipTransformation = pivotPointToolCoordinates; - - //enable button to use the computed point with the tool - m_Controls.m_UseComputedPivotPoint->setEnabled(true); + + mitk::NavigationData::Pointer markerTransformationTrackingCoordinates = m_PivotPoses.at(0); + + //Get computed pivot transfromation in tool coordinates + + + mitk::NavigationData::Pointer ToolTipToTool = mitk::NavigationData::New(); + ToolTipToTool->SetPosition(myPivotCalibration->GetResultPivotPoint()); + ToolTipToTool->SetOrientation(myPivotCalibration->GetResultPivotRotation()); + mitk::NavigationData::Pointer TrackerToTool = mitk::NavigationData::New(); + TrackerToTool->SetOrientation(markerTransformationTrackingCoordinates->GetOrientation()); + TrackerToTool->SetPosition(markerTransformationTrackingCoordinates->GetPosition()); + TrackerToTool->Compose(ToolTipToTool); + + // Compute pivot point in relation to marker transformation for preview + mitk::NavigationData::Pointer ToolTipToTracker = mitk::NavigationData::New(); + ToolTipToTracker->Compose(ToolTipToTool); + ToolTipToTracker->Compose(markerTransformationTrackingCoordinates); + + //add the preview node to the data storage + ShowToolTipPreview(ToolTipToTracker); + + //parse result string + resultString = QString("Pivot computation succeeded!\n") + + QString("RMS Error: ") + QString::number(myPivotCalibration->GetResultRMSError()) + QString("\n") + + QString("Pivot Point: ") + QString::number(myPivotCalibration->GetResultPivotPoint()[0]) + ";" + QString::number(myPivotCalibration->GetResultPivotPoint()[1]) + ";" + QString::number(myPivotCalibration->GetResultPivotPoint()[2]) + QString("\n") + + QString("Pivot Rotation: ") + QString::number(myPivotCalibration->GetResultPivotRotation()[0]) + ";" + QString::number(myPivotCalibration->GetResultPivotRotation()[1]) + ";" + QString::number(myPivotCalibration->GetResultPivotRotation()[2]) + ";" + QString::number(myPivotCalibration->GetResultPivotRotation()[3]) + QString("\n"); + + //finally: save results to member variable + m_ComputedToolTipTransformation = ToolTipToTool; + + + //enable button to use the computed point with the tool + m_Controls.m_UseComputedPivotPoint->setEnabled(true); } else { - resultString = "Pivot comutation failed!"; + resultString = "Pivot computation failed!"; } MITK_INFO << resultString.toStdString().c_str(); m_Controls.m_ResultText->setText(resultString); - */ + +} +void IGTNavigationToolCalibration::UpdatePivotCount() +{ + PivotCount = 0; + while (!m_PivotPoses.empty()) + { + m_PivotPoses.pop_back(); + } + m_Controls.m_PoseNumber->setText(QString::number(PivotCount)); } -void IGTNavigationToolCalibration::OnUseComutedPivotPoint() +void IGTNavigationToolCalibration::OnUseComputedPivotPoint() { - if (!CheckInitialization(false)) {return;} RemoveToolTipPreview(); QString resultString = QString("Pivoted tool tip transformation was written to the tool ") + m_ToolToCalibrate->GetToolName().c_str(); - ApplyToolTipTransform(m_ComputedToolTipTransformation,resultString.toStdString()); + ApplyToolTipTransform(m_ComputedToolTipTransformation, resultString.toStdString()); m_Controls.m_ResultText->setText(resultString); + UpdatePivotCount(); } void IGTNavigationToolCalibration::ApplyToolTipTransform(mitk::NavigationData::Pointer ToolTipTransformInToolCoordinates, std::string message) { - if (!CheckInitialization(false)) {return;} + if (!CheckInitialization(false)) { return; } //Update tool in tool storage m_ToolToCalibrate->SetToolTipPosition(ToolTipTransformInToolCoordinates->GetPosition()); m_ToolToCalibrate->SetToolTipOrientation(ToolTipTransformInToolCoordinates->GetOrientation()); //And also update tracking device, so the transform is directly used mitk::TrackingDeviceSource::Pointer trackingDeviceSource; try - { + { trackingDeviceSource = dynamic_cast(m_NavigationDataSourceOfToolToCalibrate.GetPointer()); mitk::TrackingTool::Pointer TrackingToolToCalibrate = trackingDeviceSource->GetTrackingDevice()->GetTool(m_IDToolToCalibrate); - TrackingToolToCalibrate->SetToolTip(ToolTipTransformInToolCoordinates->GetPosition(),ToolTipTransformInToolCoordinates->GetOrientation()); - } + TrackingToolToCalibrate->SetToolTip(ToolTipTransformInToolCoordinates->GetPosition(), ToolTipTransformInToolCoordinates->GetOrientation()); + } catch (std::exception& e) - { + { MITK_ERROR << "Error while trying to set the tool tip to the running tracking device. Aborting! (" << e.what() << ")"; - } + } MITK_INFO << message; } void IGTNavigationToolCalibration::ShowToolTipPreview(mitk::NavigationData::Pointer ToolTipInTrackingCoordinates) { mitk::DataNode::Pointer m_ToolTipPointPreview = mitk::DataNode::New(); m_ToolTipPointPreview->SetName("Modified Tool Tip Preview"); - mitk::Color red; - red.SetRed(1); - m_ToolTipPointPreview->SetColor(red); + mitk::Color blue; + blue.SetBlue(1); + m_ToolTipPointPreview->SetColor(blue); mitk::Surface::Pointer mySphere = mitk::Surface::New(); vtkSphereSource *vtkData = vtkSphereSource::New(); vtkData->SetRadius(3.0f); vtkData->SetCenter(0.0, 0.0, 0.0); vtkData->Update(); mySphere->SetVtkPolyData(vtkData->GetOutput()); vtkData->Delete(); m_ToolTipPointPreview->SetData(mySphere); m_ToolTipPointPreview->GetData()->GetGeometry()->SetIndexToWorldTransform(ToolTipInTrackingCoordinates->GetAffineTransform3D()); this->GetDataStorage()->Add(m_ToolTipPointPreview); } void IGTNavigationToolCalibration::RemoveToolTipPreview() { this->GetDataStorage()->Remove(m_ToolTipPointPreview.GetPointer()); } void IGTNavigationToolCalibration::UpdateManualToolTipCalibrationView() { - if (m_ToolToCalibrate.IsNull()) {return;} + if (m_ToolToCalibrate.IsNull()) { return; } //parse human readable transformation data and display it std::stringstream translation; std::stringstream orientation; - translation<GetToolTipPosition(); - orientation<<"Quaternion: (" << m_ToolToCalibrate->GetToolTipOrientation() <<")"<GetToolTipOrientation().rotation_euler_angles() <<")"< rotMatrix =m_ToolToCalibrate->GetToolTipOrientation().rotation_matrix_transpose(); - orientation<GetToolTipPosition(); + orientation << "Quaternion: (" << m_ToolToCalibrate->GetToolTipOrientation() << ")" << std::endl; + orientation << std::endl; + orientation << "Euler Angles [rad]: (" << m_ToolToCalibrate->GetToolTipOrientation().rotation_euler_angles() << ")" << std::endl; + orientation << std::endl; + orientation << "Matrix:" << std::endl; + vnl_matrix_fixed rotMatrix = m_ToolToCalibrate->GetToolTipOrientation().rotation_matrix_transpose(); + orientation << rotMatrix[0][0] << " " << rotMatrix[0][1] << " " << rotMatrix[0][2] << std::endl; + orientation << rotMatrix[1][0] << " " << rotMatrix[1][1] << " " << rotMatrix[1][2] << std::endl; + orientation << rotMatrix[2][0] << " " << rotMatrix[2][1] << " " << rotMatrix[2][2] << std::endl; m_Controls.m_ManualCurrentTranslation->setText(translation.str().c_str()); m_Controls.m_ManualCurrentOrientation->setPlainText(orientation.str().c_str()); } void IGTNavigationToolCalibration::OnStartManualToolTipCalibration() { - if (!CheckInitialization(false)) {return;} - m_ManualToolTipEditWidget->SetToolTipSurface(false,m_ToolToCalibrate->GetDataNode()); + if (!CheckInitialization(false)) { return; } + m_ManualToolTipEditWidget->SetToolTipSurface(false, m_ToolToCalibrate->GetDataNode()); m_ManualToolTipEditWidget->show(); m_ManualToolTipEditWidget->SetDefaultTooltip(m_ToolToCalibrate->GetToolTipTransform()); m_ManualToolTipEditWidget->ReInitialize(); } void IGTNavigationToolCalibration::OnRetrieveDataForManualTooltipManipulation() { this->GetDataStorage()->Add(m_ToolSurfaceInToolCoordinatesDataNode); - m_ManualToolTipEditWidget->SetToolTipSurface(false,m_ToolSurfaceInToolCoordinatesDataNode); + m_ManualToolTipEditWidget->SetToolTipSurface(false, m_ToolSurfaceInToolCoordinatesDataNode); } void IGTNavigationToolCalibration::OnProcessManualTooltipEditDialogCloseRequest() { mitk::NavigationData::Pointer tempND = mitk::NavigationData::New(m_ManualToolTipEditWidget->GetManipulatedToolTip()); this->ApplyToolTipTransform(tempND); UpdateManualToolTipCalibrationView(); m_ManualToolTipEditWidget->hide(); this->GetDataStorage()->Remove(m_ToolSurfaceInToolCoordinatesDataNode); } void IGTNavigationToolCalibration::SetToolToCalibrate() { m_IDToolToCalibrate = m_Controls.m_SelectionWidget->GetSelectedToolID(); m_ToolToCalibrate = m_Controls.m_SelectionWidget->GetSelectedNavigationTool(); - if (m_IDToolToCalibrate==-1) //no valid tool to calibrate + if (m_IDToolToCalibrate == -1) //no valid tool to calibrate { m_Controls.m_CalToolLabel->setText(""); m_Controls.m_StatusWidgetToolToCalibrate->RemoveStatusLabels(); m_TrackingTimer->stop(); } else { m_NavigationDataSourceOfToolToCalibrate = m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource(); m_Controls.m_CalToolLabel->setText(m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate)->GetName()); //initialize widget m_Controls.m_StatusWidgetToolToCalibrate->RemoveStatusLabels(); m_Controls.m_StatusWidgetToolToCalibrate->SetShowPositions(true); m_Controls.m_StatusWidgetToolToCalibrate->SetTextAlignment(Qt::AlignLeft); m_Controls.m_StatusWidgetToolToCalibrate->AddNavigationData(m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate)); m_Controls.m_StatusWidgetToolToCalibrate->ShowStatusLabels(); //initialize manual tool tip calibration view UpdateManualToolTipCalibrationView(); //save tool surface in tool coordinates for further editing mitk::Surface::Pointer ToolSurface = dynamic_cast(m_ToolToCalibrate->GetDataNode()->GetData())->Clone(); m_ToolSurfaceInToolCoordinatesDataNode->SetData(ToolSurface); m_ToolSurfaceInToolCoordinatesDataNode->GetData()->GetGeometry()->SetIdentity(); //start updating timer for status widgets, etc. if (!m_TrackingTimer->isActive()) m_TrackingTimer->start(100); } } void IGTNavigationToolCalibration::SetCalibrationPointer() { m_IDCalibrationPointer = m_Controls.m_SelectionWidget->GetSelectedToolID(); m_NavigationDataSourceOfCalibrationPointer = m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource(); - if (m_IDCalibrationPointer==-1) + if (m_IDCalibrationPointer == -1) { m_Controls.m_PointerLabel->setText(""); m_Controls.m_StatusWidgetCalibrationPointer->RemoveStatusLabels(); m_TrackingTimer->stop(); } else { m_Controls.m_PointerLabel->setText(m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer)->GetName()); //initialize widget m_Controls.m_StatusWidgetCalibrationPointer->RemoveStatusLabels(); m_Controls.m_StatusWidgetCalibrationPointer->SetShowPositions(true); m_Controls.m_StatusWidgetCalibrationPointer->SetTextAlignment(Qt::AlignLeft); m_Controls.m_StatusWidgetCalibrationPointer->AddNavigationData(m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer)); m_Controls.m_StatusWidgetCalibrationPointer->ShowStatusLabels(); if (!m_TrackingTimer->isActive()) m_TrackingTimer->start(100); } } void IGTNavigationToolCalibration::UpdateOffsetCoordinates() { if (m_NavigationDataSourceOfCalibrationPointer.IsNull() || m_NavigationDataSourceOfToolToCalibrate.IsNull()) { return; } mitk::NavigationData::Pointer referenceToolND = m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer); mitk::NavigationData::Pointer toolToCalibrateND = m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate); - if(referenceToolND->IsDataValid() && toolToCalibrateND->IsDataValid()) + if (referenceToolND->IsDataValid() && toolToCalibrateND->IsDataValid()) { //computation: difference between both tools (in tool coordinates) //differenceND = toolToCalibrateND^-1 * referenceToolND mitk::NavigationData::Pointer differenceND = mitk::NavigationData::New(); differenceND->Compose(referenceToolND); differenceND->Compose(toolToCalibrateND->GetInverse()); //display this orientation in the UI m_Controls.m_OffsetCoordinates->setText( - QString("x: ") + QString(QString::number(differenceND->GetPosition()[0],103,3)) + - QString("; y: ") + (QString::number(differenceND->GetPosition()[1],103,3)) + - QString("; z: ") + (QString::number(differenceND->GetPosition()[2],103,3))); + QString("x: ") + QString(QString::number(differenceND->GetPosition()[0], 103, 3)) + + QString("; y: ") + (QString::number(differenceND->GetPosition()[1], 103, 3)) + + QString("; z: ") + (QString::number(differenceND->GetPosition()[2], 103, 3))); m_Controls.m_OrientationOffsetCoordinates->setText( - QString("qx: ") + QString(QString::number(differenceND->GetOrientation().x(),103,3)) + - QString("; qy: ") + (QString::number(differenceND->GetOrientation().y(),103,3)) + - QString("; qz: ") + (QString::number(differenceND->GetOrientation().z(),103,3)) + - QString("; qr: ") + (QString::number(differenceND->GetOrientation().r(),103,3))); + QString("qx: ") + QString(QString::number(differenceND->GetOrientation().x(), 103, 3)) + + QString("; qy: ") + (QString::number(differenceND->GetOrientation().y(), 103, 3)) + + QString("; qz: ") + (QString::number(differenceND->GetOrientation().z(), 103, 3)) + + QString("; qr: ") + (QString::number(differenceND->GetOrientation().r(), 103, 3))); //also update preview if active if (m_ToolTipPointPreview.IsNotNull()) //NOT WORKING! TODO: fix or remove! - { + { mitk::NavigationData::Pointer ToolTipTransform = mitk::NavigationData::New(); ToolTipTransform->SetPosition(m_ResultOffsetVector); mitk::NavigationData::Pointer ToolTipInTrackingCoordinates = mitk::NavigationData::New(); //maybe store as for better peformance... ToolTipInTrackingCoordinates->Compose(m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate)); ToolTipInTrackingCoordinates->Compose(ToolTipTransform); m_ToolTipPointPreview->GetData()->GetGeometry()->SetIndexToWorldTransform(ToolTipInTrackingCoordinates->GetAffineTransform3D()); - } + } } } void IGTNavigationToolCalibration::UpdateTrackingTimer() { m_Controls.m_StatusWidgetToolToCalibrate->Refresh(); m_Controls.m_StatusWidgetCalibrationPointer->Refresh(); - if(m_OnLoginSingleRefToolNavigationDataClicked) LoginSingleRefToolNavigationData(); + if (m_OnLoginSingleRefToolNavigationDataClicked) LoginSingleRefToolNavigationData(); + + if (m_OnAddPivotPoseClicked) AddPivotPose(); // 1 == Single Reference Calibration Method - if(m_IndexCurrentCalibrationMethod == 1 ) UpdateOffsetCoordinates(); + if (m_IndexCurrentCalibrationMethod == 1) UpdateOffsetCoordinates(); } void IGTNavigationToolCalibration::AddLandmark() { - if (!CheckInitialization()) {return;} + if (!CheckInitialization()) { return; } mitk::NavigationData::Pointer navDataTool = m_NavigationDataSourceOfToolToCalibrate->GetOutput(m_IDToolToCalibrate); mitk::Point3D landmark = m_NavigationDataSourceOfCalibrationPointer->GetOutput(m_IDCalibrationPointer)->GetPosition(); //convert to itk transform - itk::Vector translation; - for(int k=0; k<3;k++) translation[k] = navDataTool->GetPosition()[k]; - itk::Matrix rotation; - for(int k=0; k<3; k++) for(int l=0; l<3; l++) rotation[k][l] = navDataTool->GetOrientation().rotation_matrix_transpose()[k][l]; + itk::Vector translation; + for (int k = 0; k < 3; k++) translation[k] = navDataTool->GetPosition()[k]; + itk::Matrix rotation; + for (int k = 0; k < 3; k++) for (int l = 0; l < 3; l++) rotation[k][l] = navDataTool->GetOrientation().rotation_matrix_transpose()[k][l]; rotation = rotation.GetTranspose(); itk::Vector landmarkItk; landmarkItk[0] = landmark[0]; landmarkItk[1] = landmark[1]; landmarkItk[2] = landmark[2]; //compute landmark in tool coordinates - itk::Matrix rotationInverse; - for(int k=0; k<3; k++) for(int l=0; l<3; l++) rotationInverse[k][l] = rotation.GetInverse()[k][l]; - landmarkItk = rotationInverse * (landmarkItk - translation) ; + itk::Matrix rotationInverse; + for (int k = 0; k < 3; k++) for (int l = 0; l < 3; l++) rotationInverse[k][l] = rotation.GetInverse()[k][l]; + landmarkItk = rotationInverse * (landmarkItk - translation); //convert back and add landmark to pointset landmark[0] = landmarkItk[0]; landmark[1] = landmarkItk[1]; landmark[2] = landmarkItk[2]; - m_RegistrationLandmarks->InsertPoint(m_RegistrationLandmarks->GetSize(),landmark); + m_RegistrationLandmarks->InsertPoint(m_RegistrationLandmarks->GetSize(), landmark); } void IGTNavigationToolCalibration::SaveCalibratedTool() { if (m_ToolToCalibrate.IsNotNull()) { mitk::NavigationTool::Pointer calibratedTool = m_ToolToCalibrate; calibratedTool->SetToolCalibrationLandmarks(this->m_CalibrationLandmarks); calibratedTool->SetToolRegistrationLandmarks(this->m_RegistrationLandmarks); mitk::NavigationToolWriter::Pointer myWriter = mitk::NavigationToolWriter::New(); - std::string filename = QFileDialog::getSaveFileName(NULL,tr("Save Navigation Tool"), "/", "*.IGTTool").toUtf8().data(); + std::string filename = QFileDialog::getSaveFileName(NULL, tr("Save Navigation Tool"), "/", "*.IGTTool").toUtf8().data(); filename.append(".IGTTool"); if (filename == "") return; - if (myWriter->DoWrite(filename,calibratedTool)) MITK_INFO << "Saved calibrated tool to file " << filename; + if (myWriter->DoWrite(filename, calibratedTool)) MITK_INFO << "Saved calibrated tool to file " << filename; else MITK_WARN << "Can't write tool to file " << filename; } else { MITK_ERROR << "Did not find navigation tool storage of calibrated tool, aborting!"; } } bool IGTNavigationToolCalibration::CheckInitialization(bool CalibrationPointerRequired) { -if ( (m_IDToolToCalibrate == -1) || - ( (CalibrationPointerRequired) && - (m_IDCalibrationPointer == -1) - ) - ) + if ((m_IDToolToCalibrate == -1) || + ((CalibrationPointerRequired) && + (m_IDCalibrationPointer == -1) + ) + ) { - QMessageBox msgBox; - msgBox.setText("Tool to calibrate and/or calibration pointer not initialized, cannot proceed!"); - msgBox.exec(); - return false; + QMessageBox msgBox; + msgBox.setText("Tool to calibrate and/or calibration pointer not initialized, cannot proceed!"); + msgBox.exec(); + return false; } -else {return true;} + else { return true; } } diff --git a/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibration.h b/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibration.h index f48ce2e133..ae582db713 100644 --- a/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibration.h +++ b/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibration.h @@ -1,127 +1,135 @@ /*=================================================================== 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 IGTNavigationToolCalibration_h #define IGTNavigationToolCalibration_h #include #include #include #include #include "ui_IGTNavigationToolCalibrationControls.h" +#include + //QT headers #include /*! \brief IGTNavigationToolCalibration \warning This class is not yet documented. Use "git blame" and ask the author to provide basic documentation. \ingroup ${plugin_target}_internal -*/ + */ class IGTNavigationToolCalibration : public QmitkAbstractView { // this is needed for all Qt objects that should have a Qt meta-object // (everything that derives from QObject and wants to have signal/slots) Q_OBJECT - public: +public: - IGTNavigationToolCalibration(); + IGTNavigationToolCalibration(); - virtual ~IGTNavigationToolCalibration(); + virtual ~IGTNavigationToolCalibration(); - static const std::string VIEW_ID; + static const std::string VIEW_ID; - virtual void CreateQtPartControl(QWidget *parent); + virtual void CreateQtPartControl(QWidget *parent); protected slots: - void OnAddPivotPose(); - void OnComutePivot(); - void OnUseComutedPivotPoint(); - void SetToolToCalibrate(); - void SetCalibrationPointer(); - void UpdateTrackingTimer(); - void AddLandmark(); - void SaveCalibratedTool(); - void OnToolCalibrationMethodChanged(int index); - void OnStartManualToolTipCalibration(); - void OnRetrieveDataForManualTooltipManipulation(); - void OnProcessManualTooltipEditDialogCloseRequest(); - void OnRunSingleRefToolCalibrationClicked(); - void OnLoginSingleRefToolNavigationDataClicked(); - void OnSetNewToolTipPosButtonClicked(); - - - protected: - - virtual void SetFocus(); - - void UpdateOffsetCoordinates(); - - int m_IndexCurrentCalibrationMethod; - - Ui::IGTNavigationToolCalibrationControls m_Controls; - - //some general members - mitk::NavigationTool::Pointer m_ToolToCalibrate; //<<< tool that will be calibrated - int m_IDToolToCalibrate; //<<< id of tool that will be calibrated (of the navigation data source) - mitk::NavigationDataSource::Pointer m_NavigationDataSourceOfToolToCalibrate; //<<< navigation data source of the tool that will be calibrated - mitk::NavigationDataSource::Pointer m_NavigationDataSourceOfCalibrationPointer; //<<< navigation data source of the calibration pointer - mitk::DataNode::Pointer m_ToolSurfaceInToolCoordinatesDataNode; //<<< holds the tool surface in tool coordinates (for preview purposes) - int m_IDCalibrationPointer; //<<< id of the calibration pointer (of the corresponding navigation data source) - QTimer* m_TrackingTimer; //<<< tracking timer that updates the status widgets - void ApplyToolTipTransform(mitk::NavigationData::Pointer ToolTipTransformInToolCoordinates, std::string message = "Tool was updated with the calibrated tool tip!"); //<<< applys the given tool tip transform to the tool to calibrate - bool CheckInitialization(bool CalibrationPointerRequired = true); //<<< checks if the tool to calibrate and (if required) the calibration pointer is initialized. Displays a warning and returns false if not. - mitk::NavigationData::Pointer m_ComputedToolTipTransformation; //<<< holds the new tooltip transformation after it was computed to write it into the tool later - - // members and helper methods for pivot tool calibration - std::vector m_PivotPoses; - - // members and helper methods for manual tool calibration - void UpdateManualToolTipCalibrationView(); - QmitkNavigationToolCreationAdvancedWidget* m_ManualToolTipEditWidget; - - // members and helper methods for single reference tool calibration - void LoginSingleRefToolNavigationData(); - std::vector< mitk::Point3D > m_LoggedNavigationDataOffsets; - std::vector< mitk::NavigationData::Pointer > m_LoggedNavigationDataDifferences; - bool m_OnLoginSingleRefToolNavigationDataClicked; - int m_NumberOfNavigationData; - int m_NumberOfNavigationDataCounter; - mitk::Point3D m_ResultOffsetVector; - - // members and helper methods for tool tip preview - mitk::DataNode::Pointer m_ToolTipPointPreview; //<<< Data node of the tool tip preview - void ShowToolTipPreview(mitk::NavigationData::Pointer ToolTipInTrackingCoordinates); //<<< Adds a preview of the tool tip into the data storage - void RemoveToolTipPreview(); //<<< Removes the preview - - // members for the tool landmark calibration - mitk::PointSet::Pointer m_CalibrationLandmarks; - mitk::DataNode::Pointer m_CalibrationLandmarksNode; - mitk::PointSet::Pointer m_RegistrationLandmarks; - mitk::DataNode::Pointer m_RegistrationLandmarksNode; + + void OnAddPivotPose(); + void OnComputePivot(); + void OnUseComputedPivotPoint(); + void SetToolToCalibrate(); + void SetCalibrationPointer(); + void UpdateTrackingTimer(); + void AddLandmark(); + void SaveCalibratedTool(); + void OnToolCalibrationMethodChanged(int index); + void OnStartManualToolTipCalibration(); + void OnRetrieveDataForManualTooltipManipulation(); + void OnProcessManualTooltipEditDialogCloseRequest(); + void OnRunSingleRefToolCalibrationClicked(); + void OnLoginSingleRefToolNavigationDataClicked(); + void OnSetNewToolTipPosButtonClicked(); + + +protected: + + virtual void SetFocus(); + + void UpdateOffsetCoordinates(); + + int m_IndexCurrentCalibrationMethod; + + Ui::IGTNavigationToolCalibrationControls m_Controls; + + //some general members + mitk::NavigationTool::Pointer m_ToolToCalibrate; //<<< tool that will be calibrated + int m_IDToolToCalibrate; //<<< id of tool that will be calibrated (of the navigation data source) + mitk::NavigationDataSource::Pointer m_NavigationDataSourceOfToolToCalibrate; //<<< navigation data source of the tool that will be calibrated + mitk::NavigationDataSource::Pointer m_NavigationDataSourceOfCalibrationPointer; //<<< navigation data source of the calibration pointer + mitk::DataNode::Pointer m_ToolSurfaceInToolCoordinatesDataNode; //<<< holds the tool surface in tool coordinates (for preview purposes) + int m_IDCalibrationPointer; //<<< id of the calibration pointer (of the corresponding navigation data source) + QTimer* m_TrackingTimer; //<<< tracking timer that updates the status widgets + void ApplyToolTipTransform(mitk::NavigationData::Pointer ToolTipTransformInToolCoordinates, std::string message = "Tool was updated with the calibrated tool tip!"); //<<< applys the given tool tip transform to the tool to calibrate + bool CheckInitialization(bool CalibrationPointerRequired = true); //<<< checks if the tool to calibrate and (if required) the calibration pointer is initialized. Displays a warning and returns false if not. + mitk::NavigationData::Pointer m_ComputedToolTipTransformation; //<<< holds the new tooltip transformation after it was computed to write it into the tool later + + // members and helper methods for pivot tool calibration + std::vector m_PivotPoses; + void AddPivotPose(); + void ClearOldPivot(); + void UpdatePivotCount(); + bool m_OnAddPivotPoseClicked; + int PivotCount; + + // members and helper methods for manual tool calibration + void UpdateManualToolTipCalibrationView(); + QmitkNavigationToolCreationAdvancedWidget* m_ManualToolTipEditWidget; + + // members and helper methods for single reference tool calibration + void LoginSingleRefToolNavigationData(); + std::vector< mitk::Point3D > m_LoggedNavigationDataOffsets; + std::vector< mitk::NavigationData::Pointer > m_LoggedNavigationDataDifferences; + bool m_OnLoginSingleRefToolNavigationDataClicked; + int m_NumberOfNavigationData; + int m_NumberOfNavigationDataCounter; + mitk::Point3D m_ResultOffsetVector; + + // members and helper methods for tool tip preview + mitk::DataNode::Pointer m_ToolTipPointPreview; //<<< Data node of the tool tip preview + void ShowToolTipPreview(mitk::NavigationData::Pointer ToolTipInTrackingCoordinates); //<<< Adds a preview of the tool tip into the data storage + void RemoveToolTipPreview(); //<<< Removes the preview + + // members for the tool landmark calibration + mitk::PointSet::Pointer m_CalibrationLandmarks; + mitk::DataNode::Pointer m_CalibrationLandmarksNode; + mitk::PointSet::Pointer m_RegistrationLandmarks; + mitk::DataNode::Pointer m_RegistrationLandmarksNode; }; #endif // IGTNavigationToolCalibration_h diff --git a/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibrationControls.ui b/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibrationControls.ui index 99779e5f67..d4688a157a 100644 --- a/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibrationControls.ui +++ b/Plugins/org.mitk.gui.qt.igttracking/src/internal/IGTNavigationToolCalibrationControls.ui @@ -1,976 +1,1004 @@ IGTNavigationToolCalibrationControls 0 0 430 958 0 0 QmitkTemplate <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt; font-weight:600;">Navigation Tool Calibration</span></p></body></html> Tool to calibrate 0 50 16777215 50 Calibration pointer 0 30 true 0 Initialization <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:7.8pt; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:8pt; text-decoration: underline;">Choose Tracking Device and Tools</span></p></body></html> Qt::Horizontal 120 0 Tool to Calibrate: Qt::Horizontal 40 20 <none> Qt::Horizontal 40 20 150 0 Use as Tool to Calibrate Qt::Horizontal 120 0 Calibration Pointer: Qt::Horizontal 40 20 <none> Qt::Horizontal 40 20 150 0 Use as Calibration Pointer Qt::Horizontal Qt::Vertical 20 586 Tool Tip Calibration 50 false Calibration Method: Qt::Horizontal 40 20 Manual Single Reference Tool Multiple Tools Reference Pivoting Qt::Horizontal 0 Current Tool Tip Translation: true true Current Tool Tip Orientation: true true Qt::Horizontal 40 20 Start Edit Tooltip 0 0 319 160 319 160 QFrame { border-image: url(:/IGTNavigationToolCalibration/Description.svg); } QFrame::Box QFrame::Plain 1 0 0 Current Offset between Tool to calibrate and Calibration pointer: Pos: 8 Qt::AlignCenter Rot: 8 Qt::AlignCenter Qt::Horizontal 0 0 Number of tracking data to collect: 0 0 10000 100 Qt::AlignCenter 0 0 1: Collect Navigation Data Qt::Horizontal Invert calibration transformation Calibrate position true Calibrate orientation true false 2: Run Calibration Qt::Horizontal 0 0 New Tool Tip Position and/or Orientation of Tool to calibrate: Pos: 8 Qt::AlignCenter Rot: 8 Qt::AlignCenter false 3: Set New Tool Tip Position and/or Tool Orientation Qt::Vertical 20 40 + + + + + + Number of pivot poses to collect: + + + + + + + 10 + + + 1000 + + + 100 + + + + + Number of saved poses: 0 Qt::Horizontal 40 20 - Add Current Pose + Collect Pivot Poses Calibration Result: Qt::Horizontal 40 20 175 0 - Comute Pivot Point + Compute Pivot Point Qt::Horizontal 40 20 false 0 0 175 0 Use Computed Pivot Point Qt::Vertical 20 99 Tool Landmark Calibration <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:7.8pt; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:8pt; text-decoration: underline;">Calibrate tool by marking the landmarks with the pointer</span></p></body></html> Registration Landmarks 0 170 Add Pointer Position Calibration Landmarks Qt::Vertical 20 259 Qt::Horizontal 40 20 Save Calibrated Navigation Tool + tabWidget + label + groupBox + groupBox_2 QmitkNavigationDataSourceSelectionWidget QWidget
QmitkNavigationDataSourceSelectionWidget.h
1 QmitkToolTrackingStatusWidget QWidget
QmitkToolTrackingStatusWidget.h
1 QmitkPointListWidget QWidget
QmitkPointListWidget.h
1