2D values from Quaternion

Question

i'm working in a AR application in android with the Epson Moverio BT-200. I have a quaternion that change his values with my sensor fusion algorithm. In my application i'm trying to move a 2D item changing his margin left and margin top values when I move my head. I'd like to know how can I extract, from the quaternion values, only the "horizontal" and "vertical" movements.

I could extract from the quaternion the pitch and roll values, but I read that there are several problems with euler angle. Could I do this only working with quaternions?

Answer 1

This is my actual code. I solved the problem using the Quaternions for the algorithm, and at the end I extract the euler angles from the rotation matrix.

This is the algorithm for take the values from the sensors:

private static final float NS2S = 1.0f / 1000000000.0f;

private final Quaternion deltaQuaternion = new Quaternion();

private Quaternion quaternionGyroscope = new Quaternion();

private Quaternion quaternionRotationVector = new Quaternion();

private long timestamp;

private static final double EPSILON = 0.1f;

private double gyroscopeRotationVelocity = 0;

private boolean positionInitialised = false;

private int panicCounter;

private static final float DIRECT_INTERPOLATION_WEIGHT = 0.005f;

private static final float OUTLIER_THRESHOLD = 0.85f;

private static final float OUTLIER_PANIC_THRESHOLD = 0.65f;

private static final int PANIC_THRESHOLD = 60;

@Override
public void onSensorChanged(SensorEvent event) {

    if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
        // Process rotation vector (just safe it)

        float[] q = new float[4];
        // Calculate angle. Starting with API_18, Android will provide this value as event.values[3], but if not, we have to calculate it manually.
        SensorManager.getQuaternionFromVector(q, event.values);

        // Store in quaternion
        quaternionRotationVector.setXYZW(q[1], q[2], q[3], -q[0]);
        if (!positionInitialised) {
            // Override
            quaternionGyroscope.set(quaternionRotationVector);
            positionInitialised = true;
        }

    } else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
        // Process Gyroscope and perform fusion

        // This timestep's delta rotation to be multiplied by the current rotation
        // after computing it from the gyro sample data.
        if (timestamp != 0) {
            final float dT = (event.timestamp - timestamp) * NS2S;
            // Axis of the rotation sample, not normalized yet.
            float axisX = event.values[0];
            float axisY = event.values[1];
            float axisZ = event.values[2];

            // Calculate the angular speed of the sample
            gyroscopeRotationVelocity = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);

            // Normalize the rotation vector if it's big enough to get the axis
            if (gyroscopeRotationVelocity > EPSILON) {
                axisX /= gyroscopeRotationVelocity;
                axisY /= gyroscopeRotationVelocity;
                axisZ /= gyroscopeRotationVelocity;
            }

            // Integrate around this axis with the angular speed by the timestep
            // in order to get a delta rotation from this sample over the timestep
            // We will convert this axis-angle representation of the delta rotation
            // into a quaternion before turning it into the rotation matrix.
            double thetaOverTwo = gyroscopeRotationVelocity * dT / 2.0f;
            double sinThetaOverTwo = Math.sin(thetaOverTwo);
            double cosThetaOverTwo = Math.cos(thetaOverTwo);
            deltaQuaternion.setX((float) (sinThetaOverTwo * axisX));
            deltaQuaternion.setY((float) (sinThetaOverTwo * axisY));
            deltaQuaternion.setZ((float) (sinThetaOverTwo * axisZ));
            deltaQuaternion.setW(-(float) cosThetaOverTwo);

            // Move current gyro orientation
            deltaQuaternion.multiplyByQuat(quaternionGyroscope, quaternionGyroscope);

            // Calculate dot-product to calculate whether the two orientation sensors have diverged
            // (if the dot-product is closer to 0 than to 1), because it should be close to 1 if both are the same.
            float dotProd = quaternionGyroscope.dotProduct(quaternionRotationVector);

            // If they have diverged, rely on gyroscope only (this happens on some devices when the rotation vector "jumps").
            if (Math.abs(dotProd) < OUTLIER_THRESHOLD) {
                // Increase panic counter
                if (Math.abs(dotProd) < OUTLIER_PANIC_THRESHOLD) {
                    panicCounter++;
                }

                // Directly use Gyro
                setOrientationQuaternionAndMatrix(quaternionGyroscope);

            } else {
                // Both are nearly saying the same. Perform normal fusion.

                // Interpolate with a fixed weight between the two absolute quaternions obtained from gyro and rotation vector sensors
                // The weight should be quite low, so the rotation vector corrects the gyro only slowly, and the output keeps responsive.
                Quaternion interpolate = new Quaternion();
                quaternionGyroscope.slerp(quaternionRotationVector, interpolate, DIRECT_INTERPOLATION_WEIGHT);

                // Use the interpolated value between gyro and rotationVector
                setOrientationQuaternionAndMatrix(interpolate);
                // Override current gyroscope-orientation
                quaternionGyroscope.copyVec4(interpolate);

                // Reset the panic counter because both sensors are saying the same again
                panicCounter = 0;
            }

            if (panicCounter > PANIC_THRESHOLD) {
                Log.d("Rotation Vector",
                        "Panic counter is bigger than threshold; this indicates a Gyroscope failure. Panic reset is imminent.");

                if (gyroscopeRotationVelocity < 3) {
                    Log.d("Rotation Vector",
                            "Performing Panic-reset. Resetting orientation to rotation-vector value.");

                    // Manually set position to whatever rotation vector says.
                    setOrientationQuaternionAndMatrix(quaternionRotationVector);
                    // Override current gyroscope-orientation with corrected value
                    quaternionGyroscope.copyVec4(quaternionRotationVector);

                    panicCounter = 0;
                } else {
                    Log.d("Rotation Vector",
                            String.format(
                                    "Panic reset delayed due to ongoing motion (user is still shaking the device). Gyroscope Velocity: %.2f > 3",
                                    gyroscopeRotationVelocity));
                }
            }
        }
        timestamp = event.timestamp;
    }
}



private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
    Quaternion correctedQuat = quaternion.clone();
    // We inverted w in the deltaQuaternion, because currentOrientationQuaternion required it.
    // Before converting it back to matrix representation, we need to revert this process
    correctedQuat.w(-correctedQuat.w());

    synchronized (syncToken) {
        // Use gyro only
        currentOrientationQuaternion.copyVec4(quaternion);

        // Set the rotation matrix as well to have both representations
        SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuat.ToArray());
    }
}

And this is how I take the euler angles rotation values:

     /**
     * @return Returns the current rotation of the device in the Euler-Angles
     */
    public EulerAngles getEulerAngles() {


            float[] angles = new float[3];
            float[] remappedOrientationMatrix = new float[16];
            SensorManager.remapCoordinateSystem(currentOrientationRotationMatrix.getMatrix(), SensorManager.AXIS_X,
                    SensorManager.AXIS_Z, remappedOrientationMatrix);
            SensorManager.getOrientation(remappedOrientationMatrix, angles);
            return new EulerAngles(angles[0], angles[1], angles[2]);

    }

I solved my problem with this solution. Now won't be difficult to move my 2d Object with this sensors values. Sorry for lenght of my answer, but I hope that it could be useful for someone :)