I am having a hard time trying to understand the mathematical derivation of the equations I have included below. This piece of code is part of an example from a SparkFun IMU library that can be found here.
Could someone please help me understand the theory behind the use of arctan functions to estimate roll, pitch, and yaw? And how is the magnetometer data used to get the right yaw estimate? All the resources I could find online didn't answer these questions in a way that I could understand.
float roll = atan2(ay, az);
float pitch = atan2(-ax, sqrt(ay * ay + az * az));
float heading;
if (my == 0)
heading = (mx < 0) ? PI : 0;
else
heading = atan2(mx, my);
heading -= EARTH_DECLINATION * PI / 180;
if (heading > PI) heading -= (2 * PI);
else if (heading < -PI) heading += (2 * PI);
// Convert everything from radians to degrees:
heading *= 180.0 / PI;
pitch *= 180.0 / PI;
roll *= 180.0 / PI;
Assuming ay and az are the offset from origin given by the magnetometer then atan(ay, az) will give you the angle that produced that offset.
The sqrt(ay * ay + az * az) follows by the pythagorean theorem to give you the length of the third side of the "offset triangle" to be able to calculate pitch. The -ax comes from how pitch is defined.
atan2(y,x) is the angle theta as in the following diagram:
In three dimensions you have three planes and the atan2() is applied to a pair from x, y, and z depending on which plane you are calculating theta for (roll, pitch, yaw).
When at a steady velocity, (i.e. not changing speed or direction) the values ax, ay, az each measure the gravitational component of acceleration only. The value will be inaccurate under acceleration. You have to be a bit cleverer at that point by combining information from other sensors such as the gyro or magnetometer and the other accelerators - that is when stationary ax, ay, az sum to 1G - anything else and there is an additional acceleration in effect. An accelerator measures acceleration, but that includes acceleration due to gravity. That is when at a steady velocity, an accelerator is a tilt sensor relative to gravity.
The magnetometer calculation is determining an angle relative to north from its x and y components and compensating for the difference between true north and magnetic north (the magnetic declination). The x an y components of the magnetometer are at a maximum when they are aligned with a magnetic field, because they are mounted orthogonality, their relative values resolve to a single direction using atan2(mx,my). x and y are swapped from the conventional order because compass directions increase clockwise, while mathematically angles increase counter-clockwise.
In practice you need to use sensor function to combine information from the gyro (angular velocity), accelerator, and magnetometer and probably apply some heuristics too to track motion accurately. Each of these sensors have different confounding factors and some element of measurement overlap in the sense that a single external force can have an effect on more than one sensor. This can be used to discriminate different kinds of motion and attitude. It is complicated, which is probably why this does does not attempt to deal with it.
Related
I am trying to convert the orientation of an OpenVR controller that I have stored as a glm::vec3 of Euler angles into a glm::fquat and back, but I get wildly different results and the in-game behavior is just wrong (hard to explain, but the orientation of the object behaves normally for a small range of angles, then flips in weird axes).
This is my conversion code:
// get `orientation` from OpenVR controller sensor data
const glm::vec3 eulerAnglesInDegrees{orientation[PITCH], orientation[YAW], orientation[ROLL]};
debugPrint(eulerAnglesInDegrees);
const glm::fquat quaternion{glm::radians(eulerAnglesInDegrees)};
const glm::vec3 result{glm::degrees(glm::eulerAngles(quaternion))};
debugPrint(result);
// `result` should represent the same orientation as `eulerAnglesInDegrees`
I would expect eulerAnglesInDegrees and result to either be the same or equivalent representations of the same orientation, but that is apparently not the case. These are some example values I get printed out:
39.3851 5.17816 3.29104
39.3851 5.17816 3.29104
32.7636 144.849 44.3845
-147.236 35.1512 -135.616
39.3851 5.17816 3.29104
39.3851 5.17816 3.29104
32.0103 137.415 45.1592
-147.99 42.5846 -134.841
As you can see above, for some orientation ranges the conversion is correct, but for others it is completely different.
What am I doing wrong?
I've looked at existing questions and attempted a few things, including trying out every possible rotation order listed here, conjugating the quaternion, and other random things like flipping pitch/yaw/roll. Nothing gave me the expected result.
How can I convert euler angles to quaternions and back, representing the original orientation, using glm?
Some more examples of discrepancies:
original: 4; 175; 26;
computed: -175; 4; -153;
difference: 179; 171; 179;
original: -6; 173; 32;
computed: 173; 6; -147;
difference: -179; 167; 179;
original: 9; 268; -46;
computed: -170; -88; 133;
difference: 179; 356; -179;
original: -27; -73; 266;
computed: -27; -73; -93;
difference: 0; 0; 359;
original: -33; 111; 205;
computed: 146; 68; 25;
difference: -179; 43; 180;
I tried to find a pattern to fix the final computed results, but it doesn't seem like there's one easy to identify.
GIF + video of the behavior:
Full video on YouTube
Visual representation of my intuition/current understanding:
The above picture shows a sphere, and I'm in the center. When I aim the gun towards the green half of the sphere, the orientation is correct. When I aim the gun towards the red half of the sphere, it is incorrect - it seems like every axis is inverted, but I am not 100% sure that is the case.
32.7636 144.849 44.3845
-147.236 35.1512 -135.616
Those are the same. Left 33 or right 147. You are 180 from each other. Now look up 145 - that's past up that's 35 from horizon, your back is arched.
Now roll to get your back to the sky.
If you need to use Euler, try to keep pitch in -90 to +90, and roll in -180 to +180;
if (pitch > 90) {
pitch -= 90;
yaw += 180;
roll += 180;
}
if (roll > 180) {
roll = 360 - roll;
}
or something like that.
The definition of the any kind of 3 angles to represent a rotation is not only given by the order of the rotations, if they are extrinsic or extrinsic, but also which interval of angles you choose when you define the mapping of every element of the 3D Rotation Group to a tuple of 3 angles.
Unfortunately, it is common for software libraries to fail to explicit mention which subset of angles they support, so typically it is necessary to either test their behaviour or to direct inspect the source code. For a relevant issue regarding glm see https://github.com/g-truc/glm/issues/569, and see https://github.com/robotology/idyntree/pull/504 for a related discussion on another library on which I work.
In glm master, from a quick inspection of the code (https://github.com/g-truc/glm/blob/6543cc9ad1476dd62fbfbe3194fcf19412f0cbc0/glm/gtc/quaternion.inl#L10) and from the fact that in C++ asin image is roughly (-90.0, 90.0) and atan2 image is roughly (-180.0, 180.0), the assumed interval in glm seems to be roughly (-180.0, 180.0) x (-90.0, 90.0) x (-180.0, 180.0), so by limiting the second angle (the yaw, using the names that you are using) to (-90.0, 90.0). So, what you are seeing at the GLM level is basically a mapping from the provided angles to equivalent angles in the (-180.0, 180.0) x (-90.0, 90.0) x (-180.0, 180.0) range.
However, the fact that this angles are equivalent depends on how they are used, i.e. if you have a library that clamps the euler angles outside the used ranges, instead of converting it to equivalent angles, then you will obtain strange results. For this reason, I think it would be interesting to understand your problem to know how this angles are generated (the middle angles in particular seems to be part of the range (-90, 270) that is a strange, even if valid choice) and how they are interpreted to render the object in the visualization. Once you understand that, even if the rendering function works fine for angles in the original applications ranges, you can write a function to map "original application angles" to "GLM angles" and its inverse, that you can use for your original purpose.
Roughly following tony's advice and after some trial&error and pattern identification, I managed to figure out a way to restore the original values after the conversion.
ox, oy, and oz are the original pitch, yaw, and roll in degrees, before any conversion;
fx, fy, and fz are the new pitch, yaw, and roll in degrees, obtained after converting "Euler -> quaternion -> Euler" (via glm::degrees(glm::eulerAngles(glm::normalize(quaternion)))).
if (oy > 90.f)
{
fx -= 180.f;
fy -= 180.f;
fy *= -1.f;
fz += 180.f;
if (ox > 0.f)
{
fx += 360.f;
}
}
The above code seems to make the original angle values and the one after the conversion match exactly. While it answers the original question, it doesn't solve my actual issue... I was converting to a quaternion in order to smoothly interpolate to another angle. However, it seems that using glm::mix on the quaternion after the conversion results - again - in very unpredictable rotations.
I am programming a fairly simple 3D minigolf game using OpenGl. I've run across a problem of slowing a minigolf ball that is rolling on a surface. The ball is described by velocity and position vectors.
When it moves on a flat surface, it must be slowed, but only in XZ plane (Y axis points up). The Y component must not be slowed, as only the ground applies friction - that of air versus ball is negligible, thus when the ball bounces, Y component comes into play. In order to change it, I add a gravity vector.
I'm looking for a way of decreasing speed proportionally in two axes. I tried decreasing exponentially/linearly both X and Z components, but that results in a false behavior - when moving along only one of these axes, ball slows at a lower rate than when moving in direction, say, 45 degrees, wherein both axes contribute to the velocity.
You need to find the magnitude of the 2D velocity vector in the XZ plane as a scalar quantity, then apply your friction function to that and then convert back to a 2D vector.
In C-like pseudo code:
// the current velocity in each direction
float velocityX, velocityY, velocityZ;
// compute magnitude using Pythagorean Theorem:
float magnitude = sqrt((velocityX * velocityX) + (velocityZ * velocityZ));
if(magnitude > 0.0) // is it moving?
{
// apply friction to magnitude using whatever function you want
// e.g. newMagnitude = magnitude * 0.95f;
float newMagnitude = applyFriction(magnitude);
float scaleFactor = newMagnitude / magnitude;
// compute the new velocity in each direction:
velocityX *= scaleFactor;
velocityZ *= scaleFactor;
}
I am trying to optimize the simulation function in my experiment so I can have more artificial brain-controlled agents running at a time. I profiled my code and found out that the big bottleneck in my code right now is computing the relative angle from every agent to every agent, which is O(n2), minus some small optimizations I have done. Here is the current code snippet I have for computing the angle:
[C++]
double calcAngle(double fromX, double fromY, double fromAngle, double toX, double toY)
{
double d = 0.0;
double Ux = 0.0, Uy = 0.0, Vx = 0.0, Vy = 0.0;
d = sqrt( calcDistanceSquared(fromX, fromY, toX, toY) );
Ux = (toX - fromX) / d;
Uy = (toY - fromY) / d;
Vx = cos(fromAngle * (cPI / 180.0));
Vy = sin(fromAngle * (cPI / 180.0));
return atan2(((Ux * Vy) - (Uy * Vx)), ((Ux * Vx) + (Uy * Vy))) * 180.0 / cPI;
}
I have two 2D points (x1, y1) and (x2, y2) and the facing of the "from" point (xa). I want to compute the angle that agent x needs to turn (relative to its current facing) to face agent y.
According to the profiler, the most expensive part is the atan2. I have Googled for hours and the above solution is the best solution I could find. Does anyone know of a more efficient way to compute the angle between two points? I am willing to sacrifice a little accuracy (+/- 1-2 degrees) for speed, if that affects anything.
As has been mentioned in the comments, there are probably high-level approaches to reduce your computational load.
But to the question in hand, you can just use the dot-product relationship:
theta = acos ( a . b / ||a|| ||b|| )
where a and b are your vectors, . denotes "dot product" and || || denotes "vector magnitude".
Essentially, this will replace your {sqrt, cos, sin, atan2} with {sqrt, acos}.
I would also suggest sticking to radians for all internal calculations, only converting to and from degrees for human-readable I/O.
Your comment tells a lot: "I am simulating a 180 degree frontal retina for every agent, so I need the angle". No, you don't. You just need to know whether the angle between the position vector and vision vector is more or less than 90 degrees.
That's very easy: the dot product A·B is >0 if the angle between A and B is less than 90 degrees; 0 if the angle is precisely 90 degrees, and <0 if the angle is more than 90 degrees. Calculating this takes 3 multiplications and 2 additions.
i think it's more a mathematical problem:
try
abs(arctan((y1-yfrom)/(x1-xfrom)) - arctan(1/((y2-yfrom2)/(x2-xfrom2))))
Use the dot product of these two vectors and at worst you need to do an inverse cosine instead:
A = Facing direction. B = Direction of Agent Y from Agent X
Calculating the dot is simple multiplication and addition. From that you have the cosine of the angle.
For starters, you should realize that there are a couple of simplifications that can reduce the calculations a bit:
You need not calculate the angle from an agent to itself,
If you have the angle from agent i to agent j, you already know something about the angle from agent j back to agent i.
I have to ask: what does "agent i turn to face agent j" mean? If the two surfaces are looking right at each other, do you have to do a calculation? What tolerance do you have on "looking right at each other"?
It'd be easier to recommend what to do if you'd stop focusing on the mathematics and describe the problem more fully.
This is how I calculate my line of sight vector and the up vector.
ly = sin(inclination);
lx = cos(inclination)*sin(azimuth);
lz = cos(inclination)*cos(azimuth);
uy = sin(inclination + M_PI / 2.0);
ux = cos(inclination + M_PI / 2.0)*sin(azimuth + M_PI);
uz = cos(inclination + M_PI / 2.0)*cos(azimuth + M_PI);
inclination is the angle of the line of sight vector from the xz plane and azimuth is the angle in the xz plane.
This works fine till my inclination reaches 225 degrees. At that point there is a discontinuity in the rotation for some reason. (Note By 225 degrees, I mean its past the upside-down point)
Any ideas as to why this is so?
EDIT: Never mind, figured it out. The azimuth does not need a 180 deg. tilt for the up vector.
I think you are talking of a limit angle of 90 degrees (pi). What you get is a normal behavior. When using gluLookAt, you specify an 'up' vector, used to determine the roll of the camera. In the special case where you are looking upside down, the 'up' vector is parallel to the eye direction vector, so it is not possible to determine the roll of the camera (this problem as an infinite number of solutions, so an arbitrary one is chosen by gluLookAt). May be you should compute this 'up' vector using your inclination and azimuth.
I want to ask what would be the best formula to convert mouse X,Y position into one of 16 directiones from player position.
I work in c++ ,sfml 1.6 so I get every position easily, but I dont know how to convert them based on angle from player position or something. (I was never good on math and for more than 4 directions if statements looks too complex).
Also I want to send it to server which converts direction back into delta X,Y so he can do something like:
player.Move(deltaX * speed * GetElapsedTime(), ...Y);
The "easiest" way would be to convert your two sets of co-ordinates (one for current player position, one for current mouse position) into an angle relative to the player's position, where an angle of 0 is the line straight ahead of the player (or north, depending on how your game works). Then each of your sixteen directions would translate to a given 22.5 degree interval.
However, since you said you're bad at math, I imagine you're looking for something more concrete than that.
You could use atan2 to get the angle between the mouse position and the positive X axis:
#include <cmath>
float player_x = ...;
float player_y = ...;
float mouse_x = ...;
float mouse_y = ...;
float angle = std::atan2(mouse_y - player_y, mouse_x - player_x);
The angle returned by std::atan2() is a value between -M_PI (exclusive) and M_PI (inclusive):
-M_PI Left (-180°)
-0.5 * M_PI Down (-90°)
0 Right (0°)
0.5 * M_PI Up (90°)
M_PI Left (180°)
You can transform this value depending on how you want your mapping to "one of 16 directions", i.e., depending on what value you want to assign to which discrete direction.
Given the angle, getting a unit vector to represent the X/Y delta is quite easy, too:
float dx = std::cos(angle);
float dy = std::sin(angle);