Using quaternion instead of roll, pitch and yaw to track device motion
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
I think the following things are necessary to manage to task:
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.
As
CMQuaternionis 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).Now the basic considerations:
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.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.