Using quaternion instead of roll, pitch and yaw to track device motion

Question

Please bear with my long question, I am trying to make it as clear as possible.

What i am trying to do is, get the attitude(roll pitch and yaw) when a picture is taken using camera and then save the attitude values to nsuserdefaults. After saving, the orientation is changed and then try to bring the phone to the same attitude the picture was taken by constantly comparing the attitude values(saved and current).

For the purpose of interface, the user interface has 3 dots (one for each attitude parameter)on screen which guide to the correct orientation the picture was taken. On reaching the correct attitude the match flag is shown on screen.

I have been finding the roll, pitch and yaw values as:

CMQuaternion quat = self.motionManager.deviceMotion.attitude.quaternion;
myRoll = radiansToDegrees(atan2(2*(quat.y*quat.w - quat.x*quat.z), 1 - 2*quat.y*quat.y - 2*quat.z*quat.z)) ;
myPitch = radiansToDegrees(atan2(2*(quat.x*quat.w + quat.y*quat.z), 1 - 2*quat.x*quat.x - 2*quat.z*quat.z));
myYaw = radiansToDegrees(2*(quat.x*quat.y + quat.w*quat.z));

When i noticed there is some disparity in the yaw values, i searched and could find from here:link, that

yaw, pitch and roll from a quaternion you will have the same problem as if you were using just yaw, pitch and roll. You have to use quaternions EVERYWHERE in your code and forget about yaw, pitch and roll

So now i guess i have to code everything again... Please could you be so kind to point me to a sample code for using Quaternion for this purpose?

Here is the code i am working on:

In the ViewController.m, within the image:didFinishSavingWithError:

