「3D位置のわかっているターゲット点の画像から、ガウス・ニュートン法でカメラ位置・向きを推定」のシミュレーション



# はじめに

- ターゲット点の __複数画像から__ カメラ位置・向きを推定する方法として、例えば __エピポーラ幾何__ があります
- また __ArUcoマーカーから__ カメラ位置・向きを推定する関数が __OpenCV__ にあります

それらとの比較として。

- ターゲット点の正確な位置がわかってる場合、 __１つの画像から__ どれだけ推定できるのか、試しました
- 試した推定方法は、非線形最小二乗法の１つ、 __ガウス・ニュートン法__ 
- 大したGPUのないノートPCで、実用的な速度で計算できるかにも興味がありました

### 工夫したこと
1. 実際の画像ではなく、ターゲット画像を想定して __シミュレーション__
　純粋に推定能力を調べるため、ターゲットの検出誤差は0として、正解と推定値を比較

2. まじめに __逆行列計算__
　Pythonの数値解析ソフトウェアライブラリの１つ __SciPy__ によりよい機能があるようですが、今回への適用方法がよくわからず。。。

3. ターゲット点に __重み付け__ を適用
　推定する際に、遠くの点は画像の分解能が足りずエラーが大きいと考え、各点に、入力点-カメラ間の距離の逆数の重み付け

# 内容
### 1.PC環境
　実施したPC環境は以下のとおりです

| | |
|:--:|:--:|
|CPU|Celeron N4100|
|メモリ|8GB LPDDR4|

### 2.前準備
　Windowsで使えるようにするため、以下のツール / システムをインストールしました。
インストール時に参考にしたサイトも記載します

1. __python__（使用したのはVer.3.7 ）
　実行時のベースシステムです
（参考）https://qiita.com/ssbb/items/b55ca899e0d5ce6ce963
2. __pip__（使用したのはVer.21.2.4 ）
　他のツールをダウンロードする際に使うツールです
（python3系ではバージョン3.4以降であれば、pythonのインストールと共にpipもインストールされます。）
（参考）https://gammasoft.jp/python/python-library-install/

3. __OpenCV__（使用したのはVer.4.5.3.56 ）
　画像系処理するためのライブラリです
（参考）https://qiita.com/ideagear/items/3f0807b7bde05aa18240

4. __numpy__（使用したのVer.4.7.0.72）
　行列計算が得意な数値計算モジュールです
（参考）https://qiita.com/butako/items/15d7cb5aaef90b09ccd8

### 3.pyファイルの作成
 組んだコードは次のとおりです。

~~~python

# --- ライブラリのインポート ---
import cv2 #OpenCV
import math #math
import numpy as np #numpy

# --- 初期設定 ---

## -- 関数の定義 -- ##
# アフィン変換行列の計算 == 平行移動→Z軸回転→Y軸回転→X軸回転の行列を返す ==
def ShiftRotateMat_xyz(x, y, z, theta_x, theta_y, theta_z):
    theta_x = math.radians(theta_x)
    theta_y = math.radians(theta_y)
    theta_z = math.radians(theta_z)
    rot_x = np.array([[ 1,                 0,                  0, 0],
                      [ 0, math.cos(theta_x), -math.sin(theta_x), 0],
                      [ 0, math.sin(theta_x),  math.cos(theta_x), 0],
                      [ 0,                 0,                  0, 1]])

    rot_y = np.array([[ math.cos(theta_y), 0,  math.sin(theta_y), 0],
                      [                 0, 1,                  0, 0],
                      [-math.sin(theta_y), 0,  math.cos(theta_y), 0],
                      [                 0, 0,                  0, 1]])

    rot_z = np.array([[ math.cos(theta_z), -math.sin(theta_z), 0, 0],
                      [ math.sin(theta_z),  math.cos(theta_z), 0, 0],
                      [                 0,                  0, 1, 0],
                      [                 0,                  0, 0, 1]])
    rot_s = np.array([[ 1, 0, 0, x],
                      [ 0, 1, 0, y],
                      [ 0, 0, 1, z],
                      [ 0, 0, 0, 1]], dtype=np.float64)

    rot_matrix = rot_x.dot(rot_y.dot(rot_z.dot(rot_s)))
    rot_matrix2 = rot_matrix[:3]
    return rot_matrix2

# アフィン変換行列での座標変換 == 点（x,y,z）を アフィン変換行列で移動させた座標を返す ==
def MatDot_xyz(rot_mat ,x, y, z):
    x_vec = np.array([x, y, z, 1])
    rot_vec = rot_mat.dot(x_vec.T).T
    return rot_vec

