ドイツ人と日本人のあいだ
ということで、仕事の話なんですが、挟まれてます（笑）どのくらい挟まれているかというと、タマゴサンドの卵の部分がパンの外に全部出てくるくらい挟まれてます。。。
GW明けからずっとドイツに来てまして、ひたすら大味飯（失礼！）を食べてました。なんだか今年はドイツ年になりそうで、、、ロボットをやる時間がなかなか取れなさそうです。。。うーん。どうしたもんかしら。
四年ぶりのハイデルベルク！
フランクフルトのコインランドリー！見つかるまで、パンツ二連続で履いてたことは内緒ですが。
今からスイスです！
関数の最適化 ～勾配法～
バンドル調整に向けて，数学の復習をしておきます．参考文献はいつものごとく金谷先生の本です．
最適化とは
ある制約条件のもとで，ある関数の値を最大または最小にするような変数の値を求めること．
最大値・最小値を求めるためのトリック
ニュートン法，勾配法，レーベンバーグ・マーカート法等，非線形最適化をするアルゴリズムはたくさんありますが，基本となっている考え方は，，，
関数 f(x) の極値では，一次導関数 f'(x) = 0 となる．
です．結局アルゴリズムが違っても，f'(x) = 0 となるような x を探しているという点では同じです．なので，陰関数 f(x) = 0 を解くことができるツールであれば，最適化にも使えると思ってよいと思います．最適化の場合，解くべき陰関数が関数 f(x) ではなく，一次導関数 f'(x) となります．
一変数の場合の勾配法
まず導入として，一変数関数の場合の勾配法を考えます．基本となる考え方はシンプルで，上に凸の関数の場合，勾配がプラス（マイナス）の間はずっと x を増加（減少）させます．上に凸の関数であれば，この作業を続けていればどこかで必ず勾配がマイナス（プラス）になります．この間のどこかに勾配が０である場所があると考えられるので，これを求める最適解として採用します．
一変数の場合の勾配法アルゴリズム
最大値を求める場合の勾配法アルゴリズムは下記のとおりです．傾きの符号が変化するまで同一方向に進み，符号変化を検知した時点で進む距離（ステップ幅）を減らして反対方向に進む．という作業を繰り返し，最後には f'(x) = 0 を満たす変数を求めます．
多変数の場合の勾配法
少し複雑になりますが，多変数の場合も考え方は同じです．多変数の場合も曲面に対して接平面を考え，この接平面の傾きが一番大きい方向を基準に進んでいきます．で，この方向に進んでいく中で一番大きい値を探索します．最大値を見つけたら，その点に関して接平面を考え再び傾きが一番大きい方向を基準に進んでいき．．．．と繰り返せば最大値が求まります．
多変数の場合の勾配法アルゴリズム
一変数の勾配法の実験 with Python
ということで，勾配法を実装してみました．まずは一変数の場合の勾配法です．最大値を求めたい関数として，下記の関数を考えます．
f(x) = sin(x) + cos(x/2 -1)
一変数の勾配法による実験結果
下記実験結果ですが，緑色の点が反復の途中仮定，赤色の点が最終結果になります．最大値の回りで反復が終わっていることがわかります．
多変数の勾配法の実験 with Python
ついで，多変数の勾配法です．変数の数が多いと可視化が難しいので，二変数の関数に対して最大値を求めます．
f(x, y) = -(15*(x-6)**2 + 2*(x-6)*(y-4) + 3*(y-4)**2 + 4*(x-6) + 3*(y-4) - 15)
多変数の勾配法による実験結果
下記実験結果ですが，楕円のてっぺんが最大値になります．各点で等高線に垂直な方向に進み，最終的には最大値にたどり着いている様子がわかります．緑色の点が途中経過で，黄色の点が最終解です．
Pythonコード
下記，まだちょっとおかしいです．．．．収束しないときが多々あり．．．また時間ができたら見直します．
gradient_method.py
# -*- coding: utf-8 -*-
import numpy as np
def derivativeNd(func, xVec, d):
dVec = np.zeros(xVec.shape)
for i in range(xVec.shape[0]):
xVecP = np.copy(xVec)
xVecN = np.copy(xVec)
xVecP[i] = xVec[i] + d/2
xVecN[i] = xVec[i] - d/2
dVec[i] = (func(xVecP) - func(xVecN)) / d
return dVec
def differentiate(func, xVec, dirVec, d):
dVec = np.zeros(xVec.shape)
dx = dirVec / np.linalg.norm(dirVec) * d
for i in range(xVec.shape[0]):
xVecP = np.copy(xVec)
xVecN = np.copy(xVec)
xVecP[i] = xVec[i] + dx[i] / 2
xVecN[i] = xVec[i] - dx[i] / 2
dVec[i] = (func(xVecP) - func(xVecN)) / d
return dVec
def hill_climbNd(func, xVecInit, stepInit, ipu):
xVec = np.copy(xVecInit)
xVecOld = np.copy(xVecInit)
step = stepInit
history = np.zeros((0, xVec.shape[0]))
history = np.append(history, np.array([xVec]), axis=0)
while True:
xVecOld = np.copy(xVec)
dirVec = derivativeNd(func, xVec, 0.000001)
xVec, tmp = hill_climb_to_dir(func, xVec, dirVec, step, ipu)
history = np.append(history, np.array([xVec]), axis=0)
if np.linalg.norm(xVec - xVecOld) <= ipu:
break
return xVec, history
def hill_climb_to_dir(func, xVecInit, dirVec, stepInit, ipu):
x = xVecInit
step = stepInit
d = 0.00001
normDirVec = dirVec / np.linalg.norm(dirVec)
history = [x]
MINSTEP = 1.0e-15
while True:
# Step 2 : Setup Step Length
dfVec = differentiate(func, x, normDirVec, min(d, abs(step)/1000))
if np.linalg.norm(dfVec) <= ipu:
break
sign = 1 if np.dot(dfVec, normDirVec) > 0 else -1
xb = x
xa = x + sign * abs(step) * normDirVec
# Step 3 : Find Another Side of Mountain.
if func(xb) < func(xa):
while True:
step = 2*step
xb = xa
xa = xb + sign * abs(step) * normDirVec
if func(xb) >= func(xa):
break
x = xb
step = max(step/2, MINSTEP)
# Step 4 : Overrun and Reset.
else:
while True:
step = max(step/2, MINSTEP)
xa = xa - sign * abs(step) * normDirVec
if func(xb) <= func(xa):
break
x = xa
step = 2*step
# Step 5 : Convergence Check
dfVec = differentiate(func, x, normDirVec, min(d, abs(step)/1000))
history.append(x)
if np.linalg.norm(dfVec) < ipu:
break
return x, np.array(history)test_gradient_method.py
# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
import numpy as np
import math
from optimization.gradient_method import hill_climb_to_dir
from optimization.gradient_method import hill_climbNd
def test_func1d(xVec):
return math.sin(xVec[0]) + np.cos(xVec[0]/2 - 1)
def test_func2d(xVec):
x = xVec[0]
y = xVec[1]
return (-(15*(x-6)**2 + 2*(x-6)*(y-4) + 3*(y-4)**2 + 4*(x-6) + 3*(y-4) - 15))
def test_hill_climb1d():
x = np.linspace(-5, 5, 101)
y = np.zeros(x.shape[0])
for i in range(x.shape[0]):
y[i] = test_func1d(np.array([x[i]]))
ans, hist = hill_climb_to_dir(test_func1d, np.array([-2.0]),\
np.array([1.0]), 0.1, 0.00001)
xVecInit = np.array([-2.0], dtype='float64')
stepWidth = 1.0
termCondition = 0.01
xVec, history = \
hill_climbNd(test_func1d, xVecInit, stepWidth, termCondition)
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
ax.plot(x, y)
for i in range(hist.shape[0]):
x = hist[i]
y = test_func1d(x)
if i == hist.shape[0] - 1:
size = 50
color = 'red'
else:
size = 10
color = 'green'
ax.scatter(x, y, s=size, c=color, edgecolors=color)
plt.show()
def test_hill_climb2d():
x1 = np.linspace(-10, 10, 101)
x2 = np.linspace(-10, 10, 101)
X1, X2 = np.meshgrid(x1, x2)
X = np.c_[np.ravel(X1), np.ravel(X2)]
Z = np.empty((0, 1))
for i in range(X.shape[0]):
z = test_func2d(X[i])
Z = np.append(Z, [[z]], axis=0)
Z = np.reshape(Z, (X1.shape[0], X2.shape[0]))
xVecInit = np.array([-3.0, -3.0], dtype='float64')
stepWidth = 1.0
termCondition = 0.01
xVec, history = \
hill_climbNd(test_func2d, xVecInit, stepWidth, termCondition)
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
values = set()
for i in range(history.shape[0]):
xVec = history[i]
values.add(test_func2d(xVec))
cont = ax.contourf(X1, X2, Z)
for i in range(cont.levels.shape[0]):
values.add(cont.levels[i])
levels = list(values)
levels.sort()
ax.contourf(X1, X2, Z, levels)
for i in range(history.shape[0]):
xVec = history[i]
if i == history.shape[0] - 1:
size = 50
color = 'yellow'
else:
size = 10
color = 'green'
ax.scatter(xVec[0], xVec[1], s=size, c=color, edgecolors=color)
plt.show()
if __name__ == "__main__":
print("Gradient Method Test")
#test_hill_climb1d()
test_hill_climb2d()
参考文献
- 作者: 金谷健一
- 出版社/メーカー: 共立出版
- 発売日: 2005/09/01
- メディア: 単行本
- 購入: 29人 クリック: 424回
- この商品を含むブログ (42件) を見る
最適化計算５ バンドル調整 理論編１
バンドル調整は頑張って記事を書き直しました．よければ，リンク先の記事を見て頂けたらと思います．
最適化計算３ 基礎行列の推定 実験編
下記のエントリで基礎行列計算の理論編を勉強しました．ということで，いつものごとくPythonを使って実験です．
daily-tech.hatenablog.com
問題設定
下記のように二次曲面の形状を設置し，この物体を複数の視点に置いたカメラで撮影する状況を考えます．
対称物体の投影図
OpenCVのProjectPoints関数を用いて，上図の三視点に置かれたカメラで物体を撮影します．
OpenCVのProjectPoint関数の定義は下記のようになっており，物体の座標系で点を指定することができます．物体の座標系とカメラの座標系の関係を tvecs, rvecs に指定してあげれば，入力点群が自動的にカメラ座標系に変換され，そののちに画像に投影されます．
cv2.projectPoints(pntsInObj, rvec2, tvec2, cameraMatrix, distCoeffs)
一点注意点としては，この時の rvecとtvecですが，下記数式の t と R (を Rodrigues形式に変換したもの) を渡してあげる必要があります．つまり，物体座標系からカメラ座標系への変換式の回転要素 R と並進要素 T を渡してあげればOKです．
CAMXP = CAMX t + CAMX R OBJ ・ OBJP
基礎行列の計算
今回の実験では，図の CAM1 と CAM2 の位置関係（既知）を使って基礎行列真値を計算します．そのあとに２画像の投影点ペアを用いて基礎行列を計算し，真値にたいしての誤差が計算方法によってどの程度変動するかを調べます．まず，既知の位置関係を使って，エピポーラ拘束式が満たされていることを確認します．二つの視点を図示したものが下記になります．
最後のスクリプトを実行すると，下記の結果が得られてエピポーラ拘束条件が満たされていることがわかります．
Max Element of m1Fm2 : 2.22044604925e-16 Max Element of m2Fm1 : 1.66533453694e-16
実験結果
ちょっと時間がないので，「最小二乗法 + SVDによるランク補正」と「Taubin法 + SVDによるランク補正」の二つの実行結果を載せます．基礎行列真値に対して，添付のスクリプトを実行すると下記の結果が得られました．一応，最小二乗法のよりも Taubin 法のほうが結果がかなりいいことがわかります．また時間ができたらほかのスキームも作って更新します．
Accuracy check for each numerical scheme. FTrue VS FLSSVD : 0.00273989625189 FTrue VS FTaubinSVD : 0.000641120755582
Python スクリプト
ちょっとコードが長くなってしまったのですが，一応載せておきます．
test_fund_mat.py
# -*- coding: utf-8 -*-
import cv2
import numpy as np
from geometry_k.rotation import RMatFromEulerAngle
from geometry_k.rotation import OuterProductOperator
from fund_mat import calc_fund_mat
from fund_mat import normalize_mat
from fund_mat import FundMatCalcMode
from fund_mat import calc_dist_between_2F
from data_manipulation.noise import add_gauss_noise
# Camera Intrinsic Parameter
f = 160
width = 640
height = 480
pp = (width / 2, height / 2)
cameraMatrix = np.matrix(np.array([[f, 0, pp[0]],
[0, f, pp[1]],
[0, 0, 1]], dtype = "double"))
distCoeffs = np.zeros((5, 1))
# Camera Extrinsic Parameter 0
xEul_C0_to_OBJ = 0
yEul_C0_to_OBJ = 0
zEul_C0_to_OBJ = 0
ROBJ_to_0 = RMatFromEulerAngle(xEul_C0_to_OBJ, yEul_C0_to_OBJ, zEul_C0_to_OBJ)
C0_to_OBJ_in_C0 = (0, 0, 10)
# Camera Extrinsic Parameter 1
xEul_C1_to_OBJ = 0
yEul_C1_to_OBJ = -45
zEul_C1_to_OBJ = 0
ROBJ_to_1 = RMatFromEulerAngle(xEul_C1_to_OBJ, yEul_C1_to_OBJ, zEul_C1_to_OBJ)
C1_to_OBJ_in_C1 = (0, 0, 10)
# Camera Extrinsic Parameter 2
xEul_C2_to_OBJ = 0
yEul_C2_to_OBJ = 45
zEul_C2_to_OBJ = 0
ROBJ_to_2 = RMatFromEulerAngle(xEul_C2_to_OBJ, yEul_C2_to_OBJ, zEul_C2_to_OBJ)
C2_to_OBJ_in_C2 = (0, 0, 10)
pntRow = 10
pntCol = 10
depth = 0.15
if __name__ == "__main__":
print("Fundamental Mat Test")
# Create Curve Surface in 3D World
pntsInObj = np.zeros((0, 3))
for i in range(pntCol + 1):
for j in range(pntRow + 1):
x = (j - pntCol / 2)
y = (i - pntRow / 2)
z = depth * x**2
a = np.array([[x, y, z]])
pntsInObj = np.append(pntsInObj, a, axis=0)
# Calculate Translation, Rotation for Each Camera.
rvec0, jac = cv2.Rodrigues(ROBJ_to_0)
tvec0 = np.eye(3) * np.matrix(C0_to_OBJ_in_C0).T
rvec1, jac = cv2.Rodrigues(ROBJ_to_1)
tvec1 = np.eye(3) * np.matrix(C1_to_OBJ_in_C1).T
rvec2, jac = cv2.Rodrigues(ROBJ_to_2)
tvec2 = np.eye(3) * np.matrix(C2_to_OBJ_in_C2).T
# Convert Points on 3D Curved Surface for Each Cam Image Points.
imgPnts0, jac = cv2.projectPoints(pntsInObj, rvec0, tvec0, cameraMatrix, distCoeffs)
imgPnts1, jac = cv2.projectPoints(pntsInObj, rvec1, tvec1, cameraMatrix, distCoeffs)
imgPnts2, jac = cv2.projectPoints(pntsInObj, rvec2, tvec2, cameraMatrix, distCoeffs)
# Draw Points
img0 = np.full((height, width, 3), (255, 255, 255), np.uint8)
img1 = np.full((height, width, 3), (255, 255, 255), np.uint8)
img2 = np.full((height, width, 3), (255, 255, 255), np.uint8)
for pnt in imgPnts0:
cv2.circle(img0, (int(pnt[0][0]), int(pnt[0][1])), 3, (0, 0, 0), -1)
for pnt in imgPnts1:
cv2.circle(img1, (int(pnt[0][0]), int(pnt[0][1])), 3, (255, 0, 0), -1)
for pnt in imgPnts2:
cv2.circle(img2, (int(pnt[0][0]), int(pnt[0][1])), 3, (0, 0, 255), -1)
# Calculate True Fundamental Matrix
R1_to_2 = ROBJ_to_2 * ROBJ_to_1.T
T1_in_C2 = np.matrix(C2_to_OBJ_in_C2).T - R1_to_2 * np.matrix(C1_to_OBJ_in_C1).T
tMat_1_in_C2 = OuterProductOperator(T1_in_C2)
Ainv = np.linalg.inv(cameraMatrix)
Ftrue21 = normalize_mat(Ainv.T * tMat_1_in_C2 * R1_to_2 * Ainv)
R2_to_1 = ROBJ_to_1 * ROBJ_to_2.T
T2_in_C1 = np.matrix(C1_to_OBJ_in_C1).T - R2_to_1 * np.matrix(C2_to_OBJ_in_C2).T
tMat_2_in_C1 = OuterProductOperator(T2_in_C1)
Ainv = np.linalg.inv(cameraMatrix)
Ftrue12 = normalize_mat(Ainv.T * tMat_2_in_C1 * R2_to_1 * Ainv)
# Denoise Img Points
noisedImgPnts1 = add_gauss_noise(imgPnts1, 0, 2)
noisedImgPnts2 = add_gauss_noise(imgPnts2, 0, 2)
#noisedImgPnts1 = imgPnts1
#noisedImgPnts2 = imgPnts2
# Calculate Fundamental Matrix
Fls = calc_fund_mat(noisedImgPnts1, noisedImgPnts2, FundMatCalcMode.LS)
FlsSVD = calc_fund_mat(noisedImgPnts1, noisedImgPnts2, FundMatCalcMode.LSSVD)
Ftaubin = calc_fund_mat(noisedImgPnts1, noisedImgPnts2, FundMatCalcMode.Taubin)
FtaubinSVD = calc_fund_mat(noisedImgPnts1, noisedImgPnts2, FundMatCalcMode.TaubinSVD)
m1Fm2 = []
m2Fm1 = []
for i in range(imgPnts1.shape[0]):
m1 = (np.matrix([imgPnts1[i][0][0], imgPnts1[i][0][1], 1.0])).T
m2 = (np.matrix([imgPnts2[i][0][0], imgPnts2[i][0][1], 1.0])).T
m1Fm2.append(abs((m1.T * Ftrue12 * m2)[0, 0]))
m2Fm1.append(abs((m2.T * Ftrue21 * m1)[0, 0]))
print()
print("Confirm if true F mat really satisfies epipolar equation.")
print("Max Element of m1Fm2 : ", max(m1Fm2))
print("Max Element of m2Fm1 : ", max(m2Fm1))
print()
print("Accuracy check for each numerical scheme.")
print("FTrue VS FLS : ", calc_dist_between_2F(Ftrue12, Fls))
print("FTrue VS FLSSVD : ", calc_dist_between_2F(Ftrue12, FlsSVD))
print("FTrue VS FTaubin : ", calc_dist_between_2F(Ftrue12, Ftaubin))
print("FTrue VS FTaubinSVD : ", calc_dist_between_2F(Ftrue12, FtaubinSVD))
# Display
cv2.imshow("CAM0", cv2.resize(img0, None, fx = 0.5, fy = 0.5))
cv2.imshow("CAM1", cv2.resize(img1, None, fx = 0.5, fy = 0.5))
cv2.imshow("CAM2", cv2.resize(img2, None, fx = 0.5, fy = 0.5))
cv2.waitKey(0)
fund_mat.py
# -*- coding: utf-8 -*-
import numpy as np
import scipy.linalg as splinalg
from enum import Enum
def normalize_mat(F):
sumMat = 0
for j in range(F.shape[0]):
for i in range(F.shape[1]):
sumMat = sumMat + F[j, i]**2
return F / sumMat**0.5
def create_cov_mat(dataVec1, dataVec2, f0):
x1 = dataVec1[0, 0]
y1 = dataVec1[1, 0]
x2 = dataVec2[0, 0]
y2 = dataVec2[1, 0]
cov = np.matrix(np.zeros((9, 9)))
cov[0, 0] = x1**2 + x2**2
cov[0, 1] = x2 * y2
cov[0, 2] = f0 * x2
cov[0, 3] = x1 * y1
cov[0, 6] = f0 * x1
cov[1, 0] = x2 * y2
cov[1, 1] = x1**2 + y2**2
cov[1, 2] = f0 * y2
cov[1, 4] = x1 * y1
cov[1, 7] = f0 * x1
cov[2, 0] = f0 * x2
cov[2, 1] = f0 * y2
cov[2, 2] = f0**2
cov[3, 0] = x1 * y1
cov[3, 3] = y1**2 + x2**2
cov[3, 4] = x2 * y2
cov[3, 5] = f0 * x2
cov[3, 6] = f0 * y1
cov[4, 1] = x1 * y1
cov[4, 3] = x2 * y2
cov[4, 4] = y1**2 + y2**2
cov[4, 5] = f0 * y2
cov[4, 7] = f0 * y1
cov[5, 3] = f0 * x2
cov[5, 4] = f0 * y2
cov[5, 5] = f0**2
cov[6, 0] = f0 * x1
cov[6, 3] = f0 * y1
cov[6, 6] = f0**2
cov[7, 1] = f0 * x1
cov[7, 4] = f0 * y1
cov[7, 7] = f0**2
return cov
def create_zeta_vec(dataVec1, dataVec2, f0):
x1 = dataVec1[0, 0]
y1 = dataVec1[1, 0]
x2 = dataVec2[0, 0]
y2 = dataVec2[1, 0]
zeta = np.matrix(np.zeros(9)).T
zeta[0, 0] = x1*x2
zeta[1, 0] = x1*y2
zeta[2, 0] = f0*x1
zeta[3, 0] = y1*x2
zeta[4, 0] = y1*y2
zeta[5, 0] = f0*y1
zeta[6, 0] = f0*x2
zeta[7, 0] = f0*y2
zeta[8, 0] = f0**2
return zeta
class FundMatCalcMode(Enum):
LS = 0
Taubin = 1
LSSVD = 2
TaubinSVD = 3
def calc_fund_mat(imgPnts1, imgPnts2, calcMode):
# For numeeical stability.
f1 = max(max(imgPnts1[:, 0, 0]), max(imgPnts1[:, 0, 1]))
f2 = max(max(imgPnts2[:, 0, 0]), max(imgPnts2[:, 0, 1]))
f0 = max(f1, f2)
f0 = 1
# Calculation via LS.
if (calcMode == FundMatCalcMode.LS):
F = calc_fmat_by_LS(imgPnts1[:, 0], imgPnts2[:, 0], f0)
elif (calcMode == FundMatCalcMode.Taubin):
F = calc_fmat_by_Taubin(imgPnts1[:, 0], imgPnts2[:, 0], f0)
elif (calcMode == FundMatCalcMode.LSSVD):
Finit = calc_fmat_by_LS(imgPnts1[:, 0], imgPnts2[:, 0], f0)
F = calc_fmat_by_SVD(Finit)
elif (calcMode == FundMatCalcMode.TaubinSVD):
Finit = calc_fmat_by_Taubin(imgPnts1[:, 0], imgPnts2[:, 0], f0)
F = calc_fmat_by_SVD(Finit)
else:
raise Exception("Un-Supported Calc Mode!")
return F
def create_M_mat(imgPnts1, imgPnts2, f0):
M = np.matrix(np.zeros((9, 9)))
for i in range(imgPnts1.shape[0]):
zeta = create_zeta_vec(np.matrix(imgPnts1[i, :]).T, np.matrix(imgPnts2[i, :]).T, f0)
M = M + zeta * zeta.T
return M / imgPnts1.shape[0]
def create_N_mat(imgPnts1, imgPnts2, f0):
N = np.matrix(np.zeros((9, 9)))
for i in range(imgPnts1.shape[0]):
cov = create_cov_mat(np.matrix(imgPnts1[i, :]).T, np.matrix(imgPnts2[i, :]).T, f0)
N = N + cov
return N / imgPnts1.shape[0]
def calc_fmat_by_LS(imgPnts1, imgPnts2, f0):
M = create_M_mat(imgPnts1, imgPnts2, f0)
lamdas, v = np.linalg.eigh(M)
#print(v[:, np.argmin(lamdas)])
return normalize_mat(np.reshape(v[:, np.argmin(lamdas)], (3, 3)))
def calc_fmat_by_Taubin(imgPnts1, imgPnts2, f0):
M = create_M_mat(imgPnts1, imgPnts2, f0)
N = create_N_mat(imgPnts1, imgPnts2, f0)
lamdas, v = splinalg.eig(N, M)
return normalize_mat(np.reshape(v[:, np.argmax(np.absolute(lamdas))], (3, 3)))
def calc_fmat_by_SVD(F):
u, s, v = np.linalg.svd(F)
ss = (s[0]**2 + s[1]**2)**0.5
Fsvd = u * np.matrix([[s[0]/ss, 0, 0],
[0, s[1]/ss, 0],
[0, 0, 0]]) * v
return normalize_mat(Fsvd)
def calc_dist_between_2F(F1, F2):
dist1 = 0
dist2 = 0
for j in range(F1.shape[0]):
for i in range(F1.shape[1]):
dist1 = dist1 + (F1[j, i] - F2[j, i])**2
dist2 = dist2 + (F1[j, i] + F2[j, i])**2
dist1 = dist1**0.5
dist2 = dist2**0.5
return dist1 if dist1 < dist2 else dist2
geometry_k.rotation.py
# Python System Package
import numpy as np
import math
def importCheck():
print("Geometry Imported!")
def RMatFromFixedAngle(xDeg, yDeg, zDeg):
xRad = xDeg / 180 * math.pi
yRad = yDeg / 180 * math.pi
zRad = zDeg / 180 * math.pi
Rx = np.matrix([[1, 0, 0], \
[0, math.cos(xRad), -math.sin(xRad)], \
[0, math.sin(xRad), math.cos(xRad)]])
Ry = np.matrix([[ math.cos(yRad), 0, math.sin(yRad)], \
[0, 1, 0], \
[-math.sin(yRad), 0, math.cos(yRad)]])
Rz = np.matrix([[ math.cos(zRad), -math.sin(zRad), 0], \
[ math.sin(zRad), math.cos(zRad), 0], \
[0, 0, 1]])
return Rz * Ry * Rx
def RMatFromEulerAngle(xDeg, yDeg, zDeg):
xRad = xDeg / 180 * math.pi
yRad = yDeg / 180 * math.pi
zRad = zDeg / 180 * math.pi
Rx = np.matrix([[1, 0, 0], \
[0, math.cos(xRad), -math.sin(xRad)], \
[0, math.sin(xRad), math.cos(xRad)]])
Ry = np.matrix([[ math.cos(yRad), 0, math.sin(yRad)], \
[0, 1, 0], \
[-math.sin(yRad), 0, math.cos(yRad)]])
Rz = np.matrix([[ math.cos(zRad), -math.sin(zRad), 0], \
[ math.sin(zRad), math.cos(zRad), 0], \
[0, 0, 1]])
return Rx * Ry * Rz
def OuterProductOperator(t):
T = np.matrix([[0, -t[2, 0], t[1, 0]],
[t[2, 0], 0, -t[0, 0]],
[-t[1, 0], t[0, 0], 0]])
return T
def TMatFromRt(R, t):
T = np.zeros((4, 4))
T[0:3, 0:3] = R.A
T[0:3, 3:4] = t.A
T[3, :] = np.array([0, 0, 0, 1])
return np.matrix(T)
def ExtParamsFromRt(R, t):
E = np.zeros((3, 4))
E[0:3, 0:3] = R.A
E[0:3, 3:4] = t.A
return np.matrix(E)
def InnerParamMat(f, Cu, Cv, skewU, skewV):
A = np.matrix([[f/skewU, 0, Cu], [0, f/skewV, Cv], [0, 0, 1]])
return A
data_manipulation.noise.py
# -*- coding: utf-8 -*-
import numpy as np
def add_gauss_noise(data, avg, stdv, absmax=0):
noise = np.random.normal(avg, stdv, data.shape)
if absmax != 0:
noise = np.clip(noise, -(absmax + avg), absmax + avg)
dataWError = data + noise
return dataWError
最適化計算３ 基礎行列の推定 理論編
最適化計算のエントリ第三弾ということで，ようやく基礎行列の話題まで来ました．参考文献はいつもの通り金谷先生の本です．
基礎行列・基本行列とは
下記のエントリにも書いた通り，３次元上の点を異なる二視点から撮影した時，点の二つの投影画像座標が満たす数式があります．この拘束条件をエピポーラ拘束といいますが，エピポーラ拘束式に出てくる F を基礎行列，E を基本行列といいます．今回のテーマでは，二つの画像の対応点を使って，この F を求めます．
daily-tech.hatenablog.com
基礎行列の計算
エピポーラ拘束より，下記の数式が成立します．
今回のエントリでは，上記エピポーラ方程式の 3 x 3 行列である F を計算します．
DLT法が使えるように，線形方程式に変換
楕円の当てはめエントリでも実施しましたが，線形方程式を立式するところは結局同じアプローチになります．
式展開の最後で，エピポーラ方程式を単純な内積の形に表せました．楕円の当てはめの時と同じように，すべての点ペアの内積和が最も小さくなるように F 行列の 9個の要素を決定するという問題になります．ここで一点注意ですが， F 行列は定数倍の自由度があるのでノルムを 1 であると勝手に決めて正規化します．ここで，このノルムを下記のように定義します．
共分散行列と代数的方法（最尤推定）
楕円当てはめの時と同じように，式の構造を意識しながら誤差を考えてみます．楕円当てはめの時と同じように，線形化している関係上，入力ベクトル（ζ）が (x, y) の２変数ではなく９変数あるように見えますが，これは方程式を線形化する過程で意図的に作ったもです．なので，結局誤差が発生するのは x, y となります．あとの 7 項の誤差成分に関しては x, y の誤差を適切に仮定してあげれば計算から出てきます．
上式のΔ項を確率変数としてζの定義に入れて式展開し，誤差の次数に応じて数式を分割して書くと下記のようになります．
誤差の第二項は十分小さいとして無視すると，誤差分布は誤差の第一項だけで決まります．確率変数４つがそれぞれ独立で，期待値０，分散σの正規分布に従うとすると，共分散行列は下記のようになります．
共分散行列が求まったので，楕円の当てはめの時と同じように， Taubin法，重み反復法等を使って基礎行列を求めることができるようになります．楕円当てはめの時とアルゴリズムはまったく同じですが一応メモ程度に書いておきます．
最小二乗法
重み反復法
Taubin法
ランク拘束
ここまでで，基礎行列を求めることは形的に楕円当てはめの問題と同じになることがわかりました．が，上で説明した方法では，誤差のある点群からは正しく基礎行列が求まりません．この点が「楕円当てはめ」と「基礎行列計算」の違いになります．それは，基礎行列 F はランクが 3 ではなく，2 である必要があるためです．これをランク拘束と呼びます．上の方法では，誤差のある点群に対して基礎行列を計算すると，求まる解がランク３になってしまいます．ということで，このランク拘束を満たす必要があるのですが，このやり方で手法が大きく３つに大別されます．
１．事後補正法（ランク補正）
ひとまずランク拘束を考慮せず θ を計算し，次にランク拘束を満たすように補正する方法．
まず，一番簡単な特異値分解によるランク補正法です．この方法では，一番小さい特異値を無理矢理０に変えます．
特異値によるランク補正では一番小さい特異値を無理矢理０に変えましたが，これをもっと最適に補正する方法が次の方法です．
２．内部接近法
初めから基礎行列をランク２となるようにパラメータ化し，そのパラメータ空間内で J を最小にする基礎行列を求める方法です．
ちょっと書くのに疲れたので，あとでアップします．．．．
３．外部接近法
適当な初期値から出発し，サンプソン誤差 J を最小にするために反復を重ねつつ，かつランク拘束も満たすように収束させる方法です．
と，，，いうことで，なんだか内容が難しくなってきましたが，実験編に移ります！
参考文献
参考文献１
- 作者: 金谷健一,菅谷保之,金澤靖
- 出版社/メーカー: 森北出版
- 発売日: 2016/10/27
- メディア: 単行本（ソフトカバー）
- この商品を含むブログ (1件) を見る
参考文献２
画像の三次元理解のための最適化計算［Ⅲ］
レイリー商を用いた最小値・最大値問題の解法
レイリー商と呼ばれる下記の数式を最小化することを考えてみます．
ここで，Aは実対称行列としているので，実数固有値が存在します．直交行列 Q によって A をスペクトル分解したとすると，上記レイリー商の定義式は下記のように変形できます．
変形後のレイリー商の分子に注目してほしいんですが，最小固有値を λ1，最大固有値を λnとすると分子の値はそれぞれ二つの固有値によって下限・上限が決まることがわかります．
分子項の下限・上限がはっきりしたので，これをもとにレイリー商を計算すると，
この問題の見方を少し変えると，条件付き最小値・最大値問題が簡単にとけます．例えば，下記のような問題を考えます．
上記の例のように，条件付き最小値・最大値問題が簡単にとけるようになります．この考え方を使って最適化問題を解いていきます．
最適化計算２ 楕円の当てはめ 実験編
下記のエントリで楕円の当てはめの理論的な部分のエントリをしましたが，実際にパイソンのスクリプトを作って実験しました．
daily-tech.hatenablog.com
が，これが思ったより奥が深く．．．フィッティングはするようになったのですが，データに付加するノイズを増やしていくと安定しません．．．．読んだ論文には KCR限界とかっていう理論的な限界があるみたいなのですが，おそらくきちんとは理解できないのかと．ただ，大分時間を使ってしまったので，もう基礎行列の計算に進みます．下記のスクリプトでは，一応 ”最小二乗法” による楕円当てはめと "重み付き繰り返し法" "FNS法" による楕円の当てはめを実装してます．見えている楕円弧が短い時に，当てはめをするのがすごく難しいです．
# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
import numpy as np
import math
import scipy as sp
def createMMat(data, weight):
dataMod = np.matrix(np.zeros((data.shape[0], 6)))
for i in range(data.shape[0]):
dataMod[i, 0] = data[i, 0]**2
dataMod[i, 1] = data[i, 0] * data[i, 1]
dataMod[i, 2] = data[i, 1]**2
dataMod[i, 3] = data[i, 0] * data[i, 2]
dataMod[i, 4] = data[i, 1] * data[i, 2]
dataMod[i, 5] = data[i, 2]**2
M = np.zeros((6, 6))
M = np.matrix(M)
for i in range(dataMod.shape[0]):
dM = dataMod[i, :].T * dataMod[i, :]
M = M + weight[i, 0] * dM
return M / dataMod.shape[0]
def createLMat(data, weight, theta, covs):
dataMod = np.matrix(np.zeros((data.shape[0], 6)))
for i in range(data.shape[0]):
dataMod[i, 0] = data[i, 0]**2
dataMod[i, 1] = data[i, 0] * data[i, 1]
dataMod[i, 2] = data[i, 1]**2
dataMod[i, 3] = data[i, 0] * data[i, 2]
dataMod[i, 4] = data[i, 1] * data[i, 2]
dataMod[i, 5] = data[i, 2]**2
L = np.matrix(np.zeros((6, 6)))
for i in range(dataMod.shape[0]):
coeff = weight[i, 0]**2 * (dataMod[i, :] * theta)**2
L = L + coeff[0, 0] * covs[i]
return L / dataMod.shape[0]
def fittingEllipseWithLS(data, normTerm):
weight = np.matrix(np.ones(data.shape[0])).T
dataMod = np.concatenate((data / normTerm, np.ones((data.shape[0], 1))), axis=1)
M = createMMat(dataMod, weight)
lamdas, v = np.linalg.eigh(M)
#print(lamdas)
#print(v)
theta = v[:, np.argmin(np.absolute(lamdas))]
#print(theta)
lamdas, v = sp.linalg.eigh(M)
#print(lamdas)
#print(v)
theta = np.matrix(v[:, np.argmin(np.absolute(lamdas))]).T
#print(theta)
theta[3, 0] = normTerm * theta[3, 0]
theta[4, 0] = normTerm * theta[4, 0]
theta[5, 0] = normTerm**2 * theta[5, 0]
return theta
def fittingEllipseWithTaubin(data, normTerm):
# Add normalized term.
dataMod = np.concatenate((data / normTerm, np.ones((data.shape[0], 1))), axis=1)
# Param Vector
theta = np.matrix(np.zeros(6)).T
# Covars
covavg = np.matrix(np.zeros((6, 6)))
for i in range(dataMod.shape[0]):
data = dataMod[i, :]
covavg = covavg + createCovMat(data)
covavg = covavg / dataMod.shape[0]
weight = np.ones(dataMod.shape[0])
weight = np.matrix(weight).T
M = createMMat(dataMod, weight);
lamdas, v = sp.linalg.eig(covavg, M)
theta = np.matrix(v)[:, np.argmax(np.absolute(lamdas))]
theta[3, 0] = normTerm * theta[3, 0]
theta[4, 0] = normTerm * theta[4, 0]
theta[5, 0] = normTerm**2 * theta[5, 0]
return theta
def createCovMat(data):
x = data[0, 0]
y = data[0, 1]
f0 = data[0, 2]
xx = x**2
yy = y**2
xy = x*y
f0x = f0*x
f0y = f0*y
f0f0 = f0**2
cov = np.matrix([[xx, xy, 0, f0x, 0, 0], \
[xy, xx+yy, xy, f0y, f0x, 0], \
[0, xy, yy, 0, f0y, 0], \
[f0x, f0y, 0, f0f0, 0, 0], \
[0, f0x, f0y, 0, f0f0, 0], \
[0, 0, 0, 0, 0, 0]])
return cov
def fittingEllipseWithItrReweight(data, normTerm):
# Add normalized term.
dataMod = np.concatenate((data / normTerm, np.ones((data.shape[0], 1))), axis=1)
# Param Vector
theta = np.matrix(np.zeros(6)).T
thetaOrg = theta
# Weight matrix.
weight = np.ones(dataMod.shape[0])
weight = np.matrix(weight).T
# Covars
covs = []
for i in range(dataMod.shape[0]):
data = dataMod[i, :]
covs.append(createCovMat(data))
loop = 0
while True:
# M Matrix
M = createMMat(dataMod, weight)
lamdas, v = np.linalg.eigh(M)
thetaOrg = theta
theta = v[:, np.argmin(np.absolute(lamdas))]
term = np.linalg.norm(theta - thetaOrg)
if term < 0.0001 or loop > 20:
break
for i in range(dataMod.shape[0]):
alp = theta.T * covs[i] * theta
weight[i, 0] = 1 / (alp)
loop = loop + 1
theta[3, 0] = normTerm * theta[3, 0]
theta[4, 0] = normTerm * theta[4, 0]
theta[5, 0] = normTerm**2 * theta[5, 0]
return theta
def fittingEllipseWithFNS(data, normTerm):
# Add normalized term.
dataMod = np.concatenate((data / normTerm, np.ones((data.shape[0], 1))), axis=1)
# Param Vector
theta = np.matrix(np.zeros(6)).T
thetaNew = theta
thetaOrg = theta
# Weight matrix.
weight = np.ones(dataMod.shape[0])
weight = np.matrix(weight).T
# Covars
covs = []
for i in range(dataMod.shape[0]):
data = dataMod[i, :]
covs.append(createCovMat(data))
loop = 0
while True:
# M Matrix
M = createMMat(dataMod, weight)
L = createLMat(dataMod, weight, theta, covs)
lamdas, v = np.linalg.eigh(M - L)
thetaOrg = theta
thetaNew = v[:, np.argmin(np.absolute(lamdas))]
#theta = (thetaNew + thetaOrg) / 2
theta = thetaNew
#theta = theta / np.linalg.norm(theta)
term = np.linalg.norm(theta - thetaOrg)
if term < 0.0001 or loop > 30:
break
for i in range(dataMod.shape[0]):
alp = theta.T * covs[i] * theta
weight[i, 0] = 1 / (alp)
loop = loop + 1
theta[3, 0] = normTerm * theta[3, 0]
theta[4, 0] = normTerm * theta[4, 0]
theta[5, 0] = normTerm**2 * theta[5, 0]
return theta
def plotData(dictDataPlot, dictDataScatter):
fig, ax = plt.subplots(ncols = 1, figsize=(10, 10))
plt.xlim(0, 10)
plt.ylim(0, 10)
for key in dictDataPlot:
ax.plot(dictDataPlot[key][:, 0], dictDataPlot[key][:, 1], linewidth=1, label=key)
for key in dictDataScatter:
ax.scatter(dictDataScatter[key][:, 0], dictDataScatter[key][:, 1], s = 2)
ax.legend()
def addGaussError(data, avg, stdv, absmax):
noise = np.random.normal(avg, stdv, data.shape)
noise = np.clip(noise, -(absmax + avg), absmax + avg)
dataWError = data + noise
return dataWError
if __name__ == "__main__":
# Importing Library for Testing.
import sys
sys.path.append('../')
from geometry_k.ellipse import generateVecFromEllipse
from geometry_k.ellipse import getEllipseProperty
print("Ellipse Fitting Sample")
# Define ellipse property.
axisX = 3
axisY = 3
centerX = 5
centerY = 5
rad = math.radians(45)
# Prepare argumetns.
axisOrg = np.matrix([[axisX], [axisY]])
centOrg = np.matrix([[centerX], [centerY]])
Rorg = np.matrix([[math.cos(rad), -math.sin(rad)], [math.sin(rad), math.cos(rad)]])
# Generating Data
dataOrg = generateVecFromEllipse(axisOrg, centOrg, Rorg)
dataLimited = generateVecFromEllipse(axisOrg, centOrg, Rorg, [-2*math.pi/6 * 1, 2*math.pi/6], 200)
# Add Noise
dataNoised = addGaussError(dataLimited, 0, 0.03, 10.1)
# Least Square Fitting
pLs = fittingEllipseWithLS(dataNoised, max(centerX + axisX, centerY + axisY))
print(pLs)
# Generate Estimated Ellipse Property
validLs, axisEstLs, centEstLs, TEstLs = \
getEllipseProperty(pLs[0, 0], pLs[1, 0], pLs[2, 0], pLs[3, 0], pLs[4, 0], pLs[5, 0])
# Iterative Reweight
pItr = fittingEllipseWithItrReweight(dataNoised, max(centerX + axisX, centerY + axisY))
#print(pItr)
# Generate Estimated Ellipse Property
validItr, axisEstItr, centEstItr, TEstItr = \
getEllipseProperty(pItr[0, 0], pItr[1, 0], pItr[2, 0], pItr[3, 0], pItr[4, 0], pItr[5, 0])
# Iterative Reweight
pFns = fittingEllipseWithFNS(dataNoised, max(centerX + axisX, centerY + axisY))
#print(pFns)
# Generate Estimated Ellipse Property
validFns, axisEstFns, centEstFns, TEstFns = \
getEllipseProperty(pFns[0, 0], pFns[1, 0], pFns[2, 0], pFns[3, 0], pFns[4, 0], pFns[5, 0])
# Taubin Method
pTb = fittingEllipseWithTaubin(dataNoised, max(centerX + axisX, centerY + axisY))
#pTb = fittingEllipseWithTaubin(dataNoised, 1)
print(pTb)
# Generate Estimated Ellipse Property
validTb, axisEstTb, centEstTb, TEstTb = \
getEllipseProperty(pTb[0, 0], pTb[1, 0], pTb[2, 0], pTb[3, 0], pTb[4, 0], pTb[5, 0])
# Estimating Data
dataEstLs = generateVecFromEllipse(axisEstLs, centEstLs, TEstLs)
dataEstItr = generateVecFromEllipse(axisEstItr, centEstItr, TEstItr)
dataEstFns = generateVecFromEllipse(axisEstFns, centEstFns, TEstFns)
dataEstTb = generateVecFromEllipse(axisEstTb, centEstTb, TEstTb)
dictData = {'Original' : dataOrg,\
'Least SQ' : dataEstLs,\
'Itr Weight' : dataEstItr,\
'FNS' : dataEstFns, \
'Taubin' : dataEstTb}
dictDataScatter = {'Noised Sample' : dataNoised}
plotData(dictData, dictDataScatter)
下記，当てはめの結果です．あと，上記のスクリプトに出てくる呼び出し関数は下記のエントリの関数を使ってます．ちなみに，グラフの原点 (0, 0) をまたぐような楕円をフィッティングさせようとすると桁落ちの問題が出てきて精度が出ません．これは理論的な問題というよりは計算上の問題です．なので，上記のスクリプトでは中心が (5, 5) の楕円で実験しています．
daily-tech.hatenablog.com
結果は，下記のようになります．一応，期待通り Taubin, FNS 法の結果が一番いいですね．
