旋转Apriltag角度检测

举报
tsinghuazhuoqing 发表于 2022/01/01 23:07:51 2022/01/01
【摘要】 简 介： 从实验角度介绍了Apriltag的角度检测，与Apriltag张贴方向有关系，与摄像头内参设定也有关系。通过旋转Apriltag码方向检测验证了算法的鲁棒性。 关键词： Apriltag，...

简 介： 从实验角度介绍了Apriltag的角度检测，与Apriltag张贴方向有关系，与摄像头内参设定也有关系。通过旋转Apriltag码方向检测验证了算法的鲁棒性。

关键词： Apriltag，单应矩阵

 

§00 前  言

  在 根据Apriltag进行角度和距离检测 中对于旋转的Apriltag码进行识别和角度测试。测量得到角度稳定显示的范围还比较小，检测到的角度不是很稳定。

  通过摄像头内参进行调整，寻找可以更好适应Apriltag角度测量的参数范围。 下面给出更多的测试结果。

0.1 旋转Apriltag样本集合

▲ 图1.1 测试旋转Apriltag数据集合

▲ 图1.1 测试旋转Apriltag数据集合

 

§01 角度曲线

1.1 算法代码

1.1.1 数据准备

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
    • 
  
  • 2
    • 
  
  • 3
    • 
  
  • 4
    • 
  
  • 5
    • 
  
  • 6
    • 
  
  • 7
    • 
  
  • 8
    • 
  
  • 9
    • 
  
  • 10
    • 
  
  • 11
    • 
  
  • 12
    • 
  
  • 13
    • 
  
  • 14
    • 
  
  • 15
    • 
  
  • 16
    • 
  
  • 17
    •

1.1.2 设置镜头参数

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
    • 
  
  • 2
    • 
  
  • 3
    • 
  
  • 4
    • 
  
  • 5
    • 
  
  • 6
    • 
  
  • 7
    • 
  
  • 8
    • 
  
  • 9
    •

1.1.3 计算角度

  由于同一时刻可能检测到两个Apriltag，所以需要根据角度的连续性来确定所使用角度数值曲线。

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
    • 
  
  • 3
    • 
  
  • 4
    • 
  
  • 5
    • 
  
  • 6
    • 
  
  • 7
    • 
  
  • 8
    • 
  
  • 9
    • 
  
  • 10
    • 
  
  • 11
    • 
  
  • 12
    • 
  
  • 13
    • 
  
  • 14
    • 
  
  • 15
    • 
  
  • 16
    • 
  
  • 17
    • 
  
  • 18
    • 
  
  • 19
    • 
  
  • 20
    • 
  
  • 21
    • 
  
  • 22
    • 
  
  • 23
    • 
  
  • 24
    • 
  
  • 25
    • 
  
  • 26
    • 
  
  • 27
    • 
  
  • 28
    • 
  
  • 29
    • 
  
  • 30
    • 
  
  • 31
    • 
  
  • 32
    • 
  
  • 33
    • 
  
  • 34
    • 
  
  • 35
    • 
  
  • 36
    • 
  
  • 37
    • 
  
  • 38
    • 
  
  • 39
    • 
  
  • 40
    • 
  
  • 41
    • 
  
  • 42
    • 
  
  • 43
    • 
  
  • 44
    • 
  
  • 45
    • 
  
  • 46
    • 
  
  • 47
    • 
  
  • 48
    • 
  
  • 49
    • 
  
  • 50
    • 
  
  • 51
    • 
  
  • 52
    • 
  
  • 53
    • 
  
  • 54
    •

1.2 计算结果

  如下是整个采集到的的300张旋转Apriltag对应的角度。

▲ 图1.2.1 Apriltag角度曲线

▲ 图1.2.1 Apriltag角度曲线

  其中主要问题出现在 75 ~ 115左右，出现仅仅检测到一个Apriltag的情况。而此时对应Apriltag对应镜头应该有两个Apriltag。

  下面是仅仅只有一个Apriltag被检测到的图片集合：

▲ 图1.2.2 仅仅只有一个Apriltag被检测到的图片集合：

▲ 图1.2.2 仅仅只有一个Apriltag被检测到的图片集合：

1.3 绘制方向图像

  绘制Apriltag法向量。可以看到其中tagid=1 角度法向量显示正常，但是tagid=2法向量显示始终有问题。

▲ 图1.3.1 绘制Apriltag法向量

