Google VR开发-Cardboard VR SDK头部追踪实现（罗德里格旋转公式）

一、罗德里格旋转公式

可以参考百度百科和维基百科进行了解。
概括来说就是罗德里格旋转公式就是用来求旋转后新向量的公式：



而这个公式可以转换成矩阵形式：





公式各部分的几何意义和推导原理参考下图



这个图证明：
1.Vrot = 可以由V+(kxv)sinθ+kx（kxv）（1-cosθ）这三个向量的和来表示
2.(kxv)sinθ和kx（kxv）（1-cosθ）可以用平移来求解，这两个向量这样表示是对的
3.kx（kxv）（1-cosθ）可以经由图中的公式三转换成点乘形式

进而证明


并可以参照图理解公式的几何意义

二、罗德里格旋转矩阵

我们前面推导和证明了罗德里格旋转公式，并知道罗德里格旋转公式也可以写成矩阵形式，这个矩阵就叫罗德里格旋转矩阵。矩阵公式的推导可以参考：
http://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html
两个向量叉乘即可得到旋转轴，然后求出旋转角度，由这两个因素便可以求出旋转矩阵。
求旋转矩阵的时候，先求得旋转轴模长，然后将旋转轴向量除以模长，便得到旋转轴的单位向量。
利用旋转轴的单位向量和旋转角度，通过公式便可以得到旋转矩阵：



下面我们将看下这个旋转公式在VR中的应用

三、罗德里格旋转矩阵公式在VR中的应用

在双眼绘制的时候，会求得左右眼的偏移矩阵mLeftEyeTranslate，与头部姿态矩阵mHeadTransform.getHeadView()相乘，就能得到左右眼的转换矩阵mLeftEye.getTransform().getEyeView()

Matrix.multiplyMM(mRightEye.getTransform().getEyeView(), 0,mRightEyeTranslate, 0, mHeadTransform.getHeadView(), 0);

而这个头部姿态矩阵mHeadTransform.getHeadView() 是需要预测的，这里就会利用到罗德里格公式。

下面是用来获取头部姿态矩阵的函数

// 获取头部姿态矩阵
mHeadTracker.getLastHeadView(mHeadTransform.getHeadView(), 0);

其实现为：

public void getLastHeadView(float[] headView, int offset)
{
if (offset + 16 > headView.length) {
throw new IllegalArgumentException("Not enough space to write the result");
}
//如果定义了卡尔曼滤波对象
synchronized (mTracker) {
//首先计算出当前时间距离上次预测时间的差值
double secondsSinceLastGyroEvent = (System.nanoTime() - mLastGyroEventTimeNanos) * 1.E-09D;
Log.d(TAG,"secondsSinceLastGyroEvent="+secondsSinceLastGyroEvent);
//实际上有60帧每秒，30帧算的话，一帧33ms，所以这里是提前预测一帧
//然后加上提前预测的时间33ms
double secondsToPredictForward = secondsSinceLastGyroEvent + 0.03333333333333333D;
//这里返回的是一个double类型的数组，
double[] mat = mTracker.getPredictedGLMatrix(secondsToPredictForward);
for (int i = 0; i < headView.length; i++) {
mTmpHeadView[i] = ((float)mat[i]);
}
}

Matrix.multiplyMM(headView, offset, mTmpHeadView, 0, mEkfToHeadTracker, 0);
}

可以看到这里计算了当前时间距离上次预测时间的差值，然后加了一个提前预测时间33ms，之后调用了getPredictedGLMatrix

public double[] getPredictedGLMatrix(double secondsAfterLastGyroEvent)
{
//deltaT,时间差
double dT = secondsAfterLastGyroEvent;
Log.d(TAG,"dT="+dT);
Vector3d pmu = getPredictedGLMatrixTempV1;
//应该理解为向量乘以一个常量，这个常量就是提前预测的时间，但为什么乘以一个负的呢？
Log.d(TAG,"lastGyro=("+Float.toString(lastGyro[0])+","+Float.toString(lastGyro[1])+","+Float.toString(lastGyro[2])+")");
pmu.set(lastGyro[0] * -dT, lastGyro[1] * -dT, lastGyro[2] * -dT);
Matrix3x3d so3PredictedMotion = getPredictedGLMatrixTempM1;
//Log.d(TAG,"so3PredictedMotion="+so3PredictedMotion.toString());
//这里返回的so3PredictedMotion就是一个Rodrigues矩阵
So3Util.sO3FromMu(pmu, so3PredictedMotion);

Matrix3x3d so3PredictedState = getPredictedGLMatrixTempM2;
//Log.d(TAG,"getPredictedGLMatrix,so3SensorFromWorld="+so3SensorFromWorld.toString());
Matrix3x3d.mult(so3PredictedMotion,so3SensorFromWorld,so3PredictedState);

//so3PredictedState还是一个3x3的矩阵，这里把它放到一个4x4矩阵中
return glMatrixFromSo3(so3PredictedState);
}

