最近在做VR方面的研究,看到了Google Cardboard对传感器处理的代码,对旋转矩阵、罗德里格公式做了一些研究。
罗德里格公式是计算三维空间中,一个向量绕旋转轴旋转给定角度以后得到的新向量的计算公式,具体的详细推导可以看维基,我觉得只要能知道如何使用就可以了,注意旋转轴为单位向量,来计算旋转矩阵。
https://en.wikipedia.org/wiki/Rotation_matrix
http://mathworld.wolfram.com/RodriguesRotationFormula.html
下面是Google Cardboard中罗德里格公式代码:
public static void sO3FromMu(Vector3d w, Matrix3x3d result) {
double kA;
double kB;
double thetaSq = Vector3d.dot(w, w);
double theta = Math.sqrt(thetaSq);
if (thetaSq < 1.0E-8) {
kA = 1.0 - 0.1666666716337204 * thetaSq;
kB = 0.5;
} else if (thetaSq < 1.0E-6) {
kB = 0.5 - 0.0416666679084301 * thetaSq;
kA = 1.0 - thetaSq * 0.1666666716337204 * (1.0 - 0.1666666716337204 * thetaSq);
} else {
double invTheta = 1.0 / theta;
kA = Math.sin(theta) * invTheta;
kB = (1.0 - Math.cos(theta)) * (invTheta * invTheta);
}
So3Util.rodriguesSo3Exp(w, kA, kB, result);
}
private static void rodriguesSo3Exp(Vector3d w, double kA, double kB, Matrix3x3d result) {
double wx2 = w.x * w.x;
double wy2 = w.y * w.y;
double wz2 = w.z * w.z;
result.set(0, 0, 1.0 - kB * (wy2 + wz2));
result.set(1, 1, 1.0 - kB * (wx2 + wz2));
result.set(2, 2, 1.0 - kB * (wx2 + wy2));
double a = kA * w.z;
double b = kB * (w.x * w.y);
result.set(0, 1, b - a);
result.set(1, 0, b + a);
a = kA * w.y;
b = kB * (w.x * w.z);
result.set(0, 2, b + a);
result.set(2, 0, b - a);
a = kA * w.x;
b = kB * (w.y * w.z);
result.set(1, 2, b - a);
result.set(2, 1, b + a);
}
代码中罗德里格变换矩阵为:
KA和KB两个参数在陀螺仪静态状态下,(x,y,z)角速度极小条件下做了修正,效果应该是能够增强画面移动的稳定性,这个还有待测试。
这里为了计算方便有个推导公式:
这样就和上面公式一样了。