Exemple #1
0
    /// <summary>
    /// Callback that gets called when depth is available from the Tango Service.
    /// </summary>
    /// <param name="tangoDepth">Depth information from Tango.</param>
    public void OnTangoDepthAvailable(TangoUnityDepth tangoDepth)
    {
        // Calculate the time since the last successful depth data
        // collection.
        if (m_previousDepthDeltaTime == 0.0)
        {
            m_previousDepthDeltaTime = tangoDepth.m_timestamp;
        }
        else
        {
            m_depthDeltaTime         = (float)((tangoDepth.m_timestamp - m_previousDepthDeltaTime) * 1000.0);
            m_previousDepthDeltaTime = tangoDepth.m_timestamp;
        }

        // Fill in the data to draw the point cloud.
        if (tangoDepth != null && tangoDepth.m_points != null)
        {
            m_pointsCount = tangoDepth.m_pointCount;
            if (m_pointsCount > 0)
            {
                _SetUpCameraData();
                TangoCoordinateFramePair pair;
                TangoPoseData            poseData = new TangoPoseData();

                // Query pose to transform point cloud to world coordinates, here we are using the timestamp
                // that we get from depth.
                pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
                PoseProvider.GetPoseAtTime(poseData, m_previousDepthDeltaTime, pair);
                if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
                {
                    return;
                }

                Vector3 position = new Vector3((float)poseData.translation[0],
                                               (float)poseData.translation[1],
                                               (float)poseData.translation[2]);
                Quaternion quat = new Quaternion((float)poseData.orientation[0],
                                                 (float)poseData.orientation[1],
                                                 (float)poseData.orientation[2],
                                                 (float)poseData.orientation[3]);
                m_startServiceTDevice = Matrix4x4.TRS(position, quat, Vector3.one);

                // The transformation matrix that represents the pointcloud's pose.
                // Explanation:
                // The pointcloud which is in Depth camera's frame, is put in unity world's
                // coordinate system(wrt unity world).
                // Then we are extracting the position and rotation from uwTuc matrix and applying it to
                // the PointCloud's transform.
                Matrix4x4 unityWorldTDepthCamera = m_unityWorldTStartService * m_startServiceTDevice * Matrix4x4.Inverse(m_imuTDevice) * m_imuTDepthCamera;
                transform.position = Vector3.zero;
                transform.rotation = Quaternion.identity;

                // Addoffset to the pointcloud depending on the offset from TangoDeltaPoseController
                Matrix4x4 unityWorldOffsetTDepthCamera;
                if (m_tangoDeltaPoseController != null)
                {
                    unityWorldOffsetTDepthCamera = m_tangoDeltaPoseController.UnityWorldOffset * unityWorldTDepthCamera;
                }
                else
                {
                    unityWorldOffsetTDepthCamera = unityWorldTDepthCamera;
                }

                // Converting points array to world space.
                m_overallZ = 0;
                for (int i = 0; i < m_pointsCount; ++i)
                {
                    float x = tangoDepth.m_points[(i * 3) + 0];
                    float y = tangoDepth.m_points[(i * 3) + 1];
                    float z = tangoDepth.m_points[(i * 3) + 2];

                    m_points[i] = unityWorldOffsetTDepthCamera.MultiplyPoint(new Vector3(x, y, z));
                    m_overallZ += z;
                }
                m_overallZ        = m_overallZ / m_pointsCount;
                m_pointsTimestamp = tangoDepth.m_timestamp;

                if (m_updatePointsMesh)
                {
                    // Need to update indicies too!
                    int[] indices = new int[m_pointsCount];
                    for (int i = 0; i < m_pointsCount; ++i)
                    {
                        indices[i] = i;
                    }

                    m_mesh.Clear();
                    m_mesh.vertices = m_points;
                    m_mesh.SetIndices(indices, MeshTopology.Points, 0);
                }

                // The color should be pose relative, we need to store enough info to go back to pose values.
                m_renderer.material.SetMatrix("depthCameraTUnityWorld", unityWorldOffsetTDepthCamera.inverse);
            }
            else
            {
                m_overallZ = 0;
            }
        }
    }
Exemple #2
0
    /// <summary>
    /// Callback that gets called when depth is available from the Tango Service.
    /// </summary>
    /// <param name="pointCloud">Depth information from Tango.</param>
    public void OnTangoPointCloudAvailable(TangoPointCloudData pointCloud)
    {
        // Calculate the time since the last successful depth data
        // collection.
        if (m_depthTimestamp != 0.0)
        {
            m_depthDeltaTime = (float)((pointCloud.m_timestamp - m_depthTimestamp) * 1000.0);
        }

        // Fill in the data to draw the point cloud.
        m_pointsCount = pointCloud.m_numPoints;
        if (m_pointsCount > 0)
        {
            _SetUpCameraData();
            TangoCoordinateFramePair pair;
            TangoPoseData            poseData = new TangoPoseData();

            // Query pose to transform point cloud to world coordinates, here we are using the timestamp
            // that we get from depth.
            if (m_useAreaDescriptionPose)
            {
                pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
            }
            else
            {
                pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
            }

            PoseProvider.GetPoseAtTime(poseData, pointCloud.m_timestamp, pair);
            if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
            {
                return;
            }

            Matrix4x4 startServiceTDevice = poseData.ToMatrix4x4();

            // The transformation matrix that represents the pointcloud's pose.
            // Explanation:
            // The pointcloud which is in Depth camera's frame, is put in unity world's
            // coordinate system(wrt unity world).
            // Then we are extracting the position and rotation from uwTuc matrix and applying it to
            // the PointCloud's transform.
            Matrix4x4 unityWorldTDepthCamera = m_unityWorldTStartService * startServiceTDevice * Matrix4x4.Inverse(m_imuTDevice) * m_imuTDepthCamera;
            transform.position = Vector3.zero;
            transform.rotation = Quaternion.identity;

            // Add offset to the pointcloud depending on the offset from TangoDeltaPoseController
            Matrix4x4 unityWorldOffsetTDepthCamera;
            if (m_tangoDeltaPoseController != null)
            {
                unityWorldOffsetTDepthCamera = m_tangoDeltaPoseController.UnityWorldOffset * unityWorldTDepthCamera;
            }
            else
            {
                unityWorldOffsetTDepthCamera = unityWorldTDepthCamera;
            }

            // Converting points array to world space.
            m_overallZ = 0;
            for (int i = 0; i < m_pointsCount; ++i)
            {
                Vector3 point = pointCloud[i];
                m_points[i] = unityWorldOffsetTDepthCamera.MultiplyPoint3x4(point);
                m_overallZ += point.z;
            }

            m_overallZ       = m_overallZ / m_pointsCount;
            m_depthTimestamp = pointCloud.m_timestamp;

            if (m_updatePointsMesh)
            {
                // Need to update indicies too!
                int[] indices = new int[m_pointsCount];
                for (int i = 0; i < m_pointsCount; ++i)
                {
                    indices[i] = i;
                }

                m_mesh.Clear();
                m_mesh.vertices = m_points;
                m_mesh.SetIndices(indices, MeshTopology.Points, 0);
            }

            // The color should be pose relative, we need to store enough info to go back to pose values.
            m_renderer.material.SetMatrix("depthCameraTUnityWorld", unityWorldOffsetTDepthCamera.inverse);

            // Try to find the floor using this set of depth points if requested.
            if (m_findFloorWithDepth)
            {
                _FindFloorWithDepth();
            }
        }
        else
        {
            m_overallZ = 0;
        }
    }
