CoreLocation heading base on back camera (Augmented reality)

Question

I would like to create an augmented reality view that is going to point an object in a direction. However, the CoreLocation heading is not working correctly when you are facing upwards with your camera (say to a top of a 20-storeys building when you are on the ground floor).

It is giving the opposite direction (probably the direction that is pointing by the top of the phone).

I have tried a few methods to get it to work for the direction the camera is pointing, such as:

1, +180 degree when the device orientation is > 45 degree (not accurate enough, suddenly the direction goes off by 10~20 degree)

2, attempted to calculate using CMMotionManager with formula from the below tutorial. http://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial.

3, attempted to simulate logic from android using ios deviceMotion.magneticField and deviceMotion.gravity.

4, use rotation matrix (some other post in stack overflow, but not accurate)

    double heading = M_PI + atan2(self.altitudeData.rotationMatrix.m22, self.altitudeData.rotationMatrix.m12);
    heading = heading*180/M_PI;

I am running out of idea what else could I try to get it right. I know there are some apps out there (some app that can see the sun and star) that is doing it correctly.

Answer 1

After much of research and testing. I ended up using GLKit for the calculation, as it saves me lots of trouble as well. Just leave it here for anyone who happen to get to this question.

First, I started the CMMotionManager device motion updates with CMAttitudeReferenceFrameXTrueNorthZVertical.

self.hasMotion = NO;
CMMotionManager *cmmotionManager = [[CMMotionManager alloc] init];
[cmmotionManager startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXTrueNorthZVertical
                                                     toQueue:[[NSOperationQueue alloc] init]
                                                 withHandler:^ (CMDeviceMotion *motion, NSError *error) {
                                                     self.hasMotion = YES;


                                                 }];
self.motionManager = cmmotionManager;

From some codes that I found on the web to draw an openGL world using CoreMotion rotation and mix it with getting a point from screen to 3D world:

float aspect = fabsf(self.view.bounds.size.width / self.view.bounds.size.height);
GLKMatrix4 projectionMatrix = GLKMatrix4MakePerspective(GLKMathDegreesToRadians(45.0f), aspect, 0.1f, 100.0f);

CMRotationMatrix r = self.motionManager.deviceMotion.attitude.rotationMatrix;
GLKMatrix4 camFromIMU = GLKMatrix4Make(r.m11, r.m12, r.m13, 0,
                                       r.m21, r.m22, r.m23, 0,
                                       r.m31, r.m32, r.m33, 0,
                                       0,     0,     0,     1);

GLKMatrix4 viewFromCam = GLKMatrix4Translate(GLKMatrix4Identity, 0, 0, 0);
GLKMatrix4 imuFromModel = GLKMatrix4Identity;
GLKMatrix4 viewModel = GLKMatrix4Multiply(imuFromModel, GLKMatrix4Multiply(camFromIMU, viewFromCam));
bool isInvertible;
GLKMatrix4 modelView = GLKMatrix4Invert(viewModel, &isInvertible);

int viewport[4];
viewport[0] = 0.0f;
viewport[1] = 0.0f;
viewport[2] = self.view.frame.size.width;
viewport[3] = self.view.frame.size.height;

bool success;
//assume center of the view
GLKVector3 vector3 = GLKVector3Make(self.view.frame.size.width/2, self.view.frame.size.height/2, 1.0);     
GLKVector3 calculatedPoint = GLKMathUnproject(vector3, modelView, projectionMatrix, viewport, &success);
if(success)
{
    //CMAttitudeReferenceFrameXTrueNorthZVertical always point x to true north
    //with that, -y become east in 3D world
    float angleInRadian = atan2f(-calculatedPoint.y, calculatedPoint.x);
    return angleInRadian;
}

Answer 2

To save anyone else's time, here's Chee's answer in Swift:

import GLKit    

func headingCorrectedForTilt() -> Float?{
        guard let motion = self.motionManager.deviceMotion else{
            return nil
        }

        let aspect = fabsf(Float(self.view.bounds.width / self.view.bounds.height))
        let projectionMatrix = GLKMatrix4MakePerspective(GLKMathDegreesToRadians(45.0), aspect, 0.1, 100)


        let r = motion.attitude.rotationMatrix
        let camFromIMU = GLKMatrix4Make(Float(r.m11), Float(r.m12), Float(r.m13), 0,
                           Float(r.m21), Float(r.m22), Float(r.m23), 0,
                           Float(r.m31), Float(r.m32), Float(r.m33), 0,
                           0,     0,     0,     1)

        let viewFromCam = GLKMatrix4Translate(GLKMatrix4Identity, 0, 0, 0);
        let imuFromModel = GLKMatrix4Identity
        let viewModel = GLKMatrix4Multiply(imuFromModel, GLKMatrix4Multiply(camFromIMU, viewFromCam))
        var isInvertible : Bool = false
        let modelView = GLKMatrix4Invert(viewModel, &isInvertible);
        var viewport = [Int32](count:4,repeatedValue: 0)

        viewport[0] = 0;
        viewport[1] = 0;
        viewport[2] = Int32(self.view.frame.size.width);
        viewport[3] = Int32(self.view.frame.size.height);

        var success: Bool = false
        let vector3 = GLKVector3Make(Float(self.view.frame.size.width)/2, Float(self.view.frame.size.height)/2, 1.0)
        let calculatedPoint = GLKMathUnproject(vector3, modelView, projectionMatrix, &viewport, &success)

        return success ? atan2f(-calculatedPoint.y, calculatedPoint.x) : nil
    }