Valeurs 2D de Quaternion

Question

Je travaille dans une application AR dans Android avec l'Epson Moverio BT-200. J'ai un quaternion qui change ses valeurs avec mon algorithme de fusion de capteur. Dans mon application, j'essaie de déplacer un élément 2D en changeant ses valeurs de marge gauche et marge supérieure lorsque je bouge la tête. Je voudrais savoir comment puis-je extraire, à partir des valeurs du quaternion, uniquement les mouvements "horizontaux" et "verticaux".

Je pouvais extraire du quaternion les valeurs de tangage et de roulis, mais j'ai lu qu'il y a plusieurs problèmes d'angle euler. Est-ce que je pourrais le faire seulement en travaillant avec des quaternions?

Answer 1

Ceci est mon code actuel. J'ai résolu le problème en utilisant les Quaternions pour l'algorithme, et à la fin j'extrais les angles euler de la matrice de rotation.

C'est l'algorithme pour prendre les valeurs des capteurs:

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()); 
    } 
} 

Et voilà comment je prends les angles d'Euler valeurs de rotation:

 /** 
    * @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]); 

    } 

Je résolu mon problème avec cette solution. Maintenant, il ne sera pas difficile de déplacer mon objet 2d avec ces valeurs de capteurs. Désolé pour la longueur de ma réponse, mais j'espère que cela pourrait être utile pour quelqu'un :)