/// <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;
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>
/// Callback that gets called when depth is available from the Tango Service.
/// </summary>
/// <param name="pointCloud">Depth information from Tango.</param>
public void OnTangoPointCloudAvailable(TangoPointCloudData pointCloud)
{
// Calculate the time since the last successful depth data
// collection.
if (m_depthTimestamp != 0.0)
{
m_depthDeltaTime = (float)((pointCloud.m_timestamp - m_depthTimestamp) * 1000.0);
}
// Fill in the data to draw the point cloud.
m_pointsCount = pointCloud.m_numPoints;
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.
if (m_useAreaDescriptionPose)
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
else
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
PoseProvider.GetPoseAtTime(poseData, pointCloud.m_timestamp, pair);
if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
return;
}
Matrix4x4 startServiceTDevice = poseData.ToMatrix4x4();
// 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 * startServiceTDevice * Matrix4x4.Inverse(m_imuTDevice) * m_imuTDepthCamera;
transform.position = Vector3.zero;
transform.rotation = Quaternion.identity;
// Add offset 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)
{
Vector3 point = pointCloud[i];
m_points[i] = unityWorldOffsetTDepthCamera.MultiplyPoint3x4(point);
m_overallZ += point.z;
}
m_overallZ = m_overallZ / m_pointsCount;
m_depthTimestamp = pointCloud.m_timestamp;
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);
// Try to find the floor using this set of depth points if requested.
if (m_findFloorWithDepth)
{
_FindFloorWithDepth();
}
}
else
{
m_overallZ = 0;
}
}
/// <summary>
/// Update is called once per frame.
/// </summary>
private void Update()
{
if (m_isDirty)
{
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_imuTd = 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_imuTc = Matrix4x4.TRS(position, quat, new Vector3(1.0f, 1.0f, 1.0f));
// 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);
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_ssTd = Matrix4x4.TRS(position, quat, Vector3.one);
// The transformation matrix that represents the pointcloud's pose.
Matrix4x4 uwTuc = m_uwTss * m_ssTd * Matrix4x4.Inverse(m_imuTd) * m_imuTc * m_cTuc;
transform.position = uwTuc.GetColumn(3);
transform.rotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));
m_mesh.Clear();
m_mesh.vertices = m_vertices;
m_mesh.triangles = m_triangles;
m_mesh.SetIndices(m_triangles, MeshTopology.Points, 0);
m_isDirty = false;
}
}
/// <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>
/// Callback that gets called when depth is available
/// from the Tango Service.
/// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION!
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="xyzij">Xyzij.</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)
{
int numberOfActiveVertices = tangoDepth.m_pointCount;
m_pointsCount = numberOfActiveVertices;
float validPointCount = 0;
if (numberOfActiveVertices > 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);
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);
// The transformation matrix that represents the pointcloud's pose.
// Explanation:
// The pointcloud which is in RGB's camera frame, is put in unity world's
// coordinate system(wrt unit camera).
// Then we are extracting the position and rotation from uwTuc matrix and applying it to
// the PointCloud's transform.
Matrix4x4 uwTuc = m_uwTss * m_ssTd * Matrix4x4.Inverse(m_imuTd) * m_imuTc * m_cTuc;
transform.position = uwTuc.GetColumn(3);
transform.rotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));
Vector3[] pointCloudVertices = new Vector3[VERT_COUNT];
// Converting points array to point vector array for Unity to create a mesh.
for (int i = 0; i < numberOfActiveVertices; ++i)
{
pointCloudVertices[i] = new Vector3(tangoDepth.m_points[i * 3],
tangoDepth.m_points[i * 3 + 1],
tangoDepth.m_points[i * 3 + 2]);
m_overallZ += pointCloudVertices[i].z;
++validPointCount;
}
m_mesh.Clear();
m_mesh.vertices = pointCloudVertices;
m_mesh.triangles = m_triangles;
m_mesh.SetIndices(m_triangles, MeshTopology.Points, 0);
}
// Don't divide by zero!
if (validPointCount != 0)
{
m_overallZ = m_overallZ / validPointCount;
}
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(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>
/// Updates the transformation to the latest pose.
/// </summary>
private void _UpdatePose()
{
// Query a new pose.
TangoPoseData pose = new TangoPoseData();
double queryTimestamp = IsTargetingColorCamera ? m_tangoARScreen.m_screenUpdateTime : 0.0f;
PoseProvider.GetPoseAtTime(pose, queryTimestamp, _GetFramePair());
// Do not update with invalide poses.
if (pose.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
return;
}
// Do not update if the last update was for the same timestamp.
if (pose.timestamp == LastPoseTimestamp)
{
return;
}
LastPoseTimestamp = pose.timestamp;
DMatrix4x4 globalTLocal;
if (!m_tangoApplication.GetGlobalTLocal(out globalTLocal))
{
Debug.LogError("Unable to obtain GlobalTLocal from TangoApplication.");
return;
}
DMatrix4x4 unityWorld_T_device =
DMatrix4x4.FromMatrix4x4(TangoSupport.UNITY_WORLD_T_START_SERVICE) * globalTLocal.Inverse * DMatrix4x4.TR(pose.translation, pose.orientation);
// Calculate matrix for the camera in the Unity world
if (IsTargetingColorCamera)
{
m_unityWorld_T_unityCamera =
unityWorld_T_device.ToMatrix4x4() * TangoSupport.COLOR_CAMERA_T_UNITY_CAMERA * TangoSupport.m_colorCameraPoseRotation;
}
else
{
m_unityWorld_T_unityCamera =
unityWorld_T_device.ToMatrix4x4() * TangoSupport.DEVICE_T_UNITY_CAMERA * TangoSupport.m_devicePoseRotation;
}
// Extract final position and rotation from matrix.
Matrix4x4 unityWorldOffset_T_unityCamera = m_unityWorldTransformOffset_T_unityWorld * m_unityWorld_T_unityCamera;
Vector3 finalPosition = unityWorldOffset_T_unityCamera.GetColumn(3);
Quaternion finalRotation = Quaternion.LookRotation(unityWorldOffset_T_unityCamera.GetColumn(2), unityWorldOffset_T_unityCamera.GetColumn(1));
// Filter out yaw if the clutch is enabled.
if (m_clutchEnabled)
{
finalPosition = transform.position;
finalRotation = Quaternion.Euler(finalRotation.eulerAngles.x, transform.eulerAngles.y, finalRotation.eulerAngles.z);
}
// Apply the final position.
if (m_characterController)
{
m_characterController.Move(finalPosition - transform.position);
}
else
{
transform.position = finalPosition;
}
if (transform != null && display_position != null)
{
display_position.text = "X:" + System.Math.Round(transform.position.x, 2) + "\n" + "Y:" + System.Math.Round(transform.position.y, 2) + "\n" + "Z:" + System.Math.Round(transform.position.z, 2) + "\n";
}
transform.rotation = finalRotation;
}
/// <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)
{
// 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;
}
}
/// <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;
}
}
/// <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>
protected override void _OnPoseAvailable(IntPtr callbackContext, 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)
{
// 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>
/// 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;
}
int currentFrame;
// Only interested in specific pose updates
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
// The callback pose is for device with respect to start of service pose.
currentFrame = (int)PoseFrame.DeviceToStart;
}
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
// The callback pose is for device with respect to area description file pose.
currentFrame = (int)PoseFrame.DeviceToADF;
}
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
{
// The callback pose is for start of service with respect to area description file pose.
currentFrame = (int)PoseFrame.StartToADF;
m_poseLocalized = true;
}
else
{
// Not a pose frame we are interested in.
return;
}
Debug.Log("OnPose(" + currentFrame.ToString() + ")");
// Remember the previous position, so you can do delta motion
m_prevTangoPosition[currentFrame] = m_tangoPosition[currentFrame];
m_prevTangoRotation[currentFrame] = m_tangoRotation[currentFrame];
// 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[currentFrame] = uwTuc.GetColumn(3);
m_tangoRotation[currentFrame] = 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[currentFrame])
{
m_poseCount[currentFrame] = 0;
}
m_poseCount[currentFrame]++;
// Other pose data -- Pose delta time.
m_poseDeltaTime[currentFrame] = (float)pose.timestamp - m_poseTimestamp[currentFrame];
m_poseTimestamp[currentFrame] = (float)pose.timestamp;
}
else
{
// if the current pose is not valid assume everything is invalid.
m_tangoPosition[currentFrame] = Vector3.zero;
m_tangoRotation[currentFrame] = Quaternion.identity;
m_poseLocalized = false;
}
m_poseStatus[currentFrame] = pose.status_code;
if (currentFrame != (int)PoseFrame.StartToADF)
{
// Only update position and rotation if the update was for the device's pose.
//
// Calculate final position and rotation deltas and apply them.
transform.position = m_tangoPosition[currentFrame];
transform.rotation = m_tangoRotation[currentFrame];
}
}
/// <summary>
/// Apply any needed changes to the pose.
/// </summary>
private void Update()
{
#if UNITY_ANDROID && !UNITY_EDITOR
if(m_shouldInitTango)
{
m_tangoApplication.InitApplication();
if(m_useADF)
{
// Query the full adf list.
PoseProvider.RefreshADFList();
// loading last recorded ADF
string uuid = PoseProvider.GetLatestADFUUID().GetStringDataUUID();
m_tangoApplication.InitProviders(uuid);
}
else
{
m_tangoApplication.InitProviders(string.Empty);
}
// Query extrinsics constant tranformations.
TangoPoseData poseData = new TangoPoseData();
double timestamp = 0.0;
TangoCoordinateFramePair pair;
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_deviceToIMUMatrix = Matrix4x4.TRS(position, quat, new Vector3 (1.0f, 1.0f, 1.0f));
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_cameraToIMUMatrix = Matrix4x4.TRS(position, quat, new Vector3 (1.0f, 1.0f, 1.0f));
m_alreadyInitialized = true;
m_shouldInitTango = false;
m_tangoApplication.ConnectToService();
}
if (m_isDirty)
{
// This rotation needs to be put into Unity coordinate space.
Quaternion rotationFix = Quaternion.Euler(90.0f, 0.0f, 0.0f);
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;
}
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;
}
m_isDirty = false;
}
if(Input.GetKeyDown(KeyCode.Escape))
{
if(m_tangoApplication != null)
{
m_tangoApplication.Shutdown();
}
// This is a temporary fix for a lifecycle issue where calling
// Application.Quit() here, and restarting the application immediately,
// results in a hard crash.
AndroidHelper.AndroidQuit();
}
#else
Vector3 tempPosition = transform.position;
Quaternion tempRotation = transform.rotation;
PoseProvider.GetMouseEmulation(ref tempPosition, ref tempRotation);
transform.rotation = tempRotation;
transform.position = tempPosition;
#endif
}
/// @endcond
/// <summary>
/// Updates 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;
// Choose the proper pair according to the properties of this controller
if (m_useAreaDescriptionPose)
{
if (m_tangoApplication.m_enableCloudADF)
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_GLOBAL_WGS84;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
else
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
}
else
{
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
}
PoseProvider.GetPoseAtTime(pose, timestamp, pair);
// Update properties from pose
if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
DMatrix4x4 globalTLocal;
bool success = m_tangoApplication.GetGlobalTLocal(out globalTLocal);
if (!success)
{
Debug.LogError("Unable to obtain GlobalTLocal from TangoApplication.");
return;
}
DMatrix4x4 uwTDevice = DMatrix4x4.FromMatrix4x4(TangoSupport.UNITY_WORLD_T_START_SERVICE) *
globalTLocal.Inverse * DMatrix4x4.TR(pose.translation, pose.orientation);
// Calculate matrix for the camera in the Unity world, taking into account offsets.
Matrix4x4 uwTuc = uwTDevice.ToMatrix4x4() * m_dTuc * TangoSupport.m_colorCameraPoseRotation;
// 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>
/// Updates the text view in UI screen with the Pose. Each pose is associated
/// with Target and Base Frame. We need to check for that pair ad update our
/// views accordingly.
/// </summary>
/// <param name="pose"> </param>
//JAVA TO C# CONVERTER WARNING: 'final' parameters are not available in .NET:
// Original line in class: private void updateTextViewWith(final com.google.atap.tangoservice.TangoPoseData pose)
private void updateTextViewWith(TangoPoseData pose)
{
this.RunOnUiThread(() =>
{
try
{
string translationString = "[" + threeDec.format(pose.Translation[0]) + "," + threeDec.format(pose.Translation[1]) + "," + threeDec.format(pose.Translation[2]) + "] ";
string quaternionString = "[" + threeDec.format(pose.Rotation[0]) + "," + threeDec.format(pose.Rotation[1]) + "," + threeDec.format(pose.Rotation[2]) + "," + threeDec.format(pose.Rotation[3]) + "] ";
if ((pose.BaseFrame == TangoPoseData.CoordinateFrameAreaDescription) && (pose.TargetFrame == TangoPoseData.CoordinateFrameDevice))
{
mAdf2DevicePoseCount++;
mAdf2DevicePoseDelta = (pose.Timestamp - mAdf2DevicePreviousPoseTimeStamp) * SECONDS_TO_MILLI;
mAdf2DevicePreviousPoseTimeStamp = pose.Timestamp;
mAdf2DeviceTranslationTextView.Text = translationString;
mAdf2DeviceQuatTextView.Text = quaternionString;
mAdf2DevicePoseStatusTextView.Text = getPoseStatus(pose);
mAdf2DevicePoseCountTextView.Text = Convert.ToString(mAdf2DevicePoseCount);
mAdf2DevicePoseDeltaTextView.Text = threeDec.format(mAdf2DevicePoseDelta);
}
if ((pose.BaseFrame == TangoPoseData.CoordinateFrameStartOfService) && (pose.TargetFrame == TangoPoseData.CoordinateFrameDevice))
{
mStart2DevicePoseCount++;
mStart2DevicePoseDelta = (pose.Timestamp - mStart2DevicePreviousPoseTimeStamp) * SECONDS_TO_MILLI;
mStart2DevicePreviousPoseTimeStamp = pose.Timestamp;
mStart2DeviceTranslationTextView.Text = translationString;
mStart2DeviceQuatTextView.Text = quaternionString;
mStart2DevicePoseStatusTextView.Text = getPoseStatus(pose);
mStart2DevicePoseCountTextView.Text = Convert.ToString(mStart2DevicePoseCount);
mStart2DevicePoseDeltaTextView.Text = threeDec.format(mStart2DevicePoseDelta);
}
if ((pose.BaseFrame == TangoPoseData.CoordinateFrameAreaDescription) && (pose.TargetFrame == TangoPoseData.CoordinateFrameStartOfService))
{
mAdf2StartPoseCount++;
mAdf2StartPoseDelta = (pose.Timestamp - mAdf2StartPreviousPoseTimeStamp) * SECONDS_TO_MILLI;
mAdf2StartPreviousPoseTimeStamp = pose.Timestamp;
mAdf2StartTranslationTextView.Text = translationString;
mAdf2StartQuatTextView.Text = quaternionString;
mAdf2StartPoseStatusTextView.Text = getPoseStatus(pose);
mAdf2StartPoseCountTextView.Text = Convert.ToString(mAdf2StartPoseCount);
mAdf2StartPoseDeltaTextView.Text = threeDec.format(mAdf2StartPoseDelta);
if (pose.StatusCode == TangoPoseData.PoseValid)
{
mIsRelocalized = true;
// Set the color to green
if (mRenderer.Trajectory != null)
{
mRenderer.Trajectory.Color = new float[] { 0.39f, 0.56f, 0.03f, 1.0f }
}
;
}
else
{
mIsRelocalized = false;
// Set the color blue
if (mRenderer.Trajectory != null)
{
mRenderer.Trajectory.Color = new float[] { 0.22f, 0.28f, 0.67f, 1.0f }
}
;
}
}
}
catch (System.Exception e)
{
Toast.MakeText(ApplicationContext, e.Message, Android.Widget.ToastLength.Short).Show();
}
});
}
/// <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)
{
DMatrix4x4 globalTLocal;
bool success = m_tangoApplication.GetGlobalTLocal(out globalTLocal);
if (!success)
{
Debug.LogError("Unable to obtain GlobalTLocal from Tango application.");
return;
}
DMatrix4x4 startOfServiceTDevice = globalTLocal.Inverse * DMatrix4x4.TR(pose.translation, pose.orientation);
m_uwTuc = TangoSupport.UNITY_WORLD_T_START_SERVICE * startOfServiceTDevice.ToMatrix4x4()
* TangoSupport.DEVICE_T_UNITY_CAMERA * TangoSupport.m_devicePoseRotation;
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>
/// 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="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;
}
if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
// The callback pose is for device with respect to start of service pose.
currentIndex = DEVICE_TO_START;
}
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
{
// The callback pose is for device with respect to area description file pose.
currentIndex = DEVICE_TO_ADF;
}
else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
{
// The callback pose is for start of service with respect to area description file pose.
currentIndex = START_TO_ADF;
}
if (!m_isRelocalized)
{
// check to see if we are recently relocalized
if (currentIndex == 2)
{
m_isRelocalized = true;
}
}
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 (!m_isRelocalized)
{
// If not relocalized MotionTracking pose(Device wrt Start of Service) is used.
matrixssTd = Matrix4x4.TRS(m_tangoPosition[0], m_tangoRotation[0], Vector3.one);
}
else
{
// If relocalized Device wrt ADF pose is used.
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));
}
