BoundingBoxType::Pointer SBImageUtils::GetBoundingBox(ImageType::Pointer image)
{
	ConstIteratorType constIterator(image, image->GetLargestPossibleRegion());
	
	IteratorType iterator(image, image->GetLargestPossibleRegion());
	
	PointSetType::Pointer  pointSet = PointSetType::New();
	PointsContainerPointer  points = pointSet->GetPoints();
	int pointCounter=0;
	
	for (iterator.GoToBegin(), constIterator.GoToBegin(); !iterator.IsAtEnd(); ++iterator, ++constIterator) {
		
		if(constIterator.Value() == 0)
		{
			ImageType::IndexType index = constIterator.GetIndex();
			PointType point;
			point[0] = index[0];
			point[1] = index[1];
			points->InsertElement(pointCounter, point);
			pointCounter++;
		}
	}
	
	BoundingBoxType::Pointer boundingBox = BoundingBoxType::New();
	boundingBox->SetPoints(points);
	boundingBox->ComputeBoundingBox();
	
	return boundingBox;
}
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;
}
/*
 * mexFunction(): entry point for the mex function
 */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  // interface to deal with input arguments from Matlab
  enum InputIndexType {IN_TRI, IN_X, IN_RES, IN_SIZE, IN_ORIGIN, InputIndexType_MAX};
  MatlabImportFilter::Pointer matlabImport = MatlabImportFilter::New();
  matlabImport->ConnectToMatlabFunctionInput(nrhs, prhs);

  // check the number of input arguments
  matlabImport->CheckNumberOfArguments(2, InputIndexType_MAX);

  // register the inputs for this function at the import filter
  typedef MatlabImportFilter::MatlabInputPointer MatlabInputPointer;
  MatlabInputPointer inTRI = matlabImport->RegisterInput(IN_TRI, "TRI");
  MatlabInputPointer inX = matlabImport->RegisterInput(IN_X, "X"); // (x, y, z)
  MatlabInputPointer inRES = matlabImport->RegisterInput(IN_RES, "RES"); // (r, c, s)
  MatlabInputPointer inSIZE = matlabImport->RegisterInput(IN_SIZE, "SIZE"); // (r, c, s)
  MatlabInputPointer inORIGIN = matlabImport->RegisterInput(IN_ORIGIN, "ORIGIN"); // (x, y, z)

  // interface to deal with outputs to Matlab
  enum OutputIndexType {OUT_IM, OutputIndexType_MAX};
  MatlabExportFilter::Pointer matlabExport = MatlabExportFilter::New();
  matlabExport->ConnectToMatlabFunctionOutput(nlhs, plhs);
  
  // check that the number of outputs the user is asking for is valid
  matlabExport->CheckNumberOfArguments(0, OutputIndexType_MAX);

  // register the outputs for this function at the export filter
  typedef MatlabExportFilter::MatlabOutputPointer MatlabOutputPointer;
  MatlabOutputPointer outIM = matlabExport->RegisterOutput(OUT_IM, "IM");

  // if any input point set is empty, the outputs are empty too
  if (mxIsEmpty(inTRI->pm) || mxIsEmpty(inX->pm)) {
    matlabExport->CopyEmptyArrayToMatlab(outIM);
    return;
  }

  // get number of rows in inputs X and TRI
  mwSize nrowsX = mxGetM(inX->pm);
  mwSize nrowsTRI = mxGetM(inTRI->pm);

  // instantiate mesh
  MeshType::Pointer mesh = MeshType::New();

  // read vertices
  PointSetType::Pointer xDef = PointSetType::New(); // default: empty point set
  PointSetType::Pointer x = PointSetType::New();
  x->GetPoints()->CastToSTLContainer()
    = matlabImport->ReadVectorOfVectorsFromMatlab<PointType::CoordRepType, PointType>
    (inX, xDef->GetPoints()->CastToSTLContainer());

#ifdef DEBUG
  std::cout << "Number of X points read = " << x->GetNumberOfPoints() << std::endl;
