OpenMv测距（Apriltag）

利用OpenMv测离Apriltag的距离（其他色块啥的算法都差不多，主要是Apriltag精确一些）

#本次利用OpenMv单目测距Apriltag离摄像头的距离，测试结果表明在距离小于50cm时测距较准，大于50cm误差就很大了
import sensor, image, time, math
from pyb import Servo
from pyb import UART

uart=UART(4,115200)
s3 = Servo(3)
sensor.reset()
sensor.set_framerate(0<<9|3<<11)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(90)#上面这些都是OpenMv里面的sensor模块
sensor.set_auto_gain(False)#自动增益需要关掉
sensor.set_auto_whitebal(False)#白平衡需要关掉
clock = time.clock()
f_x = (2.8 / 3.984) * 160
f_y = (2.8 / 2.952) * 120
c_x = 160 * 0.5
c_y = 120 * 0.5
chang=0 #初始化距离为零 

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):
        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())
        k=20/math.sqrt(1.4484983**2+0.7051393**2+7.4014117**2)#制作一个标准的比例，我这里是设置的20cm, math.sqrt()里面的三个参数是OpenMv摄像头中Apritag的三维坐标，可打印出来。
		#我这里选取了20cm作为标准距离，直尺量出摄像头离OpenMv的距离（20cm）（这里最好是垂直距离，误差较小）        
        s=math.sqrt(tag.x_translation()**2+tag.y_translation()**2+tag.z_translation()**2)#未知距离时的apriltag的坐标
        length=s*k#利用成像原理数学公式计算真实长度
        chang=length
        print("Tx: %f, Ty %f, Tz %f" % print_args)
        print('length: %f' %length)

先写这么多吧，有人看再写…