[3D分野の深層学習] PointNetLK[勉強しようシリーズ]

点群の深層学習の世界をのぞいていくシリーズ
![sd.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/581710/6b7fe64b-024e-fbd2-d2c9-53690de2ae25.png)

#はじめに
iphoneにもLiDARカメラが搭載される昨今ですが、
ノイズや外れ値から無縁の精度を得るには未だ時間がかかりそうです。

「深層学習ならノイズ・外れ値に対してもっと頑強になれるんじゃね？」という期待の元、
深層学習系の点群の位置合わせ等について勉強していきます。

勉強の過程をそのまま書いてるので、
本番に行く前に前提知識の説明が膨大に膨れたりしますがご容赦ください。



#勉強の方針
以前PointNetについて勉強しました。
https://qiita.com/akaiteto/items/d4d4e568fe16f73d0a1d

現在は、「Feature-metric registration」という位置合わせのアルゴリズムに興味があります。
https://arxiv.org/abs/2005.01014
https://github.com/XiaoshuiHuang/fmr

これを勉強する前に、基礎研究となっていると思われるPointNetLK、
https://arxiv.org/abs/1903.05711
https://github.com/hmgoforth/PointNetLK
について勉強します。

PointNetLKは日本人の方が研究に関わっているみたいですね。
PMVSも日本の方が考案していたりするし、点群の分野では日本は結構進んでたりするのかな？ﾆﾎﾝｼﾞﾝｽｹﾞｰ。

#前提知識
深層学習に関する知識が壊滅的に無いので補足としてこの項に記載します。
また、文章中に出てきた単語で過去に調べたものは下記を参照。
※1：https://qiita.com/akaiteto/items/d4d4e568fe16f73d0a1d

##〇RNN
めちゃくちゃ平たく言えば、
規則に基づいた一連の流れにあるデータにたいして、
その次に来るデータを予測できるのがRNNです。
以下に詳しく見てみます。

###CNN
【参考】
https://www.slideshare.net/gakhov/recurrent-neural-networks-part-1-theory

まずCNNを考えます。
![cbnn.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/581710/490408fa-86d6-535c-d885-fce883f2d371.png)
CNNから切り出したニューロンを考えたとき、
ニューロンは1つの入力に対して、1つの出力を返します。
そしてすべてのニューロンの出力を１つに全結合することでCNNが形成されます。

###FNN
そしてFNN。
出力層にて活性化関数を実行します。
これが最も一般的なネットワークの形態で、
入力層->中間層->出力層と順方向にデータが処理されていきます。

FNNは固定サイズの入力を受け取り、固定サイズの出力を行うため、
入力と出力が現実の値の連続値である場合、FNNで扱うのは非常に困難となります。

###RNN
本題のRNN。
FNNとは対照的に、連続で時系列のあるデータを扱うのが得意です。
![rnn.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/581710/93c24d06-6e27-66f4-1706-61362e0408a2.png)
現在の中間層が前の中間層の状態に依存して計算しそれを書く中間層で繰り返す、という特徴があります。

![rnns.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/581710/970f56e3-0bcf-0400-c7de-cb4ddde4c860.png)
式としては図のようにあらわされます。
W,Uは最適化を行うパラメータ、$\sigma$はtanh、ReLuなどの非線形関数です。

トレーニング方法として、「BPTT」というものがあるそうです。
BPTTでは、$x_1,\cdots,x_N$のシーケンスがあったときに最初は小さいシーケンスでトレーニングして、
徐々にシーケンスの幅をおおきくし最大Nの幅でトレーニングします。

BPTTは過去の全シーケンスすべてを扱おうとするため、
シーケンスの数が多すぎると処理も必要な記憶容量も増えてしまい、一部問題があります。

毎回すべての過去を振り返るのはとても面倒、
だからすべての過去を使わない方針が必要です。

すなわち、
「何を学習として利用するか。」
「何を学習に利用せずに忘れさせるか。」
この考慮が必要になります。

そこで、LSTMというものを導入します。
![sss.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/581710/75efd3fd-0f1a-043d-8770-cfec36511dc6.png)

```math
C_t = f_t・C_t-1 + i_t・C'
```