#endif

  // assertion check
  if (nrowsX != x->GetNumberOfPoints()) {
    mexErrMsgTxt(("Input " + inX->name 
		  + ": Number of points read different from number of points provided by user").c_str()); 
  }

  // swap XY coordinates to make them compliant with ITK convention
  // (see important programming note at the help header above)
  matlabImport->SwapXYInVectorOfVectors<PointType::CoordRepType, std::vector<PointType> >
    (x->GetPoints()->CastToSTLContainer(), x->GetNumberOfPoints());

  // populate mesh with vertices
  mesh->SetPoints(x->GetPoints());

  // read triangles
  PointType triDef;
  triDef.Fill(mxGetNaN());
  for (mwIndex i = 0; i < nrowsTRI; ++i) {

    PointType triangle = matlabImport->ReadRowVectorFromMatlab<CoordType, PointType>(inTRI, i, triDef);

    // create a triangle cell to read the vertex indices of the current input triangle
    CellAutoPointer cell;
    cell.TakeOwnership(new TriangleType);

    // assign to the 0, 1, 2 elements in the triangle cell the vertex
    // indices that we have just read. Note that we have to substract
    // 1 to convert Matlab's index convention 1, 2, 3, ... to C++
    // convention 0, 1, 2, ...
    cell->SetPointId(0, triangle[0] - 1);
    cell->SetPointId(1, triangle[1] - 1);
    cell->SetPointId(2, triangle[2] - 1);

    // insert cell into the mesh
    mesh->SetCell(i, cell);
  }

#ifdef DEBUG
  std::cout << "Number of triangles read = " << mesh->GetNumberOfCells() << std::endl;
#endif

  // assertion check
  if (nrowsTRI != mesh->GetNumberOfCells()) {
    mexErrMsgTxt(("Input " + inTRI->name 
		  + ": Number of triangles read different from number of triangles provided by user").c_str()); 
  }

  // get user input parameters for the output rasterization
  ImageType::SpacingType spacingDef;
  spacingDef.Fill(1.0);
  ImageType::SpacingType spacing = matlabImport->
    ReadRowVectorFromMatlab<ImageType::SpacingValueType, ImageType::SpacingType>(inRES, spacingDef);

  ImageType::SizeType sizeDef;
  sizeDef.Fill(10);
  ImageType::SizeType size = matlabImport->
    ReadRowVectorFromMatlab<ImageType::SizeValueType, ImageType::SizeType>(inSIZE, sizeDef);

  ImageType::PointType originDef;
  originDef.Fill(0.0);
  ImageType::PointType origin = matlabImport->
    ReadRowVectorFromMatlab<ImageType::PointType::ValueType, ImageType::PointType>(inORIGIN, originDef);
  // (see important programming note at the help header above)
  matlabImport->SwapXYInVector<ImageType::PointType::ValueType, ImageType::PointType>(origin);

  // instantiate rasterization filter
  MeshFilterType::Pointer meshFilter = MeshFilterType::New();

  // smallest voxel side length
  ImageType::SpacingValueType minSpacing = spacing[0];
  for (mwIndex i = 1; i < Dimension; ++i) {
    minSpacing = std::min(minSpacing, spacing[i]);
  }

  // pass input parameters to the filter
  meshFilter->SetInput(mesh);
  meshFilter->SetSpacing(spacing);
  meshFilter->SetSize(size);
  meshFilter->SetOrigin(origin);
  meshFilter->SetTolerance(minSpacing / 10.0);
  meshFilter->SetInsideValue(1);
  meshFilter->SetOutsideValue(0);

  ImageType::IndexType start;
  start.Fill(0);
  meshFilter->SetIndex(start);

  // convert image size from itk::Size format to std::vector<mwSize>
  // so that we can use it in GraftItkImageOntoMatlab
  std::vector<mwSize> sizeStdVector(Dimension);
  for (unsigned int i = 0; i < Dimension; ++i) {
    sizeStdVector[i] = size[i];
  }

  // graft ITK filter output onto Matlab output
  matlabExport->GraftItkImageOntoMatlab<PixelType, Dimension>
    (outIM, meshFilter->GetOutput(), sizeStdVector);

#ifdef DEBUG
  std::cout << "Resolution (spacing) = " << meshFilter->GetSpacing() << std::endl;
  std::cout << "Size = " << meshFilter->GetSize() << std::endl;
  std::cout << "Origin = " << meshFilter->GetOrigin() << std::endl;
#endif
  
  // run rasterization
  meshFilter->Update();

}