/// <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);
}
