public static int TangoSupport_calculateRelativePose(
double bseTimestamp, TangoEnums.TangoCoordinateFrameType baseFrame,
double targetTimestampe, TangoEnums.TangoCoordinateFrameType target,
TangoPoseData pose)
{
return(Common.ErrorType.TANGO_SUCCESS);
}
/// <summary>
/// Fill out <c>poseData</c> with emulated values from Tango.
/// </summary>
/// <param name="poseData">The poseData to fill out.</param>
/// <param name="baseFrame">Base frame to set.</param>
/// <param name="targetFrame">Target frame to set.</param>
private void FillEmulatedPoseData(ref TangoPoseData poseData, TangoEnums.TangoCoordinateFrameType baseFrame,
TangoEnums.TangoCoordinateFrameType targetFrame)
{
if (poseData == null)
{
TangoPoseData currentPose = m_poseDataPool.Pop();
if (currentPose != null)
{
Vector3 position;
Quaternion rotation;
PoseProvider.GetTangoEmulation(out position, out rotation);
currentPose.framePair.baseFrame = baseFrame;
currentPose.framePair.targetFrame = targetFrame;
currentPose.timestamp = Time.time * 1000; // timestamp is in ms, time is in sec.
currentPose.version = 0; // Not actually used
currentPose.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
currentPose.translation[0] = position.x;
currentPose.translation[1] = position.y;
currentPose.translation[2] = position.z;
currentPose.orientation[0] = rotation.x;
currentPose.orientation[1] = rotation.y;
currentPose.orientation[2] = rotation.z;
currentPose.orientation[3] = rotation.w;
poseData = currentPose;
}
}
}
/// <summary>
/// Gets the refrence frames coordinate system swaps one would get on an actual device going from the specified
/// base frame to a target Device reference frame.
///
/// For movement-related reference frames (e.g. Start of Service, Area Descriptions), applies the appropriate
/// coordinate system conversion to the given Unity-space transformation.
///
/// For static extrinsic reference frames (e.g. IMU, camera positions), overwrites the given matrix with a matrix
/// representing the appropriate axis swaps for that coordinate system conversion.
/// </summary>
/// <param name="baseFrame">Specified base frame.</param>
/// <param name="transformation">Transformation to apply coordinate system swaps to.</param>
private static void _GetFrameToDeviceAxisSwaps(TangoEnums.TangoCoordinateFrameType baseFrame,
ref Matrix4x4 transformation)
{
switch (baseFrame)
{
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE:
transformation = Matrix4x4.identity;
break;
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION:
// Poses are emulated in Unity space, so we have to temporarily move across a handedness
// swap into tango space to reconstruct the transformation as being SoS -> Device.
// Constant matrix converting between start of service and Unity world axis conventions.
Matrix4x4 startOfService_T_unityWorld = new Matrix4x4();
startOfService_T_unityWorld.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
startOfService_T_unityWorld.SetColumn(1, new Vector4(0.0f, 0.0f, 1.0f, 0.0f));
startOfService_T_unityWorld.SetColumn(2, new Vector4(0.0f, 1.0f, 0.0f, 0.0f));
startOfService_T_unityWorld.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
// Constant matrix converting between Unity world frame and device axis conventions.
Matrix4x4 unityCamera_T_device = new Matrix4x4();
unityCamera_T_device.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f));
unityCamera_T_device.SetColumn(1, new Vector4(0.0f, 1.0f, 0.0f, 0.0f));
unityCamera_T_device.SetColumn(2, new Vector4(0.0f, 0.0f, -1.0f, 0.0f));
unityCamera_T_device.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f));
transformation = startOfService_T_unityWorld * transformation * unityCamera_T_device;
break;
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU:
transformation.SetRow(0, new Vector4(0, -1, 0, 0));
transformation.SetRow(1, new Vector4(1, 0, 0, 0));
transformation.SetRow(2, new Vector4(0, 0, 1, 0));
transformation.SetRow(3, new Vector4(0, 0, 0, 1));
break;
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_DEPTH:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_FISHEYE:
transformation.SetRow(0, new Vector4(1, 0, 0, 0));
transformation.SetRow(1, new Vector4(0, -1, 0, 0));
transformation.SetRow(2, new Vector4(0, 0, -1, 0));
transformation.SetRow(3, new Vector4(0, 0, 0, 1));
break;
default:
Debug.Log(baseFrame.ToString() + " reference frame not handled by pose emulation.");
break;
}
}
/// <summary>
/// GetEmulatedPoseAtTime functions by getting pose information for both base and target
/// frames relative to the Device frame; this handles compositing information for one of
/// those frames.
/// </summary>
/// <param name="frame">Frame to get frame -> device frame for.</param>
/// <param name="timeStamp">Time stamp; if 0 may be modified with most up-to-date timestamp.</param>
/// <param name="frameToDeviceTransformation">Specified frame to device frame transformation.</param>
/// <param name="status">Status of the given frame to device frame pose.
/// See comments on GetEmulatedPoseAtTime().</param>
private static void _GetFrameToDevicePose(TangoEnums.TangoCoordinateFrameType frame,
ref double timeStamp,
out Matrix4x4 frameToDeviceTransformation,
out TangoEnums.TangoPoseStatusType status)
{
frameToDeviceTransformation = Matrix4x4.identity;
status = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
bool frameIsWorld = (frame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE) ||
(frame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION);
// Check that emulation supports this frame:
if (!_EmulationSupportsReferenceFrame(frame))
{
status = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
}
// Get mouse/keyboard-based emulation if appropriate.
if (frameIsWorld)
{
if (!_GetEmulatedMovementTransformAtTime(ref timeStamp, out frameToDeviceTransformation))
{
status = TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN;
}
if (frame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION)
{
bool areaDescriptionFramesAreValid =
EmulatedAreaDescriptionHelper.m_usingEmulatedDescriptionFrames &&
(EmulatedAreaDescriptionHelper.m_areaDescriptionFramesAvailableAtStart ||
_GetAreaDescriptionSyncDelayIsOver());
if (areaDescriptionFramesAreValid)
{
frameToDeviceTransformation.m03 += EmulatedAreaDescriptionHelper.m_emulationAreaOffset.x;
frameToDeviceTransformation.m13 += EmulatedAreaDescriptionHelper.m_emulationAreaOffset.y;
frameToDeviceTransformation.m23 += EmulatedAreaDescriptionHelper.m_emulationAreaOffset.z;
}
else
{
status = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID;
}
}
}
_GetFrameToDeviceAxisSwaps(frame, ref frameToDeviceTransformation);
}
/// <summary>
/// Returns whether emulation supports the given reference frame.
/// </summary>
/// <returns><c>true</c> if emulation supports the given reference frame, <c>false</c> otherwise.</returns>
/// <param name="referenceFrame">The reference frame in question.</param>
private static bool _EmulationSupportsReferenceFrame(TangoEnums.TangoCoordinateFrameType referenceFrame)
{
switch (referenceFrame)
{
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_DEPTH:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_FISHEYE:
return(true);
default:
Debug.Log(CLASS_NAME + ".GetPoseAtTime() emulation does not support the "
+ referenceFrame + " reference frame");
return(false);
}
}
/// <summary>
/// Fill out <c>poseData</c> with emulated values from Tango.
/// </summary>
/// <param name="poseData">The poseData to fill out.</param>
/// <param name="baseFrame">Base frame to set.</param>
/// <param name="targetFrame">Target frame to set.</param>
private void FillEmulatedPoseData(ref TangoPoseData poseData, TangoEnums.TangoCoordinateFrameType baseFrame,
TangoEnums.TangoCoordinateFrameType targetFrame)
{
Vector3 position;
Quaternion rotation;
PoseProvider.GetTangoEmulation(out position, out rotation);
poseData.framePair.baseFrame = baseFrame;
poseData.framePair.targetFrame = targetFrame;
poseData.timestamp = Time.time * 1000; // timestamp is in ms, time is in sec.
poseData.version = 0; // Not actually used
poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID;
poseData.translation[0] = position.x;
poseData.translation[1] = position.y;
poseData.translation[2] = position.z;
poseData.orientation[0] = rotation.x;
poseData.orientation[1] = rotation.y;
poseData.orientation[2] = rotation.z;
poseData.orientation[3] = rotation.w;
}
public static extern int TangoSupport_calculateRelativePose(
double bseTimestamp, TangoEnums.TangoCoordinateFrameType baseFrame,
double targetTimestampe, TangoEnums.TangoCoordinateFrameType target,
[In, Out] TangoPoseData pose);
/// <summary>
/// Get transformation from an arbitrary base frame to the Device frame.
/// Can be used to derive any particular frame-pair transformation.
/// </summary>
/// <returns><c>true</c>, if a valid transformation from the specified base to the
/// device frame exists for the given time, <c>false</c> otherwise.</returns>
/// <param name="baseFrame">Base frame.</param>
/// <param name="timeStamp">Time stamp being queried.</param>
/// <param name="adjustedTimeStamp">Adjusted time stamp (e.g., when querying current
/// Start of Service -> Device with GetPoseAtTime(0), this will return the current pose's timestamp).</param>
/// <param name="transformation">The transformation from the specified base frame to the Device frame
/// at the specified time.</param>
private static bool GetFrameToDeviceTransformation(TangoEnums.TangoCoordinateFrameType baseFrame, double timeStamp,
out double adjustedTimeStamp, out Matrix4x4 transformation)
{
adjustedTimeStamp = timeStamp;
switch (baseFrame)
{
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE:
transformation = Matrix4x4.identity;
return(true);
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE:
if (timeStamp == 0)
{
float poseTimestamp;
Vector3 posePosition;
Quaternion poseRotation;
if (GetTangoEmulationCurrent(out poseTimestamp, out posePosition, out poseRotation))
{
adjustedTimeStamp = poseTimestamp;
transformation = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);
return(true);
}
}
else
{
Vector3 posePosition;
Quaternion poseRotation;
if (GetTangoEmulationAtTime((float)timeStamp, out posePosition, out poseRotation))
{
transformation = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);
return(true);
}
}
transformation = Matrix4x4.identity;
return(false);
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION:
Debug.Log(CLASS_NAME + ".GetPoseAtTime() emulation does not support Area Descriptions.");
transformation = Matrix4x4.identity;
return(false);
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU:
transformation = new Matrix4x4();
transformation.SetRow(0, new Vector4(0, -1, 0, 0));
transformation.SetRow(1, new Vector4(1, 0, 0, 0));
transformation.SetRow(2, new Vector4(0, 0, 1, 0));
transformation.SetRow(3, new Vector4(0, 0, 0, 1));
return(true);
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_DEPTH:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR:
case TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_FISHEYE:
transformation = new Matrix4x4();
transformation.SetRow(0, new Vector4(1, 0, 0, 0));
transformation.SetRow(1, new Vector4(0, -1, 0, 0));
transformation.SetRow(2, new Vector4(0, 0, -1, 0));
transformation.SetRow(3, new Vector4(0, 0, 0, 1));
return(true);
default:
Debug.Log(CLASS_NAME + ".GetPoseAtTime() emulation does not support "
+ baseFrame + " frame");
transformation = Matrix4x4.identity;
return(false);
}
}
