com.google.atap.tangoservice.TangoPoseData Java Examples

/**
 * 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;
}
 

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);
}
 

/**
 * 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);
}
 

/**
 * 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();
}
 

/**
 * 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;
}
 

/**
 * Get quaternion string from a pose.
 *
 * @param pose          Pose from which quaternion string is constructed.
 * @param decimalFormat Number of decimals for each component of translation.
 * @return
 */
public static String getQuaternionString(TangoPoseData pose, DecimalFormat decimalFormat) {
    String quaternionString = "["
            + decimalFormat.format(pose.rotation[0]) + ", "
            + decimalFormat.format(pose.rotation[1]) + ", "
            + decimalFormat.format(pose.rotation[2]) + ", "
            + decimalFormat.format(pose.rotation[3]) + "] ";
    return quaternionString;
}
 

protected void connectRenderer() {
    renderer.getCurrentScene().registerFrameCallback(new ScenePreFrameCallbackAdapter() {
        @Override
        public void onPreFrame(long sceneTime, double deltaTime) {
            synchronized (MainActivity.this) {
                if (!tangoIsConnected.get()) {
                    return;
                }
                if (!renderer.isSceneCameraConfigured()) {
                    renderer.setProjectionMatrix(intrinsics);
                }
                if (connectedTextureId != renderer.getTextureId()) {
                    tango.connectTextureId(ACTIVE_CAMERA_INTRINSICS, renderer.getTextureId());
                    connectedTextureId = renderer.getTextureId();
                }
                if (tangoFrameIsAvailable.compareAndSet(true, false)) {
                    rgbFrameTimestamp = tango.updateTexture(ACTIVE_CAMERA_INTRINSICS);
                }
                if (rgbFrameTimestamp > cameraPoseTimestamp) {
                    TangoPoseData currentPose = getCurrentPose();
                    if (currentPose != null && currentPose.statusCode == TangoPoseData.POSE_VALID) {
                        renderer.updateRenderCameraPose(currentPose, extrinsics);
                        cameraPoseTimestamp = currentPose.timestamp;
                    }
                }
            }
        }
    });
}
 

public void setEndPoint(TangoPoseData currentPose, DeviceExtrinsics extrinsics) {
    endPoint = ScenePoseCalculator.toOpenGlCameraPose(currentPose, extrinsics);
    floorPlan.addPoint(endPoint.getPosition());
    if (startPoint != null && endPoint != null) {
        fillPath = true;
    }
}
 

public void setStartPoint(TangoPoseData currentPose, DeviceExtrinsics extrinsics) {
    startPoint = ScenePoseCalculator.toOpenGlCameraPose(currentPose, extrinsics);
    floorPlan.addPoint(startPoint.getPosition());
    if (startPoint != null && endPoint != null) {
        fillPath = true;
    }
}
 

/**
 * Update the scene camera based on the provided pose in Tango start of service frame.
 * The device pose should match the pose of the device at the time the last rendered RGB
 * frame, which can be retrieved with this.getTimestamp();
 * NOTE: This must be called from the OpenGL render thread - it is not thread safe.
 */
public void updateRenderCameraPose(TangoPoseData devicePose, DeviceExtrinsics extrinsics) {
    Pose cameraPose = ScenePoseCalculator.toOpenGlCameraPose(devicePose, extrinsics);
    getCurrentCamera().setRotation(cameraPose.getOrientation());
    getCurrentCamera().setPosition(cameraPose.getPosition());
    floorPlan.setTrajectoryPosition(cameraPose.getPosition());
}
 

/**
 * Converts from TangoPoseData to a Matrix4 for transformations.
 */
public static Matrix4 tangoPoseToMatrix(TangoPoseData tangoPose) {
    Vector3 v = new Vector3(tangoPose.translation[0],
            tangoPose.translation[1], tangoPose.translation[2]);
    Quaternion q = new Quaternion(tangoPose.rotation[3], tangoPose.rotation[0],
            tangoPose.rotation[1], tangoPose.rotation[2]);
    // NOTE: Rajawali quaternions use a left-hand rotation around the axis convention.
    q.conjugate();
    Matrix4 m = new Matrix4();
    m.setAll(v, new Vector3(1, 1, 1), q);
    return m;
}
 

