def detect(): cameraMatrix = np.array([[8.2177218160147447e+02, 0., 3.4694289806342221e+02],[0., 8.2177218160147447e+02, 2.4795144956871457e+02],[0., 0., 1.]]) distCoeffs = np.array([4.4824308523616324e-02, -7.4951985348854000e-01, 3.7539708742088725e-03, 8.8931335565222442e-03, 3.7214188475390984e+00]) #型宣言 rvecs = np.array([]) tvecs = np.array([]) # Capture frame-by-frame ret, frame = cap.read() #print(frame.shape) #480x640 # Our operations on the frame come here gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() #print(parameters) ''' detectMarkers(...) detectMarkers(image, dictionary[, corners[, ids[, parameters[, rejectedI mgPoints]]]]) -> corners, ids, rejectedImgPoints ''' #lists of ids and the corners beloning to each id corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) #print(corners) #It's working. # my problem was that the cellphone put black all around it. The alrogithm # depends very much upon finding rectangular black blobs if (corners != []): rvecs, tvecs = aruco.estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs) frame = aruco.drawAxis(frame, cameraMatrix, distCoeffs, rvecs, tvecs, 0.1) frame = aruco.drawDetectedMarkers(frame, corners, ids) return frame, rvecs, tvecs
def detect(): # camera: c270 cameraMatrix = np.array([[8.2177218160147447e+02, 0.0 , 3.4694289806342221e+02], [0.0 , 8.2177218160147447e+02, 2.4795144956871457e+02], [0.0 , 0.0 , 1.0 ]]) distCoeffs = np.array([4.4824308523616324e-02, -7.4951985348854000e-01, 3.7539708742088725e-03, 8.8931335565222442e-03, 3.7214188475390984e+00]) rvecs = np.array([]) tvecs = np.array([]) ret, frame = cap.read() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) if (corners != []): rvecs, tvecs = aruco.estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs) """ frame = aruco.drawAxis(frame, cameraMatrix, distCoeffs, rvecs, tvecs, 0.1) frame = aruco.drawDetectedMarkers(frame, corners, ids) """ return frame, rvecs, tvecs
time1 = time.time() ret, frame = cap.read() # operations on the frame gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY) # lists of ids and the corners belonging to each id corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) # cv2.line(frame, (640, 360), (860, 360), (0, 0, 255), 5) # cv2.line(frame, (640, 360), (640, 580), (0, 255, 0), 5) frame = draw_axis(frame, cx, cy) # index = 0 if ids is not None: # time1 = time.time() ret1 = aruco.estimatePoseSingleMarkers(corners=corners, markerLength=0.1, cameraMatrix=mtx, distCoeffs=dist) rvec, tvec = ret1[0][0, 0, :], ret1[1][0, 0, :] # -- Draw the detected marker and put a reference frame over it aruco.drawDetectedMarkers(frame, corners, ids) aruco.drawAxis(frame, mtx, dist, rvec, tvec, 0.1) (rvec - tvec).any() # get rid of that nasty numpy value array error str_position0 = "Marker Position in Camera frame: x=%f y=%f z=%f" % ( tvec[0], tvec[1], tvec[2]) cv2.putText(frame, str_position0, (0, 50), font, 1, (0, 255, 0), 2, cv2.LINE_AA) # Transformation matrix from marker to camera rmat1, j5 = cv2.Rodrigues(rvec) pose = rmat1.transpose() tvec1 = np.dot(-pose, tvec)
keyb_frame = np.zeros_like(frame) if mode == "typing" or mode == "keyboard_calibration": gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) corners, ids, _ = aruco.detectMarkers(gray_frame, aruco_dict, parameters=parameters) keyb_aruco_pos.x = keyb_aruco_pos.y = keyb_aruco_pos.theta = -1 if np.all(ids != None): for i in range(0, ids.size): M = cv2.moments(corners[i]) if ids[i,0] == 10: keyb_aruco_pos.x = kx = int(M["m10"] / M["m00"]) keyb_aruco_pos.y = ky = int(M["m01"] / M["m00"]) keyb_aruco_pos.theta = slope_deg rvec, tvec ,_ = aruco.estimatePoseSingleMarkers(corners[i], 0.05, mtx, dist) aruco.drawAxis(frame, mtx, dist, rvec, tvec, 0.1) rmat, _ = cv2.Rodrigues(rvec) _,_,rz = np.degrees(rotation_matrix_to_euler_angles(rmat)) # keyb_aruco_pos.theta = np.round(rz, 2) cv2.putText(frame, "angle= " + str(rz), (kx, ky+20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,255), 2) keyb_aruco_pos_pub.publish(keyb_aruco_pos) aruco.drawDetectedMarkers(frame, corners, ids) if detections is not None: tracked_objects = mot_tracker.update(detections.cpu()) unique_labels = detections[:, -1].cpu().unique() if tracked_objects.size != 0:
def __call__(self): #turn multiprocessing on print("Trying to detect a target in a new frame.") """Detects ARUCO targets.""" self.detected = False gray = cv2.cvtColor(self.frame, cv2.COLOR_BGR2GRAY) status = "No Marker Detected" print(gray) corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, self.aruco_dict, parameters=self.parameters) print("Got here0.") if ids is not None: #self.frame = aruco.drawDetectedMarkers(self.frame, corners, ids) rvec, tvec, _ = aruco.estimatePoseSingleMarkers( corners, .203, self.camera_matrix, self.dist_coeff) for i in range(len(ids)): #self.frame =aruco.drawAxis(self.frame,self.camera_matrix,self.dist_coeff,rvec[i],tvec[i],.1) eyedee = ids[i][0] if eyedee in [t.properties["ident"] for t in self.targs]: #for each target that we recognize targ = self.get_target(eyedee) targ.props_are_set = True #set a flag that we have seen this particular target targ.r_mat = np.matrix(cv2.Rodrigues(rvec[i])[0]) targ.t_vec = np.matrix(tvec[i]).T camera_points = cv2.projectPoints( np.array([(targ.properties["pos_rel"]).T]), rvec[i], tvec[i], self.camera_matrix, self.dist_coeff) camera_points = np.array(camera_points[0][0][0]) #obtain the position of the (0,0,0) point of the camera in the real world targ.position_camera = -targ.r_mat.T * targ.t_vec targ.d_cam_image = np.linalg.norm( targ.position_camera + (targ.properties["pos_rel"]).T) #store position of camera target targ.camera_frame_loc = camera_points ## draw a lil circle status = "Marker detected, x: %.2f, y: %.2f" % ( camera_points[0], camera_points[1]) #print("Distance to target %.2f"%targ.d_cam_image) #cv2.circle(self.frame,(int(camera_points[0]),int(camera_points[1])),4,(255,0,0)) print("Got here1.") ## Display the resulting frame #cv2.putText(self.frame, status, (20,30), cv2.FONT_HERSHEY_SIMPLEX, .5, (0,0,255),2) #self.out.write(self.frame) camera_positions = np.array([0, 0]) d_cam_image = 0 count = 0 print("Got here3.") #somehow fuse the measurements from each target (average for now) for t in self.targs: if t.props_are_set: camera_positions += t.camera_frame_loc d_cam_image += t.d_cam_image count += 1 t.props_are_set = False #reset flag if count >= 1: #if we detected at least 1 target camera_positions /= count #populate these variables so that they are passed to the copter commands self.cX = camera_positions[0] self.cY = camera_positions[1] self.d_cam_image = d_cam_image / count self.detected = True #indicate that we have detected at least one target print("Got here.") return [self.detected, self.cX, self.cY, self.d_cam_image]
# blur it and convert it to the HSV # color space #blurred = cv2.GaussianBlur(frame, (11, 11), 0) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) # draw markers frame = aruco.drawDetectedMarkers(frame, corners, ids) if corners: rvecs, tvecs, trash = aruco.estimatePoseSingleMarkers( corners, size_of_marker, mtx, dist) tAverage = None rAverage = None # get reference coordinates if refID in ids: idx = np.where(ids == refID) rRefMtx, _jacob = cv2.Rodrigues(rvecs[idx]) tRefVec = tvecs[idx] frame = aruco.drawAxis(frame, mtx, dist, rRefMtx, tRefVec, length_of_axis) else: print("ref Marker not found!!!!!!!!!!!!!!!!!!!!!!!") break
#image read ret, image = cap.read() #Aruco Marker detection corners, ids, rejectedImgPoints = aruco.detectMarkers(image=image, dictionary=arucoDict, cameraMatrix=cameraMatrix, distCoeff=distCoeffs, parameters = parameters) #Draw the Makres image image = aruco.drawDetectedMarkers(image, corners, ids, borderColor=(0, 255, 0)) #estimate posestion #second parameter is the length of the marker's size (unit is meters) #it affect translation and rotation vectors rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(corners, 0.1, cameraMatrix, distCoeffs) if ids is not None: for i in range(0,ids.size): #draw the each axis on screen aruco.drawAxis(image, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1 ) """ #픽셀로 x,y 좌표를 구하는 코드이며 이 부분은 사용되지 않았고 대신 translation vector를 이용하였습니다. #기록의 이유로 코드는 남겨놓습니다. #calculate center coordinates #x = (corners[i][0][0][0] + corners[i][0][1][0] + corners[i][0][2][0] + corners[i][0][3][0]) / 4 #y = (corners[i][0][0][1] + corners[i][0][1][1] + corners[i][0][2][1] + corners[i][0][3][1]) / 4 #pixel scaling 카메라 ~ 마커사이거리 188cm
def vision(): # Load the camera calibration values Camera = np.load('Calibration_Surface_Book_Rear_Camera.npz') CM = Camera['CM'] # camera matrix dist_coef = Camera['dist_coef'] # distortion coefficients from the camera aruco_dict = aruco.Dictionary_get( aruco.DICT_4X4_50) # Load the aruco dictionary pa = aruco.DetectorParameters_create() # Set the detection parameters # Select the correct camera (0) = front camera, (1) = rear camera cap = cv2.VideoCapture(1) # Set the width and heigth of the camera to 640x480 cap.set(3, 640) cap.set(4, 480) # Create two opencv named windows cv2.namedWindow("frame-image", cv2.WINDOW_AUTOSIZE) # Position the window cv2.moveWindow("frame-image", 0, 0) t_end = time.time() + 10 # Execute this continuously while time.time() < t_end: # Capture current frame from the camera ret, frame = cap.read() # Convert the image from the camera to Gray scale gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Run the detection formula corners, ids, rP = aruco.detectMarkers(gray, aruco_dict) # # Count the number of Arucos visible # try: # IDScount = len(ids) # except: # IDScount = 0 # Calculate the pose of the markers rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers( corners, 53, CM, dist_coef ) # <<<< IMPORTANT: number needs changing to width of printed arucos (in mm) # Draw the detected markers as an overlay out = aruco.drawDetectedMarkers(frame, corners, ids) # Create Coordinate Storage Arrays X = [] #X Coordinate Locations Array Y = [] Z = [] ID = [] # Run loop if ids are detected if ids is not None: for i, id in enumerate(ids): # Overlay the axis on the image out = aruco.drawAxis(out, CM, dist_coef, rvecs[i][0][:], tvecs[i][0][:], 30) # Print the tvecs tranformation matrix or Aruco coordinates # print("X = {:4.1f} Y = {:4.1f} Z = {:4.1f} ID = {:2d}".format(tvecs[i][0][0], tvecs[i][0][1], tvecs[i][0][2], ids[i][0])) X.append(tvecs[i][0][0]) Y.append(tvecs[i][0][1]) Z.append(tvecs[i][0][2]) ID.append(ids[i][0]) # debugTEST = [] # Display the original frame in a window and aruco markers cv2.imshow('frame-image', frame) # If the button q is pressed in one of the windows if cv2.waitKey(20) & 0xFF == ord('q'): # Exit the While loop break # When everything done, release the capture cap.release() # close all windows cv2.destroyAllWindows() # # exit the kernel # exit(0) return X, Y, Z, ID, rvecs
def main(marker_length, marker_separation): # opencv/aruco settings cam = cv2.VideoCapture(0) ardict = aruco.Dictionary_get(aruco.DICT_4X4_1000) board = aruco.GridBoard_create(4, 5, marker_length, marker_separation, ardict) parameters = aruco.DetectorParameters_create() parameters.minMarkerPerimeterRate = 0.03 # load camera parameters with open("calib.json", 'r') as f: data = json.load(f) dr = DrawClass(data.get('camera_matrix'), data.get('dist_coeff')) # and make some variables for aruco mtx = dr.mtx dist = dr.dist # print(cv2.calibrationMatrixValues(mtx, (640, 480), 3.6, 2.7)) # globals variables cs = CoordinatesSystem() while cam.isOpened(): k = cv2.waitKey(16) ret, frame = cam.read() info_screen = np.ones((500, 1000, 3), np.uint8) gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY) corners, ids, rejected_img_points = aruco.detectMarkers( gray, ardict, parameters=parameters) if corners is not None: frame = dr.draw_markers(frame, corners, ids) r_vecs, t_vecs, _ = aruco.estimatePoseSingleMarkers( corners, marker_length, mtx, dist) markers = [ Marker(ids[i][0], r_vecs[i], t_vecs[i]) for i in range(len(corners)) ] frame = dr.draw_axis_of_markers(frame, markers, 1) info_screen = dr.print_text(info_screen, markers) if len(markers) == 2: # rv, tv = relative_position(markers[0].r_vec, markers[0].t_vec, markers[1].r_vec, markers[1].t_vec) tv = relative_position2(markers[0].r_vec, markers[0].t_vec, markers[1].t_vec) info_screen = dr.draw_debug_lines(info_screen, tv) print(tv) if len(markers) == 1 and k == ord('p'): cs.reset(markers[0].r_vec, markers[0].t_vec) print(cs.rot_matrix) if len(markers) == 1 and cs.fcs_r_vec is not None: tv3 = cs.get_coordinates_in_cs(markers[0].t_vec) frame = dr.draw_axis_points(frame, cs.fcs_r_vec, cs.fcs_t_vec) info_screen = dr.draw_debug_lines(info_screen, tv3) print(tv3) cv2.line(frame, (320, 240), (320, 240), (0, 0, 0), 4) cv2.imshow("frame", frame) cv2.imshow("info", info_screen) if k == ord('q'): cam.release() break
########################################################################### # TODO: Add validation here to reject IDs/corners not part of a gridboard # ########################################################################### # Outline all of the markers detected in our image QueryImg = aruco.drawDetectedMarkers(QueryImg, corners, borderColor=(0, 0, 255)) # Require 15 markers before drawing axis if ids is not None and len(ids) > 3: # Estimate the posture of the gridboard, which is a construction of 3D space based on the 2D video #pose, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, cameraMatrix, distCoeffs) #if pose: # # Draw the camera posture calculated from the gridboard # QueryImg = aruco.drawAxis(QueryImg, cameraMatrix, distCoeffs, rvec, tvec, 0.3) # Estimate the posture per each Aruco marker rvecs, tvecs = aruco.estimatePoseSingleMarkers( corners, 1, cameraMatrix, distCoeffs) for rvec, tvec in zip(rvecs, tvecs): QueryImg = aruco.drawAxis(QueryImg, cameraMatrix, distCoeffs, rvec, tvec, 1) # Display our image cv2.imshow('QueryImage', QueryImg) # Exit at the end of the video on the 'q' keypress if cv2.waitKey(1) & 0xFF == ord('q'): break cv2.destroyAllWindows()
def detect_show_marker(img, gray, aruco_dict, parameters, cameraMatrix, distCoeffs): detected_1, detected_2 = False, False i, j = None, None distance_1, distance_2 = None, None font = cv2.FONT_HERSHEY_SIMPLEX corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) img = aruco.drawDetectedMarkers(img, corners, ids) if ids is not None: i = 6 # Id of aruco - reference system. j = 5 # Id of target aruco. for k in range(0, len(ids)): rvec, tvec, markerPoints = aruco.estimatePoseSingleMarkers( corners[k], 0.045, cameraMatrix, distCoeffs) if ids[k] == i: img = aruco.drawAxis(img, cameraMatrix, distCoeffs, rvec, tvec, 0.05) c_rvec = rvec c_tvec = tvec distance_1 = tvec[0][0][2] detected_1 = True elif ids[k] == j: n_rvec = rvec n_tvec = tvec distance_2 = tvec[0][0][2] detected_2 = True if (detected_1 == True) and (detected_2 == True): frvec, ftvec = relatPos(c_rvec, c_tvec, n_rvec, n_tvec) """ frvec - orientation vector of the marker regarding the reference system. ftvec - position of aruco regarding the rs. n_tvec - position of aruco regarding camera. """ # Orientation aruco regarding the rs. angles0 = rotmtx_to_euler_angles(cv2.Rodrigues(frvec)[0]) n_rmat = cv2.Rodrigues(n_rvec)[0] # Orientation aruco regarding camera. angles1 = rotmtx_to_euler_angles(n_rmat) # Camera position regarding aruco. pos_cam_to_aruco = -np.matrix(n_rmat).T * np.matrix(n_tvec).T cam_rotmtx = np.matrix(n_rmat).T # Reverse orientation camera to the aruco. angles2 = rotmtx_to_euler_angles(cam_rotmtx) rmat = cv2.Rodrigues(c_rvec)[0] # Camera position regarding the rs. pos_cam_to_rs = -np.matrix(rmat).T * np.matrix(c_tvec).T # Reverse orientation camera regarding the rs. angles3 = rotmtx_to_euler_angles(rmat) if (distance_1 is not None): cv2.putText(img, 'Id' + str(i) + ' %.2fsm' % (distance_1 * 100), (0, 64), font, 1, (0, 255, 0), 2, cv2.LINE_AA) if (distance_2 is not None): cv2.putText(img, 'Id' + str(j) + ' %.2fsm' % (distance_2 * 100), (0, 104), font, 1, (0, 255, 0), 2, cv2.LINE_AA) return cv2.imshow('frame', img) # Final img.
def roda_todo_frame(imagem): #print("frame") try: cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8") # imagem compressed #cv_image = cv2.flip(cv_image, -1) # Descomente se for robo real #cv_image = bridge.imgmsg_to_cv2(imagem, "bgr8") # imagem não compressed #cv_image = cv2.resize(cv_image,(cv_image.shape[1]*2,cv_image.shape[0]*2)) # resize image se necessario gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) print(ids) if ids is not None: #-- ret = [rvec, tvec, ?] #-- rvec = [[rvec_1], [rvec_2], ...] vetor de rotação #-- tvec = [[tvec_1], [tvec_2], ...] vetor de translação ret = aruco.estimatePoseSingleMarkers(corners, marker_size, camera_matrix, camera_distortion) rvec, tvec = ret[0][0, 0, :], ret[1][0, 0, :] #-- Desenha um retanculo e exibe Id do marker encontrado aruco.drawDetectedMarkers(cv_image, corners, ids) aruco.drawAxis(cv_image, camera_matrix, camera_distortion, rvec, tvec, 1) #-- Print tvec vetor de tanslação em x y z str_position = "Marker x=%4.0f y=%4.0f z=%4.0f" % ( tvec[0], tvec[1], tvec[2]) print(str_position) cv2.putText(cv_image, str_position, (0, 100), font, 1, (0, 255, 0), 1, cv2.LINE_AA) ##############----- Referencia dos Eixos------########################### # Linha referencia em X cv2.line(cv_image, (int(cv_image.shape[1] / 2), int(cv_image.shape[0] / 2)), ((int(cv_image.shape[1] / 2) + 50), (int(cv_image.shape[0] / 2))), (0, 0, 255), 5) # Linha referencia em Y cv2.line( cv_image, (int(cv_image.shape[1] / 2), int(cv_image.shape[0] / 2)), (int(cv_image.shape[1] / 2), int( (cv_image.shape[0] / 2 + 50))), (0, 255, 0), 5) #####################---- Distancia Euclidiana ----##################### # Calcula a distancia usando apenas a matriz tvec, matriz de tanslação # Pode usar qualquer uma das duas formas distance = np.sqrt(tvec[0]**2 + tvec[1]**2 + tvec[2]**2) distancenp = np.linalg.norm(tvec) #-- Print distance str_dist = "Dist aruco=%4.0f dis.np=%4.0f" % (distance, distancenp) print(str_dist) cv2.putText(cv_image, str_dist, (0, 15), font, 1, (0, 255, 0), 1, cv2.LINE_AA) #####################---- Distancia pelo foco ----##################### #https://www.pyimagesearch.com/2015/01/19/find-distance-camera-objectmarker-using-python-opencv/ # raspicam v2 focal legth FOCAL_LENGTH = 3.6 #3.04 # pixel por unidade de medida m = (camera_matrix[0][0] / FOCAL_LENGTH + camera_matrix[1][1] / FOCAL_LENGTH) / 2 # corners[0][0][0][0] = [ID][plano?][pos_corner(sentido horario)][0=valor_pos_x, 1=valor_pos_y] pixel_length1 = math.sqrt( math.pow(corners[0][0][0][0] - corners[0][0][1][0], 2) + math.pow(corners[0][0][0][1] - corners[0][0][1][1], 2)) pixel_length2 = math.sqrt( math.pow(corners[0][0][2][0] - corners[0][0][3][0], 2) + math.pow(corners[0][0][2][1] - corners[0][0][3][1], 2)) pixlength = (pixel_length1 + pixel_length2) / 2 dist = marker_size * FOCAL_LENGTH / (pixlength / m) #-- Print distancia focal str_distfocal = "Dist focal=%4.0f" % (dist) print(str_distfocal) cv2.putText(cv_image, str_distfocal, (0, 30), font, 1, (0, 255, 0), 1, cv2.LINE_AA) ####################--------- desenha o cubo -----------######################### # https://github.com/RaviJoshii/3DModeler/blob/eb7ca48fa06ca85fcf5c5ec9dc4b562ce9a22a76/opencv/program/detect.py m = marker_size / 2 pts = np.float32([[-m, m, m], [-m, -m, m], [m, -m, m], [m, m, m], [-m, m, 0], [-m, -m, 0], [m, -m, 0], [m, m, 0]]) imgpts, _ = cv2.projectPoints(pts, rvec, tvec, camera_matrix, camera_distortion) imgpts = np.int32(imgpts).reshape(-1, 2) cv_image = cv2.drawContours(cv_image, [imgpts[:4]], -1, (0, 0, 255), 4) for i, j in zip(range(4), range(4, 8)): cv_image = cv2.line(cv_image, tuple(imgpts[i]), tuple(imgpts[j]), (0, 0, 255), 4) cv_image = cv2.drawContours(cv_image, [imgpts[4:]], -1, (0, 0, 255), 4) # Exibe tela cv2.imshow("Camera", cv_image) cv2.waitKey(1) except CvBridgeError as e: print('ex', e)
def findMarker(self, req): self.rvecs_arr = np.zeros((3, NUMBER)) self.tvecs_arr = np.zeros((3, NUMBER)) res = aruco_infoResponse() for order in range (NUMBER): # Capturing each frame of our video stream # ret, self.QueryImg = self.cam.read() # frames = self.pipeline.wait_for_frames() # color_frame = frames.get_color_frame() # self.QueryImg = np.asanyarray(color_frame.get_data()) if True: # grayscale image gray = cv2.cvtColor(self.QueryImg, cv2.COLOR_BGR2GRAY) # Detect Aruco markers corners, ids, _ = aruco.detectMarkers(gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS) # Refine detected markers # Eliminates markers not part of our board, adds missing markers to the board # corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers( # image = gray, # board = board, # detectedCorners = corners, # detectedIds = ids, # rejectedCorners = rejectedImgPoints, # self.cameraMatrix = self.cameraMatrix, # self.distCoeffs = self.distCoeffs) ########################################################################### # TODO: Add validation here to reject IDs/corners not part of a gridboard # ########################################################################### # Require 15 markers before drawing axis if ids is not None and len(ids) > 0: # Estimate the posture of the gridboard, which is a construction of 3D space based on the 2D video #pose, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, self.cameraMatrix, self.distCoeffs) #if pose: # # Draw the camera posture calculated from the gridboard # self.QueryImg = aruco.drawAxis(self.QueryImg, self.cameraMatrix, self.distCoeffs, rvec, tvec, 0.3) # Estimate the posture per each Aruco marker print('ids is ', ids) self.rvecs, self.tvecs, _ = aruco.estimatePoseSingleMarkers(corners, self.markersize, self.cameraMatrix, self.distCoeffs) print('fuckkk') for _id, rvec, tvec in zip(ids, self.rvecs, self.tvecs): if _id[0] == req.id: for i in range(3): self.rvecs_arr[i][order] = rvec[0][i] self.tvecs_arr[i][order] = tvec[0][i] # self.QueryImg = aruco.drawAxis(self.QueryImg, self.cameraMatrix, self.distCoeffs, rvec, tvec, 0.02) cv2.waitKey(10) # Display our image # print('self.rvecs_arr = ', self.rvecs_arr) # print('self.tvecs_arr = ', self.tvecs_arr) cv2.destroyAllWindows() r_avg = np.zeros(3) t_avg = np.zeros(3) ra = self.rvecs_arr[0].nonzero() rb = self.rvecs_arr[1].nonzero() rc = self.rvecs_arr[2].nonzero() tx = self.tvecs_arr[0].nonzero() ty = self.tvecs_arr[1].nonzero() tz = self.tvecs_arr[2].nonzero() ra = self.rvecs_arr[0][ra] rb = self.rvecs_arr[1][rb] rc = self.rvecs_arr[2][rc] tx = self.tvecs_arr[0][tx] ty = self.tvecs_arr[1][ty] tz = self.tvecs_arr[2][tz] ra = np.sort(ra, kind = 'quicksort') rb = np.sort(rb, kind = 'quicksort') rc = np.sort(rc, kind = 'quicksort') tx = np.sort(tx, kind = 'quicksort') ty = np.sort(ty, kind = 'quicksort') tz = np.sort(tz, kind = 'quicksort') r = np.array((ra, rb, rc)) t = np.array((tx, ty, tz)) for i in range(3): rv, tv = r[i], t[i] while np.std(rv) > 0.01 and len(rv) >= NUMBER*0.2: if abs(rv[0] - np.average(rv)) > abs(rv[-1] - np.average(rv)): rv = np.delete(rv, 0) else: rv = np.delete(rv, -1) while np.std(tv) > 0.01 and len(tv) >= NUMBER*0.2: if abs(tv[0] - np.average(tv)) > abs(tv[-1] - np.average(tv)): tv = np.delete(tv, 0) else: tv = np.delete(tv, -1) r_avg[i] = np.average(rv) t_avg[i] = np.average(tv) # print('[_id, r,t] = ', [_id, r,t]) # res.ids.append(_id) # res.rvecs = np.append(res.rvecs, r_avg) # res.tvecs = np.append(res.tvecs, t_avg) res.rvecs = r_avg res.tvecs = t_avg print('res.rvecs is ', res.rvecs) print('res.tvecs is ', res.tvecs) result = np.array(()) result = np.append(result, [np.copy(r_avg), np.copy(t_avg)]) result = result.reshape(2,3) if self.name == 'test': # Outline all of the markers detected in our image self.QueryImg = aruco.drawDetectedMarkers(self.QueryImg, corners, borderColor=(0, 0, 255)) self.QueryImg = aruco.drawAxis(self.QueryImg, self.cameraMatrix, self.distCoeffs, result[0], result[1], 0.02) print('ffffffuck') curr_path = os.path.dirname(os.path.abspath(__file__)) filename = curr_path + "/pic/camera-pic-of-charucoboard-" + str(int(self.frameId)) + ".jpg" cv2.imwrite(filename, self.QueryImg) self.frameId += 1 # cv2.imwrite('./123%d.jpg'%self.cnd, self.QueryImg) # self.cnd += 1 # cv2.namedWindow('Amanda', cv2.WINDOW_AUTOSIZE) # self.QueryImg = cv2.imread('./123%d.jpg'%self.cnd) # cv2.imshow('Amanda', self.QueryImg) # cv2.waitKey(1000) # cv2.destroyAllWindows() # time.sleep(2) print('------') # while not cv2.waitKey(1) & 0xFF == ord('q'): # pass # cv2.destroyAllWindows() return res
def aruco_fun(): global pipe, cameraMatrix, distCoeffs, depth_intrinsics axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3) if True: frames = pipe.wait_for_frames() align_to = rs.stream.color align = rs.align(align_to) aligned_frames = align.process(frames) depth_frame = aligned_frames.get_depth_frame() color_frame = aligned_frames.get_color_frame() color_img = np.array(color_frame.get_data()) color_img_temp = copy.deepcopy(color_img) depth_img = np.array(depth_frame.get_data()) frame = color_img_temp gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) #print(corners,ids) #center_p(corners, depth_img,cameraMatrix) #if False: if ids is not None and len(ids) > 0: global wheel_chair_translation_vectors, find_wheel_chair #print("len",len(ids[0])) num = int(len(ids)) for id_obj in range(num): if ids[id_obj] == 6: find_wheel_chair = 1 wheel_chair_translation_vectors = get_xyz(corners[id_obj], depth_img,cameraMatrix,depth_intrinsics) elif ids[id_obj]!=6: if find_wheel_chair == 0: continue obj_translation_vectors = get_xyz(corners[id_obj], depth_img,cameraMatrix,depth_intrinsics) p = obj_translation_vectors - wheel_chair_translation_vectors print(ids[id_obj]," ","postion vetor is",p) #process_data(ids[id_obj],p) gray = aruco.drawDetectedMarkers(gray, corners) cv2.imshow('frame',gray) if cv2.waitKey(1) & 0xFF == ord('q'): cv2.destroyAllWindows() #break if False: #while(True): frames = pipe.wait_for_frames() color_frame = frames.get_color_frame() # Convert images to numpy arrays color_image = np.asanyarray(color_frame.get_data()) gray = cv2.cvtColor(color_image.copy(), cv2.COLOR_RGB2GRAY) corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS) corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers( image = gray, board = board, detectedCorners = corners, detectedIds = ids, rejectedCorners = rejectedImgPoints, cameraMatrix = cameraMatrix, distCoeffs = distCoeffs) ProjectImage = color_image.copy() ProjectImage = aruco.drawDetectedMarkers(ProjectImage, corners, borderColor=(0, 0, 255)) print(ids) if ids is not None and len(ids) > 0: # Estimate the posture per each Aruco marker rotation_vectors, translation_vectors, _objPoints = aruco.estimatePoseSingleMarkers(corners, 1, cameraMatrix, distCoeffs) #global wheel_chair_rotation_vectors, wheel_chair_translation_vectors, find_wheel_chair ,w2c print("len",len(ids[0])) num = int(len(ids)) for id_obj in range(num): if ids[id_obj] == 6: find_wheel_chair = 1 wheel_chair_rotation_vectors =cv2.Rodrigues( rotation_vectors[id_obj][0])[0] wheel_chair_translation_vectors = rotation_vectors[id_obj] c2w = np.c_[wheel_chair_rotation_vectors,wheel_chair_translation_vectors[0]] temp = np.array([0,0,0,1]) c2w = np.r_[c2w, [temp]] #w2c = np.linalg.inv(c2w) w2c = np.c_[wheel_chair_rotation_vectors.T,-wheel_chair_rotation_vectors.T.dot(wheel_chair_translation_vectors[0])] w2c = np.r_[w2c, [temp]] elif ids[id_obj]!=6: if find_wheel_chair == 0: continue obj_rotation_vectors =cv2.Rodrigues( rotation_vectors[id_obj][0])[0] obj_translation_vectors = rotation_vectors[id_obj] c2o = np.c_[obj_rotation_vectors,obj_translation_vectors[0]] temp = np.array([0,0,0,1]) c2o = np.r_[c2o, [temp]] w2o = w2c.dot(c2o) #p = w2o[:,3][:3] p = compute_w2o(wheel_chair_rotation_vectors,wheel_chair_translation_vectors,obj_rotation_vectors,obj_translation_vectors) print(ids[id_obj]," ","postion vetor is",p) #process_data(ids[id_obj],p) ids = 1 for rvec in rotation_vectors: rotation_mat = cv2.Rodrigues(rvec[0])[0] ids = ids+1 for rvec, tvec in zip(rotation_vectors, translation_vectors): if len(sys.argv) == 2 and sys.argv[1] == 'cube': try: imgpts, jac = cv2.projectPoints(axis, rvec, tvec, cameraMatrix, distCoeffs) ProjectImage = drawCube(ProjectImage, corners, imgpts) except: continue else: ProjectImage = aruco.drawAxis(ProjectImage, cameraMatrix, distCoeffs, rvec, tvec, 1) cv2.imshow('ProjectImage', ProjectImage) #frame_markers = aruco.drawDetectedMarkers(frame.copy(), corners, ids) #cv2.imshow('frame',frame_markers) #end = datetime.now() #exec_time = end - start #print("aruco control") #print(exec_time,exec_time.total_seconds()) if cv2.waitKey(1) & 0xFF == ord('q'): cv2.destroyAllWindows()
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')
print('camera matrix: ', camera_matrix) print('dist_coeffs: ', dist_coeffs) fourcc = cv2.VideoWriter_fourcc(*'XVID') vwriter = cv2.VideoWriter('./out/dist_' + args.id + '.avi', fourcc=fourcc, fps=25, frameSize=(640, 480), isColor=True) while True: try: ret, img = cam.read() corners, ids, reprojImgPts = aruco.detectMarkers(img, ar_dict) rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers( corners, 15.0, camera_matrix, dist_coeffs) # print('map_rvecs', map_rvecs) dist = np.linalg.norm(tvecs[1] - tvecs[0]) if start_measure: if not offset: offset = soll - dist starttime = time.time() dist_cm.append(dist + offset) smoothed_cm.append(smooth(dist_cm[-5:], 5)) timestamps.append(time.time() - starttime) points.setData(timestamps, smoothed_cm, pen='r')
def main(): face_landmark_path = './shape_predictor_68_face_landmarks.dat' #flags/initializations to be set skip_frame = 3 socket_connect = 1 # set to 0 if we are testing the code locally on the computer with only the realsense tracking. simplified_calib_flag = 0 # this is set to 1 if we want to do one-time calibration arucoposeflag = 1 # img_idx keeps track of image index (frame #). img_idx = 0 N_samples_calib = 10 # number of samples for computing the calibration matrix using homography if socket_connect == 1: # create a socket object s = socket.socket() print("Socket successfully created") # reserve a port on your computer in our case it is 2020 but it can be anything port = 2020 s.bind(('', port)) print("socket binded to %s" % (port)) # put the socket into listening mode s.listen(5) print("socket is listening") c, addr = s.accept() print('got connection from ', addr) if socket_connect == 1 and simplified_calib_flag == 0: arucoinstance = arucocalibclass() ReturnFlag = 1 aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250) marker_len = 0.0645 MLRSTr = arucoinstance.startcamerastreaming(c, ReturnFlag, marker_len, aruco_dict, N_samples_calib) print(MLRSTr) elif socket_connect == 1 and simplified_calib_flag == 1: T_M2_RS = np.array([ -1.0001641, 0.00756584, 0.00479072, 0.03984956, -0.00774137, -0.99988126, -0.03246199, -0.01359556, 0.00453644, -0.03251681, 0.99971441, -0.00428408, 0., 0., 0., 1. ]) T_M2_RS = T_M2_RS.reshape(4, 4) MLRSTr = simplified_calib(T_M2_RS, c) else: MLRSTr = np.array((1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1)) MLRSTr = MLRSTr.reshape(4, 4) print(MLRSTr) # end socket ###### # Configure depth and color streams of Realsense camera pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) # Start streaming profile = pipeline.start(config) align_to = rs.stream.color align = rs.align(align_to) # Intrinsics & Extrinsics of Realsense depth_intrin = profile.get_stream( rs.stream.depth).as_video_stream_profile().get_intrinsics() intr = profile.get_stream( rs.stream.color).as_video_stream_profile().get_intrinsics() mtx = np.array([[intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy], [0, 0, 1]]) dist = np.array(intr.coeffs) #load cascade classifier training file for lbpcascade lbp_face_cascade = cv2.CascadeClassifier( "./haarcascade_frontalface_alt2.xml" ) #"/home/supriya/.local/lib/python3.6/site-packages/cv2/data/haarcascade_frontalface_alt2.xml") #cv2.CascadeClassifier('/home/supriya/supriya/FSA-Net/demo/lbpcascade_frontalface.xml') #cv2.CascadeClassifier('data/lbpcascade_frontalface_improved.xml') # cv2.CascadeClassifier('/home/supriya/supriya/FSA-Net/demo/lbpcascade_frontalface.xml') facemark = cv2.face.createFacemarkLBF() facemark.loadModel('./lbfmodel.yaml') print('Start detecting pose ...') detected_pre = [] while True: # get video frame frames = pipeline.wait_for_frames() aligned_frames = align.process(frames) color_frame = aligned_frames.get_color_frame() aligned_depth_frame = aligned_frames.get_depth_frame() if not aligned_depth_frame or not color_frame: continue # read color frame input_img = np.asanyarray(color_frame.get_data()) #increment count of the image index img_idx = img_idx + 1 img_h, img_w, _ = np.shape(input_img) # Process the first frame and every frame after "skip_frame" frames if img_idx == 1 or img_idx % skip_frame == 0: # convert image to grayscale gray_img = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY) # detect faces using LBP detector faces = lbp_face_cascade.detectMultiScale(gray_img, scaleFactor=1.1, minNeighbors=5) #draw rectangle around detected face for (x, y, w, h) in faces: cv2.rectangle(input_img, (x, y), (x + w, y + h), (0, 255, 0), 2) depth_point = [0, 0, 0] if len(faces) > 0: #detect landmarks status, landmarks = facemark.fit(gray_img, faces) #draw dots on the detected facial landmarks for f in range(len(landmarks)): cv2.face.drawFacemarks(input_img, landmarks[f]) #get head pose reprojectdst, rotation_vec, rotation_mat, translation_vec, euler_angle, image_pts = get_head_pose( landmarks[0][0], mtx, dist) # draw circle on image points (nose tip, corner of eye, lip and chin) for p in image_pts: cv2.circle(input_img, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1) # draw circle on image points (nose tip, corner of eye, lip and chin) for p in image_pts: cv2.circle(input_img, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1) # draw line sticking out of the nose tip and showing the head pose (nose_end_point2D, jacobian) = cv2.projectPoints(np.array([(0.0, 0.0, 1000.0)]), rotation_vec, translation_vec, mtx, dist) p1 = (int(image_pts[0][0]), int(image_pts[0][1])) p2 = (int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1])) cv2.line(input_img, p1, p2, (255, 0, 0), 2) # get 3D co-ord of nose - to get a more accurate estimation of the translaion of the head depth = aligned_depth_frame.get_distance( landmarks[0][0][30][0], landmarks[0][0][30][1]) cv2.circle(input_img, (landmarks[0][0][30][0], landmarks[0][0][30][1]), 3, (0, 255, 0), -1) depth_point = rs.rs2_deproject_pixel_to_point( depth_intrin, [landmarks[0][0][30][0], landmarks[0][0][30][1]], depth) depth_point = np.array(depth_point) depth_point = np.reshape(depth_point, [1, 3]) #check if the depth estimation is not zero and filters out faces within 0.8 m from the camera if (depth_point[0][2] != 0 and depth_point[0][2] < 0.8): # #Combine rotation matrix and translation vector (given by the depth point) to get the head pose RSTr = np.hstack([rotation_mat, depth_point.transpose()]) RSTr = np.vstack([RSTr, [0, 0, 0, 1]]) print("head pose", RSTr) # If arucoposeflag = 1, an Aruco marker will get detected and its transformed pose will be streamed to the AR headset and the pose # of the tracking parent will be updated to reflect Aruco marker pose. This can be used to verify/test the accuracy of teh calibration if arucoposeflag == 1: print("aruco") gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY) # set dictionary size depending on the aruco marker selected aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) # detector parameters can be set here (List of detection parameters[3]) parameters = aruco.DetectorParameters_create() parameters.adaptiveThreshConstant = 10 # lists of ids and the corners belonging to each id corners, ids, rejectedImgPoindetectorts = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) # font for displaying text (below) font = cv2.FONT_HERSHEY_SIMPLEX # check if the ids list is not empty if np.all(ids != None): # estimate pose of each marker and return the values rvec, tvec, _ = aruco.estimatePoseSingleMarkers( corners, 0.045, mtx, dist ) # 0.0628 (0.061 if using Dell laptop - 95% zoom) for i in range(0, ids.size): # draw axis for the aruco markers aruco.drawAxis(input_img, mtx, dist, rvec[i], tvec[i], 0.1) # draw a square around the markers aruco.drawDetectedMarkers(input_img, corners) #Combine rotation matrix and translation vector to get Aruco pose R_rvec = R.from_rotvec(rvec[0]) R_rotmat = R_rvec.as_matrix() AruRSTr = np.hstack( [R_rotmat[0], tvec[0].transpose()]) AruRSTr = np.vstack([AruRSTr, [0, 0, 0, 1]]) RSTr = AruRSTr print("Aruco pose", AruRSTr) # Since pose detected in OpenCV will be right handed coordinate system, it needs to be converted to left-handed coordinate system of Unity RSTr_LH = np.array([ RSTr[0][0], RSTr[0][2], RSTr[0][1], RSTr[0][3], RSTr[2][0], RSTr[2][2], RSTr[2][1], RSTr[2][3], RSTr[1][0], RSTr[1][2], RSTr[1][1], RSTr[1][3], RSTr[3][0], RSTr[3][1], RSTr[3][2], RSTr[3][3] ]) # converting to left handed coordinate system RSTr_LH = RSTr_LH.reshape(4, 4) #Compute the transformed pose to be streamed to MagicLeap HeadPoseTr = np.matmul(MLRSTr, RSTr_LH) if socket_connect == 1: # Array to be sent ArrToSend = np.array([ HeadPoseTr[0][0], HeadPoseTr[0][1], HeadPoseTr[0][2], HeadPoseTr[0][3], HeadPoseTr[1][0], HeadPoseTr[1][1], HeadPoseTr[1][2], HeadPoseTr[1][3], HeadPoseTr[2][0], HeadPoseTr[2][1], HeadPoseTr[2][2], HeadPoseTr[2][3], HeadPoseTr[3][0], HeadPoseTr[3][1], HeadPoseTr[3][2], HeadPoseTr[3][3] ]) # Send transformed pose to AR headset dataTosend = struct.pack('f' * len(ArrToSend), *ArrToSend) c.send(dataTosend) else: print("No Face Detected") # display detected face with landmarks, pose. cv2.imshow('Landmark_Window', input_img) key = cv2.waitKey(1)
(w, h)) # undistort mapx, mapy = cv2.initUndistortRectifyMap(camera_matrix, dist_coeffs, None, newcameramtx, (w, h), 5) dst = cv2.remap(gray_d, mapx, mapy, cv2.INTER_LINEAR) # crop the image x, y, w, h = roi gray = dst[y:y + h, x:x + w] #cv2.imshow('frame',gray) corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, dictionary, parameters=arucoParams) if ids is not None: rvec, tvec, _ = aruco.estimatePoseSingleMarkers( corners, markerLength, camera_matrix, dist_coeffs) #retval, rvec, tvec = cv.aruco.estimatePoseBoard( corners, ids, board, camera_matrix, dist_coeffs) imgWithAruco = aruco.drawDetectedMarkers(gray, corners, ids) #, (0,255,0)) if rvec is not None: objectPoints = np.asarray([[105, 105, 105]], dtype=np.float) imagePoints = cv2.projectPoints(objectPoints, rvec[0], tvec[0], camera_matrix, dist_coeffs) corner1 = np.append(corners[0][0][0], [0]).transpose().reshape(3, 1) E0 = np.asarray([[markerLength / 2, 0, 0]]).transpose() #print('Tvec_0: ',tvec[0]) #print('****') #print('Rvec_0: ',rvec[0]) #print('****')
def track( self, frame, id_to_find=None, marker_size=None, ): marker_found = False x = y = z = pitch_camera = x_camera = y_camera = z_camera = 0 # -- Convert in gray scale gray = cv2.cvtColor( frame, cv2.COLOR_BGR2GRAY ) # -- remember, OpenCV stores color images in Blue, Green, Red # -- Find all the aruco markers in the image corners, ids, rejected = aruco.detectMarkers( image=gray, dictionary=self._aruco_dict, parameters=self._parameters, cameraMatrix=self._camera_matrix, distCoeff=self._camera_distortion, ) pitch_marker, roll_marker, yaw_marker = None, None, None pitch_camera, roll_camera, yaw_camera = None, None, None planned_ids = [] if not isinstance(ids, None): planned_ids = list(itertools.chain(*ids)) if id_to_find in planned_ids: index_id_to_find = planned_ids.index(id_to_find) marker_found = True # -- array of rotation and position of each marker in camera frame # -- rvec = [[rvec_1], [rvec_2], ...] attitude of the marker respect to camera frame # -- tvec = [[tvec_1], [tvec_2], ...] position of the marker in camera frame rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers( corners, marker_size, self._camera_matrix, self._camera_distortion) # -- Unpack the output rvec, tvec = rvecs[index_id_to_find][0], tvecs[index_id_to_find][0] x = tvec[0] y = tvec[1] z = tvec[2] # -- Obtain the rotation matrix tag->camera R_ct = np.matrix(cv2.Rodrigues(rvec)[0]) R_tc = R_ct.T # -- Get the attitude in terms of euler 321 (Needs to be flipped first) ( roll_marker, pitch_marker, yaw_marker, ) = self._rotationMatrixToEulerAngles(self._R_flip * R_tc) # -- Now get Position and attitude f the camera respect to the marker pos_camera = -R_tc * np.matrix(tvec).T x_camera = pos_camera[0] y_camera = pos_camera[1] z_camera = pos_camera[2] ( roll_camera, pitch_camera, yaw_camera, ) = self._rotationMatrixToEulerAngles(self._R_flip * R_tc) if type(None) == type(yaw_marker): marker_found = False yaw_marker = 0 if marker_found: roll_camera = math.degrees(roll_camera) yaw_camera = math.degrees(yaw_camera) pitch_camera = math.degrees(pitch_camera) roll_marker = math.degrees(roll_marker) yaw_marker = math.degrees(yaw_marker) pitch_marker = math.degrees(pitch_marker) x_camera = float(x_camera) y_camera = float(y_camera) z_camera = float(z_camera) result = ( marker_found, x, y, z, x_camera, y_camera, z_camera, roll_marker, yaw_marker, pitch_marker, roll_marker, roll_camera, yaw_camera, pitch_camera, ) return result
def image_callback(msg): global g start_time = time.time() print("Received an image frame 1 !") global i global j global pos_x global pos_y global pos_z if i % 6 == 0: #skippping 6 frames try: # Convert your ROS Image message to OpenCV2 cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8") except CvBridgeError, e: print(e) else: QueryImg = cv2_img ret = True if ret == True: # grayscale image gray = cv2.cvtColor(QueryImg, cv2.COLOR_BGR2GRAY) # Detect Aruco markers # start_time = time.time() corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS) # Refine detected markers # Eliminates markers not part of our board, adds missing markers to the board #Outline all of the markers detected in our image QueryImg = aruco.drawDetectedMarkers(QueryImg, corners, borderColor=(0, 0, 255)) camcord = numpy.zeros((3, 6)) # Require 15 markers before drawing axis i=i+1 if ids is not None and len(ids) > 0: # Estimate the posture of the gridboard, which is a construction of 3Dtransformation=numpy.append(temp,tvec,axis=1) space based on the 2D video #pose, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, cameraMatrix, distCoeffs) #if pose: # # Draw the camera posture calculated from the gridboard # QreturnpovecueryImg = aruco.drawAxis(QueryImg, cameraMatrix, distCoeffs, rvec, tvec, 0.3) # Estimate the posture per each Aruco marker rvecs, tvecs, _ = aruco.estimatePoseSingleMarkers( corners, 0.07, cameraMatrix, distCoeffs) i = 0 arucopos = numpy.zeros((len(ids) + 1, 4)) for rvec, tvec in zip(rvecs, tvecs): povec = restPovec(ids[i]) QueryImg = aruco.drawAxis(QueryImg, cameraMatrix, distCoeffs, rvec, tvec, 0.1) cv2.imwrite("1.png", QueryImg) Rt, _Jacobian = cv2.Rodrigues(rvec) tvec = numpy.transpose(tvec[0]) tvec = tvec.reshape(3, 1) temp = numpy.concatenate(Rt) temp = temp.reshape(3, 3) transformation = numpy.append(temp, tvec, axis=1) append = numpy.array([0, 0, 0, 1]).reshape(1, 4) transformation = numpy.append(transformation, append, axis=0) transformation = numpy.linalg.inv(transformation) arucocord = numpy.matmul(transformation, numpy.array([0, 0, 0, 1])) cameracord = arucocord + povec # print(cameracord) arucopos[i, :] = povec i = i + 1 if i == 1: posavg = cameracord elif i == len(ids): posavg = cameracord + posavg posavg = posavg / i #output will hve homogenous plane coordinate =2, because cameracord=arucocord+povec, and we add two ones # print(posavg) pos_x = posavg[0] pos_y = posavg[1] pos_z = posavg[2] # Set up your subscriber and define its callback targetx = 0.562 targety = 1.5742 targetz = 1.6742 print("pos_x", pos_x, "pos_y", pos_y, "pos_z", pos_z) # if pos_x<targetx-0.2 or pos_x>targetx+0.2: print(g) if pos_z < targetz - 0.1 or pos_z > targetz + 0.1 and g == 0: print("move zzzz") # dy= move1(0.03, -0.01, 0, 0, 0, 0) else: if g == 0: print("delay 5 once") time.sleep(5) g = 1 elif pos_x < targetx - 0.1 or pos_x > targetx + 0.1: print("move XXX") move1(0.005, 0.01, 0, 0, 0, 0) g = 1 else: land() time.sleep(30) arucopos[i, :] = posavg else: posavg = cameracord + posavg i = 0 print("computation time", time.time() - start_time) print(i)
def detect_motion(frameCount): # grab global references to the video stream, output frame, and # lock variables global vs, outputFrame, lock # times = [] # loop over frames from the video stream while True: # Start time # start = time.time() ret, frame = vs.read() # operations on the frame gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # set dictionary size depending on the aruco marker selected aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) # detector parameters can be set here (List of detection parameters[3]) parameters = aruco.DetectorParameters_create() parameters.adaptiveThreshConstant = 10 # lists of ids and the corners belonging to each id corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) # font for displaying text (below) font = cv2.FONT_HERSHEY_SIMPLEX # check if the ids list is not empty # if no check is added the code will crash if np.all(ids != None): # estimate pose of each marker and return the values # rvet and tvec-different from camera coefficients rvec, tvec = aruco.estimatePoseSingleMarkers( corners, 0.05, mtx, dist) # (rvec-tvec).any() # get rid of that nasty numpy value array error for i in range(0, ids.size): # draw axis for the aruco markers aruco.drawAxis(frame, mtx, dist, rvec[i], tvec[i], 0.1) # draw a square around the markers aruco.drawDetectedMarkers(frame, corners) # code to show ids of the marker found strg = '' for i in range(0, ids.size): strg += str(ids[i][0]) + ', ' cv2.putText(frame, "Id: " + strg, (0, 64), font, 1, (0, 255, 0), 2, cv2.LINE_AA) else: # code to show 'No Ids' when no markers are found cv2.putText(frame, "No Ids", (0, 64), font, 1, (0, 255, 0), 2, cv2.LINE_AA) # end = time.time() # # times.append(end - start) # # if len(times) > 10: # times = times[:9] # # cv2.putText(frame, f"FPS: {round(len(times) / sum(times))}", (0, 100), font, 1, (0, 255, 0), 2, cv2.LINE_AA) # acquire the lock, set the output frame, and release the # lock with lock: outputFrame = frame.copy()
client.loop(WORKER_TIME) while RUN: # client.loop(WORKER_TIME) ret, img = stream.read() gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) if CHECKPOINT: dst_pt = generate_dst_pt(width, height) CHECKPOINT = False draw_dst_pt(img, dst_pt) corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=parameters) if np.all(ids != None): rvec, tvec, _ = aruco.estimatePoseSingleMarkers( corners, 0.05, mtx, dist) # Estimate pose of each marker for id in ids: direction_point, cntr = detect_direction(corners[0]) CHECKPOINT = is_it_checkpoint(cntr, dst_pt) direction_point = find_direction_point(direction_point, cntr) abs_error_angle = find_angle_error(direction_point, cntr, dst_pt) error_angle = find_angle_direction(direction_point, cntr, dst_pt, abs_error_angle) cv2.line(img, direction_point, direction_point, (150, 0, 255), 1) cv2.line(img, cntr, dst_pt, (30, 255, 15), 1) cv2.line(img, cntr, direction_point, (250, 180, 140), 2) cv2.circle(img, direction_point, 1, (200, 55, 255), 15, cv2.LINE_8) cv2.putText(img, str(error_angle), (25, 25), font, 0.5,
bytes='' while True: bytes+=stream.read(1024) a = bytes.find('\xff\xd8') b = bytes.find('\xff\xd9') if a!=-1 and b!=-1: jpg = bytes[a:b+2] bytes= bytes[b+2:] image = cv2.imdecode(np.fromstring(jpg, dtype=np.uint8),cv2.IMREAD_COLOR) markerCorners, markerIds, rejectedImgPoints = aruco.detectMarkers(image, dictionary, cameraMatrix=cameraMatrix, distCoeff=distCoeff) if len(markerCorners) > 0: image = aruco.drawDetectedMarkers(image, markerCorners, markerIds) rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(markerCorners, markerLength, cameraMatrix, distCoeff) for rvec, tvec in zip(rvecs, tvecs): image = aruco.drawAxis(image, cameraMatrix, distCoeff, rvec, tvec, length) ''' valid, rvec, tvec = aruco.estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeff) if valid: image = aruco.drawAxis(image, cameraMatrix, distCoeff, rvec, tvec, length) ''' else: font = cv2.FONT_HERSHEY_SIMPLEX cv2.putText(image,'No Markers Found',(30,250), font, 2, (255,255,255),2,cv2.LINE_AA) cv2.imshow('', image) cv2.waitKey(1)
def main(): ser = serial.Serial('COM3', 115200, timeout=0) print(ser.name) # targetState = (20,80,0) robotNode = 1 robotTargetLoc1 = (80, 20) robotMap = [] # robotLocation2 = (60,70) globalMap = initializeGraph() mapping.plotGraph(robotMap, [robotTargetLoc1]) performRobotWriting = True #Holds the values of the four corners of the robotic environment. Goes clockwise starting with the top left corner #The ids for the aruco tags for each of the four corners goes from 0 to 3 clockwise, starting with the top left corner fourCorners = [[], [], [], []] vc = cv2.VideoCapture(1) # np.load("sample_images.npz") with np.load("sample_images.npz") as data: mtx = data['mtx'] dist = data['dist'] # ret,frame = vc.read() # image = frame # time.sleep(1) # ret,frame = vc.read() # image = frame aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) parameters = aruco.DetectorParameters_create() iter = 0 node = 1 x = '' y = '' a = '' initialized = False initialxya = [] iter = 0 while (vc.isOpened()): # if iter % 90 == 0: # plt.pause(0.001) iter = (iter + 1) % 25 ret, frame = vc.read() image = frame # image = cv2.imread("52814747.png") # image = cv2.imread("arucoTestImage.png") # aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) corners, ids, rejectedImgPoints = aruco.detectMarkers( image, aruco_dict, parameters=parameters) rvec, tvec, _ = aruco.estimatePoseSingleMarkers( corners, 0.1, mtx, dist) rotations = [] if ids is not None: for i in range(len(ids)): if ids[i] == 5: if rvec[i][0][0] > 0: # rvec[i][0][0] = -1 * rvec[i][0][0] image = aruco.drawAxis(image, mtx, dist, rvec[i], tvec[i], 0.1) a = rvec[i][0][1] # print(rvec[i]) a = objectLocalization.convertAtoRadians(a) # if ids[i] < 5: # print("Drawing ID: " + str(ids[i]) ) # print("before") # print(rvec[i]) # image = aruco.drawAxis(image,mtx,dist,rvec[i],tvec[i],0.1) # print("after") if ids is not None: for i in range(len(ids)): if ids[i] == 0: fourCorners[0] = objectLocalization.getRectMid( corners[i][0]) elif ids[i] == 1: fourCorners[1] = objectLocalization.getRectMid( corners[i][0]) elif ids[i] == 2: fourCorners[2] = objectLocalization.getRectMid( corners[i][0]) elif ids[i] == 3: fourCorners[3] = objectLocalization.getRectMid( corners[i][0]) elif ids[i] == 5: if (len(fourCorners[0]) > 0 and len(fourCorners[1]) > 0 and len(fourCorners[2]) > 0 and len(fourCorners[3]) > 0): [x, y] = objectLocalization.scalePoint( fourCorners, objectLocalization.getRectMid(corners[i][0])) [x, y] = objectLocalization.convertToRobotLocation(x, y) print(fourCorners) # print(corners) # print(ids) # print(corners, ids, rejectedImgPoints) #NOTE: Two lines below used to draw markers originally # aruco.drawDetectedMarkers(image, corners, ids) # aruco.drawDetectedMarkers(image, rejectedImgPoints, borderColor=(100, 0, 240)) cv2.imshow('Video', image) key = cv2.waitKey(20) if key == 27: # exit on ESC break # iter = 0 # while (iter < 5): temp = ser.read(1000) # temp = ser.readline() if temp != b'': if temp == b'q' or temp == 'Q': ser.close() break # print(temp) # if temp[0] == '$': if len(temp) > 0: RFIDinfo = parseRFIDInfo(temp) print("RFID Info: ") print(RFIDinfo) # print("RFIDInfo: " + str(RFIDinfo)) if RFIDinfo != None: if mapping.euclideanDistance( (x, y), (RFIDinfo[0][0], RFIDinfo[0][1]) ) < 10 and mapping.euclideanDistance( (x, y), robotTargetLoc1 ) < 10: #Make sure we're in the range of the tag we think we are (newTargetLoc, info, preprocessedMap ) = mapping.updateMapandDetermineNextStep( robotMap, globalMap, robotTargetLoc1, RFIDinfo) print("------------------" + str(newTargetLoc) + "---------------------") if performRobotWriting: message = prepRFIDInfo(robotTargetLoc1, info, preprocessedMap) message = message + "\r\n" ser.write(str.encode(message)) print("***********Writing Message*************") print(message) ser.flush() mapping.plotGraph(preprocessedMap, [robotTargetLoc1]) robotTargetLoc1 = newTargetLoc # mapping.plotGraph(robotMap,[robotTargetLoc1]) # if len(code) >= len(shortestSample): # # if not xVals and not yVals: # tempState = (int(xVals),int(yVals),0) # if tempState != (0,0,0): # targetState = tempState # print("------------- NEW TARGET STATE ----------") # print(targetState) first = True job = multiprocessing.Process(target=plotMap, args=(robotMap, (x, y))) # time.sleep(0.1) # print(x == '') # print(y == '') # print(a) # time.sleep(0.05) if (x != '' and y != '' and a != ''): # print("x: " + str(x)) # print("y: " + str(y)) # # plt.pause(0.001) # job.start() # job.join() # if iter % 45 == 0: # job.start() # mapping.plotGraph(robotMap,[(x,y)]) # plotMap(robotMap,(x,y)) # if first: # job.start() if ( len(initialxya) < 10 ): #looking to find the average of the first several measurements to get an accurate starting point initialxya.append((x, y, a)) else: if not initialized: avex = 0 avey = 0 avea = 0 #NOTE: Possible bug could occur if the robot is aligned along 0 degrees since this could fluctuate #between 0 and 2 pi for values (resulting in an average of pi, the exact oppposite direction) for i in range(10): avex = avex + initialxya[i][0] avey = avey + initialxya[i][1] avea = avea + initialxya[i][2] avex = avex / len(initialxya) avey = avey / len(initialxya) avea = avea / len(initialxya) prevxya = (avex, avey, avea) prevxya2 = (avex, avey, avea) initialized = True # print(initialxya) previouslyMissed = False falsePrev = prevxya else: # if withinRange(a,prevxya2[2], math.pi * 1/ 2) or (withinRange(a,falsePrev[2],math.pi * 1 /2) and previouslyMissed): #Make sure the angle doesn't flip an unreasonable amount # if withinRange(a,prevxya2[2], math.pi * 1/ 2) or (withinRange(a,falsePrev[2],math.pi * 1 /2) and previouslyMissed): #Make sure the angle doesn't flip an unreasonable amount # print("Target State: " + str(targetState)) # print("A: " + str(a)) if (True): # if (not withinRange(a,prevxya[2] - math.pi,math.pi*4/8)): # if (withinRange(a,prevxya2[2], math.pi * 3/ 4) and withinRange(a,prevxya[2], math.pi * 2/4)) or (previouslyMissed and withinRange(a,falsePrev[2],math.pi * 1 /8)): #3/4 1/2Make sure the angle doesn't flip an unreasonable amount message = robotControl.prepareControlMessage( (x, y, a), robotTargetLoc1) message = "$" + str(robotNode) + str( message) + '\r\n' #preappend the robot node number ser.write(str.encode(message)) # print(message) ser.flush() # if (withinRange(a,falsePrev[2],math.pi * 1 /2) and previouslyMissed): # prevxya2 = falsePrev # else: # prevxya2 = prevxya prevxya2 = prevxya prevxya = (x, y, a) # print(initialxya) # print((avex,avey,avea)) # ser.close() # time.sleep(0.1) # print("Just flushed") else: # print("MISSED IT") # print(prevxya) # print(prevxya2) # print(x) # print(y) # print("A: " + str(a)) falsePrev = (x, y, a) previouslyMissed = True cv2.waitKey(0) cv2.destroyAllWindows() message = "$" + str( robotNode) + "f 0\r\n" #preappend the robot node number ser.write(str.encode(message)) ser.close()
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) if ids is not None: rvec = np.zeros((ids.shape[0], 3)) tvec = np.zeros((ids.shape[0], 3)) gray = aruco.drawDetectedMarkers(gray, corners, borderColor=(0, 255, 0)) cameraMatrix = np.array([[631.43076033, 0, 319.46293155],[ 0, 630.14567027, 241.75853213],[ 0, 0, 1, ]]) distCoeffs = np.array([[ 0.06375513, -0.05092762, 0.00133275, -0.00200469, -0.56225284]]) #print(cameraMatrix) ids = ids.reshape(1,-1) #print([np.array(corners[0][0])]) for i in range(ids.shape[1]): cor = [np.array(corners[i][0])] if(ids[0,i]==CT_id): rvec[i], tvec[i], _ = aruco.estimatePoseSingleMarkers(cor, CT_t, cameraMatrix, distCoeffs, rvec[i], tvec[i]) if(ids[0,i]==TR_id): rvec[i], tvec[i], _ = aruco.estimatePoseSingleMarkers(cor, TR_t, cameraMatrix, distCoeffs, rvec[i], tvec[i]) if(ids[0,i]==TL_id): rvec[i], tvec[i], _ = aruco.estimatePoseSingleMarkers(cor, TL_t, cameraMatrix, distCoeffs, rvec[i], tvec[i]) if(ids[0,i]==BR_id): rvec[i], tvec[i], _ = aruco.estimatePoseSingleMarkers(cor, BR_t, cameraMatrix, distCoeffs, rvec[i], tvec[i]) if(ids[0,i]==BL_id): rvec[i], tvec[i], _ = aruco.estimatePoseSingleMarkers(cor, BL_t, cameraMatrix, distCoeffs, rvec[i], tvec[i]) #print(tvec[i]) #print(rvec) #print(tvec)
def detect(): if len(sys.argv) < 2: with_feed = False elif sys.argv[1] == '-withfeed': with_feed = True else: with_feed = False RESOLUTION = 90 # Use this to set the resolution for video feed DEADZONE_RIGHT = 1.45 DEADZONE_LEFT = -DEADZONE_RIGHT cap = cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_WIDTH, RESOLUTIONS[RESOLUTION][0]) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, RESOLUTIONS[RESOLUTION][1]) corners = np.array([[0, 0]] * 4) # Length of the AR Tag # In meters marker_length = 0.06 marker_size = 0.01 ar_dict = ar.Dictionary_get(ar.DICT_6X6_250) parameters = ar.DetectorParameters_create() calibration_file = "calibration.xml" calibration_params = cv2.FileStorage(calibration_file, cv2.FILE_STORAGE_READ) camera_matrix = calibration_params.getNode("cameraMatrix").mat() dist_coeffs = calibration_params.getNode("distCoeffs").mat() ret, frame = cap.read() size = frame.shape 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') while True: ret, frame = cap.read() size = frame.shape picture = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) corners, ids, rejected_img_points = ar.detectMarkers( picture, ar_dict, parameters=parameters) if with_feed: picture = ar.drawDetectedMarkers(picture, corners) if len(corners) == 0: continue # A tag is detected print('Corners:', corners[0]) # Get rotation and translation vectors rvec, tvec, _ = ar.estimatePoseSingleMarkers(corners[0], marker_length, camera_matrix, dist_coeffs) object_x = tvec[0][0][0] object_z = tvec[0][0][2] print('X Distance', object_x) print('Z Distance', object_z) direction = get_direction(object_x, object_z) print('Direction: ', direction) if DEADZONE_LEFT < direction < 0: print('Turning Left') elif 0 < direction < DEADZONE_RIGHT: print('Turning Right') else: print('Going Straight') if with_feed: picture = ar.drawAxis(picture, camera_matrix, dist_coeffs, rvec, tvec, marker_size) cv2.imshow('frame', picture) if cv2.waitKey(1) & 0xFF == ord('q'): break cap.release()
def lander(): global first_run, notfound_count, found_count, marker_size, start_time, vs global p, i, d, pid, marker_size if first_run == 0: print("First run of lander!!") first_run = 1 start_time = time.time() frame = vs.read() frame = cv2.resize(frame, (horizontal_res, vertical_res)) frame_np = np.array(frame) gray_img = cv2.cvtColor(frame_np, cv2.COLOR_BGR2GRAY) ids = '' corners, ids, rejected = aruco.detectMarkers(image=gray_img, dictionary=aruco_dict, parameters=parameters) print("DRONE IS IN THE LAND MODE - checking in loop") if vehicle.mode != 'LAND': vehicle.mode = VehicleMode("LAND") while vehicle.mode != 'LAND': print('WAITING FOR DRONE TO ENTER LAND MODE') time.sleep(1) try: print('started TRY block') if ids is not None and ids[0] == id_to_find: print('I SAW IT') ret = aruco.estimatePoseSingleMarkers(corners, marker_size, cameraMatrix=cameraMatrix, distCoeffs=cameraDistortion) (rvec, tvec) = (ret[0][0, 0, :], ret[1][0, 0, :]) x = '{:.2f}'.format(tvec[0]) y = '{:.2f}'.format(tvec[1]) z = '{:.2f}'.format(tvec[2]) y_sum = 0 x_sum = 0 x_sum = corners[0][0][0][0] + corners[0][0][1][0] + corners[0][0][ 2][0] + corners[0][0][3][0] y_sum = corners[0][0][0][1] + corners[0][0][1][1] + corners[0][0][ 2][1] + corners[0][0][3][1] x_avg = x_sum * .25 y_avg = y_sum * .25 x_ang = (x_avg - horizontal_res * .5) * (horizontal_fov / horizontal_res) y_ang = (y_avg - vertical_res * .5) * (vertical_fov / vertical_res) ########### PID CONTROL ########## pid_x = PID(0.25, 0.5, 0.01, setpoint=x_ang) pid_y = PID(0.25, 0.5, 0.01, setpoint=y_ang) pid_x.setpoint = 0 pid_y.setpoint = 0 x_ang_control = pid_x(x_ang) y_ang_control = pid_y(y_ang) px, ix, dx = pid_x.components py, iy, dy = pid_y.components #PL COMTROL STUFF WITHOUT PID #F.send_land_message(x_ang,y_ang) #THIS IS THE MOST IMPORTANT THING HERE - PL WITH PID CONTROL send_land_message(-x_ang_control, -y_ang_control) ############################################################ print("X CENTER PIXEL: " + str(x_avg) + " Y CENTER PIXEL: " + str(y_avg)) print("FOUND COUNT: " + str(found_count) + " NOTFOUND COUNT: " + str(notfound_count)) print("MARKER POSITION: x=" + x + " y= " + y + " z=" + z) found_count = found_count + 1 print("") print(x_ang, y_ang) print(-x_ang_control, -y_ang_control) print("########################################################") return None else: notfound_count = notfound_count + 1 print("FOUND COUNT: " + str(found_count) + " NOTFOUND COUNT: " + str(notfound_count)) return None except Exception as e: print('Target likely not found. Error: ' + str(e)) return None
def detect_markers_and_draw(mtx, dist, frame, drawing_img): marker_size = 2.8/100 # marker length in meters axis_length = 0.015 # get dictionary and parameters parameters = aruco.DetectorParameters_create() # create parameters parameters.detectInvertedMarker = False # enable detection of inverted markers gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # convert to gray # get marker data corners, ids, rejected_img_points = aruco.detectMarkers(gray, ARUCO_DICT, parameters=parameters) # draw corners on frame frame_m = aruco.drawDetectedMarkers(frame, corners, ids) if len(corners) == 4: # get corners of initial image y_max = frame.shape[0] x_max = frame.shape[1] frame_corners = [[0, 0], [x_max, 0], [0, y_max], [x_max, y_max]] # get transform corners transform_corners = np.zeros((len(ids), 2)) for i in range(len(ids)): corner_num = ids[i][0] # get center x and y (calculating average) x, y = 0, 0 for j in range(4): x += corners[i][0][j][0] y += corners[i][0][j][1] # add center to transform matrix transform_corners[corner_num] = [x/4, y/4] # transform drawing pts1 = np.float32(frame_corners) pts2 = np.float32(transform_corners) M = cv2.getPerspectiveTransform(pts1, pts2) drawing_img = cv2.warpPerspective(drawing_img, M, (drawing_img.shape[1], drawing_img.shape[0])) # create mask gray_square = cv2.cvtColor(drawing_img, cv2.COLOR_BGR2GRAY) ret, mask = cv2.threshold(gray_square, 5, 255, cv2.THRESH_BINARY) mask_inv = cv2.bitwise_not(mask) # get background from frame # print(f'frame_m shape {frame_m.shape}') # print(f'mask shape {mask_inv.shape}') frame_bg = cv2.bitwise_and(frame_m, frame_m, mask=mask_inv) # take only drawing from drawing image drawing_img = cv2.bitwise_and(drawing_img, drawing_img, mask=mask) frame_m = cv2.add(frame_bg, drawing_img) # point_1 = (int(corners[0][0][0][0]), (corners[0][0][0][1])) # point_2 = (int(corners[1][0][0][0]), (corners[1][0][0][1])) # cv2.line(square_img, point_1, point_2, (255, 0, 0), 3) # estimate pose rvecs, tvecs, _objPoints = aruco.estimatePoseSingleMarkers(corners, marker_size, mtx, dist) if tvecs is not None: for i in range(len(tvecs)): frame_m = aruco.drawAxis(frame_m, mtx, dist, rvecs[i], tvecs[i], axis_length) return frame_m, rvecs, tvecs
#-- Find all the aruco markers in the image #corners, ids, rejected = aruco.detectMarkers(image=gray, dictionary=aruco_dict, parameters=parameters, cameraMatrix=camera_matrix, distCoeff=camera_distortion) corners, ids, rejected = aruco.detectMarkers(image=gray, dictionary=aruco_dict, parameters=parameters) if ids is not None and ids[0] == id_to_find: droneDetected = True detectionCount += 1 framesWithoutDrone = 0 #-- ret = [rvec, tvec, ?] #-- array of rotation and position of each marker in camera frame #-- rvec = [[rvec_1], [rvec_2], ...] attitude of the marker respect to camera frame #-- tvec = [[tvec_1], [tvec_2], ...] position of the marker in camera frame ret = aruco.estimatePoseSingleMarkers(corners, marker_size, camera_matrix, camera_distortion) #-- Unpack the output, get only the first rvec, tvec = ret[0][0,0,:], ret[1][0,0,:] #-- Draw the detected marker and put a reference frame over it aruco.drawDetectedMarkers(frame, corners) #aruco.drawAxis(frame, camera_matrix, camera_distortion, rvec, tvec, 10) #-- Obtain the rotation matrix tag->camera R_ct = np.matrix(cv2.Rodrigues(rvec)[0]) R_tc = R_ct.T #-- Get the attitude in terms of euler 321 (Needs to be flipped first) roll_marker, pitch_marker, yaw_marker = rotationMatrixToEulerAngles(R_flip*R_tc)
parameters = aruco.DetectorParameters_create() parameters.adaptiveThreshConstant = 10 # lists of ids and the corners belonging to each id corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) # font for displaying text (below) font = cv2.FONT_HERSHEY_SIMPLEX # check if the ids list is not empty # if no check is added the code will crash if np.all(ids != None): # estimate pose of each marker and return the values # rvet and tvec-different from camera coefficients rvec, tvec ,_ = aruco.estimatePoseSingleMarkers(corners, marker_length, mtx, dist) #(rvec-tvec).any() # get rid of that nasty numpy value array error if rvec is None: continue for coor in range(rvec.shape[0]): for i in range(0, ids.size): # draw axis for the aruco markers print(tvec[i]) cv2.putText(frame, "tvec" + "{0:.2f}".format(tvec[i][0][0])+" "+ "{0:.2f}".format(tvec[i][0][1])+ " " +"{0:.2f}".format(tvec[i][0][2]),(0,32), font, 1, (0,255,0),2,cv2.LINE_AA) #cv2.imwrite('raw/'+str(detections) + "good.png", frame) aruco.drawAxis(frame, mtx, dist, rvec[i], tvec[i], 0.1) #cv2.imwrite('axis/'+str(detections) + "with_axis.png", frame) detections += 1 # draw a square around the markers aruco.drawDetectedMarkers(frame, corners)
def findMarker(self): self.rvecs_arr = np.zeros((100, 3, NUMBER)) self.tvecs_arr = np.zeros((100, 3, NUMBER)) QueryImg = None for order in range(NUMBER): # Capturing each frame of our video stream ret, QueryImg = self.cam.read() if ret == True: # grayscale image gray = cv2.cvtColor(QueryImg, cv2.COLOR_BGR2GRAY) # Detect Aruco markers corners, ids, _ = aruco.detectMarkers( gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS) # Refine detected markers # Eliminates markers not part of our board, adds missing markers to the board # corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers( # image = gray, # board = board, # detectedCorners = corners, # detectedIds = ids, # rejectedCorners = rejectedImgPoints, # self.cameraMatrix = self.cameraMatrix, # self.distCoeffs = self.distCoeffs) ########################################################################### # TODO: Add validation here to reject IDs/corners not part of a gridboard # ########################################################################### # Outline all of the markers detected in our image QueryImg = aruco.drawDetectedMarkers(QueryImg, corners, borderColor=(0, 0, 255)) # Require 15 markers before drawing axis if ids is not None and len(ids) > 0: # Estimate the posture of the gridboard, which is a construction of 3D space based on the 2D video #pose, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, self.cameraMatrix, self.distCoeffs) #if pose: # # Draw the camera posture calculated from the gridboard # QueryImg = aruco.drawAxis(QueryImg, self.cameraMatrix, self.distCoeffs, rvec, tvec, 0.3) # Estimate the posture per each Aruco marker self.rvecs, self.tvecs, _ = aruco.estimatePoseSingleMarkers( corners, 0.02, self.cameraMatrix, self.distCoeffs) for _id, rvec, tvec in zip(ids, self.rvecs, self.tvecs): _id = _id[0] for i in range(3): self.rvecs_arr[_id][i][order] = rvec[0][i] self.tvecs_arr[_id][i][order] = tvec[0][i] # QueryImg = aruco.drawAxis(QueryImg, self.cameraMatrix, self.distCoeffs, rvec, tvec, 0.02) # Display our image # cv2.imshow('QueryImage', QueryImg) cv2.waitKey(1) # print('self.rvecs_arr = ', self.rvecs_arr) # print('self.tvecs_arr = ', self.tvecs_arr) result = np.array(()) r_avg = np.zeros(3) t_avg = np.zeros(3) for _id in range(100): ra = self.rvecs_arr[_id][0].nonzero() if len(ra[0]) < 0.3 * NUMBER: continue rb = self.rvecs_arr[_id][1].nonzero() rc = self.rvecs_arr[_id][2].nonzero() tx = self.tvecs_arr[_id][0].nonzero() ty = self.tvecs_arr[_id][1].nonzero() tz = self.tvecs_arr[_id][2].nonzero() ra = self.rvecs_arr[_id][0][ra] rb = self.rvecs_arr[_id][1][rb] rc = self.rvecs_arr[_id][2][rc] tx = self.tvecs_arr[_id][0][tx] ty = self.tvecs_arr[_id][1][ty] tz = self.tvecs_arr[_id][2][tz] ra = np.sort(ra, kind='quicksort') rb = np.sort(rb, kind='quicksort') rc = np.sort(rc, kind='quicksort') tx = np.sort(tx, kind='quicksort') ty = np.sort(ty, kind='quicksort') tz = np.sort(tz, kind='quicksort') r = np.array((ra, rb, rc)) t = np.array((tx, ty, tz)) ctn = False for i in range(3): rv, tv = r[i], t[i] while np.std(rv) > 0.01 and len(rv) >= NUMBER * 0.2: if abs(rv[0] - np.average(rv)) > abs(rv[-1] - np.average(rv)): rv = np.delete(rv, 0) else: rv = np.delete(rv, -1) while np.std(tv) > 0.01 and len(tv) >= NUMBER * 0.2: if abs(tv[0] - np.average(tv)) > abs(tv[-1] - np.average(tv)): tv = np.delete(tv, 0) else: tv = np.delete(tv, -1) if len(rv) < NUMBER * 0.2 or len(tv) < NUMBER * 0.2: ctn = True break r_avg[i] = np.average(rv) t_avg[i] = np.average(tv) if ctn: continue # print('[_id, r,t] = ', [_id, r,t]) result = np.append(result, [_id, np.copy(r_avg), np.copy(t_avg)]) result = result.reshape(int(len(result) / 3), 3) # print(result) for rst in result: QueryImg = aruco.drawAxis(QueryImg, self.cameraMatrix, self.distCoeffs, rst[1], rst[2], 0.02) cv2.imshow('QueryImage', QueryImg) return result
camera.set(cv2.CAP_PROP_FRAME_WIDTH,1280); camera.set(cv2.CAP_PROP_FRAME_HEIGHT,960); plt.figure() cam_matrix = pickle.load(open("cam_matrix.p","rb")) dist_matrix = pickle.load(open("dist_matrix.p","rb")) while True: retval, frame = camera.read() gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY) print(gray.shape) aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250) parameters = aruco.DetectorParameters_create() corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict,parameters=parameters) if (len(corners) > 0): rot_vec, trans_vec, _ = aruco.estimatePoseSingleMarkers(corners,.1016,cam_matrix,dist_matrix); axis = np.float32([[4,0,0],[0,4,0],[0,0,-4]]).reshape(-1,3) imgpts, jac = cv2.projectPoints(axis,rot_vec,trans_vec,cam_matrix,dist_matrix); print(imgpts) frame = aruco.drawAxis(frame,cam_matrix,dist_matrix,rot_vec,trans_vec,.1) #frame = draw(frame,corners,imgpts) print("Rotation Vector:" ) print(rot_vec) print ("translation vector: ") print(trans_vec) frame = cv2.putText(frame,"X: " + str(trans_vec[0][0][0]) + " Y: " + str(trans_vec[0][0][1]) + " Z: " + str(trans_vec[0][0][2]),(30,30),cv2.FONT_HERSHEY_SIMPLEX,1,(255,0,0),4) frame_markers = aruco.drawDetectedMarkers(frame.copy(),corners,ids) cv2.imshow("live video", frame_markers) cv2.waitKey(1)