diff --git a/Modules/IGT/Algorithms/mitkPivotCalibration.cpp b/Modules/IGT/Algorithms/mitkPivotCalibration.cpp index bb1094b217..d1b519d480 100644 --- a/Modules/IGT/Algorithms/mitkPivotCalibration.cpp +++ b/Modules/IGT/Algorithms/mitkPivotCalibration.cpp @@ -1,114 +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 = _CheckedTransforms.at(i)->GetPosition().GetVnlVector();// 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; }