如何使用 CADisplayLink 在 CMRotationMatrix 上应用过滤器

Question

如何在 CMRotationMatrix 上应用过滤器？也许是卡尔曼滤波器。我需要修复 CMRotationMatrix (transformFromCMRotationMatrix) 的噪声，以获得结果矩阵的线性值

这个矩阵值将被转换为 XYZ，在我的例子中，我在 2D 屏幕上模拟 3D：

// 将矩阵转换为 x, y

vec4f_t v;
multiplyMatrixAndVector(v, projectionCameraTransform, boxMatrix);

float x = (v[0] / v[3] + 1.0f) * 0.5f;
float y = (v[1] / v[3] + 1.0f) * 0.5f;

CGPointMake(x * self.bounds.size.width, self.bounds.size.height - (y * self.bounds.size.height));

代码：

//定义变量

mat4f_t cameraTransform;

//启动显示链接循环

- (void)startDisplayLink
{
    displayLink = [CADisplayLink displayLinkWithTarget:self selector:@selector(onDisplayLink:)];
    [displayLink setFrameInterval:1];
    [displayLink addToRunLoop:[NSRunLoop currentRunLoop] forMode:NSDefaultRunLoopMode];
}

//停止显示链接循环

- (void)stopDisplayLink
{
    [displayLink invalidate];
    displayLink = nil;      
}

//显示链接事件

- (void)onDisplayLink:(id)sender
{
    CMDeviceMotion *d = motionManager.deviceMotion;

    if (d != nil) {
        CMRotationMatrix r = d.attitude.rotationMatrix;

        transformFromCMRotationMatrix(cameraTransform, &r);
        [self setNeedsDisplay];
    }
}

//[self setNeedDisplay]之前的函数触发；

void transformFromCMRotationMatrix(vec4f_t mout, const CMRotationMatrix *m)
{    
    mout[0] = (float)m->m11;
    mout[1] = (float)m->m21;
    mout[2] = (float)m->m31;
    mout[3] = 0.0f;

    mout[4] = (float)m->m12;
    mout[5] = (float)m->m22;
    mout[6] = (float)m->m32;
    mout[7] = 0.0f;

    mout[8] = (float)m->m13;
    mout[9] = (float)m->m23;
    mout[10] = (float)m->m33;
    mout[11] = 0.0f;

    mout[12] = 0.0f;
    mout[13] = 0.0f;
    mout[14] = 0.0f;
    mout[15] = 1.0f;
}

// 矩阵向量和矩阵矩阵乘法例程

void multiplyMatrixAndVector(vec4f_t vout, const mat4f_t m, const vec4f_t v)
{
    vout[0] = m[0]*v[0] + m[4]*v[1] + m[8]*v[2] + m[12]*v[3];
    vout[1] = m[1]*v[0] + m[5]*v[1] + m[9]*v[2] + m[13]*v[3];
    vout[2] = m[2]*v[0] + m[6]*v[1] + m[10]*v[2] + m[14]*v[3];
    vout[3] = m[3]*v[0] + m[7]*v[1] + m[11]*v[2] + m[15]*v[3];
}

Answer 1

一般来说，我会区分提高信噪比和平滑信号。

信号改善

如果您真的想比已经实施传感器融合算法的 Apple Core Motion 做得更好，请为结果不确定的长期项目做好准备。在这种情况下，您最好采用原始加速度计和陀螺仪信号来构建您自己的传感器融合算法，但您必须关心很多问题，例如漂移、不同 iPhone 版本的硬件依赖性、同一传感器的硬件差异一代，...所以我的建议：尽一切努力避免它。

平滑

这只是意味着插入两个或更多信号并建立一种平均值。我不知道有什么合适的方法可以直接用于旋转矩阵（也许有一个），但您可以使用 quaternions 代替（更多资源：OpenGL Tutorial Using Quaternions to represent rotation 或 Quaternion FAQ）。

这种插​​值得到的四元数可以与您的向量相乘，以获得类似于矩阵方式的投影（您可以查看Finding normal vector to iOS device了解更多信息）。

表示旋转的两个单位四元数之间的插值可以通过Slerp 完成。在实践中，您将使用 Wikipedia 中描述为 Geometric Slerp 的内容。如果你有两个时间点t1和t2以及对应的四元数q1和q2和角距离omega，公式为：

q'(q1, q2, t) = sin((1- t) * omega) / sin(omega) * q0 + sin(t * omega) / sin(omega) * q1

t 应该是 0.5，因为你想要两个旋转之间的平均值。欧米茄可以通过点积计算：

cos(omega) = q1.q2 = w1*w2 + x1*x2 + y1*y2 + z1*z2

如果这种使用两个四元数的方法仍然不能满足您的需求，您可以使用 slerp (slerp (q1, q2), slerp (q3, q4)) 重复此操作。一些注意事项：

• 从性能的角度来看，在运行循环中执行三个 sin 和一个 arccos 调用 1/frequency 次/秒并不便宜。因此，您应该避免使用过多的点
• 在您的情况下，所有信号都彼此接近，尤其是在使用高传感器频率时。您必须注意非常小的角度，让 1/sin(omega) 爆炸。在这种情况下，设置 sin(x) ≈ x
• 与low-pass filter 等其他过滤器一样，您使用的时间点越多，获得的时间延迟就越多。因此，如果您的频率为 f，则在使用两点时您将获得大约 0.5/f 秒的延迟，而对于双 slerp，您将获得 1.5/f 的延迟。
• 如果有什么奇怪的地方，检查你得到的四元数是单位四元数，即 ||q|| = 1
• 如果您遇到性能问题，您可以查看Hacking Quaternions

github 上的 C++ 项目 pbrt 包含一个四元数类，可以从中获得一些灵感。