refactored
This commit is contained in:
@@ -0,0 +1,85 @@
|
||||
import argparse
|
||||
import cv2
|
||||
import sys
|
||||
|
||||
import cv2.aruco
|
||||
|
||||
from aruco_types import ARUCO_DICT
|
||||
|
||||
|
||||
def im_resize(image, new_width: int):
|
||||
# Define new width while maintaining the aspect ratio
|
||||
aspect_ratio = new_width / image.shape[0]
|
||||
new_height = int(image.shape[1] * aspect_ratio) # Compute height based on aspect ratio
|
||||
|
||||
image = cv2.resize(image, (new_width, new_height))
|
||||
return image
|
||||
|
||||
def factory(aruco_type: int) -> cv2.aruco.ArucoDetector:
|
||||
aruco_dict = cv2.aruco.getPredefinedDictionary(aruco_type)
|
||||
aruco_params = cv2.aruco.DetectorParameters()
|
||||
aruco_detector = cv2.aruco.ArucoDetector(aruco_dict, aruco_params)
|
||||
|
||||
return aruco_detector
|
||||
|
||||
def process_video(aruco_detector: cv2.aruco.ArucoDetector):
|
||||
# Open camera
|
||||
video = cv2.VideoCapture(0)
|
||||
if not video.isOpened():
|
||||
print("Cannot open camera")
|
||||
exit()
|
||||
|
||||
while True:
|
||||
# Read a new frame
|
||||
ok, img = video.read()
|
||||
if not ok:
|
||||
return
|
||||
process(aruco_detector, img)
|
||||
k = cv2.waitKey(1) & 0xFF
|
||||
if k == ord('q'):
|
||||
break
|
||||
|
||||
def process_still(aruco_detector, filename: str):
|
||||
image = cv2.imread(filename)
|
||||
image = im_resize(image, 600)
|
||||
process(aruco_detector, image)
|
||||
k = cv2.waitKey(-1) & 0xFF
|
||||
|
||||
def process(aruco_detector, img):
|
||||
(corners, ids, rejected) = aruco_detector.detectMarkers(img)
|
||||
img = cv2.aruco.drawDetectedMarkers(img, corners, ids)
|
||||
cv2.imshow("Image", img)
|
||||
|
||||
def main():
|
||||
# construct the argument parser and parse the arguments
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("-i", "--image",
|
||||
help="path to input image containing ArUCo tag")
|
||||
ap.add_argument("-t", "--type", type=str,
|
||||
default="DICT_ARUCO_ORIGINAL",
|
||||
help="type of ArUCo tag to detect")
|
||||
args = vars(ap.parse_args())
|
||||
|
||||
# verify that the supplied ArUCo tag exists and is supported by
|
||||
# OpenCV
|
||||
if ARUCO_DICT.get(args["type"], None) is None:
|
||||
print("[INFO] ArUCo tag of '{}' is not supported".format(
|
||||
args["type"]))
|
||||
sys.exit(0)
|
||||
# load the ArUCo dictionary, grab the ArUCo parameters, and detect
|
||||
# the markers
|
||||
print("[INFO] detecting '{}' tags...".format(args["type"]))
|
||||
|
||||
aruco_detector = factory(ARUCO_DICT[args["type"]])
|
||||
|
||||
# load the input image from disk and resize it
|
||||
print("[INFO] loading image...")
|
||||
|
||||
if args["image"] is not None:
|
||||
process_still(aruco_detector, args["image"])
|
||||
else:
|
||||
process_video(aruco_detector)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,51 @@
|
||||
import numpy as np
|
||||
import argparse
|
||||
import cv2
|
||||
import sys
|
||||
from aruco_types import ARUCO_DICT
|
||||
|
||||
def generate(aruco_dict: cv2.aruco.Dictionary, tag_id: int, side_pixels: int = 300):
|
||||
tag = np.zeros((side_pixels, side_pixels, 1), dtype="uint8")
|
||||
cv2.aruco.generateImageMarker(aruco_dict, tag_id, side_pixels, tag, 1)
|
||||
|
||||
return tag
|
||||
|
||||
def main():
|
||||
# construct the argument parser and parse the arguments
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("-f", "--folder", required=True,
|
||||
help="path to output image containing ArUCo tag")
|
||||
ap.add_argument("-d", "--id", type=int, required=True,
|
||||
help="ID of ArUCo tag to generate")
|
||||
ap.add_argument("-t", "--type", type=str,
|
||||
default="DICT_ARUCO_ORIGINAL",
|
||||
help="type of ArUCo tag to generate")
|
||||
ap.add_argument("-i", "--imagetype", type=str,
|
||||
default="png",
|
||||
help="image file type of ArUCo tag image")
|
||||
args = vars(ap.parse_args())
|
||||
|
||||
# verify that the supplied ArUCo tag exists and is supported by
|
||||
# OpenCV
|
||||
if ARUCO_DICT.get(args["type"], None) is None:
|
||||
print("[INFO] ArUCo tag of '{}' is not supported".format(
|
||||
args["type"]))
|
||||
sys.exit(0)
|
||||
|
||||
# load the ArUCo dictionary
|
||||
aruco_dict = cv2.aruco.getPredefinedDictionary(ARUCO_DICT[args["type"]])
|
||||
|
||||
# allocate memory for the output ArUCo tag and then draw the ArUCo
|
||||
# tag on the output image
|
||||
print("[INFO] generating ArUCo tag type '{}' with ID '{}'".format(
|
||||
args["type"], args["id"]))
|
||||
tag = generate(aruco_dict, args["id"], 300)
|
||||
# write the generated ArUCo tag to disk and then display it to our
|
||||
# screen
|
||||
filename = f"{args["folder"]}/{args["type"]}_ID_{args["id"]:04d}.{args["imagetype"]}"
|
||||
cv2.imwrite(filename, tag)
|
||||
cv2.imshow("ArUCo Tag", tag)
|
||||
cv2.waitKey(0)
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,25 @@
|
||||
import cv2
|
||||
|
||||
ARUCO_DICT = {
|
||||
"DICT_4X4_50": cv2.aruco.DICT_4X4_50,
|
||||
"DICT_4X4_100": cv2.aruco.DICT_4X4_100,
|
||||
"DICT_4X4_250": cv2.aruco.DICT_4X4_250,
|
||||
"DICT_4X4_1000": cv2.aruco.DICT_4X4_1000,
|
||||
"DICT_5X5_50": cv2.aruco.DICT_5X5_50,
|
||||
"DICT_5X5_100": cv2.aruco.DICT_5X5_100,
|
||||
"DICT_5X5_250": cv2.aruco.DICT_5X5_250,
|
||||
"DICT_5X5_1000": cv2.aruco.DICT_5X5_1000,
|
||||
"DICT_6X6_50": cv2.aruco.DICT_6X6_50,
|
||||
"DICT_6X6_100": cv2.aruco.DICT_6X6_100,
|
||||
"DICT_6X6_250": cv2.aruco.DICT_6X6_250,
|
||||
"DICT_6X6_1000": cv2.aruco.DICT_6X6_1000,
|
||||
"DICT_7X7_50": cv2.aruco.DICT_7X7_50,
|
||||
"DICT_7X7_100": cv2.aruco.DICT_7X7_100,
|
||||
"DICT_7X7_250": cv2.aruco.DICT_7X7_250,
|
||||
"DICT_7X7_1000": cv2.aruco.DICT_7X7_1000,
|
||||
"DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL,
|
||||
"DICT_APRILTAG_16h5": cv2.aruco.DICT_APRILTAG_16h5,
|
||||
"DICT_APRILTAG_25h9": cv2.aruco.DICT_APRILTAG_25h9,
|
||||
"DICT_APRILTAG_36h10": cv2.aruco.DICT_APRILTAG_36h10,
|
||||
"DICT_APRILTAG_36h11": cv2.aruco.DICT_APRILTAG_36h11
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
import glob
|
||||
|
||||
PAT_NX = 7
|
||||
PAT_NY = 6
|
||||
|
||||
def main():
|
||||
|
||||
# termination criteria
|
||||
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.01)
|
||||
|
||||
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
|
||||
objp = np.zeros((PAT_NX * PAT_NY, 3), np.float32)
|
||||
objp[:, :2] = np.mgrid[0:PAT_NX, 0:PAT_NY].T.reshape(-1, 2)
|
||||
|
||||
# Arrays to store object points and image points from all the images.
|
||||
objpoints = [] # 3d point in real world space
|
||||
imgpoints = [] # 2d points in image plane.
|
||||
|
||||
images = glob.glob('*.jpg')
|
||||
|
||||
for fname in images:
|
||||
img = cv.imread(fname)
|
||||
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
|
||||
|
||||
# Find the chess board corners
|
||||
ret, corners = cv.findChessboardCorners(gray, (PAT_NX, PAT_NY), None)
|
||||
|
||||
# If found, add object points, image points (after refining them)
|
||||
if ret:
|
||||
objpoints.append(objp)
|
||||
|
||||
corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
|
||||
imgpoints.append(corners2)
|
||||
|
||||
# Draw and display the corners
|
||||
cv.drawChessboardCorners(img, (PAT_NX, PAT_NY), corners2, ret)
|
||||
cv.imshow('img', img)
|
||||
cv.waitKey(-1)
|
||||
|
||||
# Calibration
|
||||
ret, mtx, dist, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
|
||||
|
||||
# write to file
|
||||
np.savez("results/cam.npz", mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs)
|
||||
|
||||
# Undistortion
|
||||
img = cv.imread('checker_cam.jpg')
|
||||
h, w = img.shape[:2]
|
||||
newcameramtx, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
|
||||
|
||||
# 1. Undistort using cv.undistort()
|
||||
dst = cv.undistort(img, mtx, dist, None, newcameramtx)
|
||||
|
||||
# crop the image
|
||||
x, y, w, h = roi
|
||||
dst = dst[y:y + h, x:x + w]
|
||||
cv.imwrite('results/calibresult_1.png', dst)
|
||||
|
||||
# 2. Undistort using remapping
|
||||
mapx, mapy = cv.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)
|
||||
dst = cv.remap(img, mapx, mapy, cv.INTER_LINEAR)
|
||||
|
||||
# crop the image
|
||||
x, y, w, h = roi
|
||||
dst = dst[y:y + h, x:x + w]
|
||||
cv.imwrite('results/calibresult_2.png', dst)
|
||||
|
||||
# Re-projection Error
|
||||
mean_error = 0
|
||||
for i in range(len(objpoints)):
|
||||
imgpoints2, _ = cv.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
|
||||
error = cv.norm(imgpoints[i], imgpoints2, cv.NORM_L2) / len(imgpoints2)
|
||||
mean_error += error
|
||||
|
||||
print("total error: {}".format(mean_error / len(objpoints)))
|
||||
cv.destroyAllWindows()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,78 @@
|
||||
import sys
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
import argparse
|
||||
from aruco_types import ARUCO_DICT
|
||||
|
||||
def factory(aruco_type: int):
|
||||
aruco_dict = cv.aruco.getPredefinedDictionary(aruco_type)
|
||||
aruco_params = cv.aruco.DetectorParameters()
|
||||
aruco_detector = cv.aruco.ArucoDetector(aruco_dict, aruco_params)
|
||||
|
||||
return aruco_detector
|
||||
|
||||
def main():
|
||||
# construct the argument parser and parse the arguments
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("-t", "--type", type=str,
|
||||
default="DICT_ARUCO_ORIGINAL",
|
||||
help="type of ArUCo tag to detect")
|
||||
args = vars(ap.parse_args())
|
||||
|
||||
# verify that the supplied ArUCo tag exists and is supported by
|
||||
# OpenCV
|
||||
if ARUCO_DICT.get(args["type"], None) is None:
|
||||
print("[INFO] ArUCo tag of '{}' is not supported".format(
|
||||
args["type"]))
|
||||
sys.exit(0)
|
||||
# load the ArUCo dictionary, grab the ArUCo parameters, and detect
|
||||
# the markers
|
||||
print("[INFO] detecting '{}' tags...".format(args["type"]))
|
||||
aruco_detector = factory(ARUCO_DICT[args["type"]])
|
||||
|
||||
# load the input image from disk and resize it
|
||||
print("[INFO] start processing...")
|
||||
process_video(aruco_detector)
|
||||
|
||||
def process_video(aruco_detector: cv.aruco.ArucoDetector):
|
||||
with np.load('results/cam.npz') as X:
|
||||
mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
|
||||
|
||||
# Open camera
|
||||
video = cv.VideoCapture(0)
|
||||
if not video.isOpened():
|
||||
print("Cannot open camera")
|
||||
exit()
|
||||
|
||||
while True:
|
||||
# Read a new frame
|
||||
ok, img = video.read()
|
||||
if not ok:
|
||||
return
|
||||
process(aruco_detector, img, mtx, dist)
|
||||
k = cv.waitKey(1) & 0xFF
|
||||
if k == ord('q'):
|
||||
break
|
||||
|
||||
def process(aruco_detector: cv.aruco.ArucoDetector, img, mtx, dist): # Load previously saved data
|
||||
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
|
||||
(corners, ids, rejected) = aruco_detector.detectMarkers(gray)
|
||||
|
||||
if len(corners) > 0:
|
||||
# If markers are detected
|
||||
for i in range(0, len(ids)):
|
||||
# Estimate pose of each marker and return the values rvec and tvec---(different from those of camera coefficients)
|
||||
rvec, tvec, _ = cv.aruco.estimatePoseSingleMarkers(corners[i], 0.033, mtx, dist)
|
||||
|
||||
# Draw Axis
|
||||
cv.drawFrameAxes(img, mtx, dist, rvec, tvec, 0.02)
|
||||
|
||||
# Draw a square around the markers
|
||||
cv.aruco.drawDetectedMarkers(img, corners)
|
||||
|
||||
cv.imshow('img', img)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
cv.destroyAllWindows()
|
||||
@@ -0,0 +1,95 @@
|
||||
import sys
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
import argparse
|
||||
from aruco_types import ARUCO_DICT
|
||||
|
||||
def factory(dict_type: int, board_size: cv.typing.Size, marker_length: float=100.0, marker_sep=10, margins=10):
|
||||
aruco_dict = cv.aruco.getPredefinedDictionary(dict_type)
|
||||
params = cv.aruco.DetectorParameters()
|
||||
detector = cv.aruco.ArucoDetector(aruco_dict, params)
|
||||
board = cv.aruco.GridBoard(board_size, markerLength=marker_length, markerSeparation=marker_sep, dictionary=aruco_dict)
|
||||
im_w = int(board_size[0] * (marker_length + marker_sep) - marker_sep + 2 * margins)
|
||||
im_h = int(board_size[1] * (marker_length + marker_sep) - marker_sep + 2 * margins)
|
||||
image = board.generateImage((im_w, im_h), None, margins, 1)
|
||||
cv.imwrite("results/aruco_board.png", image)
|
||||
|
||||
return detector, board
|
||||
|
||||
def main():
|
||||
# construct the argument parser and parse the arguments
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("-t", "--type", type=str,
|
||||
default="DICT_ARUCO_ORIGINAL",
|
||||
help="type of ArUCo tag to detect")
|
||||
args = vars(ap.parse_args())
|
||||
|
||||
# verify that the supplied ArUCo tag exists and is supported by
|
||||
# OpenCV
|
||||
if ARUCO_DICT.get(args["type"], None) is None:
|
||||
print("[INFO] ArUCo tag of '{}' is not supported".format(
|
||||
args["type"]))
|
||||
sys.exit(0)
|
||||
# load the ArUCo dictionary, grab the ArUCo parameters, and detect
|
||||
# the markers
|
||||
print("[INFO] detecting '{}' tags...".format(args["type"]))
|
||||
detector, board = factory(ARUCO_DICT[args["type"]], (5, 5))
|
||||
|
||||
# load the input image from disk and resize it
|
||||
print("[INFO] start processing...")
|
||||
process_video(detector, board)
|
||||
|
||||
def process_video(detector: cv.aruco.ArucoDetector, board):
|
||||
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():
|
||||
print("Cannot open camera")
|
||||
exit()
|
||||
|
||||
while True:
|
||||
# Read a new frame
|
||||
ok, img = video.read()
|
||||
if not ok:
|
||||
return
|
||||
process(detector, board, img, mtx, dist)
|
||||
k = cv.waitKey(1) & 0xFF
|
||||
if k == ord('q'):
|
||||
break
|
||||
|
||||
def process(detector: cv.aruco.ArucoDetector, board: cv.aruco.GridBoard, img, mtx, dist, r_vecs=None, t_vecs=None, draw_marker_box=True, draw_marker_id=False, draw_marker_axis=True):
|
||||
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
|
||||
(corners, ids, rejected) = detector.detectMarkers(gray)
|
||||
|
||||
# Draw a square around the markers
|
||||
if draw_marker_box:
|
||||
cv.aruco.drawDetectedMarkers(img, corners)
|
||||
|
||||
# Draw marker id
|
||||
if draw_marker_id:
|
||||
cv.aruco.drawDetectedMarkers(img, corners, ids)
|
||||
|
||||
# Draw marker axis
|
||||
if draw_marker_axis:
|
||||
_r_vecs, _t_vecs, _ = cv.aruco.estimatePoseSingleMarkers(corners, markerLength=0.1, cameraMatrix=mtx, distCoeffs=dist)
|
||||
if _r_vecs is not None:
|
||||
for i in range(len(_r_vecs)):
|
||||
cv.drawFrameAxes(img, mtx, dist, _r_vecs[i], _t_vecs[i], 0.05)
|
||||
|
||||
if len(corners) > 0:
|
||||
# Estimate pose of each marker and return the values r_vec and t_vec---(different from those of camera coefficients)
|
||||
obj_points, img_points = board.matchImagePoints(corners, ids)
|
||||
pose, r_vec, t_vec = cv.solvePnP(obj_points, img_points, mtx, dist, r_vecs, t_vecs)
|
||||
if pose:
|
||||
# Draw Axis
|
||||
cv.drawFrameAxes(img, mtx, dist, r_vec, t_vec, board.getMarkerLength()*1.5)
|
||||
|
||||
|
||||
cv.imshow('img', img)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
cv.destroyAllWindows()
|
||||
@@ -0,0 +1,165 @@
|
||||
import sys
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
import argparse
|
||||
from aruco_types import ARUCO_DICT
|
||||
import math
|
||||
|
||||
from utils import to_board_pose_euler, cap_init
|
||||
|
||||
CAM_CALIB_FILE = "results/cam_calib_charuco.npz"
|
||||
|
||||
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, margin_length, 1)
|
||||
cv.imwrite("results/charuco_board.png", image)
|
||||
|
||||
return detector, board
|
||||
|
||||
def main():
|
||||
# construct the argument parser and parse the arguments
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("-t", "--type", type=str,
|
||||
default="DICT_ARUCO_ORIGINAL",
|
||||
help="type of ArUCo tag to detect")
|
||||
args = vars(ap.parse_args())
|
||||
|
||||
# verify that the supplied ArUCo tag exists and is supported by
|
||||
# OpenCV
|
||||
if ARUCO_DICT.get(args["type"], None) is None:
|
||||
print("[INFO] ArUCo tag of '{}' is not supported".format(
|
||||
args["type"]))
|
||||
sys.exit(0)
|
||||
# load the ArUCo dictionary, grab the ArUCo parameters, and detect
|
||||
# the markers
|
||||
print("[INFO] detecting '{}' tags...".format(args["type"]))
|
||||
detector, board = factory(ARUCO_DICT[args["type"]], (6,7))
|
||||
|
||||
# load the input image from disk and resize it
|
||||
print("[INFO] start processing...")
|
||||
process_video(detector, board)
|
||||
|
||||
def process_video(detector: cv.aruco.CharucoDetector, board: cv.aruco.CharucoBoard):
|
||||
mtx = None
|
||||
dist = None
|
||||
obj_points_list = []
|
||||
img_points_list = []
|
||||
try:
|
||||
with np.load(CAM_CALIB_FILE) as X:
|
||||
mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
except KeyError:
|
||||
pass
|
||||
|
||||
# Open camera
|
||||
cap = cv.VideoCapture(0)
|
||||
|
||||
if not cap.isOpened():
|
||||
print("Cannot open camera")
|
||||
exit()
|
||||
|
||||
cap_init(cap)
|
||||
|
||||
img_size = None
|
||||
last_rotation = [0,0,0]
|
||||
while True:
|
||||
# Read a new frame
|
||||
ok, img = cap.read()
|
||||
if not ok:
|
||||
return
|
||||
k = cv.waitKey(1) & 0xFF
|
||||
if k == ord('q'):
|
||||
break
|
||||
elif k == ord('s'):
|
||||
img_size, obj_points, img_points = store_frame(detector, board, img)
|
||||
if img_size is not None:
|
||||
obj_points_list.append(obj_points)
|
||||
img_points_list.append(img_points)
|
||||
elif k == ord('c'):
|
||||
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:
|
||||
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
|
||||
|
||||
cv.imshow('img', img)
|
||||
|
||||
def store_frame(detector: cv.aruco.CharucoDetector, board: cv.aruco.Board, img):
|
||||
img_size = None
|
||||
obj_points = None
|
||||
img_points = None
|
||||
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
|
||||
charuco_corners, charuco_ids, _, _ = detector.detectBoard(gray)
|
||||
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:
|
||||
img_size = gray.shape
|
||||
print("Frame captured")
|
||||
else:
|
||||
print("Point matching failed, try again")
|
||||
else:
|
||||
print("Point matching failed, try again")
|
||||
else:
|
||||
print("Point matching failed, try again")
|
||||
|
||||
return img_size, obj_points, img_points
|
||||
|
||||
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:
|
||||
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.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 marker box
|
||||
if draw_marker_box:
|
||||
cv.aruco.drawDetectedMarkers(img, marker_corners)
|
||||
|
||||
# Draw marker id
|
||||
if draw_marker_id:
|
||||
cv.aruco.drawDetectedCornersCharuco(img, charuco_corners, charuco_ids)
|
||||
|
||||
# Draw marker axis
|
||||
if draw_marker_axis:
|
||||
_rvecs, _tvecs, _ = cv.aruco.estimatePoseSingleMarkers(marker_corners, markerLength=0.1, 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)
|
||||
offset = int(board.getMarkerLength() * board.getSquareLength())
|
||||
obj_points -= [offset, offset, 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
|
||||
cv.drawFrameAxes(img, mtx, dist, r_vec, t_vec, 10)
|
||||
|
||||
return r_vec, t_vec
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
cv.destroyAllWindows()
|
||||
@@ -0,0 +1,119 @@
|
||||
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=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)
|
||||
image = board.generateImage((800, 600), None, margin_length, 1)
|
||||
cv.imwrite("results/charuco_diamond_board.png", image)
|
||||
|
||||
return detector, board
|
||||
|
||||
def main():
|
||||
# construct the argument parser and parse the arguments
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("-t", "--type", type=str,
|
||||
default="DICT_ARUCO_ORIGINAL",
|
||||
help="type of ArUCo tag to detect")
|
||||
args = vars(ap.parse_args())
|
||||
|
||||
# verify that the supplied ArUCo tag exists and is supported by
|
||||
# OpenCV
|
||||
if ARUCO_DICT.get(args["type"], None) is None:
|
||||
print("[INFO] ArUCo tag of '{}' is not supported".format(
|
||||
args["type"]))
|
||||
sys.exit(0)
|
||||
# load the ArUCo dictionary, grab the ArUCo parameters, and detect
|
||||
# the markers
|
||||
print("[INFO] detecting '{}' tags...".format(args["type"]))
|
||||
detector, board = factory(ARUCO_DICT[args["type"]])
|
||||
|
||||
# load the input image from disk and resize it
|
||||
print("[INFO] start processing...")
|
||||
process_video(detector, board)
|
||||
|
||||
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
|
||||
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 = cap.read()
|
||||
if not ok:
|
||||
return
|
||||
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
|
||||
|
||||
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.detectBoard(gray)
|
||||
if diamond_corners is not None:
|
||||
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 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.1, 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()):
|
||||
# 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, r_vec, t_vec, 10)
|
||||
|
||||
return r_vec, t_vec
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
cv.destroyAllWindows()
|
||||
@@ -0,0 +1,103 @@
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
import glob
|
||||
|
||||
PAT_NX = 7
|
||||
PAT_NY = 6
|
||||
|
||||
def draw_gizmo(img, corners, imgpts):
|
||||
corner = tuple(corners[0].ravel().astype("int32"))
|
||||
imgpts = imgpts.astype("int32")
|
||||
img = cv.line(img, corner, tuple(imgpts[0].ravel()), (255,0,0), 5)
|
||||
img = cv.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5)
|
||||
img = cv.line(img, corner, tuple(imgpts[2].ravel()), (0,0,255), 5)
|
||||
return img
|
||||
|
||||
|
||||
def draw_cube(img, imgpts):
|
||||
imgpts = np.int32(imgpts).reshape(-1, 2)
|
||||
|
||||
# draw ground floor in green
|
||||
img = cv.drawContours(img, [imgpts[:4]], -1, (0, 255, 0), -3)
|
||||
|
||||
# draw pillars in blue color
|
||||
for i, j in zip(range(4), range(4, 8)):
|
||||
img = cv.line(img, tuple(imgpts[i]), tuple(imgpts[j]), (255), 3)
|
||||
|
||||
# draw top layer in red color
|
||||
img = cv.drawContours(img, [imgpts[4:]], -1, (0, 0, 255), 3)
|
||||
|
||||
return img
|
||||
|
||||
def factory():
|
||||
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||||
objp = np.zeros((PAT_NX * PAT_NY, 3), np.float32)
|
||||
objp[:, :2] = np.mgrid[0:PAT_NX, 0:PAT_NY].T.reshape(-1, 2)
|
||||
|
||||
axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3)
|
||||
|
||||
return criteria, objp, axis
|
||||
|
||||
def main():
|
||||
process_video()
|
||||
|
||||
def process_video():
|
||||
with np.load('results/cam.npz') as X:
|
||||
mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
|
||||
|
||||
# Init
|
||||
criteria, objp, axis = factory()
|
||||
|
||||
# Open camera
|
||||
video = cv.VideoCapture(0)
|
||||
if not video.isOpened():
|
||||
print("Cannot open camera")
|
||||
exit()
|
||||
|
||||
while True:
|
||||
# Read a new frame
|
||||
ok, img = video.read()
|
||||
if not ok:
|
||||
return
|
||||
process(img, mtx, dist, criteria, objp, axis)
|
||||
k = cv.waitKey(1) & 0xFF
|
||||
if k == ord('q'):
|
||||
break
|
||||
|
||||
def process_still():
|
||||
with np.load('results/cam.npz') as X:
|
||||
mtx, dist, _, _ = [X[i] for i in ('mtx', 'dist', 'rvecs', 'tvecs')]
|
||||
|
||||
# Init
|
||||
criteria, objp, axis = factory()
|
||||
|
||||
for fname in glob.glob('check*.jpg'):
|
||||
img = cv.imread(fname)
|
||||
process(img, mtx, dist, criteria, objp, axis)
|
||||
k = cv.waitKey(1) & 0xFF
|
||||
if k == ord('q'):
|
||||
break
|
||||
|
||||
|
||||
def process(img, mtx, dist, criteria, objp, axis): # Load previously saved data
|
||||
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
|
||||
ret, corners = cv.findChessboardCorners(gray, (PAT_NX, PAT_NY), corners=None, flags=cv.CALIB_CB_ADAPTIVE_THRESH+cv.CALIB_CB_NORMALIZE_IMAGE+cv.CALIB_CB_FAST_CHECK)
|
||||
cv.drawChessboardCorners(img, corners=corners, patternSize=(PAT_NX, PAT_NY), patternWasFound=ret)
|
||||
|
||||
if ret:
|
||||
corners2 = cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
|
||||
|
||||
# Find the rotation and translation vectors.
|
||||
ret, rvecs, tvecs = cv.solvePnP(objp, corners2, mtx, dist)
|
||||
|
||||
# project 3D points to image plane
|
||||
imgpts, jac = cv.projectPoints(axis, rvecs, tvecs, mtx, dist)
|
||||
|
||||
img = draw_gizmo(img, corners2, imgpts)
|
||||
|
||||
cv.imshow('img', img)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
cv.destroyAllWindows()
|
||||
@@ -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