Exemplo n.º 1
0
        /// <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);
        }
Exemplo n.º 4
0
        /// <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;
                }
            }
        }
Exemplo n.º 10
0
        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();
            }
        }
Exemplo n.º 11
0
        /// <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;
                    }
                }
            }
        }
Exemplo n.º 12
0
        /// <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;
            }
        }
Exemplo n.º 13
0
        /// <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);
        }
Exemplo n.º 14
0
 /// <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);
Exemplo n.º 15
0
 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;
 }
Exemplo n.º 16
0
 public static int TangoSupport_fitPlaneModelNearClickMatrixTransform(
     TangoXYZij pointCloud, TangoCameraIntrinsics intrinsics, ref Matrix4x4 colorCameraTUnityWorld,
     float[] uvCoordinates, double[] intersectionPoint, double[] planeModel)
 {
     return(Common.ErrorType.TANGO_SUCCESS);
 }
Exemplo n.º 17
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>
    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;
            }
        }
    }
Exemplo n.º 18
0
 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);
Exemplo n.º 19
0
        /// <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;
        }
Exemplo n.º 20
0
        /// <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;
        }
Exemplo n.º 21
0
 /// <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;
 }
Exemplo n.º 22
0
        /// <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;
            }
        }
Exemplo n.º 23
0
 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);
Exemplo n.º 24
0
        /// <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;
                }
            }
        }
Exemplo n.º 26
0
 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);
Exemplo n.º 27
0
 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);
Exemplo n.º 28
0
 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);
        }
Exemplo n.º 30
0
 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;
 }
Exemplo n.º 31
0
        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;
                    }
                }
            }
        }