Estimer la posture du marqueur AR avec Python + OpenCV + drone

introduction

C'est le premier message de Qiita. J'aimerais écrire une méthode très approximative pour les étudiants en ingénierie de l'information pour générer et détecter des marqueurs AR à l'aide de Python, OpenCV et des drones, et pour estimer leur posture.

J'ai fait référence à ce blog. https://qiita.com/ReoNagai/items/a8fdee89b1686ec31d10

la mise en oeuvre

Génération de marqueurs ArUco

Cette fois, nous utiliserons ArUco, une bibliothèque AR qui est l'un des modules OpenCV. Le marqueur ArUco peut être généré avec le code suivant.

`generate.py`


#!/usr/bin/env python
# -*- coding: utf-8 -*
import cv2
aruco = cv2.aruco
dir(aruco)

dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

marker = aruco.drawMarker(dictionary, 1, 300)
cv2.imwrite('id-001.png', marker)

50 types de marqueurs peuvent être générés avec aruco.DICT_4X4_50. Pour le moment, quand j'essaye d'en générer un, ça ressemble à ça.

Il sera imprimé et collé sur le mur. Je l'ai utilisé pour suivre le drone, alors je l'ai collé à l'arrière du drone comme ça.

Détection de marqueurs ArUco

Lors de la détection depuis la caméra du drone

J'utilise AR.Drone 2.0 de parrot.

Parrot AR. Drone2.0， https://www.parrot.com/jp/doron/parrot-ardrone-20-power-edition (2020.4.22 access)

`detect_drone.py`


# -*- coding: utf-8 -*-
import numpy as np
import cv2 
import sys
from pyardrone import ARDrone
import logging

logging.basicConfig(level=logging.DEBUG)

client = ARDrone()
client.video_ready.wait()
cnt=0

aruco = cv2.aruco #bibliothèque aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

try:
    while True:
        cv2.imwrite('drone'+str(cnt)+'.png',client.frame)
        cnt+=1
        corners, ids, rejectedImgPoints = aruco.detectMarkers(client.frame, dictionary)    #Détecter le marqueur
        aruco.drawDetectedMarkers(client.frame, corners, ids, (0,255,0))   #Dessiner sur le marqueur détecté
        if cv2.waitKey(10) == ord(' '):
            break
finally:
    client.close()

Lorsque vous utilisez une caméra Web générale, etc.

`detect_camera.py`


# -*- coding: utf-8 -*-
import numpy as np
import cv2 
import sys
 
aruco = cv2.aruco #bibliothèque aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

def arReader(): 
    print(cv2.getBuildInformation())
    cap = cv2.VideoCapture(0)
    cnt=0

    while True:
 
        ret, frame = cap.read()
        if frame is None: break
 
        corners, ids, rejectedImgPoints = aruco.detectMarkers(frame, dictionary) #Détecter le marqueur
        print(corners,ids)
        aruco.drawDetectedMarkers(frame, corners, ids, (0,255,0)) #Dessiner sur le marqueur détecté

        cv2.imwrite('result'+str(cnt)+'.png',frame)
        cnt+=1

    cap.release()
    cv2.destroyAllWindows()

if __name__ == '__main__':
    arReader()

Résultat d'exécution

De cette manière, la bordure, l'ID, le coin et les axes XYZ du marqueur ont été dessinés et détectés. 座標①.png

Mesurez la distance et l'inclinaison entre le marqueur et la caméra

Lorsque vous souhaitez trouver la distance et l'inclinaison entre le marqueur et la caméra, vous utiliserez généralement un capteur infrarouge, mais cette fois, en plus des paramètres internes de la caméra et du coefficient de distorsion obtenu par l'étalonnage de la caméra, la rotation Il est calculé à l'aide d'une matrice, d'un vecteur de rotation et d'un vecteur de translation.

Calibrage de la caméra

Tout d'abord, effectuez un étalonnage de la caméra pour obtenir les paramètres internes et le coefficient de distorsion de la caméra.

`calib.py`


# -*- coding: utf-8 -*
import numpy as np
import cv2
import glob

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

objp = np.zeros((7*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:7].T.reshape(-1,2)

objpoints = []
imgpoints = []

gray_images=glob.glob('chess*.png')
cnt=0

for fname in gray_images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    ret, corners = cv2.findChessboardCorners(gray, (7,7),None)

    if ret == True:
        objpoints.append(objp)

        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        imgpoints.append(corners2)

        img = cv2.drawChessboardCorners(img, (7,7), corners2,ret)
        cv2.imwrite('calib'+str(cnt)+'.png',img)
        cnt+=1

    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)

print(mtx, dist)

cv2.destroyAllWindows()

Résultat d'exécution

`Résultat d'exécution`


Paramètres internes de la caméra
[[9.31357583e+03 0.00000000e+00 1.61931898e+03]
[0.00000000e+00 9.64867367e+03 1.92100899e+03]
[0.00000000e+00 0.00000000e+00 1.00000000e+00]]
Coefficient de déformation
[[ 0.22229833 -6.34741982  0.01145082  0.01934784 -8.43093571]]

