diff --git a/Modules/IGTBase/src/mitkStaticIGTHelperFunctions.cpp b/Modules/IGTBase/src/mitkStaticIGTHelperFunctions.cpp index 4acdce6cc1..957401e9ca 100644 --- a/Modules/IGTBase/src/mitkStaticIGTHelperFunctions.cpp +++ b/Modules/IGTBase/src/mitkStaticIGTHelperFunctions.cpp @@ -1,113 +1,115 @@ /*=================================================================== 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 #include double mitk::StaticIGTHelperFunctions::GetAngleBetweenTwoQuaterions(mitk::Quaternion a, mitk::Quaternion b, itk::Vector rotationVector) { double returnValue; itk::Vector point; //caution 5D-Tools: correct vector along the tool axis is needed point[0] = rotationVector[0]; point[1] = rotationVector[1]; point[2] = rotationVector[2]; //Quaternions used for rotations should alway be normalized, so just to be safe: a.normalize(); b.normalize(); itk::Matrix rotMatrixA; for(int i=0; i<3; i++) for(int j=0; j<3; j++) rotMatrixA[i][j] = a.rotation_matrix_transpose().transpose()[i][j]; itk::Matrix rotMatrixB; for(int i=0; i<3; i++) for(int j=0; j<3; j++) rotMatrixB[i][j] = b.rotation_matrix_transpose().transpose()[i][j]; itk::Vector pt1 = rotMatrixA * point; itk::Vector pt2 = rotMatrixB * point; returnValue = (pt1[0]*pt2[0]+pt1[1]*pt2[1]+pt1[2]*pt2[2]) / ( sqrt(pow(pt1[0],2.0)+pow(pt1[1],2.0)+pow(pt1[2],2.0)) * sqrt(pow(pt2[0],2.0)+pow(pt2[1],2.0)+pow(pt2[2],2.0))); returnValue = acos(returnValue) * 57.296; //57,296 = 180/Pi ; conversion to degrees return returnValue; } double mitk::StaticIGTHelperFunctions::GetAngleBetweenTwoQuaterions(mitk::Quaternion a, mitk::Quaternion b) { + //formula returnValue = 2 * acos ( a b ) + //derivation from here: https://fgiesen.wordpress.com/2013/01/07/small-note-on-quaternion-distance-metrics/ double returnValue = ((a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]) / (sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3])*sqrt(b[0] * b[0] + b[1] * b[1] + b[2] * b[2] + b[3] * b[3]))); returnValue = 2 * acos(returnValue); return returnValue; } itk::Matrix mitk::StaticIGTHelperFunctions::ConvertEulerAnglesToRotationMatrix(double alpha, double beta, double gamma) { double PI = 3.141592653589793; alpha = alpha * PI / 180; beta = beta * PI / 180; gamma = gamma * PI / 180; //convert angles to matrix: itk::Matrix matrix; /* x-Konvention (Z, X, Z) matrix[0][0] = cos(alpha) * cos(gamma) - sin(alpha) * cos(beta) * sin(gamma); matrix[0][1] = -cos(alpha) * sin(gamma)- sin(alpha) * cos(beta) * cos(gamma); matrix[0][2] = sin(alpha) * sin(beta); matrix[1][0] = sin(alpha) * cos(gamma) + cos(alpha) * cos(beta) * sin(gamma); matrix[1][1] = cos(alpha) * cos(beta) * cos(gamma) - sin(alpha) * sin(gamma); matrix[1][2] = -cos(alpha) * sin(beta); matrix[2][0] = sin(beta) * sin(gamma); matrix[2][1] = sin(beta) * cos(gamma); matrix[2][2] = cos(beta); */ //Luftfahrtnorm (DIN 9300) (Yaw-Pitch-Roll, Z, Y, X) matrix[0][0] = cos(beta) * cos(alpha); matrix[0][1] = cos(beta) * sin(alpha); matrix[0][2] = -sin(beta); matrix[1][0] = sin(gamma) * sin(beta) * cos(alpha) - cos(gamma) * sin(alpha) ; matrix[1][1] = sin(gamma) * sin(beta) * sin(alpha) + cos(gamma) * cos(alpha); matrix[1][2] = sin(gamma) * cos(beta); matrix[2][0] = cos(gamma) * sin(beta) * cos(alpha) + sin(gamma) * sin(alpha); matrix[2][1] = cos(gamma) * sin(beta) * sin(alpha) - sin(gamma) * cos(alpha); matrix[2][2] = cos(gamma) * cos(beta); return matrix; } double mitk::StaticIGTHelperFunctions::ComputeFRE(mitk::PointSet::Pointer imageFiducials, mitk::PointSet::Pointer realWorldFiducials, vtkSmartPointer transform) { if (imageFiducials->GetSize() != realWorldFiducials->GetSize()) return -1; double FRE = 0; for (int i = 0; i < imageFiducials->GetSize(); i++) { itk::Point current_image_fiducial_point = imageFiducials->GetPoint(i); if (transform != NULL) { current_image_fiducial_point = transform->TransformPoint(imageFiducials->GetPoint(i)[0], imageFiducials->GetPoint(i)[1], imageFiducials->GetPoint(i)[2]); } double cur_error_squared = current_image_fiducial_point.SquaredEuclideanDistanceTo(realWorldFiducials->GetPoint(i)); FRE += cur_error_squared; } FRE = sqrt(FRE / (double)imageFiducials->GetSize()); return FRE; } diff --git a/Plugins/org.mitk.gui.qt.igt.app.hummelprotocolmeasurements/src/internal/mitkNavigationDataCSVSequentialPlayer.cpp b/Plugins/org.mitk.gui.qt.igt.app.hummelprotocolmeasurements/src/internal/mitkNavigationDataCSVSequentialPlayer.cpp index 8f5241e764..2b43863ce4 100644 --- a/Plugins/org.mitk.gui.qt.igt.app.hummelprotocolmeasurements/src/internal/mitkNavigationDataCSVSequentialPlayer.cpp +++ b/Plugins/org.mitk.gui.qt.igt.app.hummelprotocolmeasurements/src/internal/mitkNavigationDataCSVSequentialPlayer.cpp @@ -1,247 +1,269 @@ /*=================================================================== 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. ===================================================================*/ #define _USE_MATH_DEFINES #include "mitkNavigationDataCSVSequentialPlayer.h" #include #include #include #include #include mitk::NavigationDataCSVSequentialPlayer::NavigationDataCSVSequentialPlayer() : mitk::NavigationDataPlayerBase() { m_NavigationDatas = std::vector(); m_CurrentPos = 0; m_Filetype = mitk::NavigationDataCSVSequentialPlayer::ManualLoggingCSV; } mitk::NavigationDataCSVSequentialPlayer::~NavigationDataCSVSequentialPlayer() { } bool mitk::NavigationDataCSVSequentialPlayer::IsAtEnd() { if (m_CurrentPos >= m_NavigationDatas.size()) return true; else return false; } void mitk::NavigationDataCSVSequentialPlayer:: SetFileName(const std::string& fileName) { this->SetNumberOfOutputs(1); FillOutputEmpty(0); MITK_INFO << "Reading file: " << fileName; m_NavigationDatas = GetNavigationDatasFromFile(fileName); this->Modified(); } void mitk::NavigationDataCSVSequentialPlayer::FillOutputEmpty(int number) { this->SetNthOutput(number, GetEmptyNavigationData()); } mitk::NavigationData::Pointer mitk::NavigationDataCSVSequentialPlayer::GetEmptyNavigationData() { mitk::NavigationData::Pointer emptyNd = mitk::NavigationData::New(); mitk::NavigationData::PositionType position; mitk::NavigationData::OrientationType orientation(0.0, 0.0, 0.0, 0.0); position.Fill(0.0); emptyNd->SetPosition(position); emptyNd->SetOrientation(orientation); emptyNd->SetDataValid(false); return emptyNd; } void mitk::NavigationDataCSVSequentialPlayer::GenerateData() { for (unsigned int index = 0; index < this->GetNumberOfOutputs(); index++) { mitk::NavigationData* output = this->GetOutput(index); if (m_CurrentPos > m_NavigationDatas.size()) { FillOutputEmpty(index); return; } output->Graft(this->m_NavigationDatas.at(m_CurrentPos)); m_CurrentPos++; } } void mitk::NavigationDataCSVSequentialPlayer::UpdateOutputInformation() { this->Modified(); // make sure that we need to be updated Superclass::UpdateOutputInformation(); } std::vector mitk::NavigationDataCSVSequentialPlayer::GetNavigationDatasFromFile(std::string filename) { std::vector returnValue = std::vector(); std::vector fileContentLineByLine = GetFileContentLineByLine(filename); for (int i = 1; (i < fileContentLineByLine.size()); i++) //skip header so start at 1 { returnValue.push_back(GetNavigationDataOutOfOneLine(fileContentLineByLine.at(i))); } return returnValue; } std::vector mitk::NavigationDataCSVSequentialPlayer::GetFileContentLineByLine(std::string filename) { std::vector readData = std::vector(); //save old locale char * oldLocale; oldLocale = setlocale(LC_ALL, 0); //define own locale std::locale C("C"); setlocale(LC_ALL, "C"); //read file std::ifstream file; file.open(filename.c_str(), std::ios::in); if (file.good()) { //read out file file.seekg(0L, std::ios::beg); // move to begin of file while (!file.eof()) { std::string buffer; std::getline(file, buffer); // read out file line by line if (buffer.size() > 0) readData.push_back(buffer); } } file.close(); //switch back to old locale setlocale(LC_ALL, oldLocale); return readData; } mitk::NavigationData::Pointer mitk::NavigationDataCSVSequentialPlayer::GetNavigationDataOutOfOneLine(std::string line) { mitk::NavigationData::Pointer returnValue = mitk::NavigationData::New(); QString myLine = QString(line.c_str()); QStringList myLineList = myLine.split(';'); mitk::Point3D position; mitk::Quaternion orientation; bool valid = false; double time; //this is for custom csv files. You have adapt the column numbers to correctly //interpret your csv file. if (m_Filetype = mitk::NavigationDataCSVSequentialPlayer::ManualLoggingCSV) { if (myLineList.size() < 10) { MITK_ERROR << "Error: cannot read line: only found " << myLineList.size() << " fields. Last field: " << myLineList.at(myLineList.size() - 1).toStdString(); returnValue = GetEmptyNavigationData(); return returnValue; } valid = true; //if no valid flag is given: simply set to true /* //############# Variant for the Aurora measurements ############### //############# (CUSTOM .csv files from MITK) ############### position[0] = myLineList.at(3).toDouble(); position[1] = myLineList.at(4).toDouble(); position[2] = myLineList.at(5).toDouble(); orientation[0] = myLineList.at(6).toDouble(); //qx orientation[1] = myLineList.at(7).toDouble(); //qy orientation[2] = myLineList.at(8).toDouble(); //qz orientation[3] = myLineList.at(9).toDouble(); //qr */ //Variant for the polhemus measurements in August 2016 //(.csv files from the polhemus software) //Important: due to the documentation, Polhemus uses //a left handed coordinate system while MITK uses a //right handed. A conversion is not included in this //read in method yet, because this is not required //for this special rotation evaliation (no matter //if it turns 11.25 degree to left or right). For //other usage this might be important to adapt! position[0] = myLineList.at(4).toDouble(); position[1] = myLineList.at(5).toDouble(); position[2] = myLineList.at(6).toDouble(); - /* + //Doesn't work... don't know how to interpret the //Polhemus quaternions. They are seem to different //different to other quaternions (NDI, Claron, etc.) - orientation[3] = myLineList.at(7).toDouble(); //qr - orientation[0] = myLineList.at(8).toDouble(); //qx - orientation[1] = myLineList.at(9).toDouble(); //qy - orientation[2] = myLineList.at(10).toDouble(); //qz + //http://www.mathepedia.de/Quaternionen.aspx + double rotationAngle = myLineList.at(7).toDouble(); + double rotationAxis[3]; + rotationAxis[0] = myLineList.at(8).toDouble(); + rotationAxis[1] = myLineList.at(9).toDouble(); + rotationAxis[2] = myLineList.at(10).toDouble(); + + /* + double betragRotationAxis = sqrt(pow(rotationAxis[0], 2) + pow(rotationAxis[1], 2) + pow(rotationAxis[2], 2)); + rotationAngle /= betragRotationAxis; + rotationAxis[0] /= betragRotationAxis; + rotationAxis[1] /= betragRotationAxis; + rotationAxis[2] /= betragRotationAxis; */ + double qr = cos(rotationAngle/2); + double qx = rotationAxis[0] * sin(rotationAngle/2); + double qy = rotationAxis[1] * sin(rotationAngle/2); + double qz = rotationAxis[1] * sin(rotationAngle/2); + + orientation[3] = qr; //qr + orientation[0] = qx; //qx + orientation[1] = qy; //qy + orientation[2] = qz; //qz + + + /* //Using Euler angles instead does work double euler1 = (myLineList.at(11).toDouble() / 180 * M_PI); double euler2 = (myLineList.at(12).toDouble() / 180 * M_PI); double euler3 = (myLineList.at(13).toDouble() / 180 * M_PI); mitk::Quaternion eulerQuat(euler3, euler2, euler1); orientation = eulerQuat; + */ } //this is for MITK csv files that have been recorded with the MITK //navigation data recorder. You can also use the navigation data player //class from the MITK-IGT module instead. else if (m_Filetype = mitk::NavigationDataCSVSequentialPlayer::NavigationDataCSV) { if (myLineList.size() < 8) { MITK_ERROR << "Error: cannot read line: only found " << myLineList.size() << " fields. Last field: " << myLineList.at(myLineList.size() - 1).toStdString(); returnValue = GetEmptyNavigationData(); return returnValue; } time = myLineList.at(2).toDouble(); if (myLineList.at(3).toStdString() == "1") valid = true; position[0] = myLineList.at(2).toDouble(); position[1] = myLineList.at(3).toDouble(); position[2] = myLineList.at(4).toDouble(); orientation[0] = myLineList.at(5).toDouble(); //qx orientation[1] = myLineList.at(6).toDouble(); //qy orientation[2] = myLineList.at(7).toDouble(); //qz orientation[3] = myLineList.at(8).toDouble(); //qr } //returnValue->SetTimeStamp(time); //DOES NOT WORK ANY MORE... CANNOT SET TIME TO itk::timestamp CLASS returnValue->SetDataValid(valid); returnValue->SetPosition(position); returnValue->SetOrientation(orientation); return returnValue; }