I'm trying to write a program to find the distance between two aruco markers and the distance seems to be about right when the markers are located to the left and right of the camera frame, but when they are located at the top and bottom of the camera frame, the distance between them is totally wrong?
Here's my code:
import numpy as np
import cv2
import cv2.aruco as aruco
def mag(x):
return np.sqrt(x[0]**2+x[1]**2+x[2]**2)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
mtx = np.array([[ 8.8213173810477792e+02, 0., 6.2265593183247574e+02],[ 0.,
9.0710890795744012e+02, 4.1207655778884924e+02],[ 0., 0., 1. ]])
dist = np.array([ 5.3943178087169294e-03, -2.0750715497990183e-01,
-1.2051405507801402e-03, -2.0261943010518283e-03,
4.2906970296298941e-01 ])
cap = cv2.VideoCapture(0) # Get the camera source
alltvec = np.zeros([50,3])
def track(matrix_coefficients, distortion_coefficients):
while True:
ret, frame = cap.read() #capture a fram
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Change grayscale
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50) # Use 4x4 dictionary to find markers
parameters = aruco.DetectorParameters_create() # Marker detection parameters
# lists of ids and the corners beloning to each id
corners, ids, rejected_img_points = aruco.detectMarkers(gray, aruco_dict,
parameters=parameters,
cameraMatrix=mtx,
distCoeff=dist)
if np.all(ids is not None): # If there are markers found by detector
for i in range(0, len(ids)): # Iterate in markers
# Estimate pos of each marker and return the values rvec and tvec---different from camera coefficients
rvec, tvec, markerPoints = aruco.estimatePoseSingleMarkers(corners[i], 0.031, mtx,dist)
(rvec - tvec).any() # get rid of that numpy value array error
aruco.drawDetectedMarkers(frame, corners) # Draw A square around the markers
aruco.drawAxis(frame, matrix_coefficients, distortion_coefficients, rvec, tvec, 0.01) # Draw Axis
alltvec[ids[i]-1] = tvec #add tvec value to array of all tvecs
if ids is not None and len(ids) > 0: # If there are two markers, reverse the second and get the difference
vector = alltvec[1]-alltvec[0]
distance = np.linalg.norm(vector)
cv2.putText(frame, "Distance: {:.2f} m".format(distance), (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
cv2.imshow('frame', frame)
#Wait 3 milisecoonds for an interaction. Check the key and do the corresponding job.
key = cv2.waitKey(3) & 0xFF
if key == ord('q'): # Quit
break
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
track(mtx,dist)
I've tried just getting the distances of the markers from the camera and this seems to be normal regardless of where in the frame the markers are
If it's of any help, the markers on the sheet of paper are 20cm apart in the long direction and 10cm apart in the shorter direction
(their distance apart is about right for when they are side to side but totally off when they are top to bottom of the frame)
