I'm making program for showing objects from map on camera and this works almost well except few degrees to left and right from vertical orientation (like in 80-110 and 260-280 degrees). In other +-320 degrees it works well. I've tried to use TYPE_ROTATION_VECTOR and accelerometer with magnetometer and they have the same result. Does anybody know any solution?
with TYPE_ROTATION_VECTOR:
if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR)
{
float[] roationV = new float[16];
SensorManager.getRotationMatrixFromVector(roationV, event.values);
float[] orientationValuesV = new float[3];
SensorManager.getOrientation(roationV, orientationValuesV);
tvHeading.setText(String.format(
"Coordinates: lat = %1$.2f, lon = %2$.2f, time = %3$.2f",
orientationValuesV[0], orientationValuesV[1], orientationValuesV[2]));
float[] rotationMatrix=new float[16];
mSensorManager.getRotationMatrixFromVector(rotationMatrix, event.values);
float[] orientationValues = new float[3];
SensorManager.getOrientation(rotationMatrix, orientationValues);
double azimuth = Math.toDegrees(orientationValues[0]);
double pitch = Math.toDegrees(orientationValues[1]);
double roll = Math.toDegrees(orientationValues[2]);
tvOrientation.setText(String.format(
"Coordinates: lat = %1$.2f, lon = %2$.2f, time = %3$.2f",
azimuth,pitch,roll));
}
with accelerometer+magnetometer
if (event.sensor == mAccelerometer) {
System.arraycopy(event.values, 0, mLastAccelerometer, 0, event.values.length);
mLastAccelerometer = meanFilterAccelSmoothing
.addSamples(mLastAccelerometer);
mLastAccelerometer = medianFilterAccelSmoothing
.addSamples(mLastAccelerometer);
for (int i = 0; i < mLastAccelerometer.length; i++) {
mLastAccelerometer[i] = (float) Math.floor(mLastAccelerometer[i] * 1000) / 1000;
}
mLastAccelerometerSet = true;
}
if (event.sensor == mMagnetometer) {
System.arraycopy(event.values, 0, mLastMagnetometer, 0, event.values.length);
mLastMagnetometer = meanFilterMagneticSmoothing.addSamples(mLastMagnetometer);
mLastMagnetometer = medianFilterMagneticSmoothing.addSamples(mLastMagnetometer);
for (int i = 0; i < mLastMagnetometer.length; i++) {
mLastMagnetometer[i] = (float) Math.floor(mLastMagnetometer[i] * 1000) / 1000;
}
mLastMagnetometerSet = true;
}
if (mLastAccelerometerSet && mLastMagnetometerSet) {
SensorManager.getRotationMatrix(mR, null, mLastAccelerometer, mLastMagnetometer);
SensorManager.getOrientation(mR, mOrientation);
if (angeles.size() > 0) {
for (int i = 0; i < mapObjects.size(); i++) {
compassFunc(i, mOrientation[0], mOrientation[1], mOrientation[2]);
}
}
private void compassFunc(int number, float... values) {
double angularXSpeed = Math.floor(values[0] * 180 / Math.PI * 100) / 100;
double angularYSpeed = Math.floor(values[1] * 180 / Math.PI * 100) / 100;
double angularZSpeed = Math.floor(values[2] * 180 / Math.PI * 100) / 100;
tvOrientation.setText(String.format(
"Screen: lt= %1$.2f : %2$.2f,rt= %3$.2f : %4$.2f,lb= %5$.2f : %6$.2f,rb= %7$.2f : %8$.2f",
xLeftTop, yLeftTop, xRightTop,yRightTop,xLeftBottom,yLeftBottom,xRightBottom,yRightBottom));
}
This sounds like a typical case of Gimbal Lock. Your description of how rotation around one axis acts up when another reaches +-90 degrees suggests that this is indeed the case.
This is a fundamental problem with Euler angles (Yaw/Azimuth, Pitch, Roll), which is why most such computations are done using rotation matrices or quaternions, and Euler angles most often only are used when a particular orientation is to be displayed to a human (humans are generally bad at interpreting rotation matrices and quaternions).
The ROTATION_VECTOR sensor outputs it's data in a quaternion format (source), albeit with rearranged values, and the getRotationMatrixFromVector() method turns this into a rotation matrix. I would suggest using one of these descriptions for your internal calculations.
The answers to this similar question provide some concrete suggestions on how to solve the issue.
Related
I am trying to figure out how to point the device marker where the user is heading. I tried the code below to get the bearing angle between the true north and where the user is heading. Once the bearing is found, I would apply it to the device marker and rotate it based on that bearing, but it only works when the device is not moving.
I tried out the code I found from the link below.
Code Source Link
// raw inputs from Android sensors
float m_Norm_Gravity; // length of raw gravity vector received in onSensorChanged(...). NB: should be about 10
float[] m_NormGravityVector = m_NormMagFieldValues = null;; // Normalised gravity vector, (i.e. length of this vector is 1), which points straight up into space
float m_Norm_MagField; // length of raw magnetic field vector received in onSensorChanged(...).
float[] m_NormMagFieldValues; // Normalised magnetic field vector, (i.e. length of this vector is 1)
// accuracy specifications. SENSOR_UNAVAILABLE if unknown, otherwise SensorManager.SENSOR_STATUS_UNRELIABLE, SENSOR_STATUS_ACCURACY_LOW, SENSOR_STATUS_ACCURACY_MEDIUM or SENSOR_STATUS_ACCURACY_HIGH
int m_GravityAccuracy; // accuracy of gravity sensor
int m_MagneticFieldAccuracy; // accuracy of magnetic field sensor
// values calculated once gravity and magnetic field vectors are available
float[] m_NormEastVector = new float[3]; // normalised cross product of raw gravity vector with magnetic field values, points east
float[] m_NormNorthVector = new float[3]; // Normalised vector pointing to magnetic north
boolean m_OrientationOK = false; // set true if m_azimuth_radians and m_pitch_radians have successfully been calculated following a call to onSensorChanged(...)
float m_azimuth_radians; // angle of the device from magnetic north
float m_pitch_radians; // tilt angle of the device from the horizontal. m_pitch_radians = 0 if the device if flat, m_pitch_radians = Math.PI/2 means the device is upright.
float m_pitch_axis_radians; // angle which defines the axis for the rotation m_pitch_radians
#Override
public void onSensorChanged(SensorEvent sensorEvent) {
int SensorType = sensorEvent.sensor.getType();
switch(SensorType) {
case Sensor.TYPE_GRAVITY:
if (m_NormGravityVector == null) m_NormGravityVector = new float[3];
System.arraycopy(sensorEvent.values, 0, m_NormGravityVector, 0, m_NormGravityVector.length);
m_Norm_Gravity = (float)Math.sqrt(m_NormGravityVector[0]*m_NormGravityVector[0] + m_NormGravityVector[1]*m_NormGravityVector[1] + m_NormGravityVector[2]*m_NormGravityVector[2]);
for(int i=0; i < m_NormGravityVector.length; i++) m_NormGravityVector[i] /= m_Norm_Gravity;
break;
case Sensor.TYPE_MAGNETIC_FIELD:
if (m_NormMagFieldValues == null) m_NormMagFieldValues = new float[3];
System.arraycopy(sensorEvent.values, 0, m_NormMagFieldValues, 0, m_NormMagFieldValues.length);
m_Norm_MagField = (float)Math.sqrt(m_NormMagFieldValues[0]*m_NormMagFieldValues[0] + m_NormMagFieldValues[1]*m_NormMagFieldValues[1] + m_NormMagFieldValues[2]*m_NormMagFieldValues[2]);
for(int i=0; i < m_NormMagFieldValues.length; i++) m_NormMagFieldValues[i] /= m_Norm_MagField;
break;
}
if (m_NormGravityVector != null && m_NormMagFieldValues != null) {
// first calculate the horizontal vector that points due east
float East_x = m_NormMagFieldValues[1] * m_NormGravityVector[2] - m_NormMagFieldValues[2] * m_NormGravityVector[1];
float East_y = m_NormMagFieldValues[2] * m_NormGravityVector[0] - m_NormMagFieldValues[0] * m_NormGravityVector[2];
float East_z = m_NormMagFieldValues[0] * m_NormGravityVector[1] - m_NormMagFieldValues[1] * m_NormGravityVector[0];
float norm_East = (float) Math.sqrt(East_x * East_x + East_y * East_y + East_z * East_z);
if (m_Norm_Gravity * m_Norm_MagField * norm_East < 0.1f) { // Typical values are > 100.
m_OrientationOK = false; // device is close to free fall (or in space?), or close to magnetic north pole.
} else {
m_NormEastVector[0] = East_x / norm_East;
m_NormEastVector[1] = East_y / norm_East;
m_NormEastVector[2] = East_z / norm_East;
// next calculate the horizontal vector that points due north
float M_dot_G = (m_NormGravityVector[0] * m_NormMagFieldValues[0] + m_NormGravityVector[1] * m_NormMagFieldValues[1] + m_NormGravityVector[2] * m_NormMagFieldValues[2]);
float North_x = m_NormMagFieldValues[0] - m_NormGravityVector[0] * M_dot_G;
float North_y = m_NormMagFieldValues[1] - m_NormGravityVector[1] * M_dot_G;
float North_z = m_NormMagFieldValues[2] - m_NormGravityVector[2] * M_dot_G;
float norm_North = (float) Math.sqrt(North_x * North_x + North_y * North_y + North_z * North_z);
m_NormNorthVector[0] = North_x / norm_North;
m_NormNorthVector[1] = North_y / norm_North;
m_NormNorthVector[2] = North_z / norm_North;
// take account of screen rotation away from its natural rotation
int rotation = getActivity().getWindowManager().getDefaultDisplay().getRotation();
float screen_adjustment = 0;
switch (rotation) {
case Surface.ROTATION_0:
screen_adjustment = 0;
break;
case Surface.ROTATION_90:
screen_adjustment = (float) Math.PI / 2;
break;
case Surface.ROTATION_180:
screen_adjustment = (float) Math.PI;
break;
case Surface.ROTATION_270:
screen_adjustment = 3 * (float) Math.PI / 2;
break;
}
// NB: the rotation matrix has now effectively been calculated. It consists of the three vectors m_NormEastVector[], m_NormNorthVector[] and m_NormGravityVector[]
// calculate all the required angles from the rotation matrix
// NB: see https://math.stackexchange.com/questions/381649/whats-the-best-3d-angular-co-ordinate-system-for-working-with-smartfone-apps
float sin = m_NormEastVector[1] - m_NormNorthVector[0], cos = m_NormEastVector[0] + m_NormNorthVector[1];
m_azimuth_radians = (float) (sin != 0 && cos != 0 ? Math.atan2(sin, cos) : 0);
m_pitch_radians = (float) Math.acos(m_NormGravityVector[2]);
sin = -m_NormEastVector[1] - m_NormNorthVector[0];
cos = m_NormEastVector[0] - m_NormNorthVector[1];
float aximuth_plus_two_pitch_axis_radians = (float) (sin != 0 && cos != 0 ? Math.atan2(sin, cos) : 0);
m_pitch_axis_radians = (float) (aximuth_plus_two_pitch_axis_radians - m_azimuth_radians) / 2;
m_azimuth_radians += screen_adjustment;
m_pitch_axis_radians += screen_adjustment;
m_OrientationOK = true;
currentAzimuth = (float) Math.toDegrees(m_azimuth_radians);
}
}
}
#Override
public void onAccuracyChanged(Sensor sensor, int accuracy) {
int SensorType = sensor.getType();
switch(SensorType) {
case Sensor.TYPE_GRAVITY: m_GravityAccuracy = accuracy; break;
case Sensor.TYPE_MAGNETIC_FIELD: m_MagneticFieldAccuracy = accuracy; break;
}
}
Thanks in advance for your help first.
I have found so many examples using Gyroscope. But I couldn't find adequate one for me.
I'd like to make a simple quiz game that do actions when I tilt VM to 90 degrees forward and backward. Many examples said I might use "pitch" value of Gyroscope. Could you give some advices for me??
I have done a similar thing where i need draw a rectangle with includes nearby places and must point it to the place and show details.
public void onSensorChanged(SensorEvent event) {
final Handler handler = new Handler();
switch (event.sensor.getType()) {
case Sensor.TYPE_ACCELEROMETER:
mAcceleromterReading =
SensorUtilities.filterSensors(event.values, mAcceleromterReading);
break;
case Sensor.TYPE_MAGNETIC_FIELD:
mMagnetometerReading =
SensorUtilities.filterSensors(event.values, mMagnetometerReading);
break;
float[] orientation =
SensorUtilities.computeDeviceOrientation(mAcceleromterReading, mMagnetometerReading);
if (orientation != null) {
float azimuth = (float) Math.toDegrees(orientation[0]);
if (azimuth < 0) {
azimuth += 360f;
}
// Convert pitch and roll from radians to degrees
float pitch = (float) Math.toDegrees(orientation[1]);
float roll = (float) Math.toDegrees(orientation[2]);
if (abs(pitch - pitchPrev) > PITCH_THRESHOLD && abs(roll - rollPrev) > ROLL_THRESHOLD
&& abs(azimuth - azimuthPrev) > AZIMUTH_THRESHOLD) { // && abs(roll - rollPrev) > rollThreshold
if (DashplexManager.getInstance().mlocation != null) {
mOverlayDisplayView.setHorizontalFOV(mPreview.getHorizontalFOV());
mOverlayDisplayView.setVerticalFOV(mPreview.getVerticalFOV());
mOverlayDisplayView.setAzimuth(azimuth);
mOverlayDisplayView.setPitch(pitch);
mOverlayDisplayView.setRoll(roll);
// Update the OverlayDisplayView to red raw when sensor dataLogin changes,
// redrawing only when the camera is not pointing straight up or down
if (pitch <= 75 && pitch >= -75) {
//Log.d("issueAR", "invalidate: ");
mOverlayDisplayView.invalidate();
}
}
pitchPrev = pitch;
rollPrev = roll;
azimuthPrev = azimuth;
}
}
computeDeviceOrientation method
public static float[] computeDeviceOrientation(float[] accelerometerReading, float[] magnetometerReading) {
if (accelerometerReading == null || magnetometerReading == null) {
return null;
}
final float[] rotationMatrix = new float[9];
SensorManager.getRotationMatrix(rotationMatrix, null, accelerometerReading, magnetometerReading);
// Remap the coordinates with the camera pointing along the Y axis.
// This way, portrait and landscape orientation return the same azimuth to magnetic north.
final float cameraRotationMatrix[] = new float[9];
SensorManager.remapCoordinateSystem(rotationMatrix, SensorManager.AXIS_X,
SensorManager.AXIS_Z, cameraRotationMatrix);
final float[] orientationAngles = new float[3];
SensorManager.getOrientation(cameraRotationMatrix, orientationAngles);
// Return a float array containing [azimuth, pitch, roll]
return orientationAngles;
}
onDraw method
#SuppressLint("DrawAllocation")
#Override
protected void onDraw(Canvas canvas) {
super.onDraw(canvas);
// Log.d("issueAR", "onDraw: ");
// Log.d("issueAR", "mVerticalFOV: "+mVerticalFOV+" "+"mHorizontalFOV"+mHorizontalFOV);
// Get the viewports only once
if (!mGotViewports && mVerticalFOV > 0 && mHorizontalFOV > 0) {
mViewportHeight = canvas.getHeight() / mVerticalFOV;
mViewportWidth = canvas.getWidth() / mHorizontalFOV;
mGotViewports = true;
//Log.d("onDraw", "mViewportHeight: " + mViewportHeight);
}
if (!mGotViewports) {
return;
}
// Set the paints that remain constant only once
if (!mSetPaints) {
mTextPaint.setTextAlign(Paint.Align.LEFT);
mTextPaint.setTextSize(getResources().getDimensionPixelSize(R.dimen.canvas_text_size));
mTextPaint.setColor(Color.WHITE);
mOutlinePaint.setStyle(Paint.Style.STROKE);
mOutlinePaint.setStrokeWidth(mOutline);
mBubblePaint.setStyle(Paint.Style.FILL);
mSetPaints = true;
}
// Center of view
float x = canvas.getWidth() / 2;
float y = canvas.getHeight() / 2;
/*
* Uncomment line below to allow rotation of display around the center point
* based on the roll. However, this "feature" is not very intuitive, and requires
* locking device orientation to portrait or changes the sensor rotation matrix
* on device rotation. It's really quite a nightmare.
*/
//canvas.rotate((0.0f - mRoll), x, y);
float dy = mPitch * mViewportHeight;
if (mNearbyPlaces != null) {
//Log.d("OverlayDisplayView", "mNearbyPlaces: "+mNearbyPlaces.size());
// Iterate backwards to draw more distant places first
for (int i = mNearbyPlaces.size() - 1; i >= 0; i--) {
NearbyPlace nearbyPlace = mNearbyPlaces.get(i);
float xDegreesToTarget = mAzimuth - nearbyPlace.getBearingToPlace();
float dx = mViewportWidth * xDegreesToTarget;
float iconX = x - dx;
float iconY = y - dy;
if (isOverlapping(iconX, iconX).isOverlapped()) {
PointF point = calculateNewXY(new PointF(iconX, iconY + mViewportHeight));
iconX = point.x;
iconY = point.y;
}
nearbyPlace.setIconX(iconX);
nearbyPlace.setIconY(iconY);
Bitmap icon = getIcon(nearbyPlace.getIcon_id());
float width = icon.getWidth() + mTextPaint.measureText(nearbyPlace.getName()) + mMargin;
RectF recf=new RectF(iconX, iconY, width, icon.getHeight());
nearbyPlace.setRect(recf);
float angleToTarget = xDegreesToTarget;
if (xDegreesToTarget < 0) {
angleToTarget = 360 + xDegreesToTarget;
}
if (angleToTarget >= 0 && angleToTarget < 90) {
nearbyPlace.setQuadrant(1);
mQuad1Places.add(nearbyPlace);
} else if (angleToTarget >= 90 && angleToTarget < 180) {
nearbyPlace.setQuadrant(2);
mQuad2Places.add(nearbyPlace);
} else if (angleToTarget >= 180 && angleToTarget < 270) {
nearbyPlace.setQuadrant(3);
mQuad3Places.add(nearbyPlace);
} else {
nearbyPlace.setQuadrant(4);
mQuad4Places.add(nearbyPlace);
}
//Log.d("TAG", " - X: " + iconX + " y: " + iconY + " angle: " + angleToTarget + " display: " + nearbyPlace.getIcon_id());
}
drawQuadrant(mQuad1Places, canvas);
drawQuadrant(mQuad2Places, canvas);
drawQuadrant(mQuad3Places, canvas);
drawQuadrant(mQuad4Places, canvas);
}
}
It doesnot contain full code, but you may understand how pitch and azimuth with roll is used.. Best of luck
I'm having a really annoying problem with a AR view acting like a compass. So when I hold the phone in portrait (so that the screen is pointing to my face), then I call the remapCoordinateSystem that the pitch is 0 when holding it portrait. Then the azimuth (compass functionality) is perfect, but as soon as I tilt the phone the azimuth gets ruined, if I bend forward the azimuth increases and if I bend backwards it decreases.
I use 2 sensors to get the readings, Sensor.TYPE_MAGNETIC_FIELD and Sensor.TYPE_GRAVITY.
I use a lowpassfilter which is pretty basic, it's implemented with an alpha constant and is used directly on the read values from the sensors.
Here is my code:
float[] rotationMatrix = new float[9];
SensorManager.getRotationMatrix(rotationMatrix, null, gravitymeterValues,
magnetometerValues);
float[] remappedRotationMatrix = new float[9];
SensorManager.remapCoordinateSystem(rotationMatrix, SensorManager.AXIS_X,
SensorManager.AXIS_Z, remappedRotationMatrix);
float results[] = new float[3];
SensorManager.getOrientation(remappedRotationMatrix, results);
float azimuth = (float) (results[0] * 180 / Math.PI);
if (azimuth < 0) {
azimuth += 360;
}
float pitch = (float) (results[1] * 180 / Math.PI);
float roll = (float) (results[2] * 180 / Math.PI);
As you see there is no magic here. I call this piece of code when the gravitymeterValues and the magnetometerValues are ready to be used.
My question is how do I stop the azimuth from going crazy when I tilt the phone?
I checked a free app on the Google Play Store, Compass and it hasn't solved this problem, but I hope there is a solution.
I have 2 solutions in mind:
Make the AR view only work in very constrainted pitch angles, right now I have something like pitch >= -5 && pitch <= 30. If this isn't fullfilled the user is shown a screen that asks him/her to rotate the phone to portrait.
Somehow use the pitch to suppress the azimuth, this seems like a pretty device-specific solution though, but of course I'm open for suggestions.
I can also add that I've been searching for a couple of hours for a decent solution and I haven't found any that has given me any better solutions than 2) here.
Thanks in advance!
For complete code see https://github.com/hoananguyen/dsensor
Keep a history and average out, I do not know the correct interpretation of pitch and roll so the following code is for azimuth only.
Class members
private List<float[]> mRotHist = new ArrayList<float[]>();
private int mRotHistIndex;
// Change the value so that the azimuth is stable and fit your requirement
private int mHistoryMaxLength = 40;
float[] mGravity;
float[] mMagnetic;
float[] mRotationMatrix = new float[9];
// the direction of the back camera, only valid if the device is tilted up by
// at least 25 degrees.
private float mFacing = Float.NAN;
public static final float TWENTY_FIVE_DEGREE_IN_RADIAN = 0.436332313f;
public static final float ONE_FIFTY_FIVE_DEGREE_IN_RADIAN = 2.7052603f;
onSensorChanged
#Override
public void onSensorChanged(SensorEvent event)
{
if (event.sensor.getType() == Sensor.TYPE_GRAVITY)
{
mGravity = event.values.clone();
}
else
{
mMagnetic = event.values.clone();
}
if (mGravity != null && mMagnetic != null)
{
if (SensorManager.getRotationMatrix(mRotationMatrix, null, mGravity, mMagnetic))
{
// inclination is the degree of tilt by the device independent of orientation (portrait or landscape)
// if less than 25 or more than 155 degrees the device is considered lying flat
float inclination = (float) Math.acos(mRotationMatrix[8]);
if (inclination < TWENTY_FIVE_DEGREE_IN_RADIAN
|| inclination > ONE_FIFTY_FIVE_DEGREE_IN_RADIAN)
{
// mFacing is undefined, so we need to clear the history
clearRotHist();
mFacing = Float.NaN;
}
else
{
setRotHist();
// mFacing = azimuth is in radian
mFacing = findFacing();
}
}
}
}
private void clearRotHist()
{
if (DEBUG) {Log.d(TAG, "clearRotHist()");}
mRotHist.clear();
mRotHistIndex = 0;
}
private void setRotHist()
{
if (DEBUG) {Log.d(TAG, "setRotHist()");}
float[] hist = mRotationMatrix.clone();
if (mRotHist.size() == mHistoryMaxLength)
{
mRotHist.remove(mRotHistIndex);
}
mRotHist.add(mRotHistIndex++, hist);
mRotHistIndex %= mHistoryMaxLength;
}
private float findFacing()
{
if (DEBUG) {Log.d(TAG, "findFacing()");}
float[] averageRotHist = average(mRotHist);
return (float) Math.atan2(-averageRotHist[2], -averageRotHist[5]);
}
public float[] average(List<float[]> values)
{
float[] result = new float[9];
for (float[] value : values)
{
for (int i = 0; i < 9; i++)
{
result[i] += value[i];
}
}
for (int i = 0; i < 9; i++)
{
result[i] = result[i] / values.size();
}
return result;
}
I'm getting pitch and roll data from my device's gyroscope using this tutorial: http://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/
All the readings are extremely accurate (there's a filter applied to the code in the tutorial to eliminate gyro drift). Unfortunately, the code only works when my device is placed flat on a surface that is parallel to the ground. The most ideal position for my app to work would be with the top of the device pointing straight up (ie, the device is perpendicular to the ground with the screen facing the user). Whenever I orient my device in this position, the pitch values go to +90 degrees (as expected). What I would like to do is set this position as the 0 degree point (or initial position) for my device so that the pitch readings are 0 degrees when my device is oriented upright (in portrait mode) with the screen facing the user.
I asked the author of the tutorial with help on this issue and he responded:
"If you want to have the upright position as the initial one, you will have to rotate your frame of reference accordingly. The simplest way would be to rotate the resulting rotation matrix by -90 degrees about the x-axis. But you have to be careful about at which point in the algorithm to apply this rotation. Always remember that rotations are not commutative operations. To be more specific on this, I would have to review the code again, since I haven’t worked with it for a while now."
I'm really really confused and stumped as to how to rotate my frame of reference. I guess the bottom line is that I have no idea how to rotate the matrix by -90 degrees about the x-axis. If someone could help me out with this part, it would be fantastic. Here's my code in case anyone would like to refer to it:
public class AttitudeDisplayIndicator extends SherlockActivity implements SensorEventListener {
private SensorManager mSensorManager = null;
// angular speeds from gyro
private float[] gyro = new float[3];
// rotation matrix from gyro data
private float[] gyroMatrix = new float[9];
// orientation angles from gyro matrix
private float[] gyroOrientation = new float[3];
// magnetic field vector
private float[] magnet = new float[3];
// accelerometer vector
private float[] accel = new float[3];
// orientation angles from accel and magnet
private float[] accMagOrientation = new float[3];
// final orientation angles from sensor fusion
private float[] fusedOrientation = new float[3];
// accelerometer and magnetometer based rotation matrix
private float[] rotationMatrix = new float[9];
public static final float EPSILON = 0.000000001f;
private static final float NS2S = 1.0f / 1000000000.0f;
private float timestamp;
private boolean initState = true;
public static final int TIME_CONSTANT = 30;
public static final float FILTER_COEFFICIENT = 0.98f;
private Timer fuseTimer = new Timer();
// The following members are only for displaying the sensor output.
public Handler mHandler;
DecimalFormat d = new DecimalFormat("#.##");
//ADI background image.
private ImageView adiBackground;
//ADI axes.
private ImageView adiAxes;
//ADI frame.
private ImageView adiFrame;
//Layout.
private RelativeLayout layout;
//Pitch and Roll TextViews.
private TextView pitchAngleText;
private TextView bankAngleText;
//Instantaneous output values from sensors as the device moves.
public static double pitch;
public static double roll;
//Matrix for rotating the ADI (roll).
Matrix mMatrix = new Matrix();
#Override
public void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_attitude_display_indicator);
gyroOrientation[0] = 0.0f;
gyroOrientation[1] = 0.0f;
gyroOrientation[2] = 0.0f;
// initialise gyroMatrix with identity matrix
gyroMatrix[0] = 1.0f; gyroMatrix[1] = 0.0f; gyroMatrix[2] = 0.0f;
gyroMatrix[3] = 0.0f; gyroMatrix[4] = 1.0f; gyroMatrix[5] = 0.0f;
gyroMatrix[6] = 0.0f; gyroMatrix[7] = 0.0f; gyroMatrix[8] = 1.0f;
// get sensorManager and initialise sensor listeners
mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
initListeners();
// wait for one second until gyroscope and magnetometer/accelerometer
// data is initialised then scedule the complementary filter task
fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(),
1000, TIME_CONSTANT);
mHandler = new Handler();
adiBackground = (ImageView) findViewById(R.id.adi_background);
adiFrame = (ImageView) findViewById(R.id.adi_frame);
adiAxes = (ImageView) findViewById(R.id.adi_axes);
layout = (RelativeLayout) findViewById(R.id.adi_layout);
new Color();
layout.setBackgroundColor(Color.rgb(150, 150, 150));
pitchAngleText = (TextView) findViewById(R.id.pitch_angle_text);
bankAngleText = (TextView) findViewById(R.id.bank_angle_text);
}
// This function registers sensor listeners for the accelerometer, magnetometer and gyroscope.
public void initListeners(){
mSensorManager.registerListener(this,
mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),
SensorManager.SENSOR_DELAY_FASTEST);
mSensorManager.registerListener(this,
mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE),
SensorManager.SENSOR_DELAY_FASTEST);
mSensorManager.registerListener(this,
mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),
SensorManager.SENSOR_DELAY_FASTEST);
}
//#Override
public void onAccuracyChanged(Sensor sensor, int accuracy) {
}
//#Override
public void onSensorChanged(SensorEvent event) {
switch(event.sensor.getType()) {
case Sensor.TYPE_ACCELEROMETER:
// copy new accelerometer data into accel array and calculate orientation
System.arraycopy(event.values, 0, accel, 0, 3);
calculateAccMagOrientation();
break;
case Sensor.TYPE_GYROSCOPE:
// process gyro data
gyroFunction(event);
break;
case Sensor.TYPE_MAGNETIC_FIELD:
// copy new magnetometer data into magnet array
System.arraycopy(event.values, 0, magnet, 0, 3);
break;
}
}
// calculates orientation angles from accelerometer and magnetometer output
public void calculateAccMagOrientation() {
if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) {
SensorManager.getOrientation(rotationMatrix, accMagOrientation);
}
}
// This function is borrowed from the Android reference
// at http://developer.android.com/reference/android/hardware/SensorEvent.html#values
// It calculates a rotation vector from the gyroscope angular speed values.
private void getRotationVectorFromGyro(float[] gyroValues,
float[] deltaRotationVector,
float timeFactor)
{
float[] normValues = new float[3];
// Calculate the angular speed of the sample
float omegaMagnitude =
(float)Math.sqrt(gyroValues[0] * gyroValues[0] +
gyroValues[1] * gyroValues[1] +
gyroValues[2] * gyroValues[2]);
// Normalize the rotation vector if it's big enough to get the axis
if(omegaMagnitude > EPSILON) {
normValues[0] = gyroValues[0] / omegaMagnitude;
normValues[1] = gyroValues[1] / omegaMagnitude;
normValues[2] = gyroValues[2] / 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 * timeFactor;
float sinThetaOverTwo = (float)Math.sin(thetaOverTwo);
float cosThetaOverTwo = (float)Math.cos(thetaOverTwo);
deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
deltaRotationVector[3] = cosThetaOverTwo;
}
// This function performs the integration of the gyroscope data.
// It writes the gyroscope based orientation into gyroOrientation.
public void gyroFunction(SensorEvent event) {
// don't start until first accelerometer/magnetometer orientation has been acquired
if (accMagOrientation == null)
return;
// initialisation of the gyroscope based rotation matrix
if(initState) {
float[] initMatrix = new float[9];
initMatrix = getRotationMatrixFromOrientation(accMagOrientation);
float[] test = new float[3];
SensorManager.getOrientation(initMatrix, test);
gyroMatrix = matrixMultiplication(gyroMatrix, initMatrix);
initState = false;
}
// copy the new gyro values into the gyro array
// convert the raw gyro data into a rotation vector
float[] deltaVector = new float[4];
if(timestamp != 0) {
final float dT = (event.timestamp - timestamp) * NS2S;
System.arraycopy(event.values, 0, gyro, 0, 3);
getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
}
// measurement done, save current time for next interval
timestamp = event.timestamp;
// convert rotation vector into rotation matrix
float[] deltaMatrix = new float[9];
SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);
// apply the new rotation interval on the gyroscope based rotation matrix
gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);
// get the gyroscope based orientation from the rotation matrix
SensorManager.getOrientation(gyroMatrix, gyroOrientation);
}
private float[] getRotationMatrixFromOrientation(float[] o) {
float[] xM = new float[9];
float[] yM = new float[9];
float[] zM = new float[9];
float sinX = (float)Math.sin(o[1]);
float cosX = (float)Math.cos(o[1]);
float sinY = (float)Math.sin(o[2]);
float cosY = (float)Math.cos(o[2]);
float sinZ = (float)Math.sin(o[0]);
float cosZ = (float)Math.cos(o[0]);
// rotation about x-axis (pitch)
xM[0] = 1.0f; xM[1] = 0.0f; xM[2] = 0.0f;
xM[3] = 0.0f; xM[4] = cosX; xM[5] = sinX;
xM[6] = 0.0f; xM[7] = -sinX; xM[8] = cosX;
// rotation about y-axis (roll)
yM[0] = cosY; yM[1] = 0.0f; yM[2] = sinY;
yM[3] = 0.0f; yM[4] = 1.0f; yM[5] = 0.0f;
yM[6] = -sinY; yM[7] = 0.0f; yM[8] = cosY;
// rotation about z-axis (azimuth)
zM[0] = cosZ; zM[1] = sinZ; zM[2] = 0.0f;
zM[3] = -sinZ; zM[4] = cosZ; zM[5] = 0.0f;
zM[6] = 0.0f; zM[7] = 0.0f; zM[8] = 1.0f;
// rotation order is y, x, z (roll, pitch, azimuth)
float[] resultMatrix = matrixMultiplication(xM, yM);
resultMatrix = matrixMultiplication(zM, resultMatrix);
return resultMatrix;
}
private float[] matrixMultiplication(float[] A, float[] B) {
float[] result = new float[9];
result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];
result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];
return result;
}
class calculateFusedOrientationTask extends TimerTask {
public void run() {
float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT;
/*
* Fix for 179° <--> -179° transition problem:
* Check whether one of the two orientation angles (gyro or accMag) is negative while the other one is positive.
* If so, add 360° (2 * math.PI) to the negative value, perform the sensor fusion, and remove the 360° from the result
* if it is greater than 180°. This stabilizes the output in positive-to-negative-transition cases.
*/
// azimuth
if (gyroOrientation[0] < -0.5 * Math.PI && accMagOrientation[0] > 0.0) {
fusedOrientation[0] = (float) (FILTER_COEFFICIENT * (gyroOrientation[0] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[0]);
fusedOrientation[0] -= (fusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0;
}
else if (accMagOrientation[0] < -0.5 * Math.PI && gyroOrientation[0] > 0.0) {
fusedOrientation[0] = (float) (FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * (accMagOrientation[0] + 2.0 * Math.PI));
fusedOrientation[0] -= (fusedOrientation[0] > Math.PI)? 2.0 * Math.PI : 0;
}
else {
fusedOrientation[0] = FILTER_COEFFICIENT * gyroOrientation[0] + oneMinusCoeff * accMagOrientation[0];
}
// pitch
if (gyroOrientation[1] < -0.5 * Math.PI && accMagOrientation[1] > 0.0) {
fusedOrientation[1] = (float) (FILTER_COEFFICIENT * (gyroOrientation[1] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[1]);
fusedOrientation[1] -= (fusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0;
}
else if (accMagOrientation[1] < -0.5 * Math.PI && gyroOrientation[1] > 0.0) {
fusedOrientation[1] = (float) (FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * (accMagOrientation[1] + 2.0 * Math.PI));
fusedOrientation[1] -= (fusedOrientation[1] > Math.PI)? 2.0 * Math.PI : 0;
}
else {
fusedOrientation[1] = FILTER_COEFFICIENT * gyroOrientation[1] + oneMinusCoeff * accMagOrientation[1];
}
// roll
if (gyroOrientation[2] < -0.5 * Math.PI && accMagOrientation[2] > 0.0) {
fusedOrientation[2] = (float) (FILTER_COEFFICIENT * (gyroOrientation[2] + 2.0 * Math.PI) + oneMinusCoeff * accMagOrientation[2]);
fusedOrientation[2] -= (fusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0;
}
else if (accMagOrientation[2] < -0.5 * Math.PI && gyroOrientation[2] > 0.0) {
fusedOrientation[2] = (float) (FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * (accMagOrientation[2] + 2.0 * Math.PI));
fusedOrientation[2] -= (fusedOrientation[2] > Math.PI)? 2.0 * Math.PI : 0;
}
else {
fusedOrientation[2] = FILTER_COEFFICIENT * gyroOrientation[2] + oneMinusCoeff * accMagOrientation[2];
}
// overwrite gyro matrix and orientation with fused orientation
// to comensate gyro drift
gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
// update sensor output in GUI
mHandler.post(updateOrientationDisplayTask);
}
}
Thanks in advance for your help!
The theory...
I'm not really sure about the format in which your "frame of reference" matrix is represented, but typically rotations are done with matrix multiplication.
Basically, you would take your "frame of reference matrix" and multiply it by a 90 degrees rotation matrix.
Such a matrix can be found on Wikipedia:
Three-dimensional rotation matrices
Since your angle is 90 degrees, your sines and cosines would resolve to 1's or 0's which you can plug directly into the matrix instead of computing the sines and cosines. For example, a matrix that would rotate 90 degrees counter-clockwise about the x axis would look like this:
1 0 0
0 0 1
0 -1 0
Also, please not that matrices like these operate on row vectors of x y z coordinates.
So for example, if you have a point in space that is at (2,5,7) and you would like to rotate it using the above matrix, you would have to do the following operation:
|2 5 7| |1 0 0|
|0 0 1|
|0 -1 0|
Which gives [2 -7 5]
...applied to your code
I have glanced quickly at your code and it seems like the modification you need to make involves the output of calculateAccMagOrientation() because it is used to initialize the orientation of the device.
1: public void calculateAccMagOrientation() {
2: if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) {
3: SensorManager.getOrientation(rotationMatrix, accMagOrientation);
4: }
5: }
At line 2 in the above snippet is where you get your initial rotationMatrix. Try multiplying rotationMatrix by a hand crafted 90 degrees rotation matrix before calling getOrientation at line 3. I think this will effectively re-align your reference orientation:
public void calculateAccMagOrientation() {
if(SensorManager.getRotationMatrix(rotationMatrix, null, accel, magnet)) {
rotationMatrix = matrixMultiplication(rotationMatrix, my90DegRotationMatrix);
SensorManager.getOrientation(rotationMatrix, accMagOrientation);
}
}
Please note that depending on how the angles work in Android, you might need to use a 90 degrees clockwise rotation matrix instead of a counter-clockwise.
Alternative solution
It just occurred to me, maybe you could also simply subtract 90 from the final pitch result before displaying it?
My app needs to show the current bearing of the device using its compass. The code I'm using (below) works perfectly fine on my Galaxy Nexus and Galaxy One, but the compass is spinning around wildly on a Samsung Galaxy S III. I've tried doing a figure-8 to recalibrate the device, but that doesn't change anything. The weird thing is that other compass apps downloaded from Google Play work just fine on the SIII. What could be the issue here?
float[] mGravity;
float[] mGeomagnetic;
public void onSensorChanged( SensorEvent event ) {
float azimuth = 0f;
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
mGravity = event.values;
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
mGeomagnetic = event.values;
if (mGravity != null && mGeomagnetic != null) {
float R[] = new float[9];
float I[] = new float[9];
boolean success = SensorManager.getRotationMatrix(R, I, mGravity, mGeomagnetic);
if (success) {
float orientation[] = new float[3];
SensorManager.getOrientation(R, orientation);
azimuth = orientation[0]; // orientation contains: azimut, pitch and roll
}
}
//Discard 0.0-values
if(azimuth == 0.0) { return; }
//Convert the sensor value to degrees
azimuth = (float) Math.toDegrees(azimuth); //same as azimuth = -azimuth*360/(2*3.14159f);
//Smooth the sensor-output
azimuth = smoothValues(azimuth);
}
//From http://stackoverflow.com/questions/4699417/android-compass-orientation-on-unreliable-low-pass-filter
//SmoothFactorCompass: The easing float that defines how smooth the movement will be (1 is no smoothing and 0 is never updating, my default is 0.5).
//SmoothThresholdCompass: The threshold in which the distance is big enough to turn immediately (0 is jump always, 360 is never jumping, my default is 30).
static final float SmoothFactorCompass = 0.5f;
static final float SmoothThresholdCompass = 30.0f;
float oldCompass = 0.0f;
private float smoothValues (float newCompass){
if (Math.abs(newCompass - oldCompass) < 180) {
if (Math.abs(newCompass - oldCompass) > SmoothThresholdCompass) {
oldCompass = newCompass;
}
else {
oldCompass = oldCompass + SmoothFactorCompass * (newCompass - oldCompass);
}
}
else {
if (360.0 - Math.abs(newCompass - oldCompass) > SmoothThresholdCompass) {
oldCompass = newCompass;
}
else {
if (oldCompass > newCompass) {
oldCompass = (oldCompass + SmoothFactorCompass * ((360 + newCompass - oldCompass) % 360) + 360) % 360;
}
else {
oldCompass = (oldCompass - SmoothFactorCompass * ((360 - newCompass + oldCompass) % 360) + 360) % 360;
}
}
}
return oldCompass;
}
Currently I am investigating compass mechanism on Android and I would recommend to start with low-pass filter in your case.
What you need to do - is to apply low-pass filter to both ACCELEROMETER and MAGNETIC_FIELD sensors data.
Here is how I implemented that:
private float[] accel;
private float[] geomagnetic;
float R[] = new float[9];
float I[] = new float[9];
float orientation[] = new float[3];
#Override
public void onSensorChanged(SensorEvent event)
{
synchronized (this)
{
float azimuth = -1f;
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
accel = lowPass( event.values.clone(), accel );
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
geomagnetic = lowPass(event.values.clone(), geomagnetic);
if (accel != null && geomagnetic != null)
{
boolean success = SensorManager.getRotationMatrix(R, I,
accel, geomagnetic);
SensorManager.remapCoordinateSystem(R,
SensorManager.AXIS_X, SensorManager.AXIS_Z, R);
if (success)
{
SensorManager.getOrientation(R, orientation);
azimuth = orientation[0]; // orientation contains:
// azimuth, pitch
// and roll
float newHeading = azimuth * 360 / (2 * 3.14159f);
//do what you need to do with new heading
}
}
}
}
/*
* time smoothing constant for low-pass filter 0 ≤ alpha ≤ 1 ; a smaller
* value basically means more smoothing See:
* http://en.wikipedia.org/wiki/Low-pass_filter#Discrete-time_realization
*/
static final float ALPHA = 0.15f;
/**
* #see http
* ://en.wikipedia.org/wiki/Low-pass_filter#Algorithmic_implementation
* #see http
* ://developer.android.com/reference/android/hardware/SensorEvent.html
* #values
*/
protected float[] lowPass(float[] input, float[] output)
{
if (output == null)
return input;
for (int i = 0; i < input.length; i++)
{
output[i] = output[i] + ALPHA * (input[i] - output[i]);
}
return output;
}
may be you should clone the value from the sensor reading first and then lowpass the filter if this does not work , I know SONY XPeria phone is very sensitive unlike samsung which is quite stable .
In my view , S3 reading is quite ok under android OS 4.3
event.values.clone;
My recent experience couple data feedback force me to apply rotation vector logic on SONY phone loaded with Invesense sensor and it seemed to be working , although I still do not like the choppy response but I have live with that kind of response on SONY android 4.3 OS . It is also appear to me that any phone loaded invensense sensor need to apply the rotation vector for it to work properly