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 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);
 }
Ejemplo n.º 3
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.º 4
0
        /// <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);
        }
Ejemplo n.º 5
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.º 6
0
 /// <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;
 }
Ejemplo n.º 7
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.º 8
0
        /// <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);
        }
Ejemplo n.º 9
0
        /// <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);
        }
Ejemplo n.º 10
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.º 11
0
        /// <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));
        }
Ejemplo n.º 12
0
 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);
 }
Ejemplo n.º 13
0
 public static extern int TangoSupport_detectMarkers(ref TangoImageBuffer image,
                                                     TangoEnums.TangoCameraId cameraId,
                                                     ref DVector3 translation,
                                                     ref DVector4 orientation,
                                                     ref APIMarkerParam param,
                                                     ref APIMarkerList apiMarkerList);
Ejemplo n.º 14
0
 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);
Ejemplo n.º 15
0
 /// <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));
 }
Ejemplo n.º 16
0
 /// <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);
 }