import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Updates every saved measurement. It re-queries the device pose at the time the measurement
 * was taken.
 */
public void updateMeasurements() {
    for (WallMeasurement wallMeasurement : mWallMeasurementList) {
        // We need to re query the depth transform when the measurements were taken.
        TangoSupport.TangoMatrixTransformData transform =
                TangoSupport.getMatrixTransformAtTime(wallMeasurement
                                .getDepthTransformTimeStamp(),
                        TangoPoseData.COORDINATE_FRAME_AREA_DESCRIPTION,
                        TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH,
                        TangoSupport.TANGO_SUPPORT_ENGINE_OPENGL,
                        TangoSupport.TANGO_SUPPORT_ENGINE_TANGO);
        if (transform.statusCode == TangoPoseData.POSE_VALID) {
            wallMeasurement.update(transform.matrix);
            mRenderer.addWallMeasurement(wallMeasurement);
        } else {
            Log.d(TAG, "Could not get a valid transform from depth to area description at time "
                    + wallMeasurement
                    .getDepthTransformTimeStamp());
        }
    }
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Converts a transform in Matrix4 format to TangoPoseData.
 */
public static TangoPoseData matrixToTangoPose(Matrix4 transform) {
    // Get translation and rotation components from the transformation matrix.
    Vector3 p = transform.getTranslation();
    Quaternion q = new Quaternion();
    q.fromMatrix(transform);

    TangoPoseData tangoPose = new TangoPoseData();
    double[] t = tangoPose.translation = new double[3];
    t[0] = p.x;
    t[1] = p.y;
    t[2] = p.z;
    double[] r = tangoPose.rotation = new double[4];
    r[0] = q.x;
    r[1] = q.y;
    r[2] = q.z;
    r[3] = q.w;

    return tangoPose;
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Given a point and a normal in depth camera frame and the device pose in start of service
 * frame at the time the point and normal were acquired, calculate a Pose object which
 * represents the position and orientation of the fitted plane with its Y vector pointing
 * up in the gravity vector, represented in the Tango start of service frame.
 *
 * @param point     Point in depth frame where the plane has been detected.
 * @param normal    Normal of the detected plane.
 * @param tangoPose Device pose with respect to start of service at the time the plane was
 *                  fitted.
 */
public static TangoPoseData planeFitToTangoWorldPose(
        double[] point, double[] normal, TangoPoseData tangoPose, DeviceExtrinsics extrinsics) {
    Matrix4 startServiceTdevice = tangoPoseToMatrix(tangoPose);

    // Calculate the UP vector in the depth frame at the provided measurement pose.
    Vector3 depthUp = TANGO_WORLD_UP.clone();
    startServiceTdevice.clone().multiply(extrinsics.getDeviceTDepthCamera())
            .inverse().rotateVector(depthUp);

    // Calculate the transform in depth frame corresponding to the plane fitting information.
    Matrix4 depthTplane = matrixFromPointNormalUp(point, normal, depthUp);

    // Convert to OpenGL frame.
    Matrix4 tangoWorldTplane = startServiceTdevice.multiply(extrinsics.getDeviceTDepthCamera()).
            multiply(depthTplane);

    return matrixToTangoPose(tangoWorldTplane);
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Converts a point, represented as a Vector3 from it's initial refrence frame to 
 * the OpenGl world refrence frame. This allows various points to be depicted in
 * the OpenGl rendering.
 */
public static Vector3 getPointInEngineFrame(
        Vector3 inPoint,
        TangoPoseData deviceTPointFramePose,
        TangoPoseData startServiceTDevicePose) {
    Matrix4 startServiceTDeviceMatrix = tangoPoseToMatrix(startServiceTDevicePose);
    Matrix4 deviceTPointFrameMatrix = tangoPoseToMatrix(deviceTPointFramePose);
    Matrix4 startServiceTDepthMatrix 
            = startServiceTDeviceMatrix.multiply(deviceTPointFrameMatrix);

    // Convert the depth point to a Matrix. 
    Matrix4 inPointMatrix = new Matrix4();
    inPointMatrix.setToTranslation(inPoint);

    // Transform Point from depth frame to start of service frame to OpenGl world frame.
    Matrix4 startServicePointMatrix = startServiceTDepthMatrix.multiply(inPointMatrix);
    Matrix4 openGlWorldPointMatrix 
            = OPENGL_T_TANGO_WORLD.clone().multiply(startServicePointMatrix);
    return matrixToPose(openGlWorldPointMatrix).getPosition();
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Get the status of the Pose as a string.
 *
 * @param pose Pose from which status string is constructed.
 * @return
 */
public static String getStatusString(TangoPoseData pose) {
    String poseStatus;
    switch (pose.statusCode) {
        case TangoPoseData.POSE_UNKNOWN:
            poseStatus = "unknown";
            break;
        case TangoPoseData.POSE_INVALID:
            poseStatus = "invalid";
            break;
        case TangoPoseData.POSE_INITIALIZING:
            poseStatus = "initializing";
            break;
        case TangoPoseData.POSE_VALID:
            poseStatus = "valid";
            break;
        default:
            poseStatus = "unknown";
    }
    return poseStatus;
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
private static DeviceExtrinsics setupExtrinsics(Tango tango) {
    // Create camera to IMU transform.
    TangoCoordinateFramePair framePair = new TangoCoordinateFramePair();
    framePair.baseFrame = TangoPoseData.COORDINATE_FRAME_IMU;
    framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR;
    TangoPoseData imuToRgbPose = tango.getPoseAtTime(0.0, framePair);

    // Create device to IMU transform.
    framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_DEVICE;
    TangoPoseData imuToDevicePose = tango.getPoseAtTime(0.0, framePair);

    // Create depth camera to IMU transform.
    framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH;
    TangoPoseData imuToDepthPose = tango.getPoseAtTime(0.0, framePair);

    return new DeviceExtrinsics(imuToDevicePose, imuToRgbPose, imuToDepthPose);
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Setup the extrinsics of the device.
 */
private void setUpExtrinsics() {
    // Get device to imu matrix.
    TangoPoseData device2IMUPose = new TangoPoseData();
    TangoCoordinateFramePair framePair = new TangoCoordinateFramePair();
    framePair.baseFrame = TangoPoseData.COORDINATE_FRAME_IMU;
    framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_DEVICE;
    device2IMUPose = mTango.getPoseAtTime(0.0, framePair);
    mRenderer.getModelMatCalculator().SetDevice2IMUMatrix(
            device2IMUPose.getTranslationAsFloats(), device2IMUPose.getRotationAsFloats());

    // Get color camera to imu matrix.
    TangoPoseData color2IMUPose = new TangoPoseData();
    framePair.baseFrame = TangoPoseData.COORDINATE_FRAME_IMU;
    framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR;
    color2IMUPose = mTango.getPoseAtTime(0.0, framePair);

    mRenderer.getModelMatCalculator().SetColorCamera2IMUMatrix(
            color2IMUPose.getTranslationAsFloats(), color2IMUPose.getRotationAsFloats());
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Converts a transform in Matrix4 format to TangoPoseData.
 */
public static TangoPoseData matrixToTangoPose(Matrix4 transform) {
    // Get translation and rotation components from the transformation matrix
    Vector3 p = transform.getTranslation();
    Quaternion q = new Quaternion();
    q.fromMatrix(transform);
    // NOTE: Rajawali Quaternions use a left-hand rotation around the axis convention
    q.conjugate();

    TangoPoseData tangoPose = new TangoPoseData();
    double[] t = tangoPose.translation = new double[3];
    t[0] = p.x;
    t[1] = p.y;
    t[2] = p.z;
    double[] r = tangoPose.rotation = new double[4];
    r[0] = q.x;
    r[1] = q.y;
    r[2] = q.z;
    r[3] = q.w;

    return tangoPose;
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Given a pose in start of service or area description frame, calculate the corresponding
 * position and orientation for a OpenGL Scene Camera in the Rajawali world.
 */
public Pose toOpenGLCameraPose(TangoPoseData tangoPose) {
    // We can't do this calculation until Extrinsics are set-up
    if (mDeviceTColorCamera == null) {
        throw new RuntimeException("You must call setupExtrinsics first.");
    }

    Matrix4 startServiceTdevice = tangoPoseToMatrix(tangoPose);

    // Get device pose in OpenGL world frame
    Matrix4 openglTDevice = OPENGL_T_TANGO_WORLD.clone().multiply(startServiceTdevice);

    // Get OpenGL camera pose in OpenGL world frame
    Matrix4 openglWorldTOpenglCamera = openglTDevice.multiply(mDeviceTColorCamera).
            multiply(COLOR_CAMERA_T_OPENGL_CAMERA);

    return matrixToPose(openglWorldTOpenglCamera);
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Given a TangoPoseData object, calculate the corresponding position and orientation for a
 * PointCloud in Depth camera coordinate system to the Rajawali world.
 */
public Pose toOpenGLPointCloudPose(TangoPoseData tangoPose) {

    // We can't do this calculation until Extrinsics are set-up
    if (mDeviceTDepthCamera == null) {
        throw new RuntimeException("You must call setupExtrinsics first.");
    }

    //conversion matrix to put point cloud data in Rajwali/Opengl Coordinate system.
    Matrix4 invertYandZMatrix = new Matrix4(new double[]{1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, -1.0f, 0.0f, 0.0f,
            0.0f, 0.0f, -1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f });

    Matrix4 startServiceTdevice = tangoPoseToMatrix(tangoPose);

    // Get device pose in OpenGL world frame
    Matrix4 openglTDevice = OPENGL_T_TANGO_WORLD.clone().multiply(startServiceTdevice);

    // Get OpenGL camera pose in OpenGL world frame
    Matrix4 openglWorldTOpenglCamera = openglTDevice.multiply(mDeviceTDepthCamera).
            multiply(DEPTH_CAMERA_T_OPENGL_CAMERA).multiply(invertYandZMatrix);

    return matrixToPose(openglWorldTOpenglCamera);
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Given a point and a normal in depth camera frame and the device pose in start of service
 * frame at the time the point and normal were measured, calculate a TangoPoseData object in
 * Tango start of service frame.
 *
 * @param point      Point in depth frame where the plane has been detected.
 * @param normal     Normal of the detected plane
 * @param tangoPose  Device pose with respect to start of service at the time the plane was
 *                   fitted
 */
public Pose planeFitToOpenGLPose(double[] point, double[] normal, TangoPoseData tangoPose) {
    if (mDeviceTDepthCamera == null) {
        throw new RuntimeException("You must call setupExtrinsics first");
    }

    Matrix4 startServiceTdevice = tangoPoseToMatrix(tangoPose);

    // Calculate the UP vector in the Depth frame at the provided measurement pose
    Vector3 depthUp = TANGO_WORLD_UP.clone();
    startServiceTdevice.clone().multiply(mDeviceTDepthCamera).inverse().rotateVector(depthUp);

    // Calculate the transform in depth frame corresponding to the plane fitting information
    Matrix4 depthTplane = matrixFromPointNormalUp(point, normal, depthUp);

    // Convert to OpenGL frame
    Matrix4 openglWorldTplane = OPENGL_T_TANGO_WORLD.clone().multiply(startServiceTdevice)
            .multiply(mDeviceTDepthCamera).multiply(depthTplane);

    return matrixToPose(openglWorldTplane);
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 *  Calculates the transform between the color camera at a time t0 and the depth camera at a
 *  time t1, given the device poses at t0 and t1.
 */
public TangoPoseData calculateColorCameraTDepthWithTime(TangoPoseData ssTdeviceT0pose,
                                                        TangoPoseData ssTdeviceT1pose) {
    if (mDeviceTDepthCamera == null) {
        throw new RuntimeException("You must call setupExtrinsics first");
    }

    // Operation at hand:
    //   rgb_t0_T_depth_t1 = rgb_T_dev * dev_t0_T_ss * ss_T_dev_t1 * dev_T_depth

    Matrix4 ssTdeviceT0 = tangoPoseToMatrix(ssTdeviceT0pose);
    Matrix4 ssTdeviceT1 = tangoPoseToMatrix(ssTdeviceT1pose);

    Matrix4 rgbt0Tdeptht1 = mDeviceTColorCamera.clone().inverse().
            multiply(ssTdeviceT0.inverse()).multiply(ssTdeviceT1).multiply(mDeviceTDepthCamera);

    return matrixToTangoPose(rgbt0Tdeptht1);
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Get the status of the Pose as a string.
 * @param pose: Pose from which status string is constructed.
 * @return
 */
public static String getStatusString(TangoPoseData pose){
    String poseStatus = "unknown";
    switch (pose.statusCode){
        case TangoPoseData.POSE_UNKNOWN:
            poseStatus = "unknown";
            break;
        case TangoPoseData.POSE_INVALID:
            poseStatus = "invalid";
            break;
        case TangoPoseData.POSE_INITIALIZING:
            poseStatus = "initializing";
            break;
        case TangoPoseData.POSE_VALID:
            poseStatus = "valid";
            break;
        default:
            poseStatus = "unknown";
    }
    return  poseStatus;
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Update the current cloud data with the provided xyzIjData from a Tango callback.
 *
 * @param from          The point cloud data
 * @param xyzIjPose     The device pose with respect to start of service at the time
 *                      the point cloud was acquired
 */
public synchronized void updateXyzIjData(TangoXyzIjData from, TangoPoseData xyzIjPose) {
    mDevicePoseAtCloudTime = xyzIjPose;

    if (mXyzIjData.xyz == null || mXyzIjData.xyz.capacity() < from.xyzCount * 3) {
        mXyzIjData.xyz = ByteBuffer.allocateDirect(from.xyzCount * 3 * 4)
                .order(ByteOrder.nativeOrder()).asFloatBuffer();
    } else {
        mXyzIjData.xyz.rewind();
    }

    mXyzIjData.xyzCount = from.xyzCount;
    mXyzIjData.timestamp = from.timestamp;

    from.xyz.rewind();
    mXyzIjData.xyz.put(from.xyz);
    mXyzIjData.xyz.rewind();
    from.xyz.rewind();
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Calculates and stores the fixed transformations between the device and the various sensors
 * to be used later for transformations between frames.
 */
private void setupExtrinsics() {
    // Create Camera to IMU Transform
    TangoCoordinateFramePair framePair = new TangoCoordinateFramePair();
    framePair.baseFrame = TangoPoseData.COORDINATE_FRAME_IMU;
    framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR;
    TangoPoseData imuTrgbPose = mTango.getPoseAtTime(0.0, framePair);

    // Create Device to IMU Transform
    framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_DEVICE;
    TangoPoseData imuTdevicePose = mTango.getPoseAtTime(0.0, framePair);

    // Create Depth camera to IMU Transform
    framePair.targetFrame = TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH;
    TangoPoseData imuTdepthPose = mTango.getPoseAtTime(0.0, framePair);

    mRenderer.setupExtrinsics(imuTdevicePose, imuTrgbPose, imuTdepthPose);
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
@Override
protected void onCreate(Bundle savedInstanceState) {
    super.onCreate(savedInstanceState);
    setContentView(R.layout.activity_area_learning);
    Intent intent = getIntent();
    mIsLearningMode = intent.getBooleanExtra(ALStartActivity.USE_AREA_LEARNING, false);
    mIsConstantSpaceRelocalize = intent.getBooleanExtra(ALStartActivity.LOAD_ADF, false);

    // Instantiate the Tango service
    mTango = new Tango(this);
    mIsRelocalized = false;
    mConfig = setTangoConfig(mTango, mIsLearningMode, mIsConstantSpaceRelocalize);
    setupTextViewsAndButtons(mConfig, mTango, mIsLearningMode, mIsConstantSpaceRelocalize);

    // Configure OpenGL renderer
    mRenderer = setupGLViewAndRenderer();

    mPoses = new TangoPoseData[3];

    // Set the number of loop closures to zero at start.
    mStart2DevicePoseCount = 0;
    mAdf2DevicePoseCount = 0;
    mAdf2StartPoseCount = 0;

    startUIThread();
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * provides a new tango service session allowing for multiple attempts to localize against
 * different ADF.
 */
private void initializeNewTangoService() {
    // Instantiate the Tango service
    mTango = new Tango(this);
    mIsRelocalized = false;

    Intent intent = getIntent();
    //auto triggers both learning and localizing
    mIsAutoMode = intent.getBooleanExtra(ALStartActivity.USE_AUTO, false);
    mIsLearningMode = intent.getBooleanExtra(ALStartActivity.USE_AREA_LEARNING,
            false) || mIsAutoMode;
    mIsConstantSpaceRelocalize = intent.getBooleanExtra(ALStartActivity.LOAD_ADF,
            false) || mIsAutoMode;
    setTangoConfig();
    mPoses = new TangoPoseData[3];
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Update the scene camera based on the provided pose in Tango start of service frame.
 * The camera pose should match the pose of the camera color at the time the last rendered RGB
 * frame, which can be retrieved with this.getTimestamp();
 * <p/>
 * NOTE: This must be called from the OpenGL render thread - it is not thread safe.
 */
public void updateRenderCameraPose(TangoPoseData cameraPose) {
    float[] rotation = cameraPose.getRotationAsFloats();
    float[] translation = cameraPose.getTranslationAsFloats();
    Quaternion quaternion = new Quaternion(rotation[3], rotation[0], rotation[1], rotation[2]);
    // Conjugating the Quaternion is need because Rajawali uses left handed convention for
    // quaternions.
    getCurrentCamera().setRotation(quaternion.conjugate());
    getCurrentCamera().setPosition(translation[0], translation[1], translation[2]);
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Set up the callback listeners for the Tango service, then begin using the Motion
 * Tracking API. This is called in response to the user clicking the 'Start' Button.
 */
private void setTangoListeners() {
    // Lock configuration and connect to Tango.
    // Select coordinate frame pair.
    final ArrayList<TangoCoordinateFramePair> framePairs =
            new ArrayList<TangoCoordinateFramePair>();
    framePairs.add(new TangoCoordinateFramePair(
            TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE,
            TangoPoseData.COORDINATE_FRAME_DEVICE));

    // Listen for new Tango data
    mTango.connectListener(framePairs, new OnTangoUpdateListener() {
        @Override
        public void onPoseAvailable(final TangoPoseData pose) {
            // We are not using TangoPoseData for this application.
        }

        @Override
        public void onXyzIjAvailable(TangoXyzIjData xyzIj) {
            // We are not using onXyzIjAvailable for this app.
        }

        @Override
        public void onPointCloudAvailable(final TangoPointCloudData pointCloudData) {
            logPointCloud(pointCloudData);
        }

        @Override
        public void onTangoEvent(final TangoEvent event) {
            // Ignoring TangoEvents.
        }

        @Override
        public void onFrameAvailable(int cameraId) {
            // We are not using onFrameAvailable for this application.
        }
    });
}
 

import com.google.atap.tangoservice.TangoPoseData; //导入依赖的package包/类
/**
 * Set up the callback listeners for the Tango service, then begin using the Motion
 * Tracking API. This is called in response to the user clicking the 'Start' Button.
 */
private void setTangoListeners() {
    // Lock configuration and connect to Tango
    // Select coordinate frame pair
    final ArrayList<TangoCoordinateFramePair> framePairs =
            new ArrayList<TangoCoordinateFramePair>();
    framePairs.add(new TangoCoordinateFramePair(
            TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE,
            TangoPoseData.COORDINATE_FRAME_DEVICE));

    // Listen for new Tango data
    mTango.connectListener(framePairs, new OnTangoUpdateListener() {
        @Override
        public void onPoseAvailable(final TangoPoseData pose) {
            logPose(pose);
        }

        @Override
        public void onXyzIjAvailable(TangoXyzIjData xyzIj) {
            // We are not using onXyzIjAvailable for this app.
        }

        @Override
        public void onPointCloudAvailable(TangoPointCloudData pointCloud) {
            // We are not using onPointCloudAvailable for this app.
        }

        @Override
        public void onTangoEvent(final TangoEvent event) {
            // Ignoring TangoEvents.
        }

        @Override
        public void onFrameAvailable(int cameraId) {
            // We are not using onFrameAvailable for this application.
        }
    });
}