利用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)
先写这么多吧,有人看再写…