RNNのユニット内での計算結果である$C_t$は、
現在のシーケンスの結果$i_t・C'$と、
前回のシーケンスの結果$f_t・C_t-1$の足し算で表されます。

$i_t$でもって、新しく計算した$C'$をどれくらい結果に反映させるか決定し、
$f_t$でもって、過去のシーケンスの結果が重要であるなら未来永劫反映させるし、
重要でないなら忘却させる、という役割を実行します。
このような計算をおこなった$C_t$は次のユニットの計算に渡し、
同様の計算を繰り返します。

中間層の計算結果$h_t$については、
出力ゲート$o_t$でもって、結果の量を調整しながら、活性化関数tanhで出力します。

##〇Lucas–Kanade法
https://qiita.com/icoxfog417/items/357e6e495b7a40da14d8
こちらの記事が丁寧でわかりやすかったです。
LKアルゴリズムによって、ある時間tと別つの時間(t+dt)に撮影された2D画像に対して、
あるピクセルの周囲3x3のピクセルはみな同じ動きをするという仮定のもと、
ピクセルの座標x,yの時間変化に対する勾配$\frac{dx}{dt}$、$\frac{dy}{dt}$が計算されます。

##〇exponential map
三次元の回転を表すものとして、回転行列やquaternionなどがありますが、
exponential mapは回転を表す表現の一つとなります。

https://swkagami.hatenablog.com/entry/lie_00toc

こちらのサイトがとても分かりやすそうです。
以下、個人的にメモしたい箇所を羅列します。

〇回転Rを1軸周りの回転$\theta$で微分した定義から、微分方程式を考える。

```math
\frac{dR(\theta)}{d\theta} = lim \frac{R(\theta + d\theta) - R(\theta)}{d\theta}\\
\cdots 省略　\cdots \\
\frac{dR(\theta)}{d\theta} = XR(\theta) , R(0)=I\\
⇔\\
R(\theta) = exp(\theta R)
```

〇三次元における回転行列Rを例に考える
回転行列$R$は$R^TR = I$を満たします。
$R(t) = exp(tR)$と考えたとき、$(exp(tR))exp(tR)$は、

```math
(exp(tR))exp(tR) = I
\cdots 省略　\cdots \\
R^Texp(tR^T)Texp(tR) + exp(tR^T)Rexp(tR)
```

が成立します。t=0の場合でも当然この式は成立するので、

```math
R^T + R =0
```

$R^T + T = 0$より、$R$は歪対称行列となります。
歪対称行列なので、$R_11,R_22,R_33$が0、
その他6つの要素は3つの変数の正負でもって構成される行列になります。

3つの変数が存在する行列となるので、
歪対称行列を3つの変数$x_1,x_2,x_3$で外に出して分解すると、以下のように$R$は表される。

```math
R = \xi_1T_1 + \xi_2T_2 + \xi_3T_3\\
⇔\\
R(\theta) = exp\theta R\\
⇔\\
R(\xi) = exp(\sum_{i=3}\xi_i T_i) \\
```

##〇ロドリゲスの回転公式
https://mathworld.wolfram.com/RodriguesRotationFormula.html
実際にexposuremapで表現する際、コードではロドリゲスの回転公式で表現されます。

```math
R(\theta) = exp(\theta R)\\
R(\theta) = I + Wsin\theta + w^2(1-cos\theta)
```

PointNetLKのコードの一部を抽出したものを下記に記載し、重要な部分を見ていきます。

