如何仅将 wintracker II 设备的四元数方向输出转换为欧拉角输出。因为 Wintracker II 设备输出欧拉角和四元数方向。我只想输出欧拉角。
问问题
1832 次
1 回答
1
我已经实现了本文中描述的算法，并且效果很好： http ://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/quat_2_euler_paper_ver2-1.pdf
答案#1 中列出的维基百科文章的问题在于它只提供了 XYZ 旋转的公式。此处引用的论文给出了适用于 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;
}
}
这将我们带到了算法本身。我保留了 Hugh 先生的变量名，以便于使用原始文档进行调试。
/**
* 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），但我相信上面的代码为人们提供了一个非常可靠的起点。
于 2010-12-07T03:21:33.190 回答