Calculé avec Rodriguez

Calculez à l'aide d'une matrice de rotation, d'un vecteur de rotation et d'un vecteur de translation. Réécrivez camera_matrix avec les paramètres obtenus par la méthode ci-dessus.

`distance.py`


#!/usr/bin/env python
# -*- coding: utf-8 -*
import cv2
import numpy as np
import sys
from pyardrone import ARDrone
import logging

logging.basicConfig(level=logging.DEBUG)

aruco = cv2.aruco #bibliothèque aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

client = ARDrone()
client.video_ready.wait()

parameters =  aruco.DetectorParameters_create()

parameters.cornerRefinementMethod = aruco.CORNER_REFINE_CONTOUR


def main():
    cnt=0
    marker_length = 0.1 # [m]

    camera_matrix = np.array( [[ , , ],
                               [ , , ],
                               [ , , ]] )
    #Trouver les paramètres par calibrage de la caméra ↑

    distortion_coeff = np.array( [[ 0.22229833, -6.34741982,  0.01145082,  0.01934784, -8.43093571]] )

    try:
        while True:
            corners, ids, rejectedImgPoints = aruco.detectMarkers(client.frame, dictionary, parameters=parameters)

            aruco.drawDetectedMarkers(client.frame, corners, ids, (0,255,255))

            if len(corners) > 0:
                for i, corner in enumerate(corners):
                    # rvec -> rotation vector, tvec -> translation vector
                    rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corner, marker_length, camera_matrix, distortion_coeff)

                    tvec = np.squeeze(tvec)
                    rvec = np.squeeze(rvec)
                    rvec_matrix = cv2.Rodrigues(rvec)
                    rvec_matrix = rvec_matrix[0]
                    transpose_tvec = tvec[np.newaxis, :].T
                    proj_matrix = np.hstack((rvec_matrix, transpose_tvec))
                    euler_angle = cv2.decomposeProjectionMatrix(proj_matrix)[6] # [deg]
                    print("x : " + str(tvec[0]))
                    print("y : " + str(tvec[1]))
                    print("z : " + str(tvec[2]))
                    print("roll : " + str(euler_angle[0]))
                    print("pitch: " + str(euler_angle[1]))
                    print("yaw  : " + str(euler_angle[2]))

                    draw_pole_length = marker_length/2 #Longueur réelle[m]
                    aruco.drawAxis(client.frame, camera_matrix, distortion_coeff, rvec, tvec, draw_pole_length)

            cv2.imwrite('distance'+str(cnt)+'.png',client.frame)
            cnt+=1

            key = cv2.waitKey(50)
            if key == 27: # ESC
                break
    finally:
        client.close()

if __name__ == '__main__':
    try:
        main()
    except KeyboardInterrupt:
        pass

Résultat d'exécution

Afin de confirmer que la distance peut être mesurée correctement, réglez la distance entre le marqueur et la caméra (drone) à 25 cm et 50 cm, et exécutez le programme. 25cm正方形.png 50cm正方形.png

`Résultat d'exécution(25cm)`


x : -0.5410338091817037 
y : -0.713790809892646
z : 3.828983632577233
roll : [173.43049198]
pitch: [-20.42010647]
yaw  : [1.58947772]

`Résultat d'exécution(50cm)`


x : -1.0633298519994792 
y : -1.3476793013004273
z : 7.311742619516305
roll : [-150.91884043]
pitch: [2.45488175]
yaw  : [3.61554034]

La valeur du résultat de l'exécution est bien doublée. De cette manière, la taille du mouvement et la valeur du lacet du pas de roulis peuvent être obtenues pour chacun des axes XYZ.

prime

À partir de la valeur obtenue, le drone a été suivi par branchement conditionnel comme indiqué dans le tableau ci-dessous.

Marqueur-Distance entre les caméras	Fonctionnement du drone
~0.5m	Planant
0.5m ~ 2.0m	Avance
2.0m ~	atterrissage
autre que ça	Planant

Le diagramme de configuration ressemble à ceci.

構成図.png Puisque la communication multi-sauts (fossile) est utilisée, si même RasPi peut être placé sur le drone, ce sera un terminal de recherche dans un endroit où les gens ne peuvent pas entrer. Pour cette raison, je dois créer une grande quantité de points d'accès sans fil RasPi et définir dnsmasq et hostapd, mais c'est aussi trop gênant, donc je l'écrirai à un autre moment.

en conclusion

Cette fois, j'ai utilisé Python et OpenCV pour générer et détecter des marqueurs AR afin de trouver la distance et l'inclinaison. Je pense que ce marqueur ArUco est un excellent marqueur qui peut facilement et précisément reconnaître la distance entre le marqueur et la caméra, la direction du marqueur et le numéro (ID) attribué à chacun, mais ce n'est pas si célèbre ... ...? J'écrirai un jour une édition de drone.