Question

I’m developing an AR app using the gyro. I have use an apple code example pARk. It use the rotation matrix to calculate the position of the coordinate and it do really well, but now I’m trying to implement a “radar” and I need to rotate this in function of the device heading. I’m using the CLLocationManager heading but it’s not correct.

The question is, how can I get the heading of the device using the CMAttitude to reflect exactly what I get in the screen??

I’m new with rotation matrix and that kind of things.

This is part of the code used to calculate the AR coordinates. Update the cameraTransform with the attitude:

CMDeviceMotion *d = motionManager.deviceMotion;
if (d != nil) {
    CMRotationMatrix r = d.attitude.rotationMatrix;
    transformFromCMRotationMatrix(cameraTransform, &r);
[self setNeedsDisplay];
}

and then in the drawRect code:

mat4f_t projectionCameraTransform;
multiplyMatrixAndMatrix(projectionCameraTransform, projectionTransform, cameraTransform);

int i = 0;
for (PlaceOfInterest *poi in [placesOfInterest objectEnumerator]) {
    vec4f_t v;
    multiplyMatrixAndVector(v, projectionCameraTransform, placesOfInterestCoordinates[i]);

    float x = (v[0] / v[3] + 1.0f) * 0.5f;
    float y = (v[1] / v[3] + 1.0f) * 0.5f;

I also rotate the view with the pitch angle.
The motions updates are started using the north:

[motionManager startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXTrueNorthZVertical];

So I think that must be possible to get the “roll”/heading of the device in any position (with any pitch and yaw…) but I don’t know how.

Answer 1

There are a few ways to calculate heading from the rotation matrix returned by CMDeviceMotion. This assumes you use the same definition of Apple’s compass, where the +y direction (top of the iPhone) pointing due north returns a heading of 0, and rotating the iPhone to the right increases the heading, so East is 90, South is 180, and so forth.

First, when you start updates, be sure to check to make sure headings are available:

if (([CMMotionManager availableAttitudeReferenceFrames] & CMAttitudeReferenceFrameXTrueNorthZVertical) != 0) {
   ...
}

Next, when you start the motion manager, ask for attitude as a rotation from X pointing true North (or Magnetic North if you need that for some reason):

[motionManager startDeviceMotionUpdatesUsingReferenceFrame: CMAttitudeReferenceFrameXTrueNorthZVertical
                                                   toQueue: self.motionQueue
                                               withHandler: dmHandler];

When the motion manager reports a motion update, you want to find out how much the device has rotated in the X-Y plane. Since we are interested in the top of the iPhone, we’ll pick a point in that direction and rotate it using the returned rotation matrix to get the point after rotation:

   [m11 m12 m13] [0]   [m12]
   [m21 m22 m23] [1] = [m22]
   [m31 m32 m33] [0]   [m32]

The funky brackets are matrices; it’s the best I can do using ASCII. 🙂

The heading is the angle between the rotated point and true North. We can use the X and Y coordinates of the rotated point to extract the arc tangent, which gives the angle between the point and the X axis. This is actually 180 degrees off from what we want, so we have to adjust accordingly. The resulting code looks like this:

CMDeviceMotionHandler dmHandler = ^(CMDeviceMotion *aMotion, NSError *error) {
    // Check for an error.
    if (error) {
        // Add error handling here.
    } else {
        // Get the rotation matrix.
        CMAttitude *attitude = self.motionManager.deviceMotion.attitude;
        CMRotationMatrix rm = attitude.rotationMatrix;

        // Get the heading.
        double heading = PI + atan2(rm.m22, rm.m12);
        heading = heading*180/PI;
        printf("Heading: %5.0f\n", heading);
    }
};

There is one gotcha: If the top of the iPhone is pointed straight up or straight down, the direction is undefined. The result is m21 and m22 are zero, or very close to it. You need to decide what this means for your app and handle the condition accordingly. You might, for example, switch to a heading based on the -Z axis (behind the iPhone) when m12*m12 + m22*m22 is close to zero.

---

This all assumes you want to rotate about the X-Y plane, as Apple usually does for their compass. It works because you are using the rotation matrix returned by the motion manager to rotate a vector pointed along the Y axis, which is this matrix:

[0]
[1]
[0]

To rotate a different vector–say, one pointed along -Z–use a different matrix, like

[0]
[0]
[-1]

Of course, you also have to take the arc tangent in a different plane, so instead of

double heading = PI + atan2(rm.m22, rm.m12);

you would use

double heading = PI + atan2(-rm.m33, -rm.m13);

to get the rotation in the X-Z plane.