Question

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

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

所以我首先使用球面插值（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

Answer 1

我发现了问题！

成本函数不正确。我会试着解释一下是否有人感兴趣。可以写出两个坐标系之间的点x的映射

x'= Qx

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

x''= QRx

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

x'= R'^ T QRx

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

x = Q ^ T R'^ T QRx

正确的成本函数应为

Q ^ T R'^ T QR

即。我忘记了Q ^ T

Scipy.optimization在描述相同旋转的两组数据之间找到未知参数

1 个答案: