/// <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 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 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);
/// <summary>
/// Transforms a position by this matrix (fast).
///
/// Returns a position transformed by the current transformation matrix. This function is a faster version of
/// MultiplyPoint; but it can only handle regular 3D transformations. MultiplyPoint is slower, but can handle
/// projective transformations as well.
///
/// </summary>
/// <returns>The transformed position.</returns>
/// <param name="v">The position to transform.</param>
public DVector3 MultiplyPoint3x4(DVector3 v)
{
DVector3 transformed = new DVector3(
(this.m00 * v.x) + (this.m01 * v.y) + (this.m02 * v.z) + this.m03,
(this.m10 * v.x) + (this.m11 * v.y) + (this.m12 * v.z) + this.m13,
(this.m20 * v.x) + (this.m21 * v.y) + (this.m22 * v.z) + this.m23);
return(transformed);
}
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);
/// <summary>
/// Initializes a new instance of the <see cref="Tango.TangoPoseData"/> class.
/// </summary>
public TangoPoseData()
{
version = 0;
timestamp = 0.0;
orientation = new DVector4();
translation = new DVector3();
status_code = TangoEnums.TangoPoseStatusType.TANGO_POSE_UNKNOWN;
framePair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
framePair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
confidence = 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);
}
/// <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>
/// Transforms a position by this matrix (generic).
///
/// Returns a position v transformed by the current fully arbitrary matrix. If the matrix is a regular 3D
/// transformation matrix, it is much faster to use MultiplyPoint3x4 instead. MultiplyPoint is slower, but can
/// handle projective transformations as well.
/// </summary>
/// <returns>The transformed point.</returns>
/// <param name="v">The point to be transformed.</param>
public DVector3 MultiplyPoint(DVector3 v)
{
DVector3 transformed = new DVector3(
(this.m00 * v.x) + (this.m01 * v.y) + (this.m02 * v.z) + this.m03,
(this.m10 * v.x) + (this.m11 * v.y) + (this.m12 * v.z) + this.m13,
(this.m20 * v.x) + (this.m21 * v.y) + (this.m22 * v.z) + this.m23);
double proj = (this.m30 * v.x) + (this.m31 * v.y) + (this.m32 * v.z) + this.m33;
transformed.x /= proj;
transformed.y /= proj;
transformed.z /= proj;
return(transformed);
}
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);
}
/// <summary>
/// Create a translation and rotation matrix.
/// </summary>
/// <param name="translation">Translation as 3 doubles in a DVector3 struct.</param>
/// <param name="orientation">Orientation as 4 doubles in a DVector4 struct.</param>
/// <returns>Double matrix.</returns>
public static DMatrix4x4 TR(DVector3 translation, DVector4 orientation)
{
double[] dTranslation = new double[3];
double[] dOrientation = new double[4];
dTranslation[0] = translation.x;
dTranslation[1] = translation.y;
dTranslation[2] = translation.z;
dOrientation[0] = orientation.x;
dOrientation[1] = orientation.y;
dOrientation[2] = orientation.z;
dOrientation[3] = orientation.w;
return(DMatrix4x4.TR(dTranslation, dOrientation));
}
public static int TangoSupport_detectMarkers(ref TangoImageBuffer image,
TangoEnums.TangoCameraId cameraId, ref DVector3 translation, ref DVector4 orientation,
ref APIMarkerParam param, ref APIMarkerList apiMarkerList)
{
apiMarkerList = new APIMarkerList();
return(Common.ErrorType.TANGO_SUCCESS);
}
public static extern int TangoSupport_detectMarkers(ref TangoImageBuffer image,
TangoEnums.TangoCameraId cameraId,
ref DVector3 translation,
ref DVector4 orientation,
ref APIMarkerParam param,
ref APIMarkerList apiMarkerList);
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);
/// <summary>
/// Dot Product of two vectors.
/// </summary>
/// <returns>Dot product as a double.</returns>
/// <param name="a">First vector.</param>
/// <param name="b">Second vector.</param>
public static double Dot(DVector3 a, DVector3 b)
{
return((a.x * b.x) + (a.y * b.y) + (a.z * b.z));
}
/// <summary>
/// Returns the distance between a and b.
/// </summary>
/// <returns>Euclidean distance as a double.</returns>
/// <param name="a">First vector.</param>
/// <param name="b">Second vector.</param>
public static double Distance(DVector3 a, DVector3 b)
{
return((a - b).Magnitude);
}
