jens/ocv_cam_pose
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

don't use rvecs and tvecs as input for solvePnP

Browse Source
This commit is contained in:
jens
2025-11-25 17:20:55 +01:00
parent 00a67273e5
commit 7350c941cd
1 changed files with 16 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cam_pose_charuco_board.py
+16 -12
View File
@@ -45,8 +45,12 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
dist = None
rvecs = None
tvecs = None
with np.load(CAM_CALIB_FILE) as X:
mtx, dist, rvecs, tvecs = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
try:
with np.load(CAM_CALIB_FILE) as X:
mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
except FileNotFoundError:
pass
# Open camera
video = cv.VideoCapture(0)
@@ -66,7 +70,7 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
elif k == ord('s'):
img_size = store_frame(detector, board, img)
elif k == ord('c'):
mtx, dist, rvecs, tvecs = calibrate_camera(mtx, dist, rvecs, tvecs, img_size)
mtx, dist, _, _ = calibrate_camera(img_size, mtx, dist, None, None)
else:
process(detector, board, img, mtx, dist, rvecs, tvecs)
@@ -78,7 +82,6 @@ def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
if charuco_corners is not None:
if len(charuco_ids) >= len(board.getIds()):
obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids)
if len(obj_points) > 0 and len(img_points) > 0:
ENTRY['charuco_corners'].append(charuco_corners)
ENTRY['charuco_ids'].append(charuco_ids)
@@ -95,15 +98,17 @@ def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
return img_size
def calibrate_camera(mtx, dist, rvecs, tvecs, img_size):
def calibrate_camera(img_size, mtx, dist, rvecs=None, tvecs=None):
global ENTRY
ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(ENTRY['obj_points'], ENTRY['img_points'], img_size, mtx, dist, rvecs, tvecs)
np.savez(f"{CAM_CALIB_FILE}", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)
ENTRY = {'charuco_corners': [], 'charuco_ids': [], 'obj_points': [], 'img_points': []}
if len(ENTRY['obj_points']) > 0 and len(ENTRY['img_points']) > 0:
ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(ENTRY['obj_points'], ENTRY['img_points'], img_size, mtx, dist, rvecs=rvecs, tvecs=tvecs)
np.savez(f"{CAM_CALIB_FILE}", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)
ENTRY = {'charuco_corners': [], 'charuco_ids': [], 'obj_points': [], 'img_points': []}
print(f"Camera calibration finished. Result = {ret}")
return mtx, dist, rvecs, tvecs
def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, dist, rvecs, tvecs): # Load previously saved data
def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, dist, rvecs=None, tvecs=None): # Load previously saved data
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(gray)
if charuco_corners is not None:
@@ -116,12 +121,11 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx,
if len(charuco_ids) >= len(board.getIds()):
obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids)
if mtx is not None and dist is not None:
pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, rvecs, tvecs)
pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, rvec=rvecs, tvec=tvecs)
if pose:
# Draw Axis
cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 20)
cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 10)
cv.imshow('img', img)