I have an app that records angles as user is walking around an object, while pointing device (preferably) at the center of the object.
Angle gets reset on user's command - so reference attitude gets reset.
Using Euler angles produces Gimbal lock, so I am currently using quaternions and calculating angles this way:
double angleFromLastPosition = acos(fromLastPositionAttitude.quaternion.w) * 2.0f;
This gives off good precision and it works perfectly IF device's pitch and yaw does not change. In other words, as the angle shows 360 degrees I end up in the same place as the start of the circle.
Problem 1: if device's yaw and pitch change slightly (user not pointing directly at center of the object), so does the angleFromLastPosition.
I understand this part, as my angle formula just shows the angle in between two device attitudes in 3D space.
Scenario:
I mark the start of rotation attitude and start moving in a circle around the object while pointing at the center
I stop at, say, 45 degrees and change pitch of the device by pointing it higher or lower. Angle changes accordingly.
What I would love to see is: angle stays at 45 degrees, even if pitch or yaw changes.
Question 1 is, how can I calculate only the Roll of the device using quaternions, and disregard changes in other two axes (at least within some reasonable number of degrees).
Problem 2: if I rotate for a bit and then freeze the device completely (on tripod so there's no shaking at all), the angleFromLastPosition drifts at a rate of 1 degree per about 10-20 seconds, and it appears not to be linear. In other words, it drifts fast at first, then slows down considerably. Sometimes I get no drift at all - angle is rock-solid if device is stationary. And this makes me lost in understanding what's going on.
Question 2, what is going on here, and how can I take care of the drift?
I went through quite a few articles and tutorials, and quaternion math is beyond me at the moment, hope someone will be able to help with a tip, link, or few lines of code.
I have tested this and it seems to work according to what you're looking for in Question 1, Andrei.
I set the homeangle initially 0, and immediately after the first pass I store the angle returned from walkaroundAngleFromAttitude:fromHomeAngle: in homeangle, for future use.
My testing included starting the device updates using a reference frame:
[_motionManager
startDeviceMotionUpdatesUsingReferenceFrame:CMAttitudeReferenceFrameXArbitraryZVertical
toQueue:operationQueue
withHandler:handler];
and using the following methods called within handler:
- (CMQuaternion) multiplyQuanternion:(CMQuaternion)left withRight:(CMQuaternion)right {
CMQuaternion newQ;
newQ.w = left.w*right.w - left.x*right.x - left.y*right.y - left.z*right.z;
newQ.x = left.w*right.x + left.x*right.w + left.y*right.z - left.z*right.y;
newQ.y = left.w*right.y + left.y*right.w + left.z*right.x - left.x*right.z;
newQ.z = left.w*right.z + left.z*right.w + left.x*right.y - left.y*right.x;
return newQ;
}
-(float)walkaroundRawAngleFromAttitude:(CMAttitude*)attitude {
CMQuaternion e = (CMQuaternion){0,0,1,1};
CMQuaternion quatConj = attitude.quaternion;
quatConj.x *= -1; quatConj.y *= -1; quatConj.z *= -1;
CMQuaternion quat1 = attitude.quaternion;
CMQuaternion quat2 = [self multiplyQuanternion:quat1 withRight:e];
CMQuaternion quat3 = [self multiplyQuanternion:quat2 withRight:quatConj];
return atan2f(quat3.y, quat3.x);
}
-(float)walkaroundAngleFromAttitude:(CMAttitude*)attitude fromHomeAngle:(float)homeangle {
float rawangle = [self walkaroundRawAngleFromAttitude:attitude];
if (rawangle <0) rawangle += M_PI *2;
if (homeangle < 0) homeangle += M_PI *2;
float finalangle = rawangle - homeangle;
if (finalangle < 0) finalangle += M_PI *2;
return finalangle;
}
This is using some modified and extended code from Finding normal vector to iOS device
Edit to deal with Question 2 & Problem 2:
This may not be solvable. I've seen it in other apps (360 pano for example) and have read about faulty readings in gyro and such. If you tried to compensate for it, of course you'll end up with a jittery experience when some authentic rotational movement gets tossed by the compensation code. So far as I've been interpreting for the last few years, this is a hardware-based issue.
Related
I'm working on an iOS app that will use CoreMotion to calculate range of motion.
I quickly abandoned Euler angles due to gimbal lock. So, now I'm trying to use quaternions.
As you probably know, CMMotionManager reports device motion data through the CMDeviceMotion class. The pertinent property here (for my purposes) is the attitude property, an instance of CMAttitude. From this, of course, you can access pitch, roll and yaw. I am still doing this when logging results just to get a better idea of the data that's coming off the device (since these values are fairly intuitive to envision). It also provides a quaternion property, which is an instance of CMQuaternion.
Based on many hours of research here and elsewhere, I was convinced that using quaternions was the correct approach to have correct results in any orientation. The problem is that I have found the CMQuaternion class/structure to be very dense and difficult to understand. Apple's documentation is sparse, and I couldn't ever back out what I considered to be a valid axis-angle representation from a CMQuaternion. (If someone has the math for calculating an axis-angle representation from a CMQuaternion, I'm all ears!)
I thought I had this issue solved when I stumbled across Apple's GLKQuaternion structure in their GLKit library. GLKit has methods that provide the axis and angle from an instance of GLKQuaternion. There's even a nice constructor method: GLKQuaternionMake(x, y, z, w).
Since CMQuaternion had x, y, z, and w properties, I reasoned that I could use this method to basically "cast" between instances of CMQuaternion and GLKQuaternion. I'm still not sure if that was correct or not.
In any case, I was logging results from my iPhone when I came across some particularly weird results.
The code that I've written has the intended purpose of capturing an initial attitude (when the user taps a button) and then sampling the CoreMotion data and determining the difference between the starting position and the current position.
Here's the code:
- (void)sampleDeviceMotion {
// I've tested this, and "self.startingAttitude" is set/reset correctly
if (self.startingAttitude == nil) {
self.startingAttitude = self.motionManager.deviceMotion.attitude;
}
CMQuaternion quaternion1 = self.startingAttitude.quaternion;
GLKQuaternion q1 = GLKQuaternionMake(quaternion1.x, quaternion1.y, quaternion1.z, quaternion1.w);
GLKVector3 v1 = GLKQuaternionAxis(q1);
float angle1 = GLKQuaternionAngle(q1);
CMQuaternion quaternion2 = self.motionManager.deviceMotion.attitude.quaternion;
GLKQuaternion q2 = GLKQuaternionMake(quaternion2.x, quaternion2.y, quaternion2.z, quaternion2.w);
GLKVector3 v2 = GLKQuaternionAxis(q2);
float angle2 = GLKQuaternionAngle(q2);
float dotProduct = GLKVector3DotProduct(v1, v2);
float length1 = GLKVector3Length(v1);
float length2 = GLKVector3Length(v2);
float cosineOfTheta = dotProduct / (length1 * length2);
float theta = acosf(cosineOfTheta);
theta = radiansToDegrees(theta);
float rotationDelta = angle2 - angle1;
rotationDelta = radiansToDegrees(rotationDelta);
printf("v1: (%.02f, %.02f, %.02f) v2: (%.02f, %.02f, %.02f) angle1: %.02f, angle2: %.02f - vectorDelta: %dº. rotationDelta: %dº (pitch: %.02f, roll: %.02f, yaw: %.02f)\n", v1.x, v1.y, v1.z, v2.x, v2.y, v2.z, angle1, angle2, (int)theta, (int)rotationDelta, self.motionManager.deviceMotion.attitude.pitch, self.motionManager.deviceMotion.attitude.roll, self.motionManager.deviceMotion.attitude.yaw);
}
I've looked that code over 20 times, and I don't see any flaws in it, unless my assumption about the ability to move between CMQuaternion and GLKQuaternion is flawed.
I would expect that when the device is lying flat on a table, tapping the button and leaving the device still would give me results where q1 and q2 are essentially identical. I would understand a slight amount of "wobble" in the data, but if the device doesn't move, the axis and angle of the different quaternions should be (almost) the same.
Sometimes I get that (when the button is tapped and the device is left still), but sometimes the "initial" (self.startingAttitude) and subsequent values are off by a lot (40º-70º).
Also, in one case where the initial and subsequent values were in line, I had a weird result when then attempting to measure a rotation.
I "spun" the phone (rotation around the "Z" axis), and go the following results:
BEFORE:
v1: (-0.03, -0.03, -1.00) v2: (0.01, -0.04, -1.00) angle1: 0.02, angle2: 0.01 - vectorDelta: 2º. rotationDelta: 0º (pitch: 0.00, roll: -0.00, yaw: -0.01)
AFTER:
v1: (-0.03, -0.03, -1.00) v2: (-0.00, -0.01, 1.00) angle1: 0.02, angle2: 0.14 - vectorDelta: 176º. rotationDelta: 7º (pitch: -0.00, roll: -0.00, yaw: 0.14)
I believe the pitch/roll/yaw data to be correct both times. That is, I imparted a small amount of yaw rotation to the phone. So, why the did Z axis flip completely with just a small change in yaw?
I want to track the motion of device along 0-180 degrees from left to right (up and downwards also) and show it on the device screen like a graph. I am using device motion sensors to draw the line but unable to find which values I need to use for.
[self.motionManager startDeviceMotionUpdatesToQueue:[NSOperationQueue currentQueue] withHandler:^(CMDeviceMotion *motion, NSError *error) {
{
CGFloat rotationRateY = motion.rotationRate.y;
CGFloat rotationRateX = motion.rotationRate.x;
if (fabs(rotationRateY) >= 0.1f || fabs(rotationRateX) >= 0.1f)
{
CMAttitude *currentAttitude = self.motionManager.deviceMotion.attitude;
newXVal = (int)round(degrees(currentAttitude.roll));
newYVal = (int)round(degrees(currentAttitude.pitch));
newZVal = (int)round(degrees(currentAttitude.yaw));
// Degrees should always be positive
if(newZVal < 0)
{
newZVal = newZVal + 360;
}
else
{
newZVal = newZVal + 180;
}
//I want to track some cgpoints to draw the line here I think but how...:(
}
}];
In the above code I am depending on yaw value to track 0 - 180 degrees but it is changing as I move the device up/down, so I couldn't use the values.
For Reference:
Thanks.
The problem with doing this is that the only information you get back from the device about movement is acceleration. (i.e. not position or speed).
You might be able to work out which direction the device is currently accelerating in. But not how far it has travelled (or how fast) in that direction.
Using this you could then put lines down.
i.e. right, up, right, down, right, diagonal up, diagonal down, right, etc...
You can get this from the userAcceleration property of CMDeviceMotion. This then has x, y, and z values.
There are many resources about why it's hard (impossible) to work out location though... This is one of the best I have found. It's the same reason that the Oculus Rift 2 has an external camera to track movement.
Good luck though :)
I'm making an app that (among other things) displays a simplified compass image that rotates according to the device's rotation. The problem is that simply doing this:
float heading = -1.0f * M_PI * trueHeading / 180.0f; //trueHeading is always between 0 and 359, never 360
self.compassNeedle.transform = CGAffineTransformMakeRotation(heading);
inside CLLocationManager's didUpdateHeading method makes the animation ugly and choppy.
I have already used Instruments to find out whether its simply my app not being able to render at more than 30-48 fps, but that's not the case.
How can I smooth out the image view's rotation so that it's more like Apple's own Compass app?
Instead of using the current instant value, try using the average of the last N values for the true heading. The value may be jumping around a lot in a single instant but settle down "in the average".
Assuming you have a member variable storedReadings which is an NSMutableArray:
-(void)addReading(float):newReading
{
[storedReadings addObject:[NSNumber numberWithFloat:newReading]];
while([storedReadings count] > MAX_READINGS)
{
[storedReadings removeObjectAtIndex:0];
}
}
then when you need the average value (timer update?)
-(float)calcReading
{
float result = 0.0f;
if([storedReadings count] > 0)
{
foreach(NSNumber* reading in storedReadings)
{
result += [reading floatValue];
}
result /= [storedReadings count];
}
return result;
}
You get to pick MAX_READINGS a priori.
NEXT LEVEL(S) UP
If the readings are not jumping around so much but the animation is still choppy, you probably need to do something like a "smooth" rotation. At any given time, you have the current angle you are displaying, theta (store this in your class, start it out at 0). You also have your target angle, call it target. This is the value you get from the smoothed calcReading function. The error is defined as the difference between the two:
error = target-theta;
Set up a timer callback with a period of something like 0.05 seconds (20x per second). What you want to do is adjust theta so that the error is driven towards 0. You can do this in a couple of ways:
thetaNext += kProp * (target - theta); //This is proportional feedback.
thetaNext += kStep * sign(target-theta); // This moves theta a fixed amount each update. sign(x) = +1 if x >= 0 and -1 if x < 0.
The first solution will cause the rotation to change sharply the further it is from the target. It will also probably oscillate a little bit as it swings past the "zero" point. Bigger values of kProp will yield faster response but also more oscillation. Some tuning will be required.
The second solution will be much easier to control...it just "ticks" the compass needle around each time. You can set kStep to something like 1/4 degree, which gives you a "speed" of rotation of about (1/4 deg/update) * (20 updates/seconds) = 5 degrees per second. This is a bit slow, but you can see the math and change kStep to suit your needs. Note that you may to "band" the "error" value so that no action is taken if the error < kStep (or something like that). This prevents your compass from shifting when the angle is really close to the target. You can change kStep when the error is small so that it "slides" into the ending position (i.e. kStep is smaller when the error is small).
For dealing with Angle Issues (wrap around), I "normalize" the angle so it is always within -Pi/Pi. I don't guarantee this is the perfect way to do it, but it seems to get the job done:
// Takes an angle greater than +/- M_PI and converts it back
// to +/- M_PI. Useful in Box2D where angles continuously
// increase/decrease.
static inline float32 AdjustAngle(float32 angleRads)
{
if(angleRads > M_PI)
{
while(angleRads > M_PI)
{
angleRads -= 2*M_PI;
}
}
else if(angleRads < -M_PI)
{
while(angleRads < -M_PI)
{
angleRads += 2*M_PI;
}
}
return angleRads;
}
By doing it this way, -pi is the angle you reach from going in either direction as you continue to rotate left/right. That is to say, there is not a discontinuity in the number going from say 0 to 359 degrees.
SO PUTTING THIS ALL TOGETHER
static inline float Sign(float value)
{
if(value >= 0)
return 1.0f;
return -1.0f;
}
//#define ROTATION_OPTION_1
//#define ROTATION_OPTION_2
#define ROTATION_OPTION_3
-(void)updateArrow
{
// Calculate the angle to the player
CGPoint toPlayer = ccpSub(self.player.position,self.arrow.position);
// Calculate the angle of this...Note there are some inversions
// and the actual image is rotated 90 degrees so I had to offset it
// a bit.
float angleToPlayerRads = -atan2f(toPlayer.y, toPlayer.x);
angleToPlayerRads = AdjustAngle(angleToPlayerRads);
// This is the angle we "wish" the arrow would be pointing.
float targetAngle = CC_RADIANS_TO_DEGREES(angleToPlayerRads)+90;
float errorAngle = targetAngle-self.arrow.rotation;
CCLOG(#"Error Angle = %f",errorAngle);
#ifdef ROTATION_OPTION_1
// In this option, we just set the angle of the rotated sprite directly.
self.arrow.rotation = CC_RADIANS_TO_DEGREES(angleToPlayerRads)+90;
#endif
#ifdef ROTATION_OPTION_2
// In this option, we apply proportional feedback to the angle
// difference.
const float kProp = 0.05f;
self.arrow.rotation += kProp * (errorAngle);
#endif
#ifdef ROTATION_OPTION_3
// The step to take each update in degrees.
const float kStep = 4.0f;
// NOTE: Without the "if(fabs(...)) check, the angle
// can "dither" around the zero point when it is very close.
if(fabs(errorAngle) > kStep)
{
self.arrow.rotation += Sign(errorAngle)*kStep;
}
#endif
}
I put this code into a demo program I had written for Cocos2d. It shows a character (big box) being chased by some monsters (smaller boxes) and has an arrow in the center that always points towards the character. The updateArrow call is made on a timer tick (the update(dt) function) regularly. The player's position on the screen is set by the user tapping on the screen and the angle is based on the vector from the arrow to the player. In the function, I show all three options for setting the angle of the arrow:
Option 1
Just set it based on where the player is (i.e. just set it).
Option 2
Use proportional feedback to adjust the arrow's angle each time step.
Option 3
Step the angle of the arrow each timestep a little bit if the error angle is more than the step size.
Here is a picture showing roughly what it looks like:
And, all the code is available here on github. Just look in the HelloWorldLayer.m file.
Was this helpful?
I'm having trouble setting up vectors for an object in my code. I tried modeling my code similarly to the answer in this question: Game enemy move towards player except that I'm using GLKVector2's. I thought my implementation seemed correct, but it's really only my first time using vectors with GLKit and in general I haven't used them too much before.
My current code looks something like:
GLKVector2 vector = GLKVector2Make(self.player.position.x - self.target.position.x, self.player.position.y - self.target.position.y);
float hypo = sqrt(vector.x*vector.x + vector.y*vector.y);
float speed = 0.25;
vector = GLKVector2Make(vector.x/hypo, vector.y/hypo);
vector = GLKVector2MultiplyScalar(vector, speed);
GLKVector2 sum = GLKVector2Add(vector, self.target.position);
self.target.moveVelocity = sum;
Is it possible that my logic just isn't correct here? I'd appreciate any help or suggestions. Thanks!
EDIT: just for clarification since I didn't really explain what happens.. Basically the "enemy" shapes either stutter/jump or just stick. They aren't moving toward the other object at all.
EDIT 2:
If I try using GLKVector2Normalize, then nothing moves. If I do something like:
GLKVector2 vector = GLKVector2Make(self.player.position.x - self.target.position.x, self.player.position.y - self.target.position.y);
float speed = 0.10;
// float distance = GLKVector2Distance(self.player.position, self.target.position);
// vector = GLKVector2Normalize(vector);
vector = GLKVector2MultiplyScalar(vector, speed);
self.target.moveVelocity = vector;
Then the movement works toward the player object, but only updates the one time even though it should be updating every second.
Two things:
There's no need to calculate the magnitude of the vector and divide yourself -- GLKit has a GLKVector2Normalize function, which takes a vector and returns the vector in the same direction with length 1. You can then use GLKVector2MultiplyScalar (as you do) to change the speed.
Your target's velocity should be set to vector, not sum, assuming that in the target's update method (which you should call once per timestep), you add self.moveVelocity.x to self.position.x and self.moveVelocity.y to self.position.y each timestep. As it is now, your sum variable will hold the position that your target should have one timestep in the future, not its velocity.
I'm using a SneakyJoystick in Cocos2d, and I'm trying to get a sprite to rotate to face the same direction as the joystick is pointed (this is top down).
I can get it to rotate to face it, but it snaps into position as the rotation is updated every frame or so.
How can I make the sprite rotate smoothly towards the target angle, without jumping to it? I wasn't able to figure out how to do this with a CCRotateTo because the angle to rotate towards could change at any time.
I ended up fixing this simply by using a rotation method that I made, which rotates the node/sprite in the correct direction at the correct amount each update.
- (void)rotateNode:(CCNode*)aNode degrees:(float)targetRotation withSpeed:(float)rotationSpeed withTimeDelta:(ccTime)dt
{
rotationSpeed = rotationSpeed * dt;
// Convert the difference between the two angles to radians
float diff = (targetRotation - aNode.rotation) * (M_PI / 180);
// Find the rotation of the vector created by the sin and cos of the difference
float rotationDifference = atan2f(sinf(diff),cosf(diff));
// Rotate the clip accordingly
aNode.rotation += MAX(
MIN(
(180/M_PI) * rotationDifference,rotationSpeed), -rotationSpeed
);
}
Have you tried:
[CCRotateBy actionWithDuration:0.5f angle:CC_DEGREES_TO_RADIANS(90.0f)];
Obtain the Angle between the last Update the current Update, also if you wanted it so the character had a set time to turn around, you can scale your duration by the amount of the angle.