Exemple #3
0
    /// <summary>
    /// Update is called once per frame.
    /// </summary>
    private void Update()
    {
        if (m_isDirty)
        {
            double timestamp = 0.0;
            TangoCoordinateFramePair pair;
            TangoPoseData            poseData = new TangoPoseData();

            // Query the extrinsics between IMU and device frame.
            pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
            pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
            PoseProvider.GetPoseAtTime(poseData, timestamp, pair);
            Vector3 position = new Vector3((float)poseData.translation[0],
                                           (float)poseData.translation[1],
                                           (float)poseData.translation[2]);
            Quaternion quat = new Quaternion((float)poseData.orientation[0],
                                             (float)poseData.orientation[1],
                                             (float)poseData.orientation[2],
                                             (float)poseData.orientation[3]);
            m_imuTd = Matrix4x4.TRS(position, quat, new Vector3(1.0f, 1.0f, 1.0f));

            // Query the extrinsics between IMU and color camera frame.
            pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
            pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR;
            PoseProvider.GetPoseAtTime(poseData, timestamp, pair);
            position = new Vector3((float)poseData.translation[0],
                                   (float)poseData.translation[1],
                                   (float)poseData.translation[2]);
            quat = new Quaternion((float)poseData.orientation[0],
                                  (float)poseData.orientation[1],
                                  (float)poseData.orientation[2],
                                  (float)poseData.orientation[3]);
            m_imuTc = Matrix4x4.TRS(position, quat, new Vector3(1.0f, 1.0f, 1.0f));

            // Query pose to transform point cloud to world coordinates, here we are using the timestamp
            // that we get from depth.
            pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
            pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
            PoseProvider.GetPoseAtTime(poseData, m_previousDepthDeltaTime, pair);
            position = new Vector3((float)poseData.translation[0],
                                   (float)poseData.translation[1],
                                   (float)poseData.translation[2]);
            quat = new Quaternion((float)poseData.orientation[0],
                                  (float)poseData.orientation[1],
                                  (float)poseData.orientation[2],
                                  (float)poseData.orientation[3]);
            m_ssTd = Matrix4x4.TRS(position, quat, Vector3.one);

            // The transformation matrix that represents the pointcloud's pose.
            Matrix4x4 uwTuc = m_uwTss * m_ssTd * Matrix4x4.Inverse(m_imuTd) * m_imuTc * m_cTuc;

            transform.position = uwTuc.GetColumn(3);
            transform.rotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));

            m_mesh.Clear();
            m_mesh.vertices  = m_vertices;
            m_mesh.triangles = m_triangles;
            m_mesh.SetIndices(m_triangles, MeshTopology.Points, 0);

            m_isDirty = false;
        }
    }
