CNNとdepth推定を用いたオドメトリの算出 前編の続きになります。前回は論文の内容を私の知る範囲で述べてきました。オドメトリの算出部分をPythonで記述しましたので、今回の後編では、それについて述べます。
オドメトリの算出 再訪
r({\bf u}, {\bf T}) = I_{k_i}({\bf u}) - I_t(\pi({\bf K}{\bf T}_{t}^{k_i} V_{ki}({\bf u})))
上式における$r$の最小化を狙います。このときの$T$（回転成分＋並進成分）を求めます。これを回転行列$R$と並進ベクトル$t$に分けて、以下のように表します。
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})))
回転行列のパラメータは3つあるので、np.array([alpha, beta, gamma])を引数として、回転行列を返すような関数をあらかじめ作っておきます。numpyモジュールを使います。
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$はカメラの内部パラメータです。レンズの焦点距離($f_x$, $f_y$)と画像の中心座標($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)
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)
キャリブレーションしても良いですし、今回はデータセットをダウンロードして実験をしましたので、その情報を利用しました。
$V$は以下の式で表されます。
V_{k_i}({\bf u}) = K^{-1}\dot{u}D_{k_i}({\bf u})
$\dot{u}$が具体的にどんな値なのかはわかっていないのですが、やりたいこととしては、depth画像$D$を用いて画像の各ピクセルに写っているものが3次元座標系、すなわちリアルの世界のどこにあるのかを計算したいという感じです。
$I$はrgb画像を表しています。画像は（今回はグレースケールの画像を扱うので）2次元配列となり、値を参照するときは整数型である必要がありますが、座標変換をしたときにその座標系における点が小数になることは十分にあり得るので、エラーを吐かないようにちょっとだけ工夫します。例えばインデックス5.3の値が欲しいという場合はインデックス5とインデックス6の値を適切にmixして返すというふうにするだけです。今回は線形補間を採用します。
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
$r$を求める関数は以下になります。
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
画像I1から画像I2への変換をするときの誤差を計算する関数です。radsは回転成分のパラメータ、tは並進成分のパラメータです。今回は最適化したいパラメータに最初、適当に初期値を与えて、少しずつ値を修正していくというやり方で最適な値に近づけていきます。最急降下法と同じスタイルです。ただし、今回はガウス・ニュートン法でやります。この場合、ヤコビアン$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)
今回は$x_1$から$x_6$まであって、それぞれ$\alpha, \beta, \gamma, x, y, z$に対応します。$r$の数は画像のピクセル数に対応します。640x480の画像を使う場合、640x480=307200もあるわけですが、こんなに大きな行列を使うと計算が遅くなってしまいます。画像内でも、真っ白な壁とか模様がない部分は勾配がないので、その部分のヤコビアンの成分は0になりやすく最適化の役に立たないので、勾配がある部分に絞って$J$を求めるほうが効率が良かったりします。
というわけで、勾配がある画像座標のリストを取得して、その点群だけを用いて回転成分と並進成分を求めるようにします。これがutil.pyのr関数にim_xs, im_ys, dep_valsの引数がある理由です。論文でも勾配が大きい部分に絞って計算しているようです。全部のピクセル使って計算するとPCのファンが頑張り始めて不安になります。
r関数を用いてヤコビアンのもとを作ります。
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
コードは割と適当と言いますか、公開することを意識した設計にはなってないのですが、こんな感じになります。変数I_transedはr関数でいうI2に相当します。変換先の画像、という意味です。変数indexは$x_1$から$x_6$までのどの変数に対して微分を行うかを設定します。各パラメータずつ計6回の関数呼び出しをすればヤコビアンが求まるという訳です。
from util import *
J_T = None # ヤコビアンの転置行列
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))
実際にはヤコビアンの転置行列が得られます。問題ないです。
パラメータの更新は行列演算をnumpyで行わせるだけです。numpyには頭が上がりません。
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:, :]
上記の説明で言ってなかったのですが、画像内で勾配の大きい部分を探し出すのにはラプラシアンフィルタをかけることで求めます。ここら辺はcv2モジュールを使いました。コードを見たほうが早いので全部載せます。
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 -> 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
# 変換する画像上の座標のリストとdepth値のリストを取得
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)
# ガウス・ニュートン法に基づいて変換行列を求める
# 初期値の設定
rads = np.array([0.0, 0.0, 0.0]).reshape(3, )
t = np.array([0.0, 0.0, 0.0]).reshape(3, 1)
# とりあえず、10回ループさせる
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 # ヤコビアンの転置行列
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("-----")
# img1の各ピクセルが回転行列と並進ベクトルによって、どのように移動するかと、
# 移動先で得られたimg2を用いて、img1を推定する
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)
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
テスト
・変換元の画像（img1）
・変換先の画像（img2）
・推定したパラメータから変換元の画像を復元（グレースケール）
ソファの傾きが補正されているようなので、完璧とは言えませんが、それなりには回転成分と並進成分の算出ができているようです。今回はICL-NUIM RGB-D datasetを利用させていただきました。他のデータセットでは試していないのでこの実装で大丈夫かどうかは要確認です。
終わりに
今回はdepth推定してないから若干タイトル詐欺っぽい。論文ではdepth推定してます。私もそろそろ推定したいです。