【画像処理】測定誤差が推定カメラパラメータに与える影響について【誤差伝播】

はじめに

　外部・内部パラメータの推定は様々なサイトでチェッカーボードを使用した方法が紹介されるのを見かけるが、日本語記事で誤差に関する考察が含まれるものは少ない。考察されていたとしても再投影誤差で確認するにとどまっている。再投影誤差はパラメータ誤差を直接的に表現しているわけではない。場合によっては推定パラメータ達にどの程度誤差が含まれているのか知りたくなると思う。
　誤差伝播に関する文献をいろいろ探していると以下の文献を見つけた。
www.jstage.jst.go.jp
　最小二乗法の解説がなされていた。さらに探すと以下の書籍のp.144で詳細に説明がなされていた。

Hartley, Richard, and Andrew Zisserman. Multiple view geometry in computer vision. Cambridge university press, 2003.

　本ページでは上記二つの文献の内容を自分なりに解釈して実装したプログラムを紹介する。

推定カメラパラメータと誤差伝播

　誤差伝播を検討するため、再投影誤差を最小化するパラメータを推定する最小二乗法を考える。
　まず測定値として、画像上に投影された特徴点位置を設定する。画像上での位置を表すので単位はpixel。そして測定誤差が乗るため分散を有する。この測定値は、特徴点一点につきx軸方向とy軸方向の2つの値を持つ。
　次に観測パラメータとして、以下のパラメータを設定する。

• ：x軸方向焦点距離[pixel]
• ：y軸方向焦点距離[pixel]
• ：x軸方向光学中心[pixel]
• ：y軸方向光学中心[pixel]
• ：せん断ひずみ[pixel]
• ：x軸方向カメラ位置[m]
• ：y軸方向カメラ位置[m]
• ：z軸方向カメラ位置[m]
• ：z軸方向回転角度[deg.]
• ：y軸方向回転角度[deg.]
• ：x軸方向回転角度[deg.]

　回転角度はZYXオイラー角であり、 である。
　以降、これら11個の観測パラメータはと表現する。
　次に観測モデルは以下のように設定する。

観測モデル

は測定位置ベクトルで、測定値である特徴点の三次元位置を表しており、以下のように設定する。

• ：x軸方向特徴点位置[m]
• ：y軸方向特徴点位置[m]
• ：z軸方向特徴点位置[m]

　はヤコビ行列であり、観測モデルをm個の観測パラメータでそれぞれ偏微分することで得られる。
　観測パラメータが多いとヤコビ行列は非常に長大になり手計算は現実的ではなくなる。そこでsympyのシンボリック計算で観測パラメータを変数としたヤコビ行列の式を得る。

import sympy as sp
import numpy as np

X = sp.Symbol('tx')
Y = sp.Symbol('ty')
Z = sp.Symbol('tz')

x = sp.Symbol('x')
y = sp.Symbol('y')

fx = sp.Symbol('fx')
fy = sp.Symbol('fy')
cx = sp.Symbol('cx')
cy = sp.Symbol('cy')
s = sp.Symbol('s')

tx = sp.Symbol('tx')
ty = sp.Symbol('ty')
tz = sp.Symbol('tz')

roll = sp.Symbol('roll')
pitch = sp.Symbol('pitch')
yaw = sp.Symbol('yaw')

#回転行列
Rx = np.array([[1,0,0],[0,sp.cos(roll),-sp.sin(roll)],[0,sp.sin(roll),sp.cos(roll)]])
Ry = np.array([[sp.cos(pitch),0,sp.sin(pitch)],[0,1,0],[-sp.sin(pitch),0,sp.cos(pitch)]])
Rz = np.array([[sp.cos(yaw),-sp.sin(yaw),0],[sp.sin(yaw),sp.cos(yaw),0],[0,0,1]])

#Z→Y→Xの順で回転
Rzyx = Rz@Ry@Rx

#内部パラメータ行列
K = np.array([[fx,s,cx],[0,fy,cy],[0,0,1]])

#外部パラメータ行列
RT = np.array([[Rzyx[0,0],Rzyx[0,1],Rzyx[0,2],tx],[Rzyx[1,0],Rzyx[1,1],Rzyx[1,2],ty],[Rzyx[2,0],Rzyx[2,1],Rzyx[2,2],tz]])

#特徴点の三次元位置
POS = np.array([X,Y,Z,1])

#投影点の計算
PPOS = K@RT@POS.T
xe = x - PPOS[0]/PPOS[2]
ye = y - PPOS[1]/PPOS[2]

#ヤコビ行列の計算
J = np.array([[sp.diff(xe,tx),sp.diff(xe,ty),sp.diff(xe,tz),sp.diff(xe,roll),sp.diff(xe,pitch),sp.diff(xe,yaw)],
              [sp.diff(ye,tx),sp.diff(ye,ty),sp.diff(ye,tz),sp.diff(ye,roll),sp.diff(ye,pitch),sp.diff(ye,yaw)]])

#ヤコビ行列の表示
print(J)

　これを表示してみると以下のようになる。長い。

