/// <summary> /// Sets the listener coordinate frame pairs. /// </summary> /// <param name="count">Count.</param> /// <param name="frames">Frames.</param> public static void SetListenerCoordinateFramePairs(int count, ref TangoCoordinateFramePair frames) { int returnValue = PoseProviderAPI.TangoService_setPoseListenerFrames (count, ref frames); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { Debug.Log(CLASS_NAME + ".SetListenerCoordinateFramePairs() Could not set frame pairs"); } }
/// <summary> /// Sets the listener coordinate frame pairs. /// </summary> /// <param name="count">Count.</param> /// <param name="frames">Frames.</param> public static void SetListenerCoordinateFramePairs(int count, ref TangoCoordinateFramePair frames) { int returnValue = PoseProviderAPI.TangoService_setPoseListenerFrames(count, ref frames); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { Debug.Log(CLASS_NAME + ".SetListenerCoordinateFramePairs() Could not set frame pairs"); } }
/// <summary> /// Get a pose at a given timestamp from the base to the target frame. /// /// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if /// they were marked as TANGO_POSE_INITIALIZING in the callback poses. /// /// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID. /// </summary> /// <param name="poseData">The pose to return.</param> /// <param name="timeStamp"> /// Time specified in seconds. /// /// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0, /// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is /// contained in the pose output structure and may differ from the queried timestamp. /// </param> /// <param name="framePair"> /// A pair of coordinate frames specifying the transformation to be queried for. /// /// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base /// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE . /// </param> public static void GetPoseAtTime([In, Out] TangoPoseData poseData, double timeStamp, TangoCoordinateFramePair framePair) { int returnValue = PoseProviderAPI.TangoService_getPoseAtTime(timeStamp, framePair, poseData); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp); } }
/// <summary> /// Get a pose at a given timestamp from the base to the target frame. /// /// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if /// they were marked as TANGO_POSE_INITIALIZING in the callback poses. /// /// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID. /// </summary> /// <param name="poseData">The pose to return.</param> /// <param name="timeStamp"> /// Time specified in seconds. /// /// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0, /// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is /// contained in the pose output structure and may differ from the queried timestamp. /// </param> /// <param name="framePair"> /// A pair of coordinate frames specifying the transformation to be queried for. /// /// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base /// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE . /// </param> public static void GetPoseAtTime([In, Out] TangoPoseData poseData, double timeStamp, TangoCoordinateFramePair framePair) { #if UNITY_EDITOR poseData.framePair = framePair; bool pairIsValid = true; Matrix4x4 baseToDevice; Matrix4x4 targetToDevice; double adjustedTimeStamp1; double adjustedTimeStamp2; bool a = !GetFrameToDeviceTransformation(framePair.baseFrame, timeStamp, out adjustedTimeStamp1, out baseToDevice); bool b = !GetFrameToDeviceTransformation(framePair.targetFrame, timeStamp, out adjustedTimeStamp2, out targetToDevice); if (a || b) { pairIsValid = false; } Matrix4x4 baseToTarget = baseToDevice * targetToDevice.inverse; Quaternion rotation = Quaternion.LookRotation(baseToTarget.GetColumn(2), baseToTarget.GetColumn(1)); poseData.translation[0] = baseToTarget.m03; poseData.translation[1] = baseToTarget.m13; poseData.translation[2] = baseToTarget.m23; poseData.orientation[0] = rotation.x; poseData.orientation[1] = rotation.y; poseData.orientation[2] = rotation.z; poseData.orientation[3] = rotation.w; if (pairIsValid) { poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID; } else { poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID; Debug.Log(string.Format( CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : ts={0}, framePair={1},{2}", timeStamp, framePair.baseFrame, framePair.targetFrame)); } // Let most recent timestamp involved in the transformation be the timestamp // (relevant when using GetPoseAtTime(0)). // Behaviour may need to be updated after implmenting Area Description emulation. poseData.timestamp = System.Math.Max(adjustedTimeStamp1, adjustedTimeStamp2); #else int returnValue = PoseProviderAPI.TangoService_getPoseAtTime(timeStamp, framePair, poseData); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp); } #endif }
/// <summary> /// Sets the listener coordinate frame pairs. /// </summary> /// <param name="count">Count.</param> /// <param name="frames">Frames.</param> public static void SetListenerCoordinateFramePairs(int count, ref TangoCoordinateFramePair frames) { int returnValue = PoseProviderAPI.TangoService_setPoseListenerFrames(count, ref frames); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { DebugLogger.GetInstance.WriteToLog(DebugLogger.EDebugLevel.DEBUG_ERROR, CLASS_NAME + ".SetListenerCoordinateFramePairs() Could not set frame pairs"); } }
/// <summary> /// Sets the callback to be used when a new Pose is /// presented by the Tango Service. /// </summary> /// <param name="callback">Callback.</param> public static void SetCallback(TangoCoordinateFramePair[] framePairs, TangoService_onPoseAvailable callback) { int returnValue = PoseProviderAPI.TangoService_connectOnPoseAvailable(framePairs.Length, framePairs, callback); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { Debug.Log(CLASS_NAME + ".SetCallback() Callback was not set!"); } else { Debug.Log(CLASS_NAME + ".SetCallback() OnPose callback was set!"); } }
/// <summary> /// Get a pose at a given timestamp from the base to the target frame. /// /// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if /// they were marked as TANGO_POSE_INITIALIZING in the callback poses. /// /// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID. /// </summary> /// <param name="poseData">The pose to return.</param> /// <param name="timeStamp"> /// Time specified in seconds. /// /// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0, /// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is /// contained in the pose output structure and may differ from the queried timestamp. /// </param> /// <param name="framePair"> /// A pair of coordinate frames specifying the transformation to be queried for. /// /// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base /// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE . /// </param> public static void GetPoseAtTime([In, Out] TangoPoseData poseData, double timeStamp, TangoCoordinateFramePair framePair) { #if UNITY_EDITOR GetEmulatedPoseAtTime(poseData, timeStamp, framePair); #else int returnValue = API.TangoService_getPoseAtTime(timeStamp, framePair, poseData); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp); } #endif }
public static extern int TangoService_setPoseListenerFrames(int count, ref TangoCoordinateFramePair frames);
public static extern int TangoService_getPoseAtTime(double timestamp, TangoCoordinateFramePair framePair, [In, Out] TangoPoseData pose);
public static extern int TangoService_connectOnPoseAvailable(int count, TangoCoordinateFramePair[] framePairs, TangoService_onPoseAvailable onPoseAvailable);
/// <summary> /// Registers the callback. /// </summary> /// <param name="framePairs">Frame pairs.</param> public virtual void SetCallback(TangoCoordinateFramePair[] framePairs) { m_poseAvailableCallback = new Tango.PoseProvider.TangoService_onPoseAvailable(_OnPoseAvailable); Tango.PoseProvider.SetCallback(framePairs, m_poseAvailableCallback); }
public static int TangoService_setPoseListenerFrames(int count, ref TangoCoordinateFramePair frames) { return(Common.ErrorType.TANGO_SUCCESS); }
/// <summary> /// Register to get Tango pose callbacks for specific reference frames. /// /// NOTE: Tango pose callbacks happen on a different thread than the main /// Unity thread. /// </summary> /// <param name="framePairs">The reference frames to get callbacks for.</param> internal static void SetCallback(TangoCoordinateFramePair[] framePairs) { if (m_poseAvailableCallback != null) { Debug.Log("PoseListener.SetCallback() called when callback is already set."); return; } Debug.Log("PoseListener.SetCallback()"); m_poseAvailableCallback = new PoseProvider.APIOnPoseAvailable(_OnPoseAvailable); PoseProvider.SetCallback(framePairs, m_poseAvailableCallback); }
public static int TangoService_setPoseListenerFrames(int count, ref TangoCoordinateFramePair frames) { return Common.ErrorType.TANGO_SUCCESS; }
/// <summary> /// Emulation for PoseProvider.GetPoseAtTime(). /// </summary> /// <param name="poseData">Pose data.</param> /// <param name="timeStamp">Requested time stamp.</param> /// <param name="framePair">Requested frame pair.</param> internal static void GetEmulatedPoseAtTime(TangoPoseData poseData, double timeStamp, TangoCoordinateFramePair framePair) { poseData.framePair = framePair; double adjustedTimeStamp1 = timeStamp; double adjustedTimeStamp2 = timeStamp; Matrix4x4 baseToDevice; Matrix4x4 targetToDevice; TangoEnums.TangoPoseStatusType status1; TangoEnums.TangoPoseStatusType status2; _GetFrameToDevicePose(framePair.baseFrame, ref adjustedTimeStamp1, out baseToDevice, out status1); _GetFrameToDevicePose(framePair.targetFrame, ref adjustedTimeStamp2, out targetToDevice, out status2); // Composit base->device and target->device into base->target. Matrix4x4 baseToTarget = baseToDevice * targetToDevice.inverse; Quaternion rotation = Quaternion.LookRotation(baseToTarget.GetColumn(2), baseToTarget.GetColumn(1)); poseData.translation[0] = baseToTarget.m03; poseData.translation[1] = baseToTarget.m13; poseData.translation[2] = baseToTarget.m23; poseData.orientation[0] = rotation.x; poseData.orientation[1] = rotation.y; poseData.orientation[2] = rotation.z; poseData.orientation[3] = rotation.w; // Use the 'less successful' of the two statuses. if (status1 == TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN || status2 == TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN) { poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN; } else if (status1 == TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID || status2 == TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID) { poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID; } else if (status1 == TangoEnums.TangoPoseStatusType.TANGO_POSE_INITIALIZING || status2 == TangoEnums.TangoPoseStatusType.TANGO_POSE_INITIALIZING) { poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INITIALIZING; } else if (status1 == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID && status2 == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID; } else { poseData.status_code = TangoEnums.TangoPoseStatusType.NA; Debug.Log(string.Format( CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : ts={0}, framePair={1},{2}", timeStamp, framePair.baseFrame, framePair.targetFrame)); } // Let most recent timestamp involved in the transformation be the timestamp // (relevant when using GetPoseAtTime(0)), // Except when getting relocalization pose (area description <-> start of service), // in which case the timestamp should be the (theoretical) relocalization time. if ((framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE) || (framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION)) { if (poseData.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // First assume that relocalization happens at start (e.g. Area Learning for new areas). poseData.timestamp = m_beginningOfPoseEmulation; if (!EmulatedAreaDescriptionHelper.m_areaDescriptionFramesAvailableAtStart) { // Then add EMULATED_RELOCALIZATION_TIME second if an area description was loaded. poseData.timestamp += EMULATED_RELOCALIZATION_TIME; } // The initially requested timestamp is only valid if it: // A.) Is 0 // B.) Falls within the range of delivered relocalization frames // (and we only deliver one in emulation, so it must match that one exactly) bool validRelocalizationTimestamp = (timeStamp == 0) || (timeStamp == poseData.timestamp); if (!validRelocalizationTimestamp) { poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID; } } } else { poseData.timestamp = System.Math.Max(adjustedTimeStamp1, adjustedTimeStamp2); } }
/// <summary> /// Set callbacks on all PoseListener objects. /// </summary> /// <param name="framePairs">Frame pairs.</param> private void _SetMotionTrackingCallbacks(TangoCoordinateFramePair[] framePairs) { if (m_poseListener != null) { m_poseListener.AutoReset = m_motionTrackingAutoReset; m_poseListener.SetCallback(framePairs); } }
/// <summary> /// Set callbacks on all PoseListener objects. /// </summary> /// <param name="framePairs">Frame pairs.</param> private void _SetMotionTrackingCallbacks(TangoCoordinateFramePair[] framePairs) { PoseListener[] poseListeners = FindObjectsOfType<PoseListener>(); foreach (PoseListener poseListener in poseListeners) { if (poseListener != null) { poseListener.AutoReset = m_motionTrackingAutoReset; poseListener.SetCallback(framePairs); } } }
/// <summary> /// Set callbacks on all PoseListener objects. /// </summary> /// <param name="framePairs">Frame pairs.</param> private void _SetMotionTrackingCallbacks(TangoCoordinateFramePair[] framePairs) { Debug.Log("TangoApplication._SetMotionTrackingCallbacks()"); if (m_poseListener != null) { m_poseListener.AutoReset = m_motionTrackingAutoReset; m_poseListener.SetCallback(framePairs); } }
/// <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; }
/// <summary> /// Registers the callback. /// </summary> /// <param name="framePairs">Frame pairs.</param> public void SetCallback(TangoCoordinateFramePair[] framePairs) { m_poseAvailableCallback = new Tango.PoseProvider.TangoService_onPoseAvailable(_OnPoseAvailable); Tango.PoseProvider.SetCallback(framePairs, m_poseAvailableCallback); m_motionTrackingData = new TangoPoseData(); m_areaLearningData = new TangoPoseData(); m_relocalizationData = new TangoPoseData(); }
public static int TangoService_connectOnPoseAvailable(int count, TangoCoordinateFramePair[] framePairs, TangoService_onPoseAvailable onPoseAvailable) { return Common.ErrorType.TANGO_SUCCESS; }
public static int TangoService_getPoseAtTime(double timestamp, TangoCoordinateFramePair framePair, [In, Out] TangoPoseData pose) { return Common.ErrorType.TANGO_SUCCESS; }
public static int TangoService_getPoseAtTime(double timestamp, TangoCoordinateFramePair framePair, [In, Out] TangoPoseData pose) { return(Common.ErrorType.TANGO_SUCCESS); }
/// <summary> /// Get a pose at a given timestamp from the base to the target frame. /// /// All poses returned are marked as TANGO_POSE_VALID (in the status_code field on TangoPoseData ) even if /// they were marked as TANGO_POSE_INITIALIZING in the callback poses. /// /// If no pose can be returned, the status_code of the returned pose will be TANGO_POSE_INVALID. /// </summary> /// <param name="poseData">The pose to return.</param> /// <param name="timeStamp"> /// Time specified in seconds. /// /// If not set to 0.0, GetPoseAtTime retrieves the interpolated pose closest to this timestamp. If set to 0.0, /// the most recent pose estimate for the target-base pair is returned. The time of the returned pose is /// contained in the pose output structure and may differ from the queried timestamp. /// </param> /// <param name="framePair"> /// A pair of coordinate frames specifying the transformation to be queried for. /// /// For example, typical device motion is given by a target frame of TANGO_COORDINATE_FRAME_DEVICE and a base /// frame of TANGO_COORDINATE_FRAME_START_OF_SERVICE . /// </param> public static void GetPoseAtTime([In, Out] TangoPoseData poseData, double timeStamp, TangoCoordinateFramePair framePair) { #if UNITY_EDITOR bool baseFrameIsWorld = framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; bool targetFrameIsWorld = framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; // Area Descriptions are explicitly not supported yet. if (framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION || framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION) { Debug.Log(CLASS_NAME + ".GetPoseAtTime() emulation does not support Area Descriptions."); poseData.framePair = framePair; poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID; poseData.timestamp = 0; poseData.translation[0] = Vector3.zero.x; poseData.translation[1] = Vector3.zero.y; poseData.translation[2] = Vector3.zero.z; poseData.orientation[0] = Quaternion.identity.x; poseData.orientation[1] = Quaternion.identity.y; poseData.orientation[2] = Quaternion.identity.z; poseData.orientation[3] = Quaternion.identity.w; return; } if (baseFrameIsWorld && !targetFrameIsWorld) { if (timeStamp == 0) { float poseTimestamp; Vector3 posePosition; Quaternion poseRotation; if (GetTangoEmulationCurrent(out poseTimestamp, out posePosition, out poseRotation)) { poseData.framePair = framePair; poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID; poseData.timestamp = poseTimestamp; poseData.translation[0] = posePosition.x; poseData.translation[1] = posePosition.y; poseData.translation[2] = posePosition.z; poseData.orientation[0] = poseRotation.x; poseData.orientation[1] = poseRotation.y; poseData.orientation[2] = poseRotation.z; poseData.orientation[3] = poseRotation.w; return; } } else { Vector3 posePosition; Quaternion poseRotation; if (GetTangoEmulationAtTime((float)timeStamp, out posePosition, out poseRotation)) { poseData.framePair = framePair; poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID; poseData.timestamp = timeStamp; poseData.translation[0] = posePosition.x; poseData.translation[1] = posePosition.y; poseData.translation[2] = posePosition.z; poseData.orientation[0] = poseRotation.x; poseData.orientation[1] = poseRotation.y; poseData.orientation[2] = poseRotation.z; poseData.orientation[3] = poseRotation.w; return; } } } else if (baseFrameIsWorld == targetFrameIsWorld) { poseData.framePair = framePair; poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID; poseData.timestamp = timeStamp; poseData.translation[0] = Vector3.zero.x; poseData.translation[1] = Vector3.zero.y; poseData.translation[2] = Vector3.zero.z; poseData.orientation[0] = Quaternion.identity.x; poseData.orientation[1] = Quaternion.identity.y; poseData.orientation[2] = Quaternion.identity.z; poseData.orientation[3] = Quaternion.identity.w; return; } Debug.Log(string.Format( CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : ts={0}, framePair={1},{2}", timeStamp, framePair.baseFrame, framePair.targetFrame)); poseData.framePair = framePair; poseData.status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_INVALID; poseData.timestamp = 0; poseData.translation[0] = Vector3.zero.x; poseData.translation[1] = Vector3.zero.y; poseData.translation[2] = Vector3.zero.z; poseData.orientation[0] = Quaternion.identity.x; poseData.orientation[1] = Quaternion.identity.y; poseData.orientation[2] = Quaternion.identity.z; poseData.orientation[3] = Quaternion.identity.w; #else int returnValue = PoseProviderAPI.TangoService_getPoseAtTime(timeStamp, framePair, poseData); if (returnValue != Common.ErrorType.TANGO_SUCCESS) { Debug.Log(CLASS_NAME + ".GetPoseAtTime() Could not get pose at time : " + timeStamp); } #endif }