openmv之ApriTag-3D定位

AprilTag最神奇的是3D定位的功能，它可以得知Tag的空間位置，一共有6個自由度，三個位置，三個角度。

# 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 # 默認值
f_y = (2.8 / 2.952) * 120 # 默認值
c_x = 160 * 0.5 # 默認值(image.w * 0.5)
c_y = 120 * 0.5 # 默認值(image.h * 0.5)

def degrees(radians):
    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): # 默認為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()))
        # 位置的單位是未知的，旋轉的單位是角度
        print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)
    print(clock.fps())

 

在三維坐標中是以openmv為坐標原點的

apriltag. x_translation ()

返回距離攝像機x方向的變換，距離的單位未知。

這個方法對於確定遠離攝像機的AprilTag的位置很有用。但是，AprilTag的大小以及您使用的鏡頭等因素都會影響X單元歸屬的確定。為使用方便，我們推薦您使用查找表將該方法的輸出轉換為對您的應用程序有用的信息。

注意：此處的方向為從左至右。

您也可以通過索引 [12] 取得這個值。

apriltag. y_translation ()

返回距離攝像機y方向的變換，距離的單位未知。

這個方法對於確定遠離攝像機的AprilTag的位置很有用。但是，AprilTag的大小以及您使用的鏡頭等因素都會影響Y單元歸屬的確定。為使用方便，我們推薦您使用查找表將該方法的輸出轉換為對您的應用程序有用的信息。

注意：此處的方向為從上至下。

您也可以通過索引 [13] 取得這個值。

apriltag. z_translation ()

返回距離攝像機z方向的變換，距離的單位未知。

T這個方法對於確定遠離攝像機的AprilTag的位置很有用。但是，AprilTag的大小以及您使用的鏡頭等因素都會影響Z單元歸屬的確定。為使用方便，我們推薦您使用查找表將該方法的輸出轉換為對您的應用程序有用的信息。

注意：此處的方向為從前至后。

您也可以通過索引 [14] 取得這個值。

apriltag. x_rotation ()

返回以弧度計的AprilTag在X平面上的旋度。例：目視AprilTag，從左至右移動攝像頭。

您也可以通過索引 [15] 取得這個值。

apriltag. y_rotation ()

返回以弧度計的AprilTag在Y平面上的旋度。例：目視AprilTag，從上至下移動攝像頭。

您也可以通過索引 [16] 取得這個值。

apriltag. z_rotation ()

返回以弧度計的AprilTag在Z平面上的旋度。例：目視AprilTag，旋轉攝像頭。

注意：這只是 apriltag.rotation() 的重命名版本。

您也可以通過索引 [17] 取得這個值。

這些都可以在openmv的函數庫中找到

以下圖像可以放大查看，在左下角串行終端中可以看到數據的變化

對比圖

 

 

圖像右移圖tx為正

 

 圖像左移圖tx為副負

 

圖像上移圖ty為正

 

 

圖像下移圖ty為負

 

 

tz都為負因為在openmv是0在其后邊為正

tag與openmv移近

 

 

tag與openmv移遠

 

 

tag逆時針旋轉

 

openmv的實際距離 k =√(tx^2+ty^2+tz^2)