Преобразование координат Opencv: поворот повернутой системы координат на одну из ее новых (повернутых) осей. ⇐ Python

[Anonymous]

Для своей диссертации я работаю над проектом компьютерного зрения. Частично это связано с множеством преобразований координат, с которыми я сейчас борюсь. Я просто не могу вращать свою систему координат по произвольной оси.
Я начинаю с 3D координат объекта. Учитывая, что я настроил камеру, знаю матрицу камеры и коэффициенты искажения, я могу спроецировать эту точку обратно на 2D-изображение.
Теперь я хочу получить эйлеровы углы системы координат так, чтобы ось Z системы координат указывает прямо на оптический центр (камеру).
Для полноты картины здесь приведены вспомогательные функции из моего кода. Для решения моей основной проблемы перейдите к следующему текстовому блоку.

Код: Выделить всё

def coordinate_system_to_image(image, custom_rotation, custom_translation):
origin_3d = np.array([(0.0, 0.0, 0.0)])
start_point_coord, jacobian = cv2.projectPoints(origin_3d,custom_rotation,custom_translation,mtx,dist,)

point0 = (int(start_point_coord[0][0][0]), int(start_point_coord[0][0][1]))
pointx_3d = np.add(origin_3d, np.array([(2.5, 0.0, 0.0)]))
end_point_x, jacobian = cv2.projectPoints(pointx_3d,custom_rotation,custom_translation,mtx,dist,)
pointx = (int(end_point_x[0][0][0]), int(end_point_x[0][0][1]))

pointy_3d = np.add(origin_3d, np.array([(0.0, 2.5, 0.0)]))
end_point_y, jacobian = cv2.projectPoints(pointy_3d,custom_rotation,custom_translation,mtx,dist,)
pointy = (int(end_point_y[0][0][0]), int(end_point_y[0][0][1]))

pointz_3d = np.add(origin_3d, np.array([(0, 0.0, 2.5)]))
end_point_z, jacobian = cv2.projectPoints(pointz_3d,custom_rotation,custom_translation,mtx,dist,)
pointz = (int(end_point_z[0][0][0]), int(end_point_z[0][0][1]))

cv2.line(image, point0, pointx, (255, 0, 0), 5)
cv2.line(image, point0, pointy, (0, 255, 0), 5)
cv2.line(image, point0, pointz, (0, 0, 255), 5)

cv2.circle(image, (int(start_point_coord[0][0][0]), int(start_point_coord[0][0][1])), 10, (0, 0, 255), -1)

return image

def coords_rotation_matrix_from_axis_and_angle(axis,angle):
# https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
ux = axis[0]
uy = axis[1]
uz = axis[2]

length = ux**2 + uy**2 + uz**2
if(length != 1.0):
print("Length of axis != 1")
return False

Mx11 = ux*ux*(1-np.cos(angle)) + np.cos(angle)
Mx12 = ux*uy*(1-np.cos(angle)) - uz*np.sin(angle)
Mx13 = ux*uz*(1-np.cos(angle)) + uy*np.sin(angle)
Mx21 = ux*uy*(1-np.cos(angle)) + uz*np.sin(angle)
Mx22 = uy*uy*(1-np.cos(angle)) + np.cos(angle)
Mx23 = uy*uz*(1-np.cos(angle)) - ux*np.sin(angle)
Mx31 = ux*uz*(1-np.cos(angle)) - uy*np.sin(angle)
Mx32 = uy*uz*(1-np.cos(angle)) + ux*np.sin(angle)
Mx33 = uz*uz*(1-np.cos(angle)) + np.cos(angle)
Mx = np.array([[Mx11, Mx12, Mx13], [Mx21, Mx22, Mx23], [Mx31, Mx32, Mx33]])
return Mx

def coords_rotation_matrix_to_euler(rotation_matrix):
r11 = rotation_matrix[0][0]
r21 = rotation_matrix[1][0]
r31 = rotation_matrix[2][0]
r32 = rotation_matrix[2][1]
r33 = rotation_matrix[2][2]

degy_reverse1 = -1*np.arcsin(r31)
degy_reverse2 = np.pi - degy_reverse1

