/// <summary>
/// Update the 3D Reconstruction with a new point cloud and pose.
///
/// It is expected this will get called in from the Tango binder thread.
/// </summary>
/// <param name="depth">Point cloud from Tango.</param>
/// <param name="depthPose">Pose matrix the point cloud corresponds too.</param>
private void _UpdateDepth(TangoXYZij depth, Matrix4x4 depthPose)
{
if (m_context == IntPtr.Zero)
{
Debug.Log("Update called before creating a reconstruction context." + Environment.StackTrace);
return;
}
APIPointCloud apiCloud;
apiCloud.numPoints = depth.xyz_count;
apiCloud.points = IntPtr.Zero;
apiCloud.timestamp = depth.timestamp;
// This copy is required until TangoXYZij stores its depth as XYZC.
long xyzPointerVal = depth.xyz.ToInt64();
for (int it = 0; it < depth.xyz_count; ++it)
{
int xyzIndex = it * 3;
int depthPointsIndex = it * 4;
Marshal.Copy(new IntPtr(xyzPointerVal + (xyzIndex * 4)), m_mostRecentDepthPoints, depthPointsIndex, 3);
m_mostRecentDepthPoints[depthPointsIndex + 3] = 1;
}
APIPose apiDepthPose = APIPose.FromMatrix4x4(ref depthPose);
if (!m_sendColorToUpdate)
{
GCHandle mostRecentDepthPointsHandle = GCHandle.Alloc(m_mostRecentDepthPoints, GCHandleType.Pinned);
apiCloud.points = Marshal.UnsafeAddrOfPinnedArrayElement(m_mostRecentDepthPoints, 0);
// No need to wait for a color image, update reconstruction immediately.
IntPtr rawUpdatedIndices;
Status result = (Status)API.Tango3DR_update(
m_context, ref apiCloud, ref apiDepthPose, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero,
out rawUpdatedIndices);
if (result != Status.SUCCESS)
{
Debug.Log("Tango3DR_update returned non-success." + Environment.StackTrace);
return;
}
_AddUpdatedIndices(rawUpdatedIndices);
API.Tango3DR_GridIndexArray_destroy(rawUpdatedIndices);
mostRecentDepthPointsHandle.Free();
}
else
{
lock (m_lockObject)
{
// We need both a color image and a depth cloud to update reconstruction. Cache the depth cloud
// because there are much less depth points than pixels.
m_mostRecentDepth = apiCloud;
m_mostRecentDepthPose = apiDepthPose;
m_mostRecentDepthIsValid = true;
}
}
}
/// <summary>
/// Callback that gets called when depth is available
/// from the Tango Service.
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="xyzij">Xyzij.</param>
protected void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij)
{
// Fill in the data to draw the point cloud.
if (xyzij != null)
{
lock (m_lockObject)
{
// copy single members
m_tangoDepth.m_version = xyzij.version;
m_tangoDepth.m_timestamp = xyzij.timestamp;
m_tangoDepth.m_ijColumns = xyzij.ij_cols;
m_tangoDepth.m_ijRows = xyzij.ij_rows;
m_tangoDepth.m_pointCount = xyzij.xyz_count;
// deep copy arrays
// Fill in the data to draw the point cloud.
if (xyzij != null)
{
int numberOfActivePoints = xyzij.xyz_count * 3;
// copy new points
if (numberOfActivePoints > 0)
{
Marshal.Copy(xyzij.xyz[0], m_tangoDepth.m_points, 0, numberOfActivePoints);
m_isDirty = true;
}
}
}
}
}
/// <summary>
/// Calculates the depth in the color camera space at a user-specified
/// location using bilateral filtering weighted by both spatial distance
/// from the user coordinate and by intensity similarity.
/// </summary>
/// <returns>
/// Common.ErrorType.TANGO_SUCCESS on success,
/// Common.ErrorType.TANGO_INVALID on invalid input, and
/// Common.ErrorType.TANGO_ERROR on failure.
/// </returns>
/// <param name="pointCloud">
/// The point cloud. Cannot be null and must have at least one point.
/// </param>
/// <param name="pointCount">
/// The number of points to read from the point cloud.
/// </param>
/// <param name="timestamp">The timestamp of the depth points.</param>
/// <param name="cameraIntrinsics">
/// The camera intrinsics for the color camera. Cannot be null.
/// </param>
/// <param name="colorImage">
/// The color image buffer. Cannot be null.
/// </param>
/// <param name="matrix">
/// Transformation matrix of the color camera with respect to the Unity
/// World frame.
/// </param>
/// <param name="uvCoordinates">
/// The UV coordinates for the user selection. This is expected to be
/// between (0.0, 0.0) and (1.0, 1.0).
/// </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>
/// <param name="isValidPoint">
/// A flag valued true if there is a point cloud point close to the user
/// selection after projection onto the image plane and valued false
/// otherwise.
/// </param>
public static int ScreenCoordinateToWorldBilateral(
Vector3[] pointCloud, int pointCount, double timestamp,
TangoCameraIntrinsics cameraIntrinsics, TangoImageBuffer colorImage,
ref Matrix4x4 matrix, Vector2 uvCoordinates,
out Vector3 colorCameraPoint, out bool isValidPoint)
{
GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud,
GCHandleType.Pinned);
TangoXYZij pointCloudXyzIj = new TangoXYZij();
pointCloudXyzIj.timestamp = timestamp;
pointCloudXyzIj.xyz_count = pointCount;
pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject();
DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix);
// Unity has Y pointing screen up; Tango camera has Y pointing
// screen down.
Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x,
1.0f - uvCoordinates.y);
int isValidPointInteger;
int returnValue = TangoSupportAPI.TangoSupport_getDepthAtPointBilateralCameraIntrinsicsMatrixTransform(
pointCloudXyzIj, cameraIntrinsics, colorImage, ref doubleMatrix,
ref uvCoordinatesTango, out colorCameraPoint,
out isValidPointInteger);
isValidPoint = isValidPointInteger != 0;
pointCloudHandle.Free();
return(returnValue);
}
/// <summary>
/// Calculates the depth in the color camera space at a user-specified
/// location using nearest-neighbor interpolation.
/// </summary>
/// <returns>
/// Common.ErrorType.TANGO_SUCCESS on success and
/// Common.ErrorType.TANGO_INVALID on invalid input.
/// </returns>
/// <param name="pointCloud">
/// The point cloud. Cannot be null and must have at least one point.
/// </param>
/// <param name="pointCount">
/// The number of points to read from the point cloud.
/// </param>
/// <param name="timestamp">The timestamp of the depth points.</param>
/// <param name="cameraIntrinsics">
/// The camera intrinsics for the color camera. Cannot be null.
/// </param>
/// <param name="matrix">
/// Transformation matrix of the color camera with respect to the Unity
/// World frame.
/// </param>
/// <param name="uvCoordinates">
/// The UV coordinates for the user selection. This is expected to be
/// between (0.0, 0.0) and (1.0, 1.0).
/// </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 0.
/// </param>
/// <param name="isValidPoint">
/// A flag valued 1 if there is a point cloud point close to the user
/// selection after projection onto the image plane and valued 0
/// otherwise.
/// </param>
public static int GetDepthAtPointNearestNeighbor(
Vector3[] pointCloud, int pointCount, double timestamp,
TangoCameraIntrinsics cameraIntrinsics, ref Matrix4x4 matrix,
Vector2 uvCoordinates, out Vector3 colorCameraPoint,
out bool isValidPoint)
{
GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud, GCHandleType.Pinned);
TangoXYZij pointCloudXyzIj = new TangoXYZij();
pointCloudXyzIj.timestamp = timestamp;
pointCloudXyzIj.xyz_count = pointCount;
pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject();
DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix);
// Unity has Y pointing screen up; Tango camera has Y pointing
// screen down.
float[] uvCoordinatesArray = new float[2];
uvCoordinatesArray[0] = uvCoordinates.x;
uvCoordinatesArray[1] = 1.0f - uvCoordinates.y;
int isValidPointInteger;
int returnValue = TangoSupportAPI.TangoSupport_getDepthAtPointNearestNeighborMatrixTransform(
pointCloudXyzIj, cameraIntrinsics, ref doubleMatrix,
uvCoordinatesArray, out colorCameraPoint, out isValidPointInteger);
isValidPoint = isValidPointInteger != 0;
pointCloudHandle.Free();
return(returnValue);
}
public static int TangoSupport_fitPlaneModelNearPointMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics,
ref DMatrix4x4 matrix, ref Vector2 uvCoordinates,
out DVector3 intersectionPoint, double[] planeModel)
{
intersectionPoint = new DVector3();
return(Common.ErrorType.TANGO_SUCCESS);
}
public static int TangoSupport_getDepthAtPointNearestNeighborMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics,
ref DMatrix4x4 matrix, ref Vector2 uvCoordinates,
out Vector3 colorCameraPoint, out int isValidPoint)
{
colorCameraPoint = Vector3.zero;
isValidPoint = 1;
return(Common.ErrorType.TANGO_SUCCESS);
}
/// <summary>
/// It's backwards, but fill emulated raw xyzij data with emulated TangoUnityDepth data.
/// It is the responsibility of the caller to GC pin/free the pointCloudData's m_points.
/// </summary>
/// <returns>Emulated raw xyzij data.</returns>
/// <param name="pointCouldData">Emulated point could data.</param>>
/// <param name="pinnedPoints">Pinned array of pointCouldData.m_points.</param>
private TangoXYZij _GetEmulatedRawXyzijData(TangoUnityDepth pointCouldData, GCHandle pinnedPoints)
{
TangoXYZij data = new TangoXYZij();
data.xyz = pinnedPoints.AddrOfPinnedObject();
data.xyz_count = pointCouldData.m_pointCount;
data.ij_cols = 0;
data.ij_rows = 0;
data.ij = IntPtr.Zero;
data.timestamp = pointCouldData.m_timestamp;
return(data);
}
/// <summary>
/// Fits a plane to a point cloud near a user-specified location. This
/// occurs in two passes. First, all points in cloud within
/// <c>maxPixelDistance</c> to <c>uvCoordinates</c> after projection are kept. Then a
/// plane is fit to the subset cloud using RANSAC. After the initial fit
/// all inliers from the original cloud are used to refine the plane
/// model.
/// </summary>
/// <returns>
/// Common.ErrorType.TANGO_SUCCESS on success,
/// Common.ErrorType.TANGO_INVALID on invalid input, and
/// Common.ErrorType.TANGO_ERROR on failure.
/// </returns>
/// <param name="pointCloud">
/// The point cloud. Cannot be null and must have at least three points.
/// </param>
/// <param name="pointCount">
/// The number of points to read from the point cloud.
/// </param>
/// <param name="timestamp">The timestamp of the point cloud.</param>
/// <param name="cameraIntrinsics">
/// The camera intrinsics for the color camera. Cannot be null.
/// </param>
/// <param name="matrix">
/// Transformation matrix of the color camera with respect to the Unity
/// World frame.
/// </param>
/// <param name="uvCoordinates">
/// The UV coordinates for the user selection. This is expected to be
/// between (0.0, 0.0) and (1.0, 1.0).
/// </param>
/// <param name="intersectionPoint">
/// The output point in depth camera coordinates that the user selected.
/// </param>
/// <param name="plane">The plane fit.</param>
public static int FitPlaneModelNearClick(
Vector3[] pointCloud, int pointCount, double timestamp,
TangoCameraIntrinsics cameraIntrinsics, ref Matrix4x4 matrix,
Vector2 uvCoordinates, out Vector3 intersectionPoint,
out Plane plane)
{
GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud,
GCHandleType.Pinned);
TangoXYZij pointCloudXyzIj = new TangoXYZij();
pointCloudXyzIj.timestamp = timestamp;
pointCloudXyzIj.xyz_count = pointCount;
pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject();
DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix);
// Unity has Y pointing screen up; Tango camera has Y pointing
// screen down.
Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x,
1.0f - uvCoordinates.y);
DVector3 doubleIntersectionPoint = new DVector3();
double[] planeArray = new double[4];
int returnValue = TangoSupportAPI.TangoSupport_fitPlaneModelNearPointMatrixTransform(
pointCloudXyzIj, cameraIntrinsics, ref doubleMatrix,
ref uvCoordinatesTango,
out doubleIntersectionPoint, planeArray);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
intersectionPoint = new Vector3(0.0f, 0.0f, 0.0f);
plane = new Plane(new Vector3(0.0f, 0.0f, 0.0f), 0.0f);
}
else
{
intersectionPoint = doubleIntersectionPoint.ToVector3();
Vector3 normal = new Vector3((float)planeArray[0],
(float)planeArray[1],
(float)planeArray[2]);
float distance = (float)planeArray[3] / normal.magnitude;
plane = new Plane(normal, distance);
}
pointCloudHandle.Free();
return(returnValue);
}
/// <summary>
/// Update the 3D Reconstruction with a new point cloud and pose.
///
/// It is expected this will get called in from the Tango binder thread.
/// </summary>
/// <param name="depth">Point cloud from Tango.</param>
/// <param name="depthPose">Pose matrix the point cloud corresponds too.</param>
private void _UpdateDepth(TangoXYZij depth, Matrix4x4 depthPose)
{
if (m_context == IntPtr.Zero)
{
Debug.Log("Update called before creating a reconstruction context." + Environment.StackTrace);
return;
}
APIPointCloud apiCloud;
apiCloud.numPoints = depth.xyz_count;
apiCloud.points = depth.xyz;
apiCloud.timestamp = depth.timestamp;
APIPose apiDepthPose = APIPose.FromMatrix4x4(ref depthPose);
if (!m_sendColorToUpdate)
{
// No need to wait for a color image, update reconstruction immediately.
IntPtr rawUpdatedIndices;
Status result = (Status)API.Tango3DR_update(
m_context, ref apiCloud, ref apiDepthPose, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero,
out rawUpdatedIndices);
if (result != Status.SUCCESS)
{
Debug.Log("Tango3DR_update returned non-success." + Environment.StackTrace);
return;
}
_AddUpdatedIndices(rawUpdatedIndices);
API.Tango3DR_GridIndexArray_destroy(rawUpdatedIndices);
}
else
{
lock (m_lockObject)
{
// We need both a color image and a depth cloud to update reconstruction. Cache the depth cloud
// because there are much less depth points than pixels.
m_mostRecentDepth = apiCloud;
m_mostRecentDepth.points = IntPtr.Zero;
m_mostRecentDepthPose = apiDepthPose;
Marshal.Copy(apiCloud.points, m_mostRecentDepthPoints, 0, apiCloud.numPoints * 3);
m_mostRecentDepthIsValid = true;
}
}
}
private static void _OnPointCloudAvailable(IntPtr callbackContext, ref TangoPointCloudIntPtr rawPointCloud)
{
// Fill in the data to draw the point cloud.
if (m_onPointCloudMultithreadedAvailable != null)
{
m_onPointCloudMultithreadedAvailable(ref rawPointCloud);
}
lock (m_lockObject)
{
// copy single members
m_pointCloud.m_timestamp = rawPointCloud.m_timestamp;
m_pointCloud.m_numPoints = rawPointCloud.m_numPoints;
if (rawPointCloud.m_numPoints > 0)
{
Marshal.Copy(rawPointCloud.m_points, m_pointCloud.m_points, 0,
rawPointCloud.m_numPoints * 4);
}
m_isDirty = true;
}
// This must be done after the above Marshal.Copy so that it can efficiently reduce the array to just XYZ.
if (m_onTangoDepthMultithreadedAvailable != null)
{
TangoXYZij xyzij = new TangoXYZij();
xyzij.version = rawPointCloud.m_version;
xyzij.timestamp = rawPointCloud.m_timestamp;
xyzij.xyz_count = rawPointCloud.m_numPoints;
xyzij.ij_rows = 0;
xyzij.ij_cols = 0;
xyzij.ij = IntPtr.Zero;
xyzij.color_image = IntPtr.Zero;
for (int it = 0; it < m_pointCloud.m_numPoints; ++it)
{
m_xyzPoints[(it * 3) + 0] = m_pointCloud.m_points[(it * 4) + 0];
m_xyzPoints[(it * 3) + 1] = m_pointCloud.m_points[(it * 4) + 1];
m_xyzPoints[(it * 3) + 2] = m_pointCloud.m_points[(it * 4) + 2];
}
GCHandle pinnedXyzPoints = GCHandle.Alloc(m_xyzPoints, GCHandleType.Pinned);
xyzij.xyz = pinnedXyzPoints.AddrOfPinnedObject();
m_onTangoDepthMultithreadedAvailable(xyzij);
pinnedXyzPoints.Free();
}
}
/// <summary>
/// Callback that gets called when depth is available
/// from the Tango Service.
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="xyzij">Xyzij.</param>
protected void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij)
{
// Fill in the data to draw the point cloud.
if (xyzij != null)
{
// copy single members
m_tangoDepth.m_version = xyzij.version;
m_tangoDepth.m_timestamp = xyzij.timestamp;
m_tangoDepth.m_ijColumns = xyzij.ij_cols;
m_tangoDepth.m_ijRows = xyzij.ij_rows;
m_tangoDepth.m_pointCount = xyzij.xyz_count;
// deep copy arrays
// Fill in the data to draw the point cloud.
if (xyzij != null)
{
int numberOfActiveVertices = xyzij.xyz_count;
if (numberOfActiveVertices > 0)
{
float[] allPositions = new float[numberOfActiveVertices * 3];
Marshal.Copy(xyzij.xyz[0], allPositions, 0, allPositions.Length);
for (int i = 0; i < numberOfActiveVertices; ++i)
{
if (i < m_tangoDepth.m_pointCount)
{
m_tangoDepth.m_vertices[i].x = allPositions[i * 3];
m_tangoDepth.m_vertices[i].y = allPositions[(i * 3) + 1];
m_tangoDepth.m_vertices[i].z = allPositions[(i * 3) + 2];
}
else
{
m_tangoDepth.m_vertices[i].x = m_tangoDepth.m_vertices[i].y = m_tangoDepth.m_vertices[i].z = 0.0f;
}
}
m_isDirty = true;
}
}
}
}
/// <summary>
/// Raise a Tango depth event if there is new data.
/// </summary>
internal void SendDepthIfAvailable()
{
#if UNITY_EDITOR
lock (m_lockObject)
{
if (DepthProvider.m_emulationIsDirty)
{
DepthProvider.m_emulationIsDirty = false;
if (m_onTangoDepthAvailable != null || m_onTangoDepthMultithreadedAvailable != null)
{
_FillEmulatedPointCloudData(m_tangoDepth);
}
if (m_onTangoDepthMultithreadedAvailable != null)
{
// Pretend to be making a call from unmanaged code.
GCHandle pinnedPoints = GCHandle.Alloc(m_tangoDepth.m_points, GCHandleType.Pinned);
TangoXYZij emulatedXyzij = _GetEmulatedRawXyzijData(m_tangoDepth, pinnedPoints);
m_onTangoDepthMultithreadedAvailable(emulatedXyzij);
pinnedPoints.Free();
}
if (m_onTangoDepthAvailable != null)
{
m_isDirty = true;
}
}
}
#endif
if (m_isDirty && m_onTangoDepthAvailable != null)
{
lock (m_lockObject)
{
m_onTangoDepthAvailable(m_tangoDepth);
}
m_isDirty = false;
}
}
/// <summary>
/// This is called each time new depth data is available.
///
/// On the Tango tablet, the depth callback occurs at 5 Hz.
/// </summary>
/// <param name="tangoDepth">Tango depth.</param>
public void OnTangoDepthMultithreadedAvailable(TangoXYZij tangoDepth)
{
if (!m_enabled)
{
return;
}
// Build World T depth camera
TangoPoseData world_T_devicePose = new TangoPoseData();
if (m_useAreaDescriptionPose)
{
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(world_T_devicePose, tangoDepth.timestamp, pair);
}
else
{
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(world_T_devicePose, tangoDepth.timestamp, pair);
}
if (world_T_devicePose.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
Debug.Log(string.Format("Time {0} has bad status code {1}",
tangoDepth.timestamp, world_T_devicePose.status_code)
+ Environment.StackTrace);
return;
}
// The 3D Reconstruction library can not handle a left handed transformation during update. Instead,
// transform into the Unity world space via the external_T_tango config.
Matrix4x4 world_T_depthCamera = world_T_devicePose.ToMatrix4x4() * m_device_T_depthCamera;
_UpdateDepth(tangoDepth, world_T_depthCamera);
}
/// <summary> /// Callback that gets called when depth is available /// from the Tango Service. /// </summary> /// <param name="callbackContext">Callback context.</param> /// <param name="xyzij">Xyzij.</param> protected abstract void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij);
public static int TangoSupport_getDepthAtPointNearestNeighborMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics,
ref DMatrix4x4 matrix, ref Vector2 uvCoordinates,
out Vector3 colorCameraPoint, out int isValidPoint)
{
colorCameraPoint = Vector3.zero;
isValidPoint = 1;
return Common.ErrorType.TANGO_SUCCESS;
}
public static int TangoSupport_fitPlaneModelNearClickMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics intrinsics, ref Matrix4x4 colorCameraTUnityWorld,
float[] uvCoordinates, double[] intersectionPoint, double[] planeModel)
{
return(Common.ErrorType.TANGO_SUCCESS);
}
/// <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>
protected override void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij)
{
// Calculate the time since the last successful depth data
// collection.
if (m_previousDepthDeltaTime == 0.0)
{
m_previousDepthDeltaTime = xyzij.timestamp;
}
else
{
m_numberOfDepthSamples++;
m_timeSinceLastDepthFrame = xyzij.timestamp - m_previousDepthDeltaTime;
m_previousDepthDeltaTime = xyzij.timestamp;
}
// Fill in the data to draw the point cloud.
if (xyzij != null && m_vertices != null)
{
int numberOfActiveVertices = xyzij.xyz_count;
m_pointsCount = numberOfActiveVertices;
if(numberOfActiveVertices > 0)
{
float[] allPositions = new float[numberOfActiveVertices * 3];
Marshal.Copy(xyzij.xyz[0], allPositions, 0, allPositions.Length);
for(int i = 0; i < m_vertices.Length; ++i)
{
if( i < xyzij.xyz_count )
{
m_vertices[i].x = allPositions[i * 3];
m_vertices[i].y = allPositions[(i * 3) + 1];
m_vertices[i].z = allPositions[(i * 3) + 2];
}
else
{
m_vertices[i].x = m_vertices[i].y = m_vertices[i].z = 0.0f;
}
}
m_isDirty = true;
}
}
}
public static extern int TangoSupport_getDepthAtPointNearestNeighborMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics,
ref DMatrix4x4 matrix, ref Vector2 uvCoordinates,
out Vector3 colorCameraPoint,
[Out, MarshalAs(UnmanagedType.I4)] out int isValidPoint);
/// <summary>
/// Calculates the depth in the color camera space at a user-specified
/// location using bilateral filtering weighted by both spatial distance
/// from the user coordinate and by intensity similarity.
/// </summary>
/// <returns>
/// Common.ErrorType.TANGO_SUCCESS on success,
/// Common.ErrorType.TANGO_INVALID on invalid input, and
/// Common.ErrorType.TANGO_ERROR on failure.
/// </returns>
/// <param name="pointCloud">
/// The point cloud. Cannot be null and must have at least one point.
/// </param>
/// <param name="pointCount">
/// The number of points to read from the point cloud.
/// </param>
/// <param name="timestamp">The timestamp of the depth points.</param>
/// <param name="cameraIntrinsics">
/// The camera intrinsics for the color camera. Cannot be null.
/// </param>
/// <param name="colorImage">
/// The color image buffer. Cannot be null.
/// </param>
/// <param name="matrix">
/// Transformation matrix of the color camera with respect to the Unity
/// World frame.
/// </param>
/// <param name="uvCoordinates">
/// The UV coordinates for the user selection. This is expected to be
/// between (0.0, 0.0) and (1.0, 1.0).
/// </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>
/// <param name="isValidPoint">
/// A flag valued true if there is a point cloud point close to the user
/// selection after projection onto the image plane and valued false
/// otherwise.
/// </param>
public static int ScreenCoordinateToWorldBilateral(
Vector3[] pointCloud, int pointCount, double timestamp,
TangoCameraIntrinsics cameraIntrinsics, TangoImageBuffer colorImage,
ref Matrix4x4 matrix, Vector2 uvCoordinates,
out Vector3 colorCameraPoint, out bool isValidPoint)
{
GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud,
GCHandleType.Pinned);
TangoXYZij pointCloudXyzIj = new TangoXYZij();
pointCloudXyzIj.timestamp = timestamp;
pointCloudXyzIj.xyz_count = pointCount;
pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject();
DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix);
// Unity has Y pointing screen up; Tango camera has Y pointing
// screen down.
Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x,
1.0f - uvCoordinates.y);
int isValidPointInteger;
int returnValue = TangoSupportAPI.TangoSupport_getDepthAtPointBilateralCameraIntrinsicsMatrixTransform(
pointCloudXyzIj, cameraIntrinsics, colorImage, ref doubleMatrix,
ref uvCoordinatesTango, out colorCameraPoint,
out isValidPointInteger);
isValidPoint = isValidPointInteger != 0;
pointCloudHandle.Free();
return returnValue;
}
/// <summary>
/// Fits a plane to a point cloud near a user-specified location. This
/// occurs in two passes. First, all points in cloud within
/// <c>maxPixelDistance</c> to <c>uvCoordinates</c> after projection are kept. Then a
/// plane is fit to the subset cloud using RANSAC. After the initial fit
/// all inliers from the original cloud are used to refine the plane
/// model.
/// </summary>
/// <returns>
/// Common.ErrorType.TANGO_SUCCESS on success,
/// Common.ErrorType.TANGO_INVALID on invalid input, and
/// Common.ErrorType.TANGO_ERROR on failure.
/// </returns>
/// <param name="pointCloud">
/// The point cloud. Cannot be null and must have at least three points.
/// </param>
/// <param name="pointCount">
/// The number of points to read from the point cloud.
/// </param>
/// <param name="timestamp">The timestamp of the point cloud.</param>
/// <param name="cameraIntrinsics">
/// The camera intrinsics for the color camera. Cannot be null.
/// </param>
/// <param name="matrix">
/// Transformation matrix of the color camera with respect to the Unity
/// World frame.
/// </param>
/// <param name="uvCoordinates">
/// The UV coordinates for the user selection. This is expected to be
/// between (0.0, 0.0) and (1.0, 1.0).
/// </param>
/// <param name="intersectionPoint">
/// The output point in depth camera coordinates that the user selected.
/// </param>
/// <param name="plane">The plane fit.</param>
public static int FitPlaneModelNearClick(
Vector3[] pointCloud, int pointCount, double timestamp,
TangoCameraIntrinsics cameraIntrinsics, ref Matrix4x4 matrix,
Vector2 uvCoordinates, out Vector3 intersectionPoint,
out Plane plane)
{
GCHandle pointCloudHandle = GCHandle.Alloc(pointCloud,
GCHandleType.Pinned);
TangoXYZij pointCloudXyzIj = new TangoXYZij();
pointCloudXyzIj.timestamp = timestamp;
pointCloudXyzIj.xyz_count = pointCount;
pointCloudXyzIj.xyz = pointCloudHandle.AddrOfPinnedObject();
DMatrix4x4 doubleMatrix = new DMatrix4x4(matrix);
// Unity has Y pointing screen up; Tango camera has Y pointing
// screen down.
Vector2 uvCoordinatesTango = new Vector2(uvCoordinates.x,
1.0f - uvCoordinates.y);
DVector3 doubleIntersectionPoint = new DVector3();
double[] planeArray = new double[4];
int returnValue = TangoSupportAPI.TangoSupport_fitPlaneModelNearPointMatrixTransform(
pointCloudXyzIj, cameraIntrinsics, ref doubleMatrix,
ref uvCoordinatesTango,
out doubleIntersectionPoint, planeArray);
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
intersectionPoint = new Vector3(0.0f, 0.0f, 0.0f);
plane = new Plane(new Vector3(0.0f, 0.0f, 0.0f), 0.0f);
}
else
{
intersectionPoint = doubleIntersectionPoint.ToVector3();
Vector3 normal = new Vector3((float)planeArray[0],
(float)planeArray[1],
(float)planeArray[2]);
float distance = (float)planeArray[3] / normal.magnitude;
plane = new Plane(normal, distance);
}
pointCloudHandle.Free();
return returnValue;
}
/// <summary>
/// It's backwards, but fill an emulated TangoXYZij 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 raw xyzij data.</returns>
/// <param name="depth">Emulated point cloud data.</param>>
/// <param name="pinnedPoints">Pinned array of pointCloudData.m_points.</param>
private static TangoXYZij _GetEmulatedRawXyzijData(TangoUnityDepth depth, GCHandle pinnedPoints)
{
TangoXYZij data = new TangoXYZij();
data.xyz = pinnedPoints.AddrOfPinnedObject();
data.xyz_count = depth.m_pointCount;
data.ij_cols = 0;
data.ij_rows = 0;
data.ij = IntPtr.Zero;
data.timestamp = depth.m_timestamp;
return data;
}
/// <summary>
/// Raise a Tango depth event if there is new data.
/// </summary>
internal static void SendIfAvailable()
{
if (m_onPointCloudAvailableCallback == null)
{
return;
}
#if UNITY_EDITOR
lock (m_lockObject)
{
if (DepthProvider.m_emulationIsDirty)
{
DepthProvider.m_emulationIsDirty = false;
if (m_onTangoDepthAvailable != null || m_onTangoDepthMultithreadedAvailable != null ||
m_onPointCloudAvailable != null | m_onPointCloudMultithreadedAvailable != null)
{
_FillEmulatedPointCloud(ref m_pointCloud);
}
if (m_onTangoDepthMultithreadedAvailable != null)
{
// Pretend to be making a call from unmanaged code.
TangoUnityDepth depth = new TangoUnityDepth(m_pointCloud);
GCHandle pinnedPoints = GCHandle.Alloc(depth.m_points, GCHandleType.Pinned);
TangoXYZij emulatedXyzij = _GetEmulatedRawXyzijData(depth, pinnedPoints);
m_onTangoDepthMultithreadedAvailable(emulatedXyzij);
pinnedPoints.Free();
}
if (m_onPointCloudMultithreadedAvailable != null)
{
// Pretend to be making a call from unmanaged code.
GCHandle pinnedPoints = GCHandle.Alloc(m_pointCloud.m_points, GCHandleType.Pinned);
TangoPointCloudIntPtr rawData = _GetEmulatedRawData(m_pointCloud, pinnedPoints);
m_onPointCloudMultithreadedAvailable(ref rawData);
pinnedPoints.Free();
}
if (m_onTangoDepthAvailable != null || m_onPointCloudAvailable != null)
{
m_isDirty = true;
}
}
}
#endif
if (m_isDirty && (m_onTangoDepthAvailable != null || m_onPointCloudAvailable != null))
{
lock (m_lockObject)
{
_ReducePointCloudPoints(m_pointCloud, m_maxNumReducedDepthPoints);
if (m_onTangoDepthAvailable != null)
{
m_onTangoDepthAvailable(new TangoUnityDepth(m_pointCloud));
}
if (m_onPointCloudAvailable != null)
{
m_onPointCloudAvailable(m_pointCloud);
}
}
m_isDirty = false;
}
}
public static extern int TangoSupport_fitPlaneModelNearClickMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics intrinsics, ref Matrix4x4 matrix,
[In, MarshalAs(UnmanagedType.LPArray, SizeConst = 2)] float[] uvCoordinates,
[Out, MarshalAs(UnmanagedType.LPArray, SizeConst = 3)] double[] intersectionPoint,
[Out, MarshalAs(UnmanagedType.LPArray, SizeConst = 4)] double[] planeModel);
/// <summary>
/// DEPRECATED: Callback that gets called when depth is available
/// from the Tango Service.
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="xyzij">Xyzij.</param>
protected void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij)
{
// Fill in the data to draw the point cloud.
if (xyzij != null)
{
lock (m_lockObject)
{
// copy single members
m_tangoDepth.m_version = xyzij.version;
m_tangoDepth.m_timestamp = xyzij.timestamp;
m_tangoDepth.m_ijColumns = xyzij.ij_cols;
m_tangoDepth.m_ijRows = xyzij.ij_rows;
m_tangoDepth.m_pointCount = xyzij.xyz_count;
// deep copy arrays
// Fill in the data to draw the point cloud.
if (xyzij != null)
{
int numberOfActivePoints = xyzij.xyz_count * 3;
// copy new points
if (numberOfActivePoints > 0)
{
Marshal.Copy(xyzij.xyz[0], m_tangoDepth.m_points, 0, numberOfActivePoints);
m_isDirty = true;
}
}
}
}
}
/// <summary>
/// Update the 3D Reconstruction with a new point cloud and pose.
///
/// It is expected this will get called in from the Tango binder thread.
/// </summary>
/// <param name="depth">Point cloud from Tango.</param>
/// <param name="depthPose">Pose matrix the point cloud corresponds too.</param>
private void _UpdateDepth(TangoXYZij depth, Matrix4x4 depthPose)
{
if (m_context == IntPtr.Zero)
{
Debug.Log("Update called before creating a reconstruction context." + Environment.StackTrace);
return;
}
APIPointCloud apiCloud;
apiCloud.numPoints = depth.xyz_count;
apiCloud.points = depth.xyz;
apiCloud.timestamp = depth.timestamp;
APIPose apiDepthPose = APIPose.FromMatrix4x4(ref depthPose);
if (!m_sendColorToUpdate)
{
// No need to wait for a color image, update reconstruction immediately.
IntPtr rawUpdatedIndices;
Status result = (Status)API.Tango3DR_update(
m_context, ref apiCloud, ref apiDepthPose, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero,
out rawUpdatedIndices);
if (result != Status.SUCCESS)
{
Debug.Log("Tango3DR_update returned non-success." + Environment.StackTrace);
return;
}
_AddUpdatedIndices(rawUpdatedIndices);
API.Tango3DR_GridIndexArray_destroy(rawUpdatedIndices);
}
else
{
lock (m_lockObject)
{
// We need both a color image and a depth cloud to update reconstruction. Cache the depth cloud
// because there are much less depth points than pixels.
m_mostRecentDepth = apiCloud;
m_mostRecentDepth.points = IntPtr.Zero;
m_mostRecentDepthPose = apiDepthPose;
Marshal.Copy(apiCloud.points, m_mostRecentDepthPoints, 0, apiCloud.numPoints * 3);
m_mostRecentDepthIsValid = true;
}
}
}
public static extern int TangoSupport_fitPlaneModelNearPointMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics,
ref DMatrix4x4 matrix, ref Vector2 uvCoordinates,
out DVector3 intersectionPoint,
[Out, MarshalAs(UnmanagedType.LPArray, SizeConst = 4)] double[] planeModel);
public static extern int TangoSupport_fitPlaneModelNearPointMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics,
ref DMatrix4x4 matrix, ref Vector2 uvCoordinates,
out DVector3 intersectionPoint,
[Out, MarshalAs(UnmanagedType.LPArray, SizeConst = 4)] double[] planeModel);
public static extern int TangoSupport_getDepthAtPointNearestNeighborMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics,
ref DMatrix4x4 matrix, ref Vector2 uvCoordinates,
out Vector3 colorCameraPoint,
[Out, MarshalAs(UnmanagedType.I4)] out int isValidPoint);
/// <summary>
/// This is called each time new depth data is available.
///
/// On the Tango tablet, the depth callback occurs at 5 Hz.
/// </summary>
/// <param name="tangoDepth">Tango depth.</param>
public void OnTangoDepthMultithreadedAvailable(TangoXYZij tangoDepth)
{
if (!m_enabled)
{
return;
}
// Build World T depth camera
TangoPoseData world_T_devicePose = new TangoPoseData();
if (m_useAreaDescriptionPose)
{
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(world_T_devicePose, tangoDepth.timestamp, pair);
}
else
{
TangoCoordinateFramePair pair;
pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
PoseProvider.GetPoseAtTime(world_T_devicePose, tangoDepth.timestamp, pair);
}
if (world_T_devicePose.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
Debug.Log(string.Format("Time {0} has bad status code {1}",
tangoDepth.timestamp, world_T_devicePose.status_code)
+ Environment.StackTrace);
return;
}
// NOTE: The 3D Reconstruction library does not handle left handed matrices correctly. For now, transform
// into the Unity world space after extraction.
Matrix4x4 world_T_depthCamera = world_T_devicePose.ToMatrix4x4() * m_device_T_depthCamera;
_UpdateDepth(tangoDepth, world_T_depthCamera);
}
public static int TangoSupport_fitPlaneModelNearPointMatrixTransform(
TangoXYZij pointCloud, TangoCameraIntrinsics cameraIntrinsics,
ref DMatrix4x4 matrix, ref Vector2 uvCoordinates,
out DVector3 intersectionPoint, double[] planeModel)
{
intersectionPoint = new DVector3();
return Common.ErrorType.TANGO_SUCCESS;
}
private static void _OnPointCloudAvailable(IntPtr callbackContext, ref TangoPointCloudIntPtr rawPointCloud)
{
// Fill in the data to draw the point cloud.
if (m_onPointCloudMultithreadedAvailable != null)
{
m_onPointCloudMultithreadedAvailable(ref rawPointCloud);
}
lock (m_lockObject)
{
// copy single members
m_pointCloud.m_timestamp = rawPointCloud.m_timestamp;
m_pointCloud.m_numPoints = rawPointCloud.m_numPoints;
if (rawPointCloud.m_numPoints > 0)
{
Marshal.Copy(rawPointCloud.m_points, m_pointCloud.m_points, 0,
rawPointCloud.m_numPoints * 4);
}
m_isDirty = true;
}
// This must be done after the above Marshal.Copy so that it can efficiently reduce the array to just XYZ.
if (m_onTangoDepthMultithreadedAvailable != null)
{
TangoXYZij xyzij = new TangoXYZij();
xyzij.version = rawPointCloud.m_version;
xyzij.timestamp = rawPointCloud.m_timestamp;
xyzij.xyz_count = rawPointCloud.m_numPoints;
xyzij.ij_rows = 0;
xyzij.ij_cols = 0;
xyzij.ij = IntPtr.Zero;
xyzij.color_image = IntPtr.Zero;
for (int it = 0; it < m_pointCloud.m_numPoints; ++it)
{
m_xyzPoints[(it * 3) + 0] = m_pointCloud.m_points[(it * 4) + 0];
m_xyzPoints[(it * 3) + 1] = m_pointCloud.m_points[(it * 4) + 1];
m_xyzPoints[(it * 3) + 2] = m_pointCloud.m_points[(it * 4) + 2];
}
GCHandle pinnedXyzPoints = GCHandle.Alloc(m_xyzPoints, GCHandleType.Pinned);
xyzij.xyz = pinnedXyzPoints.AddrOfPinnedObject();
m_onTangoDepthMultithreadedAvailable(xyzij);
pinnedXyzPoints.Free();
}
}
/// <summary>
/// Callback that gets called when depth is available
/// from the Tango Service.
/// </summary>
/// <param name="callbackContext">Callback context.</param>
/// <param name="xyzij">Xyzij.</param>
protected void _OnDepthAvailable(IntPtr callbackContext, TangoXYZij xyzij)
{
// Fill in the data to draw the point cloud.
if (xyzij != null)
{
// copy single members
m_tangoDepth.m_version = xyzij.version;
m_tangoDepth.m_timestamp = xyzij.timestamp;
m_tangoDepth.m_ijColumns = xyzij.ij_cols;
m_tangoDepth.m_ijRows = xyzij.ij_rows;
m_tangoDepth.m_pointCount = xyzij.xyz_count;
// deep copy arrays
// Fill in the data to draw the point cloud.
if (xyzij != null)
{
int numberOfActiveVertices = xyzij.xyz_count;
if(numberOfActiveVertices > 0)
{
float[] allPositions = new float[numberOfActiveVertices * 3];
Marshal.Copy(xyzij.xyz[0], allPositions, 0, allPositions.Length);
for(int i = 0; i < numberOfActiveVertices; ++i)
{
if( i < m_tangoDepth.m_pointCount )
{
m_tangoDepth.m_vertices[i].x = allPositions[i * 3];
m_tangoDepth.m_vertices[i].y = allPositions[(i * 3) + 1];
m_tangoDepth.m_vertices[i].z = allPositions[(i * 3) + 2];
}
else
{
m_tangoDepth.m_vertices[i].x = m_tangoDepth.m_vertices[i].y = m_tangoDepth.m_vertices[i].z = 0.0f;
}
}
m_isDirty = true;
}
}
}
}
