AprilTags测试代码.py
# AprilTags Example
#
# This example shows the power of the OpenMV Cam to detect April Tags
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.
import sensor, image, time, math
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
sensor.skip_frames(30)
sensor.set_auto_gain(False) # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False) # must turn this off to prevent image washout...
clock = time.clock()
while(True):
clock.tick()
img = sensor.snapshot()
for tag in img.find_apriltags(): # 寻找Apriltages的函数
img.draw_rectangle(tag.rect(), color = (255, 0, 0)) #在找到的Apriltages上画框框
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0)) #在中心(cx,cy)上画十字
degress = 180 * tag.rotation() / math.pi #求Apriltages旋转的角度（tag.rotation()返回以弧度计的AprilTag的旋度(int)，我们要改成角度）
print(tag.id(),degress) #打印找到的ID和旋转的角度
AprilTags3D测距.py
# AprilTags Example
#
# This example shows the power of the OpenMV Cam to detect April Tags
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.
import sensor, image, time, math
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
sensor.skip_frames(30)
sensor.set_auto_gain(False) # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False) # must turn this off to prevent image washout...
clock = time.clock()
# 注意！与find_qrcodes不同，find_apriltags 不需要软件矫正畸变就可以工作。
# 注意，输出的姿态的单位是弧度，可以转换成角度，但是位置的单位是和你的大小有关，需要等比例换算
# f_x 是x的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/3.984*656，这个值是用毫米为单位的焦距除以x方向的感光元件的长度，乘以x方向的感光元件的像素（OV7725）
# f_y 是y的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/2.952*488，这个值是用毫米为单位的焦距除以y方向的感光元件的长度，乘以y方向的感光元件的像素（OV7725）
# c_x 是图像的x中心位置
# c_y 是图像的y中心位置
f_x = (2.8 / 3.984) * 160 # 默认值 #2.8是标配镜头的焦距，3.984和2.952是感光元件的长宽，160和120是视野中像素的大小（我们用的分辨率是QQVGA=160*120）
f_y = (2.8 / 2.952) * 120 # 默认值 #f_x、f_y分别是以x,y的像素为单位的焦距，一般不需要更改！！！（与OpenMV感光元件的尺寸有关）
c_x = 160 * 0.5 # 默认值(image.w * 0.5)
c_y = 120 * 0.5 # 默认值(image.h * 0.5) #cx、cy是整个图像的中心位置，不是roi(感兴趣区)的中心位置！
#定义一个弧度转化为角度的函数
def degrees(radians):
return (180 * radians) / math.pi #（180°×弧度）/Π
while(True):
clock.tick()
img = sensor.snapshot()
#同样是在while中循环调用查找AprilTags的函数
for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # 如果不传递家族号则默认为TAG36H11
img.draw_rectangle(tag.rect(), color = (255, 0, 0)) #在目标周围画框框
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0)) #在目标中心画十字
#apriltag.rotation()返回以"弧度"计的AprilTag的旋度(int)。例：目视AprilTag，从左至右移动摄像头
#apriltag.x_translation()返回距离摄像机x方向的变换
print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation(),
degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
# 位置的单位是未知的，旋转的单位是角度
print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)
print(clock.fps())
#很明显我们可以看出，打印出的Tz不但是个负数，也根本不是我们距离的数值！和测距一样，我们需要用一个比例系数K来转换成实际的距离
#Kz = 实际距离(找一个参照物，用尺子量出) / Tz
#实际距离Z = Kz * Tz
#以上仅仅是z方向的距离！
#我们想要打印出AprilTags距离OpenMV的实际距离，用的是差不多的方法
#K = 实际距离(找一个参照物，用尺子量出) / 根号下(Tz*Tz + Ty*Ty + Tz*Tz) ->勾股定理
#求出比例系数K
#实际距离L = K * math.sqrt(Tz*Tz + Ty*Ty + Tz*Tz)
