bool PointSetsEqual(mitk::PointSet::Pointer pointSet1, mitk::PointSet::Pointer pointSet2)
{
  bool pointSetsEqual = true;
  if (pointSet1->GetSize()==pointSet2->GetSize())
  {
    for (unsigned int i=0; i<pointSet1->GetSize(); i++)
    {
      mitk::Point3D expectedPoint = pointSet1->GetPoint(i);
      mitk::Point3D resultPoint = pointSet2->GetPoint(i);
      if (!mitk::Equal(expectedPoint,resultPoint))
      {
        pointSetsEqual = false;
      }
    }
  }
  else
  {
    pointSetsEqual = false;
  }
  return pointSetsEqual;
}
  void TestCreateOperationAndAddPoint()
  {
    int id = 0;
    mitk::Point3D point;
    point.Fill(1);

    doOp = new mitk::PointOperation(mitk::OpINSERT, point, id);

    pointSet->ExecuteOperation(doOp);
    CPPUNIT_ASSERT_EQUAL_MESSAGE("check if added points exists",
        true, pointSet->GetSize()==4 && pointSet->IndexExists(id));


    mitk::Point3D tempPoint;
    tempPoint.Fill(0);

    tempPoint = pointSet->GetPoint(id);

    CPPUNIT_ASSERT_EQUAL_MESSAGE("check if added point contains real value",
        true, point == tempPoint);
  }
void mitk::NavigationDataLandmarkTransformFilter::SetSourceLandmarks(mitk::PointSet::Pointer mitkSourcePointSet)
{
  m_SourcePoints.clear();
  mitk::PointSet::PointType mitkSourcePoint;
  TransformInitializerType::LandmarkPointType lPoint;

  for (mitk::PointSet::PointsContainer::ConstIterator it = mitkSourcePointSet->GetPointSet()->GetPoints()->Begin();
    it != mitkSourcePointSet->GetPointSet()->GetPoints()->End(); ++it)
  {
    mitk::FillVector3D(lPoint, it->Value().GetElement(0), it->Value().GetElement(1), it->Value().GetElement(2));
    m_SourcePoints.push_back(lPoint);
  }

  if (m_SourcePoints.size() < 3)
  {
    itkExceptionMacro("SourcePointSet must contain at least 3 points");
  }

  if (this->IsInitialized())
    this->InitializeLandmarkTransform(m_SourcePoints, m_TargetPoints);
}
  void TestSwapPointPositionDownwards()
  {
    //Check SwapPointPosition downwards
    mitk::Point3D point;
    mitk::Point3D tempPoint;
    point = pointSet->GetPoint(0);
    pointSet->SwapPointPosition(0, false);
    tempPoint = pointSet->GetPoint(1);

    CPPUNIT_ASSERT_EQUAL_MESSAGE("check SwapPointPosition down",
        true, point == tempPoint);

    /*
if(point != tempPoint)
{
std::cout<<"[FAILED]"<<std::endl;
return EXIT_FAILURE;
}
std::cout<<"[PASSED]"<<std::endl;
     */
  }
  void TestPointOperationOpSelectPoint()
  {
    mitk::Point3D point3(0.);
    //check OpSELECTPOINT  ExecuteOperation

    doOp = new mitk::PointOperation(mitk::OpSELECTPOINT, point3,3);

    pointSet->ExecuteOperation(doOp);

    CPPUNIT_ASSERT_EQUAL_MESSAGE("check PointOperation OpSELECTPOINT ",
        true, pointSet->GetSelectInfo(3));

    /*
    if (!pointSet->GetSelectInfo(4))
    {
    std::cout<<"[FAILED]"<<std::endl;
    return EXIT_FAILURE;
    }
    delete doOp;
    std::cout<<"[PASSED]"<<std::endl;
     */
  }