Exemple #4
0
    /// <summary>
    /// Callback that gets called when depth is available from the Tango Service.
    /// </summary>
    /// <param name="tangoDepth">Depth information from Tango.</param>
    public void OnTangoDepthAvailable(TangoUnityDepth tangoDepth)
    {
        // Calculate the time since the last successful depth data
        // collection.

        if (m_previousDepthDeltaTime == 0.0)
        {
            m_previousDepthDeltaTime = tangoDepth.m_timestamp;
        }
        else
        {
            m_depthDeltaTime         = (float)((tangoDepth.m_timestamp - m_previousDepthDeltaTime) * 1000.0);
            m_previousDepthDeltaTime = tangoDepth.m_timestamp;
        }

        // Fill in the data to draw the point cloud.
        if (tangoDepth != null && tangoDepth.m_points != null)
        {
            m_pointsCount = tangoDepth.m_pointCount;
            if (m_pointsCount > 0)
            {
                _SetUpExtrinsics();
                TangoCoordinateFramePair pair;
                TangoPoseData            poseData = new TangoPoseData();

                // Query pose to transform point cloud to world coordinates, here we are using the timestamp
                // that we get from depth.
                pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
                PoseProvider.GetPoseAtTime(poseData, m_previousDepthDeltaTime, pair);
                if (poseData.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
                {
                    return;
                }

                Vector3 position = new Vector3((float)poseData.translation[0],
                                               (float)poseData.translation[1],
                                               (float)poseData.translation[2]);
                Quaternion quat = new Quaternion((float)poseData.orientation[0],
                                                 (float)poseData.orientation[1],
                                                 (float)poseData.orientation[2],
                                                 (float)poseData.orientation[3]);
                m_startServiceTDevice = Matrix4x4.TRS(position, quat, Vector3.one);

                // The transformation matrix that represents the pointcloud's pose.
                // Explanation:
                // The pointcloud which is in Depth camera's frame, is put in unity world's
                // coordinate system(wrt unity world).
                // Then we are extracting the position and rotation from uwTuc matrix and applying it to
                // the PointCloud's transform.
                Matrix4x4 unityWorldTDepthCamera = m_unityWorldTStartService * m_startServiceTDevice * Matrix4x4.Inverse(m_imuTDevice) * m_imuTDepthCamera;
                transform.position = Vector3.zero;
                transform.rotation = Quaternion.identity;

                //Vector3 minpts = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
                //Vector3 maxpts = new Vector3(float.MinValue, float.MinValue, float.MinValue);

                // Converting points array to world space.
                m_overallZ = 0;

                for (int i = 0; i < m_pointsCount; ++i)
                {
                    float x = tangoDepth.m_points[(i * 3) + 0];
                    float y = tangoDepth.m_points[(i * 3) + 1];
                    float z = tangoDepth.m_points[(i * 3) + 2];

                    m_points[i] = unityWorldTDepthCamera.MultiplyPoint(new Vector3(x, y, z));

                    m_overallZ += z;
                }
                m_overallZ = m_overallZ / m_pointsCount;

                // The color should be pose relative, we need to store enough info to go back to pose values.
                //m_renderer.material.SetMatrix("depthCameraTUnityWorld", unityWorldTDepthCamera.inverse);

                //VoxelExtractionPointCloud.Instance.computeDepthPlanes(ref unityWorldTDepthCamera, unityWorldTDepthCamera * new Vector4(0,0,0,1), minpts, maxpts);
                //if(isScanning)
                PerDepthFrameCallBack.Instance.CallBack(m_points, m_pointsCount);
            }
            else
            {
                m_overallZ = 0;
            }
        }
    }
    /// <summary>
    /// Callback that gets called when depth is available
    /// from the Tango Service.
    /// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION!
    /// </summary>
    /// <param name="callbackContext">Callback context.</param>
    /// <param name="xyzij">Xyzij.</param>
    public void OnTangoDepthAvailable(TangoUnityDepth tangoDepth)
    {
        // Calculate the time since the last successful depth data
        // collection.
        if (m_previousDepthDeltaTime == 0.0)
        {
            m_previousDepthDeltaTime = tangoDepth.m_timestamp;
        }
        else
        {
            m_depthDeltaTime         = (float)((tangoDepth.m_timestamp - m_previousDepthDeltaTime) * 1000.0);
            m_previousDepthDeltaTime = tangoDepth.m_timestamp;
        }

        // Fill in the data to draw the point cloud.
        if (tangoDepth != null && tangoDepth.m_points != null)
        {
            int numberOfActiveVertices = tangoDepth.m_pointCount;
            m_pointsCount = numberOfActiveVertices;
            float validPointCount = 0;
            if (numberOfActiveVertices > 0)
            {
                _SetUpExtrinsics();
                TangoCoordinateFramePair pair;
                TangoPoseData            poseData = new TangoPoseData();
                // Query pose to transform point cloud to world coordinates, here we are using the timestamp
                // that we get from depth.
                pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
                PoseProvider.GetPoseAtTime(poseData, m_previousDepthDeltaTime, pair);
                Vector3 position = new Vector3((float)poseData.translation[0],
                                               (float)poseData.translation[1],
                                               (float)poseData.translation[2]);
                Quaternion quat = new Quaternion((float)poseData.orientation[0],
                                                 (float)poseData.orientation[1],
                                                 (float)poseData.orientation[2],
                                                 (float)poseData.orientation[3]);
                m_ssTd = Matrix4x4.TRS(position, quat, Vector3.one);

                // The transformation matrix that represents the pointcloud's pose.
                // Explanation:
                // The pointcloud which is in RGB's camera frame, is put in unity world's
                // coordinate system(wrt unit camera).
                // Then we are extracting the position and rotation from uwTuc matrix and applying it to
                // the PointCloud's transform.
                Matrix4x4 uwTuc = m_uwTss * m_ssTd * Matrix4x4.Inverse(m_imuTd) * m_imuTc * m_cTuc;
                transform.position = uwTuc.GetColumn(3);
                transform.rotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));

                Vector3[] pointCloudVertices = new Vector3[VERT_COUNT];

                // Converting points array to point vector array for Unity to create a mesh.
                for (int i = 0; i < numberOfActiveVertices; ++i)
                {
                    pointCloudVertices[i] = new Vector3(tangoDepth.m_points[i * 3],
                                                        tangoDepth.m_points[i * 3 + 1],
                                                        tangoDepth.m_points[i * 3 + 2]);
                    m_overallZ += pointCloudVertices[i].z;
                    ++validPointCount;
                }
                m_mesh.Clear();
                m_mesh.vertices  = pointCloudVertices;
                m_mesh.triangles = m_triangles;
                m_mesh.SetIndices(m_triangles, MeshTopology.Points, 0);
            }
            // Don't divide by zero!
            if (validPointCount != 0)
            {
                m_overallZ = m_overallZ / validPointCount;
            }
            else
            {
                m_overallZ = 0;
            }
        }
    }
    /// <summary>
    /// Handle the callback sent by the Tango Service
    /// when a new pose is sampled.
    /// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION!
    /// </summary>
    /// <param name="callbackContext">Callback context.</param>
    /// <param name="pose">Pose.</param>
    public void OnTangoPoseAvailable(TangoPoseData pose)
    {
        int currentIndex = 0;

        // Get out of here if the pose is null
        if (pose == null)
        {
            Debug.Log("TangoPoseDate is null.");
            return;
        }

        // The callback pose is for device with respect to start of service pose.
        if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
            pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
        {
            currentIndex = DEVICE_TO_START;
        }
        // The callback pose is for device with respect to area description file pose.
        else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
                 pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
        {
            currentIndex = DEVICE_TO_ADF;
        }
        // The callback pose is for start of service with respect to area description file pose.
        else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
                 pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
        {
            currentIndex = START_TO_ADF;
        }

        // check to see if we are recently relocalized
        if (!m_isRelocalized)
        {
            m_isRelocalized = (currentIndex == 2);
        }

        if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
        {
            // Create a new Vec3 and Quaternion from the Pose Data received.
            m_tangoPosition[currentIndex] = new Vector3((float)pose.translation [0],
                                                        (float)pose.translation [2],
                                                        (float)pose.translation [1]);

            m_tangoRotation[currentIndex] = new Quaternion((float)pose.orientation [0],
                                                           (float)pose.orientation [2], // these rotation values are swapped on purpose
                                                           (float)pose.orientation [1],
                                                           (float)pose.orientation [3]);
        }
        else // if the current pose is not valid we set the pose to identity
        {
            m_tangoPosition[currentIndex] = Vector3.zero;
            m_tangoRotation[currentIndex] = Quaternion.identity;
            m_isRelocalized = false;
        }

        // Reset the current status frame count if the status code changed.
        if (pose.status_code != m_status[currentIndex])
        {
            m_frameCount[currentIndex] = 0;
        }

        // Update the stats for the pose for the debug text
        m_status[currentIndex] = pose.status_code;
        m_frameCount[currentIndex]++;

        // Compute delta frame timestamp.
        m_frameDeltaTime[currentIndex]      = (float)pose.timestamp - m_prevFrameTimestamp[currentIndex];
        m_prevFrameTimestamp [currentIndex] = (float)pose.timestamp;

        // This rotation needs to be put into Unity coordinate space. In unity +ve x is right,
        // +ve Y is up and +ve Z is forward while coordinate frame for Device wrt Start of service
        // +ve X is right, +ve Y is forward, +ve Z is up.
        // More explanation: https://developers.google.com/project-tango/overview/coordinate-systems
        Quaternion rotationFix = Quaternion.Euler(90.0f, 0.0f, 0.0f);

        // If not relocalized MotionTracking pose(Device wrt Start of Service) is used.
        if (!m_isRelocalized)
        {
            Quaternion axisFix = Quaternion.Euler(-m_tangoRotation[0].eulerAngles.x,
                                                  -m_tangoRotation[0].eulerAngles.z,
                                                  m_tangoRotation[0].eulerAngles.y);

            transform.rotation = m_startingRotation * (rotationFix * axisFix);
            transform.position = (m_startingRotation * (m_tangoPosition[0] * m_movementScale)) + m_startingOffset;
        }
        // If relocalized Device wrt ADF pose is used.
        else
        {
            Quaternion axisFix = Quaternion.Euler(-m_tangoRotation[1].eulerAngles.x,
                                                  -m_tangoRotation[1].eulerAngles.z,
                                                  m_tangoRotation[1].eulerAngles.y);

            transform.rotation = m_startingRotation * (rotationFix * axisFix);
            transform.position = (m_startingRotation * (m_tangoPosition[1] * m_movementScale)) + m_startingOffset;
        }
    }
