Follow the AR marker with a 2-axis servo

This time, when I made a demo of the system to follow the teaching materials, I was forced to follow the AR marker, so a memo for myself at that time.

Implementation flow

With OpneCV's arconv, you can identify AR markers and take their coordinates and angles from images. [^ 1]

For the time being, generate an AR marker.

make.py


#/usr/bin/env python3
# -*- coding: utf-8 -*-

import cv2
import sys
#aruco library
aruco = cv2.aruco 
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

def make():
    for i in range(30):
        generator = aruco.drowMarker(dictionary, i, 100)
        cv2.imwrite('ar' + str(i) + 'png', generator)

if __name__ == '__main__';
    make():

If you run this file, you should have 30 AR marker images. スクリーンショット 2019-07-27 14.01.53.PNG

This time we will use ar1.png. スクリーンショット 2019-07-27 14.01.53.jpg

Next, let's look at the actual program. First, the image is acquired, and the frame size is acquired and resized.

main.py


while True:
            #Get images from video capture
            ret, frame = cap.read()
            #Get size
            Height, Width = frame.shape[:2]
            # resize
            img = cv2.resize(frame,(int(Width),int(Height)))

Next, the marker is detected and identified, and the center coordinates are obtained.

main.py


            #Detect markers
            corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary) 

            if len(corners) > 0:
                #print(ids[0])
                #When marker 01 is detected
                if ids[0] == 1:
                    square_points = np.reshape(np.array(corners), (4, -1))
                    G = np.mean(square_points, axis = 0)
                    x = G[0]
                    y = G[1]
                    #Specify the coordinate point you want to match
                    markerrect = [[x,y-40]]
                    print(markerrect)
                    for rect in markerrect:
                        img_x = rect[0]
                        img_y = rect[1]

At that time, the moving coordinates are calculated. It is necessary to change the value depending on the angle at which the servo is installed. Adjust as appropriate. In addition, since it was necessary to limit the upward movement, in order to prevent the operation range from being exceeded, the value was continuously substituted when the y-coordinate exceeded the specified value.

main.py


                        #Moving coordinate set
                        move_degree_x = now_degree_x - (img_x-160)*0.06
                        move_degree_y = now_degree_y - (img_y-50)*0.06
                        #Prevention of exceeding the movable range
                        if move_degree_y > 650:
                            move_degree_y = 650
                        print('deg: ', move_degree_x , move_degree_y)
                        pwm.set_pwm(15, 0, int(move_degree_x))
                        pwm.set_pwm(14, 0, int(move_degree_y))
                        #Post-move coordinate set
                        now_degree_x = move_degree_x
                        now_degree_y = move_degree_y

Adafruit_PCA9685 was used to control the servo motor. [^ 2] With this, a plurality of servo motors can be controlled.

The final result is as follows.

main.py


#/usr/bin/env python3
# -*- coding: utf-8 -*-

import cv2
import numpy as np
import time
import Adafruit_PCA9685
#aruco library
aruco = cv2.aruco 
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

class MarkerCtl():

    def arReader(self,Sb):
        # initial   
        pwm = Adafruit_PCA9685.PCA9685()
        pwm.set_pwm_freq(60)
        pwm.set_pwm(14, 0, 600)
        time.sleep(1)
        pwm.set_pwm(15, 0, 450)
        time.sleep(1)

        #Start video capture
        cap = cv2.VideoCapture(0) 
        cap.set(3, 320)
        cap.set(4, 240)
        color = (255, 255, 255)

        #Current servo state set
        now_degree_x, now_degree_y, move_degree_x, move_degree_y = 450, 600, 0, 0
        while True:
            #Get images from video capture
            ret, frame = cap.read()
            #Get size
            Height, Width = frame.shape[:2]
            # resize
            img = cv2.resize(frame,(int(Width),int(Height)))
            #Detect marker
            corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary) 

            if len(corners) > 0:
                #print(ids[0])
                #When marker 01 is detected
                if ids[0] == 1:
                    square_points = np.reshape(np.array(corners), (4, -1))
                    G = np.mean(square_points, axis = 0)
                    x = G[0]
                    y = G[1]
                    #Specify the coordinate point you want to match
                    markerrect = [[x,y-40]]
                    print(markerrect)
                    for rect in markerrect:
                        img_x = rect[0]
                        img_y = rect[1]
                        #print('img: ',img_x, img_y)
                        #Moving coordinate set
                        move_degree_x = now_degree_x - (img_x-160)*0.06
                        move_degree_y = now_degree_y - (img_y-50)*0.06
                        #Prevention of exceeding the movable range
                        if move_degree_y > 650:
                            move_degree_y = 650
                        print('deg: ', move_degree_x , move_degree_y)
                        pwm.set_pwm(15, 0, int(move_degree_x))
                        pwm.set_pwm(14, 0, int(move_degree_y))
                        #Post-move coordinate set
                        now_degree_x = move_degree_x
                        now_degree_y = move_degree_y

        #End processing
        cap.release()
        cv2.destroyAllWindows()
        

[^ 1]: "[Programming with Python] I tried to recognize the AR marker using the aruco library" http://okatenari.com/2017/11/28/python-ar/

Recommended Posts

Follow the AR marker with a 2-axis servo
Follow the file hierarchy with fts
Search the maze with the python A * algorithm
A memo explaining the axis specification of axis
A model that identifies the guitar with fast.ai
[Python] Get the files in a folder with Python
How to draw a 2-axis graph with pyplot
Find a position above the threshold with NumPy
Save the object to a file with pickle
Create a translation tool with the Translate Toolkit
matplotlib: Replace the axis itself with another one.
Draw a graph with PyQtGraph Part 5-Increase the Y-axis
A memo to visually understand the axis of pandas.Panel
How to create a submenu with the [Blender] plugin
Create a Todo app with the Django REST framework
Make a breakpoint on the c layer with python
A story that struggled with the common set HTTP_PROXY = ~
Try moving the servo motor with RasPi (180 degree version)
Fill the background with a single color with OpenCV2 + Python
Visualize the characteristic vocabulary of a document with D3.js
Create a tweet heatmap with the Google Maps API
A memo that I touched the Datastore with python
Transit to the update screen with the Django a tag
Try to specify the axis with PyTorch's Softmax function
Calculate the product of matrices with a character expression?