mitk::PointSet::Pointer mitk::PointSetReaderService::ReadPoints(mitk::PointSet::Pointer newPointSet,
        TiXmlElement *currentTimeSeries,
        unsigned int currentTimeStep)
{
    if (currentTimeSeries->FirstChildElement("point") != NULL)
    {
        for (TiXmlElement *currentPoint = currentTimeSeries->FirstChildElement("point")->ToElement(); currentPoint != NULL;
                currentPoint = currentPoint->NextSiblingElement())
        {
            unsigned int id(0);
            mitk::PointSpecificationType spec((mitk::PointSpecificationType)0);
            double x(0.0);
            double y(0.0);
            double z(0.0);

            id = atoi(currentPoint->FirstChildElement("id")->GetText());
            if (currentPoint->FirstChildElement("specification") != NULL)
            {
                spec = (mitk::PointSpecificationType)atoi(currentPoint->FirstChildElement("specification")->GetText());
            }
            x = atof(currentPoint->FirstChildElement("x")->GetText());
            y = atof(currentPoint->FirstChildElement("y")->GetText());
            z = atof(currentPoint->FirstChildElement("z")->GetText());

            mitk::Point3D point;
            mitk::FillVector3D(point, x, y, z);
            newPointSet->SetPoint(id, point, spec, currentTimeStep);
        }
    }
    else
    {
        if (currentTimeStep != newPointSet->GetTimeSteps() + 1)
        {
            newPointSet->Expand(currentTimeStep + 1); // expand time step series with empty time step
        }
    }
    return newPointSet;
}
  void testTrimmedAicpregistration()
  {
    const double expFRE = 4.8912;
    const double expTRE = 0.0484215;

    mitk::AnisotropicIterativeClosestPointRegistration::Pointer aICP =
                      mitk::AnisotropicIterativeClosestPointRegistration::New();

    // Swap X and Y for partial overlapping registration
    aICP->SetMovingSurface(m_MovingSurface);
    aICP->SetFixedSurface(m_FixedSurface);
    aICP->SetCovarianceMatricesMovingSurface(m_SigmasMovingSurface);
    aICP->SetCovarianceMatricesFixedSurface(m_SigmasFixedSurface);
    aICP->SetFRENormalizationFactor(m_FRENormalizationFactor);
    aICP->SetThreshold(0.000001);
    aICP->SetTrimmFactor(0.50);

    // run the algorithm
    aICP->Update();

    MITK_INFO << "FRE: Expected: " << expFRE << ", computed: " << aICP->GetFRE();

    CPPUNIT_ASSERT_MESSAGE("mitkAnisotropicIterativeClosestPointRegistrationTest:AicpRegistration Test FRE",
                           mitk::Equal(aICP->GetFRE(),expFRE,0.01));

    // compute the target registration Error
    const double tre = mitk::AnisotropicRegistrationCommon::ComputeTargetRegistrationError(
                                                                                     m_TargetsMovingSurface.GetPointer(),
                                                                                     m_TargetsFixedSurface.GetPointer(),
                                                                                     aICP->GetRotation(),
                                                                                     aICP->GetTranslation()
                                                                                   );

    MITK_INFO << "TRE: Expected: " << expTRE << ", computed: " << tre;
    CPPUNIT_ASSERT_MESSAGE("mitkAnisotropicIterativeClosestPointRegistrationTest:AicpRegistration Test TRE",
                          mitk::Equal(tre,expTRE,0.01));
  }
    void PointSetLoadAndCompareTest()
    {
        try
        {
            mitk::PointSet::Pointer pointSet = mitk::IOUtil::LoadPointSet(m_FilePath);
            MITK_TEST_CONDITION(pointSet.IsNotNull(), "Testing if the loaded Data are NULL" );

            bool identical(true);
            PointSetCompare(pointSet.GetPointer(), m_SavedPointSet.GetPointer(),
                            identical);
        } catch (std::exception& e)
        {
            MITK_ERROR << "Error during pointset creation: " << e.what();
        }
    }
  void TestOpMovePointDown()
  {
    //check OpMOVEPOINTDown  ExecuteOperation

    const int id = 2;

    mitk::Point3D point = pointSet->GetPoint(id);
    mitk::Point3D point2(0.);
    doOp = new mitk::PointOperation(mitk::OpMOVEPOINTDOWN, point2, id);
    pointSet->ExecuteOperation(doOp);
    mitk::Point3D tempPoint = pointSet->GetPoint(id+1);

    CPPUNIT_ASSERT_EQUAL_MESSAGE("check PointOperation OpMOVEPOINTDOWN ",
        true, tempPoint == point);

    /*
    if (tempPoint != point)
    {
    std::cout<<"[FAILED]"<<std::endl;
    return EXIT_FAILURE;
    }
    std::cout<<"[PASSED]"<<std::endl;
     */
  }
  void TestSearchSelectedPoint()
  {
    // check SearchSelectedPoint
    CPPUNIT_ASSERT_EQUAL_MESSAGE("check SearchSelectedPoint ",
        true, pointSet->SearchSelectedPoint() == (int) selectedPointId);

          /*
      if( pointSet->SearchSelectedPoint() != 4)
      {
      std::cout<<"[FAILED]"<<std::endl;
      return EXIT_FAILURE;
      }
      std::cout<<"[PASSED]"<<std::endl;
           */
  }
  void testAicpRegistration()
  {
    const double expFRE = 27.5799;
    const double expTRE = 1.68835;
    mitk::AnisotropicIterativeClosestPointRegistration::Pointer aICP =
                      mitk::AnisotropicIterativeClosestPointRegistration::New();

    // set up parameters
    aICP->SetMovingSurface(m_MovingSurface);
    aICP->SetFixedSurface(m_FixedSurface);
    aICP->SetCovarianceMatricesMovingSurface(m_SigmasMovingSurface);
    aICP->SetCovarianceMatricesFixedSurface(m_SigmasFixedSurface);
    aICP->SetFRENormalizationFactor(m_FRENormalizationFactor);
    aICP->SetThreshold(0.000001);

    // run the algorithm
    aICP->Update();

    MITK_INFO << "FRE: Expected: " << expFRE << ", computed: " << aICP->GetFRE();
    CPPUNIT_ASSERT_MESSAGE("mitkAnisotropicIterativeClosestPointRegistrationTest:AicpRegistration Test FRE",
                           mitk::Equal(aICP->GetFRE(),expFRE,0.0001));

    // compute the target registration Error
    const double tre = mitk::AnisotropicRegistrationCommon::ComputeTargetRegistrationError(
                                                                                     m_TargetsMovingSurface.GetPointer(),
                                                                                     m_TargetsFixedSurface.GetPointer(),
                                                                                     aICP->GetRotation(),
                                                                                     aICP->GetTranslation()
                                                                                   );

   // MITK_INFO << "R:\n" << aICP->GetRotation() << "T: "<< aICP->GetTranslation();

    MITK_INFO << "TRE: Expected: " << expTRE << ", computed: " << tre;
    CPPUNIT_ASSERT_MESSAGE("mitkAnisotropicIterativeClosestPointRegistrationTest:AicpRegistration Test TRE",
                           mitk::Equal(tre,expTRE,0.00001));
  }
  void TestSwapPointPositionUpwardsNotPossible()
  {
    //Check SwapPointPosition upwards not possible
    CPPUNIT_ASSERT_EQUAL_MESSAGE("check SwapPointPosition upwards not possible",
        false, pointSet->SwapPointPosition(0, true));

    /*
if(pointSet->SwapPointPosition(0, true))
{
std::cout<<"[FAILED]"<<std::endl;
return EXIT_FAILURE;
}
std::cout<<"[PASSED]"<<std::endl;
     */
  }
  void TestIsNotEmpty()
  {
    //PointSet can not be empty!
    CPPUNIT_ASSERT_EQUAL_MESSAGE( "check if the PointSet is not empty ",
        true, !pointSet->IsEmptyTimeStep(0) );

        /*
    std::cout << "check if the PointSet is not empty ";
    if (pointSet->IsEmpty(0))
    {
    std::cout<<"[FAILED]"<<std::endl;
    return EXIT_FAILURE;
    }
    std::cout<<"[PASSED]"<<std::endl;
         */
  }
  void TestGetNumberOfSelected()
  {

    // check GetNumeberOfSelected
    CPPUNIT_ASSERT_EQUAL_MESSAGE("check GetNumeberOfSelected ",
        true, pointSet->GetNumberOfSelected() == 1);

          /*
      if(pointSet->GetNumberOfSelected() != 1)
      {
      std::cout<<"[FAILED]"<<std::endl;
      return EXIT_FAILURE;
      }
      std::cout<<"[PASSED]"<<std::endl;
           */
  }
