问题
I'm writing an Android app that requires the rotation vector. I'd like to use the TYPE_ROTATION_VECTOR but in some of my test devices the magnetometer doesn't perform well to say the least. Instead, the TYPE_GAME_ROTATION_VECTOR provides much smoother data (but I can't get direction relative to the Earth). What I ended up doing is while my data is loading, I run both virtual sensors. I now have an average quaternion for both, call them R (TYPE_ROTATION_VECTOR) and Rg (TYPE_GAME_ROTATION_VECTOR).
Once calibration is over I only run the TYPE_GAME_ROTATION_VECTOR, but would like to correct it for North. What I think I can do is something like: R = Rg * C where C is my calibration and Rg is the new TYPE_GAME_ROTATION_VECTOR data after a low pass filter. What I tried:
1. R = Rg * C
2. R * R' = Rg * C * R'
3. U = Rg * C * R' // Here U is the unit quaternion
4. C * R' = Rg' // This is because quaternion multiplication is associative
// Rg * (C * R') = U from line 3 therefore (C * R') must be
// equal to the conjugate of Rg
5. C = Rg' * R'' // I found this online somewhere (I hope this is right)
6. C = Rg' * R // R'' is just R
Now that I have C, I can take new values (after low pass filter) for the TYPE_GAME_ROTATION_VECTOR multiply them by C and get the actual rotation quaternion R that should be similar to the one that would have been provided by the TYPE_ROTATION_VECTOR with a steady North.
This gets me pretty close, but it doesn't quite work. I'm testing using a very simple AR like app that shows an item (who's position is determined by the device orientation) floating on the screen. If I leave out the calibration the character shows up and tracks perfectly, but it doesn't show up North of me (I have it fixed at (0, 1, 0) for now). If I take the rotation vector, get the quaternion, multiply by the calibration constant, the tracking gets thrown off:
- Rotating the device about the Y axis shifts the item correctly horizontally, but it also adds a vertical component where rotating in the positive direction (using right hand rule) moves my item up (negative Y on the screen).
- Rotating the device about the X axis shifts the item correctly vertically, but it also adds a horizontal component where rotation in the positive direction (using right hand rule) moves my item right (positive X on the screen).
- Rotating the device about the Z axis works.
Sorry for the long description, I just want to make sure all the details are there. Summary of the question: I want to be able to get a rotation matrix that is roughly north and avoid using the magnetometer. I'm trying to do this by taking the average difference between TYPE_ROTATION_VECTOR and TYPE_GAME_ROTATION_VECTOR and using that to "calibrate" future values from the TYPE_GAME_ROTATION_VECTOR but it doesn't work. Does anyone know what the issue might be with how I'm calculating the calibration (or any other part of this)?
Some additional info:
private float[] values = null
public void onSensorChanged(SensorEvent event) {
values = lowPass(event.values.clone(), values);
Quaternion rawQuaternion = Quaternion.fromRotationVector(values);
Quaternion calibratedQuaternion = rawQuaternion.mult(calibration);
float[] rotationMatrix = calibratedQuaternion.getRotationMatrix();
float[] pos = new float[] { 0f, 1f, 0f, 1f };
Matrix.multiplyMV(pos, 0, rotationMatrix, 0, pos, 0);
Matrix.multiplyMV(pos, 0, matrixMVP, 0, pos, 0);
// Screen position should be found at pos[0], -pos[1] on a [-1,1] scale
}
Quaternion fromRotationVector(float[] r) {
float[] Q = new float[4];
SensorManager.getQuaternionFromVector(Q, r);
return new Quaternion(Q);
}
Quaternion mult(Quaternion q) {
Quaternion qu = new Quaternion();
qu.w = w*q.w - x*q.x - y*q.y - z*q.z;
qu.x = w*q.x + x*q.w + y*q.z - z*q.y;
qu.y = w*q.y + y*q.w + z*q.x - x*q.z;
qu.z = w*q.z + z*q.w + x*q.y - y*q.x;
return qu;
}
float[] getRotationMatrix() {
float[] M = new float[16];
float[] V = new float[] { x, y, z, w };
SensorManager.getRotationMatrixFromVector(M, V);
return M;
}
回答1:
I had the same issue and did some research and realized where the problem is. So basically, by only looking at a stationary orientation of the IMU, you only align one axis of the coordinate system which is the vertical axis in the direction of gravity. That's why you rotations around Z axis works fine.
To complete your static calibrations, you have to include a planar motion and find the principal vectors of the motion which will be the, say, your X axis. Y axis follows the right-hand rule.
Simply, rotate the IMU around the global X axis and look at the gyroscope outputs of your IMU. The principal component of your gyroscope should be towards the X axis. After finding the Z axis in the first step and X axis in the second step, you can find Y axis by the cross product of the two. Using these axes, create the rotation matrix or the quaternion for the translations.
回答2:
Here's what I ended up doing (there are some changes coming soon and once done I'll publish it on jcenter as a library). What this tries to solve is being able to run the Game Rotation Vector sensor (which has much less drift than the Rotation Vector sensor) while still pointing roughly north. Answer is in Kotlin:
class RotationMatrixLiveData(context Context): LiveData<FloatArray>(), SensorEventListener {
private val sensorManager = context.getSystemService(Context.SENSOR_SERVICE) as SensorManager
private val rotationSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR)
private val gameRotationSensor =
if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.JELLY_BEAN_MR2)
sensorManager.getDefaultSensor(Sensor.TYPE_GAME_ROTATION_VECTOR)
else null
private var isActive = false
private var isCalibrating = false
private var rotationValues: FloatArray? = null
var calibrationCount = 0
var calibrationQuaternion: FloatArray? = null
var calibrationGameCount = 0
var calibrationGameQuat: FloatArray? = null
var calibration: Quaternion? = null
var rotationQuaternionValues = FloatArray(4)
var gameQuaternionValues = FloatArray(4)
private val rotationVectorQuaternion = Quaternion()
init {
value = floatArrayOf(
1f, 0f, 0f, 0f,
0f, 1f, 0f, 0f,
0f, 0f, 1f, 0f,
0f, 0f, 0f, 1f)
}
/**
* Starts calibrating the rotation matrix (if the game rotation vector sensor
* is available.
*/
fun beginCalibration() {
gameRotationSensor?.let {
isCalibrating = true
calibration = null
calibrationQuaternion = null
calibrationCount = 0
calibrationGameQuat = null
calibrationGameCount = 0
sensorManager.registerListener(this, rotationSensor, SensorManager.SENSOR_DELAY_FASTEST)
sensorManager.registerListener(this, it, SensorManager.SENSOR_DELAY_FASTEST)
}
}
/**
* Stop calibrating the rotation matrix.
*/
fun stopCalibration() {
isCalibrating = false
if (!isActive) {
// Not active, just turn off everthing
sensorManager.unregisterListener(this)
} else if (gameRotationSensor != null) {
// Active and has both sensors, turn off rotation and leave the game rotation running
sensorManager.unregisterListener(this, rotationSensor)
}
}
override fun onActive() {
super.onActive()
isActive = true
val sensor = gameRotationSensor ?: rotationSensor
sensorManager.registerListener(this, sensor, SensorManager.SENSOR_DELAY_FASTEST)
}
override fun onInactive() {
super.onInactive()
isActive = false
if (!isCalibrating) {
sensorManager.unregisterListener(this)
}
}
//
// SensorEventListener
//
override fun onAccuracyChanged(sensor: Sensor, accuracy: Int) {}
override fun onSensorChanged(event: SensorEvent) {
if (isCalibrating) {
if (event.sensor.type == Sensor.TYPE_ROTATION_VECTOR) {
SensorManager.getQuaternionFromVector(rotationQuaternionValues, event.values)
calibrationQuaternion?.let { quat ->
for (i in 0..3) {
rotationQuaternionValues[i] += quat[i]
}
}
calibrationQuaternion = rotationQuaternionValues
calibrationCount++
} else if (event.sensor.type == Sensor.TYPE_GAME_ROTATION_VECTOR) {
SensorManager.getQuaternionFromVector(gameQuaternionValues, event.values)
calibrationGameQuat?.let {quat ->
for (i in 0..3) {
gameQuaternionValues[i] += quat[i]
}
}
calibrationGameQuat = gameQuaternionValues
calibrationGameCount++
}
} else if (gameRotationSensor == null || event.sensor.type != Sensor.TYPE_ROTATION_VECTOR) {
// Only calculate rotation if there is no game rotation sensor or if the event is a game
// rotation
val calibrationQ = calibrationQuaternion
val calibrationQg = calibrationGameQuat
if (calibrationQ != null && calibrationQg != null) {
for (i in 0..3) {
calibrationQ[i] /= calibrationCount.toFloat()
calibrationQg[i] /= calibrationGameCount.toFloat()
}
calibration = (Quaternion(calibrationQg).apply { conjugate() } *
Quaternion(calibrationQ)).apply {
x = 0f
y = 0f
normalize()
}
}
calibrationQuaternion = null
calibrationGameQuat = null
// Run values through low-pass filter
val values = lowPass(event.values, rotationValues)
rotationValues = values
rotationVectorQuaternion.setFromRotationVector(values)
// Calibrate if available
calibration?.let { rotationVectorQuaternion.preMult(it) }
// Generate rotation matrix
value = rotationVectorQuaternion.getRotationMatrix(value)
}
}
}
For the quaternion class I'm using:
class Quaternion(val values: FloatArray = floatArrayOf(1f, 0f, 0f, 0f)) {
companion object {
fun fromRotationVector(rv: FloatArray): Quaternion {
val Q = FloatArray(4)
SensorManager.getQuaternionFromVector(Q, rv)
return Quaternion(Q)
}
}
private val buffer = FloatArray(4)
var w: Float
get() = values[0]
set(value) { values[0] = value }
var x: Float
get() = values[1]
set(value) { values[1] = value }
var y: Float
get() = values[2]
set(value) { values[2] = value }
var z: Float
get() = values[3]
set(value) { values[3] = value }
fun setFromRotationVector(rv: FloatArray) {
SensorManager.getQuaternionFromVector(values, rv)
}
fun conjugate() {
x = -x
y = -y
z = -z
}
fun getRotationMatrix(R: FloatArray? = null): FloatArray {
val matrix = R ?: FloatArray(16)
for (i in 0..3) {
buffer[i] = values[(i+1)%4]
}
SensorManager.getRotationMatrixFromVector(matrix, buffer)
return matrix
}
fun magnitude(): Float {
var mag = 0f
for (i in 0..3) {
mag += values[i]*values[i]
}
return Math.sqrt(mag.toDouble()).toFloat()
}
fun normalize() {
val mag = magnitude()
x /= mag
y /= mag
z /= mag
w /= mag
}
fun preMult(left: Quaternion) {
buffer[0] = left.w*this.w - left.x*this.x - left.y*this.y - left.z*this.z
buffer[1] = left.w*this.x + left.x*this.w + left.y*this.z - left.z*this.y
buffer[2] = left.w*this.y + left.y*this.w + left.z*this.x - left.x*this.z
buffer[3] = left.w*this.z + left.z*this.w + left.x*this.y - left.y*this.x
for (i in 0..3) {
values[i] = buffer[i]
}
}
operator fun times(q: Quaternion): Quaternion {
val qu = Quaternion()
qu.w = w*q.w - x*q.x - y*q.y - z*q.z
qu.x = w*q.x + x*q.w + y*q.z - z*q.y
qu.y = w*q.y + y*q.w + z*q.x - x*q.z
qu.z = w*q.z + z*q.w + x*q.y - y*q.x
return qu
}
operator fun times(v: FloatArray): FloatArray {
val conj = Quaternion(values.clone()).apply { conjugate() }
return multiplyQV(multiplyQV(values, v), conj.values)
}
override fun toString(): String {
return "(${w.toString(5)}(w), ${x.toString(5)}, ${y.toString(5)}, ${z.toString(5)}) |${magnitude().toString(5)}|"
}
private fun multiplyQV(q: FloatArray, r: FloatArray): FloatArray {
val result = FloatArray(4)
result[0] = r[0]*q[0]-r[1]*q[1]-r[2]*q[2]-r[3]*q[3]
result[1] = r[0]*q[1]+r[1]*q[0]-r[2]*q[3]+r[3]*q[2]
result[2] = r[0]*q[2]+r[1]*q[3]+r[2]*q[0]-r[3]*q[1]
result[3] = r[0]*q[3]-r[1]*q[2]+r[2]*q[1]+r[3]*q[0]
return result
}
}
来源：https://stackoverflow.com/questions/44796304/solving-for-calibration-quaternion