yingtongwang/sensor-fusion-demo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

use temporary variables instead of new allocations

Browse Source
This commit is contained in:
Cerrato Renaud
2016-09-12 13:26:14 -04:00
parent a1a1113e50
commit e2612aabd1
5 changed files with 56 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/AccelerometerCompassProvider.java
View File

@@ -16,12 +16,17 @@ public class AccelerometerCompassProvider extends OrientationProvider {
/**
* Compass values
*/
private float[] magnitudeValues = new float[3];
final private float[] magnitudeValues = new float[3];
/**
* Accelerometer values
*/
private float[] accelerometerValues = new float[3];
final private float[] accelerometerValues = new float[3];
/**
* Temporary variable to save allocations
*/
final float[] tmpFloat = new float[16];
/**
* Initialises a new AccelerometerCompassProvider
@@ -42,16 +47,14 @@ public class AccelerometerCompassProvider extends OrientationProvider {
// we received a sensor event. it is a good practice to check
// that we received the proper event
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
magnitudeValues = event.values.clone();
System.arraycopy(event.values, 0, magnitudeValues, 0, magnitudeValues.length);
} else if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
accelerometerValues = event.values.clone();
System.arraycopy(event.values, 0, accelerometerValues, 0, accelerometerValues.length);
}
if (magnitudeValues != null && accelerometerValues != null) {
float[] i = new float[16];
// Fuse accelerometer with compass
SensorManager.getRotationMatrix(currentOrientationRotationMatrix.matrix, i, accelerometerValues,
SensorManager.getRotationMatrix(currentOrientationRotationMatrix.matrix, tmpFloat, accelerometerValues,
magnitudeValues);
// Transform rotation matrix to quaternion
currentOrientationQuaternion.setRowMajor(currentOrientationRotationMatrix.matrix);

7
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/CalibratedGyroscopeProvider.java
View File

@@ -54,6 +54,11 @@ public class CalibratedGyroscopeProvider extends OrientationProvider {
*/
private double gyroscopeRotationVelocity = 0;
/**
* Temporary variable to save allocations.
*/
private Quaternion correctedQuat = new Quaternion();
/**
* Initialises a new CalibratedGyroscopeProvider
*
@@ -110,7 +115,7 @@ public class CalibratedGyroscopeProvider extends OrientationProvider {
deltaQuaternion.multiplyByQuat(currentOrientationQuaternion, currentOrientationQuaternion);
}
Quaternion correctedQuat = currentOrientationQuaternion.clone();
correctedQuat.set(currentOrientationQuaternion);
// 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());

17
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/GravityCompassProvider.java
View File

@@ -16,12 +16,17 @@ public class GravityCompassProvider extends OrientationProvider {
/**
* Compass values
*/
private float[] magnitudeValues = new float[3];
final private float[] magnitudeValues = new float[3];
/**
* Gravity values
*/
private float[] gravityValues = new float[3];
final private float[] gravityValues = new float[3];
/**
* Temporary variable to save some allocations
*/
float[] tmpFloat = new float[16];
/**
* Initialises a new GravityCompassProvider
@@ -42,16 +47,14 @@ public class GravityCompassProvider extends OrientationProvider {
// we received a sensor event. it is a good practice to check
// that we received the proper event
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
magnitudeValues = event.values.clone();
System.arraycopy(event.values, 0, magnitudeValues, 0, magnitudeValues.length);
} else if (event.sensor.getType() == Sensor.TYPE_GRAVITY) {
gravityValues = event.values.clone();
System.arraycopy(event.values, 0, gravityValues, 0, gravityValues.length);
}
if (magnitudeValues != null && gravityValues != null) {
float[] i = new float[16];
// Fuse gravity-sensor (virtual sensor) with compass
SensorManager.getRotationMatrix(currentOrientationRotationMatrix.matrix, i, gravityValues, magnitudeValues);
SensorManager.getRotationMatrix(currentOrientationRotationMatrix.matrix, tmpFloat, gravityValues, magnitudeValues);
// Transform rotation matrix to quaternion
currentOrientationQuaternion.setRowMajor(currentOrientationRotationMatrix.matrix);
}

26
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/ImprovedOrientationSensor1Provider.java
View File

@@ -126,6 +126,13 @@ public class ImprovedOrientationSensor1Provider extends OrientationProvider {
*/
private static final int PANIC_THRESHOLD = 60;
/**
* Some temporary variables to save allocations
*/
final private float[] tmpFloat = new float[4];
final private Quaternion correctedQuaternion = new Quaternion();
final private Quaternion interpolateQuaternion = new Quaternion();
/**
* Initialises a new ImprovedOrientationSensor1Provider
*
@@ -144,13 +151,11 @@ public class ImprovedOrientationSensor1Provider extends OrientationProvider {
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);
SensorManager.getQuaternionFromVector(tmpFloat, event.values);
// Store in quaternion
quaternionRotationVector.setXYZW(q[1], q[2], q[3], -q[0]);
quaternionRotationVector.setXYZW(tmpFloat[1], tmpFloat[2], tmpFloat[3], -tmpFloat[0]);
if (!positionInitialised) {
// Override
quaternionGyroscope.set(quaternionRotationVector);
@@ -213,13 +218,12 @@ public class ImprovedOrientationSensor1Provider extends OrientationProvider {
// 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);
quaternionGyroscope.slerp(quaternionRotationVector, interpolateQuaternion, DIRECT_INTERPOLATION_WEIGHT);
// Use the interpolated value between gyro and rotationVector
setOrientationQuaternionAndMatrix(interpolate);
setOrientationQuaternionAndMatrix(interpolateQuaternion);
// Override current gyroscope-orientation
quaternionGyroscope.copyVec4(interpolate);
quaternionGyroscope.copyVec4(interpolateQuaternion);
// Reset the panic counter because both sensors are saying the same again
panicCounter = 0;
@@ -257,17 +261,17 @@ public class ImprovedOrientationSensor1Provider extends OrientationProvider {
* @param quaternion The Quaternion to set (the result of the sensor fusion)
*/
private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
Quaternion correctedQuat = quaternion.clone();
correctedQuaternion.set(quaternion);
// 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());
correctedQuaternion.w(-correctedQuaternion.w());
synchronized (syncToken) {
// Use gyro only
currentOrientationQuaternion.copyVec4(quaternion);
// Set the rotation matrix as well to have both representations
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuat.array());
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuaternion.array());
}
}
}