void mitk::LabelAnnotation3D::SetLabelCoordinates(mitk::PointSet::Pointer LabelCoordinates)
{
  if (m_LabelCoordinates.IsNotNull())
  {
    m_LabelCoordinates->RemoveObserver(m_PointSetModifiedObserverTag);
    m_PointSetModifiedObserverTag = 0;
    m_LabelCoordinates = NULL;
  }
  if (LabelCoordinates.IsNull())
  {
    return;
  }
  m_LabelCoordinates = LabelCoordinates;
  itk::MemberCommand<mitk::LabelAnnotation3D>::Pointer _PropertyListModifiedCommand =
    itk::MemberCommand<mitk::LabelAnnotation3D>::New();
  _PropertyListModifiedCommand->SetCallbackFunction(this, &mitk::LabelAnnotation3D::PointSetModified);
  m_PointSetModifiedObserverTag = m_LabelCoordinates->AddObserver(itk::ModifiedEvent(), _PropertyListModifiedCommand);
  this->Modified();
}
  void TestGetPointIfExists()
  {
    //check GetPointIfExists
    mitk::Point3D point4;
    mitk::Point3D tempPoint;
    point4.Fill(5);
    mitk::PointSet::PointType tmpPoint;

    pointSet->GetPointIfExists(4, &tmpPoint);

    CPPUNIT_ASSERT_EQUAL_MESSAGE("check GetPointIfExists: ",
        true, tmpPoint == point4);
    /*
      if (tmpPoint != point5)
      {
      std::cout<<"[FAILED]"<<std::endl;
      return EXIT_FAILURE;
      }
      std::cout<<"[PASSED]"<<std::endl;
     */
  }
