Эйлеровы углы и кватернионная ориентация

Question

как преобразовать выход ориентации Quaternion устройства wintracker II только в вывод углов Эйлера. Потому что устройство Wintracker II выводит углы Эйлера и ориентацию кватерниона. я хочу только вывод углов Эйлера.

Answer 1

Я реализовал алгоритм, описанный в этой статье, и он работает очень хорошо: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/quat_2_euler_paper_ver2-1.pdf

Проблема со статьей в Википедии, указанной в ответе № 1, заключается в том, что она предоставляет только формулу вращения X-Y-Z. Ссылка на статью дает общий алгоритм, который будет работать для любой из 12 последовательностей. Возможно, вам придется прочитать его несколько раз и определенно проработать пример. Следить за ним не так просто, но я проверил сопли, и он довольно пуленепробиваемый.

За первый комментарий, вот основные компоненты моего кода. Должно быть достаточно, чтобы вы начали:

Первый класс - AxisType. Основной функцией, которую я использую, является getNextCircular (). Это также облегчает преобразование моего кода в Векторы.

public enum AxisType {

X("X"),
Y("Y"),
Z("Z");

String label;

AxisType(final String label) {
         this.label = label;
}

/**
 * Converts an axis type to a vector.
 *
 * @return
 */
public Vector3D toVector3D() {
    if (equals(AxisType.X)) {
        return new Vector3D(1,0,0);
    } else if (equals(AxisType.Y)) {
        return new Vector3D(0,1,0);
    } else {
        return new Vector3D(0,0,1);
    }
}

/**
 * gets the next circular axis from this one circular: </br> <code>
 * X --> Y
 * </br>
 * Y --> Z
 * </br>
 * Z --> X
 * </code>
 *
 * @return
 */
public AxisType nextCircular() {
    if (equals(AxisType.X)) {
        return AxisType.Y;
    } else if (equals(AxisType.Y)) {
        return AxisType.Z;
    } else {
        return AxisType.X;
    }
}

@Override
public String toString() {
    return label;
}
}

За ним следует EulerOrder, который представляет определенный порядок осей (например, XYX, ZYX и т. Д.) И набор статических конструкторов. Это много шаблонного, но вот оно ...