Exemple #7
0
    /// <summary>
    /// Updates the transformation to the latest pose.
    /// </summary>
    private void _UpdatePose()
    {
        // Query a new pose.
        TangoPoseData pose           = new TangoPoseData();
        double        queryTimestamp = IsTargetingColorCamera ? m_tangoARScreen.m_screenUpdateTime : 0.0f;

        PoseProvider.GetPoseAtTime(pose, queryTimestamp, _GetFramePair());

        // Do not update with invalide poses.
        if (pose.status_code != TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
        {
            return;
        }

        // Do not update if the last update was for the same timestamp.
        if (pose.timestamp == LastPoseTimestamp)
        {
            return;
        }

        LastPoseTimestamp = pose.timestamp;

        DMatrix4x4 globalTLocal;

        if (!m_tangoApplication.GetGlobalTLocal(out globalTLocal))
        {
            Debug.LogError("Unable to obtain GlobalTLocal from TangoApplication.");
            return;
        }

        DMatrix4x4 unityWorld_T_device =
            DMatrix4x4.FromMatrix4x4(TangoSupport.UNITY_WORLD_T_START_SERVICE) * globalTLocal.Inverse * DMatrix4x4.TR(pose.translation, pose.orientation);

        // Calculate matrix for the camera in the Unity world
        if (IsTargetingColorCamera)
        {
            m_unityWorld_T_unityCamera =
                unityWorld_T_device.ToMatrix4x4() * TangoSupport.COLOR_CAMERA_T_UNITY_CAMERA * TangoSupport.m_colorCameraPoseRotation;
        }
        else
        {
            m_unityWorld_T_unityCamera =
                unityWorld_T_device.ToMatrix4x4() * TangoSupport.DEVICE_T_UNITY_CAMERA * TangoSupport.m_devicePoseRotation;
        }

        // Extract final position and rotation from matrix.
        Matrix4x4  unityWorldOffset_T_unityCamera = m_unityWorldTransformOffset_T_unityWorld * m_unityWorld_T_unityCamera;
        Vector3    finalPosition = unityWorldOffset_T_unityCamera.GetColumn(3);
        Quaternion finalRotation = Quaternion.LookRotation(unityWorldOffset_T_unityCamera.GetColumn(2), unityWorldOffset_T_unityCamera.GetColumn(1));

        // Filter out yaw if the clutch is enabled.
        if (m_clutchEnabled)
        {
            finalPosition = transform.position;
            finalRotation = Quaternion.Euler(finalRotation.eulerAngles.x, transform.eulerAngles.y, finalRotation.eulerAngles.z);
        }

        // Apply the final position.
        if (m_characterController)
        {
            m_characterController.Move(finalPosition - transform.position);
        }
        else
        {
            transform.position = finalPosition;
        }
        if (transform != null && display_position != null)
        {
            display_position.text = "X:" + System.Math.Round(transform.position.x, 2) + "\n" + "Y:" + System.Math.Round(transform.position.y, 2) + "\n" + "Z:" + System.Math.Round(transform.position.z, 2) + "\n";
        }
        transform.rotation = finalRotation;
    }
    /// <summary>
    /// Set controller's transformation based on received pose.
    /// </summary>
    /// <param name="pose">Received Tango pose data.</param>
    private void _UpdateTransformationFromPose(TangoPoseData pose)
    {
        // Remember the previous position, so you can do delta motion
        m_prevTangoPosition = m_tangoPosition;
        m_prevTangoRotation = m_tangoRotation;

        // The callback pose is for device with respect to start of service pose.
        if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
        {
            // Construct matrix for the start of service with respect to device from the pose.
            Vector3 posePosition = new Vector3((float)pose.translation[0],
                                               (float)pose.translation[1],
                                               (float)pose.translation[2]);
            Quaternion poseRotation = new Quaternion((float)pose.orientation[0],
                                                     (float)pose.orientation[1],
                                                     (float)pose.orientation[2],
                                                     (float)pose.orientation[3]);
            Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);

            // Calculate matrix for the camera in the Unity world, taking into account offsets.
            m_uwTuc = m_uwTss * ssTd * m_dTuc;
            Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;

            // Extract final position, rotation.
            m_tangoPosition = uwOffsetTuc.GetColumn(3);
            m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));

            // Other pose data -- Pose count gets reset if pose status just became valid.
            if (pose.status_code != m_poseStatus)
            {
                m_poseCount = 0;
            }
            m_poseCount++;

            // Other pose data -- Pose delta time.
            m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp;
            m_poseTimestamp = (float)pose.timestamp;
        }
        m_poseStatus = pose.status_code;

        if (m_clutchActive)
        {
            // When clutching, preserve position.
            m_tangoPosition = m_prevTangoPosition;

            // When clutching, preserve yaw, keep changes in pitch, roll.
            Vector3 rotationAngles = m_tangoRotation.eulerAngles;
            rotationAngles.y            = m_prevTangoRotation.eulerAngles.y;
            m_tangoRotation.eulerAngles = rotationAngles;
        }

        // Calculate final position and rotation deltas and apply them.
        Vector3    deltaPosition = m_tangoPosition - m_prevTangoPosition;
        Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation);

        if (m_characterMotion)
        {
            m_characterController.Move(deltaPosition);
            transform.rotation = deltaRotation * transform.rotation;
        }
        else
        {
            transform.position = transform.position + deltaPosition;
            transform.rotation = deltaRotation * transform.rotation;
        }
    }
