Question

I want to create an augmented reality view on the iPhone. As a starting point, I took a look at Apple’s pARk demo project. There, however, the deviceMotion property is used to get the rotation matrix to do the camera transformation with. But since deviceMotion uses the gyroscope (available on the iPhone 4 and newer) and I want to support the 3GS as well (in fact, a 3GS is my only development device), I cannot use this approach. So I want to create the rotation matrix myself using the data available from the accelerometer and compass.

Unfortunately, I lack the math skills to do so myself. Searching around, it seemed to me that this is the most relevant hands-on guide for my problem, but following the implementation there doesn’t seem to adapt to my problem (the POI-views only appear momentarily and seemingly more due to device movement than to its heading; I’ve posted my onDisplayLink method (the only method with major changes) below). I’ve tried to read up on the relevant math, but at this point I simply don’t know enough about it to find an approach on my own or to find the error in my code. Any help, please?

Edit: I’ve since recognized that the sensor data should better be stored in doubles than in ints and added a bit of smoothing. Now I can see more clearly how POIs that should appear from the side upon device rotation rather come down from above. Maybe that helps pointing to what’s wrong.

CMAccelerometerData* orientation = motionManager.accelerometerData;
CMAcceleration acceleration = orientation.acceleration;

vec4f_t normalizedAccelerometer;
vec4f_t normalizedMagnetometer;

xG = (acceleration.x * kFilteringFactor) + (xG * (1.0 - kFilteringFactor));
yG = (acceleration.y * kFilteringFactor) + (yG * (1.0 - kFilteringFactor));
zG = (acceleration.z * kFilteringFactor) + (zG * (1.0 - kFilteringFactor));

xB = (heading.x * kFilteringFactor) + (xB * (1.0 - kFilteringFactor));
yB = (heading.y * kFilteringFactor) + (yB * (1.0 - kFilteringFactor));
zB = (heading.z * kFilteringFactor) + (zB * (1.0 - kFilteringFactor));

double accelerometerMagnitude = sqrt(pow(xG, 2) + pow(yG, 2) + pow(zG, 2));
double magnetometerMagnitude = sqrt(pow(xB, 2) + pow(yB, 2) + pow(zB, 2));

normalizedAccelerometer[0] = xG/accelerometerMagnitude;
normalizedAccelerometer[1] = yG/accelerometerMagnitude;
normalizedAccelerometer[2] = zG/accelerometerMagnitude;
normalizedAccelerometer[3] = 1.0f;

normalizedMagnetometer[0] = xB/magnetometerMagnitude;
normalizedMagnetometer[1] = yB/magnetometerMagnitude;
normalizedMagnetometer[2] = zB/magnetometerMagnitude;
normalizedMagnetometer[3] = 1.0f;

vec4f_t eastDirection;

eastDirection[0] = normalizedAccelerometer[1] * normalizedMagnetometer[2] - normalizedAccelerometer[2] * normalizedMagnetometer[1];
eastDirection[1] = normalizedAccelerometer[0] * normalizedMagnetometer[2] - normalizedAccelerometer[2] * normalizedMagnetometer[0];
eastDirection[2] = normalizedAccelerometer[0] * normalizedMagnetometer[1] - normalizedAccelerometer[1] * normalizedMagnetometer[0];
eastDirection[3] = 1.0f;

double eastDirectionMagnitude = sqrt(pow(eastDirection[0], 2) + pow(eastDirection[1], 2) + pow(eastDirection[2], 2));

vec4f_t normalizedEastDirection;

normalizedEastDirection[0] = eastDirection[0]/eastDirectionMagnitude;
normalizedEastDirection[1] = eastDirection[1]/eastDirectionMagnitude;
normalizedEastDirection[2] = eastDirection[2]/eastDirectionMagnitude;
normalizedEastDirection[3] = 1.0f;

vec4f_t northDirection;

