how can i get the heading of the device with CMDeviceMotion in iOS 5

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.

Weighting

I can't seem to find method multiplyMatrixAndVector. Can you tell me here is it? – Lyall

That code is in the apple's pARk code. You can find in many other libraries it's very common. – Weighting

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.

Romaic

Could you please make it clearer on how to use the matrix(the one you said having funky brackets) and on how to make the switching to heading based on Z-axis? I would like the app to work on all iPhone positions. Thanks in advance. – Chronic

The rotation manager is returning a rotation matrix. The code is using that rotation matrix to rotate a vector pointed along the Y axis, then looking at the direction of the resulting vector in the X-Y plane. – Romaic

uhm... why do you use atan2(rm.m32, rm.m12) to get the rotation in the X-Z plane? shouldn't it be atan2(-rm.m33, -rm.m13)? – Simoniac

@roberto.buratti: Yes, you're right. Thanks for pointing that out; I made the edit. – Romaic

OK but we have another problem: this solution does not work when the device is in landscape or face-down (no need to be face-down, it stops working as soon as the camera is looking over the horizon). Any suggestions? – Simoniac

This solution is locating the heading of a three-dimensional object in two-dimensional space. There is really no such thing as a 2D heading that works for an object that is free to rotate arbitrarily in three dimensions. We made it work by assuming it was held "flat", for some definition of flat. Two were shown. If the device will not be held flat, you can either detect the orientation of the device and use one of three different 2D headings, as appropriate for your needs, or use the true 3D orientation. – Romaic

@roberto: Could you find out which proper formula to use when device was "face-down" ? I would really be thankful. – Novel