I want to write an algorithm which calculates the rotation of a 3D disk/circle relative to the image plane.
I want to build an application which would determine the rotation of real world objects with circular shape, using a method described in this paper; which describes the issue using higher level of math.
To test the application, I used the code from this post to generate synthetic images of a rotated circle.
I use the opencv fitEllipse function, which is sufficient using small angles 30-45°. If only pitch or only yaw rotation is applied, I can quite easily and relatively accurately calculate the angle from the arccosine of the minor/major axis of the ellipse, but once both rotations are applied simultaneously, The method fails, and I don't know the mathematical relation between the ellipse's minor/major axes and its rotation. I'm not sure whether the synthetic images are off, or my calculation of the pitch angle is wrong.
import numpy as np
import cv2
def get_3d_rotation_matrix(width, height, theta, phi, gamma, dx, dy, dz):
w, h = width, height
d = np.sqrt(w ** 2 + h ** 2)
# theta = np.deg2rad(theta)
# phi = np.deg2rad(phi)
# gamma = np.deg2rad(gamma)
focal = f = d / (2 * np.sin(gamma) if np.sin(gamma) != 0 else 1)
dz = focal
# Projection 2D -> 3D matrix
A1 = np.array([[1, 0, -w / 2],
[0, 1, -h / 2],
[0, 0, 1],
[0, 0, 1]])
# Rotation matrices around the X, Y, and Z axis
RX = np.array([[1, 0, 0, 0],
[0, np.cos(theta), -np.sin(theta), 0],
[0, np.sin(theta), np.cos(theta), 0],
[0, 0, 0, 1]])
RY = np.array([[np.cos(phi), 0, -np.sin(phi), 0],
[0, 1, 0, 0],
[np.sin(phi), 0, np.cos(phi), 0],
[0, 0, 0, 1]])
RZ = np.array([[np.cos(gamma), -np.sin(gamma), 0, 0],
[np.sin(gamma), np.cos(gamma), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
# Composed rotation matrix with (RX, RY, RZ)
R = np.dot(np.dot(RX, RY), RZ)
# Translation matrix
T = np.array([[1, 0, 0, dx],
[0, 1, 0, dy],
[0, 0, 1, dz],
[0, 0, 0, 1]])
# Projection 3D -> 2D matrix
A2 = np.array([[f, 0, w / 2, 0],
[0, f, h / 2, 0],
[0, 0, 1, 0]])
# Final transformation matrix
return np.dot(A2, np.dot(T, np.dot(R, A1)))
def get_image_3d_rotated(image, theta, phi, gamma, dx, dy, dz):
height, width = image.shape
rtheta, rphi, rgamma = np.deg2rad(theta), np.deg2rad(phi), np.deg2rad(gamma)
mat = get_3d_rotation_matrix(width, height, rtheta, rphi, rgamma, dx, dy, dz)
return cv2.warpPerspective(image.copy(), mat, (width, height))
def callback(x):
pass
alpha = 0
beta = 15
gamma = 0
canvas2D = np.zeros((600, 800), dtype=np.uint8)
#canvas2D = cv2.circle(canvas2D, (canvas2D.shape[1]//2, canvas2D.shape[0]//2), 50, 255)
cv2.namedWindow('circleImg')
cv2.createTrackbar('pitch', 'circleImg', 0, 90, callback)
cv2.createTrackbar('yaw', 'circleImg', 0, 90, callback)
cv2.createTrackbar('roll', 'circleImg', 0, 90, callback)
cv2.createTrackbar('radius', 'circleImg', 50, 100, callback)
cv2.setTrackbarPos('pitch', 'circleImg', 0)
cv2.setTrackbarPos('yaw', 'circleImg', 0)
cv2.setTrackbarPos('roll', 'circleImg', 0)
while True:
canvas2D = np.zeros((1000, 1000), dtype = np.uint8)
pitch = cv2.getTrackbarPos('pitch', 'circleImg')
yaw = cv2.getTrackbarPos('yaw', 'circleImg')
roll = cv2.getTrackbarPos('roll', 'circleImg')
radius = cv2.getTrackbarPos('radius', 'circleImg')
cv2.circle(canvas2D, (canvas2D.shape[1] // 2, canvas2D.shape[0] // 2), radius, 255)
rotated_img = get_image_3d_rotated(canvas2D, pitch, yaw, roll, 0, 0, 0)
cv2.imshow('circleImg', rotated_img)
contours, _ = cv2.findContours(rotated_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if len(contours) > 0:
if len(contours[0]) > 4:
ellipse = cv2.fitEllipse(contours[0])
if ellipse[1][0] > 0 and ellipse[1][1] > 0:
THETA = np.arccos(ellipse[1][0] / ellipse[1][1])
FI = ellipse[2] * np.pi / 180
my_len = ellipse[1][1] / np.cos(FI)
my_angle = np.arccos(my_len / ellipse[1][1]) * np.pi / 180
print(my_angle)
img_contours = np.zeros((rotated_img.shape[0], rotated_img.shape[1], 3), dtype=np.uint8)
#calculate the offset from the center of the ellipse
offset_minor_x = ellipse[1][0] / 2 * np.cos((90-ellipse[2]) * np.pi / 180)
offset_minor_y = ellipse[1][0] / 2 * np.cos(ellipse[2] * np.pi / 180)
offset_major_x = ellipse[1][1] / 2 * np.cos(ellipse[2] * np.pi / 180)
offset_major_y = ellipse[1][1] / 2 * np.sin(ellipse[2] * np.pi / 180)
#start and endpoints of the major and minor axes
minor_end_x = int(ellipse[0][0] + offset_minor_x)
minor_end_y = int(ellipse[0][0] + offset_minor_y)
minor_start_x = int(ellipse[0][0] - offset_minor_x)
minor_start_y = int(ellipse[0][0] - offset_minor_y)
major_end_x = int(ellipse[0][1] + offset_major_x)
major_end_y = int(ellipse[0][1] - offset_major_y)
major_start_x = int(ellipse[0][1] - offset_major_x)
major_start_y = int(ellipse[0][1] + offset_major_y)
#start and endpoints of major and minor axes
cv2.circle(img_contours, (minor_start_y, minor_start_x), 2, (0, 255, 0), -1)
cv2.circle(img_contours, (minor_end_y, minor_end_x), 2, (0, 0, 255), -1)
cv2.circle(img_contours, (major_start_y, major_start_x), 2, (0, 255, 0), -1)
cv2.circle(img_contours, (major_end_y, major_end_x), 2, (0, 0, 255), -1)
#draw major and minor axes
cv2.line(img_contours, [minor_start_y, minor_start_x], [minor_end_y, minor_end_x], (0, 0, 255))
cv2.line(img_contours, [major_start_y, major_start_x], [major_end_y, major_end_x], (0, 255, 0))
cv2.ellipse(img_contours, ellipse, (255, 0, 255))
cv2.putText(img_contours, 'THETA:{:.2f}'.format(THETA*180/np.pi), (100, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2)
cv2.imshow('circleImg', img_contours)
if cv2.waitKey(5) & 0xFF == 27:
cv2.destroyAllWindows()
break
