introduction

«J'ai estimé que le marqueur ArUco était utile pour estimer l'auto-position de la caméra, mais je n'ai pas pu trouver un bon échantillon à rechercher, alors j'ai pris une note.

(Veuillez me dire s'il y a une meilleure façon de l'écrire ...) --Le code, les matériaux, les paramètres de la caméra, etc. sont placés ici ⇨ https://github.com/naruya/aruco

environnement

python3.7

Matériel

shisa.mp4

production

sample1 (1).gif sample2(1).gif

théorique

―― L'article de cette personne était facile à comprendre - https://mem-archive.com/2018/10/28/post-962/

Les paramètres externes de la caméra ($ R $ et $ T $) satisfont: --c: système de coordonnées de la caméra (** image **) --w: système de coordonnées mondial

\begin{bmatrix}
X_c \\
Y_c \\
Z_c
\end{bmatrix}
= R
\begin{bmatrix}
X_w \\
Y_w \\
Z_w
\end{bmatrix}
+ T

--Pour trouver les coordonnées de la caméra dans le système de coordonnées mondial, utilisez $ R ^ {-1} = R ^ \ top $

\begin{bmatrix}
X_w \\
Y_w \\
Z_w
\end{bmatrix}
=
R^\top
\begin{pmatrix}
\begin{bmatrix}
X_c \\
Y_c \\
Z_c
\end{bmatrix}
- T
\end{pmatrix}

--Par conséquent, la position de la caméra $ O_w $ (origine des coordonnées de la caméra) est

O_w = 
\begin{bmatrix}
O^x_w \\
O^y_w \\
O^z_w
\end{bmatrix}
=
R^\top
\begin{pmatrix}
- T
\end{pmatrix}

De plus, la face avant $ vz_w $ ($ vz_c = \ begin {bmatrix} 0 \ 0 \ 1 \ end {bmatrix} $) de la caméra correspond aux coordonnées de la caméra $ \ begin {bmatrix} X_c \ Y_c \ Z_c \ end {bmatrix} ^ \ top = \ begin {bmatrix} 0 \ 0 \ 1 \ end {bmatrix} ^ \ top $

vz_w = 
R^\top
\begin{pmatrix}
\begin{bmatrix}
0 \\
0 \\
1
\end{bmatrix}
- T
\end{pmatrix}
- O_w
=
R^\top
\begin{bmatrix}
0 \\
0 \\
1
\end{bmatrix}

--Autre orientation de la caméra $ vx_w $ ($ vx_c = \ begin {bmatrix} 1 \ 0 \ 0 \ end {bmatrix} $: caméra vers la droite), $ vy_w $ ($ vy_c = \ begin {bmatrix} 0 \ \ 1 \ 0 \ end {bmatrix} $: caméra vers le bas)

Point d'attention