这里面先调用So3Util.sO3FromMu求得一个so3PredictedMotion，这就是一个Rodrigues矩阵。而向量pmu就是前面公式中说的V向量
看下sO3FromMu的实现

//这个函数封装rodrigues算法，只不过这里根据角度的大小
//来调节kA和kB的值。角度比较大的时候，和公式上比起来，KA多除了一个θ，kB多除了一个θ平方，不知道原因是什么根据。
public static void sO3FromMu(Vector3d w, Matrix3x3d result)
{
//向量点乘等于w向量模长的平方

double thetaSq = Vector3d.dot(w, w);
//如果这里是求theta，就要用点积再除以两者的模再求arccos
//开平方，就是求得w向量的模长
double theta = Math.sqrt(thetaSq);
Log.d(TAG,"w=("+w.x+","+w.y+","+w.z+")"+",theta="+theta);
double kA, kB;
if (thetaSq < 1.0E-08D) {
//Log.d(TAG,"thetaSq < 1.0E-08D");
kA = 1.0D - 0.16666667163372D * thetaSq;
kB = 0.5D;
}
else
{
if (thetaSq < 1.0E-06D) {
//Log.d(TAG,"thetaSq < 1.0E-06D");
kB = 0.5D - 0.0416666679084301D * thetaSq;
kA = 1.0D - thetaSq * 0.16666667163372D * (1.0D - 0.16666667163372D * thetaSq);
} else {
//Log.d(TAG,"thetaSq > 1.0E-06D");
double invTheta = 1.0D / theta;
kA = Math.sin(theta) * invTheta;
kB = (1.0D - Math.cos(theta)) * (invTheta * invTheta);
}
}
//根据公式，w是一个单位向量，因为每一项都要除以一个模长，所以这里除以模长就放到了kA和kB
rodriguesSo3Exp(w, kA, kB, result);
}

函数sO3FromMu其实就是对罗德里格矩阵求法的一个封装，其中rodriguesSo3Exp函数就是罗德里格矩阵：

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.0D - kB * (wy2 + wz2));
result.set(1, 1, 1.0D - kB * (wx2 + wz2));
result.set(2, 2, 1.0D - kB * (wx2 + wy2));

double a, b;
a = kA * w.z;
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);
}

不过这部分程序好像是有问题的，说是theta，但实际求得的是模长
陀螺仪传感器叫做Gyro-sensor，返回x、y、z三轴的角加速度数据，因此这里计算出时间差
secondsAfterLastGyroEvent，求负后与加速度相乘得到向量pmu，通过向量pmu利用罗德里格旋转公式得到result，返回给
so3PredictedMotion，可见这个
so3PredictedMotion就是罗德里格旋转矩阵，这个矩阵后面与so3SensorFromWorld相乘得到预测后的状态矩阵so3PredictedState，

public double[] getPredictedGLMatrix(double secondsAfterLastGyroEvent)
{
//deltaT,时间差
double dT = secondsAfterLastGyroEvent;
Log.d(TAG,"dT="+dT);
Vector3d pmu = getPredictedGLMatrixTempV1;
//应该理解为向量乘以一个常量，这个常量就是提前预测的时间，但为什么乘以一个负的呢？
Log.d(TAG,"lastGyro=("+Float.toString(lastGyro[0])+","+Float.toString(lastGyro[1])+","+Float.toString(lastGyro[2])+")");
pmu.set(lastGyro[0] * -dT, lastGyro[1] * -dT, lastGyro[2] * -dT);
Matrix3x3d so3PredictedMotion = getPredictedGLMatrixTempM1;
//Log.d(TAG,"so3PredictedMotion="+so3PredictedMotion.toString());
//这里返回的so3PredictedMotion就是一个Rodrigues矩阵
So3Util.sO3FromMu(pmu, so3PredictedMotion);

Matrix3x3d so3PredictedState = getPredictedGLMatrixTempM2;
//Log.d(TAG,"getPredictedGLMatrix,so3SensorFromWorld="+so3SensorFromWorld.toString());
Matrix3x3d.mult(so3PredictedMotion,so3SensorFromWorld,so3PredictedState);

return glMatrixFromSo3(so3PredictedState);
}

 

四、罗德里格矩阵与卡尔曼滤波的配合

分析前面对罗德里格旋转矩阵的使用，可以知道在getPredictedGLMatrix函数中用到了
so3SensorFromWorld与罗德里格旋转矩阵相乘得到预测后的状态矩阵so3PredictedState

这个so3SensorFromWorld在processGyro和processAcc以及processMag的时候都会被进行修改
具体配合方式参见另一片博文《Google VR开发-罗德里格旋转矩阵与卡尔曼滤波的配合使用》