# AprilTag标记跟踪

## AprilTag简介

AprilTag是一个视觉基准系统，可用于各种任务，包括AR，机器人和相机校准。这个tag可以直接用打印机打印出来，而AprilTag检测程序可以计算相对于相机的精确3D位置，方向和id。在机器人领域有广泛应用．

AprilTag的算法，　可以计算出Tag在3维空间中的位置, 与其对应的ID

## AprilTag的种类

AprilTag的种类叫家族（family）,有下面的几种：

TagName 起始编码 结束编码
TAG16H5 0 29
TAG25H7 0 241
TAG25H9 0 34
TAG36H10 0 2319
TAG36H11 0 586
ARTOOLKIT 0 511

TAG+比特数+H+最小汉明距离

## AprilTag算法过程描述

TODO 待翻译

tekkotsu-AprilTags

The algorithm has nine steps:

1. Convert the image to floating point grayscale (pixel values between 0.0 and 1.0) and apply a Gaussian blur.

2. Calculate the local gradient (magnitude and direction) at each pixel.

3. Generate a list of edges, grouping connected pixels with similar directions together. An edge is present if the magnitude of the gradient for both pixels is significantly above zero.

4. Create clusters from the edges.

5. Loop over the clusters, fitting lines called Segments.

6. For each Segment, find segments that begin where this segment ends.

7. Search all connected segments to find loops of length 4, called Quads. Each quad represents the black border around a tag candidate.

8. Decode the quads by looking at the pixels inside the border to see if they represent a valid tag code, and generate a list of TagDetections

9. Search for overlapping TagDetections and take the best ones (lowest Hamming distance or largest perimeter); discard the rest.

## CODE - 寻找AprilTags

# 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(time = 2000)
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()

# Note! Unlike find_qrcodes the find_apriltags method does not need lens correction on the image to work.
# 注意, 不像寻找二维码(find_qrcodes)的例程那样, 寻找AprilTag方法, 不需要进行镜头校正(lens correlation)

# The apriltag code supports up to 6 tag families which can be processed at the same time.
# Returned tag objects will have their tag family and id within the tag family.
# OpenMV AprilTag识别函数, 支持同时识别6种Family家族的Tag.
# 返回对象信息包含Tag Family的名称, Tag ID
tag_families = 0
# 通过或位运算, 来决定是否识别某一种Family的Tag, 可以把不需要的注释掉
tag_families |= image.TAG16H5 # comment out to disable this family
tag_families |= image.TAG25H7 # comment out to disable this family
tag_families |= image.TAG25H9 # comment out to disable this family
tag_families |= image.TAG36H10 # comment out to disable this family
tag_families |= image.TAG36H11 # comment out to disable this family (default family)
tag_families |= image.ARTOOLKIT # comment out to disable this family

# What's the difference between tag families? Well, for example, the TAG16H5 family is effectively
# a 4x4 square tag. So, this means it can be seen at a longer distance than a TAG36H11 tag which
# is a 6x6 square tag. However, the lower H value (H5 versus H11) means that the false positve
# rate for the 4x4 tag is much, much, much, higher than the 6x6 tag. So, unless you have a
# reason to use the other tags families just use TAG36H11 which is the default family.

def family_name(tag):
if(tag.family() == image.TAG16H5):
return "TAG16H5"
if(tag.family() == image.TAG25H7):
return "TAG25H7"
if(tag.family() == image.TAG25H9):
return "TAG25H9"
if(tag.family() == image.TAG36H10):
return "TAG36H10"
if(tag.family() == image.TAG36H11):
return "TAG36H11"
if(tag.family() == image.ARTOOLKIT):
return "ARTOOLKIT"

while(True):
clock.tick()
img = sensor.snapshot()
for tag in img.find_apriltags(families=tag_families): # defaults to TAG36H11 without "families".
img.draw_rectangle(tag.rect(), color = (255, 0, 0))
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
print_args = (family_name(tag), tag.id(), (180 * tag.rotation()) / math.pi)
print("Tag Family %s, Tag ID %d, rotation %f (degrees)" % print_args)
print(clock.fps())


TODO 演示视频/照片/样例输出

# 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.VGA) # we run out of memory if the resolution is much bigger...
sensor.set_windowing((160, 120)) # Look at center 160x120 pixels of the VGA resolution.
sensor.skip_frames(time = 2000)
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()

# Note! Unlike find_qrcodes the find_apriltags method does not need lens correction on the image to work.

# What's the difference between tag families? Well, for example, the TAG16H5 family is effectively
# a 4x4 square tag. So, this means it can be seen at a longer distance than a TAG36H11 tag which
# is a 6x6 square tag. However, the lower H value (H5 versus H11) means that the false positve
# rate for the 4x4 tag is much, much, much, higher than the 6x6 tag. So, unless you have a
# reason to use the other tags families just use TAG36H11 which is the default family.

while(True):
clock.tick()
img = sensor.snapshot()
for tag in img.find_apriltags(): # defaults to TAG36H11
img.draw_rectangle(tag.rect(), color = (255, 0, 0))
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
print_args = (tag.id(), (180 * tag.rotation()) / math.pi)
print("Tag Family TAG36H11, Tag ID %d, rotation %f (degrees)" % print_args)
print(clock.fps())


## CODE - AprilTag 3维空间

AprilTag可以得知Tag的空间位置，一共有6个自由度，三个位置(Translation)自由度，三个角度(Degree)自由度。

TODO Translation 应该不是位置信息, 求具体准确翻译.

TODO Translation & Rotaion 两个矩阵/值代表的含义, 解释

# 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(time = 2000)
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()

# The AprilTags library outputs the pose information for tags. This is the x/y/z translation and
# x/y/z rotation. The x/y/z rotation is in radians and can be converted to degrees. As for
# translation the units are dimensionless and you must apply a conversion function.
# translation 没有单位, 所以必须添加一个转换函数

# f_x is the x focal length of the camera. It should be equal to the lens focal length in mm
# divided by the x sensor size in mm times the number of pixels in the image.
# The below values are for the

# f_y is the y focal length of the camera. It should be equal to the lens focal length in mm
# divided by the y sensor size in mm times the number of pixels in the image.
# The below values are for the OV7725 camera with a 2.8 mm lens.

# x,y焦距 计算方法
# f_x = (焦距 / 感光芯片纵向高度) * 纵向像素个数
# f_y = (焦距 / 感光芯片横向高度) * 横向向像素个数
# 下列值针对OV7725 摄像头 与 2.8 mm 镜头与QQVGA分辨率(160*120)
# 如果更换用其他镜头之后, 需要将2.8更新为当前镜头的焦距
f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not set
f_y = (2.8 / 2.952) * 120 # find_apriltags defaults to this if not set
# (c_x, c_y)为画面的中心点坐标
c_x = 160 * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)

# 弧度值转换为角度值的函数
return (180 * radians) / math.pi

while(True):
clock.tick()
img = sensor.snapshot()
for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H11
img.draw_rectangle(tag.rect(), color = (255, 0, 0))
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation(), \
degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
# Translation units are unknown. Rotation units are in degrees.
# Translation 单位是未知的, Rotaion的单位是角度
print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)
print(clock.fps())