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

[cam_pose_charuco_board]

Browse Source 
- refactored

[cam_pose_charuco_diamond]
- working version

[General]
- refactored
This commit is contained in:
jens
2025-11-27 21:47:44 +01:00
parent 066c633615
commit a67a09d8b2
3 changed files with 98 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cam_pose_charuco_board.py
+6 -36
View File
@@ -5,40 +5,16 @@ import argparse
from aruco_types import ARUCO_DICT
import math
from cam_pose_checker import draw_gizmo
from utils import to_board_pose_euler, cap_init
CAM_CALIB_FILE = "results/cam_calib_charuco.npz"
def rotation_matrix_to_euler_angles(R):
sy = math.sqrt(R[0,0]**2 + R[1,0]**2)
singular = sy < 1e-6
if not singular:
x = math.atan2(R[2,1] , R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
else:
# Handle singularity
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
return np.array([x, y, z])
def to_board_pose_euler(r_vec, t_vec):
r_mat = np.ndarray((3, 3))
cv.Rodrigues(r_vec, r_mat)
r_mat = np.hstack((r_mat, t_vec))
r_mat = np.vstack((r_mat, np.array([0, 0, 0, 1])))
r_mat = np.linalg.inv(r_mat)
res = rotation_matrix_to_euler_angles(r_mat)
return res, r_mat[:,3]
def factory(dict_type: int, board_size: cv.typing.Size, square_length: float=5.0, marker_length: float=3.0):
def factory(dict_type: int, board_size: cv.typing.Size, square_length: float=5.0, marker_length: float=3.0, margin_length=0):
aruco_dict = cv.aruco.getPredefinedDictionary(dict_type)
board = cv.aruco.CharucoBoard(board_size, square_length, marker_length, aruco_dict)
params = cv.aruco.CharucoParameters()
detector = cv.aruco.CharucoDetector(board, params)
image = board.generateImage((800, 600), None, 0, 1)
image = board.generateImage((800, 600), None, margin_length, 1)
cv.imwrite("results/charuco_board.png", image)
return detector, board
@@ -86,13 +62,7 @@ def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoa
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, 30)
cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 1)
cap_init(cap)
img_size = None
last_rotation = [0,0,0]
@@ -174,9 +144,9 @@ def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard, im
# Draw marker axis
if draw_marker_axis:
rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.062, cameraMatrix=mtx, distCoeffs=dist)
_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)
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)