Exemple #9
0
    /// <summary>
    /// OnTangoPoseAvailable is called from Tango when a new Pose is available.
    /// </summary>
    /// <param name="pose">The new Tango pose.</param>
    public void OnTangoPoseAvailable(TangoPoseData pose)
    {
        // Get out of here if the pose is null
        if (pose == null)
        {
            Debug.Log("TangoPoseDate is null.");
            return;
        }

        // Only interested in pose updates relative to the start of service pose.
        if (pose.framePair.baseFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE ||
            pose.framePair.targetFrame != TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
        {
            return;
        }

        // Remember the previous position, so you can do delta motion
        m_prevTangoPosition = m_tangoPosition;
        m_prevTangoRotation = m_tangoRotation;

        // The callback pose is for device with respect to start of service pose.
        if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
        {
            // Construct matrix for the start of service with respect to device from the pose.
            Vector3 posePosition = new Vector3((float)pose.translation[0],
                                               (float)pose.translation[1],
                                               (float)pose.translation[2]);
            Quaternion poseRotation = new Quaternion((float)pose.orientation[0],
                                                     (float)pose.orientation[1],
                                                     (float)pose.orientation[2],
                                                     (float)pose.orientation[3]);
            Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);

            // Calculate matrix for the camera in the Unity world, taking into account offsets.
            m_uwTuc = m_uwTss * ssTd * m_dTuc;
            Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;

            // Extract final position, rotation.
            m_tangoPosition = uwOffsetTuc.GetColumn(3);
            m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));

            // Other pose data -- Pose count gets reset if pose status just became valid.
            if (pose.status_code != m_poseStatus)
            {
                m_poseCount = 0;
            }
            m_poseCount++;

            // Other pose data -- Pose delta time.
            m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp;
            m_poseTimestamp = (float)pose.timestamp;
        }
        m_poseStatus = pose.status_code;

        if (m_clutchActive)
        {
            // When clutching, preserve position.
            m_tangoPosition = m_prevTangoPosition;

            // When clutching, preserve yaw, keep changes in pitch, roll.
            Vector3 rotationAngles = m_tangoRotation.eulerAngles;
            rotationAngles.y            = m_prevTangoRotation.eulerAngles.y;
            m_tangoRotation.eulerAngles = rotationAngles;
        }

        // Calculate final position and rotation deltas and apply them.
        Vector3    deltaPosition = m_tangoPosition - m_prevTangoPosition;
        Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation);

        if (m_characterMotion)
        {
            m_characterController.Move(deltaPosition);
            transform.rotation = deltaRotation * transform.rotation;
        }
        else
        {
            transform.position = transform.position + deltaPosition;
            transform.rotation = deltaRotation * transform.rotation;
        }
    }
	/// <summary>
	/// Handle the callback sent by the Tango Service
	/// when a new pose is sampled.
	/// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION!
	/// </summary>
	/// <param name="callbackContext">Callback context.</param>
	/// <param name="pose">Pose.</param>
	protected override void _OnPoseAvailable(IntPtr callbackContext, TangoPoseData pose)
	{
		int currentIndex = 0;
		
		// Get out of here if the pose is null
		if (pose == null)
		{
			Debug.Log("TangoPoseDate is null.");
			return;
		}
		
		// The callback pose is for device with respect to start of service pose.
		if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
		    pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
		{
			currentIndex = DEVICE_TO_START;
		}
		// The callback pose is for device with respect to area description file pose.
		else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
		         pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
		{
			currentIndex = DEVICE_TO_ADF;
		} 
		// The callback pose is for start of service with respect to area description file pose.
		else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
		         pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
		{
			currentIndex = START_TO_ADF;
		}
		
		// check to see if we are recently relocalized
		if(!m_isRelocalized)
		{
			m_isRelocalized = (currentIndex == 2);
		}
		
		if(pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
		{
			// Cache the position and rotation to be set in the update function.
			// This needs to be done because this callback does not
			// happen in the main game thread.
			m_tangoPosition[currentIndex] = new Vector3((float)pose.translation [0],
			                                            (float)pose.translation [2],
			                                            (float)pose.translation [1]);
			
			m_tangoRotation[currentIndex] = new Quaternion((float)pose.orientation [0],
			                                               (float)pose.orientation [2], // these rotation values are swapped on purpose
			                                               (float)pose.orientation [1],
			                                               (float)pose.orientation [3]);
		}
		else // if the current pose is not valid we set the pose to identity
		{
			m_tangoPosition[currentIndex] = Vector3.zero;
			m_tangoRotation[currentIndex] = Quaternion.identity;
			m_isRelocalized = false;
		}
		
		// Reset the current status frame count if the status code changed.
		if (pose.status_code != m_status[currentIndex])
		{
			m_frameCount[currentIndex] = 0;
		}
		
		// Update the stats for the pose for the debug text
		m_status[currentIndex] = pose.status_code;
		m_frameCount[currentIndex]++;
		
		// Compute delta frame timestamp.
		m_frameDeltaTime[currentIndex] = (float)pose.timestamp - m_prevFrameTimestamp[currentIndex];
		m_prevFrameTimestamp [currentIndex] = (float)pose.timestamp;
		
		// Switch m_isDirty to true, so that the new pose get rendered in update.
		m_isDirty = (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID);
	}
Exemple #11
0
    /// <summary>
    /// OnTangoPoseAvailable is called from Tango when a new Pose is available.
    /// </summary>
    /// <param name="pose">The new Tango pose.</param>
    public void OnTangoPoseAvailable(TangoPoseData pose)
    {
        // Get out of here if the pose is null
        if (pose == null)
        {
            Debug.Log("TangoPoseDate is null.");
            return;
        }

        int currentFrame;

        // Only interested in specific pose updates
        if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
            pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
        {
            // The callback pose is for device with respect to start of service pose.
            currentFrame = (int)PoseFrame.DeviceToStart;
        }
        else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
                 pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
        {
            // The callback pose is for device with respect to area description file pose.
            currentFrame = (int)PoseFrame.DeviceToADF;
        }
        else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
                 pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
        {
            // The callback pose is for start of service with respect to area description file pose.
            currentFrame    = (int)PoseFrame.StartToADF;
            m_poseLocalized = true;
        }
        else
        {
            // Not a pose frame we are interested in.
            return;
        }
        Debug.Log("OnPose(" + currentFrame.ToString() + ")");

        // Remember the previous position, so you can do delta motion
        m_prevTangoPosition[currentFrame] = m_tangoPosition[currentFrame];
        m_prevTangoRotation[currentFrame] = m_tangoRotation[currentFrame];

        // The callback pose is for device with respect to start of service pose.
        if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
        {
            // Construct matrix for the start of service with respect to device from the pose.
            Vector3 posePosition = new Vector3((float)pose.translation[0],
                                               (float)pose.translation[1],
                                               (float)pose.translation[2]);
            Quaternion poseRotation = new Quaternion((float)pose.orientation[0],
                                                     (float)pose.orientation[1],
                                                     (float)pose.orientation[2],
                                                     (float)pose.orientation[3]);
            Matrix4x4 ssTd = Matrix4x4.TRS(posePosition, poseRotation, Vector3.one);

            // Calculate matrix for the camera in the Unity world, taking into account offsets.
            Matrix4x4 uwTuc = m_uwTss * ssTd * m_dTuc;

            // Extract final position, rotation.
            m_tangoPosition[currentFrame] = uwTuc.GetColumn(3);
            m_tangoRotation[currentFrame] = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));

            // Other pose data -- Pose count gets reset if pose status just became valid.
            if (pose.status_code != m_poseStatus[currentFrame])
            {
                m_poseCount[currentFrame] = 0;
            }
            m_poseCount[currentFrame]++;

            // Other pose data -- Pose delta time.
            m_poseDeltaTime[currentFrame] = (float)pose.timestamp - m_poseTimestamp[currentFrame];
            m_poseTimestamp[currentFrame] = (float)pose.timestamp;
        }
        else
        {
            // if the current pose is not valid assume everything is invalid.
            m_tangoPosition[currentFrame] = Vector3.zero;
            m_tangoRotation[currentFrame] = Quaternion.identity;
            m_poseLocalized = false;
        }
        m_poseStatus[currentFrame] = pose.status_code;

        if (currentFrame != (int)PoseFrame.StartToADF)
        {
            // Only update position and rotation if the update was for the device's pose.
            //
            // Calculate final position and rotation deltas and apply them.
            transform.position = m_tangoPosition[currentFrame];
            transform.rotation = m_tangoRotation[currentFrame];
        }
    }
	/// <summary>
	/// Apply any needed changes to the pose.
	/// </summary>
	private void Update()
	{
		#if UNITY_ANDROID && !UNITY_EDITOR
		if(m_shouldInitTango)
		{
			m_tangoApplication.InitApplication();
			
			if(m_useADF)
			{
				// Query the full adf list.
				PoseProvider.RefreshADFList();
				// loading last recorded ADF
				string uuid = PoseProvider.GetLatestADFUUID().GetStringDataUUID();
				m_tangoApplication.InitProviders(uuid);
			}
			else
			{
				m_tangoApplication.InitProviders(string.Empty);
			}
			
			// Query extrinsics constant tranformations.
			TangoPoseData poseData = new TangoPoseData();
			double timestamp = 0.0;
			TangoCoordinateFramePair pair;
			
			pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
			pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
			PoseProvider.GetPoseAtTime(poseData, timestamp, pair);
			Vector3 position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]);
			Quaternion quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]);
			m_deviceToIMUMatrix = Matrix4x4.TRS(position, quat, new Vector3 (1.0f, 1.0f, 1.0f));
			
			pair.baseFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_IMU;
			pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_CAMERA_COLOR;
			PoseProvider.GetPoseAtTime(poseData, timestamp, pair);
			position = new Vector3((float)poseData.translation[0], (float)poseData.translation[1], (float)poseData.translation[2]);
			quat = new Quaternion((float)poseData.orientation[0], (float)poseData.orientation[1], (float)poseData.orientation[2], (float)poseData.orientation[3]);
			m_cameraToIMUMatrix = Matrix4x4.TRS(position, quat, new Vector3 (1.0f, 1.0f, 1.0f));
			
			m_alreadyInitialized = true;
			m_shouldInitTango = false;
			
			m_tangoApplication.ConnectToService();
		}

		if (m_isDirty)
		{
			// This rotation needs to be put into Unity coordinate space.
			Quaternion rotationFix = Quaternion.Euler(90.0f, 0.0f, 0.0f);
			
			if (!m_isRelocalized) 
			{
				Quaternion axisFix = Quaternion.Euler(-m_tangoRotation[0].eulerAngles.x,
				                                      -m_tangoRotation[0].eulerAngles.z,
				                                      m_tangoRotation[0].eulerAngles.y);
				
				transform.rotation = m_startingRotation * (rotationFix * axisFix);
				transform.position = (m_startingRotation * (m_tangoPosition[0] * m_movementScale)) + m_startingOffset;
				
			}
			else 
			{
				Quaternion axisFix = Quaternion.Euler(-m_tangoRotation[1].eulerAngles.x,
				                                      -m_tangoRotation[1].eulerAngles.z,
				                                      m_tangoRotation[1].eulerAngles.y);
				
				transform.rotation = m_startingRotation * (rotationFix * axisFix);
				transform.position = (m_startingRotation * (m_tangoPosition[1] * m_movementScale)) + m_startingOffset;
			}
			m_isDirty = false;
		}
		
		if(Input.GetKeyDown(KeyCode.Escape))
		{
			if(m_tangoApplication != null)
			{
				m_tangoApplication.Shutdown();
			}
			
			// This is a temporary fix for a lifecycle issue where calling
			// Application.Quit() here, and restarting the application immediately,
			// results in a hard crash.
			AndroidHelper.AndroidQuit();
		}
		
		#else
		Vector3 tempPosition = transform.position;
		Quaternion tempRotation = transform.rotation;
		PoseProvider.GetMouseEmulation(ref tempPosition, ref tempRotation);
		transform.rotation = tempRotation;
		transform.position = tempPosition;
		#endif
	}
