private void SetUpExtrinsics()
{
// Set device to imu matrix in Model Matrix Calculator.
TangoPoseData device2IMUPose = new TangoPoseData();
TangoCoordinateFramePair framePair = new TangoCoordinateFramePair();
framePair.BaseFrame = TangoPoseData.CoordinateFrameImu;
framePair.TargetFrame = TangoPoseData.CoordinateFrameDevice;
try
{
device2IMUPose = mTango.GetPoseAtTime(0.0, framePair);
}
catch (TangoErrorException)
{
Toast.MakeText(ApplicationContext, Resource.String.TangoError, Android.Widget.ToastLength.Short).Show();
}
mRenderer.ModelMatCalculator.SetDevice2IMUMatrix(device2IMUPose.GetTranslationAsFloats(), device2IMUPose.GetRotationAsFloats());
// Set color camera to imu matrix in Model Matrix Calculator.
TangoPoseData color2IMUPose = new TangoPoseData();
framePair.BaseFrame = TangoPoseData.CoordinateFrameImu;
framePair.TargetFrame = TangoPoseData.CoordinateFrameCameraColor;
try
{
color2IMUPose = mTango.GetPoseAtTime(0.0, framePair);
}
catch (TangoErrorException)
{
Toast.MakeText(ApplicationContext, Resource.String.TangoError, Android.Widget.ToastLength.Short).Show();
}
mRenderer.ModelMatCalculator.SetColorCamera2IMUMatrix(color2IMUPose.GetTranslationAsFloats(), color2IMUPose.GetRotationAsFloats());
}
/// <summary>
/// Read pose from file.
/// </summary>
/// <returns>The pose from file.</returns>
/// <param name="reader">File reader.</param>
/// <param name="pose">Tango pose data.</param>
public int ReadPoseFromFile(BinaryReader reader, ref TangoPoseData pose)
{
if (reader == null)
{
return(-1);
}
string frameMarker;
try
{
frameMarker = reader.ReadString();
}
catch (EndOfStreamException x)
{
reader.BaseStream.Position = 0;
Reset();
print("Restarting log file: " + x.ToString());
frameMarker = reader.ReadString();
}
if (frameMarker.CompareTo("poseframe\n") != 0)
{
m_debugText = "Failed to read pose";
return(-1);
}
pose.timestamp = double.Parse(reader.ReadString());
TangoCoordinateFramePair pair = new TangoCoordinateFramePair();
pair.baseFrame = (Tango.TangoEnums.TangoCoordinateFrameType)reader.ReadInt32();
pair.targetFrame = (Tango.TangoEnums.TangoCoordinateFrameType)reader.ReadInt32();
pose.framePair = pair;
pose.status_code = (Tango.TangoEnums.TangoPoseStatusType)reader.ReadInt32();
pose.translation[0] = reader.ReadDouble();
pose.translation[1] = reader.ReadDouble();
pose.translation[2] = reader.ReadDouble();
pose.orientation[0] = reader.ReadDouble();
pose.orientation[1] = reader.ReadDouble();
pose.orientation[2] = reader.ReadDouble();
pose.orientation[3] = reader.ReadDouble();
return(0);
}
