Я борюсь с преобразованием между некоторыми матрицами вращения и наоборот в Python и не могу понять, в чем проблема, из-за которой они не работают. Есть и другие сообщения, в которых подробно описывается, что несколько матриц вращения могут быть правильными для данного кватерниона, однако полученные матрицы вращения при преобразовании туда и обратно не соответствуют одному и тому же вращению, что должно быть так, верно?
Вот функции, которые я написал (с использованием метода matrix->quaternion, реализованного из https://doi.org/10.2514/2.4654):
В качестве эталона преобразование пакета (ортонормальных) матриц вращения туда и обратно, например
batch_quaternions_to_rotation_matrix(*batch_rotation_matrix_to_quaternions(rotation_matrices))
работает нормально для большинства из них, но некоторые, похоже, не работают.
Например, эта матрица вращения работает безупречно:
Тогда как эта:
не работает и возвращает вместо этого отрицательное значение.
Я уже перепробовал кучу вещей, например, исправление знака скалярной части (умножение кватерниона на -1, если он отрицательный), однако, поскольку q_1 и -q_1 определяют одну и ту же матрицу вращения по определению, я заранее знал, что это не поможет. Единственная особенность, которую разделяют эти несовместимые матрицы (которую я смог найти), заключается в том, что собственные значения при вычислении кватерниона содержат -1 вместо 1, как рабочие вращения. Другие методы, реализованные в различных "готовых" преобразователях, также борются с этими матрицами. К сожалению, мне действительно нужно, чтобы эти преобразования работали без этого критического сбоя, есть ли у кого-нибудь идеи, где можно это исправить?
def batch_quaternions_to_rotation_matrix(w, x, y, z):
# Normalize the quaternion
norm = torch.sqrt(w**2 + x**2 + y**2 + z**2)
w = w / norm
x = x / norm
y = y / norm
z = z / norm
# Compute the rotation matrix
R = torch.zeros((w.shape[0], 3, 3))
R[:, 0, 0] = w**2 + x**2 - y**2 - z**2
R[:, 0, 1] = 2 * (x*y - w*z)
R[:, 0, 2] = 2 * (x*z + w*y)
R[:, 1, 0] = 2 * (x*y + w*z)
R[:, 1, 1] = w**2 - x**2 + y**2 - z**2
R[:, 1, 2] = 2 * (y*z - w*x)
R[:, 2, 0] = 2 * (x*z - w*y)
R[:, 2, 1] = 2 * (y*z + w*x)
R[:, 2, 2] = w**2 - x**2 - y**2 + z**2
return torch.transpose(R, 1, 2)
def batch_rotation_matrix_to_quaternions(R):
batch_size = R.shape[0]
K = torch.zeros((batch_size, 4, 4), dtype=R.dtype)
K[:, 0, 0] = R[:, 0, 0] - R[:, 1, 1] - R[:, 2, 2]
K[:, 0, 1] = R[:, 1, 0] + R[:, 0, 1]
K[:, 0, 2] = R[:, 2, 0] + R[:, 0, 2]
K[:, 0, 3] = R[:, 1, 2] - R[:, 2, 1]
K[:, 1, 0] = R[:, 1, 0] + R[:, 0, 1]
K[:, 1, 1] = R[:, 1, 1] - R[:, 0, 0] - R[:, 2, 2]
K[:, 1, 2] = R[:, 2, 1] + R[:, 1, 2]
K[:, 1, 3] = R[:, 2, 0] - R[:, 0, 2]
K[:, 2, 0] = R[:, 2, 0] + R[:, 0, 2]
K[:, 2, 1] = R[:, 2, 1] + R[:, 1, 2]
K[:, 2, 2] = R[:, 2, 2] - R[:, 0, 0] - R[:, 1, 1]
K[:, 2, 3] = R[:, 0, 1] - R[:, 1, 0]
K[:, 3, 0] = R[:, 1, 2] - R[:, 2, 1]
K[:, 3, 1] = R[:, 2, 0] - R[:, 0, 2]
K[:, 3, 2] = R[:, 0, 1] - R[:, 1, 0]
K[:, 3, 3] = R[:, 0, 0] + R[:, 1, 1] + R[:, 2, 2]
K /= 3
eigvals, eigvecs = torch.linalg.eig(K)
eigvals = eigvals.real
eigvecs = eigvecs.real
mask = torch.logical_or(torch.isclose(eigvals,torch.ones(eigvals.shape, dtype=eigvals.dtype)),torch.isclose(eigvals,-torch.ones(eigvals.shape, dtype=eigvals.dtype)))
indices = torch.where(mask)
quaternions = eigvecs[indices[0], :, indices[1]]
return quaternions[:,3], quaternions[:,0], quaternions[:,1], quaternions[:,2]
tensor([[[ 0.7741, -0.3551, 0.5241],
[ 0.2347, 0.9298, 0.2834],
[-0.5880, -0.0964, 0.8031]]])
tensor([[[-0.2643, 0.6644, -0.6991],
[ 0.6968, 0.6327, 0.3378],
[-0.6667, 0.3979, 0.6302]]])
tensor([[[ 0.2643, -0.6644, 0.6991],
[-0.6968, -0.6327, -0.3378],
[ 0.6667, -0.3979, -0.6302]]])