はじめに

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

それらとの比較として。

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

工夫したこと

実際の画像ではなく、ターゲット画像を想定して シミュレーション
　純粋に推定能力を調べるため、ターゲットの検出誤差は0として、正解と推定値を比較
まじめに 逆行列計算
　Pythonの数値解析ソフトウェアライブラリの１つ SciPy によりよい機能があるようですが、今回への適用方法がよくわからず。。。
ターゲット点に 重み付け を適用
　推定する際に、遠くの点は画像の分解能が足りずエラーが大きいと考え、各点に、入力点-カメラ間の距離の逆数の重み付け

内容

1.PC環境

　実施したPC環境は以下のとおりです


CPU	Celeron N4100
メモリ	8GB LPDDR4

2.前準備

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

python（使用したのはVer.3.7 ）
　実行時のベースシステムです
（参考）https://qiita.com/ssbb/items/b55ca899e0d5ce6ce963
pip（使用したのはVer.21.2.4 ）
　他のツールをダウンロードする際に使うツールです
（python3系ではバージョン3.4以降であれば、pythonのインストールと共にpipもインストールされます。）
（参考）https://gammasoft.jp/python/python-library-install/
OpenCV（使用したのはVer.4.5.3.56 ）
　画像系処理するためのライブラリです
（参考）https://qiita.com/ideagear/items/3f0807b7bde05aa18240
numpy（使用したのVer.4.7.0.72）
　行列計算が得意な数値計算モジュールです
（参考）https://qiita.com/butako/items/15d7cb5aaef90b09ccd8

3.pyファイルの作成

組んだコードは次のとおりです。


# --- ライブラリのインポート ---
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.使い方

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

プロットウィンドウ上でのキー操作で、カメラの バンク角（[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