/**
 * Get translation string from a pose.
 *
 * @param pose          Pose from which translation string is constructed.
 * @param decimalFormat Number of decimals for each component of translation.
 * @return
 */
public static String getTranslationString(TangoPoseData pose, DecimalFormat decimalFormat) {
    String translationString = "["
            + decimalFormat.format(pose.translation[0]) + ", "
            + decimalFormat.format(pose.translation[1]) + ", "
            + decimalFormat.format(pose.translation[2]) + "] ";
    return translationString;
}
 

public DeviceExtrinsics(TangoPoseData imuTDevicePose, TangoPoseData imuTColorCameraPose,
                        TangoPoseData imuTDepthCameraPose) {
    Matrix4 deviceTImu = ScenePoseCalculator.tangoPoseToMatrix(imuTDevicePose).inverse();
    Matrix4 imuTColorCamera = ScenePoseCalculator.tangoPoseToMatrix(imuTColorCameraPose);
    Matrix4 imuTDepthCamera = ScenePoseCalculator.tangoPoseToMatrix(imuTDepthCameraPose);
    mDeviceTDepthCamera = deviceTImu.clone().multiply(imuTDepthCamera);
    mDeviceTColorCamera = deviceTImu.multiply(imuTColorCamera);
}
 

/**
 * Given the device pose in start of service frame, calculate the position and orientation of
 * the depth sensor in OpenGL coordinate frame.
 */
public static Pose toDepthCameraOpenGlPose(TangoPoseData devicePose,
                                           DeviceExtrinsics extrinsics) {
    Matrix4 startServiceTdevice = tangoPoseToMatrix(devicePose);

    // 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(extrinsics.getDeviceTDepthCamera());

    return matrixToPose(openglWorldTOpenglCamera);
}
 

/**
 * Given the device pose in start of service frame, calculate the corresponding
 * position and orientation for a OpenGL Scene Camera in the Rajawali world.
 */
public static Pose toOpenGlCameraPose(TangoPoseData devicePose, DeviceExtrinsics extrinsics) {
    Matrix4 startServiceTdevice = tangoPoseToMatrix(devicePose);

    // 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(extrinsics.getDeviceTColorCamera()).
                    multiply(COLOR_CAMERA_T_OPENGL_CAMERA);

    return matrixToPose(openglWorldTOpenglCamera);
}
 

/**
 * Given a pose in start of service or area description frame and a screen rotaion calculate
 * the corresponding position and orientation for a 3D object in the Rajawali world.
 *
 * @param tangoPose     The input Tango Pose in start service or area description frame.
 * @param rotationIndex The screen rotation index, the index is following Android rotation enum.
 *                      see Android documentation for detail:
 *                      http://developer.android.com/reference/android/view/Surface.html#ROTATION_0 // NO_LINT
 */
public static Pose toOpenGLPoseWithScreenRotation(TangoPoseData tangoPose, int rotationIndex) {
    Matrix4 startServiceTDevice = tangoPoseToMatrix(tangoPose);

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

    switch (rotationIndex) {
        case 0:
            openglWorldTDevice.multiply(ROTATION_0_T_DEFAULT);
            break;
        case 1:
            openglWorldTDevice.multiply(ROTATION_90_T_DEFAULT);
            break;
        case 2:
            openglWorldTDevice.multiply(ROTATION_180_T_DEFAULT);
            break;
        case 3:
            openglWorldTDevice.multiply(ROTATION_270_T_DEFAULT);
            break;
        default:
            openglWorldTDevice.multiply(ROTATION_0_T_DEFAULT);
            break;
    }

    return matrixToPose(openglWorldTDevice);
}
 

/**
 * Given a pose in start of service or area description frame calculate the corresponding
 * position and orientation for a 3D object in the Rajawali world.
 */
public static Pose toOpenGLPose(TangoPoseData tangoPose) {
    Matrix4 startServiceTDevice = tangoPoseToMatrix(tangoPose);

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

    return matrixToPose(openglWorldTDevice);
}
 

