// Pose callbacks from Project Tango public void OnTangoPoseAvailable(Tango.TangoPoseData pose) { // Do nothing if we don't get a pose if (pose == null) { Debug.Log("TangoPoseData is null."); return; } // The callback pose is for device with respect to start of service pose. if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // Cache the position and rotation to be set in the update function. // This needs to be done because this callback does not // happen in the main game thread. m_tangoPosition = new Vector3((float)pose.translation[0], (float)pose.translation[1], (float)pose.translation[2]); m_tangoRotation = new Quaternion((float)pose.orientation[0], (float)pose.orientation[1], (float)pose.orientation[2], (float)pose.orientation[3]); } else // if the current pose is not valid we set the pose to identity { m_tangoPosition = Vector3.zero; m_tangoRotation = Quaternion.identity; } } }
/// <summary> /// OnTangoPoseAvailable 事件. /// /// 此例只是监听相应于区域描述帧对的 Start-Of-Service. /// 这个帧对表明一个重定位或闭环事件发生, 基于这, 我们还开始交互初始化或 marker 位置调整. /// </summary> /// <param name="poseData">Returned pose data from TangoService.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData poseData) { // 该帧对待回调表明一个闭环或重定位已发生. // // 学习模式时, 该回调表明闭环事件发生. // 闭环发生在系统识别到一个已访问区域时, 闭环操作将修正以前保存的姿态以获取更精确的结果. // (基于以前保存的时间戳, 可以通过 GetPoseAtTime 查询姿态.) // // 非学习模式且已加载一个区域描述时, 该回调表明重定位事件发生. // 重定位是设备找到它在那里, 相应于加载的区域描述. // 此例中, 当设备重定位后, marker 将被加载, 因为知道了其相对设备位置. if (poseData.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && poseData.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && poseData.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // 在第一次闭环/重定位事件, 初始化游戏内交互. if (!m_tangoReady) { m_tangoReady = true; if (m_curAreaDescription == null) { Debug.Log("AndroidInGameController.OnTangoPoseAvailable(): m_curAreaDescription is null"); return; } Debug.Log(m_curAreaDescription.GetMetadata().m_name); _LoadUnitFromDisk(); } } }
/// <summary> /// OnTangoPoseAvailable event from Tango. /// /// In this function, we only listen to the Start-Of-Service with respect to Area-Description frame pair. This pair /// indicates a relocalization or loop closure event happened, base on that, we either start the initialize the /// interaction or do a bundle adjustment for all marker position. /// </summary> /// <param name="poseData">Returned pose data from TangoService.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData poseData) { // This frame pair's callback indicates that a loop closure or relocalization has happened. // // When learning mode is on, this callback indicates the loop closure event. Loop closure will happen when the // system recognizes a pre-visited area, the loop closure operation will correct the previously saved pose // to achieve more accurate result. (pose can be queried through GetPoseAtTime based on previously saved // timestamp). // Loop closure definition: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping#Loop_closure // // When learning mode is off, and an Area Description is loaded, this callback indicates a // relocalization event. Relocalization is when the device finds out where it is with respect to the loaded // Area Description. In our case, when the device is relocalized, the markers will be loaded because we // know the relatvie device location to the markers. if (poseData.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && poseData.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && poseData.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // When we get the first loop closure/ relocalization event, we initialized all the in-game interactions. if (!m_initialized) { m_initialized = true; if (m_curAreaDescription == null) { Debug.Log("AndroidInGameController.OnTangoPoseAvailable(): m_curAreaDescription is null"); return; } _LoadMarkerFromDisk(); } } }
/// <summary> /// Handle the callback sent by the Tango Service /// when a new pose is sampled. /// </summary> /// <param name="pose">Pose.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData pose) { // The callback pose is for device with respect to start of service pose. if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { popManager.tangoInitialized = true; popManager.TriggerAPICallbackFPS(); UpdateInterpolationData(pose); if (enableInterpolation) { if (unityTimestampOffset > float.Epsilon) { UpdateUsingInterpolatedPose(Time.realtimeSinceStartup + unityTimestampOffset); } } else { UpdateUsingInterpolatedPose(currPose.timestamp); } } } }
/// <summary> /// OnTangoPoseAvailable event from Tango. /// /// In this function, we only listen to the Start-Of-Service with respect to Area-Description frame pair. This pair /// indicates a relocalization or loop closure event happened, base on that, we either start the initialize the /// interaction or begin meshing if applicable. /// </summary> /// <param name="poseData">Returned pose data from TangoService.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData poseData) { // This frame pair's callback indicates that a loop closure or relocalization has happened. // // When learning mode is off, and an Area Description is loaded, this callback indicates a relocalization event. // In our case, when the device is relocalized, user interaction is allowed and meshing starts if applicable. if (poseData.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && poseData.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && poseData.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // When we get the first loop closure/ relocalization event, we initialized all the in-game interactions. if (!m_initialized) { Debug.Log("First loop closure/relocalization"); m_initialized = true; m_relocalizeImage.gameObject.SetActive(false); if (m_3dReconstruction) { m_tangoApplication.Set3DReconstructionEnabled(true); } else { m_tangoApplication.Set3DReconstructionEnabled(false); } } } }
/// <summary> /// Pose callbacks from Project Tango. /// </summary> /// <param name="pose">Tango pose.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData pose) { // Do nothing if we don't get a pose if (pose == null) { Debug.Log("TangoPoseData is null."); return; } // The callback pose is for device with respect to start of service pose. if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // Cache the position and rotation to be set in the update function. m_tangoPosition = new Vector3((float)pose.translation[0], (float)pose.translation[1], (float)pose.translation[2]); m_tangoRotation = new Quaternion((float)pose.orientation[0], (float)pose.orientation[1], (float)pose.orientation[2], (float)pose.orientation[3]); //// Debug.Log("Tango VALID pose: " + m_tangoPosition + " " + m_tangoRotation); } } }
/// <summary> /// Handle the callback sent by the Tango Service /// when a new pose is sampled. /// </summary> /// <param name="pose">Pose.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData pose) { // Get out of here if the pose is null if (pose == null) { Debug.Log("TangoPoseDate is null."); return; } // The callback pose is for device with respect to start of service pose. if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { // Update the stats for the pose for the debug text if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // Create new Quaternion and Vec3 from the pose data received in the event. m_tangoPosition = new Vector3((float)pose.translation[0], (float)pose.translation[1], (float)pose.translation[2]); m_tangoRotation = new Quaternion((float)pose.orientation[0], (float)pose.orientation[1], (float)pose.orientation[2], (float)pose.orientation[3]); // Reset the current status frame count if the status code changed. if (pose.status_code != m_status) { m_frameCount = 0; } m_frameCount++; // Compute delta frame timestamp. m_frameDeltaTime = (float)pose.timestamp - m_prevFrameTimestamp; m_prevFrameTimestamp = (float)pose.timestamp; // Construct the start of service with respect to device matrix from the pose. Matrix4x4 matrixssTd = Matrix4x4.TRS(m_tangoPosition, m_tangoRotation, Vector3.one); // Converting from Tango coordinate frame to Unity coodinate frame. Matrix4x4 matrixuwTuc = m_matrixuwTss * matrixssTd * m_matrixdTuc; // Extract new local position transform.position = matrixuwTuc.GetColumn(3); // Extract new local rotation transform.rotation = Quaternion.LookRotation(matrixuwTuc.GetColumn(2), matrixuwTuc.GetColumn(1)); } else { // if the current pose is not valid we set the pose to identity m_tangoPosition = Vector3.zero; m_tangoRotation = Quaternion.identity; } // Finally, apply the new pose status m_status = pose.status_code; } }
public void OnTangoPoseAvailable( TangoPoseData pose ) { // Get out of here if the pose is null if( pose == null ){//|| pose.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID ) { Debug.Log( "TangoPoseData is null." ); return; } if( TangoUtility.SetPose( pose ) ) { TangoUtility.SetUnityWorldToUnityCamera( transform ); } //// The callback pose is for device with respect to start of service pose. //if( pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && // pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE ) { // //TangoUtility.InitExtrinsics( TangoPoseRequest.IMU_TO_DEVICE ); // if( TangoUtility.SetPose( pose ) ) { // TangoUtility.SetUnityWorldToUnityCamera( transform ); // } // //Matrix4x4 uwTuc = TangoUtility.GetUnityWorldToUnityCamera(); // //// Extract new local position // //transform.position = uwTuc.GetColumn( 3 ); // //// Extract new local rotation // //transform.rotation = Quaternion.LookRotation( uwTuc.GetColumn( 2 ), uwTuc.GetColumn( 1 ) ); //} }
/// <summary> /// Tango pose event. /// </summary> /// <param name="pose">Pose.</param> public void OnTangoPoseAvailable(TangoPoseData pose) { if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { Statics.currentTangoState = TangoPoseStates.Running; UpdateTransform(pose); return; } else { Statics.currentTangoState = TangoPoseStates.Relocalizing; } } else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { UpdateTransform(pose); } } else { return; } }
/// <summary> /// OnTangoPoseAvailable event from Tango. /// /// In this function, we only listen to the Start-Of-Service with respect to Area-Description frame pair. This pair /// indicates a relocalization or loop closure event happened, base on that, we either start the initialize the /// interaction or do a bundle adjustment for all marker position. /// </summary> /// <param name="poseData">Returned pose data from TangoService.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData poseData) { // This frame pair's callback indicates that a loop closure or relocalization has happened. // // When learning mode is on, this callback indicates the loop closure event. Loop closure will happen when the // system recognizes a pre-visited area, the loop closure operation will correct the previously saved pose // to achieve more accurate result. (pose can be queried through GetPoseAtTime based on previously saved // timestamp). // Loop closure definition: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping#Loop_closure // // When learning mode is off, and an Area Description is loaded, this callback indicates a // relocalization event. Relocalization is when the device finds out where it is with respect to the loaded // Area Description. In our case, when the device is relocalized, the markers will be loaded because we // know the relatvie device location to the markers. if (poseData.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && poseData.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && poseData.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // When we get the first loop closure/ relocalization event, we initialized all the in-game interactions. if (!m_initialized) { m_initialized = true; if (m_curAreaDescription == null) { Debug.Log("AndroidInGameController.OnTangoPoseAvailable(): m_curAreaDescription is null"); return; } // Attempt to load the exsiting markers from storage. List <MarkerData> xmlDataList = _ReadFromXml(m_curAreaDescription.m_uuid + ".xml"); if (xmlDataList == null) { Debug.Log("AndroidInGameController.OnTangoPoseAvailable(): xmlDataList is null"); return; } m_markerList.Clear(); foreach (MarkerData mark in xmlDataList) { // Instantiate all markers' gameobject. GameObject temp = Instantiate(m_markPrefabs[mark.m_type], mark.m_position, mark.m_orientation) as GameObject; m_markerList.Add(temp); } } else { Debug.Log("AndroidInGameController.OnTangoPoseAvailable(): relocalized"); if (m_tangoApplication.m_enableAreaLearning) { _UpdateMarkersForLoopClosures(); } } } }
/// <summary> /// OnTangoPoseAvailable is called from Tango when a new Pose is available. /// </summary> /// <param name="pose">The new Tango pose.</param> public void OnTangoPoseAvailable(TangoPoseData pose) { if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE && pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { m_relocalizationOverlay.SetActive(false); } }
public void OnTangoPoseAvailable(Tango.TangoPoseData poseData) { if (poseData.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && poseData.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && poseData.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { } }
public void OnTangoPoseAvailable(Tango.TangoPoseData poseData) { if (poseData.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && poseData.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && poseData.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { AndroidHelper.ShowAndroidToastMessage("Loop detected"); } }
/// <summary> /// Initializes a new instance of the <see cref="Tango.PoseListener"/> class. /// </summary> internal PoseListener() { m_poseDataPool = new Stack<TangoPoseData>(); // Add pre-allocated TangoPoseData objects to the // pool stack. for (int i = 0; i < SIZE_OF_POSE_DATA_POOL; ++i) { TangoPoseData emptyPose = new TangoPoseData(); m_poseDataPool.Push(emptyPose); } }
/// <summary> /// This function will be called the there's a new pose available in the system. /// when a new pose is sampled. /// </summary> /// <param name="pose">Pose data that we get from the estimation.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData pose) { // Get out of here if the pose is null if (pose == null) { Debug.Log("TangoPoseDate is null."); return; } // The callback pose is for device with respect to start of service pose. if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { Vector3 m_tangoPosition = new Vector3((float)pose.translation [0], (float)pose.translation [1], (float)pose.translation [2]); Quaternion m_tangoRotation = new Quaternion((float)pose.orientation [0], (float)pose.orientation [1], (float)pose.orientation [2], (float)pose.orientation [3]); Matrix4x4 ssTd = Matrix4x4.TRS(m_tangoPosition, m_tangoRotation, Vector3.one); // Here we are getting the pose of Unity camaer frame with respect to Unity world. // This is the transformation of our current pose within the Unity coordinate frame. Matrix4x4 uwTuc = m_uwTss * ssTd * Matrix4x4.Inverse(m_imuTd) * m_imuTc * m_cTuc; // Extract new local position transform.position = uwTuc.GetColumn(3); // Extract new local rotation transform.rotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1)); } } // Reset the current status frame count if the status code changed. if (pose.status_code != m_status) { m_frameCount = 0; } // Update the stats for the pose for the debug text m_status = pose.status_code; m_frameCount++; // Compute delta frame timestamp. m_frameDeltaTime = (float)pose.timestamp - m_prevFrameTimestamp; m_prevFrameTimestamp = (float)pose.timestamp; }
/// <summary> /// Handle the callback sent by the Tango Service /// when a new pose is sampled. /// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION! /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="pose">Pose.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData pose) { // Get out of here if the pose is null if (pose == null) { Debug.Log("TangoPoseDate is null."); return; } // The callback pose is for device with respect to start of service pose. if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // Cache the position and rotation to be set in the update function. // This needs to be done because this callback does not // happen in the main game thread. m_tangoPosition = new Vector3((float)pose.translation [0], (float)pose.translation [1], (float)pose.translation [2]); m_tangoRotation = new Quaternion((float)pose.orientation [0], (float)pose.orientation [1], (float)pose.orientation [2], (float)pose.orientation [3]); } else // if the current pose is not valid we set the pose to identity { m_tangoPosition = Vector3.zero; m_tangoRotation = Quaternion.identity; } } // Reset the current status frame count if the status code changed. if (pose.status_code != m_status) { m_frameCount = 0; } // Update the stats for the pose for the debug text m_status = pose.status_code; m_frameCount++; // Compute delta frame timestamp. m_frameDeltaTime = (float)pose.timestamp - m_prevFrameTimestamp; m_prevFrameTimestamp = (float)pose.timestamp; // Switch m_isDirty to true, so that the new pose get rendered in update. m_isDirty = (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID); }
/// <summary> /// Initializes a new instance of the <see cref="Tango.PoseListener"/> class. /// </summary> public PoseListener() { m_motionTrackingData = null; m_areaLearningData = null; m_relocalizationData = null; m_poseDataPool = new Stack<TangoPoseData>(); // Add pre-allocated TangoPoseData objects to the // pool stack. for(int i = 0; i < SIZE_OF_POSE_DATA_POOL; ++i) { TangoPoseData emptyPose = new TangoPoseData(); m_poseDataPool.Push(emptyPose); } }
/// ※ 루프결합(Loop closure) /// 기기가 지도 상에 이미 등록한 위치를 다시 방문하였을 때, 현재 측정된 센서 데이터와 지도상에 존재하는 동일 데이터 /// 사이의 일치를 찾아내고 관련 데이터를 연결함으로써 누적된 에러를 감소시켜 지도의 정확도를 향상하는 과정을 말한다. /// ※ 타임스탬프(Timestamp) /// 일반적으로, 어떤 기준 시각(보통, Epoch)부터 경과 시간을 수치값으로 주는 문자열 /// <summary> /// OnTangoPoseAvailable event from Tango. /// Tango의 OnTangoPoseAvailable 이벤트입니다. /// /// In this function, we only listen to the Start-Of-Service with respect to Area-Description frame pair. /// This pair indicates a relocalization or loop closure event happened, base on that, we either start the initialize /// the interaction or do a bundle adjustment for all marker position. /// 이 기능에서는 Area-Description 프레임 쌍에 대해서만 Start-Of-Service를 듣습니다. /// 이 쌍은 relocalization 또는 loop closure 이벤트가 발생했음을 나타냅니다. /// 기본으로 상호 작용을 초기화하거나 모든 마커 위치에 대해 번들 조정을 시작합니다. /// </summary> /// <param name="poseData">Returned pose data from TangoService.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData poseData) { // This frame pair's callback indicates that a loop closure or relocalization has happened. //이 프레임 쌍의 콜백은 루프결합 또는 relocalization이 발생했음을 나타냅니다. // When learning mode is on, this callback indicates the loop closure event. Loop closure will happen when the // system recognizes a pre-visited area, the loop closure operation will correct the previously saved pose // to achieve more accurate result. (pose can be queried through GetPoseAtTime based on previously saved // timestamp). // Loop closure definition: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping#Loop_closure // // 학습 모드가 켜져 있으면이 콜백은 루프 폐쇄 이벤트를 나타냅니다. // 루프결합은 시스템이 미리 방문한 영역을 인식 할 때 발생합니다. // 루프결합 작업은 이전에 저장된 포즈를 수정하여보다 정확한 결과를 얻습니다. // (포즈는 이전에 저장된 타임 스탬프를 기반으로 GetPoseAtTime을 통해 쿼리 할 수 있음). // When learning mode is off, and an Area Description is loaded, this callback indicates a // relocalization event. Relocalization is when the device finds out where it is with respect to the loaded // Area Description. In our case, when the device is relocalized, the markers will be loaded because we // know the relatvie device location to the markers. // // 학습 모드가 해제되면, 공간인식(공간데이터)가 로드되면 이 콜백은 Relocalization 이벤트를 나타냅니다. // Relocalization은 장치가 로드 된 공간인식(공간데이터)과 관련하여 장치를 찾은 경우입니다. // 우리의 경우에, 장치가 relocalization 될 때, 마커에 상대적 장치 위치를 알기 때문에 마커가 로드 될 것입니다. if (poseData.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && poseData.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && poseData.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // When we get the first loop closure/ relocalization event, we initialized all the in-game interactions. // 첫 번째 루프결합 / relocalization 이벤트가 발생하면 모든 게임 내 상호 작용이 초기화됩니다. if (!m_initialized) { m_initialized = true; if (m_curAreaDescription == null) { Debug.Log("AndroidInGameController.OnTangoPoseAvailable(): m_curAreaDescription is null"); return; } _LoadMarkerFromDisk(); } } }
/// <summary> /// An event notifying when a new pose is available. OnTangoPoseAvailable events are thread safe. /// </summary> /// <param name="pose">Pose.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData pose) { // Get out of here if the pose is null if (pose == null) { Debug.Log("TangoPoseDate is null."); return; } // The callback pose is for device with respect to start of service pose. if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // Create new Quaternion and Vec3 from the pose data received in the event. m_tangoPosition = new Vector3((float)pose.translation [0], (float)pose.translation [1], (float)pose.translation [2]); m_tangoRotation = new Quaternion((float)pose.orientation [0], (float)pose.orientation [1], (float)pose.orientation [2], (float)pose.orientation [3]); // Construct the start of service with respect to device matrix from the pose. Matrix4x4 ssTd = Matrix4x4.TRS(m_tangoPosition, m_tangoRotation, Vector3.one); // Converting from Tango coordinate frame to Unity coodinate frame. Matrix4x4 uwTuc = m_uwTss * ssTd * m_dTuc; // Extract new local position transform.position = m_startingRotation * uwTuc.GetColumn(3) + m_startingOffset; // Extract new local rotation transform.rotation = m_startingRotation * Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1)); } else // if the current pose is not valid we set the pose to identity { m_tangoPosition = Vector3.zero; m_tangoRotation = Quaternion.identity; } } }
/// <summary> /// Stop getting Tango pose callbacks. /// </summary> internal static void Reset() { // Avoid calling into tango_client_api before the correct library is loaded. if (m_poseAvailableCallback != null) { PoseProvider.ClearCallback(); } m_poseAvailableCallback = null; m_motionTrackingData = new TangoPoseData(); m_areaLearningData = new TangoPoseData(); m_relocalizationData = new TangoPoseData(); m_onTangoPoseAvailable = null; m_isMotionTrackingPoseAvailable = false; m_isAreaLearningPoseAvailable = false; m_isRelocalizaitonPoseAvailable = false; #if UNITY_EDITOR m_mostRecentEmulatedRelocalizationTimestamp = -1; #endif }
private void UpdateTransform(TangoPoseData pose) { Vector3 tangoPosition = new Vector3((float)pose.translation [0], (float)pose.translation [2], (float)pose.translation [1]); Quaternion tangoRotation = new Quaternion((float)pose.orientation [0], (float)pose.orientation [2], // these rotation values are swapped on purpose (float)pose.orientation [1], (float)pose.orientation [3]); Quaternion axisFix = Quaternion.Euler(-tangoRotation.eulerAngles.x, -tangoRotation.eulerAngles.z, tangoRotation.eulerAngles.y); transform.rotation = startingRotation * (rotationFix * axisFix); transform.position = (startingRotation * tangoPosition) + positionOffest; // Fire the state change event. if (preTangoState != Statics.currentTangoState) { EventManager.instance.TangoPoseStateChanged(Statics.currentTangoState); } preTangoState = Statics.currentTangoState; }
public static TangoEnums.TangoPoseStatusType GetPose( double timestamp ) { TangoCoordinateFramePair pair; TangoPoseData poseData = new TangoPoseData(); pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; PoseProvider.GetPoseAtTime( poseData, timestamp, pair ); if( poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID ) { return poseData.status_code; } Vector3 position = new Vector3( (float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2] ); Quaternion quat = new Quaternion( (float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3] ); m_ssTd = Matrix4x4.TRS( position, quat, Vector3.one ); return TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID; }
/// <summary> /// INTERNAL USE: Update the Tango emulation state for color camera data. /// </summary> /// <param name="useByteBufferMethod">Whether to update emulation for byte-buffer method.</param> internal static void UpdateTangoEmulation(bool useByteBufferMethod) { // Get emulated position and rotation in Unity space. TangoPoseData poseData = new TangoPoseData(); TangoCoordinateFramePair pair; pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; if (!PoseProvider.GetTimestampForColorEmulation(out m_lastColorEmulationTime)) { Debug.LogError("Couldn't get a valid timestamp with which to emulate color camera. " + "Color camera emulation will be skipped this frame."); return; } PoseProvider.GetPoseAtTime(poseData, m_lastColorEmulationTime, pair); if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { return; } Vector3 position; Quaternion rotation; TangoSupport.TangoPoseToWorldTransform(poseData, out position, out rotation); // Instantiate any resources that we haven't yet. _InternResourcesForEmulation(); // Render. EmulatedEnvironmentRenderHelper.RenderEmulatedEnvironment(m_emulatedColorRenderTexture, EmulatedEnvironmentRenderHelper.EmulatedDataType.COLOR_CAMERA, position, rotation); m_emulationIsDirty = true; }
/// <summary> /// The function is for querying the camera extrinsic, for example: the transformation between /// IMU and device frame. These extrinsics is used to transform the pose from the color camera frame /// to the device frame. Because the extrinsic is being queried using the GetPoseAtTime() /// with a desired frame pair, it can only be queried after the ConnectToService() is called. /// /// The device with respect to IMU frame is not directly queryable from API, so we use the IMU /// frame as a temporary value to get the device frame with respect to IMU frame. /// </summary> private void _SetCameraExtrinsics() { double timestamp = 0.0; TangoCoordinateFramePair pair; TangoPoseData poseData = new TangoPoseData(); // Get the transformation of device frame with respect to IMU frame. pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; PoseProvider.GetPoseAtTime(poseData, timestamp, pair); Vector3 position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]); Quaternion quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]); Matrix4x4 imuTd = Matrix4x4.TRS(position, quat, new Vector3(1.0f, 1.0f, 1.0f)); // Get the transformation of IMU frame with respect to color camera frame. pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR; PoseProvider.GetPoseAtTime(poseData, timestamp, pair); position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]); quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]); Matrix4x4 imuTc = Matrix4x4.TRS(position, quat, new Vector3(1.0f, 1.0f, 1.0f)); // Get the transform of the Unity Camera frame with respect to the Color Camera frame. Matrix4x4 cTuc = new Matrix4x4(); cTuc.SetColumn(0, new Vector4(1.0f, 0.0f, 0.0f, 0.0f)); cTuc.SetColumn(1, new Vector4(0.0f, -1.0f, 0.0f, 0.0f)); cTuc.SetColumn(2, new Vector4(0.0f, 0.0f, 1.0f, 0.0f)); cTuc.SetColumn(3, new Vector4(0.0f, 0.0f, 0.0f, 1.0f)); m_dTuc = Matrix4x4.Inverse(imuTd) * imuTc * cTuc; }
/// <summary> /// Handle the callback sent by the Tango Service /// when a new pose is sampled. /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="pose">Pose.</param> private void _OnPoseAvailable(IntPtr callbackContext, TangoPoseData pose) { // MotionTracking if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { // Only set new pose once the previous pose has been returned. if (m_motionTrackingData == null) { TangoPoseData currentPose = m_poseDataPool.Pop(); if (currentPose == null) { return; } else { currentPose.DeepCopy(pose); m_motionTrackingData = currentPose; } } } // ADF Localized else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { // Only set new pose once the previous pose has been returned. if (m_areaLearningData == null) { TangoPoseData currentPose = m_poseDataPool.Pop(); if (currentPose == null) { return; } else { currentPose.DeepCopy(pose); m_areaLearningData = currentPose; } } } // Relocalized against ADF else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE) { // Only set new pose once the previous pose has been returned. if (m_relocalizationData == null) { TangoPoseData currentPose = m_poseDataPool.Pop(); if (currentPose == null) { return; } else { currentPose.DeepCopy(pose); m_relocalizationData = currentPose; } } } m_isDirty = true; }
/// <summary> /// Callback that gets called when depth is available from the Tango Service. /// </summary> /// <param name="tangoDepth">Depth information from Tango.</param> public void OnTangoDepthAvailable(TangoUnityDepth tangoDepth) { // Calculate the time since the last successful depth data // collection. if (m_previousDepthDeltaTime == 0.0) { m_previousDepthDeltaTime = tangoDepth.m_timestamp; } else { m_depthDeltaTime = (float)((tangoDepth.m_timestamp - m_previousDepthDeltaTime) * 1000.0); m_previousDepthDeltaTime = tangoDepth.m_timestamp; } // Fill in the data to draw the point cloud. if (tangoDepth != null && tangoDepth.m_points != null) { m_pointsCount = tangoDepth.m_pointCount; if (m_pointsCount > 0) { _SetUpExtrinsics(); TangoCoordinateFramePair pair; TangoPoseData poseData = new TangoPoseData(); // Query pose to transform point cloud to world coordinates, here we are using the timestamp // that we get from depth. pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; PoseProvider.GetPoseAtTime(poseData, m_previousDepthDeltaTime, pair); if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { return; } Vector3 position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]); Quaternion quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]); m_startServiceTDevice = Matrix4x4.TRS(position, quat, Vector3.one); // The transformation matrix that represents the pointcloud's pose. // Explanation: // The pointcloud which is in Depth camera's frame, is put in unity world's // coordinate system(wrt unity world). // Then we are extracting the position and rotation from uwTuc matrix and applying it to // the PointCloud's transform. Matrix4x4 unityWorldTDepthCamera = m_unityWorldTStartService * m_startServiceTDevice * Matrix4x4.Inverse(m_imuTDevice) * m_imuTDepthCamera; transform.position = Vector3.zero; transform.rotation = Quaternion.identity; //Vector3 minpts = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue); //Vector3 maxpts = new Vector3(float.MinValue, float.MinValue, float.MinValue); // Converting points array to world space. m_overallZ = 0; for (int i = 0; i < m_pointsCount; ++i) { float x = tangoDepth.m_points[(i * 3) + 0]; float y = tangoDepth.m_points[(i * 3) + 1]; float z = tangoDepth.m_points[(i * 3) + 2]; m_points[i] = unityWorldTDepthCamera.MultiplyPoint( new Vector3(x, y, z)); m_overallZ += z; } m_overallZ = m_overallZ / m_pointsCount; // The color should be pose relative, we need to store enough info to go back to pose values. //m_renderer.material.SetMatrix("depthCameraTUnityWorld", unityWorldTDepthCamera.inverse); //VoxelExtractionPointCloud.Instance.computeDepthPlanes(ref unityWorldTDepthCamera, unityWorldTDepthCamera * new Vector4(0,0,0,1), minpts, maxpts); //if(isScanning) PerDepthFrameCallBack.Instance.CallBack(m_points, m_pointsCount); } else { m_overallZ = 0; } } }
/// <summary> /// Handle the callback sent by the Tango Service /// when a new pose is sampled. /// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION! /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="pose">Pose.</param> public void OnTangoPoseAvailable(Tango.TangoPoseData pose) { int currentIndex = 0; // Get out of here if the pose is null if (pose == null) { Debug.Log("TangoPoseDate is null."); return; } // The callback pose is for device with respect to start of service pose. if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { currentIndex = DEVICE_TO_START; } // The callback pose is for device with respect to area description file pose. else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { currentIndex = DEVICE_TO_ADF; } // The callback pose is for start of service with respect to area description file pose. else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE) { currentIndex = START_TO_ADF; } // check to see if we are recently relocalized if (!m_isRelocalized) { m_isRelocalized = (currentIndex == 2); } else { // if we are currently relocalized we can throw away // motion tracking poses if (currentIndex == DEVICE_TO_START) { return; } } if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // Cache the position and rotation to be set in the update function. // This needs to be done because this callback does not // happen in the main game thread. m_tangoPosition[currentIndex] = new Vector3((float)pose.translation [0], (float)pose.translation [2], (float)pose.translation [1]); m_tangoRotation[currentIndex] = new Quaternion((float)pose.orientation [0], (float)pose.orientation [2], // these rotation values are swapped on purpose (float)pose.orientation [1], (float)pose.orientation [3]); } else // if the current pose is not valid we set the pose to identity { m_tangoPosition[currentIndex] = Vector3.zero; m_tangoRotation[currentIndex] = Quaternion.identity; m_isRelocalized = false; } // Reset the current status frame count if the status code changed. if (pose.status_code != m_status[currentIndex]) { m_frameCount[currentIndex] = 0; } // Update the stats for the pose for the debug text m_status[currentIndex] = pose.status_code; m_frameCount[currentIndex]++; // Compute delta frame timestamp. m_frameDeltaTime[currentIndex] = (float)pose.timestamp - m_prevFrameTimestamp[currentIndex]; m_prevFrameTimestamp [currentIndex] = (float)pose.timestamp; // Switch m_isDirty to true, so that the new pose get rendered in update. m_isDirty = (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID); }
/// <summary> /// Set controller's transformation based on received pose. /// </summary> /// <param name="pose">Received Tango pose data.</param> private void _UpdateTransformationFromPose(TangoPoseData pose) { // Remember the previous position, so you can do delta motion m_prevTangoPosition = m_tangoPosition; m_prevTangoRotation = m_tangoRotation; // The callback pose is for device with respect to start of service pose. if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { Vector3 position; Quaternion rotation; TangoSupport.TangoPoseToWorldTransform(pose, out position, out rotation); m_uwTuc = Matrix4x4.TRS(position, rotation, Vector3.one); Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc; m_tangoPosition = uwOffsetTuc.GetColumn(3); m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1)); // Other pose data -- Pose count gets reset if pose status just became valid. if (pose.status_code != m_poseStatus) { m_poseCount = 0; } m_poseCount++; // Other pose data -- Pose delta time. m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp; m_poseTimestamp = (float)pose.timestamp; } m_poseStatus = pose.status_code; if (m_clutchActive) { // When clutching, preserve position. m_tangoPosition = m_prevTangoPosition; // When clutching, preserve yaw, keep changes in pitch, roll. Vector3 rotationAngles = m_tangoRotation.eulerAngles; rotationAngles.y = m_prevTangoRotation.eulerAngles.y; m_tangoRotation.eulerAngles = rotationAngles; } // Calculate final position and rotation deltas and apply them. Vector3 deltaPosition = m_tangoPosition - m_prevTangoPosition; Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation); if (m_characterMotion && m_characterController != null) { m_characterController.Move(deltaPosition); transform.rotation = deltaRotation * transform.rotation; } else { transform.position = transform.position + deltaPosition; transform.rotation = deltaRotation * transform.rotation; } }
/// <summary> /// Convert a TangoPoseData into the Unity coordinate system. This only /// works on TangoPoseData that describes the device with respect to the /// start of service or area description. The result position and /// rotation can be used to set Unity's Transform.position and /// Transform.rotation. /// </summary> /// <param name="poseData"> /// The input pose data that is going to be converted, please note that /// the pose data has to be in the start of service with respect to /// device frame. /// </param> /// <param name="position">The result position data.</param> /// <param name="rotation">The result rotation data.</param> public static void TangoPoseToWorldTransform(TangoPoseData poseData, out Vector3 position, out Quaternion rotation) { if (poseData == null) { Debug.Log("Invalid poseData.\n" + Environment.StackTrace); position = Vector3.zero; rotation = Quaternion.identity; return; } if (poseData.framePair.targetFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { Debug.Log("Invalid target frame of the poseData.\n" + Environment.StackTrace); position = Vector3.zero; rotation = Quaternion.identity; return; } if (poseData.framePair.baseFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && poseData.framePair.baseFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION) { Debug.Log("Invalid base frame of the poseData.\n" + Environment.StackTrace); position = Vector3.zero; rotation = Quaternion.identity; return; } Matrix4x4 startServiceTDevice = poseData.ToMatrix4x4(); Matrix4x4 unityWorldTUnityCamera = UNITY_WORLD_T_START_SERVICE * startServiceTDevice * DEVICE_T_UNITY_CAMERA * m_devicePoseRotation; // Extract final position, rotation. position = unityWorldTUnityCamera.GetColumn(3); rotation = Quaternion.LookRotation(unityWorldTUnityCamera.GetColumn(2), unityWorldTUnityCamera.GetColumn(1)); }
public static extern int TangoService_getPoseAtTime( double timestamp, TangoCoordinateFramePair framePair, [In, Out] TangoPoseData pose);
/// <summary> /// Callback that gets called when depth is available from the Tango Service. /// </summary> /// <param name="tangoDepth">Depth information from Tango.</param> public void OnTangoDepthAvailable(TangoUnityDepth tangoDepth) { // Calculate the time since the last successful depth data // collection. if (m_previousDepthDeltaTime == 0.0) { m_previousDepthDeltaTime = tangoDepth.m_timestamp; } else { m_depthDeltaTime = (float)((tangoDepth.m_timestamp - m_previousDepthDeltaTime) * 1000.0); m_previousDepthDeltaTime = tangoDepth.m_timestamp; } // Fill in the data to draw the point cloud. if (tangoDepth != null && tangoDepth.m_points != null) { m_pointsCount = tangoDepth.m_pointCount; if (m_pointsCount > 0) { _SetUpCameraData(); TangoCoordinateFramePair pair; TangoPoseData poseData = new TangoPoseData(); // Query pose to transform point cloud to world coordinates, here we are using the timestamp // that we get from depth. pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; PoseProvider.GetPoseAtTime(poseData, m_previousDepthDeltaTime, pair); if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { return; } Vector3 position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]); Quaternion quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]); m_startServiceTDevice = Matrix4x4.TRS(position, quat, Vector3.one); // The transformation matrix that represents the pointcloud's pose. // Explanation: // The pointcloud which is in Depth camera's frame, is put in unity world's // coordinate system(wrt unity world). // Then we are extracting the position and rotation from uwTuc matrix and applying it to // the PointCloud's transform. Matrix4x4 unityWorldTDepthCamera = m_unityWorldTStartService * m_startServiceTDevice * Matrix4x4.Inverse(m_imuTDevice) * m_imuTDepthCamera; transform.position = Vector3.zero; transform.rotation = Quaternion.identity; // Addoffset to the pointcloud depending on the offset from TangoDeltaPoseController Matrix4x4 unityWorldOffsetTDepthCamera; if (m_tangoDeltaPoseController != null) { unityWorldOffsetTDepthCamera = m_tangoDeltaPoseController.UnityWorldOffset * unityWorldTDepthCamera; } else { unityWorldOffsetTDepthCamera = unityWorldTDepthCamera; } // Converting points array to world space. m_overallZ = 0; for (int i = 0; i < m_pointsCount; ++i) { float x = tangoDepth.m_points[(i * 3) + 0]; float y = tangoDepth.m_points[(i * 3) + 1]; float z = tangoDepth.m_points[(i * 3) + 2]; m_points[i] = unityWorldOffsetTDepthCamera.MultiplyPoint(new Vector3(x, y, z)); m_overallZ += z; } m_overallZ = m_overallZ / m_pointsCount; m_pointsTimestamp = tangoDepth.m_timestamp; VoxelExtractionPointCloud.Instance.addAndRender(this); /* if (m_updatePointsMesh) { // Need to update indicies too! int[] indices = new int[m_pointsCount]; for (int i = 0; i < m_pointsCount; ++i) { indices[i] = i; } m_mesh.Clear(); m_mesh.vertices = m_points; m_mesh.SetIndices(indices, MeshTopology.Points, 0); } */ // The color should be pose relative, we need to store enough info to go back to pose values. //m_renderer.material.SetMatrix("depthCameraTUnityWorld", unityWorldOffsetTDepthCamera.inverse); } else { m_overallZ = 0; } } }
/// <summary> /// Tranform Tango pose data to Unity transform. /// </summary> /// <param name="xform">Unity Transform output.</param> /// <param name="pose">Tango Pose Data.</param> private void SetTransformUsingTangoPose(Transform xform, TangoPoseData pose) { xform.position = new Vector3((float)pose.translation[0], (float)pose.translation[2], (float)pose.translation[1]) + m_initCameraPosition; Quaternion quat = new Quaternion((float)pose.orientation[0], (float)pose.orientation[2], // these rotation values are swapped on purpose (float)pose.orientation[1], (float)pose.orientation[3]); Quaternion axisFixedQuat = Quaternion.Euler(-quat.eulerAngles.x, -quat.eulerAngles.z, quat.eulerAngles.y); // FIX - should query API for depth camera extrinsics Quaternion extrinsics = Quaternion.Euler(-12.0f, 0, 0); xform.rotation = Quaternion.Euler(90.0f, 0.0f, 0.0f) * axisFixedQuat * extrinsics; }
/// <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; }
public static int TangoService_getPoseAtTime( double timestamp, TangoCoordinateFramePair framePair, [In, Out] TangoPoseData pose) { return(Common.ErrorType.TANGO_SUCCESS); }
/// <summary> /// OnTangoPoseAvailable is called from Tango when a new Pose is available. /// </summary> /// <param name="pose">The new Tango pose.</param> public void OnTangoPoseAvailable(TangoPoseData pose) { // Get out of here if the pose is null if (pose == null) { Debug.Log("TangoPoseDate is null."); return; } // Only interested in pose updates relative to the start of service pose. if (!m_useAreaDescriptionPose) { if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { _UpdateTransformationFromPose(pose); } } else { if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { _UpdateTransformationFromPose(pose); } } }
/// <summary> /// Deep copy from poseToCopy into this. /// </summary> /// <param name="poseToCopy">Pose to copy.</param> public void DeepCopy(TangoPoseData poseToCopy) { this.version = poseToCopy.version; this.timestamp = poseToCopy.timestamp; this.status_code = poseToCopy.status_code; this.framePair.baseFrame = poseToCopy.framePair.baseFrame; this.framePair.targetFrame = poseToCopy.framePair.targetFrame; this.confidence = poseToCopy.confidence; for (int i = 0; i < 4; ++i) { this.orientation[i] = poseToCopy.orientation[i]; } for (int i = 0; i < 3; ++i) { this.translation[i] = poseToCopy.translation[i]; } }
/// <summary> /// Update the camera gameobject's transformation to the pose that on current timestamp. /// </summary> /// <param name="timestamp">Time to update the camera to.</param> private void _UpdateTransformation(double timestamp) { TangoPoseData pose = new TangoPoseData(); TangoCoordinateFramePair pair; pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; PoseProvider.GetPoseAtTime(pose, timestamp, pair); m_status = pose.status_code; if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { Vector3 m_tangoPosition = new Vector3((float)pose.translation[0], (float)pose.translation[1], (float)pose.translation[2]); Quaternion m_tangoRotation = new Quaternion((float)pose.orientation[0], (float)pose.orientation[1], (float)pose.orientation[2], (float)pose.orientation[3]); Matrix4x4 ssTd = Matrix4x4.TRS(m_tangoPosition, m_tangoRotation, Vector3.one); // Here we are getting the pose of Unity camera frame with respect to Unity world. // This is the transformation of our current pose within the Unity coordinate frame. Matrix4x4 uwTuc = m_uwTss * ssTd * m_dTuc; // Extract new local position m_renderCamera.transform.position = uwTuc.GetColumn(3); // Extract new local rotation m_renderCamera.transform.rotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1)); m_frameCount++; } else { m_frameCount = 0; } }
/// <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> /// Handle the callback sent by the Tango Service /// when a new pose is sampled. /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="pose">Pose.</param> protected abstract void _OnPoseAvailable(IntPtr callbackContext, TangoPoseData pose);
/// <summary> /// Sends the pose if available. /// </summary> /// <returns>The pose status if available.</returns> public void SendPoseIfAvailable(bool usingUXLibrary) { if(m_isDirty) { if(usingUXLibrary) { AndroidHelper.ParseTangoPoseStatus((int)m_latestPoseStatus); } if(m_onTangoPoseAvailable != null) { if(m_motionTrackingData != null) { m_onTangoPoseAvailable(m_motionTrackingData); m_poseDataPool.Push(m_motionTrackingData); m_motionTrackingData = null; } if(m_areaLearningData != null) { m_onTangoPoseAvailable(m_areaLearningData); m_poseDataPool.Push(m_areaLearningData); m_areaLearningData = null; } if(m_relocalizationData != null) { m_onTangoPoseAvailable(m_relocalizationData); m_poseDataPool.Push(m_relocalizationData); m_relocalizationData = null; } } m_isDirty = false; } }
/// <summary> /// Handle the callback sent by the Tango Service /// when a new pose is sampled. /// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION! /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="pose">Pose.</param> public void OnTangoPoseAvailable(TangoPoseData pose) { int currentIndex = 0; // Get out of here if the pose is null if (pose == null) { Debug.Log("TangoPoseDate is null."); return; } // The callback pose is for device with respect to start of service pose. if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { currentIndex = DEVICE_TO_START; } // The callback pose is for device with respect to area description file pose. else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { currentIndex = DEVICE_TO_ADF; } // The callback pose is for start of service with respect to area description file pose. else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE) { currentIndex = START_TO_ADF; } // check to see if we are recently relocalized if(!m_isRelocalized) { m_isRelocalized = (currentIndex == 2); } if(pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // Create a new Vec3 and Quaternion from the Pose Data received. m_tangoPosition[currentIndex] = new Vector3((float)pose.translation [0], (float)pose.translation [2], (float)pose.translation [1]); m_tangoRotation[currentIndex] = new Quaternion((float)pose.orientation [0], (float)pose.orientation [2], // these rotation values are swapped on purpose (float)pose.orientation [1], (float)pose.orientation [3]); } else // if the current pose is not valid we set the pose to identity { m_tangoPosition[currentIndex] = Vector3.zero; m_tangoRotation[currentIndex] = Quaternion.identity; m_isRelocalized = false; } // Reset the current status frame count if the status code changed. if (pose.status_code != m_status[currentIndex]) { m_frameCount[currentIndex] = 0; } // Update the stats for the pose for the debug text m_status[currentIndex] = pose.status_code; m_frameCount[currentIndex]++; // Compute delta frame timestamp. m_frameDeltaTime[currentIndex] = (float)pose.timestamp - m_prevFrameTimestamp[currentIndex]; m_prevFrameTimestamp [currentIndex] = (float)pose.timestamp; // This rotation needs to be put into Unity coordinate space. In unity +ve x is right, // +ve Y is up and +ve Z is forward while coordinate frame for Device wrt Start of service // +ve X is right, +ve Y is forward, +ve Z is up. // More explanation: https://developers.google.com/project-tango/overview/coordinate-systems Quaternion rotationFix = Quaternion.Euler(90.0f, 0.0f, 0.0f); // If not relocalized MotionTracking pose(Device wrt Start of Service) is used. if (!m_isRelocalized) { Quaternion axisFix = Quaternion.Euler(-m_tangoRotation[0].eulerAngles.x, -m_tangoRotation[0].eulerAngles.z, m_tangoRotation[0].eulerAngles.y); transform.rotation = m_startingRotation * (rotationFix * axisFix); transform.position = (m_startingRotation * (m_tangoPosition[0] * m_movementScale)) + m_startingOffset; } // If relocalized Device wrt ADF pose is used. else { Quaternion axisFix = Quaternion.Euler(-m_tangoRotation[1].eulerAngles.x, -m_tangoRotation[1].eulerAngles.z, m_tangoRotation[1].eulerAngles.y); transform.rotation = m_startingRotation * (rotationFix * axisFix); transform.position = (m_startingRotation * (m_tangoPosition[1] * m_movementScale)) + m_startingOffset; } }
/// <summary> /// Raise a Tango pose event if there is new data. /// </summary> internal void SendPoseIfAvailable() { #if UNITY_EDITOR if (TangoApplication.m_mouseEmulationViaPoseUpdates) { PoseProvider.UpdateTangoEmulation(); lock (m_lockObject) { if (m_onTangoPoseAvailable != null) { FillEmulatedPoseData(ref m_motionTrackingData, TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE, TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE); FillEmulatedPoseData(ref m_areaLearningData, TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION, TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE); m_isDirty = true; } } } #endif if (m_isDirty) { if (m_onTangoPoseAvailable != null) { // NOTE: If this becomes a performance issue, this could be changed to use // Interlocked.CompareExchange to "consume" the motion tracking data. lock (m_lockObject) { if (m_motionTrackingData != null) { m_onTangoPoseAvailable(m_motionTrackingData); m_poseDataPool.Push(m_motionTrackingData); m_motionTrackingData = null; } if (m_areaLearningData != null) { m_onTangoPoseAvailable(m_areaLearningData); m_poseDataPool.Push(m_areaLearningData); m_areaLearningData = null; } if (m_relocalizationData != null) { m_onTangoPoseAvailable(m_relocalizationData); m_poseDataPool.Push(m_relocalizationData); m_relocalizationData = null; } } } m_isDirty = false; } }
/// <summary> /// Sets up extrinsic matrixces for this hardware. /// </summary> private void _SetUpExtrinsics() { double timestamp = 0.0; TangoCoordinateFramePair pair; TangoPoseData poseData = new TangoPoseData(); // Query the extrinsics between IMU and device frame. pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; PoseProvider.GetPoseAtTime(poseData, timestamp, pair); Vector3 position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]); Quaternion quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]); m_imuTDevice = Matrix4x4.TRS(position, quat, new Vector3(1.0f, 1.0f, 1.0f)); // Query the extrinsics between IMU and color camera frame. pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_DEPTH; PoseProvider.GetPoseAtTime(poseData, timestamp, pair); position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]); quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]); m_imuTDepthCamera = Matrix4x4.TRS(position, quat, new Vector3(1.0f, 1.0f, 1.0f)); }
/// <summary> /// Handle the callback sent by the Tango Service when a new pose is sampled. /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="pose">Pose.</param> private void _OnPoseAvailable(IntPtr callbackContext, TangoPoseData pose) { if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { // MotionTracking lock (m_lockObject) { // Only set new pose once the previous pose has been returned. if (m_motionTrackingData == null) { TangoPoseData currentPose = m_poseDataPool.Pop(); currentPose.DeepCopy(pose); m_motionTrackingData = currentPose; } } } else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { // ADF Localized lock (m_lockObject) { // Only set new pose once the previous pose has been returned. if (m_areaLearningData == null) { TangoPoseData currentPose = m_poseDataPool.Pop(); currentPose.DeepCopy(pose); m_areaLearningData = currentPose; } } } else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE) { // Relocalized against ADF lock (m_lockObject) { // Only set new pose once the previous pose has been returned. if (m_relocalizationData == null) { TangoPoseData currentPose = m_poseDataPool.Pop(); currentPose.DeepCopy(pose); m_relocalizationData = currentPose; } } } m_isDirty = true; }
/// <summary> /// Update the transformation to the pose for that timestamp. /// </summary> /// <param name="timestamp">Time in seconds to update the transformation to.</param> private void _UpdateTransformation(double timestamp) { TangoPoseData pose = new TangoPoseData(); TangoCoordinateFramePair pair; pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; PoseProvider.GetPoseAtTime(pose, timestamp, pair); // The callback pose is for device with respect to start of service pose. if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // Construct matrix for the start of service with respect to device from the pose. Vector3 posePosition = new Vector3((float)pose.translation[0], (float)pose.translation[1], (float)pose.translation[2]); Quaternion poseRotation = new Quaternion((float)pose.orientation[0], (float)pose.orientation[1], (float)pose.orientation[2], (float)pose.orientation[3]); Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one); // Calculate matrix for the camera in the Unity world, taking into account offsets. Matrix4x4 uwTuc = m_uwTss * ssTd * m_dTuc; // Extract final position, rotation. m_tangoPosition = uwTuc.GetColumn(3); m_tangoRotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1)); // Other pose data -- Pose count gets reset if pose status just became valid. if (pose.status_code != m_poseStatus) { m_poseCount = 0; } m_poseCount++; // Other pose data -- Pose time. m_poseTimestamp = timestamp; } m_poseStatus = pose.status_code; // Apply final position and rotation. transform.position = m_tangoPosition; transform.rotation = m_tangoRotation; }
/// <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> /// Handle the callback sent by the Tango Service /// when a new pose is sampled. /// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION! /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="pose">Pose.</param> public void OnTangoPoseAvailable(TangoPoseData pose) { int currentIndex = 0; // Get out of here if the pose is null if (pose == null) { Debug.Log("TangoPoseDate is null."); return; } // The callback pose is for device with respect to start of service pose. if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { currentIndex = DEVICE_TO_START; } // The callback pose is for device with respect to area description file pose. else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { currentIndex = DEVICE_TO_ADF; } // The callback pose is for start of service with respect to area description file pose. else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION && pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE) { currentIndex = START_TO_ADF; } // check to see if we are recently relocalized if(!m_isRelocalized) { m_isRelocalized = (currentIndex == 2); } if(pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // Create a new Vec3 and Quaternion from the Pose Data received. m_tangoPosition[currentIndex] = new Vector3((float)pose.translation [0], (float)pose.translation [1], (float)pose.translation [2]); m_tangoRotation[currentIndex] = new Quaternion((float)pose.orientation [0], (float)pose.orientation [1], (float)pose.orientation [2], (float)pose.orientation [3]); } else // if the current pose is not valid we set the pose to identity { m_tangoPosition[currentIndex] = Vector3.zero; m_tangoRotation[currentIndex] = Quaternion.identity; m_isRelocalized = false; } // Reset the current status frame count if the status code changed. if (pose.status_code != m_status[currentIndex]) { m_frameCount[currentIndex] = 0; } // Update the stats for the pose for the debug text m_status[currentIndex] = pose.status_code; m_frameCount[currentIndex]++; // Compute delta frame timestamp. m_frameDeltaTime[currentIndex] = (float)pose.timestamp - m_prevFrameTimestamp[currentIndex]; m_prevFrameTimestamp [currentIndex] = (float)pose.timestamp; Matrix4x4 matrixssTd; // If not relocalized MotionTracking pose(Device wrt Start of Service) is used. if (!m_isRelocalized) { // Construct the device with respect to start of service matrix from the pose. matrixssTd = Matrix4x4.TRS(m_tangoPosition[0], m_tangoRotation[0], Vector3.one); } // If relocalized Device wrt ADF pose is used. else { // Construct the device with respect to ADF matrix from the pose. matrixssTd = Matrix4x4.TRS(m_tangoPosition[1], m_tangoRotation[1], Vector3.one); } // Converting from Tango coordinate frame to Unity coodinate frame. Matrix4x4 matrixuwTuc = m_matrixuwTss * matrixssTd * m_matrixdTuc; // Extract new local position transform.position = matrixuwTuc.GetColumn(3); // Extract new local rotation transform.rotation = Quaternion.LookRotation(matrixuwTuc.GetColumn(2), matrixuwTuc.GetColumn(1)); }
/// <summary> /// INTERNAL USE: Update the Tango emulation state for depth data. /// /// Make this this is only called once per frame. /// </summary> internal static void UpdateTangoEmulation() { m_emulatedPointCloud.Clear(); // Timestamp shall be something in the past, and we'll emulate the depth cloud based on it. m_lastDepthEmulationTime = PoseProvider.GetTimestampForDepthEmulation(); // Get emulated position and rotation in Unity space. TangoPoseData poseData = new TangoPoseData(); TangoCoordinateFramePair pair; pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; PoseProvider.GetPoseAtTime(poseData, m_lastDepthEmulationTime, pair); if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { return; } Vector3 position; Quaternion rotation; TangoSupport.TangoPoseToWorldTransform(poseData, out position, out rotation); // Instantiate any resources that we haven't yet. if (m_emulatedDepthTexture == null) { m_emulatedDepthTexture = new RenderTexture(NUM_X_DEPTH_SAMPLES, NUM_Y_DEPTH_SAMPLES, 24, RenderTextureFormat.ARGB32); } if (m_emulationCaptureTexture == null) { m_emulationCaptureTexture = new Texture2D(NUM_X_DEPTH_SAMPLES, NUM_Y_DEPTH_SAMPLES, TextureFormat.ARGB32, false); } if (m_emulatedDepthShader == null) { // Find depth shader by searching for it in project. string[] foundAssetGuids = UnityEditor.AssetDatabase.FindAssets("DepthEmulation t:Shader"); if (foundAssetGuids.Length > 0) { string assetPath = UnityEditor.AssetDatabase.GUIDToAssetPath(foundAssetGuids[0]); m_emulatedDepthShader = UnityEditor.AssetDatabase.LoadAssetAtPath(assetPath, typeof(Shader)) as Shader; } } // Render emulated depth camera data. EmulatedEnvironmentRenderHelper.RenderEmulatedEnvironment(m_emulatedDepthTexture, m_emulatedDepthShader, position, rotation); // Capture rendered depth points from texture. RenderTexture.active = m_emulatedDepthTexture; m_emulationCaptureTexture.ReadPixels(new Rect(0, 0, m_emulatedDepthTexture.width, m_emulatedDepthTexture.height), 0, 0); m_emulationCaptureTexture.Apply(); // Exctract captured data. Color32[] depthDataAsColors = m_emulationCaptureTexture.GetPixels32(); // Convert depth texture to positions in camera space. Matrix4x4 projectionMatrix = GL.GetGPUProjectionMatrix(EmulatedEnvironmentRenderHelper.m_emulationCamera.projectionMatrix, false); Matrix4x4 reverseProjectionMatrix = projectionMatrix.inverse; float width = m_emulationCaptureTexture.width; float height = m_emulationCaptureTexture.height; for (int yTexel = 0; yTexel < height; yTexel++) { for (int xTexel = 0; xTexel < width; xTexel++) { Color32 depthAsColor = depthDataAsColors[xTexel + (yTexel * m_emulationCaptureTexture.width)]; float clipSpaceZ = (depthAsColor.r - 128f) + (depthAsColor.g / 255f); float ndcSpaceZ = (clipSpaceZ - projectionMatrix.m23) / projectionMatrix.m22; float perspectiveDivisor = ndcSpaceZ * projectionMatrix.m32; float ndcSpaceX = (((xTexel + 0.5f) / width) * 2f) - 1; float ndcSpaceY = (((yTexel + 0.5f) / height) * 2f) - 1; Vector4 clipSpacePos = new Vector4(ndcSpaceX * perspectiveDivisor, ndcSpaceY * perspectiveDivisor, clipSpaceZ, perspectiveDivisor); Vector4 viewSpacePos = reverseProjectionMatrix * clipSpacePos; Vector3 emulatedDepthPos = new Vector3(viewSpacePos.x, -viewSpacePos.y, -viewSpacePos.z); if (emulatedDepthPos.z > MIN_POINT_DISTANCE && emulatedDepthPos.z < MAX_POINT_DISTANCE) { m_emulatedPointCloud.Add(emulatedDepthPos); } } } }
/// <summary> /// Sets up extrinsic matrixes and camera intrinsics for this hardware. /// </summary> private void _SetUpCameraData() { if (m_cameraDataSetUp) { return; } double timestamp = 0.0; TangoCoordinateFramePair pair; TangoPoseData poseData = new TangoPoseData(); // Query the extrinsics between IMU and device frame. pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; PoseProvider.GetPoseAtTime(poseData, timestamp, pair); Vector3 position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]); Quaternion quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]); m_imuTDevice = Matrix4x4.TRS(position, quat, new Vector3(1.0f, 1.0f, 1.0f)); // Query the extrinsics between IMU and color camera frame. pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR; PoseProvider.GetPoseAtTime(poseData, timestamp, pair); position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]); quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]); m_imuTColorCamera = Matrix4x4.TRS(position, quat, new Vector3(1.0f, 1.0f, 1.0f)); // Query the extrinsics between IMU and depth camera frame. pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_DEPTH; PoseProvider.GetPoseAtTime(poseData, timestamp, pair); position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]); quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]); m_imuTDepthCamera = Matrix4x4.TRS(position, quat, new Vector3(1.0f, 1.0f, 1.0f)); // Also get the camera intrinsics m_colorCameraIntrinsics = new TangoCameraIntrinsics(); VideoOverlayProvider.GetIntrinsics(TangoEnums.TangoCameraId.TANGO_CAMERA_COLOR, m_colorCameraIntrinsics); m_cameraDataSetUp = true; }
/// <summary> /// OnTangoPoseAvailable is called from Tango when a new Pose is available. /// </summary> /// <param name="pose">The new Tango pose.</param> public void OnTangoPoseAvailable(TangoPoseData pose) { // Get out of here if the pose is null if (pose == null) { Debug.Log("TangoPoseDate is null."); return; } // Only interested in pose updates relative to the start of service pose. if (pose.framePair.baseFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE || pose.framePair.targetFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE) { return; } // Remember the previous position, so you can do delta motion m_prevTangoPosition = m_tangoPosition; m_prevTangoRotation = m_tangoRotation; // The callback pose is for device with respect to start of service pose. if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { // Construct matrix for the start of service with respect to device from the pose. Vector3 posePosition = new Vector3((float)pose.translation[0], (float)pose.translation[1], (float)pose.translation[2]); Quaternion poseRotation = new Quaternion((float)pose.orientation[0], (float)pose.orientation[1], (float)pose.orientation[2], (float)pose.orientation[3]); Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one); // Calculate matrix for the camera in the Unity world, taking into account offsets. m_uwTuc = m_uwTss * ssTd * m_dTuc; Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc; // Extract final position, rotation. m_tangoPosition = uwOffsetTuc.GetColumn(3); m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1)); // Other pose data -- Pose count gets reset if pose status just became valid. if (pose.status_code != m_poseStatus) { m_poseCount = 0; } m_poseCount++; // Other pose data -- Pose delta time. m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp; m_poseTimestamp = (float)pose.timestamp; } m_poseStatus = pose.status_code; if (m_clutchActive) { // When clutching, preserve position. m_tangoPosition = m_prevTangoPosition; // When clutching, preserve yaw, keep changes in pitch, roll. Vector3 rotationAngles = m_tangoRotation.eulerAngles; rotationAngles.y = m_prevTangoRotation.eulerAngles.y; m_tangoRotation.eulerAngles = rotationAngles; } // Calculate final position and rotation deltas and apply them. Vector3 deltaPosition = m_tangoPosition - m_prevTangoPosition; Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation); if (m_characterMotion) { m_characterController.Move(deltaPosition); transform.rotation = deltaRotation * transform.rotation; } else { transform.position = transform.position + deltaPosition; transform.rotation = deltaRotation * transform.rotation; } }