[[-(-cx*sin(pitch) + fx*cos(pitch)*cos(yaw) + fx + s*sin(yaw)*cos(pitch))/(-tx*sin(pitch) + ty*sin(roll)*cos(pitch) + tz*cos(pitch)*cos(roll) + tz) - (cx*tz + fx*tx + s*ty + tx*(-cx*sin(pitch) + fx*cos(pitch)*cos(yaw) + s*sin(yaw)*cos(pitch)) + ty*(cx*sin(roll)*cos(pitch) + fx*(sin(pitch)*sin(roll)*cos(yaw) - sin(yaw)*cos(roll)) + s*(sin(pitch)*sin(roll)*sin(yaw) + cos(roll)*cos(yaw))) + tz*(cx*cos(pitch)*cos(roll) + fx*(sin(pitch)*cos(roll)*cos(yaw) + sin(roll)*sin(yaw)) + s*(sin(pitch)*sin(yaw)*cos(roll) - sin(roll)*cos(yaw))))*sin(pitch)/(-tx*sin(pitch) + ty*sin(roll)*cos(pitch) + tz*cos(pitch)*cos(roll) + tz)**2
  -(cx*sin(roll)*cos(pitch) + fx*(sin(pitch)*sin(roll)*cos(yaw) - sin(yaw)*cos(roll)) + s*(sin(pitch)*sin(roll)*sin(yaw) + cos(roll)*cos(yaw)) + s)/(-tx*sin(pitch) + ty*sin(roll)*cos(pitch) + tz*cos(pitch)*cos(roll) + tz) + (cx*tz + fx*tx + s*ty + tx*(-cx*sin(pitch) + fx*cos(pitch)*cos(yaw) + s*sin(yaw)*cos(pitch)) + ty*(cx*sin(roll)*cos(pitch) + fx*(sin(pitch)*sin(roll)*cos(yaw) - sin(yaw)*cos(roll)) + s*(sin(pitch)*sin(roll)*sin(yaw) + cos(roll)*cos(yaw))) + tz*(cx*cos(pitch)*cos(roll) + fx*(sin(pitch)*cos(roll)*cos(yaw) + sin(roll)*sin(yaw)) + s*(sin(pitch)*sin(yaw)*cos(roll) - sin(roll)*cos(yaw))))*sin(roll)*cos(pitch)/(-tx*sin(pitch) + ty*sin(roll)*cos(pitch) + tz*cos(pitch)*cos(roll) + tz)**2
以下略

　次に観測パラメータの誤差を計算していく。以下のような計算によって求まる。

共分散行列

　は誤差行列で、n個の測定値に対応する分散共分散行列である。これが観測パラメータに誤差伝播する。分散が既知でないとき、平均値が真値であると仮定して有限個数の測定値から分散を計算することもできる。

実際にやってみた

　以上の内容を使用して、以下ページでカメラパラメータとその誤差範囲を計算した。
jonajiro.hatenablog.com
　

【画像処理】カメラ外部・内部パラメータの推定とその誤差について

はじめに

　最小二乗法は様々なパラメータフィッティングのファーストチョイスとして使われがちではある。カメラキャリブレーションでも同様で、内部パラメータなどDLT法で推定されることが多く、この手法の中で線形最小二乗法が用いられる場合もある。また、バンドル調整などではガウスニュートン法などの非線形最小二乗法が用いられることもある。
　直接計測された計測値の誤差を検討するときには単純に計測値の分散等を計算すればいい。一方で最小二乗法などを使用して間接的に計測された数値(推定されたパラメータ)は、直接計測した計測値の誤差伝播を考慮する必要がある。これらをpythonをつかって計算した。

したごしらえ（点群と画像生成）

　点群を生成し、これを既知の内部パラメータを持つカメラで画像平面上に投影したときのピクセル位置を計算する。gainは-1.0~1.0までの乱数に対するゲインで、この乱数をピクセル位置に加算することで計測誤差を再現した。とりあえず、以下プログラムでは画像の特徴点位置に2pixelの誤差を含ませてみた。投影画像gen_img.bmpと共に点群とそれに対応するpixel位置を記録したobjimgpoints.csvが出力される。

import numpy as np
import random
import cv2

#カメラ内部パラメータ
fx = 1500.0
fy = 1500.0
cx = 1920/2
cy = 1080/2

#カメラ姿勢(ZYXオイラー角)
roll = 30.0*np.pi/180.0
pitch = 7.0*np.pi/180.0
yaw = 0.0*np.pi/180.0

#カメラ位置
tx = 0.0
ty = 0.0
tz = 25.0

#ターゲット姿勢(ZYXオイラー角)
roll_t = 10.0*np.pi/180.0
pitch_t = 40.0*np.pi/180.0
yaw_t = 0.0*np.pi/180.0

w_num = 10
h_num = 8
len_point = h_num*w_num
len_point = len_point + h_num*w_num
len_point = len_point + h_num*h_num
cnt = 0
np_objpoints = np.zeros([len_point,3])
for i in range(w_num):
    for j in range(h_num):
        np_objpoints[cnt,0] = i - 0.0 - (w_num-1)/2
        np_objpoints[cnt,1] = j - 0.0 - (h_num-1)/2
        np_objpoints[cnt,2] = -(h_num-1)/2 - 0.0
        cnt = cnt + 1
for i in range(w_num):
    for j in range(h_num):
        np_objpoints[cnt,0] = i - 0.0 - (w_num-1)/2
        np_objpoints[cnt,1] = -(h_num-1)/2 - 0.0
        np_objpoints[cnt,2] = j - 0.0 - (h_num-1)/2
        cnt = cnt + 1
for i in range(h_num):
    for j in range(h_num):
        np_objpoints[cnt,0] = (w_num-1)/2 + 0.0
        np_objpoints[cnt,1] = i - 0.0 - (h_num-1)/2
        np_objpoints[cnt,2] = j - 0.0 - (h_num-1)/2
        cnt = cnt + 1
Rx = np.array([[1,0,0],[0,np.cos(roll_t),-np.sin(roll_t)],[0,np.sin(roll_t),np.cos(roll_t)]])
Ry = np.array([[np.cos(pitch_t),0,np.sin(pitch_t)],[0,1,0],[-np.sin(pitch_t),0,np.cos(pitch_t)]])
Rz = np.array([[np.cos(yaw_t),-np.sin(yaw_t),0],[np.sin(yaw_t),np.cos(yaw_t),0],[0,0,1]])
Rzyx = Rx@Ry@Rz
for i in range(len(np_objpoints)):
    np_objpoints[i,:] = Rzyx@np_objpoints[i,:]

np_imgpoints = np.zeros([len_point,2])
np_imgpoints_true = np.zeros([len_point,2])
np_mtx = np.zeros([3,3])
np_mtx[0,0] = fx
np_mtx[0,1] = 0
np_mtx[0,2] = cx
np_mtx[1,1] = fy
np_mtx[1,2] = cy
np_mtx[2,2] = 1

Rx = np.array([[1,0,0],[0,np.cos(roll),-np.sin(roll)],[0,np.sin(roll),np.cos(roll)]])
Ry = np.array([[np.cos(pitch),0,np.sin(pitch)],[0,1,0],[-np.sin(pitch),0,np.cos(pitch)]])
Rz = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])
Rzyx = Rz@Ry@Rx
RT = np.array([[Rzyx[0,0],Rzyx[0,1],Rzyx[0,2],tx],[Rzyx[1,0],Rzyx[1,1],Rzyx[1,2],ty],[Rzyx[2,0],Rzyx[2,1],Rzyx[2,2],tz]])

