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