public class EulerOrder
{
private final AxisType[] axisOrder;

/**
 * generic constructor
 * 
 * @param first
 * @param second
 * @param third
 */
public EulerOrder(
        final AxisType first,
        final AxisType second,
        final AxisType third )
{
    axisOrder = new AxisType[] {
        first,
        second,
        third
    };
}

/**
 * @return the cartesian axis that represent this sequence
 */
public Vector3D[] orderedAxis()
{
    return new Vector3D[] {
        axisOrder[0].toVector3D(),
        axisOrder[1].toVector3D(),
        axisOrder[2].toVector3D()
    };
}

public AxisType getAxisType(
        final int index )
{

    if ((index > 2) || (index < 0))
    {
        throw new ArrayIndexOutOfBoundsException(
                "EulerOrder[index] called with an invalid index");
    }

    return axisOrder[index];
}

/**
 * <code>
 * X->Y->*
 * </br>
 * Y->Z->*
 * </br> 
 * Z->X->* 
 * </code>
 * 
 * @return true if the first two rotations are in a circular order
 */
public boolean isCircular()
{
    // true if the first 2 roations are in one of these orders
    // X-Y
    // Y-Z
    // Z-X
    return axisOrder[0].nextCircular().equals(
            axisOrder[1]);
}

/**
 * <code>
 * X->*->X
 * </br>
 * Y->*->Y
 * </br>
 * Z->*->Z
 * </code>
 * 
 * @return true if the first and last axis are the same
 */
public boolean isRepeating()
{
    // returns true if the first and last axis are the same
    // X->*->X
    // Y->*->Y
    // Z->*->Z
    return axisOrder[0] == axisOrder[2];
}

@Override
public String toString()
{
    final StringBuffer buffer = new StringBuffer();
    buffer.append(axisOrder[0].toString());
    buffer.append(axisOrder[1].toString());
    buffer.append(axisOrder[2].toString());
    return buffer.toString();
}

/* STATIC CONSTRUCTORS FOR THE 12 POSSIBLE EULER SEQUENCES */

public static EulerOrder XYZ()
{
    return new EulerOrder(
            AxisType.X,
            AxisType.Y,
            AxisType.Z);
}

public static EulerOrder YZX()
{
    return new EulerOrder(
            AxisType.Y,
            AxisType.Z,
            AxisType.X);
}

public static EulerOrder ZXY()
{
    return new EulerOrder(
            AxisType.Z,
            AxisType.X,
            AxisType.Y);
}

public static EulerOrder ZYX()
{
    return new EulerOrder(
            AxisType.Z,
            AxisType.Y,
            AxisType.X);
}

public static EulerOrder YXZ()
{
    return new EulerOrder(
            AxisType.Y,
            AxisType.X,
            AxisType.Z);
}

public static EulerOrder XZY()
{
    return new EulerOrder(
            AxisType.X,
            AxisType.Z,
            AxisType.Y);
}

public static EulerOrder XYX()
{
    return new EulerOrder(
            AxisType.X,
            AxisType.Y,
            AxisType.X);
}

public static EulerOrder XZX()
{
    return new EulerOrder(
            AxisType.X,
            AxisType.Z,
            AxisType.X);
}

public static EulerOrder YZY()
{
    return new EulerOrder(
            AxisType.Y,
            AxisType.Z,
            AxisType.Y);
}

public static EulerOrder YXY()
{
    return new EulerOrder(
            AxisType.Y,
            AxisType.X,
            AxisType.Y);
}

public static EulerOrder ZXZ()
{
    return new EulerOrder(
            AxisType.Z,
            AxisType.X,
            AxisType.Z);
}

public static EulerOrder ZYZ()
{
    return new EulerOrder(
            AxisType.Z,
            AxisType.Y,
            AxisType.Z);
}

public static EulerOrder parse(String eulerOrder)
{
    if(eulerOrder.equals("XYZ")) return XYZ();
    if(eulerOrder.equals("XZY")) return XZY();
    if(eulerOrder.equals("YZX")) return YZX();
    if(eulerOrder.equals("YXZ")) return YXZ();
    if(eulerOrder.equals("ZYX")) return ZYX();
    if(eulerOrder.equals("ZXY")) return ZXY();

    if(eulerOrder.equals("XYX")) return XYX();
    if(eulerOrder.equals("XZX")) return XZX();
    if(eulerOrder.equals("YZY")) return YZY();
    if(eulerOrder.equals("YXY")) return YXY();
    if(eulerOrder.equals("ZYZ")) return ZYZ();
    if(eulerOrder.equals("ZXZ")) return ZXZ();

    return null;
}

@Override
public boolean equals(Object o) {
    if (this == o) return true;
    if (o == null || getClass() != o.getClass()) return false;

    EulerOrder that = (EulerOrder) o;

    if (!Arrays.equals(axisOrder, that.axisOrder)) return false;

    return true;
}

@Override
public int hashCode() {
    return axisOrder != null ? Arrays.hashCode(axisOrder) : 0;
}
}

Это подводит нас к самому алгоритму. Я сохранил имена переменных мистера Хью для упрощения отладки исходного документа.

