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;
}
