[PYTHON] Berechnung der Kilometerzähler mit CNN und Tiefenschätzung Teil 2 (CNN SLAM # 2)

Dies ist eine Fortsetzung von Berechnung der Kilometerzähler unter Verwendung von CNN und Tiefenschätzung, Teil 1. Letztes Mal habe ich den Inhalt des Papiers nach bestem Wissen beschrieben. Ich habe den Berechnungsteil der Kilometerzähler in Python geschrieben, daher werde ich im zweiten Teil dieser Zeit darüber sprechen.

Berechnung der Kilometerzähler überarbeitet

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

Wir wollen $ r $ in der obigen Gleichung minimieren. Suchen Sie zu diesem Zeitpunkt $ T $ (Rotationskomponente + Übersetzungskomponente). Dies wird in die Rotationsmatrix $ R $ und den Translationsvektor $ t $ unterteilt und wie folgt ausgedrückt.

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})))

Da die Rotationsmatrix drei Parameter enthält, erstellen Sie eine Funktion, die die Rotationsmatrix mit dem Argument np.array ([alpha, beta, gamma]) zurückgibt. Verwenden Sie das Numpy-Modul.

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 $ ist ein interner Parameter der Kamera. Es wird wie folgt ausgedrückt, indem die Brennweite des Objektivs ($ f_x $, $ f_y ) und die Mittelkoordinaten des Bildes ( c_x $, $ c_y $) verwendet werden.

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)

Sie können es kalibrieren, und dieses Mal habe ich den Datensatz heruntergeladen und damit experimentiert, also habe ich diese Informationen verwendet.

$ V $ wird durch die folgende Formel ausgedrückt.

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

Ich weiß nicht, was der spezifische Wert von $ \ dot {u} $ ist, aber ich möchte das Tiefenbild $ D $ verwenden, um zu sehen, was in jedem Pixel des Bildes im 3D-Koordinatensystem reflektiert wird. Das heißt, ich möchte berechnen, wo es in der realen Welt ist.

$ I $ repräsentiert das RGB-Bild. Das Bild ist ein zweidimensionales Array (da es sich diesmal um ein Graustufenbild handelt), und es muss ein ganzzahliger Typ sein, wenn auf den Wert Bezug genommen wird. Bei der Koordinatenkonvertierung sind die Punkte in diesem Koordinatensystem jedoch gebrochen. Es ist durchaus möglich, also bemühen Sie sich ein wenig, einen Fehler zu vermeiden. Wenn Sie beispielsweise den Wert von Index 5.3 möchten, mischen Sie einfach die Werte von Index 5 und Index 6 und geben Sie sie entsprechend zurück. Dieses Mal verwenden wir eine lineare Interpolation.

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

Die Funktion zum Finden von $ r $ lautet wie folgt.

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

Diese Funktion berechnet den Fehler beim Konvertieren von Bild I1 zu Bild I2. rads ist der Parameter der Rotationskomponente und t ist der Parameter der Translationskomponente. Dieses Mal nähern wir uns dem optimalen Wert, indem wir dem zu optimierenden Parameter zuerst einen geeigneten Anfangswert geben und dann den Wert schrittweise korrigieren. Es ist der gleiche Stil wie bei der Methode mit dem steilsten Abstieg. Dieses Mal werden wir jedoch die Gauß-Newton-Methode verwenden. In diesem Fall benötigen Sie 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)

Diesmal reicht es von $ x_1 $ bis $ x_6 $, was $ \ alpha, \ beta, \ gamma, x, y, z $ entspricht. Die Anzahl von $ r $ entspricht der Anzahl von Pixeln im Bild. Wenn Sie ein 640x480-Bild verwenden, gibt es auch 640x480 = 307200, aber die Verwendung einer so großen Matrix verlangsamt die Berechnung. Selbst im Bild gibt es keinen Farbverlauf im Teil ohne weiße Wand oder Muster, daher ist die Jacobi-Komponente in diesem Teil in der Regel 0 und für die Optimierung nicht nützlich. Daher ist es effizienter, auf den Teil mit einem Farbverlauf einzugrenzen und $ J $ zu finden Ist gut. Lassen Sie uns also eine Liste der Bildkoordinaten mit einem Farbverlauf erstellen und nur diese Punktgruppe verwenden, um die Rotations- und Translationskomponenten zu finden. Aus diesem Grund verfügt die Funktion r in util.py über die Argumente im_xs, im_ys und dep_vals. Selbst in der Arbeit scheint sich die Berechnung auf das Teil mit einem großen Gradienten zu konzentrieren. Wenn Sie mit allen Pixeln rechnen, werden PC-Fans hart arbeiten und ängstlich werden.

Erstellen Sie eine Jacobi-Quelle mit der Funktion 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

Der Code ist ziemlich angemessen, aber nicht für die Veröffentlichung vorgesehen, sieht aber so aus. Die Variable I_transed entspricht I2 in der Funktion r. Es bedeutet das Bild des Konvertierungsziels. Der Variablenindex legt fest, welche Variable von $ x_1 $ bis $ x_6 $ unterschieden werden soll. Der Jacobi kann erhalten werden, indem die Funktion für jeden Parameter insgesamt 6 Mal aufgerufen wird.

main.py

from util import *

J_T = None  #Jacobianische Translokationsmatrix
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))

Tatsächlich erhalten Sie die Jacobi-Translokationsmatrix. Es ist kein Problem. Um die Parameter zu aktualisieren, lassen Sie einfach numpy die Matrixoperation ausführen. Ich kann nicht aufstehen.

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:, :]

Ich habe es in der obigen Erklärung nicht erwähnt, aber um das Teil mit einem großen Farbverlauf im Bild zu finden, wenden Sie einen Laplace-Filter an. Ich habe hier das cv2-Modul verwendet. Es ist schneller, sich den Code anzusehen, also werde ich alles posten.

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 ->Bitten Sie um Umstellung auf 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

#Rufen Sie eine Liste der Koordinaten und Tiefenwerte für das zu konvertierende Bild ab
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)

#Finden Sie die Transformationsmatrix basierend auf der Gauß-Newton-Methode
#Anfangswerteinstellung
rads = np.array([0.0, 0.0, 0.0]).reshape(3, )
t = np.array([0.0, 0.0, 0.0]).reshape(3, 1)

#Schleife vorerst 10 mal
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  #Jacobianische Translokationsmatrix
    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("-----")

#Wie sich jedes Pixel von img1 durch die Rotationsmatrix und den Translationsvektor bewegt,
#Schätzen Sie img1 mit img2, das Sie am Ziel erhalten haben
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

Prüfung

・ Konvertierungsquellbild (img1)

・ Konvertierungszielbild (img2)

• Stellen Sie das Bild der Konvertierungsquelle anhand der geschätzten Parameter wieder her (Graustufen).

Es scheint, dass die Neigung des Sofas korrigiert ist, so dass es nicht perfekt ist, aber es scheint, dass die Rotationskomponente und die Translationskomponente so berechnet werden können, wie sie sind. Dieses Mal habe ich [ICL-NUIM RGB-D-Datensatz] verwendet (https://www.doc.ic.ac.uk/~ahanda/VaFRIC/iclnuim.html). Ich habe es nicht mit anderen Datensätzen versucht, stellen Sie also sicher, dass diese Implementierung in Ordnung ist.

Am Ende

Dieses Mal habe ich die Tiefe nicht geschätzt, daher sieht es aus wie ein Titelbetrug. In der Arbeit wird die Tiefe geschätzt. Ich möchte es auch bald schätzen.