I'm struggling with getting 2D camera space coordinates from my ArUco markers' corners and am only getting weirdly large results. Working with OpenCV and Python.
What I have done so far:
- Extracting 2D corner coordinates of my
ArUco
markers in their respective marker space witharuco.detectMarkers
- getting rotation vector and translation vector of the current marker by using
aruco.estimatePoseSingleMarkers
- then using the Python solution from this question to get 3D corner coordinates
- and then using
cv2.projectPoints
on the 3D corner coordinates to obtain the 2D projection (for projectPoints I'm using rotation and translation vector of the current marker)
What I get as a result are values like this:
[[-1333579392 -2147483648]
[-2147483648 -2147483648]
[-2147483648 -2147483648]
[-1272630656 -2147483648]]
...which to me seem too large. I was hoping to draw those points on the webcam livestream, but they do not show up (most probably because they are wrong). In between (only in some frames) I suddenly also receive values that seem more reasonable:
[[15526 6153]
[11828 4397]
[10722 3485]
[13921 4835]]
...but also no plotted points on my screen. And of course it's weird that those values differ so much from the other values, I really don't know why.
Am I missing some scaling after Step 3 or something? Or did I misunderstand something with the coordinate transformation? I couldn't really find anything on this problem. Any tips in the right direction will be greatly appreciated! Excuse me, if there are any super dumb errors, this is my first project with both Python and OpenCV.
This is a working example:
import numpy as np
import cv2
# rotate a markers corners by rvec and translate by tvec if given input is the size of a marker.
# In the markerworld the 4 markercorners are at (x,y) = (+- markersize/2, +- markersize/2)
# returns the rotated and translated corners to camera world and the rotation matrix
def rotate_marker_corners(rvec, markersize, tvec = None):
mhalf = markersize / 2.0
# convert rot vector to rot matrix both do: markerworld -> cam-world
mrv, jacobian = cv2.Rodrigues(rvec)
#in markerworld the corners are all in the xy-plane so z is zero at first
X = mhalf * mrv[:,0] #rotate the x = mhalf
Y = mhalf * mrv[:,1] #rotate the y = mhalf
minusX = X * (-1)
minusY = Y * (-1)
# calculate 4 corners of the marker in camworld. corners are enumerated clockwise
markercorners = []
markercorners.append(minusX + Y) #was upper left in markerworld
markercorners.append(X + Y) #was upper right in markerworld
markercorners.append(X + minusY) #was lower right in markerworld
markercorners.append(minusX + minusY) #was lower left in markerworld
# if tvec given, move all by tvec
if tvec is not None:
C = tvec #center of marker in camworld
for i, mc in enumerate(markercorners):
markercorners[i] = C + mc #add tvec to each corner
markercorners = np.array(markercorners,dtype=np.float32) # type needed when used as input to cv2
return markercorners, mrv
def loadCoefficients():
cv_file = cv2.FileStorage("calibrationCoefficients.yaml", cv2.FILE_STORAGE_READ)
camera_matrix = cv_file.getNode("camera_matrix").mat()
dist_matrix = cv_file.getNode("dist_coeff").mat()
cv_file.release()
return [camera_matrix, dist_matrix]
# load the ArUCo dictionary and grab the ArUCo parameters
arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_50)
arucoParams = cv2.aruco.DetectorParameters_create()
# initialize video stream
cap = cv2.VideoCapture(0, cv2.CAP_DSHOW)
# loop over the frames from the video stream
while True:
# grab the frame from the threaded video stream
frame = np.zeros((480,640))
res, frame = cap.read()
# detect ArUco markers in the input frame
(corners, ids, rejected) = cv2.aruco.detectMarkers(frame, arucoDict, parameters=arucoParams) #returns relative position of camera in marker's world
mtx, dist = loadCoefficients()
# verify *at least* one ArUco marker was detected
if len(corners) > 0:
# flatten the ArUco IDs list
ids = ids.flatten()
for i in range(0, len(ids)):
# draw detected markers on screen
cv2.aruco.drawDetectedMarkers(frame, corners, ids)
#extract rotation vector and translation vector for each marker, 0.012 being my marker length in meters
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners, 0.012, mtx, dist)
# draw axes onto marker for checking pose estimation
cv2.drawFrameAxes(frame, mtx, dist, rvec[i], tvec[i], 0.012)
# compute 3D coordinates of marker's corners in camera space
cornerCoordinates, _ = rotate_marker_corners(rvec[i], 0.012, tvec[i])
# compute 2D coordinates of marker's corners in camera space
reducedDimensionsto2D, _ = cv2.projectPoints(cornerCoordinates, rvec[i], tvec[i], mtx, dist)
reducedDimensionsto2D = np.int32(reducedDimensionsto2D).reshape(-1, 2) # reshape list for better readability
print(reducedDimensionsto2D) # debugging
# Draw square of projected points on the livestream for debugging
cv2.polylines(frame, [np.int32(reducedDimensionsto2D)], True, (255, 0, 0), 3)
# show the output frame
cv2.imshow("Frame", frame)
key = cv2.waitKey(1) & 0xFF
# if the `q` key was pressed, break from the loop
if key == ord("q"):
break
cv2.destroyAllWindows()
And these are my camera calibrations to be saved as calibrationCoefficients.yaml:
%YAML:1.0
---
camera_matrix: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [ 1.5026752406242376e+03, 0., 9.2256504451330136e+02, 0.,
1.5113357377940451e+03, 4.4730181258850007e+02, 0., 0., 1. ]
dist_coeff: !!opencv-matrix
rows: 1
cols: 5
dt: d
data: [ 1.3350912805940560e-01, -4.5402165375909015e-01,
-2.3676773666038882e-02, -7.3028961943791158e-03,
8.2049803330142934e-01 ]
These are my markers to test with (I cut them out along the border around each of them): Test marker sheet