```test.py

import torch.nn as nn
import torch.nn.functional as F
import torch
from torch import optim
import numpy as np
import sinc

# 1.ポイントを1024個もつ２個の点群のデータを作成
p0 = np.random.rand(2,1024,3)
p0 = torch.from_numpy(p0.astype(np.float32)).clone()


# 2.点群に対して小さな範囲で剛体変化を実行するためのﾃﾞﾙﾀを指定
batch_size = p0.size(0)
num_points = p0.size(1)
delta = 1.0e-2
w1 = delta
w2 = delta
w3 = delta
v1 = delta
v2 = delta
v3 = delta
twist = torch.Tensor([w1, w2, w3, v1, v2, v3])
dt = torch.nn.Parameter(twist.view(1, 6), requires_grad=False)
dt = dt.to(p0).expand(batch_size, 6)

# 2.点群に対して小さな範囲で剛体変化を実行するためのﾃﾞﾙﾀを指定
import se3
exp = se3.Exp  # [B, 6] -> [B, 4, 4]

# 3.２つの点群に対して、delta分の変異分ソース点群を摂動させる
transf = torch.zeros(batch_size, 6, 4, 4).to(p0)
for b in range(p0.size(0)):
    # 4.ロドリゲスの回転公式 R(θ) = I + Wsinθ +W*W(1-cosθ)
    # 5.Wの準備をする。
    print("ロドリゲスの回転公式")
    print("R(θ) = I + wsinθ +w*w(1-cosθ)")
    d = torch.diag(dt[b, :])  # [6, 6]
    x = -d
    x_ = x.view(-1, 6)
    w, v = x_[:, 0:3], x_[:, 3:6]
    def mat(x):
        # size: [*, 3] -> [*, 3, 3]
        x_ = x.view(-1, 3)
        x1, x2, x3 = x_[:, 0], x_[:, 1], x_[:, 2]
        O = torch.zeros_like(x1)

        X = torch.stack((
            torch.stack((O, -x3, x2), dim=1),
            torch.stack((x3, O, -x1), dim=1),
            torch.stack((-x2, x1, O), dim=1)), dim=1)
        return X.view(*(x.size()[0:-1]), 3, 3)
    W = mat(w)
    print("\n")
    print("[反対称行列 w]")
    print(W)

    # 5.θの準備をする。
    t = w.norm(p=2, dim=1).view(-1, 1, 1)
    print("\n")
    print("[角度 θ]")
    print(t)
    exit()

    # 5.W*Wの準備をする。
    S = W.bmm(W)
    print("\n")
    print("[反対称行列 W*W]")
    print(W)

    # 5.Iの準備をする。
    I = torch.eye(3).to(w)

    # Rodrigues' rotation formula.
    R = I + sinc.sinc1(t)*W + sinc.sinc2(t)*S
```

```
ロドリゲスの回転公式
R(θ) = I + wsinθ +w*w(1-cosθ)


[反対称行列 w]
tensor([[[ 0.0000,  0.0000, -0.0000],
         [-0.0000,  0.0000,  0.0100],
         [ 0.0000, -0.0100,  0.0000]],

        [[ 0.0000,  0.0000, -0.0100],
         [-0.0000,  0.0000,  0.0000],
         [ 0.0100, -0.0000,  0.0000]],

        [[ 0.0000,  0.0100, -0.0000],
         [-0.0100,  0.0000,  0.0000],
         [ 0.0000, -0.0000,  0.0000]],

        [[ 0.0000,  0.0000, -0.0000],
         [-0.0000,  0.0000,  0.0000],
         [ 0.0000, -0.0000,  0.0000]],

        [[ 0.0000,  0.0000, -0.0000],
         [-0.0000,  0.0000,  0.0000],
         [ 0.0000, -0.0000,  0.0000]],

        [[ 0.0000,  0.0000, -0.0000],
         [-0.0000,  0.0000,  0.0000],
         [ 0.0000, -0.0000,  0.0000]]])


[角度 θ]
tensor([[[0.0100]],

        [[0.0100]],

        [[0.0100]],

        [[0.0000]],

        [[0.0000]],

        [[0.0000]]])


[反対称行列 W*W]
tensor([[[ 0.0000,  0.0000, -0.0000],
         [-0.0000,  0.0000,  0.0100],
         [ 0.0000, -0.0100,  0.0000]],

        [[ 0.0000,  0.0000, -0.0100],
         [-0.0000,  0.0000,  0.0000],
         [ 0.0100, -0.0000,  0.0000]],

        [[ 0.0000,  0.0100, -0.0000],
         [-0.0100,  0.0000,  0.0000],
         [ 0.0000, -0.0000,  0.0000]],

        [[ 0.0000,  0.0000, -0.0000],
         [-0.0000,  0.0000,  0.0000],
         [ 0.0000, -0.0000,  0.0000]],

        [[ 0.0000,  0.0000, -0.0000],
         [-0.0000,  0.0000,  0.0000],
         [ 0.0000, -0.0000,  0.0000]],

        [[ 0.0000,  0.0000, -0.0000],
         [-0.0000,  0.0000,  0.0000],
         [ 0.0000, -0.0000,  0.0000]]])
```