Exemple #13
0
    /// @endcond
    /// <summary>
    /// Updates the transformation to the pose for that timestamp.
    /// </summary>
    /// <param name="timestamp">Time in seconds to update the transformation to.</param>
    private void _UpdateTransformation(double timestamp)
    {
        TangoPoseData            pose = new TangoPoseData();
        TangoCoordinateFramePair pair;

        // Choose the proper pair according to the properties of this controller
        if (m_useAreaDescriptionPose)
        {
            if (m_tangoApplication.m_enableCloudADF)
            {
                pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_GLOBAL_WGS84;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
            }
            else
            {
                pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION;
                pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
            }
        }
        else
        {
            pair.baseFrame   = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE;
            pair.targetFrame = TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE;
        }

        PoseProvider.GetPoseAtTime(pose, timestamp, pair);

        // Update properties from pose
        if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
        {
            DMatrix4x4 globalTLocal;
            bool       success = m_tangoApplication.GetGlobalTLocal(out globalTLocal);
            if (!success)
            {
                Debug.LogError("Unable to obtain GlobalTLocal from TangoApplication.");
                return;
            }

            DMatrix4x4 uwTDevice = DMatrix4x4.FromMatrix4x4(TangoSupport.UNITY_WORLD_T_START_SERVICE) *
                                   globalTLocal.Inverse * DMatrix4x4.TR(pose.translation, pose.orientation);

            // Calculate matrix for the camera in the Unity world, taking into account offsets.
            Matrix4x4 uwTuc = uwTDevice.ToMatrix4x4() * m_dTuc * TangoSupport.m_colorCameraPoseRotation;

            // Extract final position, rotation.
            m_tangoPosition = uwTuc.GetColumn(3);
            m_tangoRotation = Quaternion.LookRotation(uwTuc.GetColumn(2), uwTuc.GetColumn(1));

            // Other pose data -- Pose count gets reset if pose status just became valid.
            if (pose.status_code != m_poseStatus)
            {
                m_poseCount = 0;
            }

            m_poseCount++;

            // Other pose data -- Pose time.
            m_poseTimestamp = timestamp;
        }

        m_poseStatus = pose.status_code;

        // Apply final position and rotation.
        transform.position = m_tangoPosition;
        transform.rotation = m_tangoRotation;
    }
