void mitk::NavigationDataLandmarkTransformFilter::GenerateData()
{
  this->CreateOutputsForAllInputs(); // make sure that we have the same number of outputs as inputs

  TransformInitializerType::LandmarkPointType lPointIn, lPointOut;

  /* update outputs with tracking data from tools */
  for (unsigned int i = 0; i < this->GetNumberOfOutputs() ; ++i)
  {
    mitk::NavigationData* output = this->GetOutput(i);
    assert(output);
    const mitk::NavigationData* 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) // as long as there is no valid transformation matrix, only graft the outputs
      continue;

    mitk::NavigationData::PositionType tempCoordinate;
    tempCoordinate = input->GetPosition();
    lPointIn[0] = tempCoordinate[0]; // convert navigation data position to transform point
    lPointIn[1] = tempCoordinate[1];
    lPointIn[2] = tempCoordinate[2];

    /* transform position */
    lPointOut = m_LandmarkTransform->TransformPoint(lPointIn); // transform position
    tempCoordinate[0] = lPointOut[0];  // convert back into navigation data position
    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());  // convert orientation into vnl quaternion
    m_QuatTransform->SetRotation(vnlQuatIn);  // convert orientation into transform

    m_QuatLandmarkTransform->SetMatrix(m_LandmarkTransform->GetMatrix());

    m_QuatLandmarkTransform->Compose(m_QuatTransform, true); // compose navigation data transform and landmark transform

    vnl_quaternion<double> vnlQuatOut = m_QuatLandmarkTransform->GetRotation();  // convert composed transform back into a quaternion
    NavigationData::OrientationType quatOut(vnlQuatOut[0], vnlQuatOut[1], vnlQuatOut[2], vnlQuatOut[3]); // convert back into navigation data orientation
    output->SetOrientation(quatOut); // update output navigation data with new orientation
    output->SetDataValid(true); // operation was successful, therefore data of output is valid.
  }
}
void mitk::NavigationDataTransformFilter::GenerateData()
{

  // only update data if m_Transform was set
  if(m_Transform.IsNull())
  {
    itkExceptionMacro("Invalid parameter: Transform was not set!  Use SetRigid3DTransform() before updating the filter.");
    return;  
  }
  else
  {
    this->CreateOutputsForAllInputs(); // make sure that we have the same number of outputs as inputs

    /* update outputs with tracking data from tools */
    for (unsigned int i = 0; i < this->GetNumberOfOutputs() ; ++i)
    {
      mitk::NavigationData* output = this->GetOutput(i);
      assert(output);
      const mitk::NavigationData* input = this->GetInput(i);
      assert(input);

      if (input->IsDataValid() == false)
      {
        output->SetDataValid(false);
        continue;
      }

      mitk::NavigationData::PositionType tempCoordinateIn, tempCoordinateOut;
      tempCoordinateIn = input->GetPosition();

      itk::Point<float,3> itkPointIn, itkPointOut;
      itkPointIn[0] = tempCoordinateIn[0];
      itkPointIn[1] = tempCoordinateIn[1];
      itkPointIn[2] = tempCoordinateIn[2];

      //do the transform
      itkPointOut = m_Transform->TransformPoint( itkPointIn );  

      tempCoordinateOut[0] = itkPointOut[0];
      tempCoordinateOut[1] = itkPointOut[1];
      tempCoordinateOut[2] = itkPointOut[2];

      output->Graft(input); // First, copy all information from input to output
      output->SetPosition(tempCoordinateOut);// Then change the member(s): add new position of navigation data after tranformation
      output->SetDataValid(true); // operation was successful, therefore data of output is valid.

      //---transform orientation
      NavigationData::OrientationType  quatIn = input->GetOrientation();
      vnl_quaternion<double> const vnlQuatIn(quatIn.x(), quatIn.y(), quatIn.z(), quatIn.r());

      itk::Matrix<float,3,3> rotMatrix = m_Transform->GetMatrix();
      itk::Matrix<double,3,3> rotMatrixD;

      mitk::TransferMatrix(rotMatrix,rotMatrixD);

      m_QuatOrgRigidTransform->SetRotationMatrix(rotMatrixD);
      m_QuatTmpTransform->SetRotation(vnlQuatIn);
      m_QuatTmpTransform->Compose(m_QuatOrgRigidTransform,false);

      vnl_quaternion<double> vnlQuatOut = m_QuatTmpTransform->GetRotation();
      NavigationData::OrientationType quatOut(vnlQuatOut[0], vnlQuatOut[1], vnlQuatOut[2], vnlQuatOut[3]);
      
      output->SetOrientation(quatOut);


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

  }
}