diff --git a/Modules/IGT/DataManagement/mitkNavigationTool.cpp b/Modules/IGT/DataManagement/mitkNavigationTool.cpp index 0ef596e00b..870332827a 100644 --- a/Modules/IGT/DataManagement/mitkNavigationTool.cpp +++ b/Modules/IGT/DataManagement/mitkNavigationTool.cpp @@ -1,354 +1,362 @@ /*=================================================================== 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 "mitkNavigationTool.h" #include "mitkIGTException.h" #include "mitkNavigationData.h" #include "Poco/File.h" #include "mitkUnspecifiedTrackingTypeInformation.h" #include "vtkSphereSource.h" #include "vtkConeSource.h" #include "vtkLineSource.h" #include "vtkCylinderSource.h" #include "vtkTransformPolyDataFilter.h" #include #include "mitkTextAnnotation3D.h" #include "mitkManualPlacementAnnotationRenderer.h" #include "mitkBaseRenderer.h" mitk::NavigationTool::NavigationTool() : m_Identifier("None"), m_Type(mitk::NavigationTool::Unknown), m_CalibrationFile("none"), m_SerialNumber(""), m_TrackingDeviceType(mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName()), m_ToolLandmarks(mitk::PointSet::New()), m_ToolControlPoints(mitk::PointSet::New()), m_ToolAxisOrientation(mitk::Quaternion(0, 0, 0, 1)) { m_ToolTipPosition[0] = 0; m_ToolTipPosition[1] = 0; m_ToolTipPosition[2] = 0; SetDefaultSurface(); } itk::LightObject::Pointer mitk::NavigationTool::InternalClone() const { Self::Pointer tool = new Self(*this); tool->UnRegister(); return tool.GetPointer(); } mitk::NavigationTool::NavigationTool(const NavigationTool &other) : Superclass() { this->m_Identifier = other.m_Identifier; this->m_Type = other.m_Type; if (other.m_DataNode.IsNotNull()) { this->m_DataNode = other.m_DataNode->Clone(); this->m_DataNode->SetName(other.m_DataNode->GetName()); if (other.m_DataNode->GetData()) { this->m_DataNode->SetData(dynamic_cast(other.m_DataNode->GetData()->Clone().GetPointer())); } } if (other.m_SpatialObject.IsNotNull()) this->m_SpatialObject = other.m_SpatialObject->Clone(); this->m_CalibrationFile = other.m_CalibrationFile; this->m_SerialNumber = other.m_SerialNumber; this->m_TrackingDeviceType = other.m_TrackingDeviceType; if (other.m_ToolLandmarks.IsNotNull()) this->m_ToolLandmarks = other.m_ToolLandmarks->Clone(); if (other.m_ToolControlPoints.IsNotNull()) this->m_ToolControlPoints = other.m_ToolControlPoints->Clone(); this->m_ToolTipPosition = other.m_ToolTipPosition; this->m_ToolAxisOrientation = other.m_ToolAxisOrientation; } mitk::NavigationTool::~NavigationTool() { } mitk::Point3D mitk::NavigationTool::GetToolAxis() { // The tool axis in the sensor coordinate system is defined as the negative z-axis mitk::Vector3D toolAxisSensorCoordinateSystem; mitk::FillVector3D(toolAxisSensorCoordinateSystem, 0.0, 0.0, -1.0); // Apply inverse tool axis transform to calculate tool axis vnl_vector_fixed toolAxisVector = m_ToolAxisOrientation.inverse().rotate(toolAxisSensorCoordinateSystem.GetVnlVector()); // Transfer to mitk::Point3D mitk::Point3D toolAxis; toolAxis[0] = toolAxisVector[0]; toolAxis[1] = toolAxisVector[1]; toolAxis[2] = toolAxisVector[2]; return toolAxis; } void mitk::NavigationTool::SetToolAxis(mitk::Point3D toolAxis) { // The tool axis in the sensor coordinate system is defined as the negative z-axis mitk::Vector3D toolAxisSensorCoordinateSystem; mitk::FillVector3D(toolAxisSensorCoordinateSystem, 0.0, 0.0, -1.0); // Normalize the tool axis as obtained by a tool axis calibration mitk::Vector3D toolAxisFromCalibration; mitk::FillVector3D(toolAxisFromCalibration, toolAxis[0], toolAxis[1], toolAxis[2]); toolAxisFromCalibration.Normalize(); - // Determine rotation angle - mitk::ScalarType rotationAngle = acos(toolAxisSensorCoordinateSystem*toolAxisFromCalibration); - // Determine rotation axis - mitk::Vector3D rotationAxis = itk::CrossProduct(toolAxisSensorCoordinateSystem, toolAxisFromCalibration); - // Calculate transform - itk::AffineTransform::Pointer sensorToToolAxisOrientation = itk::AffineTransform::New(); - sensorToToolAxisOrientation->Rotate3D(rotationAxis, rotationAngle); - // transfer to quaternion notation. Note that the vnl_quaternion expects the matrix in row major format, hence the transpose - mitk::Quaternion toolAxisTransform(sensorToToolAxisOrientation->GetMatrix().GetVnlMatrix().transpose()); - // Update the tool tip orientation - m_ToolAxisOrientation = toolAxisTransform; + // if tool axis to be set is different to the default tool axis (0,0,-1) calculate the tool axis orientation, otherwise set it to identity + if (toolAxisSensorCoordinateSystem == toolAxisFromCalibration) + { + m_ToolAxisOrientation = mitk::Quaternion(0,0,0,1); + } + else + { + // Determine rotation angle + mitk::ScalarType rotationAngle = acos(toolAxisSensorCoordinateSystem*toolAxisFromCalibration); + // Determine rotation axis + mitk::Vector3D rotationAxis = itk::CrossProduct(toolAxisSensorCoordinateSystem, toolAxisFromCalibration); + // Calculate transform + itk::AffineTransform::Pointer sensorToToolAxisOrientation = itk::AffineTransform::New(); + sensorToToolAxisOrientation->Rotate3D(rotationAxis, rotationAngle); + // transfer to quaternion notation. Note that the vnl_quaternion expects the matrix in row major format, hence the transpose + mitk::Quaternion toolAxisTransform(sensorToToolAxisOrientation->GetMatrix().GetVnlMatrix().transpose()); + // Update the tool axis orientation + m_ToolAxisOrientation = toolAxisTransform; + } } mitk::AffineTransform3D::Pointer mitk::NavigationTool::GetToolTipTransform() { mitk::NavigationData::Pointer returnValue = mitk::NavigationData::New(); returnValue->SetPosition(this->m_ToolTipPosition); returnValue->SetOrientation(this->m_ToolAxisOrientation); return returnValue->GetAffineTransform3D(); } void mitk::NavigationTool::Graft(const DataObject *data) { // Attempt to cast data to an NavigationData const Self* nd; try { nd = dynamic_cast(data); } catch (...) { mitkThrowException(mitk::IGTException) << "mitk::NavigationData::Graft cannot cast " << typeid(data).name() << " to " << typeid(const Self *).name(); } if (!nd) { // pointer could not be cast back down mitkThrowException(mitk::IGTException) << "mitk::NavigationData::Graft cannot cast " << typeid(data).name() << " to " << typeid(const Self *).name(); } // Now copy anything that is needed m_Identifier = nd->GetIdentifier(); m_Type = nd->GetType(); m_DataNode->SetName(nd->GetDataNode()->GetName()); m_DataNode->SetData(nd->GetDataNode()->GetData()); m_SpatialObject = nd->GetSpatialObject(); m_CalibrationFile = nd->GetCalibrationFile(); m_SerialNumber = nd->GetSerialNumber(); m_TrackingDeviceType = nd->GetTrackingDeviceType(); m_ToolLandmarks = nd->GetToolLandmarks(); m_ToolControlPoints = nd->GetToolControlPoints(); m_ToolTipPosition = nd->GetToolTipPosition(); m_ToolAxisOrientation = nd->GetToolAxisOrientation(); } bool mitk::NavigationTool::IsToolTipSet() { if ((m_ToolTipPosition[0] == 0) && (m_ToolTipPosition[1] == 0) && (m_ToolTipPosition[2] == 0) && (m_ToolAxisOrientation.x() == 0) && (m_ToolAxisOrientation.y() == 0) && (m_ToolAxisOrientation.z() == 0) && (m_ToolAxisOrientation.r() == 1)) return false; else return true; } void mitk::NavigationTool::SetCalibrationFile(const std::string filename) { //check if file does exist: if (filename == "") { m_CalibrationFile = "none"; } else { Poco::File myFile(filename); if (myFile.exists()) m_CalibrationFile = filename; else m_CalibrationFile = "none"; } } std::string mitk::NavigationTool::GetToolName() { if (this->m_DataNode.IsNull()) { return ""; } else { return m_DataNode->GetName(); } } mitk::Surface::Pointer mitk::NavigationTool::GetToolSurface() { if (this->m_DataNode.IsNull()) { return nullptr; } else if (this->m_DataNode->GetData() == nullptr) { return nullptr; } else { return dynamic_cast(m_DataNode->GetData()); } } void mitk::NavigationTool::SetDefaultSurface() { if (m_DataNode.IsNull()) m_DataNode = mitk::DataNode::New(); mitk::Surface::Pointer mySphere = mitk::Surface::New(); double axisLength = 5.; vtkSmartPointer vtkSphere = vtkSmartPointer::New(); vtkSmartPointer vtkCone = vtkSmartPointer::New(); vtkSmartPointer vtkCylinder = vtkSmartPointer::New(); vtkSmartPointer axis = vtkSmartPointer::New(); vtkSmartPointer vtkLine = vtkSmartPointer::New(); vtkSmartPointer vtkLine2 = vtkSmartPointer::New(); vtkSmartPointer vtkLine3 = vtkSmartPointer::New(); vtkSmartPointer appendPolyData = vtkSmartPointer::New(); vtkSmartPointer surface = vtkSmartPointer::New(); //Y-Axis (start with y, cause cylinder is oriented in y by vtk default...) vtkCone->SetDirection(0, 1, 0); vtkCone->SetHeight(1.0); vtkCone->SetRadius(0.4f); vtkCone->SetResolution(16); vtkCone->SetCenter(0.0, axisLength, 0.0); vtkCone->Update(); vtkCylinder->SetRadius(0.05); vtkCylinder->SetHeight(axisLength); vtkCylinder->SetCenter(0.0, 0.5*axisLength, 0.0); vtkCylinder->Update(); appendPolyData->AddInputData(vtkCone->GetOutput()); appendPolyData->AddInputData(vtkCylinder->GetOutput()); appendPolyData->Update(); axis->DeepCopy(appendPolyData->GetOutput()); //y symbol vtkLine->SetPoint1(-0.5, axisLength + 2., 0.0); vtkLine->SetPoint2(0.0, axisLength + 1.5, 0.0); vtkLine->Update(); vtkLine2->SetPoint1(0.5, axisLength + 2., 0.0); vtkLine2->SetPoint2(-0.5, axisLength + 1., 0.0); vtkLine2->Update(); appendPolyData->AddInputData(vtkLine->GetOutput()); appendPolyData->AddInputData(vtkLine2->GetOutput()); appendPolyData->AddInputData(axis); appendPolyData->Update(); surface->DeepCopy(appendPolyData->GetOutput()); //X-axis vtkSmartPointer XTransform = vtkSmartPointer::New(); XTransform->RotateZ(-90); vtkSmartPointer TrafoFilter = vtkSmartPointer::New(); TrafoFilter->SetTransform(XTransform); TrafoFilter->SetInputData(axis); TrafoFilter->Update(); //x symbol vtkLine->SetPoint1(axisLength + 2., -0.5, 0.0); vtkLine->SetPoint2(axisLength + 1., 0.5, 0.0); vtkLine->Update(); vtkLine2->SetPoint1(axisLength + 2., 0.5, 0.0); vtkLine2->SetPoint2(axisLength + 1., -0.5, 0.0); vtkLine2->Update(); appendPolyData->AddInputData(vtkLine->GetOutput()); appendPolyData->AddInputData(vtkLine2->GetOutput()); appendPolyData->AddInputData(TrafoFilter->GetOutput()); appendPolyData->AddInputData(surface); appendPolyData->Update(); surface->DeepCopy(appendPolyData->GetOutput()); //Z-axis vtkSmartPointer ZTransform = vtkSmartPointer::New(); ZTransform->RotateX(90); TrafoFilter->SetTransform(ZTransform); TrafoFilter->SetInputData(axis); TrafoFilter->Update(); //z symbol vtkLine->SetPoint1(-0.5, 0.0, axisLength + 2.); vtkLine->SetPoint2(0.5, 0.0, axisLength + 2.); vtkLine->Update(); vtkLine2->SetPoint1(-0.5, 0.0, axisLength + 2.); vtkLine2->SetPoint2(0.5, 0.0, axisLength + 1.); vtkLine2->Update(); vtkLine3->SetPoint1(0.5, 0.0, axisLength + 1.); vtkLine3->SetPoint2(-0.5, 0.0, axisLength + 1.); vtkLine3->Update(); appendPolyData->AddInputData(vtkLine->GetOutput()); appendPolyData->AddInputData(vtkLine2->GetOutput()); appendPolyData->AddInputData(vtkLine3->GetOutput()); appendPolyData->AddInputData(TrafoFilter->GetOutput()); appendPolyData->AddInputData(surface); appendPolyData->Update(); surface->DeepCopy(appendPolyData->GetOutput()); //Center vtkSphere->SetRadius(0.5f); vtkSphere->SetCenter(0.0, 0.0, 0.0); vtkSphere->Update(); appendPolyData->AddInputData(vtkSphere->GetOutput()); appendPolyData->AddInputData(surface); appendPolyData->Update(); surface->DeepCopy(appendPolyData->GetOutput()); //Scale vtkSmartPointer ScaleTransform = vtkSmartPointer::New(); ScaleTransform->Scale(20., 20., 20.); TrafoFilter->SetTransform(ScaleTransform); TrafoFilter->SetInputData(surface); TrafoFilter->Update(); mySphere->SetVtkPolyData(TrafoFilter->GetOutput()); this->GetDataNode()->SetData(mySphere); } std::string mitk::NavigationTool::GetStringWithAllToolInformation() const { std::stringstream _info; _info << " Identifier: " << this->m_Identifier << "\n" << " NavigationToolType: " << m_Type << "\n" << " Calibration file: " << m_CalibrationFile << "\n" << " Serial number: " << m_SerialNumber << "\n" << " TrackingDeviceType: " << m_TrackingDeviceType << "\n" << " ToolTip Position: " << m_ToolTipPosition << "\n" << " Tool Axis Orientation: " << m_ToolAxisOrientation << "\n" << " Tool Axis: " << m_ToolAxisOrientation.inverse().rotate(vnl_vector_fixed(0.0,0.0,-1.0)) ; return _info.str(); } diff --git a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibration.cpp b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibration.cpp index b3669e14d4..b0b4612b89 100644 --- a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibration.cpp +++ b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibration.cpp @@ -1,729 +1,748 @@ /*=================================================================== 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 "QmitkIGTNavigationToolCalibration.h" // mitk #include #include #include #include #include #include #include // Qt #include #include //vtk #include const std::string QmitkIGTNavigationToolCalibration::VIEW_ID = "org.mitk.views.igtnavigationtoolcalibration"; QmitkIGTNavigationToolCalibration::QmitkIGTNavigationToolCalibration() { m_ToolTransformationWidget = new QmitkInteractiveTransformationWidget(); } QmitkIGTNavigationToolCalibration::~QmitkIGTNavigationToolCalibration() { //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(nullptr); m_Controls.m_CalibrationLandmarkWidget->SetPointSetNode(nullptr); //clean up data storage this->GetDataStorage()->Remove(m_ToolTipPointPreview); delete m_ToolTransformationWidget; } void QmitkIGTNavigationToolCalibration::SetFocus() { } void QmitkIGTNavigationToolCalibration::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 QmitkIGTNavigationToolCalibration::CreateQtPartControl(QWidget *parent) { 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(OnComputePivot())); connect(m_Controls.m_UseComputedPivotPoint, SIGNAL(clicked()), this, SLOT(OnUseComputedPivotPoint())); connect(m_Controls.m_StartEditTooltipManually, SIGNAL(clicked()), this, SLOT(OnStartManualToolTipCalibration())); connect(m_Controls.m_GetPositions, SIGNAL(clicked()), this, SLOT(OnGetPositions())); connect(m_Controls.m_ToolAxis_X, SIGNAL(valueChanged(double)), this, SLOT(OnToolAxisSpinboxChanged())); connect(m_Controls.m_ToolAxis_Y, SIGNAL(valueChanged(double)), this, SLOT(OnToolAxisSpinboxChanged())); connect(m_Controls.m_ToolAxis_Z, SIGNAL(valueChanged(double)), this, SLOT(OnToolAxisSpinboxChanged())); connect(m_Controls.m_CalibrateToolAxis, SIGNAL(clicked()), this, SLOT(OnCalibrateToolAxis())); connect((QObject*)(m_ToolTransformationWidget), SIGNAL(EditToolTipFinished(mitk::AffineTransform3D::Pointer)), this, SLOT(OnManualEditToolTipFinished(mitk::AffineTransform3D::Pointer))); 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_LoggedNavigationDataDifferences = std::vector< mitk::NavigationData::Pointer >(); } void QmitkIGTNavigationToolCalibration::OnRunSingleRefToolCalibrationClicked() { if (!CheckInitialization()) { return; } mitk::NavigationData::Pointer ToolTipTransform = mitk::NavigationData::New(); 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++) { 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); } if (m_Controls.m_CalibrateOrientation->isChecked()) { //2: Compute mean orientation mitk::Quaternion meanOrientation; std::vector allOrientations = std::vector (); for (std::size_t 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 QmitkIGTNavigationToolCalibration::OnLoginSingleRefToolNavigationDataClicked() { if (!CheckInitialization()) { return; } //reset old data m_LoggedNavigationDataOffsets.clear(); m_LoggedNavigationDataDifferences.clear(); m_OnLoginSingleRefToolNavigationDataClicked = true; m_Controls.m_CollectNavigationDataButton->setEnabled(false); m_NumberOfNavigationData = m_Controls.m_NumberOfNavigationDataToCollect->value(); MITK_INFO << "Collecting " << m_NumberOfNavigationData << " NavigationData ... " << endl; } void QmitkIGTNavigationToolCalibration::LoginSingleRefToolNavigationData() { if (!CheckInitialization()) { return; } if (m_NumberOfNavigationDataCounter < m_NumberOfNavigationData) { //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); //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()) { // 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++; } 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 QmitkIGTNavigationToolCalibration::OnSetNewToolTipPosButtonClicked() { ApplyToolTipTransform(m_ComputedToolTipTransformation); RemoveToolTipPreview(); } void QmitkIGTNavigationToolCalibration::ClearOldPivot() { mitk::NavigationData::Pointer tempND = mitk::NavigationData::New(); this->ApplyToolTipTransform(tempND); UpdateManualToolTipCalibrationView(); //m_ManualToolTipEditWidget->hide(); //TODO this->GetDataStorage()->Remove(m_ToolSurfaceInToolCoordinatesDataNode); } void QmitkIGTNavigationToolCalibration::OnAddPivotPose() { ClearOldPivot(); //When the collect Poses Button is Clicked m_OnAddPivotPoseClicked = true; m_NumberOfNavigationData = m_Controls.m_PosesToCollect->value(); } void QmitkIGTNavigationToolCalibration::AddPivotPose() { //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 QmitkIGTNavigationToolCalibration::OnComputePivot() { mitk::PivotCalibration::Pointer myPivotCalibration = mitk::PivotCalibration::New(); for (std::size_t i = 0; i < this->m_PivotPoses.size(); i++) { myPivotCalibration->AddNavigationData(m_PivotPoses.at(i)); } QString resultString; if (myPivotCalibration->ComputePivotResult()) { 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(mitk::Quaternion(0,0,0,1)); 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"); //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 computation failed!"; } MITK_INFO << resultString.toStdString().c_str(); m_Controls.m_ResultText->setText(resultString); } void QmitkIGTNavigationToolCalibration::UpdatePivotCount() { PivotCount = 0; while (!m_PivotPoses.empty()) { m_PivotPoses.pop_back(); } m_Controls.m_PoseNumber->setText(QString::number(PivotCount)); } void QmitkIGTNavigationToolCalibration::OnUseComputedPivotPoint() { RemoveToolTipPreview(); QString resultString = QString("Pivoted tool tip transformation was written to the tool ") + m_ToolToCalibrate->GetToolName().c_str(); ApplyToolTipTransform(m_ComputedToolTipTransformation, resultString.toStdString()); m_Controls.m_ResultText->setText(resultString); UpdatePivotCount(); } void QmitkIGTNavigationToolCalibration::ApplyToolTipTransform(mitk::NavigationData::Pointer ToolTipTransformInToolCoordinates, std::string message) { if (!CheckInitialization(false)) { return; } //Update tool in tool storage m_ToolToCalibrate->SetToolTipPosition(ToolTipTransformInToolCoordinates->GetPosition()); m_ToolToCalibrate->SetToolAxisOrientation(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->SetToolTipPosition(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 QmitkIGTNavigationToolCalibration::ShowToolTipPreview(mitk::NavigationData::Pointer ToolTipInTrackingCoordinates) { if(m_ToolTipPointPreview.IsNull()) { m_ToolTipPointPreview = mitk::DataNode::New(); m_ToolTipPointPreview->SetName("Modified Tool Tip Preview"); mitk::Color blue; blue.SetBlue(1); m_ToolTipPointPreview->SetColor(blue); mitk::Surface::Pointer mySphere = mitk::Surface::New(); vtkSmartPointer vtkData = vtkSmartPointer::New(); vtkData->SetRadius(3.0f); vtkData->SetCenter(0.0, 0.0, 0.0); vtkData->Update(); mySphere->SetVtkPolyData(vtkData->GetOutput()); m_ToolTipPointPreview->SetData(mySphere); this->GetDataStorage()->Add(m_ToolTipPointPreview); } m_ToolTipPointPreview->GetData()->GetGeometry()->SetIndexToWorldTransform(ToolTipInTrackingCoordinates->GetAffineTransform3D()); } void QmitkIGTNavigationToolCalibration::RemoveToolTipPreview() { this->GetDataStorage()->Remove(m_ToolTipPointPreview.GetPointer()); } void QmitkIGTNavigationToolCalibration::UpdateManualToolTipCalibrationView() { if (m_ToolToCalibrate.IsNull()) { return; } //parse human readable transformation data and display it std::stringstream translation; std::stringstream orientation; translation << m_ToolToCalibrate->GetToolTipPosition(); orientation << "Quaternion: (" << m_ToolToCalibrate->GetToolAxisOrientation() << ")" << std::endl; orientation << std::endl; orientation << "Euler Angles [rad]: (" << m_ToolToCalibrate->GetToolAxisOrientation().rotation_euler_angles() << ")" << std::endl; orientation << std::endl; orientation << "Matrix:" << std::endl; vnl_matrix_fixed rotMatrix = m_ToolToCalibrate->GetToolAxisOrientation().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 QmitkIGTNavigationToolCalibration::OnStartManualToolTipCalibration() { if (!CheckInitialization(false)) { return; } m_ToolTransformationWidget->SetToolToEdit(m_ToolToCalibrate); m_ToolTransformationWidget->SetDefaultOffset(m_ToolToCalibrate->GetToolTipPosition()); m_ToolTransformationWidget->SetDefaultRotation(m_ToolToCalibrate->GetToolAxisOrientation()); m_ToolTransformationWidget->open(); } void QmitkIGTNavigationToolCalibration::OnManualEditToolTipFinished(mitk::AffineTransform3D::Pointer toolTip) { //This function is called, when the toolTipEdit view is closed. //if user pressed cancle, nullptr is returned. Do nothing. Else, set values. if (toolTip) { mitk::NavigationData::Pointer tempND = mitk::NavigationData::New(toolTip);//Convert to Navigation data for simple transversion to quaternion QString resultString = QString("Manual edited values are written to ") + m_ToolToCalibrate->GetToolName().c_str(); ApplyToolTipTransform(tempND, resultString.toStdString()); m_Controls.m_ResultText->setText(resultString); } UpdateManualToolTipCalibrationView(); } void QmitkIGTNavigationToolCalibration::OnGetPositions() { if (!CheckInitialization(true)) { return; } //Navigation Data from Tool which should be calibrated if (!m_AxisCalibration_ToolToCalibrate) m_AxisCalibration_ToolToCalibrate = mitk::NavigationData::New(); m_AxisCalibration_ToolToCalibrate->Graft(m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource()->GetOutput(m_IDToolToCalibrate)); //Navigation Data from calibration pointer tool if (!m_AxisCalibration_NavDataCalibratingTool) m_AxisCalibration_NavDataCalibratingTool = mitk::NavigationData::New(); m_AxisCalibration_NavDataCalibratingTool->Graft(m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource()->GetOutput(m_IDCalibrationPointer)); MITK_DEBUG << "Positions for tool axis calibration:"; MITK_DEBUG << " ToolTip: " << m_AxisCalibration_ToolToCalibrate->GetPosition() << ","; MITK_DEBUG << " Rotation: \n" << m_AxisCalibration_ToolToCalibrate->GetRotationMatrix(); MITK_DEBUG << " End of the tool: " << m_AxisCalibration_NavDataCalibratingTool->GetPosition(); QString _label = "Position recorded: " + QString::number(m_AxisCalibration_NavDataCalibratingTool->GetPosition()[0], 'f', 1) + ", " + QString::number(m_AxisCalibration_NavDataCalibratingTool->GetPosition()[1], 'f', 1) + ", " + QString::number(m_AxisCalibration_NavDataCalibratingTool->GetPosition()[2], 'f', 1); m_Controls.m_ToolAxisPositionLabel->setText(_label); } void QmitkIGTNavigationToolCalibration::OnCalibrateToolAxis() { if (!m_AxisCalibration_ToolToCalibrate || !m_AxisCalibration_NavDataCalibratingTool) { MITK_ERROR << "Please record position first."; return; } //Calculate the tool tip //here is an explanation, what is happening here: /* The axis is equal to the (tool tip) minus the (end of the tool) in tool coordinates of the tool which should be calibrated. The tool tip is defined as the origin of the tool coordinate system. The end of the tool is recorded by the calibration pointer's position and is transformed into the coordinate system of the tool to be calibrated Normalize it. */ //m_CalibratedToolAxis = -m_AxisCalibration_ToolToCalibrate->TransformPoint(m_AxisCalibration_NavDataCalibratingTool->GetPosition()).GetVectorFromOrigin(); m_CalibratedToolAxis = -m_AxisCalibration_ToolToCalibrate->GetInverse()->TransformPoint(m_AxisCalibration_NavDataCalibratingTool->GetPosition()).GetVectorFromOrigin(); MITK_DEBUG << "Tool Endpoint in Tool coordinates: " << m_CalibratedToolAxis; m_CalibratedToolAxis.Normalize(); MITK_DEBUG << "Tool Axis: " << m_CalibratedToolAxis; m_ToolToCalibrate->SetToolAxis(m_CalibratedToolAxis); // Update TrackingTool m_ComputedToolTipTransformation->SetPosition(m_ToolToCalibrate->GetToolTipPosition()); m_ComputedToolTipTransformation->SetOrientation(m_ToolToCalibrate->GetToolAxisOrientation()); ApplyToolTipTransform(m_ComputedToolTipTransformation); //Update GUI QString calibratedToolAxisString = "Tool Axis: " + QString::number(m_CalibratedToolAxis.GetElement(0), 'f', 3) + ", " + QString::number(m_CalibratedToolAxis.GetElement(1), 'f', 3) + ", " + QString::number(m_CalibratedToolAxis.GetElement(2), 'f', 3); m_Controls.m_ToolAxisCalibrationLabel->setText(calibratedToolAxisString); //Block QT signals, we don't want to emit SpinboxChanged on the first value to overwrite the next ones m_Controls.m_ToolAxis_X->blockSignals(true); m_Controls.m_ToolAxis_Y->blockSignals(true); m_Controls.m_ToolAxis_Z->blockSignals(true); m_Controls.m_ToolAxis_X->setValue(m_CalibratedToolAxis[0]); m_Controls.m_ToolAxis_Y->setValue(m_CalibratedToolAxis[1]); m_Controls.m_ToolAxis_Z->setValue(m_CalibratedToolAxis[2]); m_Controls.m_ToolAxis_X->blockSignals(false); m_Controls.m_ToolAxis_Y->blockSignals(false); m_Controls.m_ToolAxis_Z->blockSignals(false); } void QmitkIGTNavigationToolCalibration::OnToolAxisSpinboxChanged() { m_CalibratedToolAxis.SetElement(0, m_Controls.m_ToolAxis_X->value()); m_CalibratedToolAxis.SetElement(1, m_Controls.m_ToolAxis_Y->value()); m_CalibratedToolAxis.SetElement(2, m_Controls.m_ToolAxis_Z->value()); m_ToolToCalibrate->SetToolAxis(m_CalibratedToolAxis); + // Update TrackingTool + if (m_ComputedToolTipTransformation.IsNull()) + { + m_ComputedToolTipTransformation = mitk::NavigationData::New(); + } + m_ComputedToolTipTransformation->SetPosition(m_ToolToCalibrate->GetToolTipPosition()); + m_ComputedToolTipTransformation->SetOrientation(m_ToolToCalibrate->GetToolAxisOrientation()); + ApplyToolTipTransform(m_ComputedToolTipTransformation); + MITK_INFO << "Tool axis changed to " << m_CalibratedToolAxis; } void QmitkIGTNavigationToolCalibration::SetToolToCalibrate() { m_IDToolToCalibrate = m_Controls.m_SelectionWidget->GetSelectedToolID(); if (m_IDToolToCalibrate == -1) //no valid tool to calibrate { m_Controls.m_CalToolLabel->setText(""); m_Controls.m_StatusWidgetToolToCalibrate->RemoveStatusLabels(); m_TrackingTimer->stop(); } else { m_ToolToCalibrate = m_Controls.m_SelectionWidget->GetSelectedNavigationTool(); 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(); - + // update tool tip and rotation information for tool tip calibration tab UpdateManualToolTipCalibrationView(); + // update tool axis information for tool axis calibration tab + mitk::Point3D toolAxis = m_ToolToCalibrate->GetToolAxis(); + m_CalibratedToolAxis[0] = toolAxis[0]; + m_CalibratedToolAxis[1] = toolAxis[1]; + m_CalibratedToolAxis[2] = toolAxis[2]; + m_Controls.m_ToolAxis_X->blockSignals(true); m_Controls.m_ToolAxis_Y->blockSignals(true); m_Controls.m_ToolAxis_Z->blockSignals(true); + m_Controls.m_ToolAxis_X->setValue(m_CalibratedToolAxis[0]); + m_Controls.m_ToolAxis_Y->setValue(m_CalibratedToolAxis[1]); + m_Controls.m_ToolAxis_Z->setValue(m_CalibratedToolAxis[2]); + m_Controls.m_ToolAxis_X->blockSignals(false); m_Controls.m_ToolAxis_Y->blockSignals(false); m_Controls.m_ToolAxis_Z->blockSignals(false); //start updating timer for status widgets, etc. if (!m_TrackingTimer->isActive()) m_TrackingTimer->start(100); } } void QmitkIGTNavigationToolCalibration::SetCalibrationPointer() { m_IDCalibrationPointer = m_Controls.m_SelectionWidget->GetSelectedToolID(); if (m_IDCalibrationPointer == -1) { m_Controls.m_PointerLabel->setText(""); m_Controls.m_StatusWidgetCalibrationPointer->RemoveStatusLabels(); m_TrackingTimer->stop(); } else { m_NavigationDataSourceOfCalibrationPointer = m_Controls.m_SelectionWidget->GetSelectedNavigationDataSource(); 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 QmitkIGTNavigationToolCalibration::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()) { //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))); 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))); //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 QmitkIGTNavigationToolCalibration::UpdateTrackingTimer() { m_Controls.m_StatusWidgetToolToCalibrate->Refresh(); m_Controls.m_StatusWidgetCalibrationPointer->Refresh(); if (m_OnLoginSingleRefToolNavigationDataClicked) LoginSingleRefToolNavigationData(); if (m_OnAddPivotPoseClicked) AddPivotPose(); // 1 == Single Reference Calibration Method if (m_IndexCurrentCalibrationMethod == 1) UpdateOffsetCoordinates(); } void QmitkIGTNavigationToolCalibration::AddLandmark() { 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]; 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); //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); } void QmitkIGTNavigationToolCalibration::SaveCalibratedTool() { if (m_ToolToCalibrate.IsNotNull()) { mitk::NavigationTool::Pointer calibratedTool = m_ToolToCalibrate; calibratedTool->SetToolControlPoints(this->m_CalibrationLandmarks); calibratedTool->SetToolLandmarks(this->m_RegistrationLandmarks); mitk::NavigationToolWriter::Pointer myWriter = mitk::NavigationToolWriter::New(); std::string filename = QFileDialog::getSaveFileName(nullptr,tr("Save Navigation Tool"), "/", "*.IGTTool").toUtf8().data(); if (filename == "") return; 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 QmitkIGTNavigationToolCalibration::CheckInitialization(bool CalibrationPointerRequired) { 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; } else { return true; } } diff --git a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibrationControls.ui b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibrationControls.ui index 8650474fe3..6a5521aa9a 100644 --- a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibrationControls.ui +++ b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkIGTNavigationToolCalibrationControls.ui @@ -1,1120 +1,1133 @@ IGTNavigationToolCalibrationControls 0 0 557 958 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 35 16777215 50 Calibration pointer 0 35 true 75 true 0 Qt::ElideMiddle 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 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 Collect Pivot Poses Calibration Result: Qt::Horizontal 40 20 175 0 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> Tool landmarks 0 170 Add Pointer Position Control points for registration Qt::Vertical 20 259 Tool Axis Calibration QLayout::SetDefaultConstraint Tool axis: Get Positions Calibrate Tool Axis 4 -1000.000000000000000 1000.000000000000000 4 -1000.000000000000000 1000.000000000000000 4 -1000.000000000000000 1000.000000000000000 Calibrate the tool axis: 1. Make sure, tool tips of both tools are calibrated 2. Hold pointer at the end of the needle 3. Get Position 4. Calibrate tool axis Alternative: Manually edit the axis using the spinboxes. Qt::Vertical 20 497 - verticalSpacer_5 Qt::Horizontal 40 20 Save Calibrated Navigation Tool + + + + Qt::Vertical + + + + 20 + 40 + + + + tabWidget label groupBox groupBox_2 + verticalSpacer_6 - QmitkPointListWidget + QmitkToolTrackingStatusWidget QWidget -
QmitkPointListWidget.h
+
QmitkToolTrackingStatusWidget.h
1
- QmitkNavigationDataSourceSelectionWidget + QmitkPointListWidget QWidget -
QmitkNavigationDataSourceSelectionWidget.h
+
QmitkPointListWidget.h
1
- QmitkToolTrackingStatusWidget + QmitkNavigationDataSourceSelectionWidget QWidget -
QmitkToolTrackingStatusWidget.h
+
QmitkNavigationDataSourceSelectionWidget.h
1
diff --git a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkMITKIGTTrackingToolboxViewControls.ui b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkMITKIGTTrackingToolboxViewControls.ui index 3dfa24914b..864a7b13ee 100644 --- a/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkMITKIGTTrackingToolboxViewControls.ui +++ b/Plugins/org.mitk.gui.qt.igttracking/src/internal/QmitkMITKIGTTrackingToolboxViewControls.ui @@ -1,928 +1,954 @@ QmitkMITKIGTTrackingToolboxViewControls 0 0 849 701 0 0 QmitkTemplate - 1 + 0 Tracking QFrame::NoFrame QFrame::Raised 0 0 QFrame::NoFrame QFrame::Raised 10 75 true true Tracking Tools Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop 0 0 ToolStorage: <none> Qt::Horizontal 40 20 0 0 Qt::Horizontal 13 49 120 0 Auto Detection 120 0 Add Single Tool 120 0 Load Tool Storage 120 0 Reset QFrame::NoFrame QFrame::Raised 10 75 true Tracking Control Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop Status: disconnected Qt::Horizontal 40 20 142 0 Connect Qt::Horizontal 40 20 142 0 Start Tracking Qt::Horizontal 40 20 <html><head/><body><p><span style=" color:#ff0000;">Tracking Frozen!</span></p></body></html> true 142 0 Freeze Tracking + + + + Qt::Vertical + + + + 20 + 40 + + + + Options - - - - Disable All Timers - - - - - - - true - - - <html><head/><body><p align="right"><span style=" color:#ff0000;">Rendering Disabled!</span></p></body></html> - - - Qt::AutoText - - - Update Rate Options Update Rate [per second] Qt::Horizontal 40 20 100 10 Use different Render and Log Update Rates false Render Update Rate [fps] Qt::Horizontal 40 20 false 0 100 10 false Log Update Rate [per second] Qt::Horizontal 40 20 false 1 120 10 60 + + + + Other Options + + + + + + Show Tool Quaternions + + + + + + + Simple UI + + + + + + + Caution, only for backward compatibility: + + + + + + + Inverse mode (Quaternions are stored inverse) + + + + + + Tracking Volume Options true Show Tracking Volume true Select Model: + + + + Tool Visualization Options + + + + + + true + + + Show Tool Projection + + + + + + + + + + false + + + Show Tool Axis + + + + + + Open IGT Link Enable Open IGT Link MicroService true Select Open IGT Link Data Format: TRANSFORM QTDATA TDATA POSITION - - - - Other Options + + + + Disable All Timers - - - - - Show Tool Quaternions - - - - - - - Simple UI - - - - - - - Caution, only for backward compatibility: - - - - - - - Inverse mode (Quaternions are stored inverse) - - - - - - - - Tool Visualization Options + + + + true + + + <html><head/><body><p align="right"><span style=" color:#ff0000;">Rendering Disabled!</span></p></body></html> + + + Qt::AutoText - - - - - true - - - Show Tool Projection - - - - - - - - - - false - - - Show Tool Axis - - - - + + + + Qt::Vertical + + + + 20 + 40 + + + + Logging Filename: Choose File Limit Number Of Logged Frames: Qt::Horizontal 40 20 1 9999 300 CSV format true XML format Skip invalid data Logging Status Logging OFF Logged Frames: 0 Qt::Horizontal 40 20 Start Logging Stop Logging Qt::Vertical 20 40 9 742 303 70 70 50 Connect 70 50 Start Tracking Qt::Horizontal 40 20 70 50 Advanced Mode QmitkToolTrackingStatusWidget QWidget -
QmitkToolTrackingStatusWidget.h
+
QmitkToolTrackingStatusWidget.h
1
QmitkTrackingDeviceConfigurationWidget QWidget
QmitkTrackingDeviceConfigurationWidget.h
1
QmitkNavigationToolCreationWidget QWidget
QmitkNavigationToolCreationWidget.h
1