northDirection[0] = (pow(normalizedAccelerometer[0], 2) + pow(normalizedAccelerometer[1],2) + pow(normalizedAccelerometer[2],2)) * xB - (normalizedAccelerometer[0] * xB + normalizedAccelerometer[1] * yB + normalizedAccelerometer[2] * zB)*normalizedAccelerometer[0];
northDirection[1] = (pow(normalizedAccelerometer[0], 2) + pow(normalizedAccelerometer[1],2) + pow(normalizedAccelerometer[2],2)) * yB - (normalizedAccelerometer[0] * xB + normalizedAccelerometer[1] * yB + normalizedAccelerometer[2] * zB)*normalizedAccelerometer[1];
northDirection[2] = (pow(normalizedAccelerometer[0], 2) + pow(normalizedAccelerometer[1],2) + pow(normalizedAccelerometer[2],2)) * zB - (normalizedAccelerometer[0] * xB + normalizedAccelerometer[1] * yB + normalizedAccelerometer[2] * zB)*normalizedAccelerometer[2];
northDirection[3] = 1.0f;

double northDirectionMagnitude;

northDirectionMagnitude = sqrt(pow(northDirection[0], 2) + pow(northDirection[1], 2) + pow(northDirection[2], 2));

vec4f_t normalizedNorthDirection;

normalizedNorthDirection[0] = northDirection[0]/northDirectionMagnitude;
normalizedNorthDirection[1] = northDirection[1]/northDirectionMagnitude;
normalizedNorthDirection[2] = northDirection[2]/northDirectionMagnitude;
normalizedNorthDirection[3] = 1.0f;

CMRotationMatrix r;
r.m11 = normalizedEastDirection[0];
r.m21 = normalizedEastDirection[1];
r.m31 = normalizedEastDirection[2];
r.m12 = normalizedNorthDirection[0];
r.m22 = normalizedNorthDirection[1];
r.m32 = normalizedNorthDirection[2];
r.m13 = normalizedAccelerometer[0];
r.m23 = normalizedAccelerometer[1];
r.m33 = normalizedAccelerometer[2];

transformFromCMRotationMatrix(cameraTransform, &r);

[self setNeedsDisplay];

When the device is placed on a table and roughly (using Compass.app) pointing to north, I log this data:

Accelerometer: x: -0.016692, y: 0.060852, z: -0.998007
Magnetometer: x: -0.016099, y: 0.256711, z: -0.966354
North Direction x: 0.011472, y: 8.561041, z:0.521807
Normalized North Direction x: 0.001338, y: 0.998147, z:0.060838
East Direction x: 0.197395, y: 0.000063, z:-0.003305
Normalized East Direction x: 0.999860, y: 0.000319, z:-0.016742

Does that appear sane?

Edit 2: I have updated the assignment of r to one that apparently leads me halfway to my goal: when the device is upright, I now see the landmarks near the horizontal plane; however, they are about 90º clock-wards off their expected location. Also, the output after the movement suggested by Beta:

Accelerometer: x: 0.074289, y: -0.997192, z: -0.009475
Magnetometer: x: 0.031341, y: -0.986382, z: -0.161458
North Direction x: -1.428996, y: -0.057306, z:-5.172881
Normalized North Direction x: -0.266259, y: -0.010678, z:-0.963842
East Direction x: 0.151658, y: -0.011698, z:-0.042025
Normalized East Direction x: 0.961034, y: -0.074126, z:-0.266305

Answer 1

After getting hold of an iPhone 4, I was able to compare the data generated by the code above with the output of the CoreMotion attitude data. With this, I found out that I should assign the values to my rotation matrix in the following manner:

CMRotationMatrix r;
r.m11 = normalizedNorthDirection[0];
r.m21 = normalizedNorthDirection[1];
r.m31 = normalizedNorthDirection[2];
r.m12 = 0 - normalizedEastDirection[0];
r.m22 = normalizedEastDirection[1];
r.m32 = 0 - normalizedEastDirection[2];
r.m13 = 0 - normalizedAccelerometer[0];
r.m23 = 0 - normalizedAccelerometer[1];
r.m33 = 0 - normalizedAccelerometer[2];

This gives roughly similar values, but of course the data produced by CoreMotion using the gyro is much better. Anyway, that’s a starting point to reasonably support the 3GS. Maybe there can be additional quality derived by some sort of filtering, but I’ve not decided yet whether that’s worth the effort.