Introduction: This paper introduces the angle detection of Apriltag from the experimental point of view, which is related to the posting direction of Apriltag and the internal parameter setting of camera. The robustness of the algorithm is verified by rotating Apriltag code direction detection.
Key words: Apriltag, homography matrix
§ 00 preamble
in Angle and distance detection according to Apriltag The rotating Apriltag code is identified and angle tested. The measured angle is stable, the display range is still relatively small, and the detected angle is not very stable.
adjust the internal parameters of the camera to find the parameter range that can better adapt to the angle measurement of Apriltag. More test results are given below.
0.1 rotate the Apriltag sample set
-
APR8: select in Rotating Apriltag code The collected APR8 sample set.
-
Data collection download link : https://aistudio.baidu.com/aistudio/datasetdetail/123271
§ 01 angle curve
1.1 algorithm code
1.1.1 data preparation
import sys,os,math,time
import matplotlib.pyplot as plt
from numpy import *
import cv2
import apriltag
from tqdm import tqdm
import zipfile
outdir = '/home/aistudio/data'
zzipfile = '/home/aistudio/data/data123271/apr8.zip'
aprdir = os.path.join(outdir, os.path.basename(zzipfile).split('.')[0])
if not os.path.isdir(aprdir):
with zipfile.ZipFile(zzipfile) as f:
f.extractall(outdir)
filedim = [s for s in sorted(os.listdir(aprdir)) if s.find('JPG') > 0]
1.1.2 setting lens parameters
f = 3e3 px = 6e2 py = 2e2 mtx = array( [[f,0.00000000e+00,px], [0.00000000e+00,f,py], [0.00000000e+00,0.00000000e+00,1.00000000e+00]] )
1.1.3 calculation angle
since two apriltags may be detected at the same time, it is necessary to determine the angle value curve used according to the continuity of the angle.
atd = apriltag.Detector(apriltag.DetectorOptions(families='tag25h9'))
angledim =[[],[],[],[]]
lastangle = 100000
tagnum = []
for id,imgfile in tqdm(enumerate(filedim)):
procfile = os.path.join(aprdir, imgfile)
img = cv2.imread(procfile)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
tags = atd.detect(gray)
if len(tags) == 0: continue
angle1 = 0
angle2 = 0
tagnum.append(len(tags)*25)
for iidd,tag in enumerate(tags):
homo = tag.homography
num,Rs,Ts,Ns = cv2.decomposeHomographyMat(homo, mtx)
r = R.from_dcm(Rs[0].T)
euler = r.as_euler('xyz').T*180/pi
angle = euler[2]
if iidd == 0: angle1 = angle
else: angle2 = angle
angleA = angle1
if len(tags) > 1:
if abs(angle1-lastangle) < abs(angle2-lastangle):
angleA = angle1
else: angleA = angle2
lastangle = angleA
angledim[0].append(angleA)
angledim[1].append(angleA)
angledim[2].append(angleA)
angledim[3].append(angleA)
print("shape(angledim): {}".format(shape(angledim)))
plt.clf()
plt.figure(figsize=(10,6))
plt.plot(angledim[0], label='L1')
plt.plot(tagnum, label='num')
plt.xlabel("Step")
plt.ylabel("Angle")
plt.grid(True)
plt.legend(loc='upper right')
plt.tight_layout()
plt.show()
1.2 calculation results
the following is the angle corresponding to the 300 rotating Apriltag collected.
the main problem is about 75 ~ 115, and only one Apriltag is detected. At this time, the corresponding lens should have two apriltags.
the following is a collection of pictures in which only one Apriltag is detected:
1.3 drawing direction image
draw the Apriltag normal vector. It can be seen that the display of angle normal vector with tagid=1 is normal, but the display of normal vector with tagid=2 is always problematic.
comparing two different apriltags and their directions, it can be seen that TAG25H9,id=2 Apriltag rotates 90 ° during pasting. Therefore, when selecting its angle, it is necessary to replace different shafts.
1.4 correction angle
according to the detected tag_id information and correct the angle information.
gifpath = '/home/aistudio/GIF'
gifdim = os.listdir(gifpath)
for f in gifdim:
fn = os.path.join(gifpath, f)
if os.path.isfile(fn):
os.remove(fn)
atd = apriltag.Detector(apriltag.DetectorOptions(families='tag25h9'))
for id,imgfile in tqdm(enumerate(filedim)):
procfile = os.path.join(aprdir, imgfile)
img = cv2.imread(procfile)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
tags = atd.detect(gray)
if len(tags) == 0: continue
for iidd,tag in enumerate(tags):
homo = tag.homography
num,Rs,Ts,Ns = cv2.decomposeHomographyMat(homo, mtx)
if tag.tag_id == 0:
r = R.from_dcm(Rs[0].T)
euler = r.as_euler('xyz').T*180/pi
angle = euler[2]
else:
r = R.from_dcm(Rs[0].T)
euler = r.as_euler('yxz').T*180/pi
angle = euler[0]
for c in tag.corners:
cv2.circle(img, tuple(c.astype(int)), 4, (255, 0, 0), 2)
cc = tag.center
cv2.circle(img, tuple(cc.astype(int)), 6, (20, 200, 120), 2)
ARROW_LENGTH = 150
shiftx = sin(angle*pi/180) * ARROW_LENGTH
shifty = ARROW_LENGTH / 2 * cos(angle*pi/180)
newcenter = array([shiftx, shifty]) + cc
cv2.circle(img, tuple(newcenter.astype(int)), 8, (0, 0, 255), 5)
cv2.line(img, tuple(newcenter.astype(int)), tuple(cc.astype(int)), (0, 0, 255), 2)
outfile = os.path.join(gifpath, '%03d.JPG'%id)
cv2.imwrite(outfile, img)
plt.clf()
plt.figure(figsize=(12,12))
plt.axis("off")
plt.imshow(img)
angle correction result:
it can be seen that the corresponding angles of the two Apriltag codes have been significantly improved.
1.5 select camera internal parameters
results of selecting new camera internal parameters:
※ test summary ※
the angle detection of Apriltag is introduced from the experimental point of view, which is related to the posting direction of Apriltag and the internal parameter setting of camera. The robustness of the algorithm is verified by rotating Apriltag code direction detection.
■ links to relevant literature:
- Angle and distance detection according to Apriltag
- Rotating Apriltag code
- Data collection download link
● relevant chart links:
- Figure 1.1 test rotation Apriltag data set
- Figure 1.2.1 Apriltag angle curve
- Figure 1.2.2 picture set with only one Apriltag detected:
- Figure 1.3.1 drawing the Apriltag normal vector
- Figure 1.3.2 two different apriltags and their directions
- Figure 1.3.3 angle correction results
- Figure 1.4.2 values corresponding to different tag tests
- Figure 1.5.1 angle change corresponding to selecting a new camera internal parameter
- Figure 1.4.3 results of selecting new camera internal parameters
#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST1.PY -- by Dr. ZhuoQing 2021-12-31
#
# Note:
#============================================================
from headm import * # =
import cv2
import apriltag
from tqdm import tqdm
import zipfile
#------------------------------------------------------------
outdir = '/home/aistudio/data'
zzipfile = '/home/aistudio/data/data123271/apr8.zip'
aprdir = os.path.join(outdir, os.path.basename(zzipfile).split('.')[0])
if not os.path.isdir(aprdir):
with zipfile.ZipFile(zzipfile) as f:
f.extractall(outdir)
filedim = [s for s in sorted(os.listdir(aprdir)) if s.find('JPG') > 0]
#printt(filedim:)
#------------------------------------------------------------
#mtx = array(
#[[1.50786300e+04,0.00000000e+00,6.54543821e+02],
# [0.00000000e+00,1.50723843e+04,3.14862050e+02],
# [0.00000000e+00,0.00000000e+00,1.00000000e+00]]
#)
f = 1.5e3
px = 6e2
py = 2e2
mtx = array(
[[f,0.00000000e+00,px],
[0.00000000e+00,f,py],
[0.00000000e+00,0.00000000e+00,1.00000000e+00]]
)
#------------------------------------------------------------
'''
gifpath = '/home/aistudio/GIF'
gifdim = os.listdir(gifpath)
for f in gifdim:
fn = os.path.join(gifpath, f)
if os.path.isfile(fn):
os.remove(fn)
atd = apriltag.Detector(apriltag.DetectorOptions(families='tag25h9'))
for id,imgfile in tqdm(enumerate(filedim)):
procfile = os.path.join(aprdir, imgfile)
img = cv2.imread(procfile)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
tags = atd.detect(gray)
if len(tags) == 0: continue
for iidd,tag in enumerate(tags):
homo = tag.homography
num,Rs,Ts,Ns = cv2.decomposeHomographyMat(homo, mtx)
if tag.tag_id == 0:
r = R.from_dcm(Rs[0].T)
euler = r.as_euler('xyz').T*180/pi
angle = euler[2]
else:
r = R.from_dcm(Rs[0].T)
euler = r.as_euler('yxz').T*180/pi
angle = euler[0]
for c in tag.corners:
cv2.circle(img, tuple(c.astype(int)), 4, (255, 0, 0), 2)
cc = tag.center
cv2.circle(img, tuple(cc.astype(int)), 6, (20, 200, 120), 2)
ARROW_LENGTH = 150
shiftx = sin(angle*pi/180) * ARROW_LENGTH
shifty = ARROW_LENGTH / 2 * cos(angle*pi/180)
newcenter = array([shiftx, shifty]) + cc
cv2.circle(img, tuple(newcenter.astype(int)), 8, (0, 0, 255), 5)
cv2.line(img, tuple(newcenter.astype(int)), tuple(cc.astype(int)), (0, 0, 255), 2)
outfile = os.path.join(gifpath, '%03d.JPG'%id)
cv2.imwrite(outfile, img)
plt.clf()
plt.figure(figsize=(12,12))
plt.axis("off")
plt.imshow(img)
'''
#------------------------------------------------------------
atd = apriltag.Detector(apriltag.DetectorOptions(families='tag25h9'))
angledim =[[],[],[],[]]
lastangle = 100000
tagnum = []
for id,imgfile in tqdm(enumerate(filedim)):
procfile = os.path.join(aprdir, imgfile)
img = cv2.imread(procfile)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
tags = atd.detect(gray)
if len(tags) == 0: continue
angle1 = 0
angle2 = 0
tagnum.append(len(tags)*25)
for iidd,tag in enumerate(tags):
homo = tag.homography
num,Rs,Ts,Ns = cv2.decomposeHomographyMat(homo, mtx)
if tag.tag_id == 0:
r = R.from_dcm(Rs[0].T)
euler = r.as_euler('xyz').T*180/pi
angle = euler[2]
else:
r = R.from_dcm(Rs[0].T)
euler = r.as_euler('yxz').T*180/pi
angle = euler[0]
if iidd == 0: angle1 = angle
else: angle2 = angle
angleA = angle1
if len(tags) > 1:
if abs(angle1-lastangle) < abs(angle2-lastangle):
angleA = angle1
else: angleA = angle2
lastangle = angleA
angledim[0].append(angleA)
angledim[1].append(angleA)
angledim[2].append(angleA)
angledim[3].append(angleA)
# for i in range(4):
# r = R.from_dcm(Rs[i].T)
# euler = r.as_euler('xyz').T*180/pi
# angle = euler[2]
# angledim[i].append(angle)
printt(shape(angledim):)
#------------------------------------------------------------
plt.clf()
plt.figure(figsize=(10,6))
plt.plot(angledim[0], label='L1')
plt.plot(tagnum, label='num')
#plt.plot(angledim[1][:200], label='L2')
#plt.plot(angledim[2], label='L3')
#plt.plot(angledim[3], label='L4')
plt.xlabel("Step")
plt.ylabel("Angle")
plt.grid(True)
plt.legend(loc='upper right')
plt.tight_layout()
plt.show()
#------------------------------------------------------------
# END OF FILE : TEST1.PY
#============================================================