[PYTHON] Calcul de l'odométrie à l'aide de CNN et estimation de la profondeur Partie 2 (CNN SLAM # 2)

Ceci est une suite de Calcul de l'odométrie utilisant CNN et estimation de la profondeur, Partie 1. La dernière fois, j'ai décrit le contenu de l'article au meilleur de ma connaissance. J'ai écrit la partie calcul de l'odométrie en Python, j'en parlerai donc dans la seconde partie de ce temps.

Le calcul de l'odométrie revisité

r({\bf u}, {\bf T}) = I_{k_i}({\bf u}) - I_t(\pi({\bf K}{\bf T}_{t}^{k_i} V_{ki}({\bf u})))

Nous visons à minimiser $ r $ dans l'équation ci-dessus. Trouvez $ T $ (composant de rotation + composant de traduction) à ce moment. Celui-ci est divisé en la matrice de rotation $ R $ et le vecteur de translation $ t $ et s'exprime comme suit.

r({\bf u}, {\bf T}) = I_{k_i}({\bf u}) - I_t(\pi({\bf K}({\bf R}_{t}^{k_i} V_{ki}({\bf u}) + {\bf t}_{t}^{k_i})))

Puisqu'il y a trois paramètres de la matrice de rotation, créez une fonction qui renvoie la matrice de rotation avec np.array ([alpha, beta, gamma]) comme argument. Utilisez le module numpy.

util.py

def make_R(rads):
    if len(rads) != 3:
        print("len(rads) != 3")
    alpha, beta, gamma = rads
    R_alpha = np.array([[np.cos(alpha), 0.0, np.sin(alpha)],
                        [0.0, 1.0, 0.0],
                        [-np.sin(alpha), 0.0, np.cos(alpha)]])
    R_beta = np.array([[1.0, 0.0, 0.0],
                       [0.0, np.cos(beta), -np.sin(beta)],
                       [0.0, np.sin(beta), np.cos(beta)]])
    R_gamma = np.array([[np.cos(gamma), -np.sin(gamma), 0.0],
                        [np.sin(gamma), np.cos(gamma), 0.0],
                        [0.0, 0.0, 1.0]])
    R = np.dot(R_gamma, np.dot(R_beta, R_alpha))
    return R

$ K $ est un paramètre interne de la caméra. Il s'exprime comme suit en utilisant la distance focale de l'objectif ($ f_x $, $ f_y ) et les coordonnées du centre de l'image ( c_x $, $ c_y $).

K = \left(
  \begin{array}{c}
    f_x & 0 & c_x \\
    0 & f_y & c_y \\
    0 & 0 & 1
  \end{array}
\right)

util.py

fx = 525.0  # focal length x
fy = 525.0  # focal length y
cx = 319.5  # optical center x
cy = 239.5  # optical center y

K = np.array([[fx, 0, cx],
              [0, fy, cy],
              [0, 0, 1]], dtype=np.float64)

Vous pouvez le calibrer, et cette fois j'ai téléchargé l'ensemble de données et l'ai expérimenté, alors j'ai utilisé ces informations.

$ V $ est exprimé par la formule suivante.

V_{k_i}({\bf u}) = K^{-1}\dot{u}D_{k_i}({\bf u})

Je ne sais pas quelle est la valeur spécifique de $ \ dot {u} $, mais ce que je veux faire est d'utiliser l'image de profondeur $ D $ pour voir ce qui se reflète dans chaque pixel de l'image dans le système de coordonnées 3D. Autrement dit, je veux calculer où il se trouve dans le monde réel.

$ I $ représente l'image rgb. L'image sera un tableau à deux dimensions (car nous avons affaire à une image en échelle de gris cette fois), et elle doit être de type entier en se référant à la valeur, mais lors de la conversion de coordonnées, les points de ce système de coordonnées seront fractionnaires. C'est tout à fait possible, alors faites un petit effort pour éviter de lancer une erreur. Par exemple, si vous voulez la valeur de l'indice 5.3, mélangez et renvoyez les valeurs de l'indice 5 et de l'index 6 de manière appropriée. Cette fois, nous utiliserons une interpolation linéaire.

util.py

def get_pixel(img, x, y):
    rx = x - np.floor(x)
    ry = y - np.floor(y)
    left_up_x = int(np.floor(x))
    left_up_y = int(np.floor(y))
    val = (1.0 - rx) * (1.0 - ry) * img[left_up_y, left_up_x] + \
          rx * (1.0 - ry) * img[left_up_y, left_up_x + 1] + \
          (1.0 - rx) * ry * img[left_up_y + 1, left_up_x] + \
          rx * ry * img[left_up_y + 1, left_up_x + 1]
    return val

La fonction pour trouver $ r $ est la suivante.

util.py

def translate(rads, t, im_xs, im_ys, dep_vals):
    if not (len(im_xs) == len(im_ys) == len(dep_vals)):
        print("len(im_xs) == len(im_ys) == len(dep_vals) is False!!")
        raise ValueError

    n_data = len(im_xs)
    U = np.vstack((im_xs, im_ys, [1.0] * n_data))

    R = make_R(rads)
    invK = np.linalg.inv(K)

    invK_u = np.dot(invK, U)
    R_invK_u = np.dot(R, invK_u)
    s_R_invK_u_t = dep_vals * R_invK_u + t
    K_s_R_invK_u_t = np.dot(K, s_R_invK_u_t)
    translated_u = K_s_R_invK_u_t / K_s_R_invK_u_t[2, :]

    return translated_u


def r(rads, t, im_xs, im_ys, dep_vals, I1, I2):
    if not (len(im_xs) == len(im_ys) == len(dep_vals)):
        print("len(im_xs) == len(im_ys) == len(dep_vals) is False!!")
        raise ValueError

    n_data = len(im_xs)
    transed_u = translate(rads=rads, t=t, im_xs=im_xs, im_ys=im_ys, dep_vals=dep_vals)

    diff_arr = np.empty((n_data, 1))
    for i in range(n_data):
        im_x1, im_y1 = im_xs[i], im_ys[i]
        im_x2, im_y2 = transed_u[0, i], transed_u[1, i]
        val1 = get_pixel(img=I1, x=im_x1, y=im_y1)  # I[im_y1, im_x1]
        val2 = get_pixel(img=I2, x=im_x2, y=im_y2)  # I[im_y2, im_x2]
        diff_arr[i, 0] = val1 - val2

    return diff_arr

C'est une fonction pour calculer l'erreur lors de la conversion de l'image I1 en image I2. rads est le paramètre de composante de rotation et t est le paramètre de composante de translation. Cette fois, nous approcherons la valeur optimale en donnant d'abord une valeur initiale appropriée au paramètre à optimiser puis en corrigeant progressivement la valeur. C'est le même style que la méthode de descente la plus raide. Cependant, cette fois, nous utiliserons la méthode Gauss-Newton. Dans ce cas, vous aurez besoin de Jacobian $ J $.

J = \left(
  \begin{array}{c}
    \frac{\partial {\bf r}_1}{\partial {\bf x}_1} & \ldots & \frac{\partial {\bf r}_1}{\partial {\bf x}_n} \\
    \vdots & \ddots & \vdots \\
    \frac{\partial {\bf r}_m}{\partial {\bf x}_1} & \ldots & \frac{\partial {\bf r}_m}{\partial {\bf x}_n}
  \end{array}
\right)

Cette fois, il va de $ x_1 $ à $ x_6 $, ce qui correspond respectivement à $ \ alpha, \ beta, \ gamma, x, y, z $. Le nombre de $ r $ correspond au nombre de pixels de l'image. Lors de l'utilisation d'une image 640x480, il y a aussi 640x480 = 307200, mais l'utilisation d'une matrice aussi grande ralentira le calcul. Même dans l'image, il n'y a pas de dégradé dans la pièce sans mur ou motif blanc, donc le composant jacobien dans cette partie a tendance à être 0 et il n'est pas utile pour l'optimisation, il est donc plus efficace de se réduire à la partie avec un dégradé et de trouver $ J $ Est bon. Alors, obtenons une liste de coordonnées d'image avec un dégradé et n'utilisons que ce groupe de points pour trouver les composants de rotation et de translation. C'est pourquoi la fonction r dans util.py a des arguments im_xs, im_ys, dep_vals. Même dans le papier, il semble que le calcul se concentre sur la pièce à fort gradient. Si vous calculez en utilisant tous les pixels, les fans de PC commenceront à travailler dur et deviendront anxieux.

Créez une source jacobienne à l'aide de la fonction r.

util.py

rad_eps = np.pi / 900.0
t_eps = 1e-04

def grad_r(rads, t, im_xs, im_ys, dep_vals, I_transed, index=-1):
    if not (len(im_xs) == len(im_ys) == len(dep_vals)):
        print("len(im_xs) == len(im_ys) == len(dep_vals) is False!!")
        raise ValueError

    I = I_transed

    if index < 0 or 5 < index:
        print("index is out of range or not defined")
        raise ValueError

    n_data = len(im_xs)

    if index < 3:
        rads_p, rads_m = rads.copy(), rads.copy()
        rads_p[index] += rad_eps
        rads_m[index] -= rad_eps
        u_p = translate(rads=rads_p, t=t, im_xs=im_xs, im_ys=im_ys, dep_vals=dep_vals)
        u_m = translate(rads=rads_m, t=t, im_xs=im_xs, im_ys=im_ys, dep_vals=dep_vals)
    else:
        t_p, t_m = t.copy(), t.copy()
        t_p[index - 3, 0] += t_eps
        t_m[index - 3, 0] -= t_eps
        u_p = translate(rads=rads, t=t_p, im_xs=im_xs, im_ys=im_ys, dep_vals=dep_vals)
        u_m = translate(rads=rads, t=t_m, im_xs=im_xs, im_ys=im_ys, dep_vals=dep_vals)

    grad = np.empty(n_data, )
    for i in range(n_data):
        im_x_p, im_y_p = u_p[0, i], u_p[1, i]
        im_x_m, im_y_m = u_m[0, i], u_m[1, i]
        val_p = get_pixel(img=I, x=im_x_p, y=im_y_p)  # I[im_y_p, im_x_p]
        val_m = get_pixel(img=I, x=im_x_m, y=im_y_m)  # I[im_y_m, im_x_m]
        grad[i] = -(val_p - val_m)

    if index < 3:
        grad /= (2.0 * rad_eps)
    else:
        grad /= (2.0 * t_eps)

    return grad

Le code est assez approprié, mais il n'est pas conçu pour être publié, mais il ressemble à ceci. La variable I_transed correspond à I2 dans la fonction r. Cela signifie l'image de la destination de conversion. L'index de variable définit quelle variable de $ x_1 $ à $ x_6 $ à différencier. Le jacobien peut être obtenu en appelant la fonction un total de 6 fois pour chaque paramètre.

main.py

from util import *

J_T = None  #Matrice de translocation jacobienne
for ind in range(6):
    grad = grad_r(rads=rads, t=t, im_xs=xs, im_ys=ys, dep_vals=dep_vals, I_transed=img2, index=ind)
    if J_T is None:
        J_T = np.copy(grad)
    else:
        J_T = np.vstack((J_T, grad))

En fait, vous obtenez la matrice de translocation jacobienne. Ce n'est pas un problème. Pour mettre à jour les paramètres, laissez simplement numpy effectuer l'opération de matrice. Je ne peux pas me lever pour être engourdi.

main.py

JJ = np.dot(J_T, J_T.T)
invJJ = np.linalg.inv(JJ)
invJJ_J = np.dot(invJJ, J_T)
invJJ_J_r = np.dot(invJJ_J, diff_arr)

rads -= invJJ_J_r[:3, 0]
t -= invJJ_J_r[3:, :]

Je ne l'ai pas mentionné dans l'explication ci-dessus, mais pour trouver la partie avec un grand dégradé dans l'image, appliquez un filtre laplacien. J'ai utilisé le module cv2 ici. Il est plus rapide de regarder le code, donc je vais tout poster.

main.py

from PIL import Image
import numpy as np
import cv2
from util import *

WIDTH = 640
HEIGHT = 480

data_dir = "XXX/living_room_traj2_frei_png/"

# 1 ->Demander la conversion en 2
image_file_1 = data_dir + "rgb/10.png "
depth_file_1 = data_dir + "depth/10.png "
image_file_2 = data_dir + "rgb/30.png "

# get image
img1 = Image.open(image_file_1)
img1 = img1.resize([WIDTH, HEIGHT], Image.ANTIALIAS)
raw_img1 = cv2.cvtColor(np.array(img1), cv2.COLOR_BGR2GRAY)
img1 = raw_img1.astype('float64') / 255.0

dep1 = Image.open(depth_file_1)
dep1 = dep1.resize([WIDTH, HEIGHT], Image.ANTIALIAS)
dep1 = np.array(dep1, 'float64') / 5000.0

img2 = Image.open(image_file_2)
img2 = img2.resize([WIDTH, HEIGHT], Image.ANTIALIAS)
raw_img2 = cv2.cvtColor(np.array(img2), cv2.COLOR_BGR2GRAY)
img2 = raw_img2.astype('float64') / 255.0

#Obtenez une liste de coordonnées et de valeurs de profondeur sur l'image à convertir
lap1 = np.abs(cv2.Laplacian(raw_img1, cv2.CV_32F, ksize=5))
th = sorted(lap1.flatten())[::-1][2000]

xs, ys, dep_vals = list(), list(), list()
bias = 30
for y in range(bias, HEIGHT - bias):
    for x in range(bias, WIDTH - bias):
        if lap1[y, x] > th:
            xs.append(x)
            ys.append(y)
            dep_vals.append(dep1[y, x])

xs = np.array(xs, dtype=np.float64)
ys = np.array(ys, dtype=np.float64)
dep_vals = np.array(dep_vals, dtype=np.float64)

#Trouvez la matrice de transformation basée sur la méthode Gauss-Newton
#Réglage de la valeur initiale
rads = np.array([0.0, 0.0, 0.0]).reshape(3, )
t = np.array([0.0, 0.0, 0.0]).reshape(3, 1)

#Pour le moment, boucle 10 fois
for _ in range(10):
    diff_arr = r(rads=rads, t=t, im_xs=xs, im_ys=ys, dep_vals=dep_vals, I1=img1, I2=img2)

    J_T = None  #Matrice de translocation jacobienne
    for ind in range(6):
        grad = grad_r(rads=rads, t=t, im_xs=xs, im_ys=ys, dep_vals=dep_vals, I_transed=img2, index=ind)
        if J_T is None:
            J_T = np.copy(grad)
        else:
            J_T = np.vstack((J_T, grad))

    JJ = np.dot(J_T, J_T.T)
    invJJ = np.linalg.inv(JJ)
    invJJ_J = np.dot(invJJ, J_T)
    invJJ_J_r = np.dot(invJJ_J, diff_arr)

    rads -= invJJ_J_r[:3, 0]
    t -= invJJ_J_r[3:, :]

    print(rads.reshape(-1))
    print(t.reshape(-1))
    print("-----")

#Comment chaque pixel de img1 se déplace par la matrice de rotation et le vecteur de translation,
#Estimer img1 en utilisant img2 obtenu à destination
out = transform_image(rads=rads, t=t, dep_img=dep1, I=img2)
out = (255.0 * out).astype(np.uint8)

cv2.imshow('output', cv2.hconcat((raw_img1, raw_img2, out)))
cv2.waitKey(0)

util.py

def transform_image(rads, t, dep_img, I):
    im_xs, im_ys = np.meshgrid(np.arange(WIDTH), np.arange(HEIGHT))
    im_xs = im_xs.reshape(-1)
    im_ys = im_ys.reshape(-1)
    dep_vals = dep_img.reshape(-1)
    transed_u = translate(rads=rads, t=t, im_xs=im_xs, im_ys=im_ys, dep_vals=dep_vals)

    out = np.zeros((HEIGHT, WIDTH))
    for i in range(len(im_xs)):
        im_x1, im_y1 = im_xs[i], im_ys[i]
        im_x2, im_y2 = transed_u[0, i], transed_u[1, i]
        try:
            out[im_y1, im_x1] = get_pixel(img=I, x=im_x2, y=im_y2)
        except:
            out[im_y1, im_x1] = 0.0

    return out

tester

・ Image de la source de conversion (img1)

・ Image de destination de conversion (img2)

-Restaurer l'image source de la conversion à partir des paramètres estimés (échelle de gris)

Il semble que l'inclinaison du canapé soit corrigée, donc ce n'est pas parfait, mais il semble que la composante de rotation et la composante de translation puissent être calculées telles quelles. Cette fois, j'ai utilisé jeu de données ICL-NUIM RGB-D. Je ne l'ai pas essayé sur d'autres ensembles de données, alors assurez-vous que cette implémentation est correcte.

À la fin

Cette fois, je n'ai pas estimé la profondeur, donc cela ressemble à une arnaque au titre. Dans l'article, la profondeur est estimée. Je veux aussi l’estimer bientôt.