# Rotation Apriltag angle detection

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

▲ Figure 1.1 test rotation Apriltag data set

# § 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)
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.

▲ figure 1.2.1 Apriltag angle curve

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:

▲ figure 1.2.2 picture set with only one Apriltag 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.

▲ figure 1.3.1 drawing the Apriltag normal vector

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.

▲ figure 1.3.2 two different apriltags and their directions

## 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)
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:

▲ figure 1.3.3 angle correction results

it can be seen that the corresponding angles of the two Apriltag codes have been significantly improved.

▲ figure 1.4.2 values corresponding to different tag tests

## 1.5 select camera internal parameters

▲ figure 1.5.1 select the angle change corresponding to the new camera internal parameter

results of selecting new camera internal parameters:

▲ figure 1.4.3 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.

```#!/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)
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)
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
#============================================================
```

Keywords: Deep Learning

Added by garry on Sat, 01 Jan 2022 00:23:47 +0200