26
app/src/main/java/org/hitlabnz/sensor_fusion_demo/orientationProvider/ImprovedOrientationSensor2Provider.java
View File

@@ -126,6 +126,13 @@ public class ImprovedOrientationSensor2Provider extends OrientationProvider {
*/
private static final int PANIC_THRESHOLD = 60;
/**
* Some temporary variable to save allocations.
*/
final private Quaternion correctedQuaternion = new Quaternion();
final private Quaternion interpolateQuaternion = new Quaternion();
final private float[] tmpFloat = new float[4];
/**
* Initialises a new ImprovedOrientationSensor2Provider
*
@@ -144,13 +151,11 @@ public class ImprovedOrientationSensor2Provider extends OrientationProvider {
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);
SensorManager.getQuaternionFromVector(tmpFloat, event.values);
// Store in quaternion
quaternionRotationVector.setXYZW(q[1], q[2], q[3], -q[0]);
quaternionRotationVector.setXYZW(tmpFloat[1], tmpFloat[2], tmpFloat[3], -tmpFloat[0]);
if (!positionInitialised) {
// Override
quaternionGyroscope.set(quaternionRotationVector);
@@ -213,14 +218,13 @@ public class ImprovedOrientationSensor2Provider extends OrientationProvider {
// 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,
quaternionGyroscope.slerp(quaternionRotationVector, interpolateQuaternion,
(float) (INDIRECT_INTERPOLATION_WEIGHT * gyroscopeRotationVelocity));
// Use the interpolated value between gyro and rotationVector
setOrientationQuaternionAndMatrix(interpolate);
setOrientationQuaternionAndMatrix(interpolateQuaternion);
// Override current gyroscope-orientation
quaternionGyroscope.copyVec4(interpolate);
quaternionGyroscope.copyVec4(interpolateQuaternion);
// Reset the panic counter because both sensors are saying the same again
panicCounter = 0;
@@ -258,17 +262,17 @@ public class ImprovedOrientationSensor2Provider extends OrientationProvider {
* @param quaternion The Quaternion to set (the result of the sensor fusion)
*/
private void setOrientationQuaternionAndMatrix(Quaternion quaternion) {
Quaternion correctedQuat = quaternion.clone();
correctedQuaternion.set(quaternion);
// 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());
correctedQuaternion.w(-correctedQuaternion.w());
synchronized (syncToken) {
// Use gyro only
currentOrientationQuaternion.copyVec4(quaternion);
// Set the rotation matrix as well to have both representations
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuat.array());
SensorManager.getRotationMatrixFromVector(currentOrientationRotationMatrix.matrix, correctedQuaternion.array());
}
}
}