for i in range(len(np_objpoints)):
    gain = 0.0
    XYZ = np.array([np_objpoints[i,0] + gain*(random.random()*2-1),np_objpoints[i,1] + gain*(random.random()*2-1),np_objpoints[i,2] + gain*(random.random()*2-1),1])
    uv = np_mtx@RT@XYZ.T
    gain = 2.0
    np_imgpoints[i,0] = uv[0]/uv[2] + gain*(random.random()*2-1)+0.0
    np_imgpoints[i,1] = uv[1]/uv[2] + gain*(random.random()*2-1)+0.0

    np_imgpoints_true[i,0] = uv[0]/uv[2]
    np_imgpoints_true[i,1] = uv[1]/uv[2]

height = int(cy*2)
width = int(cx*2)
img = np.zeros((height, width, 3))
for i in range(len(np_imgpoints)):
    cv2.circle(img,center=(int(np_imgpoints[i,0]), int(np_imgpoints[i,1])),radius=5,color=(255, 255, 255),thickness=-1,lineType=cv2.LINE_4,shift=0)

cv2.imwrite('gen_img.bmp',img)
np.savetxt('objimgpoints.csv', np.concatenate((np_objpoints, np_imgpoints), axis=1),delimiter=",")

　画像はこんな感じ。

生成した点群

計算結果

----DLT法による外部・内部パラメータ推定結果----
----並進位置----
X    :        0.278±        0.079 m
Y    :        0.102±        0.081 m
Z    :       25.229±        0.102 m
----姿勢----
ROLL :       29.771±        0.172 deg.
PITCH:        7.639±        0.165 deg.
YAW  :       -0.031±        0.035 deg.
----内部パラメータ----
fx   :     1515.146±        6.544 pixel
fy   :     1515.383±        6.516 pixel
cx   :      943.035±        4.846 pixel
cy   :      533.659±        4.971 pixel
s    :        0.284±        0.976 pixel
----ガウスニュートン法で調整した内部パラメータ推定結果----
----並進位置----
X    :        0.092±        0.075 m
Y    :       -0.106±        0.077 m
Z    :       24.969±        0.097 m
----姿勢----
ROLL :       30.225±        0.166 deg.
PITCH:        7.221±        0.159 deg.
YAW  :        0.024±        0.034 deg.
----内部パラメータ----
fx   :     1498.502±        6.259 pixel
fy   :     1499.060±        6.231 pixel
cx   :      954.436±        4.605 pixel
cy   :      546.377±        4.754 pixel
s    :       -0.276±        0.941 pixel

プログラム

　ヤコビ行列計算式の長さがえぐいことになっているが単純に偏微分してるだけ。sympyなどのシンボリック計算ツールを使えばコピペするだけでよい。昔(10年くらい前)は便利ツールの情報が少なくて渋々手計算したが便利な世の中になったものである。

#
# 内部パラメータ推定
#

import os
import numpy as np

DIST_PATH = "dist.csv"
BASE_PATH = "objimgpoints.csv"

# メイン関数
def main():
    calcCamera() # カメラの歪みを計算

