The official development documentation suggests the following way of obtaining the quaternion from the 3D rotation rate vector (wx, wy, wz).
// Create a constant to convert nanoseconds to seconds.
private static final float NS2S = 1.0f / 1000000000.0f;
private final float[] deltaRotationVector = new float[4]();
private float timestamp;
public void onSensorChanged(SensorEvent event) {
// This timestep's delta rotation to be multiplied by the current rotation
// after computing it from the gyro sample data.
if (timestamp != 0) {
final float dT = (event.timestamp - timestamp) * NS2S;
// Axis of the rotation sample, not normalized yet.
float axisX = event.values[0];
float axisY = event.values[1];
float axisZ = event.values[2];
// Calculate the angular speed of the sample
float omegaMagnitude = sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);
// Normalize the rotation vector if it's big enough to get the axis
// (that is, EPSILON should represent your maximum allowable margin of error)
if (omegaMagnitude > EPSILON) {
axisX /= omegaMagnitude;
axisY /= omegaMagnitude;
axisZ /= omegaMagnitude;
}
// Integrate around this axis with the angular speed by the timestep
// in order to get a delta rotation from this sample over the timestep
// We will convert this axis-angle representation of the delta rotation
// into a quaternion before turning it into the rotation matrix.
float thetaOverTwo = omegaMagnitude * dT / 2.0f;
float sinThetaOverTwo = sin(thetaOverTwo);
float cosThetaOverTwo = cos(thetaOverTwo);
deltaRotationVector[0] = sinThetaOverTwo * axisX;
deltaRotationVector[1] = sinThetaOverTwo * axisY;
deltaRotationVector[2] = sinThetaOverTwo * axisZ;
deltaRotationVector[3] = cosThetaOverTwo;
}
timestamp = event.timestamp;
float[] deltaRotationMatrix = new float[9];
SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
// User code should concatenate the delta rotation we computed with the current rotation
// in order to get the updated rotation.
// rotationCurrent = rotationCurrent * deltaRotationMatrix;
}
}
My question is:
It is quite different from the acceleration case, where computing the resultant acceleration using the accelerations ALONG the 3 axes makes sense.
I am really confused why the resultant rotation rate can also be computed with the sub-rotation rates AROUND the 3 axes. It does not make sense to me.
Why would this method - finding the composite rotation rate magnitude - even work?
Since your title does not really match your questions, I'm trying to answer as much as I can.
Gyroscopes don't give an absolute orientation (as the ROTATION_VECTOR) but only rotational velocities around those axis they are built to 'rotate' around. This is due to the design and construction of a gyroscope. Imagine the construction below. The golden thing is rotating and due to the laws of physics it does not want to change its rotation. Now you can rotate the frame and measure these rotations.
Now if you want to obtain something as the 'current rotational state' from the Gyroscope, you will have to start with an initial rotation, call it q0 and constantly add those tiny little rotational differences that the gyroscope is measuring around the axis to it: q1 = q0 + gyro0, q2 = q1 + gyro1, ...
In other words: The Gyroscope gives you the difference it has rotated around the three constructed axis, so you are not composing absolute values but small deltas.
Now this is very general and leaves a couple of questions unanswered:
Where do I get an initial position from? Answer: Have a look at the Rotation Vector Sensor - you can use the Quaternion obtained from there as an initialisation
How to 'sum' q and gyro?
Depending on the current representation of a rotation: If you use a rotation matrix, a simple matrix multiplication should do the job, as suggested in the comments (note that this matrix-multiplication implementation is not efficient!):
/**
* Performs naiv n^3 matrix multiplication and returns C = A * B
*
* #param A Matrix in the array form (e.g. 3x3 => 9 values)
* #param B Matrix in the array form (e.g. 3x3 => 9 values)
* #return A * B
*/
public float[] naivMatrixMultiply(float[] B, float[] A) {
int mA, nA, mB, nB;
mA = nA = (int) Math.sqrt(A.length);
mB = nB = (int) Math.sqrt(B.length);
if (nA != mB)
throw new RuntimeException("Illegal matrix dimensions.");
float[] C = new float[mA * nB];
for (int i = 0; i < mA; i++)
for (int j = 0; j < nB; j++)
for (int k = 0; k < nA; k++)
C[i + nA * j] += (A[i + nA * k] * B[k + nB * j]);
return C;
}
To use this method, imagine that mRotationMatrix holds the current state, these two lines do the job:
SensorManager.getRotationMatrixFromVector(deltaRotationMatrix, deltaRotationVector);
mRotationMatrix = naivMatrixMultiply(mRotationMatrix, deltaRotationMatrix);
// Apply rotation matrix in OpenGL
gl.glMultMatrixf(mRotationMatrix, 0);
If you chose to use Quaternions, imagine again that mQuaternion contains the current state:
// Perform Quaternion multiplication
mQuaternion.multiplyByQuat(deltaRotationVector);
// Apply Quaternion in OpenGL
gl.glRotatef((float) (2.0f * Math.acos(mQuaternion.getW()) * 180.0f / Math.PI),mQuaternion.getX(),mQuaternion.getY(), mQuaternion.getZ());
Quaternion multiplication is described here - equation (23). Make sure, you apply the multiplication correctly, since it is not commutative!
If you want to simply know rotation of your device (I assume this is what you ultimately want) I strongly recommend the ROTATION_VECTOR-Sensor. On the other hand Gyroscopes are quite precise for measuring rotational velocity and have a very good dynamic response, but suffer from drift and don't give you an absolute orientation (to magnetic north or according to gravity).
UPDATE: If you want to see a full example, you can download the source-code for a simple demo-app from https://bitbucket.org/apacha/sensor-fusion-demo.
Makes sense to me. Acceleration sensors typically work by having some measurable quantity change when force is applied to the axis being measured. E.g. if gravity is pulling down on the sensor measuring that axis, it conducts electricity better. So now you can tell how hard gravity, or acceleration in some direction, is pulling. Easy.
Meanwhile gyros are things that spin (OK, or bounce back and forth in a straight line like a tweaked diving board). The gyro is spinning, now you spin, the gyro is going to look like it is spinning faster or slower depending on the direction you spun. Or if you try to move it, it will resist and try to keep going the way it is going. So you just get a rotation change out of measuring it. Then you have to figure out the force from the change by integrating all the changes over the amount of time.
Typically none of these things are one sensor either. They are often 3 different sensors all arranged perpendicular to each other, and measuring a different axis. Sometimes all the sensors are on the same chip, but they are still different things on the chip measured separately.
Related
I have an object which moves on a terrain and a third person camera follow it, after I move it for some distance in different directions it begin to shaking or vibrating even if it is not moving and the camera rotates around it, this is the moving code of the object
double& delta = engine.getDeltaTime();
GLfloat velocity = delta * movementSpeed;
glm::vec3 t(glm::vec3(0, 0, 1) * (velocity * 3.0f));
//translate the objet atri before rendering
matrix = glm::translate(matrix, t);
//get the forward vetor of the matrix
glm::vec3 f(matrix[2][0], matrix[2][1], matrix[2][2]);
f = glm::normalize(f);
f = f * (velocity * 3.0f);
f = -f;
camera.translate(f);
and the camera rotation is
void Camera::rotate(GLfloat xoffset, GLfloat yoffset, glm::vec3& c, double& delta, GLboolean constrainpitch) {
xoffset *= (delta * this->rotSpeed);
yoffset *= (delta * this->rotSpeed);
pitch += yoffset;
yaw += xoffset;
if (constrainpitch) {
if (pitch >= maxPitch) {
pitch = maxPitch;
yoffset = 0;
}
if (pitch <= minPitch) {
pitch = minPitch;
yoffset = 0;
}
}
glm::quat Qx(glm::angleAxis(glm::radians(yoffset), glm::vec3(1.0f, 0.0f, 0.0f)));
glm::quat Qy(glm::angleAxis(glm::radians(xoffset), glm::vec3(0.0f, 1.0f, 0.0f)));
glm::mat4 rotX = glm::mat4_cast(Qx);
glm::mat4 rotY = glm::mat4_cast(Qy);
view = glm::translate(view, c);
view = rotX * view;
view = view * rotY;
view = glm::translate(view, -c);
}
float is sometimes not enough.
I use double precision matrices on CPU side to avoid such problems. But as you are on Android it might not be possible. For GPU use floats again as there are no 64bit interpolators yet.
Big numbers are usually the problem
If your world is big then you are passing big numbers into the equations multiplying any errors and only at the final stage the stuff is translated relative to camera position meaning the errors stay multiplied but the numbers got clamped so error/data ratio got big.
To lower this problem before rendering convert all vertexes to coordinate system with origin at or near your camera. You can ignore rotations just offset the positions.
This way you will got higher errors only far away from camera which is with perspective not visible anyway... For more info see:
ray and ellipsoid intersection accuracy improvement
Use cumulative transform matrix instead of Euler angles
for more info see Understanding 4x4 homogenous transform matrices and all the links at bottom of that answer.
This sounds like a numerical effect to me. Even small offsets coming from your game object will influence the rotation of the following camera with small movements / rotations and it looks like a vibrating object / camera.
So what you can do is:
Check if the movement above a threshold value before calculating a new rotation for your camera
When you are above this threshold: do a linear interpolation between the old and the new rotation using the lerp-algorithm for the quaternion ( see this unity answer to get a better understanding how your code can look like: Unity lerp discussion )
i'm working in a AR application in android with the Epson Moverio BT-200.
I have a quaternion that change his values with my sensor fusion algorithm.
In my application i'm trying to move a 2D item changing his margin left and margin top values when I move my head.
I'd like to know how can I extract, from the quaternion values, only the "horizontal" and "vertical" movements.
I could extract from the quaternion the pitch and roll values, but I read that there are several problems with euler angle. Could I do this only working with quaternions?
This is my actual code. I solved the problem using the Quaternions for the algorithm, and at the end I extract the euler angles from the rotation matrix.
This is the algorithm for take the values from the sensors:
private static final float NS2S = 1.0f / 1000000000.0f;
private final Quaternion deltaQuaternion = new Quaternion();
private Quaternion quaternionGyroscope = new Quaternion();
private Quaternion quaternionRotationVector = new Quaternion();
private long timestamp;
private static final double EPSILON = 0.1f;
private double gyroscopeRotationVelocity = 0;
private boolean positionInitialised = false;
private int panicCounter;
private static final float DIRECT_INTERPOLATION_WEIGHT = 0.005f;
private static final float OUTLIER_THRESHOLD = 0.85f;
private static final float OUTLIER_PANIC_THRESHOLD = 0.65f;
private static final int PANIC_THRESHOLD = 60;
#Override
public void onSensorChanged(SensorEvent event) {
if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
// Process rotation vector (just safe it)
float[] q = new float[4];
// Calculate angle. Starting with API_18, Android will provide this value as event.values[3], but if not, we have to calculate it manually.
SensorManager.getQuaternionFromVector(q, event.values);
// Store in quaternion
quaternionRotationVector.setXYZW(q[1], q[2], q[3], -q[0]);
if (!positionInitialised) {
// Override
quaternionGyroscope.set(quaternionRotationVector);
positionInitialised = true;
}
} else if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
// Process Gyroscope and perform fusion
// This timestep's delta rotation to be multiplied by the current rotation
// after computing it from the gyro sample data.
if (timestamp != 0) {
final float dT = (event.timestamp - timestamp) * NS2S;
// Axis of the rotation sample, not normalized yet.
float axisX = event.values[0];
float axisY = event.values[1];
float axisZ = event.values[2];
// Calculate the angular speed of the sample
gyroscopeRotationVelocity = Math.sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
// Normalize the rotation vector if it's big enough to get the axis
if (gyroscopeRotationVelocity > EPSILON) {
axisX /= gyroscopeRotationVelocity;
axisY /= gyroscopeRotationVelocity;
axisZ /= gyroscopeRotationVelocity;
}
// Integrate around this axis with the angular speed by the timestep
// in order to get a delta rotation from this sample over the timestep
// We will convert this axis-angle representation of the delta rotation
// into a quaternion before turning it into the rotation matrix.
double thetaOverTwo = gyroscopeRotationVelocity * dT / 2.0f;
double sinThetaOverTwo = Math.sin(thetaOverTwo);
double cosThetaOverTwo = Math.cos(thetaOverTwo);
deltaQuaternion.setX((float) (sinThetaOverTwo * axisX));
deltaQuaternion.setY((float) (sinThetaOverTwo * axisY));
deltaQuaternion.setZ((float) (sinThetaOverTwo * axisZ));
deltaQuaternion.setW(-(float) cosThetaOverTwo);
// Move current gyro orientation
deltaQuaternion.multiplyByQuat(quaternionGyroscope, quaternionGyroscope);
// Calculate dot-product to calculate whether the two orientation sensors have diverged
// (if the dot-product is closer to 0 than to 1), because it should be close to 1 if both are the same.
float dotProd = quaternionGyroscope.dotProduct(quaternionRotationVector);
// If they have diverged, rely on gyroscope only (this happens on some devices when the rotation vector "jumps").
if (Math.abs(dotProd) < OUTLIER_THRESHOLD) {
// Increase panic counter
if (Math.abs(dotProd) < OUTLIER_PANIC_THRESHOLD) {
panicCounter++;
}
// Directly use Gyro
setOrientationQuaternionAndMatrix(quaternionGyroscope);
} else {
// Both are nearly saying the same. Perform normal fusion.
// Interpolate with a fixed weight between the two absolute quaternions obtained from gyro and rotation vector sensors
// The weight should be quite low, so the rotation vector corrects the gyro only slowly, and the output keeps responsive.
Quaternion interpolate = new Quaternion();
quaternionGyroscope.slerp(quaternionRotationVector, interpolate, DIRECT_INTERPOLATION_WEIGHT);
// Use the interpolated value between gyro and rotationVector
setOrientationQuaternionAndMatrix(interpolate);
// Override current gyroscope-orientation
quaternionGyroscope.copyVec4(interpolate);
// Reset the panic counter because both sensors are saying the same again
panicCounter = 0;
}
if (panicCounter > PANIC_THRESHOLD) {
Log.d("Rotation Vector",
"Panic counter is bigger than threshold; this indicates a Gyroscope failure. Panic reset is imminent.");
if (gyroscopeRotationVelocity < 3) {
Log.d("Rotation Vector",
"Performing Panic-reset. Resetting orientation to rotation-vector value.");
// Manually set position to whatever rotation vector says.
setOrientationQuaternionAndMatrix(quaternionRotationVector);
// Override current gyroscope-orientation with corrected value
quaternionGyroscope.copyVec4(quaternionRotationVector);
panicCounter = 0;
} else {
Log.d("Rotation Vector",
String.format(
"Panic reset delayed due to ongoing motion (user is still shaking the device). Gyroscope Velocity: %.2f > 3",
gyroscopeRotationVelocity));
}
}
}
timestamp = event.timestamp;
}
}
private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
Quaternion correctedQuat = quaternion.clone();
// We inverted w in the deltaQuaternion, because currentOrientationQuaternion required it.
// Before converting it back to matrix representation, we need to revert this process
correctedQuat.w(-correctedQuat.w());
synchronized (syncToken) {
// Use gyro only
currentOrientationQuaternion.copyVec4(quaternion);
// Set the rotation matrix as well to have both representations
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuat.ToArray());
}
}
And this is how I take the euler angles rotation values:
/**
* #return Returns the current rotation of the device in the Euler-Angles
*/
public EulerAngles getEulerAngles() {
float[] angles = new float[3];
float[] remappedOrientationMatrix = new float[16];
SensorManager.remapCoordinateSystem(currentOrientationRotationMatrix.getMatrix(), SensorManager.AXIS_X,
SensorManager.AXIS_Z, remappedOrientationMatrix);
SensorManager.getOrientation(remappedOrientationMatrix, angles);
return new EulerAngles(angles[0], angles[1], angles[2]);
}
I solved my problem with this solution. Now won't be difficult to move my 2d Object with this sensors values. Sorry for lenght of my answer, but I hope that it could be useful for someone :)
Can any one help me to draw a cylinder in OpenGL-es android. Whatever i draw its look like a rectangle.
I would appreciate any tips or link.
Here is the code i've tried:
int VERTICES=180; // more than needed
float coords[] = new float[VERTICES * 3];
float theta = 0;
for (int i = 0; i < VERTICES * 3; i += 3) {
coords[i + 0] = (float) Math.cos(theta);
coords[i + 1] = (float) Math.sin(theta);
coords[i + 2] = 0;
_vertexBuffer.put(coords[i + 0]);
_vertexBuffer.put(coords[i + 1]);
_vertexBuffer.put(coords[i + 2]);
theta += Math.PI / 90;
}
This will only draw a circle. A cylinder is more complicated as you will need to define vertices in a translated z plane. And define them with correct normals (either facing in as if you were inside the cylinder -ie a tunnel or out as in looking at a pipe) which is the trickier part.
I'm currently doing this now (which is what brought me here) and have the cylinder drawn but pretty sure my normals are incorrect as my lighting looks a bit off. I'll post some code when I figure it out.
Edit : realized the code also doesn't actually draw a circle. Here is how to do that (in 2D) :
R = Radius
NUM_VERTICES = Number of vertices you want to use in circle
delta = (Math.PI / 180) * (360 / NUM_VERTICES); //get delta in radians between vertex definition
for i = 0 ; i < NUM_VERTICES ; i ++
x = R * cos(Delta * i)
y = R * sin(Delta * i))
vertices[i] = x; vertices[i+1] = y; vertices[i+2] = 0;
end for
//note may need to redefine the original vertex to complete the circle depending on which GL draw type you are using. If so just take the arg to sin / cos to be 0 to complete the loop
Last Edit* : just realized I was overcomplicating the normals by re-using some calculate normal from triangle code I had. Instead I realized how simple the normal calculation is for a cylinder if you consider the the origin 0,0 to be the center of each circular strip. The normal will be = vertex position scaled to length 1. for normals facing in on a cylinder (ie tunnel) the x,y values would be inverted (this is a assuming you are looking down the -z axis).
I'm using libgdx to develop a basic 3d game for android and I'm having difficulty properly orienting the camera given three rotation angles provided from the compass (azimuth - rotation about Z, roll - rotation about Y, pitch - rotation about X). I've had some slight success with the following code in that I can properly aim the virtual camera down the Z-axis and X-axis as I expected. (Angles are in degrees [-180,180])
camera.direction.x = 0;
camera.direction.y = 0;
camera.direction.z = 1;
camera.up.x = -1;
camera.up.y = 0;
camera.up.z = 0;
camera.rotate(azimuth,0,0,1);
camera.rotate(roll,0,1,0);
camera.rotate(pitch,1,0,0);
I've also had some success with this, but it does not orient the camera's up-vector. (Angles have been converted to radians in this version)
float x,y,z;
roll = (float) (roll + Math.PI/2.0);
x = (float) (Math.cos(azimuth) * Math.cos(roll));
y = (float) (Math.sin(azimuth) * Math.cos(roll));
z = (float) (Math.sin(roll));
Vector3 lookat = new Vector3(x,y,z);
camera.lookAt(lookat.x, lookat.y, lookat.z);
Can someone shed some light on how to properly orient the virtual camera from these three angles?
Also, I'm trying to have the phone be in landscape mode such that the top of the phone is to the left and the bottom is to the right. Hence the camera's default direction (all rotations are at 0, top of the phone is aimed north) be the camera aiming toward the ground (positive Z) with the up aiming east (negative X).
By coding other things for a while, I eventually reached a point where I was trying to separate the rendering, simulation, and input. Because of that I have come up with the following solution that works for me. I haven't tested it rigorously, but it appears to do what I want (ignoring camera roll).
On the android part of the program, I needed to set the orientation to landscape mode:
<activity android:name=".MySuperAwesomeApplication"
android:label="#string/app_name"
android:screenOrientation="landscape">
>
I created a player class to store yaw, pitch, roll, and position
public class Player {
public final Vector3 position = new Vector3(0,1.5f,0);
/** Angle left or right of the vertical */
public float yaw = 0.0f;
/** Angle above or below the horizon */
public float pitch = 0.0f;
/** Angle about the direction as defined by yaw and pitch */
public float roll = 0.0f;
}
And then when I update the player based on input I do the following:
player.yaw = -Gdx.input.getAzimuth();
player.pitch = -Gdx.input.getRoll()-90;
player.roll = -Gdx.input.getPitch();
Note that pitch maps to input.roll and roll maps to input.pitch. Not sure why, but it works for me. Finally update the camera:
camera.direction.x = 0;
camera.direction.y = 0;
camera.direction.z = 1;
camera.up.x = 0;
camera.up.y = 1;
camera.up.z = 0;
camera.position.x = 0;
camera.position.y = 0;
camera.position.z = 0;
camera.update();
// The world up vector is <0,1,0>
camera.rotate(player.yaw,0,1,0);
Vector3 pivot = camera.direction.cpy().crs(camera.up);
camera.rotate(player.pitch, pivot.x,pivot.y,pivot.z);
camera.rotate(player.roll, camera.direction.x, camera.direction.y, camera.direction.z);
camera.translate(player.position.x, player.position.y, player.position.z);
camera.update();
EDIT: added camera roll to the code. For me and my Droid 2, roll appears to only have values in [-90,90] such that if you rotate past -90 or 90 the values start changing back towards 0.
I have to write a compass app in Android. The only thing the user sees on the screen is a cube with a red wall which has to point north. This is not important. What's important is that I need to rotate that cube accordingly to the rotation of the device itself so that the red wall continues to point north no matter how the phone is being held. My code is simple and straightforward:
#Override
public void onSensorChanged(SensorEvent event) {
synchronized (this) {
switch (event.sensor.getType()){
case Sensor.TYPE_ACCELEROMETER:
direction = event.values[2];
break;
case Sensor.TYPE_ORIENTATION:
if (direction < 0) {
angleX = event.values[1];
angleY = -event.values[2];
angleZ = event.values[0];
} else {
angleX = -event.values[1];
angleY = -event.values[2];
angleZ = event.values[0];
}
break;
}
}
}
I have added this extra direction variable that simply stores whether the phone's display is pointing downwards or upwards. I don't know if I need it but it seems to fix some bugs. I am using the SensorSimulator for android but whenever my pitch slider goes in the [-90, 90] interval the other variables get mixed up. It's like they get a 180 offset. But I can't detect when I am in this interval because the range of the pitch is from -90 to 90 so I can move that slider from left to write and I will always be in that interval.
This was all just to show you how far has my code advanced. I am not saying how this problem should be solved because I will only probably stir myself into a dead end. You see, I have been trying to write that app for 3 days now, and you can imagine how pissed my boss is. I have read all sorts of tutorials and tried every formula I could find or think of. So please help me. All I have to do is know how to rotate my cube, the rotation angles of which are EULER ANGLES in degrees.
Here's some code I wrote to do something pretty similar, really only caring about the rotation of the device in the roll direction. Hope it helps! It just uses the accelerometer values to determine the pitch, no need to get orientation of the view.
public void onSensorChanged(SensorEvent event) {
float x = -1 * event.values[0] / SensorManager.GRAVITY_EARTH;
float y = -1 * event.values[1] / SensorManager.GRAVITY_EARTH;
float z = -1 * event.values[2] / SensorManager.GRAVITY_EARTH;
float signedRawRoll = (float) (Math.atan2(x, y) * 180 / Math.PI);
float unsignedRawRoll = Math.abs(signedRawRoll);
float rollSign = signedRawRoll / unsignedRawRoll;
float rawPitch = Math.abs(z * 180);
// Use a basic low-pass filter to only keep the gravity in the accelerometer values for the X and Y axes
// adjust the filter weight based on pitch, as roll is harder to define as pitch approaches 180.
float filterWeight = rawPitch > 165 ? 0.85f : 0.7f;
float newUnsignedRoll = filterWeight * Math.abs(this.roll) + (1 - filterWeight) * unsignedRawRoll;
this.roll = rollSign * newUnsignedRoll;
if (Float.isInfinite(this.roll) || Float.isNaN(this.roll)) {
this.roll = 0;
}
this.pitch = filterWeight * this.pitch + (1 - filterWeight) * rawPitch;
for (IAngleListener listener : listeners) {
listener.deviceRollAndPitch(this.roll, this.pitch);
}
}