# スクリーン投影の計算 	== 点（x,y,z）をアフィン変換行列で移動し、カメラ前dの位置にある、幅w、高さhのスクリーンに投影される座標を返す ==
def XY(rot_mat ,x, y, z, w, h, d):
    t_vec = MatDot_xyz(rot_mat, x, y, z)
    ans = False
    ans_vec = np.zeros((1,2))
    if t_vec[0] > d:
        ans = True
        ans_vec = np.array([t_vec[1]*d/t_vec[0], t_vec[2]*d/t_vec[0]])
        if abs(ans_vec[0])<w/2.0 and abs(ans_vec[1]<h/2.0):
            ans = True
        else:
            ans = False
    return ans, ans_vec

# ガウス・ニュートン法の変化量計算 == 入力点リストから、推定されるカメラの位置・向きへの変更量を返す ==
def calc_dxtx(lst, theta_x, theta_y, theta_z, x, y, z, dx, dtx):

    #微小変化したときのアフィン変換行列を計算
    r_mat = ShiftRotateMat_xyz(x, y, z, theta_x, theta_y, theta_z)
    r_mat_x = ShiftRotateMat_xyz(x + dx, y, z, theta_x, theta_y, theta_z)
    r_mat_y = ShiftRotateMat_xyz(x, y+ dx, z, theta_x, theta_y, theta_z)
    r_mat_z = ShiftRotateMat_xyz(x, y, z + dx, theta_x, theta_y, theta_z)
    r_mat_tx = ShiftRotateMat_xyz(x, y, z, theta_x + dtx, theta_y, theta_z)
    r_mat_ty = ShiftRotateMat_xyz(x, y, z, theta_x, theta_y + dtx, theta_z)
    r_mat_tz = ShiftRotateMat_xyz(x, y, z, theta_x, theta_y, theta_z + dtx)

    #合計計算値を入れる行列、ベクトルを準備
    Aa = np.zeros((6,6))
    Bb = np.zeros((6,1))
    Cc = np.zeros((3,1))

    #合計計算
    for l_i in lst:
        if l_i[5]>0:
            s_flg, s_vec = XY(r_mat , l_i[2], l_i[3], l_i[4], Ww, Hh, Dd)
            s_flgx, s_vec_x = XY(r_mat_x , l_i[2], l_i[3], l_i[4], Ww, Hh, Dd)
            s_flgy, s_vec_y = XY(r_mat_y , l_i[2], l_i[3], l_i[4], Ww, Hh, Dd)
            s_flgz, s_vec_z = XY(r_mat_z , l_i[2], l_i[3], l_i[4], Ww, Hh, Dd)
            s_flgtx, s_vec_tx = XY(r_mat_tx , l_i[2], l_i[3], l_i[4], Ww, Hh, Dd)
            s_flgty, s_vec_ty = XY(r_mat_ty , l_i[2], l_i[3], l_i[4], Ww, Hh, Dd)
            s_flgtz, s_vec_tz = XY(r_mat_tz , l_i[2], l_i[3], l_i[4], Ww, Hh, Dd)

            if s_flg*s_flgx*s_flgy*s_flgz*s_flgtx*s_flgty*s_flgtz == True:
                Xdiv_x = (s_vec_x[0] - s_vec[0])/dx
                Xdiv_y = (s_vec_y[0] - s_vec[0])/dx
                Xdiv_z = (s_vec_z[0] - s_vec[0])/dx
                Xdiv_tx = (s_vec_tx[0] - s_vec[0])/dtx
                Xdiv_ty = (s_vec_ty[0] - s_vec[0])/dtx
                Xdiv_tz = (s_vec_tz[0] - s_vec[0])/dtx
                
                Ydiv_x = (s_vec_x[1] - s_vec[1])/dx
                Ydiv_y = (s_vec_y[1] - s_vec[1])/dx
                Ydiv_z = (s_vec_z[1] - s_vec[1])/dx
                Ydiv_tx = (s_vec_tx[1] - s_vec[1])/dtx
                Ydiv_ty = (s_vec_ty[1] - s_vec[1])/dtx
                Ydiv_tz = (s_vec_tz[1] - s_vec[1])/dtx

                Cc[0] += ((l_i[0] - s_vec[0]) ** 2 + (l_i[1] - s_vec[1]) ** 2)*l_i[5]
                Cc[1] += 1.0*l_i[5]
                Cc[2] += 1.0

                Bb[0] += ((l_i[0] - s_vec[0]) * Xdiv_x + (l_i[1] - s_vec[1]) * Ydiv_x)*l_i[5]
                Bb[1] += ((l_i[0] - s_vec[0]) * Xdiv_y + (l_i[1] - s_vec[1]) * Ydiv_y)*l_i[5]
                Bb[2] += ((l_i[0] - s_vec[0]) * Xdiv_z + (l_i[1] - s_vec[1]) * Ydiv_z)*l_i[5]
                Bb[3] += ((l_i[0] - s_vec[0]) * Xdiv_tx + (l_i[1] - s_vec[1]) * Ydiv_tx)*l_i[5]
                Bb[4] += ((l_i[0] - s_vec[0]) * Xdiv_ty + (l_i[1] - s_vec[1]) * Ydiv_ty)*l_i[5]
                Bb[5] += ((l_i[0] - s_vec[0]) * Xdiv_tz+ (l_i[1] - s_vec[1]) * Ydiv_tz)*l_i[5]

                Aa[0][0] += (Xdiv_x * Xdiv_x + Ydiv_x * Ydiv_x)*l_i[5]
                Aa[0][1] += (Xdiv_x * Xdiv_y + Ydiv_x * Ydiv_y)*l_i[5]
                Aa[0][2] += (Xdiv_x * Xdiv_z + Ydiv_x * Ydiv_z)*l_i[5]
                Aa[0][3] += (Xdiv_x * Xdiv_tx + Ydiv_x * Ydiv_tx)*l_i[5]
                Aa[0][4] += (Xdiv_x * Xdiv_ty + Ydiv_x * Ydiv_ty)*l_i[5]
                Aa[0][5] += (Xdiv_x * Xdiv_tz + Ydiv_x * Ydiv_tz)*l_i[5]

                Aa[1][1] += (Xdiv_y * Xdiv_y + Ydiv_y * Ydiv_y)*l_i[5]
                Aa[1][2] += (Xdiv_y * Xdiv_z + Ydiv_y * Ydiv_z)*l_i[5]
                Aa[1][3] += (Xdiv_y * Xdiv_tx + Ydiv_y * Ydiv_tx)*l_i[5]
                Aa[1][4] += (Xdiv_y * Xdiv_ty + Ydiv_y * Ydiv_ty)*l_i[5]
                Aa[1][5] += (Xdiv_y * Xdiv_tz + Ydiv_y * Ydiv_tz)*l_i[5]

                Aa[2][2] += (Xdiv_z * Xdiv_z + Ydiv_z * Ydiv_z)*l_i[5]
                Aa[2][3] += (Xdiv_z * Xdiv_tx + Ydiv_z * Ydiv_tx)*l_i[5]
                Aa[2][4] += (Xdiv_z * Xdiv_ty + Ydiv_z * Ydiv_ty)*l_i[5]
                Aa[2][5] += (Xdiv_z * Xdiv_tz + Ydiv_z * Ydiv_tz)*l_i[5]

                Aa[3][3] += (Xdiv_tx * Xdiv_tx + Ydiv_tx * Ydiv_tx)*l_i[5]
                Aa[3][4] += (Xdiv_tx * Xdiv_ty + Ydiv_tx * Ydiv_ty)*l_i[5]
                Aa[3][5] += (Xdiv_tx * Xdiv_tz + Ydiv_tx * Ydiv_tz)*l_i[5]

                Aa[4][4] += (Xdiv_ty * Xdiv_ty + Ydiv_ty * Ydiv_ty)*l_i[5]
                Aa[4][5] += (Xdiv_ty * Xdiv_tz + Ydiv_ty * Ydiv_tz)*l_i[5]
                
                Aa[5][5] += (Xdiv_tz * Xdiv_tz + Ydiv_tz * Ydiv_tz)*l_i[5]

    #行列を反転行列化
    for i in range(1,5):
        for j in range(0,i):
            Aa[i][j] = Aa[j][i]

    #逆行列計算による変化量算出
    try:
        invAa = np.linalg.inv(Aa)
        dxtx = np.squeeze(invAa.dot(Bb).T)
    except:
        dxtx = np.zeros(6)

    #分散の算出
    Cc = np.squeeze(Cc.T)
    Ff = Cc[0]/Cc[1]

    #途中経過の推定値と分散のコマンドライン表示：＜今回はコメントアウトしてます。表示させたければ、2行下の#を消してください＞
    disp_value = "{:#} | {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}: {:.3e}"
