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