mitk::PointSet::Pointer QmitkIGTTrackingLabView::GetVirtualPointSetFromPosition(mitk::NavigationData::Pointer navigationData)
{
typedef itk::QuaternionRigidTransform<double> QuaternionTransformType;
mitk::NavigationData::PositionType pointA;
mitk::NavigationData::PositionType pointB;
mitk::NavigationData::PositionType pointC;
//initializing three points with position(0|0|0)
pointA.Fill(0);
pointB.Fill(0);
pointC.Fill(0);
// changing position off all points in order to make them orthogonal
pointA[0] = 1;
pointB[1] = 1;
pointC[2] = 1;
QuaternionTransformType::Pointer quatTransform = QuaternionTransformType::New();
// orientation of NavigationData from parameter
mitk::NavigationData::OrientationType quatIn = navigationData->GetOrientation();
// set orientation to quaternion transform
vnl_quaternion<double> const vnlQuatIn(quatIn.x(), quatIn.y(), quatIn.z(), quatIn.r());
quatTransform->SetRotation(vnlQuatIn);
// transform each point
pointA = quatTransform->TransformPoint(pointA);
pointB = quatTransform->TransformPoint(pointB);
pointC = quatTransform->TransformPoint(pointC);
// add position data from NavigationData parameter to each point
pointA[0] += navigationData->GetPosition()[0];
pointA[1] += navigationData->GetPosition()[1];
pointA[2] += navigationData->GetPosition()[2];
pointB[0] += navigationData->GetPosition()[0];
pointB[1] += navigationData->GetPosition()[1];
pointB[2] += navigationData->GetPosition()[2];
pointC[0] += navigationData->GetPosition()[0];
pointC[1] += navigationData->GetPosition()[1];
pointC[2] += navigationData->GetPosition()[2];
// insert points in source points pointset for the permanent registration landmark transform
m_PermanentRegistrationSourcePoints->InsertPoint(0,pointA);
m_PermanentRegistrationSourcePoints->InsertPoint(1,pointB);
m_PermanentRegistrationSourcePoints->InsertPoint(2,pointC);
return m_PermanentRegistrationSourcePoints;
}
bool mitk::NavigationDataReferenceTransformFilter::InitializeReferenceLandmarks()
{
if (m_ReferenceInputIndexes.size() != 1 || (m_SourcePoints.size() < 3) || (m_ReferenceInputIndexes.size() <= 0) || m_ReferenceInputIndexes.size() > this->GetNumberOfInputs())
return false;
mitk::NavigationData::ConstPointer referenceND = this->GetInput(m_ReferenceInputIndexes.at(0));
if(!referenceND->IsDataValid())
return false;
QuaternionTransformType::Pointer referenceTransform = QuaternionTransformType::New();
QuaternionTransformType::VnlQuaternionType doubleOrientation(referenceND->GetOrientation().x(), referenceND->GetOrientation().y(),referenceND->GetOrientation().z(),referenceND->GetOrientation().r());
referenceTransform->SetRotation(doubleOrientation);
referenceTransform->SetOffset(referenceND->GetPosition().GetVectorFromOrigin());
referenceTransform->Modified();
try
{
bool okay = referenceTransform->GetInverse(m_QuaternionReferenceTransform); // inverse transform to compensate rotation
if(!okay)
return false;
}
catch( ... )
{
return false;
}
m_ReferencePoints.clear();
for(NavigationDataReferenceTransformFilter::LandmarkPointContainer::const_iterator it = m_SourcePoints.begin(); it != m_SourcePoints.end(); ++it)
{
QuaternionTransformType::InputPointType doublePoint;
mitk::itk2vtk(*it,doublePoint); // workaround for itk bug
TransformInitializerType::LandmarkPointType rLPoint;
rLPoint = m_QuaternionReferenceTransform->TransformPoint(doublePoint);
m_ReferencePoints.push_back(rLPoint);
}
return true;
}
void mitk::NavigationDataReferenceTransformFilter::GenerateData()
{
LandmarkPointContainer newSourcePoints; // for the quaternion transformed reference landmarks
if(m_OneSourceRegistration) // check if less than 3 reference inputs
{
NavigationData::ConstPointer nd = this->GetInput(m_ReferenceInputIndexes.at(0));
if (nd->IsDataValid() == false)
{
for (unsigned int i = 0; i < this->GetNumberOfOutputs() ; ++i)
{
mitk::NavigationData::Pointer output = this->GetOutput(i);
assert(output);
output->SetDataValid(false);
}
return;
}
QuaternionTransformType::Pointer referenceTransform = QuaternionTransformType::New();
QuaternionTransformType::VnlQuaternionType doubleOrientation(nd->GetOrientation().x(), nd->GetOrientation().y(), nd->GetOrientation().z(), nd->GetOrientation().r()); // convert to double quaternion as workaround for ITK 3.10 bug
referenceTransform->SetRotation(doubleOrientation);
referenceTransform->SetOffset(nd->GetPosition().GetVectorFromOrigin());
referenceTransform->Modified();
for (NavigationDataReferenceTransformFilter::LandmarkPointContainer::const_iterator it = m_ReferencePoints.begin(); it != m_ReferencePoints.end(); ++it)
{
TransformInitializerType::LandmarkPointType rLPoint; // reference landmark point
rLPoint = referenceTransform->TransformPoint(*it);
newSourcePoints.push_back(rLPoint);
}
this->UpdateLandmarkTransform(newSourcePoints, m_TargetPoints);
m_SourcePoints = newSourcePoints;
}
if(this->IsInitialized() && !m_OneSourceRegistration && m_ReferenceRegistration)
this->GenerateSourceLandmarks(); // generates landmarks from the moving points
this->CreateOutputsForAllInputs();
TransformInitializerType::LandmarkPointType lPointIn, lPointOut;
for(unsigned int i = 0; i < this->GetNumberOfOutputs(); i++)
{
mitk::NavigationData::Pointer output = this->GetOutput(i);
assert(output);
mitk::NavigationData::ConstPointer input = this->GetInput(i);
assert(input);
if(input->IsDataValid() == false)
{
output->SetDataValid(false);
continue;
}
output->Graft(input); // First, copy all information from input to output
if(this->IsInitialized() == false)
continue;
mitk::NavigationData::PositionType tempCoordinate;
tempCoordinate = input->GetPosition();
lPointIn[0] = tempCoordinate[0];
lPointIn[1] = tempCoordinate[1];
lPointIn[2] = tempCoordinate[2];
/* transform position */
lPointOut = m_LandmarkTransform->TransformPoint(lPointIn);
tempCoordinate[0] = lPointOut[0];
tempCoordinate[1] = lPointOut[1];
tempCoordinate[2] = lPointOut[2];
output->SetPosition(tempCoordinate); // update output navigation data with new position
/* transform orientation */
NavigationData::OrientationType quatIn = input->GetOrientation();
vnl_quaternion<double> const vnlQuatIn(quatIn.x(), quatIn.y(), quatIn.z(), quatIn.r());
m_QuaternionTransform->SetRotation(vnlQuatIn);
m_QuaternionLandmarkTransform->SetMatrix(m_LandmarkTransform->GetRotationMatrix());
m_QuaternionLandmarkTransform->Compose(m_QuaternionTransform, true);
vnl_quaternion<double> vnlQuatOut = m_QuaternionLandmarkTransform->GetRotation();
NavigationData::OrientationType quatOut( vnlQuatOut[0], vnlQuatOut[1], vnlQuatOut[2], vnlQuatOut[3]);
output->SetOrientation(quatOut);
output->SetDataValid(true);
}
}