#    print(disp_value.format(int(Cc[2]), x, y, z, theta_x, theta_y, theta_z, Ff))

    #戻り値は分散と変化量
    return Ff, dxtx


## -- 変数の定義 -- ##
#スクリーンの寸法パラメータ
Ww = 640
Hh = 480
Dd = 300.0

#シミュレーションのターゲット点リスト
target_lst = [] # 3次元上の座標[x, y, z]のリスト

#ガウス・ニュートン法で使用する入力点リスト
input_lst = [] # [スクリーンに投影された座標[X,Y], 対応するターゲット点[x, y, z], その点の重み W ]のリスト

#シミュレーションのターゲット値
#シフト値
#方向の定義：前方向が+x方向、左方向が+y方向、上方向が+z方向
x = 1000.0
y = 0.0
z = -1000.0

#回転値（度）
#方向の定義：バンク角がtheta_x、仰俯角がtheta_y、方位角がtheta_z
theta_x = 0.0
theta_y = 0.0
theta_z = 0.0

#ガウス・ニュートン法で求める推定値の初期設定：ターゲット値をコピー
#シフト値
x0 = x
y0 = y
z0 = z

#回転値（度）
theta_x0 = theta_x
theta_y0 = theta_y
theta_z0 = theta_z

#ガウス・ニュートン法で偏微分を求めるための微小変化量パラメータ
dx = .1 #x, y, z の微小変化量
dtx = .1 #theta_x, theta_y, theta_z の微小変化量