/**
 * This class is a direct implementation of the algorithm described in   the
 * paper: "Quaternion to Euler Angle Conversion for Arbitrary Rotation Sequence
 * Using Geometric Methods" by Noel H Hughes
 * 
 * All variables are named using the names that the author uses in the paper to
 * ensure tracability with the original document
 * 
 * The general algorithm for this is really quite simple: Given a unit
 * quaternion and a desired sequence, decompose that quaternion into a sequence
 * of 3 rotations about the principle axis in the correct sequence.
 * 
 * This involves 2 steps: step 1: take the last axis of rotation and rotate it
 * through the quaternion. From this, you can determine (with some clever trig
 * and the knowledge of the order of the first two rotations) what the first two
 * angles are step 2: construct a quaternion from the first 2 rotations, and run
 * the next circular axis after the last axis (ie, if the last axis of rotation
 * is 'X', then use 'Y') through the original quaternion and the new 2-step one.
 * The included angle between these two vectors must be your third Euler angle.
 * Using some clever cross product tests you can determine the sign and you're
 * done!
 * 
 * Note - This has been tested extensively to make sure that the angles that are
 * returned produce an equivalent rotation using the same sequence as the
 * original. This does not, in fact, quarantee that you will get the same
 * angles! There is a 'short way' and a 'long way' to get from here to there,
 * and as of yet I haven't figured out how to force one over the other
 */
public class EulerAngleDecomposer
{

private static EulerAngleDecomposer instance = null;

// made the constructor private because this is a singleton
private EulerAngleDecomposer()
{

}

public static EulerAngleDecomposer getInstance()
{
    if(instance == null)
        instance = new EulerAngleDecomposer();

    return instance;
}

private class IndexData
{
    // for all of these indices:
    // 0 = X
    // 1 = Y
    // 2 = Z

    private final AxisType m_i1; // zero based index of first euler rotation 
    private final AxisType m_i1n; // next circular index following i1
    private final AxisType m_i1nn; // next circular index following i1n

    private final AxisType m_i2; // zero based index of second euler rotation
    private final AxisType m_i2n; // next circular index following i2
    private final AxisType m_i2nn; // next circular index following i2n

    private final AxisType m_i3; // zero based index of third euler rotation
    private final AxisType m_i3n; // next circular index following i3
    private final AxisType m_i3nn; // next circular index following i3n

    // m_unitAxis[0] = first euler rotation axis
    // m_unitAxis[1] = second euler rotation axis
    // m_unitAxis[2] = third euler rotation axis
    private final Vector3D[] m_unitAxis;

    // create from a EulerOrder
    public IndexData(
            final EulerOrder order )
    {
        m_i1 = order.getAxisType(0);
        m_i2 = order.getAxisType(1);
        m_i3 = order.getAxisType(2);

        // now populate m_ixn, ans ixnn's
        m_i1n = m_i1.nextCircular();
        m_i1nn = m_i1n.nextCircular();

        m_i2n = m_i2.nextCircular();
        m_i2nn = m_i2n.nextCircular();

        m_i3n = m_i3.nextCircular();
        m_i3nn = m_i3n.nextCircular();

        m_unitAxis = order.orderedAxis();
    }

    // first axis of rotation
    public Vector3D V1()
    {
        return m_unitAxis[0];
    }

    // second axis of rotation
    public Vector3D V2()
    {
        return m_unitAxis[1];
    }

    // third axis of rotation
    public Vector3D V3()
    {
        return m_unitAxis[2];
    }

    // next axis after V3 (circular)
    // a little table:
    // V3()     -->     V3n()
    // X        -->     Y
    // Y        -->     Z
    // Z        -->     X

    public Vector3D V3n()
    {
        return m_i3n.toVector3D();
    }

    // first rotation axis
    public AxisType i1()
    {
        return m_i1;
    }

    // next circular axis folowing i1()
    // not to be confused with the second axis of rotation (i2)
    public AxisType i1n()
    {
        return m_i1n;
    }

