/// <summary>
/// Reduces depth points down to below a fixed number of points.
///
/// TODO: Do this sort of thing in C code before before passing to Unity instead.
/// </summary>
/// <param name="pointCloud">Tango depth data to reduce.</param>
/// <param name="maxNumPoints">Max points to reduce down to.</param>
private static void _ReducePointCloudPoints(TangoPointCloudData pointCloud, int maxNumPoints)
{
if (maxNumPoints > 0 && pointCloud.m_numPoints > maxNumPoints)
{
// Here (maxNumPoints - 1) rather than maxPoints is just a quick and
// dirty way to avoid any possibile edge-case accumulated FP error
// in the sketchy code below.
float keepFraction = (maxNumPoints - 1) / (float)pointCloud.m_numPoints;
int keptPoints = 0;
float keepCounter = 0;
for (int i = 0; i < pointCloud.m_numPoints; i++)
{
keepCounter += keepFraction;
if (keepCounter > 1)
{
pointCloud.m_points[keptPoints] = pointCloud.m_points[i];
keepCounter--;
keptPoints++;
}
}
pointCloud.m_numPoints = keptPoints;
}
}
/// <summary>
/// Calculates the depth in the color camera space at a user-specified
/// location using nearest-neighbor interpolation.
/// </summary>
/// <returns>
/// <c>true</c>, if a point is found is found successfully, <c>false</c> otherwise.
/// </returns>
/// <param name="pointCloud">
/// The point cloud. Cannot be null and must have at least three points.
/// </param>
/// <param name="colorCameraTimestamp">
/// Color camera's timestamp when UV is captured.
/// </param>
/// <param name="uvCoordinates">
/// The UV coordinates for the user selection. This is expected to be
/// in Unity viewport space. The bottom-left of the camera is (0,0);
/// the top-right is (1,1).
/// </param>
/// <param name="colorCameraPoint">
/// The point (x, y, z), where (x, y) is the back-projection of the UV
/// coordinates to the color camera space and z is the z coordinate of
/// the point in the point cloud nearest to the user selection after
/// projection onto the image plane. If there is not a point cloud point
/// close to the user selection after projection onto the image plane,
/// then the point will be set to (0.0, 0.0, 0.0) and isValidPoint will
/// be set to false.
/// </param>
public static bool ScreenCoordinateToWorldNearestNeighbor(
TangoPointCloudData pointCloud,
double colorCameraTimestamp,
Vector2 uvCoordinates,
out Vector3 colorCameraPoint)
{
TangoPoseData depth_T_colorCameraPose = new TangoPoseData();
int returnValue = TangoSupportAPI.TangoSupport_calculateRelativePose(
pointCloud.m_timestamp,
TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
colorCameraTimestamp,
TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR,
depth_T_colorCameraPose);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.LogError("TangoSupport_calculateRelativePose error. " + Environment.StackTrace);
colorCameraPoint = Vector3.zero;
return(false);
}
GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud.m_points,
GCHandleType.Pinned);
TangoPointCloudIntPtr tangoPointCloudIntPtr = new TangoPointCloudIntPtr();
tangoPointCloudIntPtr.m_points = pointCloudHandle.AddrOfPinnedObject();
tangoPointCloudIntPtr.m_timestamp = pointCloud.m_timestamp;
tangoPointCloudIntPtr.m_numPoints = pointCloud.m_numPoints;
// Unity viewport space is: the bottom-left of the camera is (0,0);
// the top-right is (1,1).
// Tango (Android) defined UV space is: the top-left of the camera is (0,0);
// the bottom-right is (1,1).
Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x, 1.0f - uvCoordinates.y);
DVector4 pointCloudRotation = DVector4.IdentityQuaternion;
DVector3 pointCloudTranslation = DVector3.Zero;
returnValue = TangoSupportAPI.TangoSupport_getDepthAtPointNearestNeighbor(
ref tangoPointCloudIntPtr,
ref pointCloudTranslation,
ref pointCloudRotation,
ref uvCoordinatesTango,
AndroidHelper.GetDisplayRotation(),
ref depth_T_colorCameraPose.translation,
ref depth_T_colorCameraPose.orientation,
out colorCameraPoint);
pointCloudHandle.Free();
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.LogError("TangoSupport_getDepthAtPointNearestNeighbor error. " + Environment.StackTrace);
colorCameraPoint = Vector3.zero;
return(false);
}
return(true);
}
/// <summary>
/// Register to get Tango depth callbacks.
///
/// NOTE: Tango depth callbacks happen on a different thread than the main
/// Unity thread.
/// </summary>
internal void SetCallback()
{
m_pointCloud = new TangoPointCloudData();
m_pointCloud.m_points = new float[Common.MAX_NUM_POINTS * 4];
m_xyzPoints = new float[Common.MAX_NUM_POINTS * 3];
m_onPointCloudAvailableCallback = new DepthProvider.APIOnPointCloudAvailable(_OnPointCloudAvailable);
Tango.DepthProvider.SetCallback(m_onPointCloudAvailableCallback);
}
/// <summary>
/// It's backwards, but fill an emulated TangoPointCloudIntPtr instance from an emulated TangoPointCloudData
/// instance. It is the responsibility of the caller to GC pin/free the pointCloudData's m_points.
/// </summary>
/// <returns>Emulated TangoPointCloudIntPtr instance.</returns>
/// <param name="pointCloud">Emulated point cloud data.</param>>
/// <param name="pinnedPoints">Pinned array of pointCloudData.m_points.</param>
private static TangoPointCloudIntPtr _GetEmulatedRawData(TangoPointCloudData pointCloud, GCHandle pinnedPoints)
{
TangoPointCloudIntPtr raw;
raw.m_version = 0;
raw.m_timestamp = pointCloud.m_timestamp;
raw.m_numPoints = pointCloud.m_numPoints;
raw.m_points = pinnedPoints.AddrOfPinnedObject();
return(raw);
}
/// <summary>
/// Fill out <c>pointCloudData</c> with emulated values from Tango.
/// </summary>
/// <param name="pointCloudData">The point cloud data to fill out.</param>
private static void _FillEmulatedPointCloud(ref TangoPointCloudData pointCloud)
{
List <Vector3> emulated = DepthProvider.GetTangoEmulation(out pointCloud.m_timestamp);
pointCloud.m_numPoints = emulated.Count;
for (int it = 0; it < emulated.Count; ++it)
{
pointCloud.m_points[(it * 4) + 0] = emulated[it].x;
pointCloud.m_points[(it * 4) + 1] = emulated[it].y;
pointCloud.m_points[(it * 4) + 2] = emulated[it].z;
pointCloud.m_points[(it * 4) + 3] = 1;
}
}
/// <summary>
/// Stop getting Tango depth callbacks, clear all listeners.
/// </summary>
internal static void Reset()
{
// Avoid calling into tango_client_api before the correct library is loaded.
if (m_onPointCloudAvailableCallback != null)
{
Tango.DepthProvider.ClearCallback();
}
m_onPointCloudAvailableCallback = null;
m_isDirty = false;
m_pointCloud = new TangoPointCloudData();
m_pointCloud.m_points = new float[Common.MAX_NUM_POINTS * 4];
m_xyzPoints = new float[Common.MAX_NUM_POINTS * 3];
m_maxNumReducedDepthPoints = 0;
m_onTangoDepthAvailable = null;
m_onTangoDepthMultithreadedAvailable = null;
m_onPointCloudAvailable = null;
m_onPointCloudMultithreadedAvailable = null;
}
/// <summary>
/// Initializes a new instance of the <see cref="Tango.TangoUnityDepth"/> class from a
/// <see cref="PointCloud"/> instance.
/// </summary>
/// <param name="pointCloud">Point cloud.</param>
public TangoUnityDepth(TangoPointCloudData pointCloud)
{
m_points = new float[MAX_POINTS_ARRAY_SIZE];
m_ij = new int[MAX_IJ_ARRAY_SIZE];
m_timestamp = pointCloud.m_timestamp;
m_pointCount = pointCloud.m_numPoints;
for (int it = 0; it < pointCloud.m_points.Length / 4; ++it)
{
m_points[(it * 3) + 0] = pointCloud.m_points[(it * 4) + 0];
m_points[(it * 3) + 1] = pointCloud.m_points[(it * 4) + 1];
m_points[(it * 3) + 2] = pointCloud.m_points[(it * 4) + 2];
}
m_ijRows = m_ijColumns = 0;
for (int it = 0; it < m_ij.Length; ++it)
{
m_ij[it] = -1;
}
}
/// <summary>
/// Initializes a new instance of the <see cref="Tango.TangoUnityDepth"/> class from a
/// <see cref="PointCloud"/> instance.
/// </summary>
/// <param name="pointCloud">Point cloud.</param>
public TangoUnityDepth(TangoPointCloudData pointCloud)
{
m_points = new float[MAX_POINTS_ARRAY_SIZE];
m_ij = new int[MAX_IJ_ARRAY_SIZE];
m_timestamp = pointCloud.m_timestamp;
m_pointCount = pointCloud.m_numPoints;
for (int it = 0; it < pointCloud.m_points.Length / 4; ++it)
{
m_points[(it * 3) + 0] = pointCloud.m_points[(it * 4) + 0];
m_points[(it * 3) + 1] = pointCloud.m_points[(it * 4) + 1];
m_points[(it * 3) + 2] = pointCloud.m_points[(it * 4) + 2];
}
m_ijRows = m_ijColumns = 0;
for (int it = 0; it < m_ij.Length; ++it)
{
m_ij[it] = -1;
}
}
/// <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>
/// It's backwards, but fill an emulated TangoPointCloudIntPtr instance from an emulated TangoPointCloudData
/// instance. It is the responsibility of the caller to GC pin/free the pointCloudData's m_points.
/// </summary>
/// <returns>Emulated TangoPointCloudIntPtr instance.</returns>
/// <param name="pointCloud">Emulated point cloud data.</param>>
/// <param name="pinnedPoints">Pinned array of pointCloudData.m_points.</param>
private static TangoPointCloudIntPtr _GetEmulatedRawData(TangoPointCloudData pointCloud, GCHandle pinnedPoints)
{
TangoPointCloudIntPtr raw;
raw.m_version = 0;
raw.m_timestamp = pointCloud.m_timestamp;
raw.m_numPoints = pointCloud.m_numPoints;
raw.m_points = pinnedPoints.AddrOfPinnedObject();
return raw;
}
/// <summary>
/// Fill out <c>pointCloudData</c> with emulated values from Tango.
/// </summary>
/// <param name="pointCloudData">The point cloud data to fill out.</param>
private static void _FillEmulatedPointCloud(ref TangoPointCloudData pointCloud)
{
List<Vector3> emulated = DepthProvider.GetTangoEmulation(out pointCloud.m_timestamp);
pointCloud.m_numPoints = emulated.Count;
for (int it = 0; it < emulated.Count; ++it)
{
pointCloud.m_points[(it * 4) + 0] = emulated[it].x;
pointCloud.m_points[(it * 4) + 1] = emulated[it].y;
pointCloud.m_points[(it * 4) + 2] = emulated[it].z;
pointCloud.m_points[(it * 4) + 3] = 1;
}
}
/// <summary>
/// Reduces depth points down to below a fixed number of points.
///
/// TODO: Do this sort of thing in C code before before passing to Unity instead.
/// </summary>
/// <param name="pointCloud">Tango depth data to reduce.</param>
/// <param name="maxNumPoints">Max points to reduce down to.</param>
private static void _ReducePointCloudPoints(TangoPointCloudData pointCloud, int maxNumPoints)
{
if (maxNumPoints > 0 && pointCloud.m_numPoints > maxNumPoints)
{
// Here (maxNumPoints - 1) rather than maxPoints is just a quick and
// dirty way to avoid any possibile edge-case accumulated FP error
// in the sketchy code below.
float keepFraction = (maxNumPoints - 1) / (float)pointCloud.m_numPoints;
int keptPoints = 0;
float keepCounter = 0;
for (int i = 0; i < pointCloud.m_numPoints; i++)
{
keepCounter += keepFraction;
if (keepCounter > 1)
{
pointCloud.m_points[keptPoints] = pointCloud.m_points[i];
keepCounter--;
keptPoints++;
}
}
pointCloud.m_numPoints = keptPoints;
}
}
/// <summary>
/// Stop getting Tango depth callbacks, clear all listeners.
/// </summary>
internal static void Reset()
{
// Avoid calling into tango_client_api before the correct library is loaded.
if (m_onPointCloudAvailableCallback != null)
{
Tango.DepthProvider.ClearCallback();
}
m_onPointCloudAvailableCallback = null;
m_isDirty = false;
m_pointCloud = new TangoPointCloudData();
m_pointCloud.m_points = new float[Common.MAX_NUM_POINTS * 4];
m_xyzPoints = new float[Common.MAX_NUM_POINTS * 3];
m_maxNumReducedDepthPoints = 0;
m_onTangoDepthAvailable = null;
m_onTangoDepthMultithreadedAvailable = null;
m_onPointCloudAvailable = null;
m_onPointCloudMultithreadedAvailable = null;
}
