/// <summary> /// Estimates the depth of a point on a screen, based on nearest neighbors. /// </summary> /// <returns> /// <c>true</c> if a successful depth estimate was obtained. /// </returns> /// <param name="cam">The Unity camera.</param> /// <param name="pos">The point in pixel coordinates to perform depth estimation.</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 bool EstimateDepthOnScreen(Camera cam, Vector2 pos, out Vector3 colorCameraPoint) { // Set up parameters Matrix4x4 colorCameraTUnityWorld = m_colorCameraTUnityCamera * cam.transform.worldToLocalMatrix; Vector2 normalizedPos = cam.ScreenToViewportPoint(pos); // If the camera has a TangoARScreen attached, it is not displaying the entire color camera image. Correct // the normalized coordinates by taking the clipping into account. TangoARScreen arScreen = cam.gameObject.GetComponent <TangoARScreen>(); if (arScreen != null) { normalizedPos = arScreen.ViewportPointToCameraImagePoint(normalizedPos); } bool isValidPoint; int returnType = TangoSupport.ScreenCoordinateToWorldNearestNeighbor( m_points, m_pointsCount, m_depthTimestamp, m_colorCameraIntrinsics, ref colorCameraTUnityWorld, normalizedPos, out colorCameraPoint, out isValidPoint); if (returnType != Common.ErrorType.TANGO_SUCCESS) { Debug.LogErrorFormat("TangoSupport.ScreenCoordinateToWorldNearestNeighbor failed with error code {0}.", returnType); } return((returnType == Common.ErrorType.TANGO_SUCCESS) && isValidPoint); }
/// <summary> /// Fix this! Doesn't output correct values. Input matrix probably not correct. /// Generates the depth map using nearest neighbor upsampling. /// </summary> private void GenerateDepthMap_NearestNeighbor(ref TangoUnityDepth tangoUnityDepth) { TangoPoseData poseData = new TangoPoseData(); TangoCoordinateFramePair pair; pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE; pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE; PoseProvider.GetPoseAtTime(poseData, tangoUnityDepth.m_timestamp, pair); if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID) { return; } Vector3 position; Quaternion rotation; TangoSupport.TangoPoseToWorldTransform(poseData, out position, out rotation); Matrix4x4 ccWorld = Matrix4x4.TRS(position, rotation, Vector3.one); bool isValid = false; Vector3 colorCameraPoint = new Vector3(); for (int i = 0; i < _depthMapWidth; i++) { for (int j = 0; j < _depthMapHeight; j++) { if (TangoSupport.ScreenCoordinateToWorldNearestNeighbor( _PointCloud.m_points, _PointCloud.m_pointsCount, tangoUnityDepth.m_timestamp, _ccIntrinsics, ref ccWorld, new Vector2(i / (float)_depthMapWidth, j / (float)_depthMapHeight), out colorCameraPoint, out isValid) == Common.ErrorType.TANGO_INVALID) { _depthTexture.SetPixel(i, j, Color.red); continue; } if (isValid) { float c = 1 - colorCameraPoint.z / 4.5f; _depthTexture.SetPixel(i, j, new Color(c, c, c)); } else { _depthTexture.SetPixel(i, j, Color.black); } } } _depthTexture.Apply(); _DepthMapQuad.sharedMaterial.mainTexture = _depthTexture; //_debugMessage = "DepthAvailable: " + _waitingForDepth.ToString() + "\n" + // " points: " + _PointCloud.m_pointsCount + "\n" + // " timestamp: " + tangoUnityDepth.m_timestamp.ToString("0.00") + "\n" + // " XYZ:" + colorCameraPoint.ToString(); }
/// <summary> /// Estimates the depth of a point on a screen, based on nearest neighbors. /// </summary> /// <returns> /// <c>true</c> if a successful depth estimate was obtained. /// </returns> /// <param name="cam">The Unity camera.</param> /// <param name="pos">The point in pixel coordinates to perform depth estimation.</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 bool EstimateDepthOnScreen(Camera cam, Vector2 pos, out Vector3 colorCameraPoint) { // Set up parameters Matrix4x4 colorCameraTUnityWorld = TangoSupport.COLOR_CAMERA_T_UNITY_CAMERA * cam.transform.worldToLocalMatrix; Vector2 normalizedPos = cam.ScreenToViewportPoint(pos); // If the camera has a TangoARScreen attached, it is not displaying the entire color camera image. Correct // the normalized coordinates by taking the clipping into account. TangoARScreen arScreen = cam.gameObject.GetComponent <TangoARScreen>(); if (arScreen != null) { normalizedPos = arScreen.ViewportPointToCameraImagePoint(normalizedPos); } bool returnValue = TangoSupport.ScreenCoordinateToWorldNearestNeighbor( m_mostRecentPointCloud, arScreen.m_screenUpdateTime, normalizedPos, out colorCameraPoint); return(returnValue); }