[self.motionManager startDeviceMotionUpdatesToQueue:[NSOperationQueue mainQueue] withHandler:^(CMDeviceMotion *motion, NSError *error) {

        CMQuaternion quat = self.motionManager.deviceMotion.attitude.quaternion;
        double tempYaw = radiansToDegrees(asin(2*(quat.x*quat.y + quat.w*quat.z)));
        double tempRoll = radiansToDegrees(atan2(2*(quat.y*quat.w - quat.x*quat.z), 1 - 2*quat.y*quat.y - 2*quat.z*quat.z)) ;
        double tempPitch = radiansToDegrees(atan2(2*(quat.x*quat.w + quat.y*quat.z), 1 - 2*quat.x*quat.x - 2*quat.z*quat.z));

        if (savingGyroOrientation == YES) {

            NSLog(@"Roll = %f degrees",tempRoll);
            NSLog(@"Pitch = %f degrees",tempPitch);
            NSLog(@"Yaw = %f degrees",tempYaw);

            [self.deviceStatus setDouble:tempRoll forKey:@"DeviceRoll"];
            [self.deviceStatus setDouble:tempPitch forKey:@"DevicePitch"];
            [self.deviceStatus setDouble:tempYaw forKey:@"DeviceYaw"];
            [self.deviceStatus synchronize];

            savingGyroOrientation = NO;
            checkingGyroOrientation = YES;
            self.savingLabel.hidden = YES;
            self.startTimerButton.hidden = NO;

        }
        savingGyroOrientation = NO;
        checkingGyroOrientation = YES;
        self.savingLabel.hidden = YES;
        self.startTimerButton.hidden = NO;
    }

     if (timerRunning == YES) {
         if (checkingGyroOrientation == YES) {

             self.takePicButton.hidden = YES;

             int xRoll, yPitch, xYaw, yYaw;
             // Roll Checking
             if (tempRoll >= [self.deviceStatus doubleForKey:@"DeviceRoll"]-1 && tempRoll <= [self.deviceStatus doubleForKey:@"DeviceRoll"]+1 ) {

                 [self.rollDot setFrame:CGRectMake(150, 195, 20, 20)];
                 self.rollToR.hidden = YES;
                 self.rollToL.hidden = YES;
                 self.rollDot.hidden = NO;
                 rollOk = YES;
             }else{
                 rollOk = NO;
                 self.rollDot.hidden = YES;
                 self.rollToR.hidden = NO;
                 self.rollToL.hidden = NO;

                 if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] < 0) {
                     xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]);
                     self.rollToR.hidden = YES;
                     if (xRoll <= 0) {
                         [self.rollToL setFrame:CGRectMake(0, 195, 20, 20)];
                     }else if (xRoll>= 300){
                         [self.rollToL setFrame:CGRectMake(300, 195, 20, 20)];
                     }else{
                         [self.rollToL setFrame:CGRectMake(xRoll, 195, 20, 20)];
                     }
                 }
                 if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] > 0){
                     xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]);
                     self.rollToL.hidden = YES;
                     if (xRoll <= 0) {
                         [self.rollToR setFrame:CGRectMake(0, 195, 20, 20)];
                     }else if (xRoll>=300){
                         [self.rollToR setFrame:CGRectMake(300, 195, 20, 20)];
                     }else{
                         [self.rollToR setFrame:CGRectMake(xRoll, 195, 20, 20)];
                     }
                 }
                 if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] > 180){
                     xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]-360);
                     self.rollToR.hidden = YES;
                     if (xRoll <= 0) {
                         [self.rollToL setFrame:CGRectMake(0, 195, 20, 20)];
                     }else if (xRoll>=300){
                         [self.rollToL setFrame:CGRectMake(300, 195, 20, 20)];
                     }else{
                         [self.rollToL setFrame:CGRectMake(xRoll, 195, 20, 20)];
                     }
                 }
                 if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] < -180){
                     xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]+360);
                     self.rollToL.hidden = YES;
                     if (xRoll <= 0) {
                         [self.rollToR setFrame:CGRectMake(0, 195, 20, 20)];
                     }else if (xRoll >= 300){
                         [self.rollToR setFrame:CGRectMake(300, 195, 20, 20)];
                     }else{
                         [self.rollToR setFrame:CGRectMake(xRoll, 195, 20, 20)];
                     }
                 }
             }
             //Pitch Checking
             if (tempPitch >= [self.deviceStatus doubleForKey:@"DevicePitch"]-1 && tempPitch <= [self.deviceStatus doubleForKey:@"DevicePitch"]+1) {
                 [self.pitchDot setFrame:CGRectMake(150, 195, 20, 20)];
                 self.pitchDot.hidden = NO;
                 self.pitchToDown.hidden = YES;
                 self.pitchToUp.hidden = YES;
                 pitchOk = YES;
             }else{
                 pitchOk = NO;
                 self.pitchDot.hidden = YES;
                 self.pitchToDown.hidden = NO;
                 self.pitchToUp.hidden = NO;
                 if (tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] < 0) {
                     yPitch = 195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]);
                     //                         NSLog(@"tempPitch is %0.02f Difference is %0.02f, yPitch is %0.02f",tempPitch, tempPitch-[self.deviceStatus doubleForKey:@"DevicePitch"],195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]));
                     self.pitchToDown.hidden = YES;
                     if (yPitch <= 0) {
                         [self.pitchToUp setFrame:CGRectMake(150, 0, 20, 20)];
                     }else if (yPitch >= 390) {
                         [self.pitchToUp setFrame:CGRectMake(150, 390, 20, 20)];
                     }else{
                         [self.pitchToUp setFrame:CGRectMake(150, yPitch, 20, 20)];
                     }
                 }
                 if (tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] > 0){
                     yPitch = 195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]);
                     //                         NSLog(@"tempPitch is %0.02f Difference is %0.02f, yPitch is %0.02f",tempPitch, tempPitch-[self.deviceStatus doubleForKey:@"DevicePitch"],195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]));
                     self.pitchToUp.hidden = YES;
                     if (yPitch <= 0) {
                         [self.pitchToDown setFrame:CGRectMake(150, 0, 20, 20)];
                     }else if (yPitch >= 390) {
                         [self.pitchToDown setFrame:CGRectMake(150, 390, 20, 20)];
                     }else{
                         [self.pitchToDown setFrame:CGRectMake(150, yPitch, 20, 20)];
                     }
                 }
                 if (tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] < -180){
                     yPitch = 195+tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] + 360;
                     //                         NSLog(@"tempPitch is %0.02f Difference is %0.02f, yPitch is %0.02f",tempPitch, tempPitch-[self.deviceStatus doubleForKey:@"DevicePitch"],195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]));
                     //                         NSLog(@"*yPitch is %d",yPitch);
                     self.pitchToUp.hidden = YES;
                     self.pitchToDown.hidden = NO;
                     if (yPitch <= 0 ) {
                         [self.pitchToDown setFrame:CGRectMake(150, 0, 20, 20)];
                     }else if (yPitch >= 390) {
                         [self.pitchToDown setFrame:CGRectMake(150, 390, 20, 20)];
                     }else{
                         [self.pitchToDown setFrame:CGRectMake(150, yPitch, 20, 20)];
                     }
                 }
             }
             if (tempYaw >= [self.deviceStatus doubleForKey:@"DeviceYaw"]-2 && tempYaw <= [self.deviceStatus doubleForKey:@"DeviceYaw"]+2) {

                 [self.yawDot setFrame:CGRectMake(150, 195, 20, 20)];
                 self.yawDot.hidden = NO;
                 self.rotateRight.hidden = YES;
                 self.rotateLeft.hidden = YES;
                 yawOk = YES;
             }else{
                 yawOk = NO;
                 self.yawDot.hidden = YES;
                 self.rotateRight.hidden = NO;
                 self.rotateLeft.hidden = NO;

                 if (tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"] < 0 ) {
                     xYaw = 150+(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     yYaw = 195-1.3*(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"current yaw is %0.02f Difference is %0.02f",tempYaw, tempYaw-[self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"saved Yaw is %0.02f",[self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"xYaw is %d, yYaw is %d",xYaw,yYaw);
                     self.rotateRight.hidden = YES;
                     if (xYaw <=0 && yYaw >=390) {
                         [self.rotateLeft setFrame:CGRectMake(0, 390, 20, 20)];
                     }else{
                         [self.rotateLeft setFrame:CGRectMake(xYaw, yYaw, 20, 20)];
                     }

                 }if (tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"] > 0){
                     xYaw = 150+(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     yYaw = 195-1.3*(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"current yaw is %0.02f Difference is %0.02f",tempYaw, tempYaw-[self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"saved Yaw is %0.02f",[self.deviceStatus doubleForKey:@"DeviceYaw"]);
                     NSLog(@"*xYaw is %d, yYaw is %d",xYaw,yYaw);
                     self.rotateLeft.hidden = YES;
                     if (xYaw >=300 && yYaw <=0) {
                         [self.rotateRight setFrame:CGRectMake(300, 0, 20, 20)];
                     }else{
                         [self.rotateRight setFrame:CGRectMake(xYaw, yYaw, 20, 20)];
                     }
                 }
             }

             if (rollOk == YES && pitchOk == YES && yawOk ==YES) {
                 self.orientationOkay.hidden = NO;
                 self.centerCircle.hidden = YES;
                 self.rollDot.hidden = YES;
                 self.pitchDot .hidden =YES;
                 self.yawDot.hidden = YES;
                 [self.clickTimer invalidate];
                 self.clickTimer = nil;
                 self.takePicButton.hidden = NO;
                 timerRunning = NO;
                 [self.motionManager stopDeviceMotionUpdates];
                 [self.deviceStatus removeObjectForKey:@"DeviceRoll"];
                 [self.deviceStatus removeObjectForKey:@"DevicePitch"];
                 [self.deviceStatus removeObjectForKey:@"DeviceYaw"];
                 [self.deviceStatus removeObjectForKey:@"DeviceAngle"];

             }else{

                 self.orientationOkay.hidden = YES;
                 if (flagger == YES) {
                     self.centerCircle.hidden = NO;
                 }
                 else{
                     self.centerCircle.hidden = YES;
                 }
             }
         }
     }else{
         self.rotateRight.hidden = YES;
         self.rotateLeft.hidden = YES;
         self.rollToL.hidden = YES;
         self.rollToR.hidden = YES;
         self.pitchToDown.hidden = YES;
         self.pitchToUp.hidden = YES;
         self.rollDot.hidden = NO;
         self.pitchDot .hidden =NO;
         self.yawDot.hidden = NO;
         [self.yawDot setFrame:CGRectMake(0, 390, 20, 20)];
         [self.rollDot setFrame:CGRectMake(0, 195, 20, 20)];
         [self.pitchDot setFrame:CGRectMake(150, 0, 20, 20)];

     }
     }];

Please let me know if any further details are needed on this.

Any suggestions or advice is always welcome, :) I am a noob to programming and to ios.

