Azimuth reading changes to opposite when user hold

2019-04-15 07:32发布

问题:

I have implemented the compass reading according to the usual recommendations that I could find on the web. I use the ROTATION_VECTOR sensor type and I transform it into the (azimuth, pitch, roll) triple using the standard API calls. Here's my code:

fun Fragment.receiveAzimuthUpdates(
        azimuthChanged: (Float) -> Unit,
        accuracyChanged: (Int) -> Unit
) {
    val sensorManager = activity!!.getSystemService(Context.SENSOR_SERVICE)
            as SensorManager
    val sensor = sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR)!!
    sensorManager.registerListener(OrientationListener(azimuthChanged, accuracyChanged),
            sensor, 10_000)
}

private class OrientationListener(
        private val azimuthChanged: (Float) -> Unit,
        private val accuracyChanged: (Int) -> Unit
) : SensorEventListener {
    private val rotationMatrix = FloatArray(9)
    private val orientation = FloatArray(3)

    override fun onSensorChanged(event: SensorEvent) {
        if (event.sensor.type != Sensor.TYPE_ROTATION_VECTOR) return
        SensorManager.getRotationMatrixFromVector(rotationMatrix, event.values)
        SensorManager.getOrientation(rotationMatrix, orientation)
        azimuthChanged(orientation[0])
    }

    override fun onAccuracyChanged(sensor: Sensor, accuracy: Int) {
        if (sensor.type == Sensor.TYPE_ROTATION_VECTOR) {
            accuracyChanged(accuracy)
        }
    }
}

This results in behavior that's quite good when you hold the phone horizontally, like you would a real compass. However, when you hold it like a camera, upright and in front of you, the reading breaks down. If you tilt it even slightly beyond upright, so it leans towards you, the azimuth turns to the opposite direction (sudden 180 degree rotation).

Apparently this code tracks the orientation of the phone's y-axis, which becomes vertical on an upright phone, and its ground orientation is towards you when the phone leans towards you.

What could I do to improve this behavior so it's not sensitive to the phone's pitch?

回答1:

Analysis

Apparently this code tracks the orientation of the phone's y-axis, which becomes vertical on an upright phone, and its ground orientation is towards you when the phone leans towards you.

Yes, this is correct. You can inspect the code of getOrientation() to see what's going on:

public static float[] getOrientation(float[] R, float[] values) {
    /*
     *   /  R[ 0]   R[ 1]   R[ 2]  \
     *   |  R[ 3]   R[ 4]   R[ 5]  |
     *   \  R[ 6]   R[ 7]   R[ 8]  /
     */
     values[0] = (float) Math.atan2(R[1], R[4]);
     ...

values[0] is the azimuth value you got.

You can interpret the rotation matrix R as the components of the vectors that point in the device's three major axes:

  • column 0: vector pointing to phone's right
  • column 1: vector pointing to phone's up
  • column 2: vector pointing to phone's front

The vectors are described from the perspective of the Earth's coordinate system (east, north, and sky).

With this in mind we can interpret the code in getOrientation():

  1. select the phone's up axis (matrix column 1, stored in array elements 1, 4, 7)
  2. project it to the Earth's horizontal plane (this is easy, just ignore the sky component stored in element 7)
  3. Use atan2 to deduce the angle from the remaining east and north components of the vector.

There's another subtlety hiding here: the signature of atan2 is

public static double atan2(double y, double x);

Note the parameter order: y, then x. But getOrientation passes the arguments in the east, north order. This achieves two things:

  • makes north the reference axis (in geometry it's the x axis)
  • mirrors the angle: geometrical angles are anti-clockwise, but azimuth must be the clockwise angle from north

Naturally, when the phone's up axis goes vertical ("skyward") and then beyond, its azimuth flips by 180 degrees. We can fix this in a very simple way: we'll use the phone's right axis instead. Note the following:

  • when the phone is horizontal and facing north, its right axis is aligned with the east axis. The east axis, in the Earth's coordinate system, is the "x" geometrical axis, so our 0-angle reference is correct out-of-the-box.
  • when the phone turns right (eastwards), its azimuth should rise, but its geometrical angle goes negative. Therefore we must flip the sign of the geometrical angle.

Solution

So our new formula is this:

val azimuth = -atan2(R[3], R[0])

And this trivial change is all you need! No need to call getOrientation, just apply this to the orientation matrix.

Improved Solution

So far, so good. But what if the user is using the phone in the landscape orientation? The phone's axes are unaffected, but now the user perceives the phone's "left" or "right" direction as "ahead" (depending on how the user turned the phone). We can correct for this by inspecting the Display.rotation property. If the screen is rotated, we'll use the up axis of the phone to play the same role as the right axis above.

So the full code of the orientation listener becomes this:

private class OrientationListener(
        private val activity: Activity,
        private val azimuthChanged: (Float) -> Unit,
        private val accuracyChanged: (Int) -> Unit
) : SensorEventListener {
    private val rotationMatrix = FloatArray(9)

    override fun onSensorChanged(event: SensorEvent) {
        if (event.sensor.type != Sensor.TYPE_ROTATION_VECTOR) return
        SensorManager.getRotationMatrixFromVector(rotationMatrix, event.values)
        val (matrixColumn, sense) = when (val rotation = 
                activity.windowManager.defaultDisplay.rotation
        ) {
            Surface.ROTATION_0 -> Pair(0, 1)
            Surface.ROTATION_90 -> Pair(1, -1)
            Surface.ROTATION_180 -> Pair(0, -1)
            Surface.ROTATION_270 -> Pair(1, 1)
            else -> error("Invalid screen rotation value: $rotation")
        }
        val x = sense * rotationMatrix[matrixColumn]
        val y = sense * rotationMatrix[matrixColumn + 3]
        azimuthChanged(-atan2(y, x))
    }

    override fun onAccuracyChanged(sensor: Sensor, accuracy: Int) {
        if (sensor.type == Sensor.TYPE_ROTATION_VECTOR) {
            accuracyChanged(accuracy)
        }
    }
}

With this code, you're getting the exact same behavior as on Google Maps.