# カメラの内部パラメータを計算する関数
def calcCamera():
    #作業領域変更
    os.chdir(os.path.dirname(os.path.abspath(__file__)))

    base_cam = loadMFile(BASE_PATH)#三次元位置と画像特徴点位置のペア情報読み込み
    cam0 = base_cam[:,3:5]#画像特徴点位置の切り出し
    base = base_cam[:,0:3]#三次元位置の切り出し
    # dist = loadDistortionFile(DIST_PATH)#レンズ歪がある場合は歪係数ファイル読み込み
    # cam0 = calc_distortion(loadMFile(cam0_PATH),dist)#歪係数ファイルを画像特徴点位置に適用

    M = np.zeros([len(cam0)*3,12+len(cam0)])#特異値分解分解される側行列
    v = np.zeros([12+len(cam0),1])#特異値分解パラメータ行列
    P = np.zeros([3,4])#透視投影行列
    for i in range(len(cam0)):
        M[i*3+0,0] = base[i,0]
        M[i*3+0,1] = base[i,1]
        M[i*3+0,2] = base[i,2]
        M[i*3+0,3] = 1
        M[i*3+1,4+0] = base[i,0]
        M[i*3+1,4+1] = base[i,1]
        M[i*3+1,4+2] = base[i,2]
        M[i*3+1,4+3] = 1
        M[i*3+2,8+0] = base[i,0]
        M[i*3+2,8+1] = base[i,1]
        M[i*3+2,8+2] = base[i,2]
        M[i*3+2,8+3] = 1
        M[i*3+0,12+i] = -cam0[i,0]
        M[i*3+1,12+i] = -cam0[i,1]
        M[i*3+2,12+i] = -1

    U,S,Vt = np.linalg.svd(M,full_matrices=True)#特異値分解実行
    v = Vt[-1]#最小特異値に対応する特異値ベクトル
    ramda = np.mean(v[12:-1])#定数ラムダの平均
    #透視投影行列に代入
    P[0,0] = v[0]/ramda
    P[0,1] = v[1]/ramda
    P[0,2] = v[2]/ramda
    P[0,3] = v[3]/ramda
    P[1,0] = v[4]/ramda
    P[1,1] = v[5]/ramda
    P[1,2] = v[6]/ramda
    P[1,3] = v[7]/ramda
    P[2,0] = v[8]/ramda
    P[2,1] = v[9]/ramda
    P[2,2] = v[10]/ramda
    P[2,3] = v[11]/ramda
    
    #RQ分解
    A = P[:,0:3]
    B = P[:,3]
    R = np.zeros([3,3])#回転行列
    t = np.zeros(3)#並進ベクトル
    R[2,:] = A[2,:]/np.linalg.norm(A[2,:])
    f = np.linalg.norm(A[2,:])
    e = A[1,:].T@R[2,:]
    d = np.linalg.norm(A[1,:]-e*R[2,:])
    R[1,:] = (A[1,:]-e*R[2,:])/np.linalg.norm(A[1,:]-e*R[2,:])
    c = A[0,:].T@R[2,:]
    b = A[0,:].T@R[1,:]
    a = np.linalg.norm(A[0,:]-b*R[1,:]-c*R[2,:])
    R[0,:] = (A[0,:]-b*R[1,:]-c*R[2,:])/np.linalg.norm(A[0,:]-b*R[1,:]-c*R[2,:])

    mtx = np.zeros([3,3])#内部パラメータ
    mtx[0,0] = a#fx
    mtx[0,1] = b#s
    mtx[0,2] = c#cx
    mtx[1,1] = d#fy
    mtx[1,2] = e#cy
    mtx[2,2] = f
    mtx = mtx/mtx[2,2]

    t[2] = B[2]/f
    t[1] = (B[1]-e*t[2])/d
    t[0] = (B[0]-b*t[1]-c*t[2])/a

    #カメラ姿勢角(ZYXオイラー表現)
    rpy = rot2rpy(R)
    yaw = rpy[2]
    pitch = rpy[1]
    roll = rpy[0]

    #再投影誤差を最小化する目的関数周りで内部パラメータ分散を計算する
    #初期値入力
    new_x = np.zeros(11)
    new_x[0] = t[0]
    new_x[1] = t[1]
    new_x[2] = t[2]
    new_x[3] = rpy[0]
    new_x[4] = rpy[1]
    new_x[5] = rpy[2]
    new_x[6] = mtx[0,0]
    new_x[7] = mtx[1,1]
    new_x[8] = mtx[0,2]
    new_x[9] = mtx[1,2]
    new_x[10] = mtx[0,1]
    J = []
    E = []
    Ex = []
    Ey = []
    for i in range(len(base)):
        #観測値入力
        X = base[i,0]
        Y = base[i,1]
        Z = base[i,2]

        #計測値入力
        x = cam0[i,0]
        y = cam0[i,1]

        #推定内部パラメータ入力
        fx = new_x[6]
        fy = new_x[7]
        cx = new_x[8]
        cy = new_x[9]
        s = new_x[10]

        tx = new_x[0]
        ty = new_x[1]
        tz = new_x[2]

        roll = new_x[3]
        pitch = new_x[4]
        yaw = new_x[5]

        #ヤコビ行列計算
        j00 = -fx/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
        j01 = -s/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
        j02 = -cx/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) + (X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
        j03 = -(Y*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + Z*(-cx*np.sin(roll)*np.cos(pitch) + fx*(-np.sin(pitch)*np.sin(roll)*np.cos(yaw) + np.sin(yaw)*np.cos(roll)) + s*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (-Y*np.cos(pitch)*np.cos(roll) + Z*np.sin(roll)*np.cos(pitch))*(X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
        j04 = -(X*(-cx*np.cos(pitch) - fx*np.sin(pitch)*np.cos(yaw) - s*np.sin(pitch)*np.sin(yaw)) + Y*(-cx*np.sin(pitch)*np.sin(roll) + fx*np.sin(roll)*np.cos(pitch)*np.cos(yaw) + s*np.sin(roll)*np.sin(yaw)*np.cos(pitch)) + Z*(-cx*np.sin(pitch)*np.cos(roll) + fx*np.cos(pitch)*np.cos(roll)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)*np.cos(roll)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (X*np.cos(pitch) + Y*np.sin(pitch)*np.sin(roll) + Z*np.sin(pitch)*np.cos(roll))*(X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
        j05 = -(X*(-fx*np.sin(yaw)*np.cos(pitch) + s*np.cos(pitch)*np.cos(yaw)) + Y*(fx*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw)) + s*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll))) + Z*(fx*(-np.sin(pitch)*np.sin(yaw)*np.cos(roll) + np.sin(roll)*np.cos(yaw)) + s*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
        j06 = -(X*np.cos(pitch)*np.cos(yaw) + Y*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + Z*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + tx)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
        j07 = 0
        j08 = -1
        j09 = 0
        j0a = -(X*np.sin(yaw)*np.cos(pitch) + Y*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw)) + Z*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw)) + ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
        
        j10 = 0
        j11 = -fy/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
        j12 = -cy/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) + (X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
        j13 = -(Y*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + Z*(-cy*np.sin(roll)*np.cos(pitch) + fy*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (-Y*np.cos(pitch)*np.cos(roll) + Z*np.sin(roll)*np.cos(pitch))*(X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
        j14 = -(X*(-cy*np.cos(pitch) - fy*np.sin(pitch)*np.sin(yaw)) + Y*(-cy*np.sin(pitch)*np.sin(roll) + fy*np.sin(roll)*np.sin(yaw)*np.cos(pitch)) + Z*(-cy*np.sin(pitch)*np.cos(roll) + fy*np.sin(yaw)*np.cos(pitch)*np.cos(roll)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (X*np.cos(pitch) + Y*np.sin(pitch)*np.sin(roll) + Z*np.sin(pitch)*np.cos(roll))*(X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
        j15 = -(X*fy*np.cos(pitch)*np.cos(yaw) + Y*fy*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + Z*fy*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
        j16 = 0
        j17 = -(X*np.sin(yaw)*np.cos(pitch) + Y*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw)) + Z*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw)) + ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
        j18 = 0
        j19 = -1
        j1a = 0

        J.append([j00,j01,j02,j03,j04,j05,j06,j07,j08,j09,j0a])
        J.append([j10,j11,j12,j13,j14,j15,j16,j17,j18,j19,j1a])

        #再投影誤差算出
        Rzyx = rpy2rot(np.array([roll,pitch,yaw]))
        K = np.array([[fx,s,cx],[0,fy,cy],[0,0,1]])
        RT = np.array([[Rzyx[0,0],Rzyx[0,1],Rzyx[0,2],tx],[Rzyx[1,0],Rzyx[1,1],Rzyx[1,2],ty],[Rzyx[2,0],Rzyx[2,1],Rzyx[2,2],tz]])
        XYZ = np.array([X,Y,Z,1])
        PXYZ = K@RT@XYZ.T
        ex = x - PXYZ[0]/PXYZ[2]
        ey = y - PXYZ[1]/PXYZ[2]
        Ex.append(ex)
        Ey.append(ey)

    J = np.array(J)
    Ex = np.array(Ex)
    Ey = np.array(Ey)

    #再投影誤差から不偏分散算出
    xe = np.zeros(2)
    for i in range(len(Ex)):
        b = np.zeros(2)
        b[0] = Ex[i]**2
        b[1] = Ey[i]**2
        xe = xe + b
    xe = xe/(len(Ex) - 11)#11個の未知数があるので計測数から11自由度分減算する

    Ssig = np.zeros([len(Ex)*2,len(Ex)*2])#誤差行列
    for i in range(len(Ex)):
        Ssig[i*2,i*2] = xe[0]
        Ssig[i*2+1,i*2+1] = xe[1]

    errM = np.linalg.inv(J.T@np.linalg.inv(Ssig)@J)#共分散行列
    err = np.zeros(len(errM))
    for i in range(len(errM)):
        err[i] = np.sqrt(errM[i,i])#対角成分の平方根が標準偏差
    print("----DLT法による外部・内部パラメータ推定結果----")
    print("----並進位置----")
    print("X    :{:13.3f}±{:13.3f} m".format(tx,err[0]))
    print("Y    :{:13.3f}±{:13.3f} m".format(ty,err[1]))
    print("Z    :{:13.3f}±{:13.3f} m".format(tz,err[2]))
    print("----姿勢----")
    print("ROLL :{:13.3f}±{:13.3f} deg.".format(roll*180/np.pi,err[3]*180/np.pi))
    print("PITCH:{:13.3f}±{:13.3f} deg.".format(pitch*180/np.pi,err[4]*180/np.pi))
    print("YAW  :{:13.3f}±{:13.3f} deg.".format(yaw*180/np.pi,err[5]*180/np.pi))
    print("----内部パラメータ----")
    print("fx   :{:13.3f}±{:13.3f} pixel".format(fx,err[6]))
    print("fy   :{:13.3f}±{:13.3f} pixel".format(fy,err[7]))
    print("cx   :{:13.3f}±{:13.3f} pixel".format(cx,err[8]))
    print("cy   :{:13.3f}±{:13.3f} pixel".format(cy,err[9]))
    print("s    :{:13.3f}±{:13.3f} pixel".format(s,err[10]))


    #DLT法で推定した外部・内部パラメータを初期値としてガウスニュートン法で調整する
    #初期値入力
    new_x[0] = t[0]
    new_x[1] = t[1]
    new_x[2] = t[2]
    new_x[3] = rpy[0]
    new_x[4] = rpy[1]
    new_x[5] = rpy[2]
    new_x[6] = mtx[0,0]
    new_x[7] = mtx[1,1]
    new_x[8] = mtx[0,2]
    new_x[9] = mtx[1,2]
    new_x[10] = mtx[0,1]
    
    #最大繰り返し回数
    ita_num = 100
    for j in range(ita_num):
        J = []#ヤコビ行列初期化
        E = []#再投影誤差初期化
        Ex = []#x軸方向のみ再投影誤差初期化
        Ey = []#y軸方向のみ再投影誤差初期化
        for i in range(len(base)):
            #観測値入力
            X = base[i,0]
            Y = base[i,1]
            Z = base[i,2]

            #計測値入力
            x = cam0[i,0]
            y = cam0[i,1]

            #内部パラメータ入力
            fx = new_x[6]
            fy = new_x[7]
            cx = new_x[8]
            cy = new_x[9]
            s = new_x[10]

            #外部パラメータ入力
            tx = new_x[0]
            ty = new_x[1]
            tz = new_x[2]

            roll = new_x[3]
            pitch = new_x[4]
            yaw = new_x[5]

            #ヤコビ行列算出
            j00 = -fx/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
            j01 = -s/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
            j02 = -cx/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) + (X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
            j03 = -(Y*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + Z*(-cx*np.sin(roll)*np.cos(pitch) + fx*(-np.sin(pitch)*np.sin(roll)*np.cos(yaw) + np.sin(yaw)*np.cos(roll)) + s*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (-Y*np.cos(pitch)*np.cos(roll) + Z*np.sin(roll)*np.cos(pitch))*(X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
            j04 = -(X*(-cx*np.cos(pitch) - fx*np.sin(pitch)*np.cos(yaw) - s*np.sin(pitch)*np.sin(yaw)) + Y*(-cx*np.sin(pitch)*np.sin(roll) + fx*np.sin(roll)*np.cos(pitch)*np.cos(yaw) + s*np.sin(roll)*np.sin(yaw)*np.cos(pitch)) + Z*(-cx*np.sin(pitch)*np.cos(roll) + fx*np.cos(pitch)*np.cos(roll)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)*np.cos(roll)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (X*np.cos(pitch) + Y*np.sin(pitch)*np.sin(roll) + Z*np.sin(pitch)*np.cos(roll))*(X*(-cx*np.sin(pitch) + fx*np.cos(pitch)*np.cos(yaw) + s*np.sin(yaw)*np.cos(pitch)) + Y*(cx*np.sin(roll)*np.cos(pitch) + fx*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + s*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cx*np.cos(pitch)*np.cos(roll) + fx*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + s*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cx*tz + fx*tx + s*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
            j05 = -(X*(-fx*np.sin(yaw)*np.cos(pitch) + s*np.cos(pitch)*np.cos(yaw)) + Y*(fx*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw)) + s*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll))) + Z*(fx*(-np.sin(pitch)*np.sin(yaw)*np.cos(roll) + np.sin(roll)*np.cos(yaw)) + s*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
            j06 = -(X*np.cos(pitch)*np.cos(yaw) + Y*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + Z*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)) + tx)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
            j07 = 0
            j08 = -1
            j09 = 0
            j0a = -(X*np.sin(yaw)*np.cos(pitch) + Y*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw)) + Z*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw)) + ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
            
            j10 = 0
            j11 = -fy/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
            j12 = -cy/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) + (X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
            j13 = -(Y*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + Z*(-cy*np.sin(roll)*np.cos(pitch) + fy*(-np.sin(pitch)*np.sin(roll)*np.sin(yaw) - np.cos(roll)*np.cos(yaw))))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (-Y*np.cos(pitch)*np.cos(roll) + Z*np.sin(roll)*np.cos(pitch))*(X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
            j14 = -(X*(-cy*np.cos(pitch) - fy*np.sin(pitch)*np.sin(yaw)) + Y*(-cy*np.sin(pitch)*np.sin(roll) + fy*np.sin(roll)*np.sin(yaw)*np.cos(pitch)) + Z*(-cy*np.sin(pitch)*np.cos(roll) + fy*np.sin(yaw)*np.cos(pitch)*np.cos(roll)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz) - (X*np.cos(pitch) + Y*np.sin(pitch)*np.sin(roll) + Z*np.sin(pitch)*np.cos(roll))*(X*(-cy*np.sin(pitch) + fy*np.sin(yaw)*np.cos(pitch)) + Y*(cy*np.sin(roll)*np.cos(pitch) + fy*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw))) + Z*(cy*np.cos(pitch)*np.cos(roll) + fy*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw))) + cy*tz + fy*ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)**2
            j15 = -(X*fy*np.cos(pitch)*np.cos(yaw) + Y*fy*(np.sin(pitch)*np.sin(roll)*np.cos(yaw) - np.sin(yaw)*np.cos(roll)) + Z*fy*(np.sin(pitch)*np.cos(roll)*np.cos(yaw) + np.sin(roll)*np.sin(yaw)))/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
            j16 = 0
            j17 = -(X*np.sin(yaw)*np.cos(pitch) + Y*(np.sin(pitch)*np.sin(roll)*np.sin(yaw) + np.cos(roll)*np.cos(yaw)) + Z*(np.sin(pitch)*np.sin(yaw)*np.cos(roll) - np.sin(roll)*np.cos(yaw)) + ty)/(-X*np.sin(pitch) + Y*np.sin(roll)*np.cos(pitch) + Z*np.cos(pitch)*np.cos(roll) + tz)
            j18 = 0
            j19 = -1
            j1a = 0

            J.append([j00,j01,j02,j03,j04,j05,j06,j07,j08,j09,j0a])
            J.append([j10,j11,j12,j13,j14,j15,j16,j17,j18,j19,j1a])

            #再投影誤差算出
            Rzyx = rpy2rot(np.array([roll,pitch,yaw]))
            K = np.array([[fx,s,cx],[0,fy,cy],[0,0,1]])
            RT = np.array([[Rzyx[0,0],Rzyx[0,1],Rzyx[0,2],tx],[Rzyx[1,0],Rzyx[1,1],Rzyx[1,2],ty],[Rzyx[2,0],Rzyx[2,1],Rzyx[2,2],tz]])
            XYZ = np.array([X,Y,Z,1])
            PXYZ = K@RT@XYZ.T
            ex = x - PXYZ[0]/PXYZ[2]
            ey = y - PXYZ[1]/PXYZ[2]
            E.append(ex)
            E.append(ey) 
            Ex.append(ex)
            Ey.append(ey)
        J = np.array(J)
        E = np.array(E)
        Ex = np.array(Ex)
        Ey = np.array(Ey)

        #更新誤差算出
        delta = (np.linalg.inv(J.T@J)@J.T)@E.T
        #各測定値との再投影誤差最大値が規定値を下回ったら繰り返し計算終了
        if np.abs(np.max(E)) < 0.001:
            break
        #パラメータ更新
        new_x = new_x - delta

    #再投影誤差から不偏分散算出
    xe = np.zeros(2)
    for i in range(len(Ex)):
        b = np.zeros(2)
        b[0] = Ex[i]**2
        b[1] = Ey[i]**2
        xe = xe + b
    xe = xe/(len(Ex) - 11)#11個の未知数があるので計測数から11自由度分減算する

    Ssig = np.zeros([len(Ex)*2,len(Ex)*2])#誤差行列
    for i in range(len(Ex)):
        Ssig[i*2,i*2] = xe[0]
        Ssig[i*2+1,i*2+1] = xe[1]

    errM = np.linalg.inv(J.T@np.linalg.inv(Ssig)@J)#共分散行列
    err = np.zeros(len(errM))
    for i in range(len(errM)):
        err[i] = np.sqrt(errM[i,i])#対角成分の平方根が標準偏差
    print("----ガウスニュートン法で調整した内部パラメータ推定結果----")
    print("----並進位置----")
    print("X    :{:13.3f}±{:13.3f} m".format(tx,err[0]))
    print("Y    :{:13.3f}±{:13.3f} m".format(ty,err[1]))
    print("Z    :{:13.3f}±{:13.3f} m".format(tz,err[2]))
    print("----姿勢----")
    print("ROLL :{:13.3f}±{:13.3f} deg.".format(roll*180/np.pi,err[3]*180/np.pi))
    print("PITCH:{:13.3f}±{:13.3f} deg.".format(pitch*180/np.pi,err[4]*180/np.pi))
    print("YAW  :{:13.3f}±{:13.3f} deg.".format(yaw*180/np.pi,err[5]*180/np.pi))
    print("----内部パラメータ----")
    print("fx   :{:13.3f}±{:13.3f} pixel".format(fx,err[6]))
    print("fy   :{:13.3f}±{:13.3f} pixel".format(fy,err[7]))
    print("cx   :{:13.3f}±{:13.3f} pixel".format(cx,err[8]))
    print("cy   :{:13.3f}±{:13.3f} pixel".format(cy,err[9]))
    print("s    :{:13.3f}±{:13.3f} pixel".format(s,err[10]))

def rot2rpy(R):
    yaw = 0
    pitch = np.arcsin(-R[2,0])
    roll = 0
    if np.abs(R[2,0]) != 1:
        yaw = np.arctan2(R[1,0],R[0,0])
        roll = np.arctan2(R[2,1],R[2,2])
    if R[2,0] == 1:
        roll = 0
        yaw = np.arctan2(R[0,1],R[1,1])
    if R[2,0] == -1:
        roll = 0
        yaw = np.arctan2(R[0,1],R[1,1])
        
    if roll > np.pi:
        roll = roll - 2*np.pi
    if roll < -np.pi:
        roll = roll + 2*np.pi

    if pitch > np.pi:
        pitch = pitch - 2*np.pi
    if pitch < -np.pi:
        pitch = pitch + 2*np.pi

    if yaw > np.pi:
        yaw = yaw - 2*np.pi
    if yaw < -np.pi:
        yaw = yaw + 2*np.pi
    return np.array([roll,pitch,yaw])

def rpy2rot(rpy):
    yaw = rpy[2]
    pitch = rpy[1]
    roll = rpy[0]
    Rx = np.array([[1,0,0],[0,np.cos(roll),-np.sin(roll)],[0,np.sin(roll),np.cos(roll)]])
    Ry = np.array([[np.cos(pitch),0,np.sin(pitch)],[0,1,0],[-np.sin(pitch),0,np.cos(pitch)]])
    Rz = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])
    return Rz@Ry@Rx

