利用棋盘格重新校正摄像头，求取Apriltag取向
【摘要】
简 介： 利用Apriltag提取过程中的单应矩阵（homography）可以获得单个Apriltag的角度。通过上述测试可以看到对于Apriltag的法向量基本与镜头夹角不大的情况下，测量角度比较...
简 介： 利用Apriltag提取过程中的单应矩阵（homography）可以获得单个Apriltag的角度。通过上述测试可以看到对于Apriltag的法向量基本与镜头夹角不大的情况下，测量角度比较准确。一旦当Apriltag的法向量与镜头夹角增大到一定程度，求出的法向量的夹角会出现较大的误差。
关键词： Apriltag，单应矩阵
§00 背 景
在 旋转Apriltag角度检测 中测试Apriltag定位立方体使用的是 旋转的Apriltag码 制作贴有Apriltag的立方体。在测试过程中发现
- Apriltag张贴的时候应该是方向保持向上一致；
- 需要更加精确对于摄像头的内参和外参进行校正；
下面工作：
- 制作更大的立方体制作Apriltag的定位立方体；
- 使用更大的棋盘格进行视野校正。
§01 定位立方体
1.1 六面体盒子
今天（2022-01-01）收到了来自百度在 WAVE Summit 平行论坛 纪念品，其中有一个特色魔方的盒子，呈现正六边形，适合制作Apriltag定位立方体。
- 立方盒的边长为：73mm。
▲ 图1.1.1 特色魔方包装盒
1.2 打印Apriltag码
在 APRILTAG 标准图片：TAG25H9 下载TAG25H9 ID=1图标。
▲ 图1.2.1 TAG25H9 id=1 Apriltag 图标
根据 竞赛规则对于定位Apriltag尺寸参数 要求，Apriltag的边长应该为立方体的0.9倍。所以打印的Apriltag的边长应该为： 73 × 0.9 ≈ 66 m m 73 \times 0.9 \approx 66mm 73×0.9≈66mm。
根据A4纸的边长为210mm，可以得到对应的编辑图片尺寸。
▲ 图1.2.2 编辑打印出编程为66mm的Apriltag
▲ 图1.2.3 打印之后的Apriltag码
1.3 粘贴制作定位立方体
使用胶水棒将打印的Apriltag码粘接在立方体盒子上。旁边还有一个粘贴在圆柱体上的Apriltag码，用于测试识别问题。
▲ 图1.2.4 粘贴好的Apriltag的定位立方体
§02 摄像机校正
2.1 拍摄棋盘格
棋盘格是 棋盘格氧化铝标定板漫反射不反光12×9方格视觉光学校正板 。手边有两种棋盘格：
- GP100参数：
-
角点矩阵：11×8
方格边长：6mm
▲ 图2.1.1 GP100定标棋盘格
- GP070参数：
-
角点点阵：11×8
方格边长：5mm
▲ 图2.1.3 GP070定标棋盘格
2.2 矫正程序
from headm import * # =
import cv2
cbfile1 = '/home/aistudio/work/GP100.jpg'
cbfile2 = '/home/aistudio/work/chessb1.jpg'
CORNER_NUM = (11,8)
def calcam(cbfile, gridsize=5):
img = cv2.imread(cbfile)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, CORNER_NUM, None)
if ret:
cv2.drawChessboardCorners(img, CORNER_NUM, corners, ret)
plt.clf()
plt.figure(figsize=(12,12))
plt.axis("off")
plt.imshow(img)
obj_p = zeros((CORNER_NUM[0] * CORNER_NUM[1], 3), float32)
obj_p[:,:2] = (mgrid[0:CORNER_NUM[0], 0:CORNER_NUM[1]].T.reshape(-1,2))*gridsize
obj_points = []
obj_points.append(obj_p)
img_points = []
img_points.append(corners)
ret,mtx,dist,rvecs,tvecs = cv2.calibrateCamera(obj_points, img_points, CORNER_NUM, None, None)
return ret,mtx,dist,rvecs,tvecs
ret,mtx,dist,rvecs,tvecs = calcam(cbfile1,6)
printt(ret:,mtx\,dist:,rvecs\,tvecs)
- 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
2.3 摄像头
存在两个摄像头，分别拍摄了上述两个棋盘格校正图片。因此下面在进行校正的时候，所得到的校正参数可能不同。
2.3.1 摄像头1校正
▲ 图2.3.1 GP100角点提取
ret: 0.12002146401949448
mtx:
[[5.94399097e+03 0.00000000e+00 6.82760115e+00]
[0.00000000e+00 6.36697742e+03 9.10752915e-01]
[0.00000000e+00 0.00000000e+00 1.00000000e+00]]
dist: [[ -1.13942937 11.40997843 0.0427266 0.06904507 -33.65814797]]
rvecs:
[array([[-0.18694408],
[ 0.7794577 ],
[-3.03463076]])]
[array([[ 108.09875682],
[ 78.08952974],
[1372.04963641]])]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
2.3.2 摄像头2校正
▲ 图2.3.2 GP070角点提取
ret: 0.16806419784453866
mtx:
[[ 1.82831585e+04 0.00000000e+00 1.44044114e+02]
[ 0.00000000e+00 1.93289214e+04 -1.85425454e+01]
[ 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
dist: [[-5.35721122e-01 4.45459910e+02 6.54421136e-04 -6.56847763e-03
1.52761970e+00]]
rvecs:
[array([[ 0.44598183],
[ 0.05775509],
[-0.02442412]])]
[array([[ 19.31487417],
[ 30.2872056 ],
[2350.10439739]])]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
§03 旋转Apriltag
根据 旋转Apriltag角度检测 中的测试方式，测量Apriltag的角度。
3.1 测量Apriltag的角度
3.1.1 摄像头1测量序列
下面是摄像头1测量得到的旋转Apriltag序列。
▲ 图3.1.1 摄像头1测量的旋转Apriltag序列
3.1.2 摄像头2测量序列
摄像头2拍摄的Apriltag序列：
▲ 图3.1.2 摄像头2拍摄的Apriltag序列
▲ 图3.1.3 动态提取Apriltag法向量
▲ 图3.1.4 Apriltag方向提取结果
※ 总 结 ※
利用Apriltag提取过程中的单应矩阵（homography）可以获得单个Apriltag的角度。通过上述测试可以看到对于Apriltag的法向量基本与镜头夹角不大的情况下，测量角度比较准确。一旦当Apriltag的法向量与镜头夹角增大到一定程度，求出的法向量的夹角会出现较大的误差。
■ 相关文献链接:
- 旋转Apriltag角度检测
- 旋转的Apriltag码
- WAVE SUMMIT平行论坛 ：产教融合，人才共育
- APRILTAG 标准图片：TAG25H9
- 竞赛规则对于定位Apriltag尺寸参数
- 棋盘格氧化铝标定板漫反射不反光12*9方格视觉光学校正板
● 相关图表链接:
- 图1.1.1 特色魔方包装盒
- 图1.2.1 TAG25H9 id=1 Apriltag 图标
- 图1.2.2 编辑打印出编程为66mm的Apriltag
- 图1.2.3 打印之后的Apriltag码
- 图1.2.4 粘贴好的Apriltag的定位立方体
- 图2.1.1 GP100定标棋盘格
- 图2.1.3 GP070定标棋盘格
- 图2.3.1 GP100角点提取
- 图2.3.2 GP070角点提取
- 图3.1.1 摄像头1测量的旋转Apriltag序列
- 图3.1.2 摄像头2拍摄的Apriltag序列
- 图3.1.3 动态提取Apriltag法向量
- 图3.1.4 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
from scipy.spatial.transform import Rotation as R
#------------------------------------------------------------
'''
outdir = '/home/aistudio/data'
zzipfile = '/home/aistudio/data/data123271/rap1.zip'
aprdir = os.path.join(outdir, os.path.basename(zzipfile).split('.')[0])
if not os.path.isdir(aprdir):
printf('Open file {}'.format(aprdir))
with zipfile.ZipFile(zzipfile) as f:
f.extractall(outdir)
printf('Extract all.')
'''
#------------------------------------------------------------
#aprdir = '/home/aistudio/data/220101155505'
aprdir = '/home/aistudio/data/data123271/220101160051'
filedim = [s for s in sorted(os.listdir(aprdir)) if s.find('JPG') > 0]
#printt(filedim:)
#------------------------------------------------------------
'''
mtx = array(
[[5.94399097e+03,0.00000000e+00,6.82760115e+00],
[0.00000000e+00,6.36697742e+03,9.10752915e-01],
[0.00000000e+00,0.00000000e+00,1.00000000e+00]]
)
'''
mtx = array(
[[1.82831585e+04,0.00000000e+00,1.44044114e+02],
[0.00000000e+00,1.93289214e+04,-1.85425454e+01],
[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]
r = R.from_dcm(Rs[0].T)
euler = r.as_euler('xyz').T*180/pi
angle = euler[2]
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 = 250
shiftx = sin(angle*pi/180) * ARROW_LENGTH
shifty = ARROW_LENGTH * cos(euler[1]*pi/180) * cos(angle*pi/180) / 2
newcenter = array([shiftx, shifty]) + cc
cv2.circle(img, tuple(newcenter.astype(int)), 16, (0, 0, 255), 8)
cv2.line(img, tuple(newcenter.astype(int)), tuple(cc.astype(int)), (0, 0, 255), 4)
# break
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
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
文章来源: zhuoqing.blog.csdn.net，作者：卓晴，版权归原作者所有，如需转载，请联系作者。
原文链接：zhuoqing.blog.csdn.net/article/details/122266415
【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱：
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论（0）