/// <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);
}
public static extern int TangoSupport_getDepthAtPointNearestNeighbor(
ref TangoPointCloudIntPtr point_cloud,
ref DVector3 poitnCloudTranslation,
ref DVector4 pointCloudOrientation,
ref Vector2 uvCoordinatesInColorCamera,
OrientationManager.Rotation display_rotation,
ref DVector3 colorCameraTranslation,
ref DVector4 colorCameraOrientation,
out Vector3 outputPoint);
public static extern int TangoSupport_fitPlaneModelNearPoint(
ref TangoPointCloudIntPtr pointCloud,
ref DVector3 pointCloudTranslation,
ref DVector4 pointCloundOrientation,
ref Vector2 uvCoordinates,
OrientationManager.Rotation rotation,
ref DVector3 cameraTranslation,
ref DVector4 cameraOrientation,
out DVector3 intersectionPoint,
out DVector4 planeModel);
public static int TangoSupport_getDepthAtPointNearestNeighbor(
ref TangoPointCloudIntPtr point_cloud,
ref DVector3 poitnCloudTranslation,
ref DVector4 pointCloudOrientation,
ref Vector2 uvCoordinatesInColorCamera,
OrientationManager.Rotation display_rotation,
ref DVector3 colorCameraTranslation,
ref DVector4 colorCameraOrientation,
out Vector3 outputPoint)
{
outputPoint = Vector3.zero;
return(Common.ErrorType.TANGO_SUCCESS);
}
public static int TangoSupport_fitPlaneModelNearPoint(
ref TangoPointCloudIntPtr pointCloud,
ref DVector3 pointCloudTranslation,
ref DVector4 pointCloundOrientation,
ref Vector2 uvCoordinates,
OrientationManager.Rotation rotation,
ref DVector3 cameraTranslation,
ref DVector4 cameraOrientation,
out DVector3 intersectionPoint,
out DVector4 planeModel)
{
intersectionPoint = new DVector3();
planeModel = new DVector4();
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>
/// 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;
}
}
/// <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="pointCloud">Point cloud from Tango.</param>
/// <param name="depthPose">Pose matrix the point cloud corresponds too.</param>
private void _UpdateDepth(TangoPointCloudIntPtr pointCloud, Matrix4x4 depthPose)
{
if (m_context == IntPtr.Zero)
{
Debug.Log("Update called before creating a reconstruction context." + Environment.StackTrace);
return;
}
APIPointCloud apiCloud;
apiCloud.numPoints = pointCloud.m_numPoints;
apiCloud.points = pointCloud.m_points;
apiCloud.timestamp = pointCloud.m_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, 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;
Marshal.Copy(pointCloud.m_points, m_mostRecentDepthPoints, 0, pointCloud.m_numPoints * 4);
m_mostRecentDepthPose = apiDepthPose;
m_mostRecentDepthIsValid = true;
}
}
}
/// <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="pointCloud">Tango depth.</param>
public void OnTangoPointCloudMultithreadedAvailable(ref TangoPointCloudIntPtr pointCloud)
{
if (!m_enabled || pointCloud.m_points == IntPtr.Zero)
{
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, pointCloud.m_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, pointCloud.m_timestamp, pair);
}
if (world_T_devicePose.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
{
Debug.LogFormat("Time {0} has bad status code {1}{2}",
pointCloud.m_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(pointCloud, world_T_depthCamera);
}
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();
}
}