def calc_distortion(point_data,params):
    cx = params[0]
    cy = params[1]
    k1 = params[2]
    k2 = params[3]
    k3 = params[4]
    k4 = params[5]
    point_data_ans = point_data.copy()
    for i in range(len(point_data)):
        x = point_data[i,0] - cx
        y = point_data[i,1] - cy
        r2 = x**2 + y**2
        f = (1+k1*r2+k2*r2**2)/(1+k3*r2+k4*r2**2)
        point_data_ans[i,0] = x*f + cx
        point_data_ans[i,1] = y*f + cy
    return point_data_ans

# 歪曲収差係数CSVファイルを読み込む関数
def loadDistortionFile(dist_path):
    try:
        dist = np.loadtxt(dist_path, delimiter=',')
    except Exception as e:
        raise e
    return dist

# 測量CSVファイルを読み込む関数
def loadMFile(m_path):
    try:
        r = np.loadtxt(m_path, delimiter=',')
    except Exception as e:
        raise e
    return r

if __name__ == '__main__':
    main()

【画像処理】DLT法を使用した内部・外部パラメータの推定

はじめに

　カメラの内部パラメータ、外部パラメータ知りたいよね。DLT法で求まるんだわ。

下ごしらえ（点群と画像生成）

　点群を生成し、これを既知の内部パラメータを持つカメラで画像平面上に投影したときのピクセル位置を計算する。gainは-1.0~1.0までの乱数に対するゲインで、この乱数をピクセル位置に加算することで計測誤差を再現した。投影画像gen_img.bmpと共に点群とそれに対応するpixel位置を記録したobjimgpoints.csvが出力される。
画像はこんな感じ。