▲ 图1.3.1 绘制Apriltag法向量

  对照两种不同的Apriltag及其方向，可以看到在粘贴的时候，TAG25H9,id=2 Apriltag在粘贴的时候旋转了90°。 因此选择它的角度的时候，需要更换不同的轴。

▲ 图1.3.2 两种不同的Apriltag及其方向

▲ 图1.3.2 两种不同的Apriltag及其方向

1.4 修正角度

  根据检测到tag_id信息，对于角度信息进行校正。

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)

  
 
    
  
  • 1
    • 
  
  • 2
    • 
  
  • 3
    • 
  
  • 4
    • 
  
  • 5
    • 
  
  • 6
    • 
  
  • 7
    • 
  
  • 8
    • 
  
  • 9
    • 
  
  • 10
    • 
  
  • 11
    • 
  
  • 12
    • 
  
  • 13
    • 
  
  • 14
    • 
  
  • 15
    • 
  
  • 16
    • 
  
  • 17
    • 
  
  • 18
    • 
  
  • 19
    • 
  
  • 20
    • 
  
  • 21
    • 
  
  • 22
    • 
  
  • 23
    • 
  
  • 24
    • 
  
  • 25
    • 
  
  • 26
    • 
  
  • 27
    • 
  
  • 28
    • 
  
  • 29
    • 
  
  • 30
    • 
  
  • 31
    • 
  
  • 32
    • 
  
  • 33
    • 
  
  • 34
    • 
  
  • 35
    • 
  
  • 36
    • 
  
  • 37
    • 
  
  • 38
    • 
  
  • 39
    • 
  
  • 40
    • 
  
  • 41
    • 
  
  • 42
    • 
  
  • 43
    • 
  
  • 44
    • 
  
  • 45
    • 
  
  • 46
    • 
  
  • 47
    • 
  
  • 48
    • 
  
  • 49
    • 
  
  • 50
    • 
  
  • 51
    • 
  
  • 52
    • 
  
  • 53
    •

  角度修正结果：

▲ 图1.3.3 角度修正结果

▲ 图1.3.3 角度修正结果

  可以看到对于两种Apriltag码对应的角度出现了明显的改善。

▲ 图1.4.2 对应不同tag检测的数值

▲ 图1.4.2 对应不同tag检测的数值

1.5 选择摄像头内参

▲ 图1.5.1 选择新的摄像头内参对应的角度变化

▲ 图1.5.1 选择新的摄像头内参对应的角度变化

  选择新的摄像头内参的结果：

▲ 图1.4.3 选择新的摄像头内参的结果

▲ 图1.4.3 选择新的摄像头内参的结果

 

※ 检测总结 ※

  从实验角度介绍了Apriltag的角度检测，与Apriltag张贴方向有关系，与摄像头内参设定也有关系。通过旋转Apriltag码方向检测验证了算法的鲁棒性。

■ 相关文献链接:

● 相关图表链接:

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

  
 
    
  
  • 1
    • 
  
  • 2
    • 
  
  • 3
    • 
  
  • 4
    • 
  
  • 5
    • 
  
  • 6
    • 
  
  • 7
    • 
  
  • 8
    • 
  
  • 9
    • 
  
  • 10
    • 
  
  • 11
    • 
  
  • 12
    • 
  
  • 13
    • 
  
  • 14
    • 
  
  • 15
    • 
  
  • 16
    • 
  
  • 17
    • 
  
  • 18
    • 
  
  • 19
    • 
  
  • 20
    • 
  
  • 21
    • 
  
  • 22
    • 
  
  • 23
    • 
  
  • 24
    • 
  
  • 25
    • 
  
  • 26
    • 
  
  • 27
    • 
  
  • 28
    • 
  
  • 29
    • 
  
  • 30
    • 
  
  • 31
    • 
  
  • 32
    • 
  
  • 33
    • 
  
  • 34
    • 
  
  • 35
    • 
  
  • 36
    • 
  
  • 37
    • 
  
  • 38
    • 
  
  • 39
    • 
  
  • 40
    • 
  
  • 41
    • 
  
  • 42
    • 
  
  • 43
    • 
  
  • 44
    • 
  
  • 45
    • 
  
  • 46
    • 
  
  • 47
    • 
  
  • 48
    • 
  
  • 49
    • 
  
  • 50
    • 
  
  • 51
    • 
  
  • 52
    • 
  
  • 53
    • 
  
  • 54
    • 
  
  • 55
    • 
  
  • 56
    • 
  
  • 57
    • 
  
  • 58
    • 
  
  • 59
    • 
  
  • 60
    • 
  
  • 61
    • 
  
  • 62
    • 
  
  • 63
    • 
  
  • 64
    • 
  
  • 65
    • 
  
  • 66
    • 
  
  • 67
    • 
  
  • 68
    • 
  
  • 69
    • 
  
  • 70
    • 
  
  • 71
    • 
  
  • 72
    • 
  
  • 73
    • 
  
  • 74
    • 
  
  • 75
    • 
  
  • 76
    • 
  
  • 77
    • 
  
  • 78
    • 
  
  • 79
    • 
  
  • 80
    • 
  
  • 81
    • 
  
  • 82
    • 
  
  • 83
    • 
  
  • 84
    • 
  
  • 85
    • 
  
  • 86
    • 
  
  • 87
    • 
  
  • 88
    • 
  
  • 89
    • 
  
  • 90
    • 
  
  • 91
    • 
  
  • 92
    • 
  
  • 93
    • 
  
  • 94
    • 
  
  • 95
    • 
  
  • 96
    • 
  
  • 97
    • 
  
  • 98
    • 
  
  • 99
    • 
  
  • 100
    • 
  
  • 101
    • 
  
  • 102
    • 
  
  • 103
    • 
  
  • 104
    • 
  
  • 105
    • 
  
  • 106
    • 
  
  • 107
    • 
  
  • 108
    • 
  
  • 109
    • 
  
  • 110
    • 
  
  • 111
    • 
  
  • 112
    • 
  
  • 113
    • 
  
  • 114
    • 
  
  • 115
    • 
  
  • 116
    • 
  
  • 117
    • 
  
  • 118
    • 
  
  • 119
    • 
  
  • 120
    • 
  
  • 121
    • 
  
  • 122
    • 
  
  • 123
    • 
  
  • 124
    • 
  
  • 125
    • 
  
  • 126
    • 
  
  • 127
    • 
  
  • 128
    • 
  
  • 129
    • 
  
  • 130
    • 
  
  • 131
    • 
  
  • 132
    • 
  
  • 133
    • 
  
  • 134
    • 
  
  • 135
    • 
  
  • 136
    • 
  
  • 137
    • 
  
  • 138
    • 
  
  • 139
    • 
  
  • 140
    • 
  
  • 141
    • 
  
  • 142
    • 
  
  • 143
    • 
  
  • 144
    • 
  
  • 145
    • 
  
  • 146
    • 
  
  • 147
    • 
  
  • 148
    • 
  
  • 149
    • 
  
  • 150
    • 
  
  • 151
    • 
  
  • 152
    • 
  
  • 153
    • 
  
  • 154
    • 
  
  • 155
    • 
  
  • 156
    • 
  
  • 157
    • 
  
  • 158
    • 
  
  • 159
    • 
  
  • 160
    • 
  
  • 161
    • 
  
  • 162
    • 
  
  • 163
    • 
  
  • 164
    • 
  
  • 165
    • 
  
  • 166
    • 
  
  • 167
    • 
  
  • 168
    • 
  
  • 169
    • 
  
  • 170
    • 
  
  • 171
    • 
  
  • 172
    • 
  
  • 173
    • 
  
  • 174
    • 
  
  • 175
    • 
  
  • 176
    • 
  
  • 177
    • 
  
  • 178
    • 
  
  • 179
    • 
  
  • 180
    • 
  
  • 181
    • 
  
  • 182
    • 
  
  • 183
    • 
  
  • 184
    • 
  
  • 185
    • 
  
  • 186
    • 
  
  • 187
    • 
  
  • 188
    • 
  
  • 189
    •

文章来源: zhuoqing.blog.csdn.net，作者：卓晴，版权归原作者所有，如需转载，请联系作者。

原文链接：zhuoqing.blog.csdn.net/article/details/122264698

【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱： cloudbbs@huaweicloud.com
  • 点赞
  • 收藏
  • 关注作者

评论（0）

0/1000
抱歉，系统识别当前为高风险访问，暂不支持该操作

全部回复

上滑加载中

设置昵称

在此一键设置昵称，即可参与社区互动！

*长度不超过10个汉字或20个英文字符，设置后3个月内不可修改。

*长度不超过10个汉字或20个英文字符，设置后3个月内不可修改。

确认 取消

加入云驻计划，成为创作者

  • 华为云周边好礼
  • 免费体验产品
  • 特殊身份标识
  • 线下官方门票
  • 内部专家零距离
  • 与10000+优质创作者共同成长
立即加入