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]