投影画像

csvは以下のような感じ。

三次元位置X[m]	三次元位置Y[m]	三次元位置Z[m]	ピクセル位置X[pixel]	ピクセル位置Y[pixel]
-4.5	-3.0	14.1	629.2	142.1
...	...	...	...	...

import numpy as np
import random
import cv2

#カメラ内部パラメータ
fx = 1200.0
fy = 1200.0
cx = 1920/2
cy = 1080/2

#カメラ姿勢(ZYXオイラー角)
roll = 5.0*np.pi/180.0
pitch = -5.0*np.pi/180.0
yaw = 2.0*np.pi/180.0

#カメラ位置
tx = 1.0
ty = -1.0
tz = 3.0

#pixel位置ノイズゲイン[-]
gain = 3.0

#計測点の個数
w_n = 10#横
h_n = 7#縦
np_objpoints = np.zeros([w_n*h_n,3])
cnt = 0 
for i in range(w_n):
    for j in range(h_n):
        gain = 3
        np_objpoints[cnt,0] = i - (w_n-1)/2
        np_objpoints[cnt,1] = j - (h_n-1)/2
        np_objpoints[cnt,2] = gain*(random.random()*2-1) + 15
        cnt = cnt + 1

np_mtx = np.zeros([3,3])
np_mtx[0,0] = fx
np_mtx[0,1] = 0
np_mtx[0,2] = cx
np_mtx[1,1] = fy
np_mtx[1,2] = cy
np_mtx[2,2] = 1