void QmitkUSNavigationStepPunctuationIntervention::UpdateCriticalStructures(mitk::NavigationData::Pointer needle, mitk::PointSet::Pointer path)
{
  // update the distances for the risk structures widget
  ui->riskStructuresRangeWidget->UpdateDistancesToNeedlePosition(needle);

  //iterate through all zones
  for (mitk::DataStorage::SetOfObjects::ConstIterator it = m_ZoneNodes->Begin();
       it != m_ZoneNodes->End(); ++it)
  {
    mitk::DataNode::Pointer currentNode = it->Value();
    //get center point and radius
    float radius = -1;
    mitk::Point3D center;
    currentNode->GetFloatProperty("zone.size", radius);
    center = currentNode->GetData()->GetGeometry()->GetIndexToWorldTransform()->GetTranslation();
    mitk::Point3D point0 = path->GetPoint(0);
    mitk::Point3D point1 = path->GetPoint(1);
    if (CheckSphereLineIntersection(center,radius,point0,point1))
      {currentNode->SetColor(mitk::IGTColor_WARNING);}
    else
      {currentNode->SetColor(m_OldColors[currentNode]);}
  }
}
double mitk::StaticIGTHelperFunctions::ComputeFRE(mitk::PointSet::Pointer imageFiducials, mitk::PointSet::Pointer realWorldFiducials, vtkSmartPointer<vtkLandmarkTransform> transform)
{
  if (imageFiducials->GetSize() != realWorldFiducials->GetSize()) return -1;
  double FRE = 0;
  for (int i = 0; i < imageFiducials->GetSize(); i++)
  {
    itk::Point<double> current_image_fiducial_point = imageFiducials->GetPoint(i);
    if (transform != NULL)
    {
      current_image_fiducial_point = transform->TransformPoint(imageFiducials->GetPoint(i)[0], imageFiducials->GetPoint(i)[1], imageFiducials->GetPoint(i)[2]);
    }
    double cur_error_squared = current_image_fiducial_point.SquaredEuclideanDistanceTo(realWorldFiducials->GetPoint(i));
    FRE += cur_error_squared;
  }

  FRE = sqrt(FRE / (double)imageFiducials->GetSize());

  return FRE;
}
  /**
* @brief Setup Always call this method before each Test-case to ensure correct and new intialization of the used members for a new test case. (If the members are not used in a test, the method does not need to be called).
*/
  void setUp()
  {
    m_PointSet = mitk::PointSet::New();
    m_AnotherPointSet = m_PointSet->Clone();
  }