## -- ループ前の準備 -- ##
#ターゲット点リストの準備：今回は、Z=0平面の格子状配置の点
for n in range(0,21):
  for m in range(-10,11):
    target_lst.append([200.0 * n, 200.0 * m, 0.0])

#シミュレーションのためのキー操作の説明をコンソールに表示
print("Key command:")
print("w/e/r = theta_xyz+5, x/c/v = theta_xyz-5, s/d/f = theta_xyz_reset")
print("t/y/u = x/y/z+50, b/n/m = xyz-50, g/h/j = xyz_reset")
print("q = exit")

# --- シミュレーション ループ 開始 ---
while True:
#（breakするまで繰り返し）

#プロット画面の初期化
    img = np.ones((Hh, Ww, 3), np.uint8)*255 
    #プロット画面のクリア

#シミュレーション ターゲット点の描画とガウス・ニュートン法の入力点リスト準備
#アフィン変換の行列計算
    r_mat = ShiftRotateMat_xyz(x, y, z, theta_x, theta_y, theta_z)
#アフィン変換を使って、ターゲット点のプロット位置を計算・描画・入力点リスト準備
    input_lst.clear()
    for l_i in target_lst:
        s_flg, s_vec = XY(r_mat , l_i[0], l_i[1], l_i[2], Ww, Hh, Dd)
        if s_flg == True:
            w_i = round(-s_vec[0] + Ww / 2.0)
            h_i = round(-s_vec[1] + Hh / 2.0)

            cv2.circle(img, (w_i, h_i), 3, (255, 0, 0), -1) #imgにターゲット点を青(BGR)で塗りつぶし描画

            input_lst.append([round(s_vec[0]),round(s_vec[1]), l_i[0], l_i[1], l_i[2], 1])

#入力点リストの重みを計算：今回は入力点-カメラ間の仮の距離の逆数を重みで使用
    r_mat = ShiftRotateMat_xyz(x0, y0, z0, theta_x0, theta_y0, theta_z0)
    d1_vec = np.array([-x0, -y0, -z0])
    for l_i in input_lst:
        p_vec = np.array([300, l_i[0], l_i[1]])
        d2_vec = p_vec.dot(r_mat)
        d2_vec = d2_vec[:3]
        l_i[5] = 1 / (np.sqrt(np.sum(p_vec**2))*d1_vec[2]/(-d2_vec[2]))

#（今回は、仮の距離が3000以上の点は使わないようにするために、その点の重みを（-1）に変更：負の重みの点はあとの計算で使用しない）
        if l_i[5] < 1/3000.0:
            l_i[5] = -1

#（ガウス・ニュートン法ループを必ず１回は回すための仮の分散値2を代入）
    Ff = 2

## -- ガウス・ニュートン法 ループ 開始 -- ##
    while Ff > 0.3:
#（分散が0.3より大きい間、繰り返す）

        #ガウス・ニュートン法の変化量計算
        Ff, dxtx = calc_dxtx(input_lst, theta_x0, theta_y0, theta_z0, x0, y0, z0, dx, dtx)

        #変化量を加算
        x0 += dxtx[0]
        y0 += dxtx[1]
        z0 += dxtx[2]
        theta_x0 += dxtx[3]
        theta_y0 += dxtx[4]
        theta_z0 += dxtx[5]

