Пример #1
0
mitk::PointSet::Pointer QmitkIGTTrackingLabView::GetVirtualPointSetFromPosition(mitk::NavigationData::Pointer navigationData)
{
  typedef itk::QuaternionRigidTransform<double> QuaternionTransformType;

  mitk::NavigationData::PositionType pointA;
  mitk::NavigationData::PositionType pointB;
  mitk::NavigationData::PositionType pointC;

  //initializing three points with position(0|0|0)
  pointA.Fill(0);
  pointB.Fill(0);
  pointC.Fill(0);

  // changing position off all points in order to make them orthogonal
  pointA[0] = 1;
  pointB[1] = 1;
  pointC[2] = 1;

  QuaternionTransformType::Pointer quatTransform = QuaternionTransformType::New();

  // orientation of NavigationData from parameter
  mitk::NavigationData::OrientationType quatIn = navigationData->GetOrientation();

  // set orientation to quaternion transform
  vnl_quaternion<double> const vnlQuatIn(quatIn.x(), quatIn.y(), quatIn.z(), quatIn.r());
  quatTransform->SetRotation(vnlQuatIn);

  // transform each point
  pointA = quatTransform->TransformPoint(pointA);
  pointB = quatTransform->TransformPoint(pointB);
  pointC = quatTransform->TransformPoint(pointC);

  // add position data from NavigationData parameter to each point
  pointA[0] += navigationData->GetPosition()[0];
  pointA[1] += navigationData->GetPosition()[1];
  pointA[2] += navigationData->GetPosition()[2];

  pointB[0] += navigationData->GetPosition()[0];
  pointB[1] += navigationData->GetPosition()[1];
  pointB[2] += navigationData->GetPosition()[2];

  pointC[0] += navigationData->GetPosition()[0];
  pointC[1] += navigationData->GetPosition()[1];
  pointC[2] += navigationData->GetPosition()[2];

  // insert points in source points pointset for the permanent registration landmark transform
  m_PermanentRegistrationSourcePoints->InsertPoint(0,pointA);
  m_PermanentRegistrationSourcePoints->InsertPoint(1,pointB);
  m_PermanentRegistrationSourcePoints->InsertPoint(2,pointC);


  return m_PermanentRegistrationSourcePoints;
}
bool mitk::NavigationDataReferenceTransformFilter::InitializeReferenceLandmarks()
{

  if (m_ReferenceInputIndexes.size() != 1 || (m_SourcePoints.size() < 3) || (m_ReferenceInputIndexes.size() <= 0) || m_ReferenceInputIndexes.size() > this->GetNumberOfInputs())
    return false;

  mitk::NavigationData::ConstPointer referenceND = this->GetInput(m_ReferenceInputIndexes.at(0));

  if(!referenceND->IsDataValid())
    return false;

  QuaternionTransformType::Pointer referenceTransform = QuaternionTransformType::New();
  QuaternionTransformType::VnlQuaternionType doubleOrientation(referenceND->GetOrientation().x(), referenceND->GetOrientation().y(),referenceND->GetOrientation().z(),referenceND->GetOrientation().r());

  referenceTransform->SetRotation(doubleOrientation);
  referenceTransform->SetOffset(referenceND->GetPosition().GetVectorFromOrigin());
  referenceTransform->Modified();

  try
  {
    bool okay = referenceTransform->GetInverse(m_QuaternionReferenceTransform);  // inverse transform to compensate rotation
    if(!okay)
      return false;
  }
  catch( ... )
  {
    return false;
  }

  m_ReferencePoints.clear();

  for(NavigationDataReferenceTransformFilter::LandmarkPointContainer::const_iterator it = m_SourcePoints.begin(); it != m_SourcePoints.end(); ++it)
  {
    QuaternionTransformType::InputPointType doublePoint;
    mitk::itk2vtk(*it,doublePoint); // workaround for itk bug
    TransformInitializerType::LandmarkPointType rLPoint;
    rLPoint = m_QuaternionReferenceTransform->TransformPoint(doublePoint);
    m_ReferencePoints.push_back(rLPoint);
  }
  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);

  }
}