def auto_keypoint_searching(img1, img2):
# 仿射旋轉
cols = img1.shape[1]
rows = img1.shape[0]
M = cv2.getRotationMatrix2D((cols / 2, rows / 2), 90, 1)
img1 = cv2.warpAffine(img1, M, (cols, rows))
# Initiate SIFT detector
orb = cv2.ORB_create()
# find the keypoints and descriptors with SIFT
kp1, des1 = orb.detectAndCompute(img1, None)
kp2, des2 = orb.detectAndCompute(img2, None)
# create BFMatcher object
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
# Match descriptors.
matches = bf.match(des1, des2)
# Sort them in the order of their distance.
matches = sorted(matches, key=lambda x: x.distance)
# Draw first 10 matches.
img3 = cv2.drawMatches(img1, kp1, img2, kp2, matches[:40], None, flags=2)
# Initialize lists
list_kp1 = []
list_kp2 = []
# For each match...
for mat in matches[:40]:
# Get the matching keypoints for each of the images
img1_idx = mat.queryIdx
img2_idx = mat.trainIdx
# x - columns
# y - rows
# Get the coordinates
(x1, y1) = kp1[img1_idx].pt
(x2, y2) = kp2[img2_idx].pt
# Append to each list
list_kp1.append((x1, y1))
list_kp2.append((x2, y2))
list_kp1, list_kp2 = np.array(list_kp1), np.array(list_kp2)
F = cv2.findFundamentalMat(list_kp1, list_kp2, cv2.FM_RANSAC)[0]
u, d, v = np.linalg.svd(F)
Z = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 0]])
W = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
S = np.dot(np.dot(u, Z), np.transpose(u))
R = np.dot(np.dot(u, W), v)
displace = [[S[1][2], S[2][0], S[0][1]]]
print(cv2.RQDecomp3x3(R))
print(displace)
plt.imshow(img3), plt.show()
def poseForFace(self):
flag, rvec, tvec = self.pose()
rodrigues = cv2.Rodrigues(rvec)
#ret, mtxR, mtxQ, qx, qy, qz = cv2.RQDecomp3x3(rodrigues[0])
R = numpy.array([0,0,0])
Q = numpy.array([0,0,0])
eulerAngles = cv2.RQDecomp3x3(rodrigues[0])
#print eulerAngles
return eulerAngles[0]
def compute_pose_var(rvecs, tvecs):
ret = np.empty(6)
reuler = np.array([cv2.RQDecomp3x3(cv2.Rodrigues(r)[0])[0] for r in rvecs])
# workaround for the given board so r_x does not oscilate between +-180°
reuler[:, 0] = reuler[:, 0] % 360
ret[0:3] = np.var(reuler, axis=0)
ret[3:6] = np.var(np.array(tvecs) / 10, axis=0).ravel() # [mm]
return ret
def rot_to_euler(R: np.array) -> np.array:
"""
:return: Euler angles in rad in ZYX
"""
import cv2 as cv
if torch.is_tensor(R):
R = R.detach().cpu().numpy()
angles = np.radians(cv.RQDecomp3x3(R)[0])
angles[0], angles[2] = angles[2], angles[0]
return angles
def printPose(mat, name):
mat = mat.getA()
x, y, z = (round(mat[0][3], 2), round(mat[1][3], 2), round(mat[2][3], 2))
mat = np.matrix([[mat[0][0], mat[0][1], mat[0][2]],
[mat[1][0], mat[1][1], mat[1][2]],
[mat[2][0], mat[2][1], mat[2][2]]])
#get Euler angles
retval, mtxR, mtxQ, Qx, Qy, Qz = cv2.RQDecomp3x3(mat)
pitch, yaw, roll = retval
txt = name + " [x: " + str(x) + " cm, y: " + str(y) + " cm, z: " + str(
z) + " cm, roll: " + str(round(roll, 2)) + "°, pitch: " + str(
round(pitch, 2)) + "°, yaw: " + str(round(yaw, 2)) + "°]"
print(txt)
def __getHeadPosePnP(self, frame):
res = []
cam_w, cam_h = np.shape(frame)
c_x = cam_w / 2
c_y = cam_h / 2
f_x = c_x / np.tan(60 / 2 * np.pi / 180)
f_y = f_x
camera_matrix = np.float32([[f_x, 0.0, c_x], [0.0, f_y, c_y],
[0.0, 0.0, 1.0]])
if debug:
print("Estimated camera matrix: \n" + str(camera_matrix) + "\n")
camera_distortion = np.float32([0.0, 0.0, 0.0, 0.0, 0.0])
faces_array = self.face.find_faces(frame)
if debug:
print("Total Faces: " + str(len(faces_array)))
for i, pos in enumerate(faces_array):
# face_x1 = pos.left()
# face_y1 = pos.top()
# face_x2 = pos.right()
# face_y2 = pos.bottom()
shape = self.face.find_landmarks(frame, pos)
landmarks_2D = np.zeros((len(HeadEstimator.TRACKED_POINTS), 2),
dtype=np.float32)
for j, k in enumerate(HeadEstimator.TRACKED_POINTS):
landmarks_2D[j] = [shape.parts()[k].x, shape.parts()[k].y]
# 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 model coordinate system to the camera coordinate system.
# tvec - Output translation vector.
retval, rvec, tvec = cv2.solvePnP(self.landmarks_3D, landmarks_2D,
camera_matrix, camera_distortion)
rodrigues = cv2.Rodrigues(rvec)
# ret, mtxR, mtxQ, qx, qy, qz = cv2.RQDecomp3x3(rodrigues[0])
eulerAngles = cv2.RQDecomp3x3(rodrigues[0])[0]
# Now we project the 3D points into the image plane
# Creating a 3-axis to be used as reference in the image.
# axis = np.float32([[50, 0, 0],
# [0, 50, 0],
# [0, 0, 50]])
# imgpts, jac = cv2.projectPoints(axis, rvec, tvec, camera_matrix, camera_distortion)
res.append(transformToRad(eulerAngles))
return res[0]
def eular_angles_from_landmarks(self, landmarks_2D):
"""
Estimates Euler angles from 2D landmarks
Parameters:
----------
landmarks_2D: numpy array of shape(N, 2) as N is num of landmarks (usualy 98 from WFLW)
Returns:
-------
rvec: rotation numpy array that transform model space to camera space (3D in both)
tvec: translation numpy array that transform model space to camera space
euler_angles: (pitch yaw roll) in degrees
"""
# WFLW(98 landmark) tracked points
TRACKED_POINTS_MASK = [
33, 38, 50, 46, 60, 64, 68, 72, 55, 59, 76, 82, 85, 16
]
landmarks_2D = landmarks_2D[TRACKED_POINTS_MASK]
"""
solve for extrensic matrix (rotation & translation) with 2D-3D correspondences
returns:
rvec: rotation vector (as rotation is 3 degree of freedom, it is represented as 3d-vector)
tvec: translate vector (world origin position relative to the camera 3d coord system)
_ : error -not important-.
"""
_, rvec, tvec = cv2.solvePnP(self.landmarks_3D,
landmarks_2D,
self.camera_intrensic_matrix,
distCoeffs=None)
"""
note:
tvec is almost constant = the world origin coord with respect to the camera
avarage value of tvec = [-1,-2,-21]
we can use this directly without computing tvec
"""
# convert rotation vector to rotation matrix .. note: function is used for vice versa
# rotation matrix that transform from model coord(object model 3D space) to the camera 3D coord space
rotation_matrix, _ = cv2.Rodrigues(rvec)
# [R T] may be used in cv2.decomposeProjectionMatrix(extrinsic)[6]
extrensic_matrix = np.hstack((rotation_matrix, tvec))
# decompose the extrensic matrix to many things including the 3 eular angles
# (pitch yaw roll) in degrees
euler_angles = cv2.RQDecomp3x3(rotation_matrix)[0]
return rvec, tvec, euler_angles
def estimatePos(self):
if len(self.corners) > 0:
self.retval, self.rvec, self.tvec = aruco.estimatePoseBoard(
self.corners, self.ids, board, self.cameraMatrix,
self.distanceCoefficients)
self.dst, jacobian = cv2.Rodrigues(self.rvec)
self.extristics = np.matrix([[
self.dst[0][0], self.dst[0][1], self.dst[0][2], self.tvec[0][0]
], [
self.dst[1][0], self.dst[1][1], self.dst[1][2], self.tvec[1][0]
], [
self.dst[2][0], self.dst[2][1], self.dst[2][2], self.tvec[2][0]
], [0.0, 0.0, 0.0, 1.0]])
#print(self.dst,self.tvec)
#print("self.extr:", self.extristics)
#print("self.extr.I:",self.extristics.I )
#self.worldRot = cv2.Rodrigues(self.rvec_trs)
self.extristics_I = self.extristics.I
self.worldPos = np.array([round(self.extristics_I[0,3]*100,3),\
round(self.extristics_I[1,3]*100,3),\
round(self.extristics_I[2,3]*100,3)])
self.worldRotM = np.zeros(shape=(3, 3))
cv2.Rodrigues(self.rvec, self.worldRotM, jacobian=0)
self.worldRot = cv2.RQDecomp3x3(self.worldRotM)
#self.worldPos = - self.tvec * self.rvec_trs
#print( self.tvec, self.rvec)
#self.worldPos = [self.worldPos[0][0],self.worldPos[1][1], self.worldPos[2][2]]
print("X:%.0f " % (self.worldPos[0]),\
"Y:%.0f "% (self.worldPos[1]),\
"Z:%.0f "% (self.worldPos[2]),\
"rot:%.0f "% (self.worldRot[0][2]+94))
#self.rvec, self.tvec, _ = aruco.estimatePoseSingleMarkers(self.corners[0], arucoMarkerLength, self.cameraMatrix, self.distanceCoefficients)
if self.retval != 0:
self.frame = aruco.drawAxis(self.frame, self.cameraMatrix,
self.distanceCoefficients,
self.rvec, self.tvec, 0.1)
def main():
predictor = dlib.shape_predictor(PREDICTOR_PATH)
cap = cv2.VideoCapture(args["camsource"])
while True:
GAZE = "Face Not Found"
ret, img = cap.read()
image = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
h, w, c = image.shape
# To improve performance, optionally mark the image as not writeable to
# pass by reference.
image.flags.writeable = False
results = face_detection.process(image)
# Draw the face detection annotations on the image.
image.flags.writeable = True
image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
if not ret:
print(
f'[ERROR - System]Cannot read from source: {args["camsource"]}'
)
break
if results.detections:
for detection in results.detections:
location = detection.location_data
relative_bounding_box = location.relative_bounding_box
x_min = relative_bounding_box.xmin
y_min = relative_bounding_box.ymin
widthh = relative_bounding_box.width
heightt = relative_bounding_box.height
absx, absy = mp_drawing._normalized_to_pixel_coordinates(
x_min, y_min, w, h)
abswidth, absheight = mp_drawing._normalized_to_pixel_coordinates(
x_min + widthh, y_min + heightt, w, h)
newrect = dlib.rectangle(absx, absy, abswidth, absheight)
cv2.rectangle(image, (absx, absy), (abswidth, absheight),
(0, 255, 0), 2)
shape = predictor(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), newrect)
draw(image, shape)
refImgPts = world.ref2dImagePoints(shape)
height, width, channels = img.shape
focalLength = args["focal"] * width
cameraMatrix = world.cameraMatrix(focalLength,
(height / 2, width / 2))
mdists = np.zeros((4, 1), dtype=np.float64)
# calculate rotation and translation vector using solvePnP
success, rotationVector, translationVector = cv2.solvePnP(
face3Dmodel, refImgPts, cameraMatrix, mdists)
noseEndPoints3D = np.array([[0, 0, 1000.0]], dtype=np.float64)
noseEndPoint2D, jacobian = cv2.projectPoints(
noseEndPoints3D, rotationVector, translationVector,
cameraMatrix, mdists)
# draw nose line
p1 = (int(refImgPts[0, 0]), int(refImgPts[0, 1]))
p2 = (int(noseEndPoint2D[0, 0, 0]), int(noseEndPoint2D[0, 0, 1]))
cv2.line(image,
p1,
p2, (110, 220, 0),
thickness=2,
lineType=cv2.LINE_AA)
# calculating euler angles
rmat, jac = cv2.Rodrigues(rotationVector)
angles, mtxR, mtxQ, Qx, Qy, Qz = cv2.RQDecomp3x3(rmat)
print('*' * 80)
# print(f"Qx:{Qx}\tQy:{Qy}\tQz:{Qz}\t")
x = np.arctan2(Qx[2][1], Qx[2][2])
y = np.arctan2(
-Qy[2][0],
np.sqrt((Qy[2][1] * Qy[2][1]) + (Qy[2][2] * Qy[2][2])))
z = np.arctan2(Qz[0][0], Qz[1][0])
# print("ThetaX: ", x)
print("ThetaY: ", y)
# print("ThetaZ: ", z)
print('*' * 80)
if angles[1] < -15:
GAZE = "Looking: Left"
elif angles[1] > 15:
GAZE = "Looking: Right"
else:
GAZE = "Forward"
cv2.putText(image, GAZE, (20, 20), cv2.FONT_HERSHEY_SIMPLEX, 1,
(0, 255, 80), 2)
cv2.imshow("Head Pose", image)
key = cv2.waitKey(10) & 0xFF
if key == 27:
break
cap.release()
cv2.destroyAllWindows()
def main():
# ###------------------------------------------------------------------
# ###-------------------------section starts here----------------------
# ### Quick detection of the chessboard pattern and saving pictures
#
# predetectChessboard(calib_path)
#
# ###-------------------------section ends here------------------------
# ###------------------------------------------------------------------
# ###------------------------------------------------------------------
# ###-------------------------section starts here----------------------
# ### Individual calibration of each camera
# ### variable cam must be changed, as well as the names of the sample images
# ### paramaters (scaling_factor, refine) should be adapted to camera
# ### - rgb cam 1920x1080 : scaling_factor=1, refine=True
# ### - depth cam 160x120 : scaling_factor=3-5, refine=False
# ### Correction of distortions is illustrated to ensure that results are ok
# ### Results are stored in an NPZ archive file for reuse
#
#
# cams = ['left','middle','right','e40','e70']
# scale = [1,1,1,4,3]
# refine = [True,True,True,False,False]
# threshold_average = [ 1.0, 1.0, 1.0, 0.25, 0.50]
# threshold_individual = [ 1.0, 1.0, 1.0, 0.25, 0.50]
#
# for k in range(4,5):
#
# cam = cams[k]
# archive = os.path.join(calib_path,'calib_'+cam+'.npz')
#
# # listing calibration images concerning selected camera
# calibimages = getCalibrationImages(calib_path,cam)
# # individual calibration from the calibration images
# cameraMatrix, distCoeffs, imgpoints, calibrationimages = calibrateMonoCamera(calibimages,
# reprojection_error_threshold_average = threshold_average[k],
# reprojection_error_threshold_individual = threshold_individual[k],
# scaling_factor = scale[k],
# refine_corners = refine[k],
# visualize = False)
# print(cameraMatrix)
# print(distCoeffs)
#
# # undistort 2 test images
# img1 = readCamerasFrames_20140523_MVcapture(3500,4,cam)
# img1_undistort = undistortImage(img1, cameraMatrix, distCoeffs, crop=False)
# show( img1, cam+' distorted1', height=600)
# show( img1_undistort, cam+' undistorted1', height=600)
#
# img2 = readCamerasFrames_20140523_MVcapture(975,5,cam)
# img2_undistort = undistortImage(img2, cameraMatrix, distCoeffs, crop=False)
# show( img2, cam+' distorted2', height=600)
# show( img2_undistort, cam+' undistorted2', height=600)
#
# # saving results
# np.savez(archive, K=cameraMatrix, k=distCoeffs, imgpoints=imgpoints, calibrationimages=calibrationimages)
#
# # display
# cv2.waitKey(0)
#
# ###-------------------------section ends here------------------------
# ###------------------------------------------------------------------
###------------------------------------------------------------------
###-------------------------section starts here----------------------
### Stereo calibration
cam1 = 'e40'
cam2 = 'e70'
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = calibrateStereoCamera(
cam1, cam2, calib_path)
print(cameraMatrix1)
print(cameraMatrix2)
print('Rotation matrix')
print(R)
print('Translation vector')
print(T)
# Euler angles
ret, _, _, _, _, _ = cv2.RQDecomp3x3(R)
print('Rotation angles:', ret)
img1 = readCamerasFrames_20140523_MVcapture(975, 5, cam1)
img2 = readCamerasFrames_20140523_MVcapture(975, 5, cam2)
show(img1, cam1 + ' original', height=500, position=(0, 0))
show(img2,
cam2 + ' original',
height=500,
position=(int(500 * img1.shape[1] / img1.shape[0]), 0))
(w1, h1) = img1.shape[1::-1]
(w2, h2) = img2.shape[1::-1]
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(
cameraMatrix1,
distCoeffs1,
cameraMatrix2,
distCoeffs2, (w1, h1),
R,
T,
alpha=1,
flags=cv2.CALIB_ZERO_DISPARITY)
mapx, mapy = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1,
P1, img1.shape[1::-1],
cv2.CV_32FC1)
img1_undist = cv2.remap(img1, mapx, mapy, cv2.INTER_LINEAR)
mapx, mapy = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2,
P2, img2.shape[1::-1],
cv2.CV_32FC1)
img2_undist = cv2.remap(img2, mapx, mapy, cv2.INTER_LINEAR)
show(img1_undist, cam1 + ' rectified', height=500, position=(0, 550))
show(img2_undist,
cam2 + ' rectified',
height=500,
position=(int(500 * img1_undist.shape[1] / img1_undist.shape[0]),
550))
cv2.waitKey(0)
cv2.destroyAllWindows()
def pose_estimator(self, frame):
# check to see if we have reached the end of the
# video
# convert the frame to grayscale, blur it, and detect edges
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
blurred = cv2.GaussianBlur(gray, (7, 7), 0)
edged = cv2.Canny(blurred, 50, 150)
# find contours in the edge map
# print cv2.findContours(edged.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
(_, cnts, _) = cv2.findContours(edged.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
Area = 0
# loop over the contours
for c in cnts:
# approximate the contour
peri = cv2.arcLength(c, True)
approx = cv2.approxPolyDP(c, 0.01 * peri, True)
# ensure that the approximated contour is "roughly" rectangular
if len(approx) >= 4 and len(approx) <= 6:
# compute the bounding box of the approximated contour and
# use the bounding box to compute the aspect ratio
(x1, y1, w, h) = cv2.boundingRect(approx)
# print x1
# print y1
aspectRatio = w / float(h)
# compute the solidity of the original contour
Area = []
area = cv2.contourArea(c)
Area = max(area, Area)
# print Area, "area"
hullArea = cv2.contourArea(cv2.convexHull(c))
solidity = area / float(hullArea)
# compute whether or not the width and height, solidity, and
# aspect ratio of the contour falls within appropriate bounds
keepDims = w > 25 and h > 25
keepSolidity = solidity > 0.9
keepAspectRatio = aspectRatio >= 0.8 and aspectRatio <= 1.2
# ensure that the contour passes all our tests
if keepDims and keepSolidity and keepAspectRatio:
# draw an outline around the target and update the status
# text
cv2.drawContours(frame, [approx], -1, (0, 0, 255), 4)
status = "Target(s) Acquired"
# This will give you the Pixel location of the rectangular box
rc = cv2.minAreaRect(approx[:])
box = cv2.boxPoints(rc)
pt = []
for p in box:
val = (p[0], p[1])
# print val
pt.append(val)
# print pt,'pt'
# cv2.circle(frame, val, 5, (200, 0, 0), 2)
# print 'came to 2'
M = cv2.moments(approx)
(cX, cY) = (int(M["m10"] / M["m00"]),
int(M["m01"] / M["m00"]))
(startX, endX) = (int(cX - (w * 0.15)),
int(cX + (w * 0.15)))
(startY, endY) = (int(cY - (h * 0.15)),
int(cY + (h * 0.15)))
cv2.line(frame, (startX, cY), (endX, cY), (0, 255, 0), 3)
cv2.line(frame, (cX, startY), (cX, endY), (0, 255, 0), 3)
# print cX, "cxxx"
# print cY, "cyyyy"
x_value = cX
print x_value, 'cX'
# print x_value, "xfierst"
# perspective
rows, cols, ch = frame.shape
pts1 = np.float32([pt[1], pt[0], pt[2], pt[3]])
# print pts1
pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])
M = None
# try:
M = cv2.getPerspectiveTransform(pts1, pts2)
# except:
# print "Perspective failed"
# print M
ret, mtxr, mtxq, qx, qy, qz = cv2.RQDecomp3x3(M)
# print qx
C = M[2, 1] * M[2, 2]
D = M[1, 0] * M[0, 0]
Theta_xp = math.degrees(math.atan(C) * 2)
# Theta_y = math.degrees(math.atan(C)*2)
Theta_zp = math.degrees(math.atan(D) * 2)
# print Theta_zp,'z'
# print Theta_xp,'x'
dst = cv2.warpPerspective(frame, M, (640, 480))
# 2D image points. If you change the image, you need to change vector
image_points = np.array([pt], dtype="double")
# print image_points
size = frame.shape
# 3D model points.
model_points = np.array([
(0.0, 0.0, 0.0), # Rectangle center
(0.0, 13.6, 0.0), #
(13.6, 13.6, 0.0), #
(13.6, 0.0, 0.0), #
])
# Camera intrinsic parameters
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 {0}".format(camera_matrix)
criteria = (cv2.TERM_CRITERIA_EPS +
cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
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, criteria)
rotationMatrix = np.zeros((3, 3), np.float32)
# print "Rotation Vector:\n {0}".format(rotation_vector)
# print "Translation Vector:\n {0}".format(translation_vector)
# Rodrigues to convert it into a Rotation vector
dst, jacobian = cv2.Rodrigues(rotation_vector)
# print "Rotation matrix:\n {0}".format(dst)
# sy = math.sqrt(dst[0, 0] * dst[0, 0] + dst[1, 0] * [1, 0])
A = dst[2, 1] * dst[2, 2]
B = dst[1, 0] * dst[0, 0]
# C = -dst[2,0]*sy
# Eular angles
Theta_x = math.degrees(math.atan(A) * 2)
# Theta_y = math.degrees(math.atan(C)*2)
Theta_z = math.degrees(math.atan(B) * 2)
(img_pts, jacobian) = cv2.projectPoints(
np.array([(0.0, 0.0, 1000.0)]), rotation_vector,
translation_vector, camera_matrix, dist_coeffs)
print "I am here"
self.testPID(frame, 1.0, 1.0, 0)
cv2.putText(frame, status, (20, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(0, 255, 0), 2)
newGray = aruco.drawDetectedMarkers(newGray, corners)
rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers(
corners, 0.18, cameraMatrix['mtx'], cameraMatrix['dist'],
np.float32(cameraMatrix['rvecs']),
np.float32(cameraMatrix['tvecs']))
positions = tvecs.tolist()
blah = list()
for i in range(len(rvecs)):
rotM = np.zeros(shape=(3, 3))
cv2.Rodrigues(rvecs[i], rotM, jacobian=0)
ypr = cv2.RQDecomp3x3(rotM)
ypr = list(ypr[0])
positions[i][0].extend(
[ypr[0], ypr[1], ypr[2], ids[i].tolist()[0]])
newGray = aruco.drawAxis(newGray, cameraMatrix['mtx'],
cameraMatrix['dist'], rvecs[i], tvecs[i],
0.1)
gray = newGray
print(positions[i])
blah.append([positions[i]])
clientID.send_message("/", positions)
def main(source=0):
detector = dlib.get_frontal_face_detector()
predictor = dlib.shape_predictor(PREDICTOR_PATH)
cap = cv2.VideoCapture(source)
while True:
GAZE = "Face Not Found"
ret, img = cap.read()
if not ret:
print(f"[ERROR - System]Cannot read from source: {source}")
break
faces = detector(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), 0)
face3Dmodel = world.ref3DModel()
for face in faces:
shape = predictor(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), face)
draw(img, shape)
refImgPts = world.ref2dImagePoints(shape)
height, width, channels = img.shape
focalLength = args.focal * width
cameraMatrix = world.cameraMatrix(focalLength, (height / 2, width / 2))
mdists = np.zeros((4, 1), dtype=np.float64)
# calculate rotation and translation vector using solvePnP
success, rotationVector, translationVector = cv2.solvePnP(
face3Dmodel, refImgPts, cameraMatrix, mdists)
noseEndPoints3D = np.array([[0, 0, 1000.0]], dtype=np.float64)
noseEndPoint2D, jacobian = cv2.projectPoints(
noseEndPoints3D, rotationVector, translationVector, cameraMatrix, mdists)
# draw nose line
p1 = (int(refImgPts[0, 0]), int(refImgPts[0, 1]))
p2 = (int(noseEndPoint2D[0, 0, 0]), int(noseEndPoint2D[0, 0, 1]))
cv2.line(img, p1, p2, (110, 220, 0),
thickness=2, lineType=cv2.LINE_AA)
# calculating euler angles
rmat, jac = cv2.Rodrigues(rotationVector)
angles, mtxR, mtxQ, Qx, Qy, Qz = cv2.RQDecomp3x3(rmat)
if angles[1] < -15:
GAZE = "Looking: Left"
elif angles[1] > 15:
GAZE = "Looking: Right"
else:
GAZE = "Forward"
cv2.putText(img, GAZE, (20, 20), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 80), 2)
cv2.imshow("Head Pose", img)
key = cv2.waitKey(10) & 0xFF
if key == 27:
break
cap.release()
cv2.destroyAllWindows()
#############################
# Intrinsic parameters of virtual camera
K_virtual = np.array([[1732.87, 0.0, 943.23], [0.0, 1729.90, 548.845040],
[0, 0, 1]])
# Extrinsic parameters of virtual camera
R0 = np.eye(3)
T0 = np.zeros((3, 1))
R = np.eye(3)
T = np.zeros((3, 1))
T[0, 0] = 5
R = eulerAnglesToRotationMatrix([0, 1.5, 0])
angles, _, _, _, _, _ = cv2.RQDecomp3x3(R)
print(angles)
print(R)
scale = .5
D = cv2.imread(u"..//..//Task//D_original.png")
D = D[:, :, 0]
V = cv2.imread(u"..//..//Task//V_original.png")
D = cv2.resize(D, None, fx=scale, fy=scale, interpolation=cv2.INTER_LINEAR)
V = cv2.resize(V, None, fx=scale, fy=scale, interpolation=cv2.INTER_LINEAR)
for (i, j) in [(0, 0), (0, 2), (1, 1), (1, 2)]:
K_original[i, j] *= scale
# Display the xyz position coordinates
position = "Marker %d position: x=%4.0f y=%4.0f z=%4.0f" % (
marker_id, tvec[0], tvec[1], tvec[2])
cv2.putText(frame, position, (20, 650), font, 1, (255, 255, 255),
1, cv2.LINE_AA)
# Create empty rotation matrix
rotation_matrix = np.zeros(shape=(3, 3))
# Convert rotation vector to rotation matrix
cv2.Rodrigues(rvec, rotation_matrix, jacobian=0)
# Get yaw/pitch/roll of rotation matrix
# We are most concerned with rotation around pitch axis which translates to Tello's yaw
ypr = cv2.RQDecomp3x3(rotation_matrix)
# Display the yaw/pitch/roll angles
attitude2 = "Marker %d attitude: y=%4.0f p=%4.0f r=%4.0f" % (
marker_id, ypr[0][0], ypr[0][1], ypr[0][2])
cv2.putText(frame, attitude2, (20, 700), font, 1, (255, 255, 255),
1, cv2.LINE_AA)
else:
img_aruco = frame
cv2.imshow("Tello", img_aruco)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
cameraMatrix = world.cameraMatrix(focalLength, (height / 2, width / 2))
mdists = np.zeros((4, 1), dtype=np.float64)
# calculate rotation and translation vector using solvePnP
success, rotationVector, translationVector = cv2.solvePnP(face3Dmodel, refImgPts, cameraMatrix, mdists)
noseEndPoints3D = np.array([[0, 0, 1000.0]], dtype=np.float64)
noseEndPoint2D, jacobian = cv2.projectPoints(noseEndPoints3D, rotationVector, translationVector, cameraMatrix, mdists)
# draw nose line
p1 = (int(refImgPts[0, 0]), int(refImgPts[0, 1]))
p2 = (int(noseEndPoint2D[0, 0, 0]), int(noseEndPoint2D[0, 0, 1]))
cv2.line(frame, p1, p2, (110, 220, 0),thickness=2, lineType=cv2.LINE_AA)
# calculating euler angles
rmat, jac = cv2.Rodrigues(rotationVector)
angles, mtxR, mtxQ, Qx, Qy, Qz = cv2.RQDecomp3x3(rmat)
Thetay = np.arctan2(-Qy[2][0], np.sqrt((Qy[2][1] * Qy[2][1] ) + (Qy[2][2] * Qy[2][2])))
return ThetaY
def getFrame(sec):
start = 180000
vidcap.set(cv2.CAP_PROP_POS_MSEC, start + sec*1000)
hasFrames,image = vidcap.read()
return hasFrames, image
data = []
angles = []
for j in [60]:
def main():
calib_path = u'.//CalibAuto'
cam1 = {'name': 'left', 'id': 2}
cam2 = {'name': 'right', 'id': 0}
cams = [cam1, cam2]
### Automatic detection of chessboards
predetectChessboardAuto(cams, calib_path)
### Individual calibration of each camera
for cam in cams:
archive = os.path.join(calib_path, 'calib_' + cam['name'] + '.npz')
# listing calibration images concerning selected camera
calibimages = getCalibrationImages(calib_path, cam['name'])
# individual calibration from the calibration images
cameraMatrix, distCoeffs, imgpoints, calibrationimages = calibrateMonoCamera(
calibimages, visualize=True)
print(cameraMatrix)
print(distCoeffs)
# saving results
np.savez(archive,
K=cameraMatrix,
k=distCoeffs,
imgpoints=imgpoints,
calibrationimages=calibrationimages)
# check distortion correction
vid = cv2.VideoCapture(cam['id'])
while (1):
ret, img = vid.read()
img_corrected = undistortImage(img,
cameraMatrix,
distCoeffs,
crop=False)
img_disp = np.concatenate((img, img_corrected), 1)
show(
img_disp,
'Checking the correction of distortion. Left: Original, Right: Corrected',
stats=False)
key = cv2.waitKey(10)
if key == 27: # if ESC is pressed, abort
break
cv2.destroyAllWindows()
### Stereo calibration
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = calibrateStereoCamera(
cam1['name'], cam2['name'], calib_path=calib_path)
print('Rotation matrix')
print(R)
print('Translation vector')
print(T)
# Euler angles
ret, _, _, _, _, _ = cv2.RQDecomp3x3(R)
print('Rotation angles:', ret)
# perspective
rows, cols, ch = frame.shape
pts1 = np.float32([pt[1], pt[0], pt[2], pt[3]])
# print pts1
pts2 = np.float32([[0, 0], [640, 0], [0, 480], [640, 480]])
M = None
# try:
M = cv2.getPerspectiveTransform(pts1, pts2)
# except:
# print "Perspective failed"
# print M
ret, mtxr, mtxq, qx, qy, qz = cv2.RQDecomp3x3(M)
# print qx
C = M[2, 1] * M[2, 2]
D = M[1, 0] * M[0, 0]
Theta_xp = math.degrees(math.atan(C) * 2)
# Theta_y = math.degrees(math.atan(C)*2)
Theta_zp = math.degrees(math.atan(D) * 2)
# print Theta_zp,'z'
# print Theta_xp,'x'
dst = cv2.warpPerspective(frame, M, (640, 480))
# 2D image points. If you change the image, you need to change vector
image_points = np.array([pt], dtype="double")
# print image_points
size = frame.shape
# 3D model points.
def analyze_post():
request_data = request.get_json()
# Validación parámetro id
if not 'id' in request_data:
return jsonify(message='El parámetro id es requerido'), 400
elif not isinstance(
request_data['id'],
int) or request_data['id'] <= 0 or request_data['id'] is None:
return jsonify(message='El parámetro id debe un entero mayor a 0'), 400
else:
affiliate_path = os.path.join(path, str(request_data['id']))
if not os.path.exists(affiliate_path):
os.mkdir(affiliate_path)
# Validación parámetro is_base64
if not 'is_base64' in request_data:
return jsonify(message='El parámetro is_base64 es requerido'), 400
elif not isinstance(request_data['is_base64'], str) and not isinstance(
request_data['is_base64'], bool) and not isinstance(
request_data['is_base64'],
int) or request_data['is_base64'] is None:
return jsonify(message='El parámetro is_base64 debe ser booleano'), 400
else:
if isinstance(request_data['is_base64'], int):
if request_data['is_base64'] not in (1, 0):
return jsonify(
message='El parámetro is_base64 debe ser booleano'), 400
else:
is_base64 = bool(request_data['is_base64'])
elif isinstance(request_data['is_base64'], bool):
is_base64 = request_data['is_base64']
elif isinstance(request_data['is_base64'], str):
is_base64 = utils.str2bool(request_data['is_base64'])
# Validación parámetro image
if not 'image' in request_data:
return jsonify(message='El parámetro image es requerido'), 400
else:
if is_base64:
if not isinstance(request_data['image'], str) or len(
request_data['image']
) < 100 or request_data['image'] is None:
return jsonify(
message=
'La cadena de texto en base64 image debe tener al menos 100 caracteres'
), 400
else:
image_path = os.path.join(affiliate_path,
str(uuid.uuid4().hex) + '.jpg')
else:
image_path = os.path.join(affiliate_path, request_data['image'])
if not os.path.exists(image_path):
return jsonify(message='Archivo inexistente: ' +
image_path), 400
# Validación parámetro gaze
if not 'gaze' in request_data:
get_gaze = True
else:
if not isinstance(request_data['gaze'], bool):
get_gaze = True
else:
get_gaze = utils.str2bool(str(request_data['gaze']))
# Validación parámetro emotion
if not 'gaze' in request_data:
get_emotion = True
else:
if not isinstance(request_data['emotion'], bool):
get_emotion = True
else:
get_emotion = utils.str2bool(str(request_data['emotion']))
try:
if is_base64:
contains_base64 = request_data['image'].find('base64,')
if contains_base64 != -1:
request_data['image'] = request_data['image'][contains_base64 +
7:]
img = Image.open(BytesIO(base64.b64decode(request_data['image'])))
img.save(image_path)
img = cv2.cvtColor(np.array(img), cv2.COLOR_BGR2RGB)
else:
img = cv2.imread(image_path, 1)
faces = detector(img, 0)
# faces = face_recognition.face_locations(image)
if len(faces) == 1:
if get_gaze:
face = faces[0]
shape = predictor(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), face)
refImgPts = utils.ref2dImagePoints(shape)
height, width, channel = img.shape
focalLength = 1 * width
cameraMatrix = utils.cameraMatrix(focalLength,
(height / 2, width / 2))
# Cálculo de vector de rotación y traslación mediante solvePnP
success, rotationVector, translationVector = cv2.solvePnP(
face3Dmodel, refImgPts, cameraMatrix, mdists)
# Cálculo del ángulo del rostro
rmat, jac = cv2.Rodrigues(rotationVector)
angles, mtxR, mtxQ, Qx, Qy, Qz = cv2.RQDecomp3x3(rmat)
if angles[1] < (-1 * gaze_angle):
gaze = 'left'
elif angles[1] > gaze_angle:
gaze = 'right'
else:
gaze = 'forward'
else:
gaze = 'undefined'
# DeepFace analysis
if get_emotion:
df_analysis = DeepFace.analyze(
image_path,
models=model_actions,
actions=selected_actions,
detector_backend=os.environ.get('DF_ANALYZE_BACKEND'))
else:
df_analysis = None
return jsonify({
'message': 'Imagen analizada',
'data': {
'file': image_path,
'gaze': gaze,
'analysis': df_analysis
}
})
except:
return jsonify({
'message': 'Imagen inválida',
}), 400
def estimate_depth(self, face_info):
lms = np.concatenate((face_info.lms, np.array([[face_info.eye_state[0][1], face_info.eye_state[0][2], face_info.eye_state[0][3]], [face_info.eye_state[1][1], face_info.eye_state[1][2], face_info.eye_state[1][3]]], np.float)), 0)
image_pts = np.array(lms)[face_info.contour_pts, 0:2]
success = False
if not face_info.rotation is None:
success, face_info.rotation, face_info.translation = cv2.solvePnP(face_info.contour, image_pts, self.camera, self.dist_coeffs, useExtrinsicGuess=True, rvec=np.transpose(face_info.rotation), tvec=np.transpose(face_info.translation), flags=cv2.SOLVEPNP_ITERATIVE)
else:
rvec = np.array([0, 0, 0], np.float32)
tvec = np.array([0, 0, 0], np.float32)
success, face_info.rotation, face_info.translation = cv2.solvePnP(face_info.contour, image_pts, self.camera, self.dist_coeffs, useExtrinsicGuess=True, rvec=rvec, tvec=tvec, flags=cv2.SOLVEPNP_ITERATIVE)
rotation = face_info.rotation
translation = face_info.translation
pts_3d = np.zeros((70,3), np.float32)
if not success:
face_info.rotation = np.array([0.0, 0.0, 0.0], np.float32)
face_info.translation = np.array([0.0, 0.0, 0.0], np.float32)
return False, np.zeros(4), np.zeros(3), 99999., pts_3d, lms
else:
face_info.rotation = np.transpose(face_info.rotation)
face_info.translation = np.transpose(face_info.translation)
rmat, _ = cv2.Rodrigues(rotation)
inverse_rotation = np.linalg.inv(rmat)
pnp_error = 0.0
for i, pt in enumerate(face_info.face_3d):
if i == 68:
# Right eyeball
# Eyeballs have an average diameter of 12.5mm and and the distance between eye corners is 30-35mm, so a conversion factor of 0.385 can be applied
eye_center = (pts_3d[36] + pts_3d[39]) / 2.0
d_corner = np.linalg.norm(pts_3d[36] - pts_3d[39])
depth = 0.385 * d_corner
pt_3d = np.array([eye_center[0], eye_center[1], eye_center[2] - depth])
pts_3d[i] = pt_3d
continue
if i == 69:
# Left eyeball
eye_center = (pts_3d[42] + pts_3d[45]) / 2.0
d_corner = np.linalg.norm(pts_3d[42] - pts_3d[45])
depth = 0.385 * d_corner
pt_3d = np.array([eye_center[0], eye_center[1], eye_center[2] - depth])
pts_3d[i] = pt_3d
continue
if i == 66:
d1 = np.linalg.norm(lms[i,0:2] - lms[36,0:2])
d2 = np.linalg.norm(lms[i,0:2] - lms[39,0:2])
d = d1 + d2
pt = (pts_3d[36] * d1 + pts_3d[39] * d2) / d
if i == 67:
d1 = np.linalg.norm(lms[i,0:2] - lms[42,0:2])
d2 = np.linalg.norm(lms[i,0:2] - lms[45,0:2])
d = d1 + d2
pt = (pts_3d[42] * d1 + pts_3d[45] * d2) / d
reference = rmat.dot(pt)
reference = reference + face_info.translation
reference = self.camera.dot(reference)
depth = reference[2]
if i < 17 or i == 30:
reference = reference / depth
e1 = lms[i][0] - reference[0]
e2 = lms[i][1] - reference[1]
pnp_error += e1*e1 + e2*e2
pt_3d = np.array([lms[i][0] * depth, lms[i][1] * depth, depth], np.float32)
pt_3d = self.inverse_camera.dot(pt_3d)
pt_3d = pt_3d - face_info.translation
pt_3d = inverse_rotation.dot(pt_3d)
pts_3d[i,:] = pt_3d[:]
pnp_error = np.sqrt(pnp_error / (2.0 * image_pts.shape[0]))
if pnp_error > 300:
face_info.fail_count += 1
if face_info.fail_count > 5:
# Something went wrong with adjusting the 3D model
if not self.silent:
print(f"Detected anomaly when 3D fitting face {face_info.id}. Resetting.")
face_info.face_3d = copy.copy(self.face_3d)
face_info.rotation = None
face_info.translation = np.array([0.0, 0.0, 0.0], np.float32)
face_info.update_counts = np.zeros((66,2))
face_info.update_contour()
else:
face_info.fail_count = 0
euler = cv2.RQDecomp3x3(rmat)[0]
return True, matrix_to_quaternion(rmat), euler, pnp_error, pts_3d, lms
def set_goal(self, matrix, distortion):
if self.set:
if self.goal.visible:
index = np.where(self.ids == 0)[0][0]
self.goal.corners = [np.array(self.corners[index], np.float32)]
self.goal.rvec, self.goal.tvec, _ = aruco.estimatePoseSingleMarkers(self.goal.corners, self.goal.length,
matrix, distortion)
self.goal.rvec = self.goal.rvec.reshape(3, 1)
self.goal.tvec = self.goal.tvec.reshape(3, 1)
image_points_marker, jacobian = cv2.projectPoints(np.array([np.zeros((3,1))]), self.goal.rvec, self.goal.tvec, matrix, None)
image_points_marker = image_points_marker.astype(int)
image_point = np.array([[image_points_marker[0][0][0]], [image_points_marker[0][0][1]], [1]])
Z_CONST = 0.000
# calculate projection of center of goal marker on board plane
temp_mat1 = np.matmul(self.inv_rot_mtx, np.matmul(self.inv_mtx, image_point))
temp_mat2 = np.matmul(self.inv_rot_mtx, self.tvec)
s = Z_CONST + temp_mat2[2]
s /= temp_mat1[2]
board_point = np.matmul(self.inv_rot_mtx, (s * np.matmul(self.inv_mtx, image_point) - self.tvec))
# set z to Z_CONST again
board_point[2] = Z_CONST
# calculate intersection angle of line tvecCamera to boardPoint and board plane, calculate goal marker point on board plane
board_normal = np.array([0, 0, 1])
line_camera_point = board_point - self.tvec_camera
alpha = np.arcsin(np.abs(np.matmul(board_normal, line_camera_point))
/ (np.linalg.norm(line_camera_point) * np.linalg.norm(board_normal)))
# if clause to prevent tan(90) error
if alpha > np.radians(89):
delta_board_point = 0
else:
delta_board_point = (self.goal.heigth) / np.tan(alpha)
line_camera_point_project_board = line_camera_point
line_camera_point_project_board[2] = 0
line_camera_point_project_board_unit = line_camera_point_project_board / np.linalg.norm(
line_camera_point_project_board)
self.goal.position = board_point - delta_board_point * line_camera_point_project_board_unit
# calculate rotation of goal in board coordinates
self.goal.rot_mtx, jacobian = cv2.Rodrigues(self.goal.rvec)
_, _, _, _, _, self.goal.rotation = cv2.RQDecomp3x3(np.matmul(self.goal.rot_mtx, np.transpose(self.rot_mtx)))
rot_direction = np.matmul(self.goal.rotation, np.array([[1], [0], [0]]))
self.goal.rotation_z = np.arctan2(rot_direction.item(1,0), rot_direction.item(0,0))
self.goal.goalline_position = self.goal.position - self.goal.marker_position.item(0) * rot_direction
arrow_start = self.goal.goalline_position + self.goal.length * rot_direction
object_points = np.array([self.goal.goalline_position, arrow_start])
image_points_arrow, jacobian = cv2.projectPoints(object_points, self.rvec, self.tvec, matrix, distortion)
image_points_arrow = image_points_arrow.astype(int)
self.goal.end = np.array([image_points_arrow[0][0][0], image_points_arrow[0][0][1]])
self.goal.start = np.array([image_points_arrow[1][0][0], image_points_arrow[1][0][1]])
# find valid goal and scoring polygon
goal_x = self.goal.goalline_position - self.goal.position
goal_x = goal_x / np.linalg.norm(goal_x)
goal_y = np.array([[- goal_x[1][0]], [goal_x[0][0]], [0]])
goalline_point = self.goal.goalline_position
goalline_point.itemset(2, self.ball.radius)
goal_area_fr_world = goalline_point + self.goal.goal_area_fr[0] * goal_x + self.goal.goal_area_fr[1] * goal_y
goal_area_fl_world = goalline_point + self.goal.goal_area_fl[0] * goal_x + self.goal.goal_area_fl[1] * goal_y
goal_area_br_world = goalline_point + self.goal.goal_area_br[0] * goal_x + self.goal.goal_area_br[1] * goal_y
goal_area_bl_world = goalline_point + self.goal.goal_area_bl[0] * goal_x + self.goal.goal_area_bl[1] * goal_y
scoring_area_fr_world = goalline_point + self.goal.goal_area_fr[0] * goal_x + self.goal.goal_area_fr[1] * goal_y
scoring_area_fl_world = goalline_point + self.goal.goal_area_fl[0] * goal_x + self.goal.goal_area_fl[1] * goal_y
scoring_area_br_world = goalline_point + self.goal.goal_area_br[0] * goal_x + self.goal.goal_area_br[1] * goal_y
scoring_area_bl_world = goalline_point + self.goal.goal_area_bl[0] * goal_x + self.goal.goal_area_bl[1] * goal_y
goal_area = np.array([goal_area_fr_world, goal_area_fl_world, goal_area_bl_world, goal_area_br_world])
scoring_area = np.array([scoring_area_fr_world, scoring_area_fl_world, scoring_area_bl_world, scoring_area_br_world])
self.goal.goal_area_points, jacobian = cv2.projectPoints(goal_area, self.rvec, self.tvec, matrix,
distortion)
self.goal.scoring_area_points, jacobian = cv2.projectPoints(scoring_area, self.rvec, self.tvec, matrix,
distortion)
self.goal.goal_area_points = self.goal.goal_area_points.astype(int)
self.goal.scoring_area_points = self.goal.scoring_area_points.astype(int)
self.goal.set = True
else:
self.goal.set = False
print('Goal Not Visible')
else:
print('Field Not Set')
def main(image):
detector = dlib.get_frontal_face_detector()
predictor = dlib.shape_predictor(PREDICTOR_PATH)
while True:
im = cv2.imread(image)
faces = detector(cv2.cvtColor(im, cv2.COLOR_BGR2RGB), 0)
face3Dmodel = world.ref3DModel()
for face in faces:
shape = predictor(cv2.cvtColor(im, cv2.COLOR_BGR2RGB), face)
draw(im, shape)
refImgPts = world.ref2dImagePoints(shape)
height, width, channel = im.shape
focalLength = args.focal * width
cameraMatrix = world.cameraMatrix(focalLength, (height / 2, width / 2))
mdists = np.zeros((4, 1), dtype=np.float64)
# calculate rotation and translation vector using solvePnP
success, rotationVector, translationVector = cv2.solvePnP(
face3Dmodel, refImgPts, cameraMatrix, mdists)
noseEndPoints3D = np.array([[0, 0, 1000.0]], dtype=np.float64)
noseEndPoint2D, jacobian = cv2.projectPoints(
noseEndPoints3D, rotationVector, translationVector, cameraMatrix, mdists)
# draw nose line
p1 = (int(refImgPts[0, 0]), int(refImgPts[0, 1]))
p2 = (int(noseEndPoint2D[0, 0, 0]), int(noseEndPoint2D[0, 0, 1]))
cv2.line(im, p1, p2, (110, 220, 0),
thickness=2, lineType=cv2.LINE_AA)
# calculating angle
rmat, jac = cv2.Rodrigues(rotationVector)
angles, mtxR, mtxQ, Qx, Qy, Qz = cv2.RQDecomp3x3(rmat)
print('*' * 80)
print("Angle: ", angles)
# print(f"Qx:{Qx}\tQy:{Qy}\tQz:{Qz}\t")
x = np.arctan2(Qx[2][1], Qx[2][2])
y = np.arctan2(-Qy[2][0], np.sqrt((Qy[2][1] * Qy[2][1] ) + (Qy[2][2] * Qy[2][2])))
z = np.arctan2(Qz[0][0], Qz[1][0])
print("AxisX: ", x)
print("AxisY: ", y)
print("AxisZ: ", z)
print('*' * 80)
gaze = "Looking: "
if angles[1] < -15:
gaze += "Left"
elif angles[1] > 15:
gaze += "Right"
else:
gaze += "Forward"
cv2.putText(im, gaze, (20, 20), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 80), 2)
cv2.imshow("Head Pose", im)
key = cv2.waitKey(10) & 0xFF
if key == 27:
cv2.imwrite(f"joye-{gaze}.jpg", im)
break
cv2.destroyAllWindows()
def estimate_pose(self):
"""
Estimate the pose of each image using the corners from the aruco vision object
"""
estimated_poses = pd.DataFrame()
# get the first set of corners
ic = self.vision_data.corners.iloc[0]
_, init_corners = self.vision_data.row_to_corner(ic)
init_rvec, init_tvec, _ = aruco.estimatePoseSingleMarkers(
[init_corners], self.vision_data.marker_side, self.mtx, self.dist)
init_pose = np.concatenate((init_rvec, init_tvec))
# orig_corners = orig_corners[0].squeeze()
total_successes = 0
final_i = 0
for i, next_corners in self.vision_data.yield_corners():
try:
# print(f"Estimating pose in image {i}")
next_rvec, next_tvec, _ = aruco.estimatePoseSingleMarkers(
[next_corners], self.vision_data.marker_side, self.mtx,
self.dist)
# next_corners = next_corners[0].squeeze()
#print(f"calculating angle, {next_corners}")
rel_angle = self.angle_between(
init_corners[0] - init_corners[2],
next_corners[0] - next_corners[2])
rel_rvec, rel_tvec = self.relative_position(
init_rvec, init_tvec, next_rvec, next_tvec)
translation_val = np.round(np.linalg.norm(rel_tvec), 4)
rotation_val = rel_angle * 180 / np.pi
# found the stack overflow for it?
# https://stackoverflow.com/questions/51270649/aruco-marker-world-coordinates
rotM = np.zeros(shape=(3, 3))
cv2.Rodrigues(rel_rvec, rotM, jacobian=0)
ypr = cv2.RQDecomp3x3(
rotM
) # TODO: not sure what we did with this earlier... need to check
total_successes += 1
except Exception as e:
print(f"Error with ARuco corners in image {i}.")
print(e)
rel_rvec, rel_tvec = (None, None, None), (None, None, None)
translation_val = None
rotation_val = None
rel_pose = np.concatenate((rel_rvec, rel_tvec))
rel_pose = rel_pose.squeeze()
rel_df = pd.Series({
"frame": i,
"roll": rel_pose[0],
"pitch": rel_pose[1],
"yaw": rel_pose[2],
"x": rel_pose[3],
"y": rel_pose[4],
"z": rel_pose[5],
"tmag": translation_val,
"rmag": rotation_val
})
estimated_poses = estimated_poses.append(rel_df, ignore_index=True)
final_i = i
# print(" ")
# print(f"Successfully analyzed: {total_successes} / {final_i+1} corners")
estimated_poses = estimated_poses.set_index("frame")
estimated_poses = estimated_poses.round(4)
return init_pose, estimated_poses
def estimateLocation(self, frame, draw_axis_to_frame=False):
# Initialize the output variables
out_position_two_markers_1x = None
out_position_two_markers_2x = None
out_position_11 = None
out_position_12 = None
out_position_21 = None
out_position_22 = None
absolute_angle_to_blue = None
absolute_angle_to_pink = None
relative_angle_to_blue = None
relative_angle_to_pink = None
# Convert the frame to grayscale so that ArUco detection would work
grayscale = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Detect the markers and try to do pose estimation
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
grayscale, self.ARUCO_DICT, parameters=self.ARUCO_PARAMETERS)
rvec, tvec, _objPoints = cv2.aruco.estimatePoseSingleMarkers(
corners, self.MARKER_SIZE, self.camera_matrix,
self.distortion_coefficients)
# If any markers were detected ...
if rvec is not None:
detected_markers_with_corrent_ids = []
for i in range(len(rvec)):
if ids[i] in self.marker_ids_to_detect:
detected_markers_with_corrent_ids.append(tvec)
if draw_axis_to_frame:
frame = cv2.aruco.drawAxis(
frame, self.camera_matrix,
self.distortion_coefficients, rvec[i], tvec[i],
0.1)
frame = cv2.aruco.drawDetectedMarkers(
frame, corners, ids)
# Read in some metadata about the detected marker
marker_id = int(ids[i])
markerdata = self.markerdata[marker_id]
marker_absolute_position = (markerdata['loc_x'],
markerdata['loc_y'])
# Calculate the marker's distance and angle from camera (position)
marker_distance_from_camera = numpy.sqrt(tvec[i][0][0]**2 +
tvec[i][0][2]**2)
marker_angle_from_camera = numpy.tan(tvec[i][0][0] /
tvec[i][0][2])
#print(marker_angle_from_camera*180/3.14159265358979323)
if markerdata['angle'] == 0:
# This is the blue basket
relative_angle_to_blue = marker_angle_from_camera
else:
# This is the pink basket
relative_angle_to_pink = marker_angle_from_camera
# Calculate the marker's attitude/orientation (rotation)
rotM = numpy.zeros(shape=(3, 3))
cv2.Rodrigues(rvec[i - 1], rotM, jacobian=0)
ypr = cv2.RQDecomp3x3(rotM)
angle_between_marker_normal_and_camera = numpy.arccos(
ypr[4][2][2])
sign_calculation = rvec[i][0][2]
if rvec[i][0][0] < 0:
sign_calculation = sign_calculation * -1
if sign_calculation < 0:
angle = -angle_between_marker_normal_and_camera
else:
angle = angle_between_marker_normal_and_camera
# Do some orientation measurements and saving first
angle_to_basket = markerdata[
'angle'] - angle # angle_between_marker_normal_and_camera
if markerdata['angle'] == 0:
# This is the blue basket
absolute_angle_to_blue = angle_to_basket
else:
# This is the pink basket
absolute_angle_to_pink = angle_to_basket
# print(angle_to_basket * 180 / 3.14159265358979323)
# Combine the markers location angle and orientation angle to get the real angle
angle += marker_angle_from_camera
angle += markerdata['angle']
# Knowing the angle and distance, calculate the location of the camera
robot_x = marker_absolute_position[
0] - marker_distance_from_camera * numpy.cos(angle)
robot_y = marker_absolute_position[
1] + marker_distance_from_camera * numpy.sin(angle)
if marker_id == 11:
out_position_11 = robot_x, robot_y
if marker_id == 12:
out_position_12 = robot_x, robot_y
if marker_id == 21:
out_position_21 = robot_x, robot_y
if marker_id == 22:
out_position_22 = robot_x, robot_y
if len(detected_markers_with_corrent_ids) == 2:
dist = [
numpy.sqrt(tvec[0][0][0]**2 + tvec[0][0][2]**2),
numpy.sqrt(tvec[1][0][0]**2 + tvec[1][0][2]**2)
]
print(dist)
if ids[0] == 11 or ids[0] == 12:
dist11 = -1
dist12 = -1
if ids[0] == 11 and ids[1] == 12:
dist11 = dist[0]
dist12 = dist[1]
elif ids[0] == 12 and ids[1] == 11:
dist12 = dist[0]
dist11 = dist[1]
if dist11 != -1 and dist12 != -1:
# dist between the markers is 0.46 meters
dist_between_markers = 0.46
# Apply the law of cosines
cos_b = (dist_between_markers**2 + dist12**2 - dist11**
2) / (2 * dist12 * dist_between_markers)
angle = numpy.arccos(cos_b)
out_x = self.markerdata[12]['loc_x'] + numpy.sin(
angle) * dist12
out_y = self.markerdata[12]['loc_y'] - numpy.cos(
angle) * dist12
out_position_two_markers_1x = out_x, out_y
if ids[0] == 21 or ids[0] == 22:
dist21 = -1
dist22 = -1
if ids[0] == 21 and ids[1] == 22:
dist21 = dist[0]
dist22 = dist[1]
elif ids[0] == 22 and ids[1] == 21:
dist22 = dist[0]
dist21 = dist[1]
if dist21 != -1 and dist22 != -1:
# dist between the markers is 0.46 meters
dist_between_markers = 0.46
# Apply the law of cosines
cos_b = (dist_between_markers**2 + dist22**2 - dist21**
2) / (2 * dist22 * dist_between_markers)
angle = numpy.arccos(cos_b)
out_x = self.markerdata[22]['loc_x'] + numpy.sin(
angle) * dist22
out_y = self.markerdata[22]['loc_y'] - numpy.cos(
angle) * dist22
out_position_two_markers_2x = out_x, out_y
elif len(detected_markers_with_corrent_ids) > 2:
print("Too many markers found!")
out_map = map.BasketballFieldMap(None, out_position_11,
out_position_12, out_position_21,
out_position_22,
out_position_two_markers_1x,
out_position_two_markers_2x)
out_map.absolute_angle_to_pink = absolute_angle_to_pink
out_map.absolute_angle_to_blue = absolute_angle_to_blue
out_map.relative_angle_to_pink = relative_angle_to_pink
out_map.relative_angle_to_blue = relative_angle_to_blue
if absolute_angle_to_pink is not None or absolute_angle_to_blue is not None:
if absolute_angle_to_pink is not None:
out_map.absolute_robot_angle = absolute_angle_to_pink
else:
out_map.absolute_robot_angle = absolute_angle_to_blue
#if out_position_two_markers_1x is not None:
# out_map.robot_position = out_position_two_markers_1x
#el
if out_position_11 is not None:
out_map.robot_position = out_position_11
elif out_position_12 is not None:
out_map.robot_position = out_position_12
elif out_position_21 is not None:
out_map.robot_position = out_position_21
elif out_position_22 is not None:
out_map.robot_position = out_position_22
else:
out_map.robot_position = (None, None)
self.previousMap = self.latestMap
self.latestMap = out_map
self.updateMapWithMotorData(0, 0, 0, 0)
return out_map
def extractaruco(img, image):
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
cv2.imshow('120', gray)
cv2.waitKey(0)
x4, y4, w1, h1 = cv2.boundingRect(gray)
print(w1)
print(h1)
'''ret, th = cv2.threshold(gray,127,255,cv2.THRESH_BINARY_INV)
_,contours, hierarchy = cv2.findContours(th,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
cv2.imshow('567',th)
cv2.waitKey(0)
contours = contours[0]
epsilon = 0.1 * cv2.arcLength(contours, True)
approx = cv2.approxPolyDP(contours, epsilon, True)
image_crop = gray[approx[0, 0, 1]:approx[2, 0, 1], approx[0, 0, 0]:approx[2, 0, 0]]
cv2.imshow('crop', image_crop)
cv2.waitKey(0)'''
gray = image
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_100)
parameters = aruco.DetectorParameters_create()
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
#gray = aruco.drawDetectedMarkers(gray, corners)
x = int(corners[0][0][0][0])
y = int(corners[0][0][0][1])
x1 = int(corners[0][0][1][0])
y1 = int(corners[0][0][1][1])
x2 = int(corners[0][0][2][0])
y2 = int(corners[0][0][2][1])
x3 = int(corners[0][0][3][0])
y3 = int(corners[0][0][3][1])
w = x1 - x
h = y2 - y1
cv2.putText(gray,
"%i" % ids, (int(x + w / 2 + 5), int(y + h / 2 + 5)),
cv2.FONT_HERSHEY_SIMPLEX,
0.4, (140, 140, 140),
thickness=1)
cv2.line(gray, (int(x + w / 2), int(y)), (int(x + w / 2), int(y + h / 2)),
(127, 127, 127),
thickness=1)
cv2.circle(gray, (int(x + w / 2), int(y + h / 2)),
2, (140, 140, 140),
thickness=4)
#aruco.drawAxis(gray,0.1)
#gray2 = cv2.cvtColor(gray,cv2.COLOR_GRAY2RGB)
x5 = int(x + w / 2)
y5 = int(y + h / 2)
a = int(math.sqrt(int(math.pow(w1 - x4, 2) + int(math.pow(h1 - h1, 2)))))
b = int(math.sqrt(int(math.pow(w1 - x5, 2) + int(math.pow(h1 - y5, 2)))))
c = int(math.sqrt(int(math.pow(x5 - x4, 2) + int(math.pow(y5 - h1, 2)))))
angle = math.acos(
int((math.pow(c, 2) + math.pow(b, 2) - math.pow(a, 2)) / (2 * b * c)))
angle1 = int(angle * 180 / math.pi)
cv2.putText(gray,
"%i" % angle1, (int(x + w / 2 - 27), int(y + 20)),
cv2.FONT_HERSHEY_SIMPLEX,
0.4, (140, 140, 140),
thickness=1)
print(angle1)
print(a)
print(b)
print(c)
print(angle)
print(ids)
cv2.imshow('789', gray)
cv2.waitKey(0)
#gray = cv2.cvtColor(image,cv2.COLOR_GRAY2RGB)
#x, y, w, h = cv2.boundingRect(image_crop)
#x1,y1,w1,h1 = cv2.boundingRect(gray)
#gray = image
#cv2.putText(gray,"%i"%ids,(int(x1+w1/2),int(y1+h1/2)), cv2.FONT_HERSHEY_SIMPLEX,0.4,(140, 140, 140),thickness=1)
#gray2 = cv2.line(gray,(int(x1+w1/2),int((h1-h)/2)),(int(x1+w1/2),int(y1+h1/2)),(127, 127, 127),thickness=1)
#gray2 = cv2.circle(gray2,(207,210),2,(140, 140, 140),thickness=4)
rotM = np.zeros(shape=(3, 3))
cv2.Rodrigues(corners[0][0], rotM, jacobian=0)
ypr = cv2.RQDecomp3x3(rotM)
#cv2.imshow('frame', gray)
#cv2.imshow('3546',gray2)
cv2.waitKey(0)
print(ypr)
print(ids)
print(corners[0][0])
cv2.waitKey(0)
return image_crop
def rotation_vector_to_euler(rotation_vector):
#Convert an OpenCV-style rotation vector into Euler angles in degrees.
rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
euler_angles, _, _, _, _, _ = cv2.RQDecomp3x3(rotation_matrix)
return euler_angles
def main():
predictor = dlib.shape_predictor(PREDICTOR_PATH)
cap = cv2.VideoCapture(args["camsource"])
while True:
GAZE = "Face Not Found"
ret, img = cap.read()
if not ret:
print(
f'[ERROR - System]Cannot read from source: {args["camsource"]}'
)
break
#faces = detector(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), 0)
faces = face_recognition.face_locations(img, model="cnn")
for face in faces:
#Extracting the co cordinates to convert them into dlib rectangle object
x = int(face[3])
y = int(face[0])
w = int(abs(face[1] - x))
h = int(abs(face[2] - y))
u = int(face[1])
v = int(face[2])
newrect = dlib.rectangle(x, y, u, v)
cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)
shape = predictor(cv2.cvtColor(img, cv2.COLOR_BGR2RGB), newrect)
draw(img, shape)
refImgPts = world.ref2dImagePoints(shape)
height, width, channels = img.shape
focalLength = args["focal"] * width
cameraMatrix = world.cameraMatrix(focalLength,
(height / 2, width / 2))
mdists = np.zeros((4, 1), dtype=np.float64)
# calculate rotation and translation vector using solvePnP
success, rotationVector, translationVector = cv2.solvePnP(
face3Dmodel, refImgPts, cameraMatrix, mdists)
noseEndPoints3D = np.array([[0, 0, 1000.0]], dtype=np.float64)
noseEndPoint2D, jacobian = cv2.projectPoints(
noseEndPoints3D, rotationVector, translationVector,
cameraMatrix, mdists)
# draw nose line
p1 = (int(refImgPts[0, 0]), int(refImgPts[0, 1]))
p2 = (int(noseEndPoint2D[0, 0, 0]), int(noseEndPoint2D[0, 0, 1]))
cv2.line(img,
p1,
p2, (110, 220, 0),
thickness=2,
lineType=cv2.LINE_AA)
# calculating euler angles
rmat, jac = cv2.Rodrigues(rotationVector)
angles, mtxR, mtxQ, Qx, Qy, Qz = cv2.RQDecomp3x3(rmat)
print('*' * 80)
# print(f"Qx:{Qx}\tQy:{Qy}\tQz:{Qz}\t")
x = np.arctan2(Qx[2][1], Qx[2][2])
y = np.arctan2(
-Qy[2][0],
np.sqrt((Qy[2][1] * Qy[2][1]) + (Qy[2][2] * Qy[2][2])))
z = np.arctan2(Qz[0][0], Qz[1][0])
# print("ThetaX: ", x)
print("ThetaY: ", y)
# print("ThetaZ: ", z)
print('*' * 80)
if angles[1] < -15:
GAZE = "Looking: Left"
elif angles[1] > 15:
GAZE = "Looking: Right"
else:
GAZE = "Forward"
cv2.putText(img, GAZE, (20, 20), cv2.FONT_HERSHEY_SIMPLEX, 1,
(0, 255, 80), 2)
cv2.imshow("Head Pose", img)
key = cv2.waitKey(10) & 0xFF
if key == 27:
break
cap.release()
cv2.destroyAllWindows()
def detect(self, img_src=None):
"""
Detect an ARTag.
:param img_src: Path to an image. Used to bypass the camera feed and
track ARTags from images instead. Used primarily for
testing purposes. Disabled by default.
:type img_src: str
:return: Dictionary containing the x distance, z distance, direction,
decision, and yaw if an ARTag is detected.
:rtype: float, float, float, int, float
:return: None if a camera does not recognize an ARTag.
:rtype: None
:raise IOError: Thrown if img_src contains an invalid path.
"""
if not img_src:
self._ret, self._frame = self._cap.read()
else:
file_exists = os.path.isfile(img_src)
if file_exists:
self._frame = cv2.imread(img_src)
self._frame = cv2.resize(
self._frame, (self.RESOLUTIONS[self._RESOLUTION][0],
self.RESOLUTIONS[self._RESOLUTION][1]))
else:
raise IOError('File does not exist')
self._picture = cv2.cvtColor(self._frame, cv2.COLOR_BGR2GRAY)
self._picture = cv2.addWeighted(self._picture, self._CONTRAST,
self._picture, 0, self._BRIGHTNESS)
self._corners, ids, rejected_img_points = ar.detectMarkers(
self._picture, self._AR_DICT, parameters=self._PARAMETERS)
# If no corners found, return an empty object.
if len(self._corners) == 0:
return None
rvec, tvec, _ = ar.estimatePoseSingleMarkers(self._corners[0],
self._MARKER_LENGTH,
self._CAMERA_MATRIX,
self._DIST_COEFFS)
object_x = tvec[0][0][0]
object_z = tvec[0][0][2]
direction = self.get_direction(object_x, object_z)
decision = self.make_decision(direction)
# Get the rotation matrix of the tag. This will help find the yaw angle.
rotation_matrix = np.zeros(shape=(3, 3))
cv2.Rodrigues(rvec[0][0], rotation_matrix, jacobian=0)
# Decompose the rotation matrix into three rotation matrices for pitch,
# yaw, and roll.
pyr = cv2.RQDecomp3x3(rotation_matrix)
# The rotation matrix of the yaw is as follows:
# [cos(th) 0 sin(th)]
# [ 0 1 0 ]
# [-sin(th) 0 cos(th)]
#
# We take the first cos result and feed it into the arccos function to
# get the yaw angle.
yaw_matrix = pyr[4]
yaw_angle = np.arccos(yaw_matrix[0][0])
yaw_angle = np.degrees(yaw_angle)
self._tag_data['x'] = object_x
self._tag_data['z'] = object_z
self._tag_data['direction'] = direction
self._tag_data['decision'] = decision
self._tag_data['yaw'] = yaw_angle
return self._tag_data
def get_euler(self):
return cv2.RQDecomp3x3(self.get_rotation())[0]
