find_apriltags_3d_pose#
# AprilTags Example
#
# This example shows the power of the CanMV Cam to detect April Tags.
from media.camera import *
from media.display import *
from media.media import *
import time, math, os, gc, sys
DISPLAY_WIDTH = ALIGN_UP(1920, 16)
DISPLAY_HEIGHT = 1080
SCALE = 4
DETECT_WIDTH = DISPLAY_WIDTH // SCALE
DETECT_HEIGHT = DISPLAY_HEIGHT // SCALE
# 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.
# 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.
# 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 OV7725 camera with a 2.8 mm lens.
# 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.
# c_x is the image x center position in pixels.
# c_y is the image y center position in pixels.
f_x = (2.8 / 3.984) * DETECT_WIDTH # find_apriltags defaults to this if not set
f_y = (2.8 / 2.952) * DETECT_HEIGHT # find_apriltags defaults to this if not set
c_x = DETECT_WIDTH * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
c_y = DETECT_HEIGHT * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
def degrees(radians):
return (180 * radians) / math.pi
def camera_init():
# use hdmi for display
display.init(LT9611_1920X1080_30FPS)
# config vb for osd layer
config = k_vb_config()
config.max_pool_cnt = 1
config.comm_pool[0].blk_size = 4*DISPLAY_WIDTH*DISPLAY_HEIGHT
config.comm_pool[0].blk_cnt = 1
config.comm_pool[0].mode = VB_REMAP_MODE_NOCACHE
# meida buffer config
media.buffer_config(config)
# init default sensor
camera.sensor_init(CAM_DEV_ID_0, CAM_DEFAULT_SENSOR)
# set chn0 output size
camera.set_outsize(CAM_DEV_ID_0, CAM_CHN_ID_0, DISPLAY_WIDTH, DISPLAY_HEIGHT)
# set chn0 output format
camera.set_outfmt(CAM_DEV_ID_0, CAM_CHN_ID_0, PIXEL_FORMAT_YUV_SEMIPLANAR_420)
# create meida source device
globals()["meida_source"] = media_device(CAMERA_MOD_ID, CAM_DEV_ID_0, CAM_CHN_ID_0)
# create meida sink device
globals()["meida_sink"] = media_device(DISPLAY_MOD_ID, DISPLAY_DEV_ID, DISPLAY_CHN_VIDEO1)
# create meida link
media.create_link(meida_source, meida_sink)
# set display plane with video channel
display.set_plane(0, 0, DISPLAY_WIDTH, DISPLAY_HEIGHT, PIXEL_FORMAT_YVU_PLANAR_420, DISPLAY_MIRROR_NONE, DISPLAY_CHN_VIDEO1)
# set chn1 output nv12
camera.set_outsize(CAM_DEV_ID_0, CAM_CHN_ID_1, DETECT_WIDTH, DETECT_HEIGHT)
camera.set_outfmt(CAM_DEV_ID_0, CAM_CHN_ID_1, PIXEL_FORMAT_YUV_SEMIPLANAR_420)
# media buffer init
media.buffer_init()
# request media buffer for osd image
globals()["buffer"] = media.request_buffer(4 * DISPLAY_WIDTH * DISPLAY_HEIGHT)
# start stream for camera device0
camera.start_stream(CAM_DEV_ID_0)
def camera_deinit():
# stop stream for camera device0
camera.stop_stream(CAM_DEV_ID_0)
# deinit display
display.deinit()
os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
time.sleep_ms(100)
# release media buffer
media.release_buffer(globals()["buffer"])
# destroy media link
media.destroy_link(globals()["meida_source"], globals()["meida_sink"])
# deinit media buffer
media.buffer_deinit()
def capture_picture():
# create image for drawing
draw_img = image.Image(DISPLAY_WIDTH, DISPLAY_HEIGHT, image.ARGB8888)
# create image for osd
buffer = globals()["buffer"]
osd_img = image.Image(DISPLAY_WIDTH, DISPLAY_HEIGHT, image.ARGB8888, alloc=image.ALLOC_VB, phyaddr=buffer.phys_addr, virtaddr=buffer.virt_addr, poolid=buffer.pool_id)
osd_img.clear()
display.show_image(osd_img, 0, 0, DISPLAY_CHN_OSD0)
fps = time.clock()
while True:
fps.tick()
try:
os.exitpoint()
yuv420_img = camera.capture_image(CAM_DEV_ID_0, CAM_CHN_ID_1)
img = image.Image(yuv420_img.width(), yuv420_img.height(), image.GRAYSCALE, alloc=image.ALLOC_HEAP, data=yuv420_img)
camera.release_image(CAM_DEV_ID_0, CAM_CHN_ID_1, yuv420_img)
draw_img.clear()
for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H11
draw_img.draw_rectangle([v*SCALE for v in tag.rect()], color=(255, 0, 0))
draw_img.draw_cross(tag.cx()*SCALE, tag.cy()*SCALE, 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.
print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)
draw_img.copy_to(osd_img)
print(fps.fps())
del img
gc.collect()
except KeyboardInterrupt as e:
print("user stop: ", e)
break
except BaseException as e:
sys.print_exception(e)
break
def main():
os.exitpoint(os.EXITPOINT_ENABLE)
camera_is_init = False
try:
print("camera init")
camera_init()
camera_is_init = True
print("camera capture")
capture_picture()
except Exception as e:
sys.print_exception(e)
finally:
if camera_is_init:
print("camera deinit")
camera_deinit()
if __name__ == "__main__":
main()
具体接口使用请参考相关文档说明：