利用OpenMv测离Apriltag的距离(其他色块啥的算法都差不多,主要是Apriltag精确一些)
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)
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)
s=math.sqrt(tag.x_translation()**2+tag.y_translation()**2+tag.z_translation()**2)
length=s*k
chang=length
print("Tx: %f, Ty %f, Tz %f" % print_args)
print('length: %f' %length)
先写这么多吧,有人看再写…
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)