def getPoseEstimation(imgSize, imagePoints):
# 3D model points.
modelPoints = 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
focalLength = imgSize[1]
center = (imgSize[1] / 2, imgSize[0] / 2)
cameraMatrix = np.array([[focalLength, 0, center[0]],
[0, focalLength, center[1]], [0, 0, 1]],
dtype="double")
dist_coeffs = np.zeros((4, 1)) # Assuming no lens distortion
(success, rotationVector,
translationVector) = cv2.solvePnP(modelPoints,
imagePoints,
cameraMatrix,
dist_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE)
return success, rotationVector, translationVector, cameraMatrix, dist_coeffs
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 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 solve(self):
cubic_slopes = [0.0, 0.0]
self.K = np.float32([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
_, rvec, tvec = cv.solvePnP(
self.world_list, self.image_list, self.K, np.zeros(5))
parmas = np.hstack((np.array(rvec).flatten(),
np.array(tvec).flatten(),
np.array(cubic_slopes).flatten(),
))
return parmas
def find_extrinsic(self):
with np.load('img_obj_points.npz') as X:
imgpoints, objpoints = [X[i] for i in ('arr_0', 'arr_1')]
with np.load('output.npz') as X:
mtx, dist, _, _ = [
X[i] for i in ('arr_0', 'arr_1', 'arr_2', 'arr_3')
]
num = self.spinBox.value()
retval, rvecs, tvecs = cv.solvePnP(objpoints[num - 1],
imgpoints[num - 1], mtx, dist)
dst, _ = cv.Rodrigues(rvecs)
extrinsic_mtx = cv.hconcat([dst, tvecs])
print(extrinsic_mtx)
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 return_roll_pitch_yaw(self, image, radians=False):
"""
Return the the roll pitch and yaw angles associated with the input image.
@param image It is a colour image. It must be >= 64 pixel.
@param radians When True it returns the angle in radians, otherwise in degrees.
"""
# The dlib shape predictor returns 68 points,
# we are interested only in a few of those
TRACKED_POINTS = (0, 4, 8, 12, 16, 17, 26, 27, 30, 33, 36, 39, 42, 45,
62)
# Antropometric constant values of the human head.
# Check the wikipedia EN page and:
# "Head-and-Face Anthropometric Survey of U.S. Respirator Users"
#
# X-Y-Z with X pointing forward and Y on the left and Z up.
# The X-Y-Z coordinates used are like the standard
# coordinates of ROS (robotic operative system)
# OpenCV uses the reference usually used in computer vision:
# X points to the right, Y down, Z to the front #
# The Male mean interpupillary distance is 64.7 mm
# (https://en.wikipedia.org/wiki/Interpupillary_distance)
#
P3D_RIGHT_SIDE = np.float32([-100.0, -77.5, -5.0]) #0
P3D_GONION_RIGHT = np.float32([-110.0, -77.5, -85.0]) #4
P3D_MENTON = np.float32([0.0, 0.0, -122.7]) #8
P3D_GONION_LEFT = np.float32([-110.0, 77.5, -85.0]) #12
P3D_LEFT_SIDE = np.float32([-100.0, 77.5, -5.0]) #16
P3D_FRONTAL_BREADTH_RIGHT = np.float32([-20.0, -56.1, 10.0]) #17
P3D_FRONTAL_BREADTH_LEFT = np.float32([-20.0, 56.1, 10.0]) #26
P3D_SELLION = np.float32([0.0, 0.0, 0.0]) #27 This is the world origin
P3D_NOSE = np.float32([21.1, 0.0, -48.0]) #30
P3D_SUB_NOSE = np.float32([5.0, 0.0, -52.0]) #33
P3D_RIGHT_EYE = np.float32([-20.0, -32.35, -5.0]) #36
P3D_RIGHT_TEAR = np.float32([-10.0, -20.25, -5.0]) #39
P3D_LEFT_TEAR = np.float32([-10.0, 20.25, -5.0]) #42
P3D_LEFT_EYE = np.float32([-20.0, 32.35, -5.0]) #45
#P3D_LIP_RIGHT = np.float32([-20.0, 65.5,-5.0]) #48
#P3D_LIP_LEFT = np.float32([-20.0, 65.5,-5.0]) #54
P3D_STOMION = np.float32([10.0, 0.0, -75.0]) #62
# This matrix contains the 3D points of the
# 11 landmarks we want to find. It has been
# obtained from antrophometric measurement
# of the human head.
landmarks_3D = np.float32([
P3D_RIGHT_SIDE, P3D_GONION_RIGHT, P3D_MENTON, P3D_GONION_LEFT,
P3D_LEFT_SIDE, P3D_FRONTAL_BREADTH_RIGHT, P3D_FRONTAL_BREADTH_LEFT,
P3D_SELLION, P3D_NOSE, P3D_SUB_NOSE, P3D_RIGHT_EYE, P3D_RIGHT_TEAR,
P3D_LEFT_TEAR, P3D_LEFT_EYE, P3D_STOMION
])
# Return the 2D position of our landmarks
img_h, img_w, img_d = image.shape
landmarks_2D = self._return_landmarks(inputImg=image,
roiX=0,
roiY=img_w,
roiW=img_w,
roiH=img_h,
points_to_return=TRACKED_POINTS)
# Print som red dots on the image
# for point in landmarks_2D:
# cv2.circle(frame,( point[0], point[1] ), 2, (0,0,255), -1)
# Applying the PnP solver to find the 3D pose
# of the head from the 2D position of the #landmarks.
# retval - bool
# rvec - Output rotation vector that, together with tvec, brings
# points from the world coordinate system to the camera coordinate system.
# tvec - Output translation vector. It is the position of the world origin (SELLION) in camera co-ords
retval, rvec, tvec = cv2.solvePnP(landmarks_3D, landmarks_2D,
self.camera_matrix,
self.camera_distortion)
# Get as input the rotational vector
# Return a rotational matrix
rmat, _ = cv2.Rodrigues(rvec)
# euler_angles contain (pitch, yaw, roll)
# euler_angles = cv.DecomposeProjectionMatrix(
# projMatrix=rmat,
# cameraMatrix=camera_matrix,
# rotMatrix,
# transVect,
# rotMatrX=None,
# rotMatrY=None,
# rotMatrZ=None)
head_pose = [
rmat[0, 0], rmat[0, 1], rmat[0, 2], tvec[0], rmat[1, 0],
rmat[1, 1], rmat[1, 2], tvec[1], rmat[2, 0], rmat[2, 1],
rmat[2, 2], tvec[2], 0.0, 0.0, 0.0, 1.0
]
# print(head_pose) #TODO remove this line
return self.rotationMatrixToEulerAngles(rmat)
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 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]