Rx = np.array([[1,0,0],[0,np.cos(roll),-np.sin(roll)],[0,np.sin(roll),np.cos(roll)]])
Ry = np.array([[np.cos(pitch),0,np.sin(pitch)],[0,1,0],[-np.sin(pitch),0,np.cos(pitch)]])
Rz = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])
Rzyx = Rz@Ry@Rx
RT = np.array([[Rzyx[0,0],Rzyx[0,1],Rzyx[0,2],tx],[Rzyx[1,0],Rzyx[1,1],Rzyx[1,2],ty],[Rzyx[2,0],Rzyx[2,1],Rzyx[2,2],tz]])

np_imgpoints = np.zeros([w_n*h_n,2])
np_imgpoints_true = np.zeros([w_n*h_n,2])
for i in range(len(np_objpoints)):
    XYZ = np.array([np_objpoints[i,0],np_objpoints[i,1],np_objpoints[i,2],1])
    uv = np_mtx@RT@XYZ.T
    np_imgpoints[i,0] = uv[0]/uv[2] + gain*(random.random()*2-1)
    np_imgpoints[i,1] = uv[1]/uv[2] + gain*(random.random()*2-1)
    np_imgpoints_true[i,0] = uv[0]/uv[2]
    np_imgpoints_true[i,1] = uv[1]/uv[2]