@Override
public void onPoseAvailable(TangoPoseData pose) {
    if (tangoUx != null) {
        tangoUx.updatePoseStatus(pose.statusCode);
    }
}
 

public TangoPoseData getCurrentPose() {
    return tango.getPoseAtTime(rgbFrameTimestamp, SOS_T_DEVICE_FRAME_PAIR);
}
 

private void saveScanData(TangoPoseData pose, final TangoXyzIjData xyzIj) throws IOException {
        saveScan = false;

        Log.i(TAG, "Saving");
        if (!isExternalStorageWritable()) {
            Log.e(TAG, "External storage unavailable");
            return;
        }
        File dir = getScanStorageDir();
        if (dir == null) {
            Log.e(TAG, "Failed to create scan directory");
            return;
        }
        String filename = String.format("Scan%05d.data", scanNumber);
        scanNumber += 1;
        File file = new File(dir, filename);
        Log.i(TAG, "Writing data to "+file.getAbsolutePath());

        // Set up buffers
        BufferedOutputStream out = new BufferedOutputStream(new FileOutputStream(file));
        ByteBuffer buf = ByteBuffer.allocate(4*8);
        buf.order(ByteOrder.LITTLE_ENDIAN);

        // Write translation
        for (int i=0; i<pose.translation.length; i++) {
            buf.putDouble(pose.translation[i]);
        }
        out.write(buf.array(), 0, 3*8);

        // Write Rotation
        buf.clear();
        for (int i=0; i<pose.rotation.length; i++) {
            buf.putDouble(pose.rotation[i]);
        }
        out.write(buf.array(), 0, 4 * 8);

        // Write field of view
        TangoCameraIntrinsics intrinsics = mTango.getCameraIntrinsics(TangoCameraIntrinsics.TANGO_CAMERA_COLOR);
        double vFOV = 2*Math.atan(0.5*intrinsics.height/intrinsics.fy);
        double hFOV = 2*Math.atan(0.5*intrinsics.width/intrinsics.fx);
        buf.clear();
        buf.putDouble(hFOV);
        buf.putDouble(vFOV);
        out.write(buf.array(), 0, 16);

        // Write points
        buf.clear();
        buf.putInt(xyzIj.xyzCount);
        out.write(buf.array(), 0, 4);
        Log.i(TAG, "xyzCount="+Integer.toString(xyzIj.xyzCount));
        for (int i=0; i<10; i++) {
            Log.i(TAG, Float.toString(xyzIj.xyz.get(i*3))+", "+Float.toString(xyzIj.xyz.get(i*3+1))+", "+Float.toString(xyzIj.xyz.get(i*3+2)));
        }
        for (int i=0; i<xyzIj.xyzCount*3; i++) {
            buf.clear();
            buf.putFloat(xyzIj.xyz.get(i));
            out.write(buf.array(), 0, 4);
        }

        // Write IJ (Not yet implemented in Tango)
        buf.clear();
        int ijCount = xyzIj.ijRows*xyzIj.ijCols;
        Log.i(TAG, "IJCount="+Integer.toString(ijCount));
        Log.i(TAG, "ijRows="+Integer.toString(xyzIj.ijRows)+" ijCols="+Integer.toString(xyzIj.ijCols));
        ijCount=0;
        buf.putInt(ijCount);
        out.write(buf.array(), 0, 4);
//        if (ijCount>0) {
//            byte[] ijBuffer = new byte[ijCount * 4];
//            FileInputStream ijStream = new FileInputStream(
//                    xyzIj.ijParcelFileDescriptor.getFileDescriptor());
//            try {
//                ijStream.read(ijBuffer, 0, ijCount * 4);
//                ijStream.close();
//            } catch (IOException e) {
//                e.printStackTrace();
//            }
//            out.write(ijBuffer);
//        }
        out.close();

        MediaScannerConnection.scanFile(
                getApplicationContext(),
                new String[] {file.getAbsolutePath()},
                null,
                new MediaScannerConnection.OnScanCompletedListener() {
                    @Override
                    public void onScanCompleted(String path, Uri uri) {
                        Log.v(TAG, "file " + path + " was scanned successfully: " + uri);
                    }
                });

        Log.i(TAG, "Done");
    }