itk::ProcessObject::DataObjectPointerArraySizeType mitk::NavigationDataToNavigationDataFilter::GetInputIndex( std::string navDataName )
{
DataObjectPointerArray outputs = this->GetInputs();
for (DataObjectPointerArray::size_type i = 0; i < outputs.size(); ++i)
if (navDataName == (static_cast<NavigationData*>(outputs.at(i).GetPointer()))->GetName())
return i;
throw std::invalid_argument("output name does not exist");
}
itk::ProcessObject::DataObjectPointerArraySizeType
mitk::IGTLDeviceSource::GetInputIndex( std::string msgName )
{
DataObjectPointerArray outputs = this->GetInputs();
for (DataObjectPointerArray::size_type i = 0; i < outputs.size(); ++i)
if (msgName ==
(static_cast<IGTLMessage*>(outputs.at(i).GetPointer()))->GetName())
return i;
throw std::invalid_argument("output name does not exist");
}
void mitk::NavigationDataRecorder::GenerateData()
{
// get each input, lookup the associated BaseData and transfer the data
DataObjectPointerArray inputs = this->GetIndexedInputs(); //get all inputs
//This vector will hold the NavigationDatas that are copied from the inputs
std::vector< mitk::NavigationData::Pointer > clonedDatas;
// For each input
for (unsigned int index=0; index < inputs.size(); index++)
{
// First copy input to output
this->GetOutput(index)->Graft(this->GetInput(index));
// if we are not recording, that's all there is to do
if (! m_Recording) continue;
// Clone a Navigation Data
mitk::NavigationData::Pointer clone = mitk::NavigationData::New();
clone->Graft(this->GetInput(index));
clonedDatas.push_back(clone);
if (m_StandardizeTime)
{
mitk::NavigationData::TimeStampType igtTimestamp = mitk::IGTTimeStamp::GetInstance()->GetElapsed(this);
clonedDatas[index]->SetIGTTimeStamp(igtTimestamp);
}
}
// if limitation is set and has been reached, stop recording
if ((m_RecordCountLimit > 0) && (m_NavigationDataSet->Size() >= m_RecordCountLimit)) m_Recording = false;
// We can skip the rest of the method, if recording is deactivated
if (!m_Recording) return;
// Add data to set
m_NavigationDataSet->AddNavigationDatas(clonedDatas);
}
void mitk::NavigationDataRecorderDeprecated::Update()
{
if (m_Recording)
{
DataObjectPointerArray inputs = this->GetInputs(); //get all inputs
mitk::NavigationData::TimeStampType timestamp=0.0; // timestamp for mitk time
timestamp = mitk::IGTTimeStamp::GetInstance()->GetElapsed();
mitk::NavigationData::TimeStampType sysTimestamp = 0.0; // timestamp for system time
sysTimestamp = m_SystemTimeClock->GetCurrentStamp();
// cast system time double value to stringstream to avoid low precision rounding
std::ostringstream strs;
strs.precision(15); // rounding precision for system time double value
strs << sysTimestamp;
std::string sysTimeStr = strs.str();
//if csv-mode: write csv header and timestamp at beginning
if (m_OutputFormat == mitk::NavigationDataRecorderDeprecated::csv)
{
//write header only when it's the first line
if (m_firstLine)
{
m_firstLine = false;
*m_Stream << "TimeStamp";
for (unsigned int index = 0; index < inputs.size(); index++) {
*m_Stream << ";Valid_Tool" << index <<
";X_Tool" << index <<
";Y_Tool" << index <<
";Z_Tool" << index <<
";QX_Tool" << index <<
";QY_Tool" << index <<
";QZ_Tool" << index <<
";QR_Tool" << index;
}
*m_Stream << "\n";
}
//write timestamp (always)
*m_Stream << timestamp;
}
//write tool data for every tool
for (unsigned int index = 0; index < inputs.size(); index++)
{
mitk::NavigationData* nd = dynamic_cast<mitk::NavigationData*>(inputs[index].GetPointer());
nd->Update(); // call update to propagate update to previous filters
mitk::NavigationData::PositionType position;
mitk::NavigationData::OrientationType orientation(0.0, 0.0, 0.0, 0.0);
mitk::NavigationData::CovarianceMatrixType matrix;
bool hasPosition = true;
bool hasOrientation = true;
bool dataValid = false;
position.Fill(0.0);
matrix.SetIdentity();
position = nd->GetPosition();
orientation = nd->GetOrientation();
matrix = nd->GetCovErrorMatrix();
hasPosition = nd->GetHasPosition();
hasOrientation = nd->GetHasOrientation();
dataValid = nd->IsDataValid();
//use this one if you want the timestamps of the source
//timestamp = nd->GetIGTTimeStamp();
//a timestamp is never < 0! this case happens only if you are using the timestamp of the nd object instead of getting a new one
if (timestamp >= 0)
{
if (this->m_OutputFormat == mitk::NavigationDataRecorderDeprecated::xml)
{
auto elem = new TiXmlElement("NavigationData");
elem->SetDoubleAttribute("Time", timestamp);
elem->SetAttribute("SystemTime", sysTimeStr); // tag for system time
elem->SetDoubleAttribute("Tool", index);
elem->SetDoubleAttribute("X", position[0]);
elem->SetDoubleAttribute("Y", position[1]);
elem->SetDoubleAttribute("Z", position[2]);
elem->SetDoubleAttribute("QX", orientation[0]);
elem->SetDoubleAttribute("QY", orientation[1]);
elem->SetDoubleAttribute("QZ", orientation[2]);
elem->SetDoubleAttribute("QR", orientation[3]);
elem->SetDoubleAttribute("C00", matrix[0][0]);
elem->SetDoubleAttribute("C01", matrix[0][1]);
elem->SetDoubleAttribute("C02", matrix[0][2]);
elem->SetDoubleAttribute("C03", matrix[0][3]);
elem->SetDoubleAttribute("C04", matrix[0][4]);
elem->SetDoubleAttribute("C05", matrix[0][5]);
elem->SetDoubleAttribute("C10", matrix[1][0]);
elem->SetDoubleAttribute("C11", matrix[1][1]);
elem->SetDoubleAttribute("C12", matrix[1][2]);
elem->SetDoubleAttribute("C13", matrix[1][3]);
elem->SetDoubleAttribute("C14", matrix[1][4]);
elem->SetDoubleAttribute("C15", matrix[1][5]);
if (dataValid)
elem->SetAttribute("Valid",1);
else
elem->SetAttribute("Valid",0);
if (hasOrientation)
elem->SetAttribute("hO",1);
else
elem->SetAttribute("hO",0);
if (hasPosition)
elem->SetAttribute("hP",1);
else
elem->SetAttribute("hP",0);
// set additional attribute?
auto
it = m_AdditionalAttributes.find( nd );
if( it != m_AdditionalAttributes.end() )
{
elem->SetAttribute(it->second.first, it->second.second);
}
*m_Stream << " " << *elem << std::endl;
delete elem;
}
else if (this->m_OutputFormat == mitk::NavigationDataRecorderDeprecated::csv)
{
*m_Stream << ";" << dataValid << ";" << position[0] << ";" << position[1] << ";" << position[2] << ";" << orientation[0] << ";" << orientation[1] << ";" << orientation[2] << ";" << orientation[3];
}
}
}
if (this->m_OutputFormat == mitk::NavigationDataRecorderDeprecated::csv)
{
*m_Stream << "\n";
}
}
m_RecordCounter++;
if ((m_RecordCountLimit<=m_RecordCounter)&&(m_RecordCountLimit != -1)) {
StopRecording();
}
}
void mitk::NavigationDataObjectVisualizationFilter::GenerateData()
{
/*get each input, lookup the associated BaseData and transfer the data*/
DataObjectPointerArray inputs = this->GetInputs(); //get all inputs
for (unsigned int index=0; index < inputs.size(); index++)
{
//get the needed variables
const mitk::NavigationData* nd = this->GetInput(index);
assert(nd);
mitk::NavigationData* output = this->GetOutput(index);
assert(output);
//check if the data is valid
if (!nd->IsDataValid())
{
output->SetDataValid(false);
continue;
}
output->Graft(nd); // copy all information from input to output
const mitk::BaseData* data = this->GetRepresentationObject(index);
if (data == NULL)
{
MITK_WARN << "No BaseData associated with input " << index;
continue;
}
//get the transform from data
mitk::AffineTransform3D::Pointer affineTransform = data->GetGeometry()->GetIndexToWorldTransform();
if (affineTransform.IsNull())
{
MITK_WARN << "AffineTransform IndexToWorldTransform not initialized!";
continue;
}
//check for offset
mitk::AffineTransform3D::Pointer offset = this->GetOffset(index);
//store the current scaling to set it after transformation
mitk::Vector3D spacing = data->GetGeometry()->GetSpacing();
//clear spacing of data to be able to set it again afterwards
ScalarType scale[] = {1.0, 1.0, 1.0};
data->GetGeometry()->SetSpacing(scale);
/*now bring quaternion to affineTransform by using vnl_Quaternion*/
affineTransform->SetIdentity();
if (this->GetTransformOrientation(index) == true)
{
mitk::NavigationData::OrientationType orientation = nd->GetOrientation();
/* because of an itk bug, the transform can not be calculated with float data type.
To use it in the mitk geometry classes, it has to be transfered to mitk::ScalarType which is float */
static AffineTransform3D::MatrixType m;
//convert quaternion to rotation matrix depending on the rotation mode
if(m_RotationMode == RotationStandard)
{
//calculate the transform from the quaternions
static itk::QuaternionRigidTransform<double>::Pointer quatTransform = itk::QuaternionRigidTransform<double>::New();
// convert mitk::ScalarType quaternion to double quaternion because of itk bug
vnl_quaternion<double> doubleQuaternion(orientation.x(), orientation.y(), orientation.z(), orientation.r());
quatTransform->SetIdentity();
quatTransform->SetRotation(doubleQuaternion);
quatTransform->Modified();
mitk::TransferMatrix(quatTransform->GetMatrix(), m);
}
else if(m_RotationMode == RotationTransposed)
{
vnl_matrix_fixed<mitk::ScalarType,3,3> rot = orientation.rotation_matrix_transpose();
for(int i=0; i<3; i++) for (int j=0; j<3; j++) m[i][j] = rot[i][j];
}
affineTransform->SetMatrix(m);
}
if (this->GetTransformPosition(index) == true)
{
///*set the offset by convert from itkPoint to itkVector and setting offset of transform*/
mitk::Vector3D pos;
pos.SetVnlVector(nd->GetPosition().GetVnlVector());
affineTransform->SetOffset(pos);
}
affineTransform->Modified();
//set the transform to data
if(offset.IsNotNull()) //first use offset if there is one.
{
mitk::AffineTransform3D::Pointer overallTransform = mitk::AffineTransform3D::New();
overallTransform->SetIdentity();
overallTransform->Compose(offset);
overallTransform->Compose(affineTransform);
data->GetGeometry()->SetIndexToWorldTransform(overallTransform);
}
else
{
data->GetGeometry()->SetIndexToWorldTransform(affineTransform);
}
//set the original spacing to keep scaling of the geometrical object
data->GetGeometry()->SetSpacing(spacing);
data->GetGeometry()->Modified();
data->Modified();
output->SetDataValid(true); // operation was successful, therefore data of output is valid.
}
//m_Measurement->AddMeasurement(10);
}