height = int(cy*2)
width = int(cx*2)
img = np.zeros((height, width, 3))
for i in range(len(np_imgpoints)):
    cv2.circle(img,center=(int(np_imgpoints_true[i,0]), int(np_imgpoints_true[i,1])),radius=5,color=(0, 255, 0),thickness=-1,lineType=cv2.LINE_4,shift=0)
 
cv2.imwrite('gen_img.bmp',img)
np.savetxt('objimgpoints.csv', np.concatenate((np_objpoints, np_imgpoints), axis=1),delimiter=",")

こんな感じ。

DLT法本体

　生成したobjimgpoints.csvを読み込んで内部パラメータ、外部パラメータを推定する。設定した数値と近い値になるはず。

import numpy as np
import cv2

#ファイル入力
filename = "objimgpoints.csv"
csv_input = np.loadtxt(filename,delimiter = ",",encoding = "shift-JIS",dtype = "unicode_",skiprows = 0)
imgpos = np.zeros([len(csv_input),2])#ピクセル位置[pixel](x,y)
objpos = np.zeros([len(csv_input),3])#三次元位置[m](x,y,z)
for i in range(len(csv_input)):
    objpos[i,:] = np.array([float(csv_input[i,0]),float(csv_input[i,1]),float(csv_input[i,2])])
    imgpos[i,:] = np.array([float(csv_input[i,3]),float(csv_input[i,4])])

#同次連立方程式Mv=0
M = np.zeros([len(imgpos)*3,12+len(imgpos)])
for i in range(len(imgpos)):
    M[i*3+0,0] = objpos[i,0]
    M[i*3+0,1] = objpos[i,1]
    M[i*3+0,2] = objpos[i,2]
    M[i*3+0,3] = 1
    M[i*3+1,4+0] = objpos[i,0]
    M[i*3+1,4+1] = objpos[i,1]
    M[i*3+1,4+2] = objpos[i,2]
    M[i*3+1,4+3] = 1
    M[i*3+2,8+0] = objpos[i,0]
    M[i*3+2,8+1] = objpos[i,1]
    M[i*3+2,8+2] = objpos[i,2]
    M[i*3+2,8+3] = 1
    M[i*3+0,12+i] = -imgpos[i,0]
    M[i*3+1,12+i] = -imgpos[i,1]
    M[i*3+2,12+i] = -1

#特異値分解
U,S,Vt = np.linalg.svd(M,full_matrices=True)
#最小特異値
v = Vt[-1]

#スケール係数の平均
ramda = np.mean(v[12:-1])

#透視投影変換行列
P = np.zeros([3,4])
P[0,0] = v[0]/ramda
P[0,1] = v[1]/ramda
P[0,2] = v[2]/ramda
P[0,3] = v[3]/ramda
P[1,0] = v[4]/ramda
P[1,1] = v[5]/ramda
P[1,2] = v[6]/ramda
P[1,3] = v[7]/ramda
P[2,0] = v[8]/ramda
P[2,1] = v[9]/ramda
P[2,2] = v[10]/ramda
P[2,3] = v[11]/ramda

A = P[:,0:3]#[KR]
B = P[:,3]#[Kt]
R = np.zeros([3,3])#回転行列
t = np.zeros(3)#併進位置

#RQ分解
R[2,:] = A[2,:]/np.linalg.norm(A[2,:])
f = np.linalg.norm(A[2,:])
e = A[1,:].T@R[2,:]
d = np.linalg.norm(A[1,:]-e*R[2,:])
R[1,:] = (A[1,:]-e*R[2,:])/np.linalg.norm(A[1,:]-e*R[2,:])
c = A[0,:].T@R[2,:]
b = A[0,:].T@R[1,:]
a = np.linalg.norm(A[0,:]-b*R[1,:]-c*R[2,:])
R[0,:] = (A[0,:]-b*R[1,:]-c*R[2,:])/np.linalg.norm(A[0,:]-b*R[1,:]-c*R[2,:])

mtx = np.zeros([3,3])#内部パラメータ
mtx[0,0] = a
mtx[0,1] = b
mtx[0,2] = c
mtx[1,1] = d
mtx[1,2] = e
mtx[2,2] = f
mtx = mtx/mtx[2,2]

t[2] = B[2]/f
t[1] = (B[1]-e*t[2])/d
t[0] = (B[0]-b*t[1]-c*t[2])/a

yaw = 0
pitch = np.arcsin(-R[2,0])
roll = 0
if np.abs(R[2,0]) != 1:
    yaw = np.arctan2(R[1,0],R[0,0])
    roll = np.arctan2(R[2,1],R[2,2])
if R[2,0] == 1:
    roll = 0
    yaw = np.arctan2(R[0,1],R[1,1])
if R[2,0] == -1:
    roll = 0
    yaw = np.arctan2(R[0,1],R[1,1])
if roll > np.pi:
    roll = roll - 2*np.pi
if roll < -np.pi:
    roll = roll + 2*np.pi
    
if pitch > np.pi:
    pitch = pitch - 2*np.pi
if pitch < -np.pi:
    pitch = pitch + 2*np.pi

if yaw > np.pi:
    yaw = yaw - 2*np.pi
if yaw < -np.pi:
    yaw = yaw + 2*np.pi

print("内部パラメータ推定結果")
print(mtx)
print("opencv decomposeProjectionMatrix関数によるRQ分解結果")
d = cv2.decomposeProjectionMatrix(P)
print(d[0]/d[0][2,2])
print("外部パラメータ推定結果")
print("カメラ位置[m]")
print(t)
print("カメラ姿勢(ZYXオイラー)[deg.]")
print([roll*180/np.pi,pitch*180/np.pi,yaw*180/np.pi])