3

我有两组数据，R1 和 R2，它们都代表不同坐标系中的相同旋转。它们的采样不均等，因此在相同的时间跨度内，R1 中的样本比 R2 中的样本多得多。两组之间也有时间延迟。因此，可以在 R2 中的时间 t' = t + delta 处找到 R1 中时间 t 的旋转。

我想找到这个时间延迟以及坐标系之间的未知转换（我假设是纯旋转）

因此，我首先使用球面插值 (SLERP) 对 R1 数据集进行插值。然后我将它乘以 R，它是变换矩阵，所以旋转现在在同一个坐标系中表示。然后我用 R2 的转置（与旋转矩阵的逆矩阵相同）将整个东西旋转回来，如果 R1 和 R2 描述相同的旋转，它应该生成一个单位矩阵。

我用过这个成本函数

def cost_func(x,R1,R2)  

    R = x[1:].reshape(3,3)
    time_delay = x[0]

    For n in range(R2)

        Rtot = transpose(R2)*R*SLERP(R1,time_delay)
        alpha[n] = rotation_matrix_to_axis_angle(Rtot)

    return alpha**2

然后尝试了 scipy.optimize.fmin 和 scipy.optimize.leastsq。

optimize.fmin(cost_func,initial_guess,args=(R1,R2))

当我尝试使用合成数据时，我得到了非常好的时间延迟值，但我没有得到非常准确的转换矩阵值。数学可能有点难以理解，但我很确定它是正确的。我不确定我应该使用哪种优化功能。此外，是否会最小化其他东西（例如更多参数）而不仅仅是 alpha 生成更准确的值？

提前致谢！

* 编辑* *

好的，这就是我创建合成数据的方式。我为 R1 创建随机四元数（旋转的 4D 矢量表示），然后为 R2 添加延迟和旋转 (Rtrans)。

def create_random_quaternion():

    v = np.zeros(4)
    v[0] = random.random()
    v[1] = random.random()
    v[2] = random.random()
    v[3] = random.random()

    return v / np.linalg.norm(v)


def create_synthetic_gyro_sequence(sequence_length):

    q = np.zeros((sequence_length, 4))
    ts = np.zeros(sequence_length)
    for i in range(sequence_length):
        q[i] = create_random_quaternion()
        ts[i] = 0.01*i

    return q, ts


def create_synthetic_data(l):

    gyro_data, gyro_ts = create_synthetic_gyro_sequence(l)
    camera_data = np.zeros((l/5,3,3))
    camera_ts = np.zeros(l/5)
    i = 0

    for n in range(l):

        if n % 5 == 0:
            camera_data[i] = Rtrans.dot(SLERP.quat2rot(gyro_data[n]))
            camera_ts[i] = 0.004+0.01*n
            i += 1

    return gyro_data, gyro_ts, camera_data, camera_ts
4

1 回答 1

0

我发现了问题！

成本函数不正确。如果有人感兴趣，我会尽力解释。点 x 在两个坐标系之间的映射可以写成

x' = Qx

现在让 R 和 R' 是在两个不同坐标系中表示的相同旋转。由 R 旋转，然后是变换 Q

x'' = QRx

将点旋转回其新坐标系（对于旋转矩阵，转置与逆矩阵相同）

x' = R'^T QRx

作为最后一步，将点转换回其原始坐标系

x = Q^T R'^T QRx

正确的成本函数应该是

Q^T R'^T 二维码

即我忘记了Q^T

于 2013-07-15T20:03:34.710 回答