def calibrate():
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
Nx_cor = 9
Ny_cor = 6
objp = np.zeros((Nx_cor * Ny_cor, 3), np.float32)
objp[:, :2] = np.mgrid[0:Nx_cor, 0:Ny_cor].T.reshape(-1, 2)
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.
count = 0 # count 用来标志成功检测到的棋盘格画面数量
while (True):
ret, frame = cap.read()
if cv2.waitKey(1) & 0xFF == ord(' '):
# Our operations on the frame come here
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (Nx_cor, Ny_cor),
None) # Find the corners
# If found, add object points, image points
if ret == True:
corners = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
criteria)
objpoints.append(objp)
imgpoints.append(corners)
cv2.drawChessboardCorners(frame, (Nx_cor, Ny_cor), corners,
ret)
count += 1
if count > 20:
break
# Display the resulting frame
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
global mtx, dist
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None,
None)
print(mtx, dist)
mean_error = 0
for i in range(len(objpoints)):
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i],
mtx, dist)
error = cv2.norm(imgpoints[i], imgpoints2,
cv2.NORM_L2) / len(imgpoints2)
mean_error += error
print("total error: ", mean_error / len(objpoints))
# # When everything done, release the capture
np.savez('calibrate.npz', mtx=mtx, dist=dist[0:4])
def PoseEstimation(path_for_read, image_points):
# Read Image
im = cv2.imread(path_for_read)
size = im.shape
# Default 3D model points.
model_points = np.array([
(0.0, 0.0, 0.0), # Nose tip
(0.0, -330.0, -65.0), # Chin
(-225.0, 170.0, -135.0), # Left eye left corner
(225.0, 170.0, -135.0), # Right eye right corne
(-150.0, -150.0, -125.0), # Left Mouth corner
(150.0, -150.0, -125.0) # Right mouth corner
])
# Camera internals
focal_length = size[1]
center = (size[1] / 2, size[0] / 2)
camera_matrix = np.array([[focal_length, 0, center[0]],
[0, focal_length, center[1]], [0, 0, 1]],
dtype="double")
print('Camera Matrix: ' + '\n' + format(camera_matrix))
dist_coeffs = np.zeros((4, 1)) # Assuming no lens distortion
(success, rotation_vector,
translation_vector) = cv2.solvePnP(model_points,
image_points,
camera_matrix,
dist_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE)
print('Rotation Vector: ' + '\n' + format(rotation_vector))
print('Translation Vector: ' + '\n' + format(translation_vector))
# Project a 3D point (0, 0, 1000.0) onto the image plane.
# We use this to draw a line sticking out of the nose
nose_end_point2D = cv2.projectPoints(
np.array([(0.0, 0.0, float(focal_length))]), rotation_vector,
translation_vector, camera_matrix, dist_coeffs)[0]
print('nose_end_point2D: ' + str(nose_end_point2D))
for p in image_points:
cv2.circle(im, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1)
p1 = (int(image_points[0][0]), int(image_points[0][1]))
p2 = (int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))
p12_vector = ((int(p1[0]) - int(p2[0])), (int(p1[1]) - int(p2[1])))
p2p1_distance = np.sqrt(
np.square(int(p1[0]) - int(p2[0])) +
np.square(int(p1[1]) - int(p2[1])))
p2p1x_tangent = (int(p1[1]) - int(p2[1])) / (int(p1[0]) - int(p2[0]))
print('p12_vector: ' + str(p12_vector))
print('p2p1x_tangent' + str(p2p1x_tangent))
print('p2p1_distance: ' + str(p2p1_distance / focal_length))
cv2.line(im, p1, p2, (255, 0, 0), 2)
# Display image
cv2.imshow("Output", im)
cv2.waitKey(0)
def AxisAndBox():
# 读取相机参数
with open(path2+'//calibration_parameters.yaml') as file:
documents = yaml.full_load(file)
data = list(documents.values())
mtx = np.asarray(data[2])
#dist = np.asarray(data[1])
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((13 * 6, 3), np.float32)
objp[:, :2] = np.mgrid[0:13, 0:6].T.reshape(-1, 2)
axis = np.float32([[0, 0, 0], [0, 3, 0], [3, 3, 0], [3, 0, 0],
[0, 0, -3], [0, 3, -3], [3, 3, -3], [3, 0, -3]])
axis2 = np.float32([[4, 0, 0], [0, 4, 0], [0, 0, -4]]).reshape(-1, 3)
num = 0 # 计数器
images2 = glob.glob(path2+'//tianyi_gao_undistorted*.jpg')
for fname in images2:
num = num+1
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (13, 6), None)
if ret is True:
corners2 = cv2.cornerSubPix(
gray, corners, (11, 11), (-1, -1), criteria)
# 计算外参,估计相机位姿
_, rvecs, tvecs = cv2.solvePnP(objp, corners2, mtx, None)
# 将3-D点投影到像平面
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, None)
imgpts2, jac2 = cv2.projectPoints(axis2, rvecs, tvecs, mtx, None)
img = drawBox(img, corners2, imgpts)
img = drawAxis(img, corners2, imgpts2)
cv2.imwrite(path2+"//tianyi_gao_"+str(num)+".jpg", img)
if len(images2) < 16: # 图片过多时，不在UI中展示
cv2.namedWindow('press any key to continue', cv2.WINDOW_NORMAL)
cv2.imshow('press any key to continue', img)
cv2.waitKey(0)
print('Draw Done')
cv2.destroyAllWindows()
return num, path2#返回处理图片数和结果存储路径
def apply_projection(points, calibration_file):
intrinsic = get_intrinsic_matrix_depth(calibration_file)
ext_d = get_extrinsic_matrix_depth(calibration_file, idx=4)
r_vec = ext_d[:3, :3]
t_vec = -ext_d[:3, 3]
k1, k2, k3 = get_k_depth(calibration_file)
im_coords, _ = cv2.projectPoints(points, r_vec, t_vec, intrinsic[:3, :3],
np.array([k1, k2, 0, 0]))
return im_coords
def solve(world, worldlist, imagelist):
worldx = world.shape[0]
worldy = world.shape[1]
world = world.reshape((worldx * worldy, 3))
K = np.float32([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
_, rotation_vector, translation_vector = cv2.solvePnP(
worldlist, imagelist, K, np.zeros(5))
image_points, _ = cv2.projectPoints(world, rotation_vector,
translation_vector, K, np.zeros(5))
image_points = image_points.reshape((worldx, worldy, 2))
return image_points
def augmentation3d(self):
plt.close('all')
with np.load('output.npz') as X:
mtx, dist, _, _ = [
X[i] for i in ('arr_0', 'arr_1', 'arr_2', 'arr_3')
]
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30,
0.001)
objp = np.zeros((11 * 8, 3), np.float32)
objp[:, :2] = np.mgrid[0:11, 0:8].T.reshape(-1, 2)
axis = np.float32([[1, 1, 0], [5, 1, 0], [3, 5, 0], [3, 3, -3]])
filepath = list()
for i in range(5):
path = os.path.join('Datasets/Q3_Image', str(i + 1) + '.bmp')
filepath.append(path)
ims = list()
fig = plt.figure('augumentation3d')
for fname in filepath:
img = cv.imread(fname)
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
ret, corners = cv.findChessboardCorners(gray, (11, 8), None)
if ret == True:
corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
# Find the rotation and translation vectors.
_, rvecs, tvecs, _ = cv.solvePnPRansac(objp, corners2, mtx,
dist)
# project 3D points to image plane
imgpts, _ = cv.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = self.draw(img, corners2, imgpts)
plt_img = img[:, :, ::-1]
im = plt.imshow(plt_img, animated=True)
ims.append([im])
_ = animation.ArtistAnimation(fig,
ims,
interval=500,
blit=True,
repeat_delay=500)
plt.show()
def project_points(pcd_points, calibration_file):
#get the data for calibration
intrinsic = get_intrinsic_matrix(calibration_file)
ext_d = get_extrinsic_matrix(calibration_file, idx=4)
r_vec = ext_d[:3, :3]
t_vec = -ext_d[:3, 3]
k1, k2, k3 = get_k(calibration_file)
im_coords, jac = cv2.projectPoints(pcd_points, r_vec,
t_vec, intrinsic[:3, :3],
np.array([k1, k2, 0, 0]))
return im_coords, jac
def remap(self, parmas):
rvec = parmas[:3]
tvec = parmas[3:6]
image_points, _ = cv.projectPoints(
self.objpoints, rvec, tvec, self.K, np.zeros(5))
img_gray = cv.cvtColor(self.input_image, cv.COLOR_BGR2GRAY)
x,y = img_gray.shape
image_height_coords = image_points[:, 0, 0].reshape(
(y, x)).astype(np.float32).T
image_width_coords = image_points[:, 0, 1].reshape(
(y, x)).astype(np.float32).T
remapped = cv.remap(img_gray, image_height_coords,
image_width_coords, cv.INTER_CUBIC, None, cv.BORDER_REPLICATE)
plt.imshow(remapped)
plt.show()
def _render_3d_line(img,
start,
end,
rvecs,
tvecs,
mtx,
dist,
colour=(0, 0, 255),
thickness=2):
# get line start and end points
objpts = np.float32([[start], [end]]).reshape(-1, 3)
# project these to image plane
imgpts, jac = cv2.projectPoints(objpts, rvecs, tvecs, mtx, dist)
imgpts = imgpts.reshape(-1, 2)
# render the line between projected start and end points
cv2.line(
img=img,
pt1=tuple(imgpts[0]),
pt2=tuple(imgpts[1]),
color=colour,
thickness=thickness,
)
np.save("./camera_params/dist", dist)
np.save("./camera_params/rvecs", rvecs)
np.save("./camera_params/tvecs", tvecs)
#Get exif data in order to get focal length.
exif_img = PIL.Image.open(calibration_paths[0])
stuff = exif_img.getexif().items()
exif_data = {
PIL.ExifTags.TAGS[k]:v
for k, v in exif_img.getexif().items()
if k in PIL.ExifTags.TAGS}
#Get focal length in tuple form
focal_length_exif = exif_data['FocalLength']
#Get focal length in decimal form
focal_length = focal_length_exif[0]/focal_length_exif[1]
#Save focal length
np.save("./camera_params/FocalLength", focal_length)
#Calculate projection error.
mean_error = 0
for i in range(len(obj_points)):
img_points2, _ = cv2.projectPoints(obj_points[i],rvecs[i],tvecs[i], K, dist)
error = cv2.norm(img_points[i], img_points2, cv2.NORM_L2)/len(img_points2)
mean_error += error
total_error = mean_error/len(obj_points)
print (total_error)
def calibration(images):
# termination criteria
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((7 * 10, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:10].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
for fname in images:
img = cv.imread(fname)
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv.findChessboardCorners(gray, (7, 10), None)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
imgpoints.append(corners)
# Draw and display the corners
cv.drawChessboardCorners(img, (7, 10), corners2, ret)
img_resized = cv.resize(img, (1400, 700))
cv.imshow('img', img_resized)
cv.waitKey(10)
cv.destroyAllWindows()
ret, K, dist, rvecs, tvecs, std_int, std_ext, pVE = \
cv.calibrateCameraExtended(objpoints, imgpoints, gray.shape[::-1], None, None)
mean_error = []
error_vecs = np.zeros(
(70 * len(images),
2)) # vertical stack of [x, y] errors for all points in all pictures
for i in range(len(objpoints)): # calculating errors
imgpoints2, _ = cv.projectPoints(objpoints[i], rvecs[i], tvecs[i], K,
dist)
error = cv.norm(imgpoints[i], imgpoints2, cv.NORM_L2) / len(imgpoints2)
mean_error.append(error)
imgpoints2 = np.array(imgpoints2)
imgpoints2 = imgpoints2[:, 0, :]
imgpoints1 = np.array(imgpoints[i])
imgpoints1 = imgpoints1[:, 0, :]
error_vecs[i * 70:(i + 1) * 70, :] = imgpoints1 - imgpoints2
fig = plt.figure(1) # mean reprojection error plot
img_nr = [f"{i+1}" for i in range(len(images))]
plt.bar(img_nr, mean_error)
plt.ylabel("mean reprojection error")
plt.xlabel("image number")
plt.savefig("Calibration_errors")
plt.show()
fig2 = plt.figure(2)
plt.scatter(error_vecs[:, 0], error_vecs[:, 1])
plt.ylabel("y error")
plt.xlabel("x error")
plt.savefig("Reprojection_scatter")
plt.show()
print('Standard errors')
print('Focal length and principal point')
print('--------------------------------')
print('fx: %g +/- %g' % (K[0, 0], std_int[0]))
print('fy: %g +/- %g' % (K[1, 1], std_int[1]))
print('cx: %g +/- %g' % (K[0, 2], std_int[2]))
print('cy: %g +/- %g' % (K[1, 2], std_int[3]))
print('Distortion coefficients')
print('--------------------------------')
print('k1: %g +/- %g' % (dist[0, 0], std_int[4]))
print('k2: %g +/- %g' % (dist[0, 1], std_int[5]))
print('p1: %g +/- %g' % (dist[0, 2], std_int[6]))
print('p2: %g +/- %g' % (dist[0, 3], std_int[7]))
print('k3: %g +/- %g' % (dist[0, 4], std_int[8]))
np.savetxt('cam_matrix.txt', K)
np.savetxt('dist.txt', dist)
np.savetxt('stdInt.txt', std_int)
np.savetxt('stdExt.txt', std_ext)
while (True):
img = hp.im
ret, imagePoints = hp.getImagePoints(img)
ret, rotationVector, translationVector, eulerAngle = hp.readHeadPose()
ret, pitch, yaw, roll = hp.getEulerAngle(rotationVector)
eulerAngle_str = 'Y:{}, X:{}, Z:{}'.format(pitch, yaw, roll)
print(eulerAngle_str)
# Project a 3D point (0, 0, 1000.0) onto the image plane.
# We use this to draw a line sticking out of the nose
(nose_end_point2D,
jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]),
rotationVector, translationVector,
cameraMatrix, dist_coeffs)
for p in imagePoints:
cv2.circle(img, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1)
p1 = (int(imagePoints[0][0]), int(imagePoints[0][1]))
p2 = (int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))
cv2.line(im, p1, p2, (255, 0, 0), 2)
# Display image
#cv2.putText( im, str(rotationVector), (0, 100), cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 255), 1 )
cv2.putText(im, eulerAngle_str, (0, 120), cv2.FONT_HERSHEY_PLAIN, 1,
(0, 0, 255), 1)
cv2.imshow("Output", im)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)
# If found, add object points, image points (after refining them)
objpoints.append(objp)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6 * 7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)
axis = np.float32([[0, 0, 0], [0, 3, 0], [3, 3, 0], [3, 0, 0], [0, 0, -3],
[0, 3, -3], [3, 3, -3], [3, 0, -3]])
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
imgpoints.append(corners2)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None,
None)
# Find the rotation and translation vectors.
ret, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, None)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(img, corners2, imgpts)
cv2.imshow('img', img)
cv2.waitKey(0)
cv2.destroyAllWindows()
def square_line(origin, edges, hough):
'''
输入：原始图片，黑白边线图，hough数组
输出：带坐标的原图，Table_2D
'''
internal_calibration = get_camera_intrinsic()
internal_calibration = np.array(internal_calibration, dtype='float32')
distCoeffs = np.zeros((8, 1), dtype='float32')
img = copy.copy(origin)
lines = cv2.HoughLines(edges, hough[0], hough[1], hough[2])
# rho：ρ，图片左上角向直线所做垂线的长度
# theta：Θ，图片左上角向直线所做垂线与顶边夹角
# 垂足高于原点时，ρ为负，Θ为垂线关于原点的对称线与顶边的夹角
top_line_theta = []
top_line_rho = []
left_line_theta = []
left_line_rho = []
right_line_theta = []
right_line_rho = []
bottom_line_theta = []
bottom_line_rho = []
horizon = []
summ = 0
final_lines = np.zeros((4, 2))
if len(lines) < 4:
print("\033[0;31m未检测到方桌！\033[0m")
return edges
else:
for line in lines:
for rho, theta in line:
if (theta > math.pi / 3 and theta < math.pi * 2 / 3): # 横线
horizon.append(line)
elif rho < 0: # 右边
right_line_rho.append(rho)
right_line_theta.append(theta)
else: # 左边
left_line_theta.append(theta)
left_line_rho.append(rho)
top, bottom = Cluster(horizon, 180, 360) # 将横线依据abs(rho)分为上下
for line in top:
for rho, theta in line:
top_line_rho.append(rho)
top_line_theta.append(theta)
for line in bottom:
for rho, theta in line:
bottom_line_rho.append(rho)
bottom_line_theta.append(theta)
for i in right_line_theta:
summ += i
right_line_theta_average = summ / len(right_line_theta)
final_lines[0, 1] = right_line_theta_average
summ = 0
for i in right_line_rho:
summ += i
right_line_rho_average = summ / len(right_line_rho)
final_lines[0, 0] = right_line_rho_average
summ = 0
for i in left_line_theta:
summ += i
left_line_theta_average = summ / len(left_line_theta)
final_lines[1, 1] = left_line_theta_average
summ = 0
for i in left_line_rho:
summ += i
left_line_rho_average = summ / len(left_line_rho)
final_lines[1, 0] = left_line_rho_average
summ = 0
for i in top_line_theta:
summ += i
top_line_theta_average = summ / len(top_line_theta)
final_lines[2, 1] = top_line_theta_average
summ = 0
for i in top_line_rho:
summ += i
top_line_rho_average = summ / len(top_line_rho)
final_lines[2, 0] = top_line_rho_average
summ = 0
for i in bottom_line_theta:
summ += i
bottom_line_theta_average = summ / len(bottom_line_theta)
final_lines[3, 1] = bottom_line_theta_average
summ = 0
for i in bottom_line_rho:
summ += i
bottom_line_rho_average = summ / len(bottom_line_rho)
final_lines[3, 0] = bottom_line_rho_average
summ = 0
# print(final_lines)
final_lines = np.array(final_lines)
for i in range(4):
theta = final_lines[i, 1]
rho = final_lines[i, 0]
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
x1 = int(x0 + 1000 * (-b))
y1 = int(y0 + 1000 * (a))
x2 = int(x0 - 1000 * (-b))
y2 = int(y0 - 1000 * (a))
cv2.line(img, (x1, y1), (x2, y2), (200, 135, 100), 2)
# 标记直线编号
string = str(i)
cv2.putText(img, string, (int(x0), int(y0)),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (255, 0, 255), 1)
left_top_point_x, left_top_point_y = getcrosspoint(
left_line_rho_average, left_line_theta_average,
top_line_rho_average, top_line_theta_average)
left_bottom_point_x, left_bottom_point_y = getcrosspoint(
left_line_rho_average, left_line_theta_average,
bottom_line_rho_average, bottom_line_theta_average)
right_top_point_x, right_top_point_y = getcrosspoint(
right_line_rho_average, right_line_theta_average,
top_line_rho_average, top_line_theta_average)
right_bottom_point_x, right_bottom_point_y = getcrosspoint(
right_line_rho_average, right_line_theta_average,
bottom_line_rho_average, bottom_line_theta_average)
Table_2D = []
Table_2D.append([left_top_point_x, left_top_point_y])
Table_2D.append([left_bottom_point_x, left_bottom_point_y])
Table_2D.append([right_top_point_x, right_top_point_y])
Table_2D.append([right_bottom_point_x, right_bottom_point_y])
cv2.circle(img, (left_top_point_x, left_top_point_y), 3, (255, 0, 0),
-1)
cv2.circle(img, (left_bottom_point_x, left_bottom_point_y), 3,
(255, 0, 0), -1)
cv2.circle(img, (right_top_point_x, right_top_point_y), 3, (255, 0, 0),
-1)
cv2.circle(img, (right_bottom_point_x, right_bottom_point_y), 3,
(255, 0, 0), -1)
Table_3D = []
Table_3D.append([0, 0, 0])
Table_3D.append([0, 55, 0])
Table_3D.append([55, 0, 0])
Table_3D.append([55, 55, 0])
Table_3D = np.array(Table_3D, dtype='float32')
Table_2D = np.array(Table_2D, dtype='float32')
_, rvector, tvector = cv2.solvePnP(Table_3D, Table_2D,
internal_calibration, distCoeffs)
axis = np.float32([[55, 0, 0], [0, 55, 0], [0, 0, -20]]).reshape(-1, 3)
imgpts, _ = cv2.projectPoints(
axis,
rvector,
tvector,
internal_calibration,
distCoeffs,
)
lined = draw(img, (left_top_point_x, left_top_point_y), imgpts)
return lined, Table_2D
def render(self, index, points, colors, size=4, view_distance=50):
"""
Project given points into the image with given index. Returns rendered image
:param index: index of the image / camera pose. See self.poses to select an image / pose.
:param points: 3d points as n x 3 numpy array in image coordinates
:param colors: colors of each point as n x 3 numpy array. Defined between 0 and 255
:param size: size of each point in. points further away are drawn more small
:param view_distance: only points that are within this distance in meter to the camera origin are drawn.
:return: rendered color image
"""
assert 0 <= index < len(self.poses)
img_path = self.folder / self.poses[index]['filename']
img = cv2.imread(str(img_path))
# project_and_draw(img, points, colors, self.poses[index]['full-pose'], size, max_view_distance)
pose = self.poses[index]['full-pose']
rot_vec = -np.array([pose['rx'], pose['ry'], pose['rz']])
t_vec = -np.array([pose['tx'], pose['ty'], pose['tz']
]) @ cv2.Rodrigues(rot_vec)[0].T
# select points which are close
cam_pos = -np.matmul(cv2.Rodrigues(rot_vec)[0].T, t_vec)
distances = np.linalg.norm(points - cam_pos, axis=1)
view_mask = distances < view_distance
# select points which are in front of camera
cam_points3d = points @ cv2.Rodrigues(rot_vec)[0].T + t_vec
view_mask = view_mask & (cam_points3d[:, 2] > 0)
view_points3d = points[view_mask]
view_distances = distances[view_mask]
view_colors = colors[view_mask]
if len(view_points3d) == 0:
return
view_points2d = cv2.projectPoints(view_points3d, rot_vec, t_vec,
self.K,
self.distortion)[0].reshape(-1, 2)
p = view_points2d
selection = np.all((p[:, 0] >= 0, p[:, 0] < img.shape[1], p[:, 1] >= 0,
p[:, 1] < img.shape[0]),
axis=0)
p = p[selection]
# closest points are at 4 meter distance
norm_distances = view_distances[selection] / 4.0
shift = 3
factor = (1 << shift)
def I(x_):
return int(x_ * factor + 0.5)
for i in range(0, len(p)):
cv2.circle(img, (I(p[i][0]), I(p[i][1])),
I(size / norm_distances[i]),
view_colors[i],
-1,
shift=shift)
return img
def check_drowsiness_yawn(self, img, rect, dtype="int"):
self.frame = img
img = img[:, :, [2, 1, 0]] # BGR => RGB
yawn = False
drowsiness = False
landmarks = self.predictor(img, rect)
# get the left and right eye coordinates
left_eye = []
for i in range(36, 42):
left_eye.append([landmarks.part(i).x, landmarks.part(i).y])
right_eye = []
for i in range(42, 48):
right_eye.append([landmarks.part(i).x, landmarks.part(i).y])
# calculate the eye aspect ratio for both eyes
left_ear = self.eye_aspect_ratio(left_eye)
right_ear = self.eye_aspect_ratio(right_eye)
# average the eye aspect ratio together for both eyes
ear = (left_ear + right_ear) / 2.0
# print('ear:', ear, 'eye_threshold', self.eye_threshold)
# check to see if the eye aspect ratio is below the eye threshold
if self.t_start and ear < self.eye_threshold:
self.t_end = time.time()
t = self.t_end - self.t_start
if t >= 1.3:
drowsiness = True
else:
self.t_start = time.time()
# check yawn
top_lips = []
bottom_lips = []
for i in range(0, 68):
if 50 <= i <= 53 or 61 <= i <= 64:
top_lips.append((landmarks.part(i).x, landmarks.part(i).y))
elif 65 <= i <= 68 or 56 <= i <= 59:
bottom_lips.append((landmarks.part(i).x, landmarks.part(i).y))
top_lips = np.squeeze(np.asarray(top_lips))
bottom_lips = np.squeeze(np.asarray(bottom_lips))
top_lips_mean = np.array(np.mean(top_lips, axis=0), dtype=dtype)
bottom_lips_mean = np.array(np.mean(bottom_lips, axis=0), dtype=dtype)
top_lips_mean = top_lips_mean.reshape(-1)
bottom_lips_mean = bottom_lips_mean.reshape(-1)
#distance=math.sqrt((bottom_lips_mean[0] - top_lips_mean[0])**2 + (bottom_lips_mean[-1] - top_lips_mean[-1])**2)
distance = bottom_lips_mean[-1] - top_lips_mean[-1]
threshold = (rect.bottom() - rect.top()) * self.mouth_threshold
if distance > threshold:
yawn = True
# gaze detection
left_gaze_ratio = self.get_gaze_ratio(
[landmarks.part(i) for i in range(36, 42)])
right_gaze_ratio = self.get_gaze_ratio(
[landmarks.part(i) for i in range(42, 48)])
gaze_ratio = (right_gaze_ratio + left_gaze_ratio) / 2
if gaze_ratio <= 0.75:
gaze = 'RIGHT'
elif 0.75 < gaze_ratio < 1.3:
gaze = 'CENTER'
else:
gaze = 'LEFT'
# head pose
shape0 = np.array(face_utils.shape_to_np(landmarks))
image_points = np.array(
[
(shape0[33, :]), # Nose tip
(shape0[8, :]), # Chin
(shape0[36, :]), # Left eye left corner
(shape0[45, :]), # Right eye right corner
(shape0[48, :]), # Left Mouth corner
(shape0[54, :]) # Right mouth corner
],
dtype="double")
model_points = np.array([
(0.0, 0.0, 0.0), # Nose tip
(0.0, -330.0, -65.0), # Chin
(-225.0, 170.0, -135.0), # Left eye left corner
(225.0, 170.0, -135.0), # Right eye right corne
(-150.0, -150.0, -125.0), # Left Mouth corner
(150.0, -150.0, -125.0) # Right mouth corner
])
focal_length = 640
center = (320, 180)
camera_matrix = np.array([[focal_length, 0, center[0]],
[0, focal_length, center[1]], [0, 0, 1]],
dtype="double")
# print ("Camera Matrix :\n {0}".format(camera_matrix))
dist_coeffs = np.zeros((4, 1)) # Assuming no lens distortion
(success, rotation_vector,
translation_vector) = cv.solvePnP(model_points,
image_points,
camera_matrix,
dist_coeffs,
flags=cv.SOLVEPNP_ITERATIVE)
# print ("Rotation Vector:\n {0}".format(rotation_vector))
# print ("Translation Vector:\n {0}".format(translation_vector))
(nose_end_point2D,
jacobian) = cv.projectPoints(np.array([(0.0, 0.0, 1000.0)]),
rotation_vector, translation_vector,
camera_matrix, dist_coeffs)
p1 = (int(image_points[0][0]), int(image_points[0][1]))
p2 = (int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))
# print(str(p2[0] - p1[0]), str(p2[1] - p1[1]))
# axis = np.float32([[500,0,0],
# [0,500,0],
# [0,0,500]])
# imgpts, jac = cv.projectPoints(axis, rotation_vector, translation_vector, camera_matrix, dist_coeffs)
# modelpts, jac2 = cv.projectPoints(model_points, rotation_vector, translation_vector, camera_matrix, dist_coeffs)
rvec_matrix = cv.Rodrigues(rotation_vector)[0]
proj_matrix = np.hstack((rvec_matrix, translation_vector))
eulerAngles = cv.decomposeProjectionMatrix(proj_matrix)[6]
pitch, yaw, roll = [math.radians(_) for _ in eulerAngles]
pitch = math.degrees(math.asin(math.sin(pitch)))
roll = -math.degrees(math.asin(math.sin(roll)))
yaw = math.degrees(math.asin(math.sin(yaw)))
# print(pitch, roll, yaw)
# ''' x & y rotation '''
# v_x = p2[0] - p1[0]
# v_y = p2[1] - p1[1]
# area = (shape0[45, 0] - shape0[36, 0]) * (shape0[8, 1] - (shape0[45, 1] + shape0[36, 1]) / 2)
# area = math.sqrt(area) if area > 0 else 0
# length = 1000 * area / 200
# if length**2 - v_x**2 - v_y**2 <= 0:
# v_x = 0
# v_y = 0
# h = math.sqrt(length**2 - v_x**2 - v_y**2)
# theta_x = math.degrees(math.atan2(h, v_x)) - 90.0
# theta_y = math.degrees(math.atan2(h, v_y)) - 90.0
# theta_x = (theta_x / 3.5) ** 2 * 3.5 if theta_x > 0 else -(theta_x / 3.5) ** 2 * 3.5
# theta_y = (theta_y / 3) ** 2 * 3 if theta_y > 0 else -(theta_y / 3) ** 2 * 3
# print('angle:', theta_x, theta_y)
return drowsiness, yawn, gaze, [p1, p2], [yaw, pitch * 4]