degx_reverse1 = np.arctan2(r32/np.cos(degy_reverse1), r33/np.cos(degy_reverse1))
degx_reverse2 = np.arctan2(r32/np.cos(degy_reverse2), r33/np.cos(degy_reverse2))

degz_reverse1 = np.arctan2(r21/np.cos(degy_reverse1), r11/np.cos(degy_reverse1))
degz_reverse2 = np.arctan2(r21/np.cos(degy_reverse2), r11/np.cos(degy_reverse2))
print(f"Extracted Eulers 2: X:({degx_reverse1},{degx_reverse2}) Y:({degy_reverse1},{degy_reverse2}) Z:({degz_reverse1},{degz_reverse2})")
return degx_reverse1, degy_reverse1, degz_reverse1

Первая часть системы работает хорошо. Я могу установить трехмерные координаты, определить углы X и Y и напечатать соответствующую систему координат, ось Z которой направлена ​​к камере, как и ожидалось (ось Z «исчезает» на графике).

Код: Выделить всё

demo = np.zeros((2988, 5312, 3))
demo = np.uint8(demo)

pos3dx_wchess = 5.0
pos3dy_wchess = -3
pos3dz_wchess = 30
degx_camera3d = np.arctan(pos3dy_wchess/pos3dz_wchess)
degy_camera3d = -1*np.arctan(pos3dx_wchess/pos3dz_wchess)
degz_camera3d = 0

connector_pointing_to_camera_orientation = np.array([[-1*degx_camera3d, -1*degy_camera3d, -1*degz_camera3d]])
print(connector_pointing_to_camera_orientation)
custom_translation = np.array([[pos3dx_wchess, pos3dy_wchess, pos3dz_wchess]])

demo = coordinate_system_to_image(demo, custom_rotation=connector_pointing_to_camera_orientation, custom_translation=custom_translation)

M_rot = coords_euler_to_rotation_matrix(connector_pointing_to_camera_orientation[0][0],connector_pointing_to_camera_orientation[0][1],connector_pointing_to_camera_orientation[0][2])
print("Test retreive angles:",coords_rotation_matrix_to_euler(M_rot))

[[0.09966865 0.16514868 0. ]]
Тестовые углы извлечения: (0.09966865249116204, 0.1651486774146268, 0.0)
Тестовая печать также показывает, что преобразование углов из матрицы вращения работает.
Теперь я хочу повернуть эту систему координат. вокруг своей новой оси Z. Эта ось Z, конечно, больше не совпадает с осью Z камеры, поскольку она была повернута в результате предыдущего вращения.
Поэтому я сначала получаю новую ось вращения, а затем создаю соответствующее вращение. матрицу для поворота на 45 градусов, объедините две матрицы поворота, получите углы Эйлера из объединенной матрицы поворота и постройте новую повернутую систему координат.

Код: Выделить всё

world_point_z_axis = np.array([0,0,1])
second_axis = np.matmul(M_rot, world_point_z_axis)  # equals M_rot @ world_point_z_axis

second_matrix = coords_rotation_matrix_from_axis_and_angle(second_axis,45*np.pi/180)

combined = second_matrix @ M_rot #np.matmul(second_matrix, M_rot)

new_eulerx, new_eulery, new_eulerz = coords_rotation_matrix_to_euler(combined)
new_rotation = np.array([new_eulerx, new_eulery, new_eulerz])

print(img_shape)

demo2 = coordinate_system_to_image(demo2, custom_rotation=new_rotation, custom_translation=custom_translation)

По какой-то причине это не работает. На новом графике оси X и Y выглядят нормально, но кажется, что вращение вокруг повернутой оси Z не было выполнено с первого поворота, потому что теперь эта ось снова видна. Похоже, что вращение происходило вокруг оси Z камеры, а не вокруг нее.


Есть ли ошибка в моем конвейере? Почему второй поворот не выполняется вокруг отдельно изготовленной оси?
С уважением,
Феликс

Подробнее здесь: https://stackoverflow.com/questions/792 ... y-one-of-i