Exemple #14
0
        /// <summary>
        /// Updates the text view in UI screen with the Pose. Each pose is associated
        /// with Target and Base Frame. We need to check for that pair ad update our
        /// views accordingly.
        /// </summary>
        /// <param name="pose"> </param>
//JAVA TO C# CONVERTER WARNING: 'final' parameters are not available in .NET:
        // Original line in class:  private void updateTextViewWith(final com.google.atap.tangoservice.TangoPoseData pose)
        private void updateTextViewWith(TangoPoseData pose)
        {
            this.RunOnUiThread(() =>
            {
                try
                {
                    string translationString = "[" + threeDec.format(pose.Translation[0]) + "," + threeDec.format(pose.Translation[1]) + "," + threeDec.format(pose.Translation[2]) + "] ";

                    string quaternionString = "[" + threeDec.format(pose.Rotation[0]) + "," + threeDec.format(pose.Rotation[1]) + "," + threeDec.format(pose.Rotation[2]) + "," + threeDec.format(pose.Rotation[3]) + "] ";

                    if ((pose.BaseFrame == TangoPoseData.CoordinateFrameAreaDescription) && (pose.TargetFrame == TangoPoseData.CoordinateFrameDevice))
                    {
                        mAdf2DevicePoseCount++;
                        mAdf2DevicePoseDelta                = (pose.Timestamp - mAdf2DevicePreviousPoseTimeStamp) * SECONDS_TO_MILLI;
                        mAdf2DevicePreviousPoseTimeStamp    = pose.Timestamp;
                        mAdf2DeviceTranslationTextView.Text = translationString;
                        mAdf2DeviceQuatTextView.Text        = quaternionString;
                        mAdf2DevicePoseStatusTextView.Text  = getPoseStatus(pose);
                        mAdf2DevicePoseCountTextView.Text   = Convert.ToString(mAdf2DevicePoseCount);
                        mAdf2DevicePoseDeltaTextView.Text   = threeDec.format(mAdf2DevicePoseDelta);
                    }

                    if ((pose.BaseFrame == TangoPoseData.CoordinateFrameStartOfService) && (pose.TargetFrame == TangoPoseData.CoordinateFrameDevice))
                    {
                        mStart2DevicePoseCount++;
                        mStart2DevicePoseDelta                = (pose.Timestamp - mStart2DevicePreviousPoseTimeStamp) * SECONDS_TO_MILLI;
                        mStart2DevicePreviousPoseTimeStamp    = pose.Timestamp;
                        mStart2DeviceTranslationTextView.Text = translationString;
                        mStart2DeviceQuatTextView.Text        = quaternionString;
                        mStart2DevicePoseStatusTextView.Text  = getPoseStatus(pose);
                        mStart2DevicePoseCountTextView.Text   = Convert.ToString(mStart2DevicePoseCount);
                        mStart2DevicePoseDeltaTextView.Text   = threeDec.format(mStart2DevicePoseDelta);
                    }

                    if ((pose.BaseFrame == TangoPoseData.CoordinateFrameAreaDescription) && (pose.TargetFrame == TangoPoseData.CoordinateFrameStartOfService))
                    {
                        mAdf2StartPoseCount++;
                        mAdf2StartPoseDelta                = (pose.Timestamp - mAdf2StartPreviousPoseTimeStamp) * SECONDS_TO_MILLI;
                        mAdf2StartPreviousPoseTimeStamp    = pose.Timestamp;
                        mAdf2StartTranslationTextView.Text = translationString;
                        mAdf2StartQuatTextView.Text        = quaternionString;
                        mAdf2StartPoseStatusTextView.Text  = getPoseStatus(pose);
                        mAdf2StartPoseCountTextView.Text   = Convert.ToString(mAdf2StartPoseCount);
                        mAdf2StartPoseDeltaTextView.Text   = threeDec.format(mAdf2StartPoseDelta);


                        if (pose.StatusCode == TangoPoseData.PoseValid)
                        {
                            mIsRelocalized = true;
                            // Set the color to green
                            if (mRenderer.Trajectory != null)
                            {
                                mRenderer.Trajectory.Color = new float[] { 0.39f, 0.56f, 0.03f, 1.0f }
                            }
                            ;
                        }
                        else
                        {
                            mIsRelocalized = false;
                            // Set the color blue
                            if (mRenderer.Trajectory != null)
                            {
                                mRenderer.Trajectory.Color = new float[] { 0.22f, 0.28f, 0.67f, 1.0f }
                            }
                            ;
                        }
                    }
                }
                catch (System.Exception e)
                {
                    Toast.MakeText(ApplicationContext, e.Message, Android.Widget.ToastLength.Short).Show();
                }
            });
        }
