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

[cam_pose_charuco_board]

Browse Source 
- improved capture resolution and exposure
- 'x'-key clears camera calibration and pattern
This commit is contained in:
jens
2025-11-25 20:14:36 +01:00
parent 8949c93800
commit c2710494a7
1 changed files with 29 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cam_pose_charuco_board.py
+29 -14
View File
@@ -54,15 +54,25 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
pass
# Open camera
video = cv.VideoCapture(0)
if not video.isOpened():
cap = cv.VideoCapture(0)
if not cap.isOpened():
print("Cannot open camera")
exit()
cap.set(cv.CAP_PROP_SETTINGS, 1)
cap.set(cv.CAP_PROP_FOURCC, cv.VideoWriter.fourcc('M', 'J', 'P', 'G'))
cap.set(cv.CAP_PROP_FPS, 60.0)
cap.set(cv.CAP_PROP_FRAME_WIDTH, 1280)
cap.set(cv.CAP_PROP_FRAME_HEIGHT, 1024)
cap.set(cv.CAP_PROP_EXPOSURE, 20)
cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1)
img_size = None
last_rotation = [0,0,0]
while True:
# Read a new frame
ok, img = video.read()
ok, img = cap.read()
if not ok:
return
k = cv.waitKey(1) & 0xFF
@@ -77,10 +87,19 @@ def process_video(detector: cv.aruco.CharucoDetector, board):
mtx, dist, rvecs, tvecs = calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, None, None)
np.savez(f"{CAM_CALIB_FILE}", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)
elif k == ord('x'):
mtx = None
dist = None
obj_points_list = []
img_points_list = []
else:
process(detector, board, img, mtx, dist, None, None)
r_vec, t_vec = process(detector, board, img, mtx, dist, None, None)
if r_vec is not None:
rotation = [int(180.0 / math.pi * v) for v in r_vec[:, 0]]
if last_rotation != rotation:
print(f"Rotation X|Y|Z: {rotation[0]:.1f} | {rotation[1]:.1f} | {rotation[2]:.1f}")
last_rotation = rotation
cv.imshow('img', img)
def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
img_size = None
@@ -103,17 +122,16 @@ def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
return img_size, obj_points, img_points
def calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, rvecs=None, tvecs=None):
def calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, rvecs=None, tvecs=None, flags=0):
if len(obj_points_list) > 0 and len(img_points_list) > 0:
flags = 0
if mtx is not None:
flags = cv.CALIB_USE_INTRINSIC_GUESS
ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(obj_points_list, img_points_list, img_size, mtx, dist, rvecs=rvecs, tvecs=tvecs, flags=flags)
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=None, tvecs=None): # Load previously saved data
r_vec = None
t_vec = None
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
charuco_corners, charuco_ids, marker_corners, marker_ids = detector.detectBoard(gray)
if charuco_corners is not None:
@@ -127,15 +145,12 @@ 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, rvec=rvecs, tvec=tvecs)
rot = [float(180/math.pi*v) for v in rvec[:,0]]
print(f"Rotation X|Y|Z: {rot[0]:.1f} | {rot[1]:.1f} | {rot[2]:.1f}")
pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, mtx, dist, rvec=rvecs, tvec=tvecs)
if pose:
# Draw Axis
cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 10)
cv.imshow('img', img)
cv.drawFrameAxes(img, mtx, dist, r_vec, t_vec, 10)
return r_vec, t_vec
if __name__ == '__main__':
main()