;;
;; 6点以上のPnP問題を特異点分解で線形問題として解く
;; こちらを参考にした=> http://daily-tech.hatenablog.com/entry/2018/01/21/185633
;;
;; Input ::
;; X_earth: world 座標 3 x n_gcpの2次元配列
;; gcp_pix_vec: 画像から抽出した視線方向ベクトル, 2 x n_gcpの2次元配列 (第三要素は1とする)
;;
;; 使い方: total_error = PnP_solver(X_earth,gcp_pix_vec, P_pnp, K_pnp,R_pnp, T_pnp)
;;
function PnP_solver,X_earth,gcp_pix_vec, P_pnp, K_pnp,R_pnp, T_pnp
;; Number of samples ;;
x_siz = size(X_earth)
n_gcp = x_siz[2]
;;;;;;;;;;;;;;
;; 行列定義 ;;
;;;;;;;;;;;;;;
M_pnp = dblarr(12+n_gcp,n_gcp*3)
for i=0l, n_gcp-1, 1 do begin
offset_i = 3l*i
M_pnp[0,offset_i+0] = X_earth[0,i]
M_pnp[1,offset_i+0] = X_earth[1,i]
M_pnp[2,offset_i+0] = X_earth[2,i]
M_pnp[3 ,offset_i+0] = 1.
M_pnp[12+i, offset_i+0] = -gcp_pix_vec[0,i]
M_pnp[4,offset_i+1] = X_earth[0,i]
M_pnp[5,offset_i+1] = X_earth[1,i]
M_pnp[6,offset_i+1] = X_earth[2,i]
M_pnp[7,offset_i+1] = 1.
M_pnp[12+i, offset_i+1] = -gcp_pix_vec[1,i]
M_pnp[8,offset_i+2] = X_earth[0,i]
M_pnp[9,offset_i+2] = X_earth[1,i]
M_pnp[10,offset_i+2] = X_earth[2,i]
M_pnp[11,offset_i+2] = 1.
M_pnp[12+i, offset_i+2] = -1.
endfor
;; Solve SVDC ;;
SVDC, M_pnp, W, U, V
;; 最小固有値;;
min_W = min(W,min_pos)
;; 最小固有値に対応する固有ベクトル ;;
tmp_V = reform(V[min_pos,*])
;; tmp_Vの0-11要素を4x3行列に並べなおしたものが透視投影行列 ;;
;; M_pnp##tmp_Vが最小値になるか確認 ;;
;for i=0, n_gcp-1, 1 do begin
; tmp_V = reform(V[i,*])
; tmp_V /= norm(tmp_V)
; print,i,norm(M_pnp##tmp_V)
;endfor
;;
;; A = K ## Rot として (Rは求めたい回転行列, Kは未知だが上三角行列とする)
;;
A_pnp = dblarr(3,3)
A_pnp[*,0] = tmp_V[0:2]
A_pnp[*,1] = tmp_V[4:6]
A_pnp[*,2] = tmp_V[8:10]
R_pnp = dblarr(3,3)
f3 = norm(A_pnp[*,2])
R_pnp[*,2] = A_pnp[*,2] / f3
e2 = total(A_pnp[*,1]*R_pnp[*,2])
d2 = norm(A_pnp[*,1] - e2 * R_pnp[*,2])
R_pnp[*,1] = (A_pnp[*,1] - e2 * R_pnp[*,2])/d2
c1 = total(A_pnp[*,0] * R_pnp[*,2])
b1 = total(A_pnp[*,0] * R_pnp[*,1])
a1 = norm(A_pnp[*,0] - b1*R_pnp[*,1] - c1*R_pnp[*,2])
R_pnp[*,0] = (A_pnp[*,0] - b1*R_pnp[*,1] - c1*R_pnp[*,2])/a1
;; 内部行列 ;;
K_pnp = dblarr(3,3)
K_pnp[0,0] = a1
K_pnp[1,0] = b1
K_pnp[2,0] = c1
K_pnp[1,1] = d2
K_pnp[2,1] = e2
K_pnp[2,2] = f3
;; P = K[R T]
;; [R T] = invert(K) ## P
B_pnp = dblarr(3)
B_pnp = [tmp_V[3],tmp_V[7],tmp_V[11]]
P_pnp = dblarr(4,3)
P_pnp[0:2,*] = A_pnp
P_pnp[3,*] = B_pnp
tmp_P_pnp = invert(K_pnp) ## (P_pnp)
T_pnp = reform(tmp_P_pnp[3,*])
;; 再投影誤差解析 ;;
total_error = 0d
for i=0, n_gcp-1, 1 do begin
oi = i + 12
tmp_x = total(P_pnp[*,0] * [X_earth[*,i],1]) / tmp_V[oi] - gcp_pix_vec[0,i]
tmp_y = total(P_pnp[*,1] * [X_earth[*,i],1]) / tmp_V[oi] - gcp_pix_vec[1,i]
tmp_z = total(P_pnp[*,2] * [X_earth[*,i],1]) / tmp_V[oi] - 1.
total_error += tmp_x^2+tmp_y^2+tmp_z^2
endfor
return, total_error
end