void milxQtDiffusionTensorModel::colourByDirection()
{
vtkSmartPointer<vtkPolyData> currentMesh = model.Result();
typedef double projectionType;
///Determine colours based on axes directions
vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
scalars->SetNumberOfComponents(3);
scalars->SetNumberOfTuples(currentMesh->GetNumberOfPoints());
scalars->SetName("Fibre Colours");
scalars->FillComponent(0, 0);
scalars->FillComponent(1, 0);
scalars->FillComponent(2, 0);
vtkSmartPointer<vtkFloatArray> projections = vtkSmartPointer<vtkFloatArray>::New();
projections->SetNumberOfComponents(3);
projections->SetNumberOfTuples(currentMesh->GetNumberOfPoints());
projections->SetName("Fibre Projections");
projections->FillComponent(0, 0.0);
projections->FillComponent(1, 0.0);
projections->FillComponent(2, 0.0);
emit working(-1);
if(currentMesh->GetNumberOfLines() == 0)
{
printInfo("No lines in model. Computing colours for mesh.");
///Use the dot product in each axis
for(size_t j = 0; j < 3; j ++)
{
projectionType axis[3] = {0.0, 0.0, 0.0};
axis[j] = 1.0;
cout << "Computing toward axis " << j << endl;
///Colour based on each of the gradient values in that direction
for(vtkIdType k = 0; k < currentMesh->GetNumberOfPoints(); k ++)
{
coordinate currentProjection(projections->GetTuple3(k)), position(currentMesh->GetPoint(k));
projectionType projection = vtkMath::Dot(axis, position.data_block()); //project to axis being done,
currentProjection[j] = projection; //projection in each direction
projections->SetTuple3(k, currentProjection[0], currentProjection[1], currentProjection[2]);
}
}
///Colour based on each of the gradient values in that direction
for(vtkIdType k = 0; k < currentMesh->GetNumberOfPoints(); k ++)
{
coordinate currentProjection(projections->GetTuple3(k));
coordinate currentProjSquared = element_product(currentProjection, currentProjection);
projectionType maxProjection = currentProjSquared.max_value();
currentProjSquared /= maxProjection;
unsigned char colourOfPoint[3] = {0, 0, 0};
colourOfPoint[0] = static_cast<unsigned char>( currentProjSquared[0]*255.0 );
colourOfPoint[1] = static_cast<unsigned char>( currentProjSquared[1]*255.0 );
colourOfPoint[2] = static_cast<unsigned char>( currentProjSquared[2]*255.0 );
scalars->SetTupleValue(k, colourOfPoint);
}
currentMesh->GetPointData()->SetVectors(projections);
currentMesh->GetPointData()->SetScalars(scalars);
}
else
{
printInfo("Re-colouring lines by axes directions");
//Ensure lines done properly so can colour appropriately
currentMesh->GetLines()->InitTraversal();
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkIdList> idList = vtkSmartPointer<vtkIdList>::New();
std::vector<vtkIdType> trackLengths;
for(vtkIdType j = 0; j < currentMesh->GetNumberOfLines(); j ++)
{
currentMesh->GetLines()->GetNextCell(idList);
trackLengths.push_back(idList->GetNumberOfIds());
for(vtkIdType pointId = 0; pointId < idList->GetNumberOfIds(); pointId ++)
{
// std::cout << idList->GetId(pointId) << " ";
double position[3];
currentMesh->GetPoint(idList->GetId(pointId), position);
points->InsertNextPoint(position);
}
}
//Re-stitch lines together
vtkIdType step = 0;
vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
for(size_t j = 0; j < trackLengths.size(); j ++)
{
for(vtkIdType k = step; k < trackLengths[j]-1; k ++)
{
vtkSmartPointer<vtkLine> line = vtkSmartPointer<vtkLine>::New();
line->GetPointIds()->SetId(0, k);
line->GetPointIds()->SetId(1, k+1);
lines->InsertNextCell(line);
}
qDebug() << trackLengths[j] << endl;
step += trackLengths[j];
}
vtkSmartPointer<vtkPolyData> linesPolyData = vtkSmartPointer<vtkPolyData>::New();
//Add the points to the dataset
linesPolyData->SetPoints(points);
//Add the lines to the dataset
linesPolyData->SetLines(lines);
linesPolyData->Modified();
model.SetInput(linesPolyData);
}
emit done(-1);
generateModel();
}
bool PivotCalibration2::ComputeSpinCalibration(bool snapRotation)
{
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;
}
// Setup our system to find the axis of rotation
unsigned int rows = 3, columns = 3;
vnl_matrix<double> A(rows, columns, 0);
vnl_matrix<double> I(3, 3, 0);
I.set_identity();
vnl_matrix<double> RI(rows, columns);
// this will store the maximum difference in orientation between the first transform and all the other transforms
double maximumOrientationDifferenceDeg = 0;
std::vector< vtkSmartPointer<vtkMatrix4x4> >::const_iterator previt = this->ToolToReferenceMatrices.end();
for (std::vector< vtkSmartPointer<vtkMatrix4x4> >::const_iterator it = this->ToolToReferenceMatrices.begin(); it != this->ToolToReferenceMatrices.end(); it++)
{
if (previt == this->ToolToReferenceMatrices.end())
{
previt = it;
continue; // No comparison to make for the first matrix
}
vtkSmartPointer< vtkMatrix4x4 > itinverse = vtkSmartPointer< vtkMatrix4x4 >::New();
vtkMatrix4x4::Invert((*it), itinverse);
vtkSmartPointer< vtkMatrix4x4 > instRotation = vtkSmartPointer< vtkMatrix4x4 >::New();
vtkMatrix4x4::Multiply4x4(itinverse, (*previt), instRotation);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
RI(i, j) = instRotation->GetElement(i, j);
}
}
RI = RI - I;
A = A + RI.transpose() * RI;
previt = it;
}
// Note: If the needle orientation protocol changes, only the definitions of shaftAxis and secondaryAxes need to be changed
// Define the shaft axis and the secondary shaft axis
// Current needle orientation protocol dictates: shaft axis -z, orthogonal axis +x
// If StylusX is parallel to ShaftAxis then: shaft axis -z, orthogonal axis +y
vnl_vector<double> shaftAxis_Shaft(columns, 0); shaftAxis_Shaft(0) = 0; shaftAxis_Shaft(1) = 0; shaftAxis_Shaft(2) = -1;
vnl_vector<double> orthogonalAxis_Shaft(columns, 0); orthogonalAxis_Shaft(0) = 1; orthogonalAxis_Shaft(1) = 0; orthogonalAxis_Shaft(2) = 0;
vnl_vector<double> backupAxis_Shaft(columns, 0); backupAxis_Shaft(0) = 0; backupAxis_Shaft(1) = 1; backupAxis_Shaft(2) = 0;
// Find the eigenvector associated with the smallest eigenvalue
// This is the best axis of rotation over all instantaneous rotations
vnl_matrix<double> eigenvectors(columns, columns, 0);
vnl_vector<double> eigenvalues(columns, 0);
vnl_symmetric_eigensystem_compute(A, eigenvectors, eigenvalues);
// Note: eigenvectors are ordered in increasing eigenvalue ( 0 = smallest, end = biggest )
vnl_vector<double> shaftAxis_ToolTip(columns, 0);
shaftAxis_ToolTip(0) = eigenvectors(0, 0);
shaftAxis_ToolTip(1) = eigenvectors(1, 0);
shaftAxis_ToolTip(2) = eigenvectors(2, 0);
shaftAxis_ToolTip.normalize();
// Snap the direction vector to be exactly aligned with one of the coordinate axes
// This is if the sensor is known to be parallel to one of the axis, just not which one
if (snapRotation)
{
int closestCoordinateAxis = element_product(shaftAxis_ToolTip, shaftAxis_ToolTip).arg_max();
shaftAxis_ToolTip.fill(0);
shaftAxis_ToolTip.put(closestCoordinateAxis, 1); // Doesn't matter the direction, will be sorted out in the next step
}
// Make sure it is in the correct direction (opposite the StylusTipToStylus translation)
vnl_vector<double> toolTipToToolTranslation(3);
toolTipToToolTranslation(0) = this->ToolTipToToolMatrix->GetElement(0, 3);
toolTipToToolTranslation(1) = this->ToolTipToToolMatrix->GetElement(1, 3);
toolTipToToolTranslation(2) = this->ToolTipToToolMatrix->GetElement(2, 3);
if (dot_product(shaftAxis_ToolTip, toolTipToToolTranslation) > 0)
{
shaftAxis_ToolTip = shaftAxis_ToolTip * (-1);
}
//set the RMSE
this->SpinRMSE = (A * shaftAxis_ToolTip).rms();
// If the secondary axis 1 is parallel to the shaft axis in the tooltip frame, then use secondary axis 2
vnl_vector<double> orthogonalAxis_ToolTip;
double angle = acos(dot_product(shaftAxis_ToolTip, orthogonalAxis_Shaft));
// Force angle to be between -pi/2 and +pi/2
if (angle > vtkMath::Pi() / 2)
{
angle -= vtkMath::Pi();
}
if (angle < -vtkMath::Pi() / 2)
{
angle += vtkMath::Pi();
}
if (fabs(angle) > vtkMath::RadiansFromDegrees(PARALLEL_ANGLE_THRESHOLD_DEGREES)) // If shaft axis and orthogonal axis are not parallel
{
orthogonalAxis_ToolTip = orthogonalAxis_Shaft;
}
else
{
orthogonalAxis_ToolTip = backupAxis_Shaft;
}
// Do the registration find the appropriate rotation
orthogonalAxis_ToolTip = orthogonalAxis_ToolTip - dot_product(orthogonalAxis_ToolTip, shaftAxis_ToolTip) * shaftAxis_ToolTip;
orthogonalAxis_ToolTip.normalize();
// Register X,Y,O points in the two coordinate frames (only spherical registration - since pure rotation)
vnl_matrix<double> ToolTipPoints(3, 3, 0.0);
vnl_matrix<double> ShaftPoints(3, 3, 0.0);
ToolTipPoints.put(0, 0, shaftAxis_ToolTip(0));
ToolTipPoints.put(0, 1, shaftAxis_ToolTip(1));
ToolTipPoints.put(0, 2, shaftAxis_ToolTip(2));
ToolTipPoints.put(1, 0, orthogonalAxis_ToolTip(0));
ToolTipPoints.put(1, 1, orthogonalAxis_ToolTip(1));
ToolTipPoints.put(1, 2, orthogonalAxis_ToolTip(2));
ToolTipPoints.put(2, 0, 0);
ToolTipPoints.put(2, 1, 0);
ToolTipPoints.put(2, 2, 0);
ShaftPoints.put(0, 0, shaftAxis_Shaft(0));
ShaftPoints.put(0, 1, shaftAxis_Shaft(1));
ShaftPoints.put(0, 2, shaftAxis_Shaft(2));
ShaftPoints.put(1, 0, orthogonalAxis_Shaft(0));
ShaftPoints.put(1, 1, orthogonalAxis_Shaft(1));
ShaftPoints.put(1, 2, orthogonalAxis_Shaft(2));
ShaftPoints.put(2, 0, 0);
ShaftPoints.put(2, 1, 0);
ShaftPoints.put(2, 2, 0);
vnl_svd<double> ShaftToToolTipRegistrator(ShaftPoints.transpose() * ToolTipPoints);
vnl_matrix<double> V = ShaftToToolTipRegistrator.V();
vnl_matrix<double> U = ShaftToToolTipRegistrator.U();
vnl_matrix<double> Rotation = V * U.transpose();
// Make sure the determinant is positve (i.e. +1)
double determinant = vnl_determinant(Rotation);
if (determinant < 0)
{
// Switch the sign of the third column of V if the determinant is not +1
// This is the recommended approach from Huang et al. 1987
V.put(0, 2, -V.get(0, 2));
V.put(1, 2, -V.get(1, 2));
V.put(2, 2, -V.get(2, 2));
Rotation = V * U.transpose();
}
// Set the elements of the output matrix
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
this->ToolTipToToolMatrix->SetElement(i, j, Rotation[i][j]);
}
}
this->ErrorText.empty();
return true;
}
