bool mitk::PivotCalibration::ComputePivotPoint()
{
double defaultThreshold = 1e-1;
std::vector<mitk::NavigationData::Pointer> _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().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<double> 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;
}
/*
Description:
*/
bool PivotCalibration2::ComputePivotCalibration()
{
if (this->ToolToReferenceMatrices.size() < 10)
{
this->ErrorText = "Not enough input transforms are available";
return false;
}
if (this->GetMaximumToolOrientationDifferenceDeg() < this->MinimumOrientationDifferenceDeg)
{
this->ErrorText = "Not enough variation in the input transforms";
return false;
}
unsigned int rows = 3 * this->ToolToReferenceMatrices.size();
unsigned int columns = 6;
vnl_matrix<double> A(rows, columns);
vnl_matrix<double> minusI(3, 3, 0);
minusI(0, 0) = -1;
minusI(1, 1) = -1;
minusI(2, 2) = -1;
vnl_matrix<double> R(3, 3);
vnl_vector<double> b(rows);
vnl_vector<double> x(columns);
vnl_vector<double> t(3);
std::vector<vtkSmartPointer<vtkMatrix4x4> >::const_iterator it;
std::vector<vtkSmartPointer<vtkMatrix4x4> >::const_iterator matricesEnd = this->ToolToReferenceMatrices.end();
unsigned int currentRow;
vtkMatrix4x4* referenceOrientationMatrix = this->ToolToReferenceMatrices.front();
for (currentRow = 0, it = this->ToolToReferenceMatrices.begin(); it != matricesEnd; it++, currentRow += 3)
{
for (int i = 0; i < 3; i++)
{
t(i) = (*it)->GetElement(i, 3);
}
t *= -1;
b.update(t, currentRow);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
R(i, j) = (*it)->GetElement(i, j);
}
}
A.update(R, currentRow, 0);
A.update(minusI, currentRow, 3);
}
vnl_svd<double> svdA(A);
svdA.zero_out_absolute(1e-1);
x = svdA.solve(b);
//set the RMSE
this->PivotRMSE = (A * x - b).rms();
//set the transformation
this->ToolTipToToolMatrix->SetElement(0, 3, x[0]);
this->ToolTipToToolMatrix->SetElement(1, 3, x[1]);
this->ToolTipToToolMatrix->SetElement(2, 3, x[2]);
this->ErrorText.empty();
return true;
}
