[cam_pose_charuco_board]
- refactored [cam_pose_charuco_diamond] - working version [General] - refactored
This commit is contained in:
@@ -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)
|
||||
|
||||
+56
-23
@@ -1,16 +1,17 @@
|
||||
import sys
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
import math
|
||||
import argparse
|
||||
from utils import to_board_pose_euler, cap_init
|
||||
from aruco_types import ARUCO_DICT
|
||||
|
||||
def factory(dict_type: int, square_length: float=200.0, marker_length: float=120.0, margin_length=10):
|
||||
def factory(dict_type: int, square_length: float=5.0, marker_length: float=3.0, margin_length=0):
|
||||
aruco_dict = cv.aruco.getPredefinedDictionary(dict_type)
|
||||
board = cv.aruco.CharucoBoard((3,3), square_length, marker_length, aruco_dict)
|
||||
params = cv.aruco.CharucoParameters()
|
||||
detector = cv.aruco.CharucoDetector(board, params)
|
||||
side_len = int(3.0*square_length + 2.0*margin_length)
|
||||
image = board.generateImage((side_len, side_len), None, margin_length, 1)
|
||||
image = board.generateImage((800, 600), None, margin_length, 1)
|
||||
cv.imwrite("results/charuco_diamond_board.png", image)
|
||||
|
||||
return detector, board
|
||||
@@ -38,48 +39,80 @@ def main():
|
||||
print("[INFO] start processing...")
|
||||
process_video(detector, board)
|
||||
|
||||
def process_video(detector: cv.aruco.CharucoDetector, board):
|
||||
with np.load('results/cam.npz') as X:
|
||||
def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard):
|
||||
with np.load('results/cam_calib_charuco.npz') as X:
|
||||
mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
|
||||
|
||||
# Open camera
|
||||
video = cv.VideoCapture(0)
|
||||
if not video.isOpened():
|
||||
cap = cv.VideoCapture(0)
|
||||
if not cap.isOpened():
|
||||
print("Cannot open camera")
|
||||
exit()
|
||||
|
||||
cap_init(cap)
|
||||
|
||||
last_rotation = [0,0,0]
|
||||
while True:
|
||||
# Read a new frame
|
||||
ok, img = video.read()
|
||||
ok, img = cap.read()
|
||||
if not ok:
|
||||
return
|
||||
process(detector, board, img, mtx, dist)
|
||||
k = cv.waitKey(1) & 0xFF
|
||||
if k == ord('q'):
|
||||
break
|
||||
else:
|
||||
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 = [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='')
|
||||
last_rotation = rot
|
||||
|
||||
def process(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img, mtx, dist): # Load previously saved data
|
||||
cv.imshow('img', img)
|
||||
|
||||
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
|
||||
use_detect_diamonds = 0
|
||||
r_vec = None
|
||||
t_vec = None
|
||||
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
|
||||
diamond_corners, diamond_ids, marker_corners, marker_ids = detector.detectDiamonds(gray)
|
||||
diamond_corners, diamond_ids, marker_corners, marker_ids = detector.detectBoard(gray)
|
||||
if diamond_corners is not None:
|
||||
# Draw a square around the markers
|
||||
cv.aruco.drawDetectedCornersCharuco(img, diamond_corners, diamond_ids)
|
||||
if use_detect_diamonds:
|
||||
diamond_corners1, diamond_ids1, marker_corners1, marker_ids1 = detector.detectDiamonds(gray)
|
||||
diamond_corners = diamond_corners1[0]
|
||||
diamond_ids = np.transpose(diamond_ids1[0])
|
||||
marker_corners = marker_corners1
|
||||
marker_ids = marker_ids1[0]
|
||||
|
||||
# Draw a square around the markers
|
||||
cv.aruco.drawDetectedMarkers(img, marker_corners)
|
||||
# Draw marker box
|
||||
if draw_marker_box:
|
||||
cv.aruco.drawDetectedMarkers(img, marker_corners)
|
||||
|
||||
# Draw marker id
|
||||
if draw_marker_id:
|
||||
cv.aruco.drawDetectedDiamonds(img, diamond_corners, diamond_ids)
|
||||
|
||||
# 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(diamond_ids) >= len(board.getIds()):
|
||||
# Draw a square around the markers
|
||||
cv.aruco.drawDetectedDiamonds(img, diamond_corners, diamond_ids)
|
||||
if False and diamond_ids is not None and len(diamond_ids) > 0:
|
||||
obj_points, img_points = board.matchImagePoints(diamond_corners, diamond_ids)
|
||||
pose, rvec, tvec = cv.solvePnP(obj_points, img_points, mtx, dist, None, None)
|
||||
# Estimate diamond pose
|
||||
obj_points, img_points = board.matchImagePoints(diamond_corners, diamond_ids)
|
||||
offset = int(board.getMarkerLength() * board.getSquareLength())
|
||||
obj_points -= [offset/2, offset/2, 0]
|
||||
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
|
||||
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()
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
import math
|
||||
|
||||
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 cap_init(cap: cv.VideoCapture):
|
||||
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)
|
||||
Reference in New Issue
Block a user