void mitk::NavigationDataLandmarkTransformFilter::UpdateLandmarkTransform(const LandmarkPointContainer &sources, const LandmarkPointContainer &targets)
{
try
{
/* calculate transform from landmarks */
m_LandmarkTransformInitializer->SetMovingLandmarks(targets);
m_LandmarkTransformInitializer->SetFixedLandmarks(sources); // itk registration always maps from fixed object space to moving object space
m_LandmarkTransform->SetIdentity();
m_LandmarkTransformInitializer->InitializeTransform();
/* Calculate error statistics for the transform */
TransformInitializerType::LandmarkPointType curData;
m_Errors.clear();
for (LandmarkPointContainer::size_type index = 0; index < sources.size(); index++)
{
curData = m_LandmarkTransform->TransformPoint(sources.at(index));
m_Errors.push_back(curData.EuclideanDistanceTo(targets.at(index)));
}
this->AccumulateStatistics(m_Errors);
this->Modified();
}
catch (std::exception& e)
{
m_Errors.clear();
m_LandmarkTransform->SetIdentity();
itkExceptionMacro("Initializing landmark-transform failed\n. " << e.what());
}
}
const mitk::NavigationDataReferenceTransformFilter::LandmarkPointContainer mitk::NavigationDataReferenceTransformFilter::GenerateReferenceLandmarks()
{
LandmarkPointContainer lPoints;
if(!m_ReferenceInputIndexes.empty())
{
TransformInitializerType::LandmarkPointType lPoint;
if(m_ReferenceInputIndexes.size() < 3)
{
NavigationData::ConstPointer nD = this->GetInput(m_ReferenceInputIndexes.at(0));
NavigationData::PositionType pos = nD->GetPosition();
// fill position of reference source into sourcepoints container
mitk::FillVector3D(lPoint, pos.GetElement(0), pos.GetElement(1), pos.GetElement(2));
lPoints.push_back(lPoint);
// generate additional virtual landmark point
mitk::FillVector3D(lPoint, pos.GetElement(0), pos.GetElement(1)+100, pos.GetElement(2)); // reference source position + (0|100|0)
lPoints.push_back(lPoint);
// generate additional virtual landmark point
mitk::FillVector3D(lPoint, pos.GetElement(0), pos.GetElement(1), pos.GetElement(2)+100); // reference source position + (0|0|100)
lPoints.push_back(lPoint);
}
else if(m_ReferenceInputIndexes.size()>2) // if there are at least 3 reference inputs
{
for(unsigned int i=0; i<m_ReferenceInputIndexes.size(); ++i)
{
NavigationData::ConstPointer nD = this->GetInput(m_ReferenceInputIndexes.at(i));
NavigationData::PositionType pos = nD->GetPosition();
mitk::FillVector3D(lPoint, pos.GetElement(0), pos.GetElement(1), pos.GetElement(2));
lPoints.push_back(lPoint);
}
}
}
return lPoints;
}
bool mitk::NavigationDataLandmarkTransformFilter::FindCorrespondentLandmarks(LandmarkPointContainer& sources, const LandmarkPointContainer& targets) const
{
if (sources.size() < 6 || targets.size() < 6)
return false;
//throw std::invalid_argument("ICP correspondence finding needs at least 6 landmarks");
/* lots of type definitions */
typedef itk::PointSet<mitk::ScalarType, 3> PointSetType;
//typedef itk::BoundingBox<PointSetType::PointIdentifier, PointSetType::PointDimension> BoundingBoxType;
typedef itk::EuclideanDistancePointMetric< PointSetType, PointSetType> MetricType;
//typedef MetricType::TransformType TransformBaseType;
//typedef MetricType::TransformType::ParametersType ParametersType;
//typedef TransformBaseType::JacobianType JacobianType;
//typedef itk::Euler3DTransform< double > TransformType;
typedef itk::VersorRigid3DTransform< double > TransformType;
typedef TransformType ParametersType;
typedef itk::PointSetToPointSetRegistrationMethod< PointSetType, PointSetType > RegistrationType;
/* copy landmarks to itk pointsets for registration */
PointSetType::Pointer sourcePointSet = PointSetType::New();
unsigned int i = 0;
for (LandmarkPointContainer::const_iterator it = sources.begin(); it != sources.end(); ++it)
{
PointSetType::PointType doublePoint;
mitk::itk2vtk(*it, doublePoint); // copy mitk::ScalarType point into double point as workaround to ITK 3.10 bug
sourcePointSet->SetPoint(i++, doublePoint /**it*/);
}
i = 0;
PointSetType::Pointer targetPointSet = PointSetType::New();
for (LandmarkPointContainer::const_iterator it = targets.begin(); it != targets.end(); ++it)
{
PointSetType::PointType doublePoint;
mitk::itk2vtk(*it, doublePoint); // copy mitk::ScalarType point into double point as workaround to ITK 3.10 bug
targetPointSet->SetPoint(i++, doublePoint /**it*/);
}
/* get centroid and extends of our pointsets */
//BoundingBoxType::Pointer sourceBoundingBox = BoundingBoxType::New();
//sourceBoundingBox->SetPoints(sourcePointSet->GetPoints());
//sourceBoundingBox->ComputeBoundingBox();
//BoundingBoxType::Pointer targetBoundingBox = BoundingBoxType::New();
//targetBoundingBox->SetPoints(targetPointSet->GetPoints());
//targetBoundingBox->ComputeBoundingBox();
TransformType::Pointer transform = TransformType::New();
transform->SetIdentity();
//transform->SetTranslation(targetBoundingBox->GetCenter() - sourceBoundingBox->GetCenter());
itk::LevenbergMarquardtOptimizer::Pointer optimizer = itk::LevenbergMarquardtOptimizer::New();
optimizer->SetUseCostFunctionGradient(false);
RegistrationType::Pointer registration = RegistrationType::New();
// Scale the translation components of the Transform in the Optimizer
itk::LevenbergMarquardtOptimizer::ScalesType scales(transform->GetNumberOfParameters());
const double translationScale = 5000; //sqrtf(targetBoundingBox->GetDiagonalLength2()) * 1000; // dynamic range of translations
const double rotationScale = 1.0; // dynamic range of rotations
scales[0] = 1.0 / rotationScale;
scales[1] = 1.0 / rotationScale;
scales[2] = 1.0 / rotationScale;
scales[3] = 1.0 / translationScale;
scales[4] = 1.0 / translationScale;
scales[5] = 1.0 / translationScale;
//scales.Fill(0.01);
unsigned long numberOfIterations = 80000;
double gradientTolerance = 1e-10; // convergence criterion
double valueTolerance = 1e-10; // convergence criterion
double epsilonFunction = 1e-10; // convergence criterion
optimizer->SetScales( scales );
optimizer->SetNumberOfIterations( numberOfIterations );
optimizer->SetValueTolerance( valueTolerance );
optimizer->SetGradientTolerance( gradientTolerance );
optimizer->SetEpsilonFunction( epsilonFunction );
registration->SetInitialTransformParameters( transform->GetParameters() );
//------------------------------------------------------
// Connect all the components required for Registration
//------------------------------------------------------
MetricType::Pointer metric = MetricType::New();
registration->SetMetric( metric );
registration->SetOptimizer( optimizer );
registration->SetTransform( transform );
registration->SetFixedPointSet( targetPointSet );
registration->SetMovingPointSet( sourcePointSet );
try
{
//registration->StartRegistration();
registration->Update();
}
catch( itk::ExceptionObject & e )
{
MITK_INFO << "Exception caught during ICP optimization: " << e;
return false;
//throw e;
}
MITK_INFO << "ICP successful: Solution = " << transform->GetParameters() << std::endl;
MITK_INFO << "Metric value: " << metric->GetValue(transform->GetParameters());
/* find point correspondences */
//mitk::PointLocator::Pointer pointLocator = mitk::PointLocator::New(); // <<- use mitk::PointLocator instead of searching manually?
//pointLocator->SetPoints()
for (LandmarkPointContainer::const_iterator sourcesIt = sources.begin(); sourcesIt != sources.end(); ++sourcesIt)
{
}
//MetricType::MeasureType closestDistances = metric->GetValue(transform->GetParameters());
//unsigned int index = 0;
LandmarkPointContainer sortedSources;
for (LandmarkPointContainer::const_iterator targetsIt = targets.begin(); targetsIt != targets.end(); ++targetsIt)
{
double minDistance = itk::NumericTraits<double>::max();
LandmarkPointContainer::iterator minDistanceIterator = sources.end();
for (LandmarkPointContainer::iterator sourcesIt = sources.begin(); sourcesIt != sources.end(); ++sourcesIt)
{
TransformInitializerType::LandmarkPointType transformedSource = transform->TransformPoint(*sourcesIt);
double dist = targetsIt->EuclideanDistanceTo(transformedSource);
MITK_INFO << "target: " << *targetsIt << ", source: " << *sourcesIt << ", transformed source: " << transformedSource << ", dist: " << dist;
if (dist < minDistance )
{
minDistanceIterator = sourcesIt;
minDistance = dist;
}
}
if (minDistanceIterator == sources.end())
return false;
MITK_INFO << "minimum distance point is: " << *minDistanceIterator << " (dist: " << targetsIt->EuclideanDistanceTo(transform->TransformPoint(*minDistanceIterator)) << ", minDist: " << minDistance << ")";
sortedSources.push_back(*minDistanceIterator); // this point is assigned
sources.erase(minDistanceIterator); // erase it from sources to avoid duplicate assigns
}
//for (LandmarkPointContainer::const_iterator sortedSourcesIt = sortedSources.begin(); targetsIt != sortedSources.end(); ++targetsIt)
sources = sortedSources;
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);
}
}
