def draw(self): # Get the ROI. roi = self.image[self.roiPt0[1]:self.roiPt1[1], self.roiPt0[0]:self.roiPt1[0]] # Draw detected markers. aruco.drawDetectedMarkers(roi, self.corners) # Put the ROI back. self.image[self.roiPt0[1]:self.roiPt1[1], self.roiPt0[0]:self.roiPt1[0]] = roi #Draw the detected Charuco corners. aruco.drawDetectedCornersCharuco(self.image, self.charucoCorners, cornerColor=self.COLOR_BLUE) #Draw the detected calibration points. for idx, corners in enumerate(self.calibrationCorners): #print idx #aruco.drawDetectedCornersCharuco(self.image, self.calibrationCorners[idx], self.calibrationIds[idx], self.COLOR_LBLUE) aruco.drawDetectedCornersCharuco(self.image, self.calibrationCorners[idx], cornerColor=self.COLOR_LBLUE) return #Draw region of interest cv2.rectangle(self.image, self.roiPt0, self.roiPt1, self.COLOR_PURPLE, 2)
def detect_board(): vs = VideoStream(usePiCamera=True, resolution=res).start() fps = FPS().start() time.sleep(1) freq = 0 i=0 target_x = [] target_y = [] while(i<5000): tf = time.time() # Capture frame-by-frame frame = vs.read() # 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)) status = "No Marker Detected" position_dict = {} corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters) if ids is not None: ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco(corners, ids, gray, board ) # if there are enough corners to get a reasonable result if( ret > 5 ): aruco.drawDetectedCornersCharuco(frame,ch_corners,ch_ids,(0,0,255) ) retval, rvec, tvec = aruco.estimatePoseCharucoBoard( ch_corners, ch_ids, board, camera_matrix, dist_coeff ) # if a pose could be estimated if( retval ) : frame = aruco.drawAxis(frame,camera_matrix,dist_coeff,rvec,tvec,0.1) rmat = cv2.Rodrigues(rvec)[0] position_camera = -np.matrix(rmat).T * np.matrix(tvec) position_dict[ids[0][0]] = position_camera status = ("x: %.2f, y: %.2f, z: %.2f")%(position_camera[0],position_camera[1],position_camera[2]) # Display the resulting frame cv2.putText(frame, status, (20,30), cv2.FONT_HERSHEY_SIMPLEX, .5, (0,0,255),2) out.write(frame) #cv2.imshow('frame',gray) #if cv2.waitKey(1) & 0xFF == ord('q'): # break i += 1 freq = .7*freq + .3 * (1/(time.time()-tf)) fps.update() #print("Operating frequency: %.2f Hz"%freq) # When everything done, release the capture out.release() vs.stop() cv2.destroyAllWindows()
def calib_charucoboard(nrow, ncolumn, aruco_markerdict=aruco.DICT_6X6_250, square_markersize=25, imgs_path='./', img_format='png', save_name='mycam_charuco_data.yaml'): """ :param nrow: :param ncolumn: :param marker_dict: :param imgs_path: :param save_name: :return: author: weiwei date: 20190420 """ # read images and detect cornders aruco_dict = aruco.Dictionary_get(aruco_markerdict) allCorners = [] allIds = [] # SUB PIXEL CORNER DETECTION CRITERION criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 27, 0.0001) board = aruco.CharucoBoard_create(ncolumn, nrow, square_markersize, .57 * square_markersize, aruco_dict) print(imgs_path) images = glob.glob(imgs_path + '*.' + img_format) candfiles=[] for fname in images: img = cv2.imread(fname) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, aruco_dict) if len(corners) > 0: # SUB PIXEL DETECTION for corner in corners: cv2.cornerSubPix(gray, corner, winSize=(2, 2), zeroZone=(-1, -1), criteria=criteria) res2 = cv2.aruco.interpolateCornersCharuco(corners, ids, gray, board) # require len(res2[1]) > nrow*ncolumn/2 at least half of the corners are detected if res2[1] is not None and res2[2] is not None and len(res2[1]) > (nrow-1)*(ncolumn-1)/2: allCorners.append(res2[1]) allIds.append(res2[2]) candfiles.append((fname.split("/")[-1]).split("\\")[-1]) imaxis = aruco.drawDetectedMarkers(img, corners, ids) imaxis = aruco.drawDetectedCornersCharuco(imaxis, res2[1], res2[2], (255,255,0)) cv2.imshow('img', imaxis) cv2.waitKey(100) # The calibratedCameraCharucoExtended function additionally estimate calibration errors # Thus, stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors are returned # We dont use them here though # see https://docs.opencv.org/3.4.6/d9/d6a/group__aruco.html for details (ret, mtx, dist, rvecs, tvecs, stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors) = \ cv2.aruco.calibrateCameraCharucoExtended(charucoCorners=allCorners, charucoIds=allIds, board=board, imageSize=gray.shape, cameraMatrix=None, distCoeffs=None, flags=cv2.CALIB_RATIONAL_MODEL, criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9)) print(ret, mtx, dist, rvecs, tvecs, candfiles) if ret: with open(save_name, "w") as f: yaml.dump([mtx, dist, rvecs, tvecs, candfiles], f)
def calibrate_charucoboard(self, image, display_image): # ChAruco board variables CHARUCOBOARD_ROWCOUNT = 5 CHARUCOBOARD_COLCOUNT = 5 ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_50) SQUARE_LENGTH = (1 + 5.0 / 8) * 0.0254 MARKER_LENGTH = (1 + 9.0 / 32) * 0.0254 # Create constants to be passed into OpenCV and Aruco methods print(CHARUCOBOARD_COLCOUNT > 1, CHARUCOBOARD_ROWCOUNT > 1, SQUARE_LENGTH > MARKER_LENGTH) CHARUCO_BOARD = aruco.CharucoBoard_create( squaresX=CHARUCOBOARD_COLCOUNT, squaresY=CHARUCOBOARD_ROWCOUNT, squareLength=SQUARE_LENGTH, markerLength=MARKER_LENGTH, dictionary=ARUCO_DICT) # Create the arrays and variables we'll use to store info like corners and IDs from images processed corners_all = [] # Corners discovered in all images processed ids_all = [] # Aruco ids corresponding to corners discovered image_size = None # Determined at runtime # Grayscale the image gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) # Find aruco markers in the query image corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=ARUCO_DICT) # Outline the aruco markers found in our query image display_image = aruco.drawDetectedMarkers(image=display_image, corners=corners) # Get charuco corners and ids from detected aruco markers response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( markerCorners=corners, markerIds=ids, image=gray, board=CHARUCO_BOARD) # If a Charuco board was found, let's collect image/corner points # Requiring at least 20 squares if response > 5: # Add these corners and ids to our calibration arrays corners_all.append(charuco_corners) ids_all.append(charuco_ids) # Draw the Charuco board we've detected to show our calibrator the board was properly detected display_image = aruco.drawDetectedCornersCharuco( image=display_image, charucoCorners=charuco_corners, charucoIds=charuco_ids) return [], corners_all, display_image else: print("Not able to detect a charuco board in image") return [], [], display_image
def callback(self, data): try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) (rows, cols, channels) = cv_image.shape if cols > 60 and rows > 60: corners, ids, rejectedImgPoints = aruco.detectMarkers( cv_image, self.aruco_dict, parameters=self.parameters) #print("aruco") #print(corners) if len(corners) > 0: retval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco( corners, ids, cv_image, self.board) #print(diamondCorners) img = aruco.drawDetectedCornersCharuco(cv_image, charucoCorners) if self.camera_info_read and (not charucoCorners is None): retval, rvec, tvec = aruco.estimatePoseCharucoBoard( charucoCorners, charucoIds, self.board, self.camera_matrix, self.dist_coeffs) #print(retval) if not rvec is None: img = aruco.drawAxis(img, self.camera_matrix, self.dist_coeffs, rvec, tvec, 0.1) marker_msg = MarkerArray() marker_msg.header = data.header marker_msg.markers = [] #print(corners) if not ids is None: if len(ids) != 0: for i in range(len(ids)): marker_msg.markers.append(Marker()) marker_msg.markers[i].id = int(ids[i]) marker_msg.markers[i].corners = [] for j in range((corners[i]).shape[1]): marker_msg.markers[i].corners.append(Point2D()) marker_msg.markers[i].corners[j].x = corners[i][0, j, 0] marker_msg.markers[i].corners[j].y = corners[i][0, j, 1] #cv2.imshow("Image window", cv_image) #cv2.waitKey(3) try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8")) self.marker_pub.publish(marker_msg) except CvBridgeError as e: print(e)
def findPointsAndSaveImages(img, img_save_prefex, img_nr): imgName = img_save_prefex+str(img_nr) cv2.imwrite(save_path+imgName+".png", img) corners, ids, rejected = aruco.detectMarkers(img, BoardInfo.charucoBoard.dictionary) cimg = aruco.drawDetectedMarkers(img.copy(), corners, ids) cv2.imwrite(save_path+"detected__"+imgName+".png", cimg) if len(ids) < 10: return cimg, None, None numCorners, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(corners, ids, img, BoardInfo.charucoBoard) cimg = aruco.drawDetectedCornersCharuco(img.copy(), charucoCorners, charucoIds) cv2.imwrite(save_path+"corners__"+imgName+".png", cimg) if numCorners <= 0: return cimg, None, None return cimg, charucoCorners, charucoIds
def get_corners(self, cv2_img, cam_id): gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY) corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=aruco_dict) if ids is not None and len(ids) > 5: _, Chcorners, Chids = aruco.interpolateCornersCharuco( corners, ids, gray, charuco_board) self.current_markers[cam_id] = len(Chcorners) corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=aruco_dict) _, Chcorners, Chids = aruco.interpolateCornersCharuco( corners, ids, gray, charuco_board) QueryImg = aruco.drawDetectedCornersCharuco( gray, Chcorners, Chids, (0, 0, 255)) # Display our image #cv2.imshow('QueryImage', QueryImg) print("{} markers detected".format(len(ids))) a = raw_input()
def est_board_pose(self, cv2_img): gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY) corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=aruco_dict) if ids is not None and len(ids) > 5: _, Chcorners, Chids = aruco.interpolateCornersCharuco( corners, ids, gray, charuco_board) retval, rvecs, tvecs = aruco.estimatePoseCharucoBoard( Chcorners, Chids, charuco_board, self.camera_matrix[:, :], self.distCoeffs[:, :]) ####################### X_C_M = np.eye(4) X_C_M[0, 3] = tvecs[0] X_C_M[1, 3] = tvecs[1] X_C_M[2, 3] = tvecs[2] R = cv2.Rodrigues(rvecs[:])[0] X_C_M[0:3, 0:3] = R X_C_E = X_C_M.dot(self.X_M_E) tvecs_new = X_C_E[:3, 3] rvecs_new = cv2.Rodrigues(X_C_E[0:3, 0:3])[0] # ##################### QueryImg = aruco.drawDetectedCornersCharuco( cv2_img, Chcorners, Chids, (0, 0, 255)) if (retval): QueryImg = aruco.drawAxis(cv2_img, self.camera_matrix[:, :], self.distCoeffs[:, :], rvecs, tvecs, 0.032) QueryImg = aruco.drawAxis(QueryImg, self.camera_matrix[:, :], self.distCoeffs[:, :], rvecs_new, tvecs_new, 0.032) # Display our image # cv2.imshow('QueryImage', QueryImg) # cv2.waitKey(0) else: rvecs = np.zeros((3, 1)) tvecs = np.zeros((3, 1)) cv2.imshow('QueryImage', cv2_img) print("board not detected") cv2.waitKey(1) #print("{} markers detected".format(len(ids))) self.tvecs = tvecs self.rvecs = rvecs
def manually_verify_board_detection(self, image, corners, ids=None): height, width = image.shape[:2] image = aruco.drawDetectedCornersCharuco(image, corners, ids) cv2.putText(image, '(a) Accept (d) Reject', (int(width / 1.35), int(height / 16)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, 255, 1, cv2.LINE_AA) cv2.imshow('verify_detection', image) while 1: key = cv2.waitKey(0) & 0xFF if key == ord('a'): cv2.putText(image, 'Accepted!', (int(width / 2.5), int(height / 1.05)), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2, cv2.LINE_AA) cv2.imshow('verify_detection', image) cv2.waitKey(100) return True elif key == ord('d'): cv2.putText(image, 'Rejected!', (int(width / 2.5), int(height / 1.05)), cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2, cv2.LINE_AA) cv2.imshow('verify_detection', image) cv2.waitKey(100) return False
def generateCalibrationImg(self, rows=7, columns=5, lengthSquare=0.0354, lengthMarker=0.0177): # Create charuco board with actual measured dimensions from print out board = aruco.CharucoBoard_create( squaresX=columns, squaresY=rows, squareLength=lengthSquare, markerLength=lengthMarker, dictionary=self.arucoDict) # Get image paths from calibration folder paths = glob.glob(self.calibrationDir + '*' + self.imgExtension) # Image list data = [] # Counter ii = 0 # Loop through all images for filePath in paths: # Read image and convert to gray img = cv2.imread(filePath) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Find aruco markers in the query image corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=self.arucoDict) # Outline the aruco markers found in the query image aruco.drawDetectedMarkers(image=img, corners=corners) # Get charuco corners and ids from detected aruco markers response, charucoCorners, charucoIDs = aruco.interpolateCornersCharuco( markerCorners=corners, markerIds=ids, image=gray, board=board) # If at least 20 corners were found if response > 20: # Draw the Charuco board detected to show calibration results aruco.drawDetectedCornersCharuco(image=img, charucoCorners=charucoCorners, charucoIds=charucoIDs) # Resize image and save to list data.append(img) # Incrememnt counter ii += 1 else: # Error message print('Error in: ' + str(filePath)) # Exit condition if (ii >= 9): break # Create matrix image A = np.concatenate((data[0], data[1], data[2], data[3]), axis=1) B = np.concatenate((data[4], data[5], data[6], data[7]), axis=1) D = np.concatenate((A, B), axis=0) # Display and save matrix image cv2.imshow('Photo Matrix', D) cv2.imwrite('PhotoMatrix.png', D) cv2.waitKey(0) cv2.destroyAllWindows()
def startcamerastreaming(self, c, ReturnFlag, n_sq_y, n_sq_x, squareLen, markerLen, aruco_dict): count = 0 # start realsense camera streaming pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) profile = pipeline.start(config) # get Realsense stream 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) #create charuco board with specified number of squares given marker length and square length and dictionary type. charuco_board = aruco.CharucoBoard_create(n_sq_x, n_sq_y, squareLen, markerLen, aruco_dict) rvec = np.zeros((1, 3)) # , np.float32) tvec = np.zeros((1, 3)) # , np.float32) calibDone = False MLRSSum = np.zeros((4, 4)) while (True): # start streaming from Realsense if calibDone != True: print("came here") # receive the marker 1 pose in AR headset space. dataRecv = c.recv(64) MLArr = np.frombuffer(dataRecv, dtype=np.float32) print(MLArr) frames = pipeline.wait_for_frames() # if the marker 1 pose is not None, proceed to get marker 1 pose in Realsense space if MLArr[3] != 0: color_frame = frames.get_color_frame() input_img = np.asanyarray(color_frame.get_data()) # cv2.imshow('input',input_img) # operations on the frame - conversion to grayscale gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY) # 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 (ids is not None): ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco( corners, ids, gray, charuco_board) # if there are enough corners to get a reasonable result if (ret > 3): aruco.drawDetectedCornersCharuco( input_img, ch_corners, ch_ids, (0, 0, 255)) retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard( ch_corners, ch_ids, charuco_board, mtx, dist, None, None) # if the board pose could be estimated, continue with remaining steps if (retval): print("board pose", rvec, tvec) rvec = np.reshape(rvec, (1, 3)) tvec = np.reshape(tvec, (1, 3)) print("board pose", rvec, tvec) # draw charuco board axis aruco.drawAxis(input_img, mtx, dist, rvec, tvec, 0.1) print(MLArr, rvec, tvec) # compute the calibration matrix between realsense space and AR headset space for the 'i'th sample of marker 1 pose in realsense and AR headset space. MLRSi = self.computecalibmatrix( MLArr, rvec, tvec, n_sq_y, n_sq_x, squareLen) print("MLRSi", MLRSi) # convert MLRSi to euler #R_M1_RS_reshape = np.array(MLRSi[:3,:3]).reshape([3,3]) R_M1_RS = R.from_rotvec(rvec) R_M1_RS_euler = R_M1_RS.as_euler('xyz', degrees=True) # save the MLRSi values in csv with open(file_name_TM1RS, 'a+', newline='') as write_obj: csv_writer = writer(write_obj) csv_writer.writerow( [R_M1_RS_euler, tvec.reshape([1, 3])]) MLRSSum = MLRSSum + MLRSi count = count + 1 cv2.imshow('current frame', input_img) #if the specified number of samples for homography are taken, compute the transformation matrix if count >= n_samples: print("count", count) MLRSFinal = MLRSSum / count #once n_samples homography matrices are computed, their average is computed and returned as the final transformation matrix from Realsense to AR headset space. print("MLRSFinal", MLRSFinal) print("calib done") calibDone = True if ReturnFlag == 1: return MLRSFinal if cv2.waitKey(1) & 0xFF == ord('q'): break # When everything done, release the capture #cap.release() cv2.destroyAllWindows()
def cal(): # ChAruco board variables CHARUCOBOARD_ROWCOUNT = 9 CHARUCOBOARD_COLCOUNT = 6 ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_250) # Create constants to be passed into OpenCV and Aruco methods CHARUCO_BOARD = aruco.CharucoBoard_create( squaresX=CHARUCOBOARD_COLCOUNT, squaresY=CHARUCOBOARD_ROWCOUNT, squareLength=0.028, markerLength=0.014, dictionary=ARUCO_DICT) # Create the arrays and variables we'll use to store info like corners and IDs from images processed corners_all = [] # Corners discovered in all images processed ids_all = [] # Aruco ids corresponding to corners discovered image_size = None # Determined at runtime # This requires a set of images or a video taken with the camera you want to calibrate # I'm using a set of images taken with the camera with the naming convention: # 'camera-pic-of-charucoboard-<NUMBER>.jpg' # All images used should be the same size, which if taken with the same camera shouldn't be a problem images = glob.glob(f'{FOLDER}/*.{IM_TYPE}') # Loop through images glob'ed for iname in images: # Open the image img = cv2.imread(iname) img = cv2.flip(img, 1) # Grayscale the image gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Find aruco markers in the query image corners, ids, _ = aruco.detectMarkers( image=gray, dictionary=ARUCO_DICT) # Outline the aruco markers found in our query image img = aruco.drawDetectedMarkers( image=img, corners=corners) # Get charuco corners and ids from detected aruco markers response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( markerCorners=corners, markerIds=ids, image=gray, board=CHARUCO_BOARD) # If a Charuco board was found, let's collect image/corner points # Requiring at least 20 squares if response > 20: # Add these corners and ids to our calibration arrays corners_all.append(charuco_corners) ids_all.append(charuco_ids) # Draw the Charuco board we've detected to show our calibrator the board was properly detected img = aruco.drawDetectedCornersCharuco( image=img, charucoCorners=charuco_corners, charucoIds=charuco_ids) # If our image size is unknown, set it now if not image_size: image_size = gray.shape[::-1] # Reproportion the image, maxing width or height at 1000 proportion = max(img.shape) / 1000.0 img = cv2.resize( img, (int(img.shape[1]/proportion), int(img.shape[0]/proportion))) # Pause to display each image, waiting for key press cv2.imshow('Charuco board', img) cv2.waitKey(0) else: print("Not able to detect a charuco board in image: {}".format(iname)) # Destroy any open CV windows cv2.destroyAllWindows() # Make sure at least one image was found if len(images) < 1: # Calibration failed because there were no images, warn the user print("Calibration was unsuccessful. No images of charucoboards were found. Add images of charucoboards and use or alter the naming conventions used in this file.") # Exit for failure exit() # Make sure we were able to calibrate on at least one charucoboard by checking # if we ever determined the image size if not image_size: # Calibration failed because we didn't see any charucoboards of the PatternSize used print("Calibration was unsuccessful. We couldn't detect charucoboards in any of the images supplied. Try changing the patternSize passed into Charucoboard_create(), or try different pictures of charucoboards.") # Exit for failure exit() # Now that we've seen all of our images, perform the camera calibration # based on the set of points we've discovered calibration, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( charucoCorners=corners_all, charucoIds=ids_all, board=CHARUCO_BOARD, imageSize=image_size, cameraMatrix=None, distCoeffs=None) return cameraMatrix, distCoeffs, rvecs, tvecs
frame, map1, map2, cv2.INTER_LINEAR, cv2.BORDER_CONSTANT) # for fisheye remapping frame_remapped_gray = cv2.cvtColor( frame_remapped, cv2.COLOR_BGR2GRAY) # aruco.detectMarkers() requires gray image corners, ids, rejectedImgPoints = aruco.detectMarkers( frame_remapped_gray, aruco_dict, parameters=arucoParams) # First, detect markers aruco.refineDetectedMarkers(frame_remapped_gray, board, corners, ids, rejectedImgPoints) if ids != None: # if there is at least one marker detected charucoretval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco( corners, ids, frame_remapped_gray, board) im_with_charuco_board = aruco.drawDetectedCornersCharuco( frame_remapped, charucoCorners, charucoIds, (0, 255, 0)) retval, rvec, tvec = aruco.estimatePoseCharucoBoard( charucoCorners, charucoIds, board, camera_matrix, dist_coeffs) # posture estimation from a charuco board if retval == True: im_with_charuco_board = aruco.drawAxis( im_with_charuco_board, camera_matrix, dist_coeffs, rvec, tvec, 100 ) # axis length 100 can be changed according to your requirement else: im_with_charuco_left = frame_remapped cv2.imshow("charucoboard", im_with_charuco_board) if cv2.waitKey(2) & 0xFF == ord('q'): break
def calibrate_camera(): # Generate Charuco board dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) squareSize = 0.03907 #0.03 markerSize = squareSize / 1.5 #0.02 board = aruco.CharucoBoard_create(7, 5, squareSize, markerSize, dictionary) img = board.draw((int(1000 * 1.414), 1000)) cv2.imwrite('charuco.png', img) cap = cv2.VideoCapture(2) # Camera resolution res = (int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)), int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))) ret, frame = cap.read() print(frame.shape) allCharucoCorners = [] allCharucoIds = [] frame_idx = 0 frame_spacing = 20 # Only check every fifth frame required_count = 60 success = False while True: # Capture and image ret, frame = cap.read() # Convert to gray scale grayframe = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Detect markers marker_corners, marker_ids, _ = aruco.detectMarkers( grayframe, dictionary) # If detected any markers if len(marker_corners) > 0 and frame_idx % frame_spacing == 0: aruco.drawDetectedMarkers(grayframe, marker_corners, marker_ids) # Detect corners ret, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( marker_corners, marker_ids, grayframe, board) #If detected any corners if charuco_corners is not None and charuco_ids is not None and len( charuco_corners) > 3: allCharucoCorners.append(charuco_corners) allCharucoIds.append(charuco_ids) # Draw detected corners aruco.drawDetectedCornersCharuco(grayframe, charuco_corners, charuco_ids) cv2.imshow('grayframe', grayframe) key = cv2.waitKey(1) & 0xFF if key == ord('q'): break frame_idx += 1 print("Found: " + str(len(allCharucoIds)) + "/" + str(required_count)) if len(allCharucoIds) > required_count: success = True break if success: print("Finished") try: err, camera_matrix, dist_coeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( allCharucoCorners, allCharucoIds, board, res, None, None) print('Calibrated with error', err) print(camera_matrix) save_json({ 'camera_matrix': camera_matrix.tolist(), 'dist_coeffs': dist_coeffs.tolist(), 'err': err }) except: success = False print("failed!") return # Generate the corrections new_camera_matrix, valid_pix_roi = cv2.getOptimalNewCameraMatrix( camera_matrix, dist_coeffs, res, 0) #new_camera_matrix = camera_matrix mapx, mapy = cv2.initUndistortRectifyMap(camera_matrix, dist_coeffs, None, new_camera_matrix, res, 5) while True: ret, frame = cap.read() if mapx is not None and mapy is not None: frame = cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR) cv2.imshow('frame', frame) if cv2.waitKey(1) & 255 == ord('q'): break cap.release() cv2.destroyAllWindows()
def callback(self, data): try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=ARUCO_DICT) if ids is not None: cv_image = aruco.drawDetectedMarkers(image=cv_image, corners=corners) response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( markerCorners=corners, markerIds=ids, image=gray, board=CHARUCO_BOARD) # Pause to display each image, waiting for key press cv2.imshow('Charuco board', cv_image) key = cv2.waitKey(1) if key & 0xFF == ord('q') or key == 27: rospy.loginfo('Total {} capture images for Calibration'.format( self.capturecount)) cv2.destroyAllWindows() rospy.signal_shutdown('Program terminate') elif key == ord('s'): if response > 20: # Add these corners and ids to our calibration arrays self.corners_all.append(charuco_corners) self.ids_all.append(charuco_ids) # Draw the Charuco board we've detected to show our calibrator the board was properly detected cv_image = aruco.drawDetectedCornersCharuco( image=cv_image, charucoCorners=charuco_corners, charucoIds=charuco_ids) # If our image size is unknown, set it now if not self.image_size: self.image_size = gray.shape[::-1] # Reproportion the image, maxing width or height at 1000 proportion = max(cv_image.shape) / 1000.0 cv_image = cv2.resize( cv_image, (int(cv_image.shape[1] / proportion), int(cv_image.shape[0] / proportion))) self.capturecount += 1 rospy.loginfo('Detected 20 markers and SAVE : {}'.format( self.capturecount)) else: cv2.imshow('Charuco board', cv_image) key = cv2.waitKey(1) if key & 0xFF == ord('q') or key == 27: rospy.loginfo('Total {} capture images for Calibration'.format( self.capturecount)) cv2.destroyAllWindows() rospy.signal_shutdown('Program terminate')
assert ret, "Cannot connect with camera" gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Find aruco markers in the query image corners, ids, _ = arc.detectMarkers(image=gray, dictionary=dictionary) # Outline the aruco markers found in our query image img = arc.drawDetectedMarkers(image=img, corners=corners) if len(corners) > 10: # Get charuco corners and ids from detected aruco markers response, charuco_corners, charuco_ids = arc.interpolateCornersCharuco( markerCorners=corners, markerIds=ids, image=gray, board=board) # Draw the Charuco board we've detected to show our calibrator the board was properly detected img = arc.drawDetectedCornersCharuco(image=img, charucoCorners=charuco_corners, charucoIds=charuco_ids) if cv2.waitKey(1) & 0xFF == ord('s'): # Add these corners and ids to our calibration arrays cornerpoints.append(charuco_corners) aruco_ids.append(charuco_ids) img_list.append(img) print("Saved image") image_count += 1 cv2.putText(img, f"Images taken: {image_count}", (10, 50), font, 0.8, (0, 0, 255), 2, cv2.LINE_AA) cv2.imshow( "Frame", cv2.resize(img, (int(0.6 * height), int(0.6 * width)),
def startTracking(kalman_flag, marker_type, MLRSTr, n_sq_y, n_sq_x, squareLen, markerLen, aruco_dict, socket_connect, c): RSTr = None img_idx = 0 img_sent = 0 reinit_thresh = 10 skip_frame_reinit = 120 #after every 150 frames, reinitialize detection skip_frame_send = 4 #if Charuco board is the marker type, execute following if statement if marker_type == 1: rvec = np.zeros((1, 3)) tvec = np.zeros((1, 3)) objectPoints = np.zeros((1, 3)) #configure Realsense camera stream pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) profile = pipeline.start(config) #get realsense stream intrinsics intr = profile.get_stream(rs.stream.color).as_video_stream_profile( ).get_intrinsics() # profile.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) while (True): #start streaming frames = pipeline.wait_for_frames() #get color frame color_frame = frames.get_color_frame() input_img = np.asanyarray(color_frame.get_data()) # operations on the frame - convert to grayscale gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY) img_idx = img_idx + 1 # detector parameters can be set here. 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 if marker_type == 1: # check if the ids list is not empty if (ids is not None): # create Charudo board charuco_board = aruco.CharucoBoard_create( n_sq_x, n_sq_y, squareLen, markerLen, aruco_dict) ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco( corners, ids, gray, charuco_board) # if there are enough corners to get a reasonable result if (ret > 3): aruco.drawDetectedCornersCharuco(input_img, ch_corners, ch_ids, (0, 0, 255)) #estimate Charuco board pose retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard( ch_corners, ch_ids, charuco_board, mtx, dist, None, None) # , useExtrinsicGuess=False) print("pose", retval, rvec, tvec) # if a pose could be estimated if (retval): # draw axis showing pose of board rvec = np.reshape(rvec, (1, 3)) tvec = np.reshape(tvec, (3, 1)) aruco.drawAxis(input_img, mtx, dist, rvec, tvec, 0.1) #convert to rotation matrix from rotation vector R_rvec = R.from_rotvec(rvec) R_rotmat = R_rvec.as_matrix() # get transformation matrix combining tvec and rotation matrix RSTr = np.hstack([R_rotmat.reshape((3, 3)), tvec]) RSTr = np.vstack([RSTr, [0, 0, 0, 1]]) print("RSTr", RSTr) else: # show 'No Ids' when no markers are found cv2.putText(input_img, "No Ids", (0, 64), font, 1, (0, 255, 0), 2, cv2.LINE_AA) # following part is executed if single Aruco marker is selected else: if np.all(ids != None): # estimate pose of each marker and return the values rvec, tvec, _ = aruco.estimatePoseSingleMarkers( corners, markerLen, 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(input_img, mtx, dist, rvec[i], tvec[i], 0.1) # draw a square around the markers aruco.drawDetectedMarkers(input_img, corners) #select only the first marker in the list (assumes single Aruco marker is in the scene) #convert to rotation matrix from rotation vector R_rvec = R.from_rotvec(rvec[0]) R_rotmat = R_rvec.as_matrix() # get transformation matrix combining tvec and rotation matrix RSTr = np.hstack([R_rotmat[0], tvec[0].transpose()]) RSTr = np.vstack([RSTr, [0, 0, 0, 1]]) tvec = tvec.reshape(1, 3) print(tvec) # Send the transformed pose to the AR Headset only if the marker detected is within 0.8 m if (tvec[0][2] != 0 and tvec[0][2] < 0.8): ##print("img_idx img_sent", img_idx, img_sent) if kalman_flag == 0 or img_idx - img_sent > reinit_thresh or img_idx % skip_frame_reinit == 0: #convert to rotation matrix from rotation vector R_rvec = R.from_rotvec(rvec[0]) R_rotmat = R_rvec.as_matrix() # get transformation matrix combining tvec and rotation matrix RSTr = np.hstack([R_rotmat[0], tvec.transpose()]) RSTr = np.vstack([RSTr, [0, 0, 0, 1]]) img_sent = img_idx else: # execute the following if kalman filter to be applied rvec = rvec.reshape((3, 1)) rvec = rvec.reshape((1, 3)) current_measurement = np.array([ np.float32(tvec[0][0]), np.float32(tvec[0][1]), np.float32(tvec[0][2]), np.float32(rvec[0][0]), np.float32(rvec[0][1]), np.float32(rvec[0][2]) ]).reshape([1, 6]) current_prediction = kalman.predict() current_prediction = np.array(current_prediction, np.float32) current_prediction = current_prediction.transpose()[0] # predicted rotation rvec_P = current_prediction[3:6] # predicted tvec tvec_P = np.array(current_prediction[:3]).reshape( [1, 3]) # convert to rotation matrix using r function R_rvec = R.from_rotvec(rvec_P) R_RotMat = R_rvec.as_matrix() tvec = tvec_P # update the estimate of the kalman filter RSTr = np.hstack([R_rotmat[0], tvec.transpose()]) RSTr = np.vstack([RSTr, [0, 0, 0, 1]]) kalman.correct(np.transpose(current_measurement)) if socket_connect == 1 and RSTr is not None: # transform the pose from OpenCV's right-handed rule to Unity's left-handed rule 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 transformed pose to send to MagicLeap PoseTr = np.matmul(MLRSTr, RSTr_LH) # Head Pose matrix in the form of array to be sent ArrToSend = np.array([ PoseTr[0][0], PoseTr[0][1], PoseTr[0][2], PoseTr[0][3], PoseTr[1][0], PoseTr[1][1], PoseTr[1][2], PoseTr[1][3], PoseTr[2][0], PoseTr[2][1], PoseTr[2][2], PoseTr[2][3], PoseTr[3][0], PoseTr[3][1], PoseTr[3][2], PoseTr[3][3] ]) # pack the array to be sent before sending dataTosend = struct.pack('f' * len(ArrToSend), *ArrToSend) if socket_connect == 1 and img_idx % skip_frame_send == 0: # img_sent = img_idx c.send(dataTosend) # display the resulting frame cv2.imshow('frame', input_img) if cv2.waitKey(1) & 0xFF == ord('q'): break cv2.destroyAllWindows()
calids = [] for im in imgs: gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY) # corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary, parameters=parameters) corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary) # print('ids 1:', len(ids)) # if ids were found, then if len(ids) > 0: # print(corners, ids, rejectedImgPoints) ret, chcorners, chids = aruco.interpolateCornersCharuco(corners, ids, gray, board) calcorners.append(chcorners) calids.append(chids) im2 = im.copy() aruco.drawDetectedCornersCharuco(im2, chcorners, chids) # aruco.drawDetectedMarkers(im2, rejectedImgPoints, borderColor=(100, 0, 240)) cv2.imshow('markers', im2) cv2.waitKey() ret, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco(calcorners, calids, board, gray.shape[::-1], None, None) print('rms', ret) print(cameraMatrix) print(distCoeffs) cam_params = { 'marker_type': 'aruco', 'cameraMatrix': cameraMatrix, 'distCoeffs': distCoeffs, 'image_size': imgs[0].shape[:2],
im_gray = cv2.cvtColor(img_charuco, cv2.COLOR_RGB2GRAY) h, w = im_gray.shape[:2] dst = cv2.undistort(im_gray, camera_matrix, distortion_coff, None, newcameramtx) corners, ids, rejectedImgPoints = aruco.detectMarkers( dst, aruco_dict, parameters=arucoParams) cv2.imshow("original", im_gray) if not corners: print("pass") else: aruco.refineDetectedMarkers(im_gray, board, corners, ids, rejectedImgPoints) charucoretval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco( corners, ids, im_gray, board) image_with_charuco_board = aruco.drawDetectedCornersCharuco( img_charuco, charucoCorners, charucoIds, (0, 255, 0)) status, rvec, tvec = aruco.estimatePoseCharucoBoard( charucoCorners, charucoIds, board, camera_matrix, distortion_coff) if status != 0: img_aruco = aruco.drawAxis(image_with_charuco_board, newcameramtx, distortion_coff, rvec, tvec, 20) else: print('no markers detected') if cv2.waitKey(0) & 0xFF == ord('q'): cv2.imwrite(base_folder + 'detected charuco pattern.jpg', img_charuco) break cv2.imshow("World co-ordinate frame axes", img_charuco) camera.release()
camera.getFrames() cv2.imshow("image", camera.color_img) # convert frame to grayscale gray = cv2.cvtColor(camera.color_img, cv2.COLOR_BGR2GRAY) corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, dictionary, parameters=arucoParams) # First, detect markers aruco.refineDetectedMarkers(gray, charucoBoard, corners, ids, rejectedImgPoints) if ids is not None: # if there is at least one marker detected charucoretval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco( corners, ids, gray, charucoBoard) im_with_charuco_board = aruco.drawDetectedCornersCharuco( gray, charucoCorners, charucoIds, (0, 255, 0)) rvec, tvec = np.zeros(3), np.zeros(3) retval, _, _ = aruco.estimatePoseCharucoBoard( charucoCorners, charucoIds, charucoBoard, cameraMatrix, distCoeffs, rvec, tvec) # posture estimation from a charuco board if retval == True: im_with_charuco_board = aruco.drawAxis( im_with_charuco_board, cameraMatrix, distCoeffs, rvec, tvec, 0.05 ) # axis length 100 can be changed according to your requirement camera.close() cv2.destroyAllWindows()
def collect_charuco_detections(capture, aruco_dict, charuco_board): # Create the arrays and variables we'll use to store info like corners and IDs from images processed corners_all = [] # Corners discovered in all images processed ids_all = [] # Aruco ids corresponding to corners discovered image_size = None # Determined at runtime good = 0 def show(frame): # resize proportion = max(frame.shape) / 800.0 im = cv2.resize(frame, (int( frame.shape[1] / proportion), int(frame.shape[0] / proportion))) # add text txt = f'good frames so far: {good}' im = cv2.putText(im, txt, (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2, cv2.LINE_AA) # show the debug image cv2.imshow('calibration', im) while True: more, img = capture.read() if not more: break # Grayscale the image gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Find aruco markers in the query image corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=aruco_dict) # Outline the aruco markers found in our query image img = aruco.drawDetectedMarkers(image=img, corners=corners) try: # Get charuco corners and ids from detected aruco markers response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( markerCorners=corners, markerIds=ids, image=gray, board=charuco_board) # If a Charuco board was found, let's collect image/corner points # Requiring at least 20 squares if response > 20: # Add these corners and ids to our calibration arrays corners_all.append(charuco_corners) ids_all.append(charuco_ids) # Draw the Charuco board we've detected to show our calibrator the board was properly detected img = aruco.drawDetectedCornersCharuco( image=img, charucoCorners=charuco_corners, charucoIds=charuco_ids) # If our image size is unknown, set it now if not image_size: image_size = gray.shape[::-1] good += 1 show(img) else: show(img) except cv2.error as e: show(img) if cv2.waitKey(1) & 0xFF == ord('q'): break return corners_all, ids_all, image_size
corners, ids, rejected = aruco.detectMarkers(img, BoardInfo.aurcoDict) cimg = aruco.drawDetectedMarkers(img.copy(), corners, ids) cv2.imwrite(outPath + "DetectedMarkers.png", cimg) charucoCorners, charucoIds = [], [] if len(ids) > 0: numCorners, charucoCorners, charucoIds = aruco.interpolateCornersCharuco( corners, ids, img, BoardInfo.charucoBoard) if len(charucoIds) < 0: continue all_charco_corners_camera.append(charucoCorners.copy()) all_charco_ids_camera.append(charucoIds.copy()) cimg = aruco.drawDetectedCornersCharuco(img.copy(), charucoCorners, charucoIds) cv2.imwrite(outPath + "DetectedCorners.png", cimg) valid_points, new_points_cam, new_points_projector = getCameraCoordinates( img, validV, validH, coordsV, coordsH, charucoCorners) charucoIds = charucoIds[valid_points] if len(charucoIds) < 0: continue all_charco_corners_camera_2.append(new_points_cam) all_real_points.append( BoardInfo.charucoBoard.chessboardCorners[charucoIds[:, 0]]) print(new_points_projector) all_charco_corners_projector.append(new_points_projector)
ret, frame = cap.read() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=aruco_parameters) # if enough markers were detected # then process the board if( ids is not None ): #gray = aruco.drawDetectedMarkers(gray, corners) ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco(corners, ids, gray, charuco_board ) # if there are enough corners to get a reasonable result if( ret > 5 ): aruco.drawDetectedCornersCharuco(frame,ch_corners,ch_ids,(0,0,255) ) retval, rvec, tvec = aruco.estimatePoseCharucoBoard( ch_corners, ch_ids, charuco_board, camera_matrix, dist_coeffs ) # if a pose could be estimated if( retval ) : frame = aruco.drawAxis(frame,camera_matrix,dist_coeffs,rvec,tvec,0.032) # imshow and waitKey are required for the window # to open on a mac. cv2.imshow('frame', frame) if( cv2.waitKey(1) & 0xFF == ord('q') ): break cap.release() cv2.destroyAllWindows()
color_frame = frames.get_color_frame() input_img = np.asanyarray(color_frame.get_data()) # convert frame to grayscale gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY) # detect Aruco markers corners, ids, rejectedImgPoints = aruco.detectMarkers( gray, aruco_dict, parameters=aruco_parameters) # if enough markers were detected, then process the board if (ids is not None): ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco( corners, ids, gray, charuco_board) # if there are enough corners to get a reasonable result, proceed to estimate board pose if (ret > 3): aruco.drawDetectedCornersCharuco(input_img, ch_corners, ch_ids, (0, 0, 255)) #estimate charuco board pose. The axis is centered at one of the corners of the Charuco board. retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard( ch_corners, ch_ids, charuco_board, mtx, dist, None, None) #, useExtrinsicGuess=False) print("pose", retval, rvec, tvec) #In the next few lines of code, the transformation matrix of Aruco board pose is obtained by combining tvec and rvec. rvec = np.reshape(rvec, (1, 3)) tvec = np.reshape(tvec, (1, 3)) R_rvec = R.from_rotvec(rvec) R_rotmat = R_rvec.as_matrix() R_euler = R_rvec.as_euler('xyz', degrees=True) print("R_euler", R_euler) RSTr = np.hstack([R_rotmat[0], tvec.transpose()]) RSTr = np.vstack([RSTr, [0, 0, 0, 1]])
image=grayL, board=CHARUCO_BOARD) responseR, charuco_cornersR, charuco_idsR = aruco.interpolateCornersCharuco( markerCorners=cornersR, markerIds=idsR, image=grayR, board=CHARUCO_BOARD) # If a Charuco board was found, let's collect image/corner points # Requiring all squares if responseL == 24 and responseR == 24: # Add these corners and ids to our calibration arrays objectPoints.append(objp) imgL = aruco.drawDetectedCornersCharuco( image=imgL, charucoCorners=charuco_cornersL, charucoIds=charuco_idsL) imgR = aruco.drawDetectedCornersCharuco( image=imgR, charucoCorners=charuco_cornersR, charucoIds=charuco_idsR) #cv2.imshow('Charuco board Left', imgL) #cv2.waitKey(0) #cv2.imshow('Charuco board Right', imgR) #cv2.waitKey(0) rt = cv2.cornerSubPix(grayL, charuco_cornersL, (11, 11), (-1, -1), criteria) imagePointsLeft.append(charuco_cornersL) rt = cv2.cornerSubPix(grayR, charuco_cornersR, (11, 11), (-1, -1),