Thanks!!

Answer 1

I think the following things are necessary to manage to task:

1. First of all you need a good understanding of quaternions (skip this if you already made friends with them). I recommend OpenGL:Tutorials:Using Quaternions to represent rotation or The Matrix and Quaternions FAQ. It helps to bear in mind that (x, y, z) represent the axis to rotate around (not normalised) and w = cos (alpha/2) i.e. stands approximately for the amount of rotation.

2. As CMQuaternion is just a struct thus it's hard to do all the calculations. Use a full functional quaternion class instead like cocoamath (you need at least Quaternion.h, .m and QuaternionOperation.m from trunk).

3. Now the basic considerations:

   1. The difference (or sometimes stated as division) between two quaternions being defined as the angular displacement from one orientation to another might be the way to go. It is defined as
      d = a^-1 * b
      So this expresses the delta from the current position to the target position.

   2. Having this delta you need to define the conditions to met for considering the target orientation as reached. My first idea is to use the delta angle. This can be easily retrieved from the w component of the above calculated d quaternion by:
      alpha = 2 * arccos (w)
      The domain of arccos is restricted but this shouldn't be a problem in this case as we are especially interested in small values.

Maybe it's worth to emphasize that every 3D rotation has two unit quaternion representations, q and -q. This might be confusing but doesn't matter.

