Ejemplo n.º 1
0
        /// <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);
        }
Ejemplo n.º 2
0
 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);
Ejemplo n.º 3
0
 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);
Ejemplo n.º 4
0
 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);
 }
Ejemplo n.º 5
0
 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);
 }
Ejemplo n.º 6
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();
            }
        }
Ejemplo n.º 7
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;
            }
        }
        /// <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);
        }
Ejemplo 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();
            }
        }