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

improved marker drawing

Browse Source
This commit is contained in:
jens
2025-11-26 18:23:24 +01:00
parent 7c68f489b0
commit 066c633615
1 changed files with 16 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cam_pose_charuco_board.py
+16 -11
View File
@@ -5,6 +5,8 @@ import argparse
from aruco_types import ARUCO_DICT
import math
from cam_pose_checker import draw_gizmo
CAM_CALIB_FILE = "results/cam_calib_charuco.npz"
def rotation_matrix_to_euler_angles(R):
@@ -119,7 +121,7 @@ def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoa
r_vec, t_vec = process(detector, board, img, mtx, dist, None, None)
if r_vec is not None and t_vec is not None:
r_vec_obj, _ = to_board_pose_euler(r_vec, t_vec)
rot = [int(180.0 / math.pi * v) for v in r_vec_obj]
rot = [round(180.0 / math.pi * v, 0) for v in r_vec_obj]
if rot != last_rotation:
print("\r ", end='')
print(f"\rTilt: {rot[0]:.1f}, Roll: {rot[1]:.1f}, Azimuth: {rot[2]:.1f}", end='')
@@ -155,23 +157,26 @@ def calibrate_camera(img_size, obj_points_list, img_points_list, mtx, dist, rvec
return mtx, dist, rvecs, tvecs
def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, img, mtx, dist, rvecs=None, tvecs=None): # Load previously saved data
def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, img, mtx, dist, rvecs=None, tvecs=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=False): # 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:
# Draw a square around the markers
cv.aruco.drawDetectedCornersCharuco(img, charuco_corners, charuco_ids)
# Draw marker box
if draw_marker_box:
cv.aruco.drawDetectedMarkers(img, marker_corners)
# Draw a square around the markers
cv.aruco.drawDetectedMarkers(img, marker_corners)
# Draw marker id
if draw_marker_id:
cv.aruco.drawDetectedCornersCharuco(img, charuco_corners, charuco_ids)
# Draw an axis around the markers
rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.062, cameraMatrix=mtx, distCoeffs=dist)
for i in range(len(rvecs)):
cv.drawFrameAxes(img, mtx, dist, rvecs[i], tvecs[i],0.05)
# Draw marker axis
if draw_marker_axis:
rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.062, cameraMatrix=mtx, distCoeffs=dist)
for i in range(len(rvecs)):
cv.drawFrameAxes(img, mtx, dist, rvecs[i], tvecs[i],0.05)
if len(charuco_ids) >= len(board.getIds()):
obj_points, img_points = board.matchImagePoints(charuco_corners, charuco_ids)
@@ -180,7 +185,7 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, im
if mtx is not None and dist is not None:
pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, mtx, dist, useExtrinsicGuess=False, rvec=rvecs, tvec=tvecs, flags=cv.SOLVEPNP_ITERATIVE)
if pose:
# Draw Axis
# Draw
cv.drawFrameAxes(img, mtx, dist, r_vec, t_vec, 10)
return r_vec, t_vec