/// <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>
/// Fits a plane to a point cloud near a user-specified location. This
/// occurs in two passes. First, all points in cloud within
/// certain pixel distance 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>
/// <c>true</c>, if plane 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="intersectionPoint">
/// The output point in depth camera coordinates that the user selected.
/// </param>
/// <param name="planeModel">
/// The four parameters a, b, c, d for the general plane
/// equation ax + by + cz + d = 0 of the plane fit. The first three
/// components are a unit vector. The output is in the coordinate system of
/// the requested output frame. Cannot be NULL.
/// </param>
public static bool FitPlaneModelNearClick(
TangoPointCloudData pointCloud,
double colorCameraTimestamp,
Vector2 uvCoordinates,
out Vector3 intersectionPoint,
out DVector4 planeModel)
{
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);
intersectionPoint = new Vector3(0.0f, 0.0f, 0.0f);
planeModel = new DVector4();
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);
DVector3 doubleIntersectionPoint = new DVector3();
DVector4 pointCloudRotation = DVector4.IdentityQuaternion;
DVector3 pointCloudTranslation = DVector3.Zero;
returnValue = TangoSupportAPI.TangoSupport_fitPlaneModelNearPoint(
ref tangoPointCloudIntPtr,
ref pointCloudTranslation,
ref pointCloudRotation,
ref uvCoordinatesTango,
AndroidHelper.GetDisplayRotation(),
ref depth_T_colorCameraPose.translation,
ref depth_T_colorCameraPose.orientation,
out doubleIntersectionPoint,
out planeModel);
pointCloudHandle.Free();
if (returnValue != Common.ErrorType.TANGO_SUCCESS)
{
Debug.LogError("TangoSupport_fitPlaneModelNearPoint error. " + Environment.StackTrace);
intersectionPoint = new Vector3(0.0f, 0.0f, 0.0f);
planeModel = new DVector4();
return(false);
}
else
{
intersectionPoint = doubleIntersectionPoint.ToVector3();
}
return(true);
}