##〇線形近似
![ssss.png](https://upload.wikimedia.org/wikipedia/commons/thumb/a/a7/TangentGraphic2.svg/300px-TangentGraphic2.svg.png)緑の線は非線形である。しかし、x=aのあるものすごく狭い範囲で見れば線形と言える。
2階微分可能なfに対して、テイラー展開を行ったfに対して、次式のようにあらわせる。

```math
f(x) \simeq f(a) + (\frac{d}{dx}f(a))f(x-a)
```

#○疑似逆行列
逆行列は縦横が等しい正方行列のみ計算可能です。
長方形のような形の行列は逆行列を計算できないので、
疑似逆行列を用いて計算します。

```math
A^+ = (AA^T)^{-1}A^T
```

#読む(論文)
メモ代わりに翻訳した内容の概要をたらたら記載します。

## 1
この論文の中心的な内容は以下の点です。
(1)2D画像の位置合わせに用いられるLucas＆Kanade（LK）アルゴリズムをPointNet向けに実行する方法
(2)PointNetとLKアルゴリズムをRNNに展開する方法

LKアルゴリズムを適用することで、3D点群にどれほどの変化があるかをとらえます。
ただ、PointLKでは畳み込みで抽出された特徴に対して行うため、
従来の勾配を計算する方法は適用できません。
PointNetLKでは、LKアルゴリズムの変更と、新たに変更したLKアルゴリズムを
RNNに適用することで、PointNetLKを構築します。

## 3.1
$\phi : R^{3×N} ⇒ R^K$は$\phi$はN個のポイントを持つ点群に対して
mlp(多層パーセプトロン※1)を適用し、最終出力としてK次元のデータを得たものです。

```math
\phi(P_T) = \phi(G・P_S)
```

問題の本質は、ターゲットである点群$P_T$に、
ソースの点群$P_S$を剛体変換$G$をかけることで、
上式が成立するような剛体変換$G$を見つけることが目的です。

```math
G=exp(\sum_{i}\xi_i T_i)
```

Gは上式のようにツイストパラメーター$\xi$をもつ"exponential map"で表されます。

```math
\phi(P_T) = \phi(G・P_S)
```

２D画像に適用されるLKアルゴリズムではピクセル値$I$に対して、
I(x(t),t)=I(x(t),t)となるような最小の$\frac{dx(t)}{dt}$を求めます。
ピクセル値の差が最小になる値を探すLKアルゴリズムと
$\phi(P_T) = \phi(G・P_S)$が成立する各点群の差が最小になる値Gを探す問題は
非常に類似していることがわかります。

また、PointNetLKではPointNetにTnetを持ちません。
Tnetは点群の向きをそろえるためのミニネットですが、
かわりにPointNetLK用のLKアルゴリズムを適用するので不要となります。



## 3.2 
導出過程。

```math
\phi(P_T) = \phi(G・P_S)\\
⇔\\
\phi(P_S) = \phi(G^-1・P_T)\\

```

線形化させます。

```math
\phi(P_S) = \phi(P_T) + \frac{d}{d\xi}[\phi(G^-1・P_T)]\xi\\
\phi(P_S) = \phi(P_T) + J\xi
```

Jは$J=\frac{d}{d\xi}[\phi(G^-1・P_T)]$のヤコビ行列です。
ヤコビ行列Jを計算しなければなりません。

Jは以下のように計算します。

```math
J = \frac
{\phi(exp(t_iT_i)・P_T) - \phi(P_T)}
{t_i}
```

tiはツイストパラメータ$\xi$の微小摂動です。
$exp(t_iT_i)$はexponential mapで表された剛体変換であり、
点群$P_T$をわずかだけ摂動させたときの点群$P_T$の点群$P_S$に対する微小変化を計算します。

PointNetLKでは、exponential mapJは初めの一回だけ上式によって計算されます。

```math
\xi = J^+[\phi(P_S) - \phi(P_T)]
```

そして$\xi$を計算し、Gを計算した後、
$P_S$に計算した剛体変換Gを適用させて更新します。

```math
P_s ⇐ G・P_S\\
\Delta G=exp(\sum_{i}\xi_i T_i)
```

反復処理の中で$G$は更新され続けるので、$G=G_nG_{n-1} \cdots G_0$と連なります。


## 3.3
損失関数は以下の定義の物を使います。

```math
|(G_est)^-1・G_{groundtruth} - I_4|
```

PoninNetでは点群内部の登録された情報がバラバラな問題に対してプーリングで対応します。
実験から、ノイズの多い点群では、平均プーリングのほうが有利と考えられます。



#読む(コード)
ということでつまみ摘みコードを読んでみながら、
実際にどういう計算をやるか見てみます。
https://github.com/hmgoforth/PointNetLK/blob/master/experiments/train_classifier.py

## 1.PointNetの学習
PointNetのトレーニングをまずは行います。

### １．モデルにトレーニングデータを入れます。
https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointnet.py#L97


### ２．特徴の計算
https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointnet.py#L97

PointNetの通り、input_transform(T_NET)->mlp(64,64)->feature_transform(TNet)と計算していきます。
ここが論文で言うところの$\phi(x)$です。PointNetLKでも登場します。
![無題.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/581710/ac23e50a-5cb1-e878-d6ad-b7e22d6699e3.png)
コードを見ると、global featureはコメントアウトして計算していないように見えます。
ローカルな特徴だけを使うようです。

### ３．入力点群の数×分類クラスの行列出力
https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointnet.py#L144
そして分類。SegmentationNetworkの実行です。
これにより、入力した点群が各分類クラスにどれくらいマッチしてるかの行列が出ました。


### ４．損失関数計算
https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/experiments/train_classifier.py#L224
最適化を行うために、「SegmentationNetwork」と「feature_transform(Tnet)」の損失を計算します。
ここの計算でinput_transformの結果を盛り込まないのは、input_transformで点群の回転・移動の変位を
吸収する処理を、PointNetLK内のLKレイヤーが代わりに行うので、
ここではあえてやっていないのかもしれません。


## 2.PointNetLKの学習
つづけてPointNetLKのトレーニングの処理。

### １．モデルにトレーニングデータを入れます。
同じように処理します。

### ２．特徴の計算
https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointlk.py#L143
論文で言うところの$\phi(x)$を計算します。

### ３．Jの計算
https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointlk.py#L106

```math
J = \frac
{\phi(exp(t_iT_i)・P_T) - \phi(P_T)}
{t_i}
```

ヤコビ行列Ｊを計算します。
その前に、
(1)デルタ分だけ摂動させる変換行列をexponential mapの表現に変換する。
https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointlk.py#L122
（$exp(t_iT_i)$の実装部分）

(2)摂動した点群に対して特徴を抽出
https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointlk.py#L126
（$\phi(exp(t_iT_i)・P_T)$の実装部分）
PointNetのFeature_transformを適用させます。
ちなみに、「２．特徴の計算」で特徴の計算を行っていますが、これはターゲットの点群のほう。
式でいうところの$\phi(P_T)$にあたります。



### ４．Jの逆行列計算

```math
\xi = J^+[\phi(P_S) - \phi(P_T)]
```
(1)疑似逆行列の計算

https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointlk.py#L158

Jの疑似逆行列を計算します。

（２）$J^+[\phi(P_S) - \phi(P_T)]$を計算します。
https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointlk.py#L181

### ５．変換行列Gに微小変化分を適用
https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointlk.py#L101

```math
P_s ⇐ \Delta G・P_S\\
\Delta G=exp(\sum_{i}\xi_i T_i)
```

常識の計算後、$G=G_nG_{n-1} \cdots G_0$を計算するために各Gを保持します。



### ６．損失関数評価

https://github.com/hmgoforth/PointNetLK/blob/fab02ef0fcd48cb7838c50a61b4d183a80eab65b/ptlk/pointlk.py#L43

```math
|(G_est)^-1・G_{groundtruth} - I_4|
```

Igtは正解の剛体変換結果。