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

Question

我有两组数据，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

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