Transform3D IGTLinkConversion::decode(igtl::TransformMessage::Pointer msg)
{
igtl::Matrix4x4 matrix;
msg->GetMatrix(matrix);
Transform3D retval = Transform3D::fromFloatArray(matrix);
return retval;
}
void PlusProtocol::translate(const igtl::TransformMessage::Pointer body)
{
CX_LOG_DEBUG() << "Transform incoming to plusprotocol";
QString deviceName = body->GetDeviceName();
this->registerTransformDeviceName(deviceName);
IGTLinkConversion converter;
Transform3D matrix = converter.decode(body);
if(this->isCalibration(deviceName))
{
Transform3D s_M_igtltool = matrix;
Transform3D s_M_custustool = s_M_igtltool * igtltool_M_custustool;
Transform3D sMt = s_M_custustool;
QString calibrationBelongsToDeviceName = this->findDeviceForCalibration(deviceName);
if(calibrationBelongsToDeviceName != "NOT_FOUND")
{
emit calibration(calibrationBelongsToDeviceName, sMt);
}
}
else
{
IGTLinkConversionBase baseConverter;
double timestamp_ms = baseConverter.decode_timestamp(body).toMSecsSinceEpoch();
timestamp_ms = this->getSyncedTimestampForTransformsAndImages(timestamp_ms);
Transform3D prMs = matrix;
emit transform(deviceName, prMs, timestamp_ms);
}
}
void OpenIGTLinkProtocol::translate(const igtl::TransformMessage::Pointer body)
{
//CX_LOG_DEBUG() << "Transform incoming to OpenIGTLinkProtocol";
QString deviceName = body->GetDeviceName();
IGTLinkConversion converter;
Transform3D prMs = converter.decode(body);
IGTLinkConversionBase baseConverter;
QDateTime timestamp = baseConverter.decode_timestamp(body);
if (mStreamSynchronizer)
{
mStreamSynchronizer->addTimestamp(timestamp);
timestamp = timestamp.addMSecs(mStreamSynchronizer->getShift());
}
emit transform(deviceName, prMs, timestamp.toMSecsSinceEpoch());
}