---

Update: So a bit of pseudo code would look something like:

CMQuaternion cmQ = attitude.quaternion;
// Get an instance of cocoamath's Quaternion for our currently reported quaternion
Quaternion current = [Quaternion initWithRe:(double)cmQ.w i:(double)cmQ.x j:(double)cmQ.y k:(double)cmQ.z];
// the complex conjugate and normalised to be on the safe side
Quaternion inverse = [current inverse];
// build the delta, assuming you have your stored direction as class member targetQuaternion
Quaternion diff = [inverse multiply:targetQuaternion];
float alpha = 2 * acos (diff.Re);
// maxDeltaAngle is your class member variable defining the angle from which you assume the position as restored (radians)
if (fabs (alpha) < maxDeltaAngle) {
    // do my fancy camera things
}

Yes it's not that trivial at the beginning. As Ali stated in his answer, visualisation is an important issue as well. My solution just solves the raw maths part.

Answer 2

I am not saying that the following is the solution to your problem, I have no idea how user-friendly this will be but I believe it is worth a shot. I see two issues: how you store the attitude and how you visualize them.

Storage. I would save the attitude either as a quaternion or as a rotation matrix; CMAttitude provides both. (I personally prefer rotation matrices to quaternions as rotation matrices are easier to understand in my opinion. It is just a personal preference.)

Visualization. You are using 3 markers to bring the phone to the same attitude as the saved one. These 3 markers came from yaw, pitch and roll, effectively ruining the stability of your application. Instead of these 3 markers I would visualize the rotation between the saved attitude and the current one. One way of doing it is to project the rotated 3 basis vectors onto the phone's screen. The nice thing about rotation matrices is that they give you exactly this without any extra computation. For example, to visualize the

| 1 0 0 |
| 0 1 0 |
| 0 0 1 |

rotation this rotation matrix represents, I only need to plot 3 arrows from [0, 0, 0] to (i) [1, 0, 0], (ii) [0, 1, 0] and (iii) [0, 0, 1]. That is, I only need to plot either the rows or the columns of the matrix (also depends on what you want whether the rows or the columns are better).

To get the rotation between a saved rotation matrix S and the current rotation matrix C you need to compute C^TS (in words: C transpose times S). You have aligned the phone with the saved attitude if your rotation matrix is the one shown above.

An excellent tutorial on rotation matrices is the Direction Cosine Matrix IMU: Theory manuscript.

Another way of visualizing the rotation between the saved and the current attitude is to transform it to angle-axis form. Then, I would project the axis to the phone's screen. The user first aligns the phone with the axis, than needs to rotate around the axis to match the saved attitude. Quaternions basically represent the axis-angle form, although in a nontrivial way.

---

It requires significant further work on your part to alter this idea according to your needs; this is definitely not a complete answer. Nevertheless, I hope this help a bit.