Exemple #15
0
    /// <summary>
    /// Set controller's transformation based on received pose.
    /// </summary>
    /// <param name="pose">Received Tango pose data.</param>
    private void _UpdateTransformationFromPose(TangoPoseData pose)
    {
        // Remember the previous position so you can do delta motion.
        m_prevTangoPosition = m_tangoPosition;
        m_prevTangoRotation = m_tangoRotation;

        // The callback pose is for device with respect to start of service pose.
        if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
        {
            DMatrix4x4 globalTLocal;
            bool       success = m_tangoApplication.GetGlobalTLocal(out globalTLocal);
            if (!success)
            {
                Debug.LogError("Unable to obtain GlobalTLocal from Tango application.");
                return;
            }

            DMatrix4x4 startOfServiceTDevice = globalTLocal.Inverse * DMatrix4x4.TR(pose.translation, pose.orientation);

            m_uwTuc = TangoSupport.UNITY_WORLD_T_START_SERVICE * startOfServiceTDevice.ToMatrix4x4()
                      * TangoSupport.DEVICE_T_UNITY_CAMERA * TangoSupport.m_devicePoseRotation;
            Matrix4x4 uwOffsetTuc = m_uwOffsetTuw * m_uwTuc;

            m_tangoPosition = uwOffsetTuc.GetColumn(3);
            m_tangoRotation = Quaternion.LookRotation(uwOffsetTuc.GetColumn(2), uwOffsetTuc.GetColumn(1));

            // Other pose data: pose count gets reset if pose status just became valid.
            if (pose.status_code != m_poseStatus)
            {
                m_poseCount = 0;
            }

            m_poseCount++;

            // Other pose data: pose delta time.
            m_poseDeltaTime = (float)pose.timestamp - m_poseTimestamp;
            m_poseTimestamp = (float)pose.timestamp;
        }

        m_poseStatus = pose.status_code;

        if (m_clutchActive)
        {
            // When clutching, preserve position.
            m_tangoPosition = m_prevTangoPosition;

            // When clutching, preserve yaw, keep changes in pitch, roll.
            Vector3 rotationAngles = m_tangoRotation.eulerAngles;
            rotationAngles.y            = m_prevTangoRotation.eulerAngles.y;
            m_tangoRotation.eulerAngles = rotationAngles;
        }

        // Calculate final position and rotation deltas and apply them.
        Vector3    deltaPosition = m_tangoPosition - m_prevTangoPosition;
        Quaternion deltaRotation = m_tangoRotation * Quaternion.Inverse(m_prevTangoRotation);

        if (m_characterMotion && m_characterController != null)
        {
            m_characterController.Move(deltaPosition);
            transform.rotation = deltaRotation * transform.rotation;
        }
        else
        {
            transform.position = transform.position + deltaPosition;
            transform.rotation = deltaRotation * transform.rotation;
        }
    }
Exemple #16
0
    /// <summary>
    /// Handle the callback sent by the Tango Service
    /// when a new pose is sampled.
    /// DO NOT USE THE UNITY API FROM INSIDE THIS FUNCTION.
    /// </summary>
    /// <param name="pose">Pose.</param>
    public void OnTangoPoseAvailable(TangoPoseData pose)
    {
        int currentIndex = 0;

        // Get out of here if the pose is null
        if (pose == null)
        {
            Debug.Log("TangoPoseDate is null.");
            return;
        }

        if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE &&
            pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
        {
            // The callback pose is for device with respect to start of service pose.
            currentIndex = DEVICE_TO_START;
        }
        else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
                 pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_DEVICE)
        {
            // The callback pose is for device with respect to area description file pose.
            currentIndex = DEVICE_TO_ADF;
        }
        else if (pose.framePair.baseFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_AREA_DESCRIPTION &&
                 pose.framePair.targetFrame == TangoEnums.TangoCoordinateFrameType.TANGO_COORDINATE_FRAME_START_OF_SERVICE)
        {
            // The callback pose is for start of service with respect to area description file pose.
            currentIndex = START_TO_ADF;
        }

        if (!m_isRelocalized)
        {
            // check to see if we are recently relocalized
            if (currentIndex == 2)
            {
                m_isRelocalized = true;
            }
        }

        if (pose.status_code == TangoEnums.TangoPoseStatusType.TANGO_POSE_VALID)
        {
            // Create a new Vec3 and Quaternion from the Pose Data received.
            m_tangoPosition[currentIndex] = new Vector3((float)pose.translation[0],
                                                        (float)pose.translation[1],
                                                        (float)pose.translation[2]);

            m_tangoRotation[currentIndex] = new Quaternion((float)pose.orientation[0],
                                                           (float)pose.orientation[1],
                                                           (float)pose.orientation[2],
                                                           (float)pose.orientation[3]);
        }
        else
        {
            // if the current pose is not valid we set the pose to identity
            m_tangoPosition[currentIndex] = Vector3.zero;
            m_tangoRotation[currentIndex] = Quaternion.identity;
            m_isRelocalized = false;
        }

        // Reset the current status frame count if the status code changed.
        if (pose.status_code != m_status[currentIndex])
        {
            m_frameCount[currentIndex] = 0;
        }

        // Update the stats for the pose for the debug text
        m_status[currentIndex] = pose.status_code;
        m_frameCount[currentIndex]++;

        // Compute delta frame timestamp.
        m_frameDeltaTime[currentIndex]     = (float)pose.timestamp - m_prevFrameTimestamp[currentIndex];
        m_prevFrameTimestamp[currentIndex] = (float)pose.timestamp;

        Matrix4x4 matrixssTd;

        if (!m_isRelocalized)
        {
            // If not relocalized MotionTracking pose(Device wrt Start of Service) is used.
            matrixssTd = Matrix4x4.TRS(m_tangoPosition[0], m_tangoRotation[0], Vector3.one);
        }
        else
        {
            // If relocalized Device wrt ADF pose is used.
            matrixssTd = Matrix4x4.TRS(m_tangoPosition[1], m_tangoRotation[1], Vector3.one);
        }

        // Converting from Tango coordinate frame to Unity coodinate frame.
        Matrix4x4 matrixuwTuc = m_matrixuwTss * matrixssTd * m_matrixdTuc;

        // Extract new local position
        transform.position = matrixuwTuc.GetColumn(3);

        // Extract new local rotation
        transform.rotation = Quaternion.LookRotation(matrixuwTuc.GetColumn(2), matrixuwTuc.GetColumn(1));
    }