    // next circular axis following i1n()
    // not to be confused with the third axis of rotation (i3)
    public AxisType i1nn()
    {
        return m_i1nn;
    }
}

public RotationSequence DecomposeFromQuaternion(
        final Quaternion q,
        final EulerOrder order )
{
    // crappy variable name, I know
    // it's used a lot, so I wanted a one letter
    // one!
    final IndexData d = new IndexData(
            order);

    final Vector3D v3Rot = q.Rotate(
            d.V3()).unit(); // q->GetRotatedVector(d.V3()).unit();

    // recall:
    // i1;      // zero based index of first euler rotation 
    // i1n;     // next circular index following i1
    // i1nn;    // next circular index following i1n

    Angle theta1 = Angle.Zero();
    Angle theta2 = Angle.Zero();
    Angle theta3 = Angle.Zero();

    if (order.isRepeating())
    {
        if (order.isCircular())
        {

            // circular, repeating
            //theta1 = atan2( v3Rot[d.i1n()], -v3Rot[d.i1nn()]);
            theta1 = Angle.fromRadians(org.apache.commons.math3.util.FastMath.atan2(
                    v3Rot.at(d.i1n()),
                    -v3Rot.at(d.i1nn())));
            theta2 = Angle.fromRadians(-org.apache.commons.math3.util.FastMath.acos(v3Rot.at(d.i1())));
        }
        else
        {

            // non circular, repeating
            theta1 = Angle.fromRadians(org.apache.commons.math3.util.FastMath.atan2(
                    v3Rot.at(d.i1nn()),
                    v3Rot.at(d.i1n())));
            theta2 = Angle.fromRadians(org.apache.commons.math3.util.FastMath.acos(v3Rot.at(d.i1())));
        }

        // By convention, repeating sequences restrict theta2 to 0-->180
        if (theta2.radians() < 0)
        {
            // need to resolve the ambiguity restrict theta2 to 0 --> 180
            theta2 = theta2.negate();
            //theta1 = theta1 - pi;
        }

        // special case where theta2 is zero, which is somewhat nonsense
        // for a repeating sequence
        // in this case, put all the entire angle into theta3
        if ((theta2.radians() == 0) || (theta2.radians() == Math.PI))
        {
            theta1 = Angle.Zero();
        }
    }
    else
    // non-repeating sequence
    {
        if (order.isCircular())
        {
            // circular, non-repeating
            theta1 = Angle.fromRadians(org.apache.commons.math3.util.FastMath.atan2(
                    -v3Rot.at(d.i1n()),
                    v3Rot.at(d.i1nn())));
            theta2 = Angle.fromRadians(-org.apache.commons.math3.util.FastMath.asin(-v3Rot.at(d.i1())));
        }
        else
        {
            // non circular, non repeating
            theta1 = Angle.fromRadians(org.apache.commons.math3.util.FastMath.atan2(
                    v3Rot.at(d.i1nn()),
                    v3Rot.at(d.i1n())));
            theta2 = Angle.fromRadians(-org.apache.commons.math3.util.FastMath.asin(v3Rot.at(d.i1())));
        }
    }

    // Create the Q12 quaternion using the first two axis and angles
    final Quaternion Q1 = Quaternion.createFromAxisAngle(
            d.V1(),
            theta1);
    final Quaternion Q2 = Quaternion.createFromAxisAngle(
            d.V2(),
            theta2);

    final Quaternion Q12 = Q1.times(Q2);
    /* Q12 = Q1 * Q2 */

    // get the next circular vector after V3
    final Vector3D V3n = d.V3n();

    // rotate V3n through Q12 and q
    final Vector3D V3n12 = Q12.Rotate(V3n);
    final Vector3D V3nG = q.Rotate(V3n);

    // get the angle between them - theta3
    theta3 = Vector3D.angleBetween(
            V3n12,
            V3nG);

    // use a cross product to determine the direction of the angle
    final Vector3D Vc = Vector3D.crossProduct(
            V3n12,
            V3nG);

    final double m = Vector3D.dotProduct(
            Vc,
            v3Rot);

    final double sign = m > 0 ? 1.0 : -1.0;

    theta3 = Angle.fromRadians(sign * org.apache.commons.math3.util.FastMath.abs(theta3.radians()));

    return new RotationSequence(
            order,
            theta1,
            theta2,
            theta3);
}

}

Здесь отсутствуют некоторые классы, которые я не включил (Angle, RotationSequence, Quaternion), но я считаю, что приведенный выше код дает людям очень прочную точку отсчета.