diff --git a/Modules/IGTBase/include/mitkStaticIGTHelperFunctions.h b/Modules/IGTBase/include/mitkStaticIGTHelperFunctions.h index 285a7b4af2..6267ff3e57 100644 --- a/Modules/IGTBase/include/mitkStaticIGTHelperFunctions.h +++ b/Modules/IGTBase/include/mitkStaticIGTHelperFunctions.h @@ -1,54 +1,53 @@ /*=================================================================== 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 #include #include "MitkIGTBaseExports.h" #include #include #include namespace mitk { class MITKIGTBASE_EXPORT StaticIGTHelperFunctions { public: /** Computes the angle in the plane perpendicular to the rotation axis of the two quaterions. * Therefore, a vector is rotated with the difference of both rotations and the angle is computed. - * In some cases you might want to defice this vector e.g., if working with 5D tools. By default - * the vector is defined along the Z-axis which works for Aurora 5D tools. + * In some cases you might want to define this vector e.g., if working with 5D tools. For NDI Aurora + * 5D tools you need to defined this vector along the Z-axis. * @return Returns the angle in degrees. **/ static double GetAngleBetweenTwoQuaterions(mitk::Quaternion a, mitk::Quaternion b, itk::Vector rotationVector); - /** Computes the angle in the plane perpendicular to the rotation axis of the two quaterions. - * Therefore, a vector is rotated with the difference of both rotations and the angle is computed. - * In some cases you might want to defice this vector e.g., if working with 5D tools. By default - * the vector is defined along the Z-axis which works for Aurora 5D tools. + /** Computes difference between two quaternions in degree, which is the minimum rotation angle between + * these two quaternions. + * The used formula is described here: https://fgiesen.wordpress.com/2013/01/07/small-note-on-quaternion-distance-metrics/ * @return Returns the angle in degrees. **/ static double GetAngleBetweenTwoQuaterions(mitk::Quaternion a, mitk::Quaternion b); /** Converts euler angles (in degrees) to a rotation matrix. */ static itk::Matrix ConvertEulerAnglesToRotationMatrix(double alpha, double beta, double gamma); /** @brief Computes the fiducial registration error out of two sets of fiducials. * The two sets must have the same size and the points must correspond to each other. * @param transform This transform is applied to the image fiducials before the FRE calculation if it is given. * @return Returns the FRE. Returns -1 if there was an error. */ static double ComputeFRE(mitk::PointSet::Pointer imageFiducials, mitk::PointSet::Pointer realWorldFiducials, vtkSmartPointer transform = NULL); }; } diff --git a/Modules/IGTBase/src/mitkStaticIGTHelperFunctions.cpp b/Modules/IGTBase/src/mitkStaticIGTHelperFunctions.cpp index 5a7fd5af40..4acdce6cc1 100644 --- a/Modules/IGTBase/src/mitkStaticIGTHelperFunctions.cpp +++ b/Modules/IGTBase/src/mitkStaticIGTHelperFunctions.cpp @@ -1,115 +1,113 @@ /*=================================================================== 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) { - itk::Vector rotationVector = itk::Vector(); - rotationVector[0] = 0; - rotationVector[1] = 0; - rotationVector[2] = 1000; - return GetAngleBetweenTwoQuaterions(a,b,rotationVector); + 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; }