De subtiles différences dans la façon de prendre le système de coordonnées (les deux sont des systèmes de coordonnées droitiers et c'est comme ça, mais pour le moment)
Système de coordonnées mondial (système de coordonnées Arcomarker):
Le marqueur lui-même est sur le plan xy, la droite est l'axe x positif, ** au-dessus ** est l'axe y positif et le ciel au-dessus du marqueur est l'axe z positif.
Système de coordonnées de la caméra (système de coordonnées pour l'image de la caméra OpenCV):
Le côté droit de l'image est l'axe x positif, le bas ** de l'image est l'axe y positif et le dos de l'image est l'axe z positif.

code

1. Préparation

import cv2
import numpy as np
import matplotlib.pyplot as plt
from skvideo.io import vread
import moviepy.editor as mpy
from tqdm import tqdm
from mpl_toolkits.mplot3d import axes3d, Axes3D

def npy_to_gif(npy, filename):
    clip = mpy.ImageSequenceClip(list(npy), fps=10)
    clip.write_gif(filename)

vid = vread("src/shisa.mp4")
print(vid.shape)

(524, 1920, 1080, 3)

2. Marqueur Arco

aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
aruco = cv2.aruco.drawMarker(aruco_dict, 0, 256)
cv2.imwrite("aruco.png ", aruco)

--Je l'ai imprimé et collé sur l'escabeau

3. Paramètres de l'appareil photo

Prenez avec le mode d'enregistrement Google Pixel3, grossissement 1.

marker_length = 0.07 # [m] ###Mise en garde!
mtx = np.load("camera/mtx.npy")
dist = np.load("camera/dist.npy")

4. Principal

--rpy (roll pitch yaw) n'est pas utilisé pour le tracé, mais pour le moment

XYZ = []
RPY = []
V_x = []
V_y = []
V_z = []

for frame in vid[:500:25]:  #C'est lourd de tout traiter ...
    frame = frame[...,::-1]  # BGR2RGB
    frame = cv2.resize(frame, (360, 640))
    corners, ids, _ = cv2.aruco.detectMarkers(frame, aruco_dict)
    rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners, marker_length, mtx, dist)

    R = cv2.Rodrigues(rvec)[0]  #Vecteur de rotation->Matrice de rotation
    R_T = R.T
    T = tvec[0].T

    xyz = np.dot(R_T, - T).squeeze()
    XYZ.append(xyz)

    rpy = np.deg2rad(cv2.RQDecomp3x3(R_T)[0])
    RPY.append(rpy)
    
    V_x.append(np.dot(R_T, np.array([1,0,0])))
    V_y.append(np.dot(R_T, np.array([0,1,0])))
    V_z.append(np.dot(R_T, np.array([0,0,1])))

    # ----dessin
    cv2.aruco.drawDetectedMarkers(frame, corners, ids, (0,255,255))
    cv2.aruco.drawAxis(frame, mtx, dist, rvec, tvec, marker_length/2)
    cv2.imshow('frame', frame)
    cv2.waitKey(1)
    # ----

cv2.destroyAllWindows()

Comme ça

Au fait, rpy est --Comment utiliser cv2.RQDecomp3x3 () --Comment utiliser cv2.decomposeProjectionMatrix () --ʻArctan () Yara ʻarcsin ()
Il semble y avoir un moyen de l'utiliser (décrit plus tard)

5. Dessin de la position de la caméra

def plot_all_frames(elev=90, azim=270):
    frames = []

    for t in tqdm(range(len(XYZ))):
        fig = plt.figure(figsize=(4,3))
        ax = Axes3D(fig)
        ax.view_init(elev=elev, azim=azim)
        ax.set_xlim(-2, 2); ax.set_ylim(-2, 2); ax.set_zlim(-2, 2)
        ax.set_xlabel("x"); ax.set_ylabel("y"); ax.set_zlabel("z")

        x, y, z = XYZ[t]
        ux, vx, wx = V_x[t]
        uy, vy, wy = V_y[t]
        uz, vz, wz = V_z[t]

        # draw marker
        ax.scatter(0, 0, 0, color="k")
        ax.quiver(0, 0, 0, 1, 0, 0, length=1, color="r")
        ax.quiver(0, 0, 0, 0, 1, 0, length=1, color="g")
        ax.quiver(0, 0, 0, 0, 0, 1, length=1, color="b")
        ax.plot([-1,1,1,-1,-1], [-1,-1,1,1,-1], [0,0,0,0,0], color="k", linestyle=":")

        # draw camera
        ax.quiver(x, y, z, ux, vx, wx, length=0.5, color="r")
        ax.quiver(x, y, z, uy, vy, wy, length=0.5, color="g")
        ax.quiver(x, y, z, uz, vz, wz, length=0.5, color="b")

        # save for animation
        fig.canvas.draw()
        frames.append(np.array(fig.canvas.renderer.buffer_rgba()))
        plt.close()

    return frames

frames = plot_all_frames(elev=105, azim=270)
npy_to_gif(frames, "src/sample1.gif")

frames = plot_all_frames(elev=165, azim=270)
npy_to_gif(frames, "src/sample2.gif")