I am using ArUco Board to get camera position and attitude. In this case, the marker (ArUco Board) is my reference (origin).
But when I rotate camera(drone) around its own axis (around yaw angle), after -10 degree yaw angle, position estimation error is becoming very large (especially y-axis).
Yaw Degree: 19
Yaw Degree: 0
Yaw Degree: -10
. .
I also collected whole x and y axes position estimation datas and printed on graphs. When you review the graph, after -10 degree yaw angle, y-axis position estimation error is becoming very large.
Why? How can I increase accuracy? How can I prevent this situation?
I also used all refinement methods.
aruco.CORNER_REFINE_NONE
aruco.CORNER_REFINE_SUBPIX
aruco.CORNER_REFINE_CONTOUR
aruco.CORNER_REFINE_APRILTAG
The result is same.
Code:
from djitellopy import tello
import time
from time import sleep
import numpy as np
import cv2
from cv2 import aruco
import sys, math
telloDroneEnabled = 1 # Aruco Board detection Laptop Camera or Tello Drone Camera
# telloDroneEnabled = 1 : Aruco Detection with Tello Drone Camera
# telloDroneEnabled = 0 : Aruco Detection with Laptop Camera
w, h = 640, 480
#---------------- ARUCO MARKER ---------------#
# Create vectors we'll be using for rotations and translations for postures
rvec, tvec = None, None
R_ct = 0
R_tc = 0
corners = 0
#--- Get the camera calibration path
calib_path = ""
if telloDroneEnabled == 0:
cameraMatrix = np.loadtxt(calib_path+'cameraMatrix_asusWebcam.txt', delimiter=',')
distCoeffs = np.loadtxt(calib_path+'cameraDistortion_asusWebcam.txt', delimiter=',')
elif telloDroneEnabled == 1:
cameraMatrix = np.loadtxt(calib_path + 'cameraMatrix_telloCamera.txt', delimiter=',')
distCoeffs = np.loadtxt(calib_path + 'cameraDistortion_telloCamera.txt', delimiter=',')
#--- 180 deg rotation matrix around the x axis
R_flip = np.zeros((3,3), dtype=np.float32)
R_flip[0,0] = 1.0
R_flip[1,1] =-1.0
R_flip[2,2] =-1.0
#--- Define the aruco dictionary
# Constant parameters used in Aruco methods
ARUCO_PARAMETERS = aruco.DetectorParameters_create()
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_1000)
# Create grid board object we're using in our stream
board = aruco.GridBoard_create(
markersX=4,
markersY=6,
markerLength=0.06,
markerSeparation=0.01,
dictionary=ARUCO_DICT)
landingPadPosXY = [0.0] * 2
landingPadDetected = 0
landingPadLostCNTR = 0
landingPadDistanceXY = 0.0
newPadPositionReady = 0
rangefinder = 0
thresholdQR = 5
battery = 0
#------------------------------------------------------------------------------
#------- ROTATIONS https://www.learnopencv.com/rotation-matrix-to-euler-angles/
#------------------------------------------------------------------------------
# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R):
Rt = np.transpose(R)
shouldBeIdentity = np.dot(Rt, R)
I = np.identity(3, dtype=R.dtype)
n = np.linalg.norm(I - shouldBeIdentity)
return n < 1e-2
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R):
assert (isRotationMatrix(R))
sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
singular = sy < 1e-2
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:
x = math.atan2(-R[1, 2], R[1, 1])
y = math.atan2(-R[2, 0], sy)
z = 0
return np.array([x, y, z])
if telloDroneEnabled == 0:
#--- Capture the videocamera (this may also be a video or a picture)
cap = cv2.VideoCapture(0)
# -- Set the camera size as the one it was calibrated with
cap.set(cv2.CAP_PROP_FRAME_WIDTH, w)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, h)
# cap.set(cv2.CAP_PROP_FPS, 40)
elif telloDroneEnabled == 1:
me = tello.Tello()
me.connect()
print(me.get_battery())
me.streamon()
#-- Font for the text in the image
font = cv2.FONT_HERSHEY_PLAIN
prev_frame_time = 0
while True:
# -- Read the camera frame
if telloDroneEnabled == 0:
ret, frame = cap.read()
elif telloDroneEnabled == 1:
frame = me.get_frame_read().frame
frame = cv2.resize(frame, (w, h))
# -- Convert in gray scale
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # -- remember, OpenCV stores color images in Blue, Green, Red
# Detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)
# Refine detected markers
# Eliminates markers not part of our board, adds missing markers to the board
corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(
image=gray,
board=board,
detectedCorners=corners,
detectedIds=ids,
rejectedCorners=rejectedImgPoints,
cameraMatrix=cameraMatrix,
distCoeffs=distCoeffs)
# Outline all of the markers detected in our image
frame = aruco.drawDetectedMarkers(frame, corners, ids, borderColor=(0, 255, 0))
# Require 1 markers before drawing axis
if ids is not None and len(ids) > 0:
# Estimate the posture of the gridboard, which is a construction of 3D space based on the 2D video
pose, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, cameraMatrix, distCoeffs, rvec, tvec)
if pose:
# Draw the camera posture calculated from the gridboard
#tvec[2] = tvec[2] * (-1)
#frame = aruco.drawAxis(frame, cameraMatrix, distCoeffs, rvec, tvec, 0.3)
frame = cv2.drawFrameAxes(frame, cameraMatrix, distCoeffs, rvec, tvec, 0.2)
tvec[0] *= 95
tvec[1] *= 95
tvec[2] *= 95
# -- Print the tag position in camera frame
# str_position = "MARKER Position x=%4.0f y=%4.0f z=%4.0f"%(tvec[0], tvec[1], tvec[2])
# cv2.putText(frame, str_position, (0, 360), font, 1.3, (0, 255, 0), 2, cv2.LINE_AA)
# -- Obtain the rotation matrix tag->camera
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_tc = R_ct.T
# -- Get the attitude in terms of euler 321 (Needs to be flipped first)
roll_marker, pitch_marker, yaw_marker = rotationMatrixToEulerAngles(R_flip*R_tc)
#-- Print the marker's attitude respect to camera frame
# str_attitude = "MARKER Attitude r=%4.0f p=%4.0f y=%4.0f"%(math.degrees(roll_marker),math.degrees(pitch_marker),
# math.degrees(yaw_marker))
# cv2.putText(frame, str_attitude, (0, 380), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
# -- Now get Position and attitude f the camera respect to the marker
pos_camera = -R_tc * np.matrix(tvec)
str_position = "CAMERA Position x=%4.0f y=%4.0f"%(pos_camera[2], pos_camera[0])
cv2.putText(frame, str_position, (0, 430), font, 1.4, (0, 255, 0), 2, cv2.LINE_AA)
# -- Get the attitude of the camera respect to the frame
pitch_camera, yaw_camera, roll_camera = rotationMatrixToEulerAngles(R_flip * R_tc)
str_attitude = "CAMERA Attitude roll=%4.1f pitch=%4.1f yaw=%4.1f"%(math.degrees(roll_camera),math.degrees(pitch_camera),
math.degrees(yaw_camera))
cv2.putText(frame, str_attitude, (0, 460), font, 1.4, (0, 255, 0), 2, cv2.LINE_AA)
# calculate the FPS and display on frame
new_frame_time = time.time()
if telloDroneEnabled == 0:
fps = 1 / (new_frame_time - prev_frame_time)
prev_frame_time = new_frame_time
cv2.putText(frame, "FPS" + str(int(fps)), (10, 40), font, 1.3, (100, 255, 0), 2, cv2.LINE_AA)
elif telloDroneEnabled == 1:
cv2.putText(frame, "Battery:%" + str(battery), (10, 40), font, 1.3, (100, 255, 0), 2, cv2.LINE_AA)
# cv2.putText(frame, "Land OK: " + str(int(landingPadDetected)), (10, 60), font, 1.3, (100, 255, 0), 2, cv2.LINE_AA)
# --- Display the frame
cv2.imshow('frame', frame)
if telloDroneEnabled == 1:
battery = me.get_battery()
# time.sleep(0.5)
# --- use 'q' to quit
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
if telloDroneEnabled == 0:
cap.release()
cv2.destroyAllWindows()
break
