diff --git a/cam_pose_aruco_board.py b/cam_pose_aruco_board.py index b856d5f..cc69563 100644 --- a/cam_pose_aruco_board.py +++ b/cam_pose_aruco_board.py @@ -59,7 +59,7 @@ def process_video(detector: cv.aruco.ArucoDetector, board): if k == ord('q'): break -def process(detector: cv.aruco.ArucoDetector, board: cv.aruco.GridBoard, img, mtx, dist, rvecs=None, tvecs=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=True): # Load previously saved data +def process(detector: cv.aruco.ArucoDetector, board: cv.aruco.GridBoard, img, mtx, dist, r_vecs=None, t_vecs=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=True): gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) (corners, ids, rejected) = detector.detectMarkers(gray) @@ -73,17 +73,18 @@ def process(detector: cv.aruco.ArucoDetector, board: cv.aruco.GridBoard, img, mt # Draw marker axis if draw_marker_axis: - _rvecs, _tvecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=mtx, distCoeffs=dist) - for i in range(len(_rvecs)): - cv.drawFrameAxes(img, mtx, dist, _rvecs[i], _tvecs[i], 0.05) + _r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=mtx, distCoeffs=dist) + if _r_vecs is not None: + for i in range(len(_r_vecs)): + cv.drawFrameAxes(img, mtx, dist, _r_vecs[i], _t_vecs[i], 0.05) if len(corners) > 0: - # Estimate pose of each marker and return the values rvec and tvec---(different from those of camera coefficients) + # Estimate pose of each marker and return the values r_vec and t_vec---(different from those of camera coefficients) obj_points, img_points = board.matchImagePoints(corners, ids) - pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, rvecs, tvecs) + pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, mtx, dist, r_vecs, t_vecs) if pose: # Draw Axis - cv.drawFrameAxes(img, mtx, dist, rvec, tvec, board.getMarkerLength()*1.5) + cv.drawFrameAxes(img, mtx, dist, r_vec, t_vec, board.getMarkerLength()*1.5) cv.imshow('img', img)