#ガウス・ニュートン法の推定点の描画
#アフィン変換の行列計算
        r_mat = ShiftRotateMat_xyz(x0, y0, z0, theta_x0, theta_y0, theta_z0)
#アフィン変換を使って、推定点のプロット位置を計算・描画
        for l_i in input_lst:
            if l_i[5]>0:
                s_flg, s_vec = XY(r_mat , l_i[2], l_i[3], 0.0, Ww, Hh, Dd)
                if s_flg == True:
                    w_i = round(-s_vec[0] + Ww / 2.0)
                    h_i = round(-s_vec[1] + Hh / 2.0)

                    cv2.circle(img, (w_i, h_i), 2, (0, 0, 255), -1) #imgに赤(BGR)で推定点を塗りつぶし描画

#プロット画面の表示
        cv2.imshow("Simulation",img) #別ウィンドウを開きimgを表示

#キー操作：キー操作でターゲット値を変更 → 推定点が追随できるかを追う
        keys = cv2.waitKey(1) & 0xFF

#キー操作：シフト値の変更
        if keys == ord('g'):
            x = 1000.0
        if keys == ord('h'):
            y = 0.0
        if keys == ord('j'):
            z = -1000.0
        if keys == ord('t'):
            x = x + 50
        if keys == ord('y'):
            y = y + 50
        if keys == ord('u'):
            z = z + 50
        if keys == ord('b'):
            x = x - 50
        if keys == ord('n'):
            y = y - 50
        if keys == ord('m'):
            z = z - 50

#キー操作：回転値の変更
        if keys == ord('s'):
            theta_x = 0.0
        if keys == ord('d'):
            theta_y = 0.0
        if keys == ord('f'):
            theta_z = 0.0
        if keys == ord('w'):
            theta_x = theta_x + 5
        if keys == ord('e'):
            theta_y = theta_y + 5
        if keys == ord('r'):
            theta_z = theta_z + 5
        if keys == ord('x'):
            theta_x = theta_x - 5
        if keys == ord('c'):
            theta_y = theta_y - 5
        if keys == ord('v'):
            theta_z = theta_z - 5

#キー操作：シミュレーション終了
        if keys == ord('q'):
            Ff = -1.0
            break
## -- ガウス・ニュートン法 ループ 終了 -- ##

    if Ff < 0:
        break
# --- シミュレーション ループ 終了 ---

cv2.destroyAllWindows() #プロット画面を閉じる

~~~

### 4.使い方
![s_start.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2449065/59d19277-f7c2-12b6-cc4f-810f55be9a48.png)

- 実行すると、__ターゲット点を青__ で、推定された位置・向きから計算した __推定点を赤__ でプロット表示（最初はそろってるので同じ位置にプロットされます）

![s_estimatings.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2449065/02d76652-7826-ab0a-cb3a-e297f5ab8200.png)

- プロットウィンドウ上でのキー操作で、カメラの __バンク角（[w,x]）__ 、__仰俯角（[e,c]）__ 、__方位角（[r,v]）__ の他、カメラの __前後（[t,b]）__ 、__左右（[y,n]）__ 、__上下（[u,m]）__ の位置を変更でき、それに合わせて推定点を一致させるよう動きます。__qキーで終了__ です

### 5.シミュレーションの前提
- ターゲット点は、カメラの前方の床に4$m$ x 4$m$のエリアに20$cm$間隔で並んだの正方格子状の点
- スクリーンはカメラ前方30$cm$のところに、幅64$cm$ x 縦48$cm$のサイズであり、解像度は640 x 480
- カメラからの距離3$m$以内のターゲット点を使って推定します


### 6.結果
- カメラ位置・向きの収束誤差はだいたい位置で数$mm$、向きで0.1$度$くらい。スクリーン上での標準偏差は0.40 $mm$くらい
- スクリーン解像度が収束誤差に効きます。解像度を上げれば、収束誤差は小さくなります


# 参考

エピポーラ幾何）
https://qiita.com/ykoga/items/14300e8cdf5aa7bd8d31

ArUcoマーカーでの姿勢推定）
https://qiita.com/namahoge/items/69b4e2c66f5446dc8798

ガウス・ニュートン法）
https://qiita.com/SolKul/items/61fce5278b26df9ca77f

scipy.optimize.least_squares）
https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.least_squares.html