関数の最適化 ~勾配法~

バンドル調整に向けて,数学の復習をしておきます.参考文献はいつものごとく金谷先生の本です.

最適化とは

ある制約条件のもとで,ある関数の値を最大または最小にするような変数の値を求めること.

最大値・最小値を求めるためのトリック

ニュートン法,勾配法,レーベンバーグ・マーカート法等,非線形最適化をするアルゴリズムはたくさんありますが,基本となっている考え方は,,,

関数 f(x) の極値では,一次導関数 f'(x) = 0 となる.

です.結局アルゴリズムが違っても,f'(x) = 0 となるような x を探しているという点では同じです.なので,陰関数 f(x) = 0 を解くことができるツールであれば,最適化にも使えると思ってよいと思います.最適化の場合,解くべき陰関数が関数 f(x) ではなく,一次導関数 f'(x) となります.

一変数の場合の勾配法

まず導入として,一変数関数の場合の勾配法を考えます.基本となる考え方はシンプルで,上に凸の関数の場合,勾配がプラス(マイナス)の間はずっと x を増加(減少)させます.上に凸の関数であれば,この作業を続けていればどこかで必ず勾配がマイナス(プラス)になります.この間のどこかに勾配が0である場所があると考えられるので,これを求める最適解として採用します.
f:id:rkoichi2001:20180503161215j:plain

一変数の場合の勾配法アルゴリズム

最大値を求める場合の勾配法アルゴリズムは下記のとおりです.傾きの符号が変化するまで同一方向に進み,符号変化を検知した時点で進む距離(ステップ幅)を減らして反対方向に進む.という作業を繰り返し,最後には f'(x) = 0 を満たす変数を求めます.
f:id:rkoichi2001:20180503164237j:plain

多変数の場合の勾配法

少し複雑になりますが,多変数の場合も考え方は同じです.多変数の場合も曲面に対して接平面を考え,この接平面の傾きが一番大きい方向を基準に進んでいきます.で,この方向に進んでいく中で一番大きい値を探索します.最大値を見つけたら,その点に関して接平面を考え再び傾きが一番大きい方向を基準に進んでいき....と繰り返せば最大値が求まります.
f:id:rkoichi2001:20180503172209j:plain

多変数の場合の勾配法アルゴリズム

f:id:rkoichi2001:20180503172250j:plain

一変数の勾配法の実験 with Python

ということで,勾配法を実装してみました.まずは一変数の場合の勾配法です.最大値を求めたい関数として,下記の関数を考えます.

f(x) = sin(x) + cos(x/2 -1)
一変数の勾配法による実験結果

下記実験結果ですが,緑色の点が反復の途中仮定,赤色の点が最終結果になります.最大値の回りで反復が終わっていることがわかります.
f:id:rkoichi2001:20180504001838p:plain

多変数の勾配法の実験 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)
多変数の勾配法による実験結果

下記実験結果ですが,楕円のてっぺんが最大値になります.各点で等高線に垂直な方向に進み,最終的には最大値にたどり着いている様子がわかります.緑色の点が途中経過で,黄色の点が最終解です.
f:id:rkoichi2001:20180504060114p:plain

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()

参考文献

これなら分かる最適化数学―基礎原理から計算手法まで

これなら分かる最適化数学―基礎原理から計算手法まで

最適化計算5 バンドル調整 理論編1

長いこと最適化の勉強をやってきましたが,やっとこさバンドル調整まで来ました.ここまでくれば,三次元復元を理解するのに必要な道具はある程度勉強したといえるのではと思います.

バンドル調整とは

三次元シーンを複数のカメラ・視点で撮影した時,全体的に「一番つじつまが合っている.」構成を求めるための手法です.ここで,「つじつまが合う」というのが何なのかを定義してやる必要があります.三次元復元の場合,「再投影誤差を小さくする.」ということがそれに該当します.

問題設定のイメージ

問題設定のイメージですが,下記のようになります.
f:id:rkoichi2001:20180502235639j:plain

で,それぞれのカメラに関して,三次元座標とスクリーンの投影座標には透視投影の関係が成立します.
f:id:rkoichi2001:20180503001832j:plain

ここで,三次元復元をするときには基準座標(世界座標系)を決めてあげないといけません.今回はカメラ0の位置を基準座標として設定します.次に,それぞれの座標系であらわされている三次元座標を世界座標系(=カメラ0)を基準座標として表します.各座標系の関係は下記で表せます.
f:id:rkoichi2001:20180503003405j:plain

各カメラで成立する透視投影の関係に,もう一工夫施して,世界座標で記述します.
f:id:rkoichi2001:20180503011717j:plain

これで,カメラ行列が与えられました.次に,このカメラ行列が与えられたときに,三次元点 p = (Xα, Yα, Zα)T がスクリーンのどこに投影されるかということを考えてみます.
f:id:rkoichi2001:20180503013005j:plain

上式より,「カメラ行列,三次元点 p の世界座標が与えられたとき,該当カメラの画像面のどこに投影されるか」ということがわかりました.

最小化したい評価関数・誤差関数

ここでは評価関数を設定してあげないといけません.いつものごとく,ここでも再投影誤差を使います.
f:id:rkoichi2001:20180503014127j:plain

上記の評価関数で,わかっている変数って3Dの点の各カメラでのスクリーン投影座標 (xαk, yαk) だけなんですよね.なので,「3Dの点の世界座標」「カメラの内部パラメータ(焦点距離,主点)」「カメラ外部パラメータ(位置,回転)」は,この評価関数を最小化するうえで更新されていきます.

更新対象となる変数

最小化対象となる評価関数ははっきりしましたが,実際に更新対象となる未知数をリストアップしてみます.
1.三次元点の世界座標系での位置 ΔXα = (ΔXα, ΔYα, ΔZα)T -> 三次元点の個数 : N x 3
2.カメラの焦点距離 Δfk -> カメラ(視点)の個数 : M
3.カメラの主点 (Δuk, Δvk) -> カメラ(視点)の個数 : M x 2
4.カメラの世界座標系での位置 Δtk = (Δtxk, Δtyk, Δtzk)T -> カメラ(視点)の個数 : M x 3
5.カメラの世界座標系での回転 Δωk = (Δωxk, Δωyk, Δωzk)T -> カメラ(視点)の個数 : M x 3

すべてを足しこむと,未知数の数は 3N + 9M となります.ただし,どこかの視点を基準にしてあげないといけません.(今回の場合はカメラ0を基準にしようとしてます.)そうすると,カメラ一つ分の位置・回転に関する未知数(合計6個)が減ります.また,画像を用いた三次元復元ではスケールが不定なので,カメラ0とどこかのカメラの距離を1として決め打ちで設定してあげる必要があります.これを鑑みると,最終的に未知数の数は下記のようになります.

3N + 9M - 7

例えば,一つの画像からとれる特徴点を100個として,写真を100枚撮影したとすると,未知数が1193個.おおいですね...長くなってしまったので,アルゴリズムの導出編は次のエントリにします.

最適化計算3 基礎行列の推定 実験編

下記のエントリで基礎行列計算の理論編を勉強しました.ということで,いつものごとくPythonを使って実験です.
daily-tech.hatenablog.com

問題設定

下記のように二次曲面の形状を設置し,この物体を複数の視点に置いたカメラで撮影する状況を考えます.
f:id:rkoichi2001:20180430142030j:plain

対称物体の投影図

OpenCVのProjectPoints関数を用いて,上図の三視点に置かれたカメラで物体を撮影します.

f:id:rkoichi2001:20180430143400p:plainf:id:rkoichi2001:20180430143331p:plainf:id:rkoichi2001:20180430143414p:plain

OpenCVのProjectPoint関数の定義は下記のようになっており,物体の座標系で点を指定することができます.物体の座標系とカメラの座標系の関係を tvecs, rvecs に指定してあげれば,入力点群が自動的にカメラ座標系に変換され,そののちに画像に投影されます.

cv2.projectPoints(pntsInObj, rvec2, tvec2, cameraMatrix, distCoeffs)

一点注意点としては,この時の rvecとtvecですが,下記数式の t と R (を Rodrigues形式に変換したもの) を渡してあげる必要があります.つまり,物体座標系からカメラ座標系への変換式の回転要素 R と並進要素 T を渡してあげればOKです.
CAMXP = CAMX t + CAMX R OBJOBJP

基礎行列の計算

今回の実験では,図の CAM1 と CAM2 の位置関係(既知)を使って基礎行列真値を計算します.そのあとに2画像の投影点ペアを用いて基礎行列を計算し,真値にたいしての誤差が計算方法によってどの程度変動するかを調べます.まず,既知の位置関係を使って,エピポーラ拘束式が満たされていることを確認します.二つの視点を図示したものが下記になります.
f:id:rkoichi2001:20180430153250j:plain

最後のスクリプトを実行すると,下記の結果が得られてエピポーラ拘束条件が満たされていることがわかります.

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
  

最適化計算3 基礎行列の推定 理論編

最適化計算のエントリ第三弾ということで,ようやく基礎行列の話題まで来ました.参考文献はいつもの通り金谷先生の本です.

基礎行列・基本行列とは

下記のエントリにも書いた通り,3次元上の点を異なる二視点から撮影した時,点の二つの投影画像座標が満たす数式があります.この拘束条件をエピポーラ拘束といいますが,エピポーラ拘束式に出てくる F を基礎行列,E を基本行列といいます.今回のテーマでは,二つの画像の対応点を使って,この F を求めます.
daily-tech.hatenablog.com

基礎行列の計算

エピポーラ拘束より,下記の数式が成立します.
f:id:rkoichi2001:20180429141317j:plain

今回のエントリでは,上記エピポーラ方程式の 3 x 3 行列である F を計算します.
f:id:rkoichi2001:20180429141718j:plain

DLT法が使えるように,線形方程式に変換

楕円の当てはめエントリでも実施しましたが,線形方程式を立式するところは結局同じアプローチになります.
f:id:rkoichi2001:20180429143438j:plain

式展開の最後で,エピポーラ方程式を単純な内積の形に表せました.楕円の当てはめの時と同じように,すべての点ペアの内積和が最も小さくなるように F 行列の 9個の要素を決定するという問題になります.ここで一点注意ですが, F 行列は定数倍の自由度があるのでノルムを 1 であると勝手に決めて正規化します.ここで,このノルムを下記のように定義します.
f:id:rkoichi2001:20180429144316j:plain

共分散行列と代数的方法(最尤推定

楕円当てはめの時と同じように,式の構造を意識しながら誤差を考えてみます.楕円当てはめの時と同じように,線形化している関係上,入力ベクトル(ζ)が (x, y) の2変数ではなく9変数あるように見えますが,これは方程式を線形化する過程で意図的に作ったもです.なので,結局誤差が発生するのは x, y となります.あとの 7 項の誤差成分に関しては x, y の誤差を適切に仮定してあげれば計算から出てきます.
f:id:rkoichi2001:20180429164656j:plain

上式のΔ項を確率変数としてζの定義に入れて式展開し,誤差の次数に応じて数式を分割して書くと下記のようになります.
f:id:rkoichi2001:20180429165809j:plain

誤差の第二項は十分小さいとして無視すると,誤差分布は誤差の第一項だけで決まります.確率変数4つがそれぞれ独立で,期待値0,分散σの正規分布に従うとすると,共分散行列は下記のようになります.
f:id:rkoichi2001:20180429174312j:plain

共分散行列が求まったので,楕円の当てはめの時と同じように, Taubin法,重み反復法等を使って基礎行列を求めることができるようになります.楕円当てはめの時とアルゴリズムはまったく同じですが一応メモ程度に書いておきます.

最小二乗法

f:id:rkoichi2001:20180429175848j:plain

重み反復法

f:id:rkoichi2001:20180429175945j:plain

Taubin法

f:id:rkoichi2001:20180429180421j:plain

ランク拘束

ここまでで,基礎行列を求めることは形的に楕円当てはめの問題と同じになることがわかりました.が,上で説明した方法では,誤差のある点群からは正しく基礎行列が求まりません.この点が「楕円当てはめ」と「基礎行列計算」の違いになります.それは,基礎行列 F はランクが 3 ではなく,2 である必要があるためです.これをランク拘束と呼びます.上の方法では,誤差のある点群に対して基礎行列を計算すると,求まる解がランク3になってしまいます.ということで,このランク拘束を満たす必要があるのですが,このやり方で手法が大きく3つに大別されます.

1.事後補正法(ランク補正)

 ひとまずランク拘束を考慮せず θ を計算し,次にランク拘束を満たすように補正する方法.

まず,一番簡単な特異値分解によるランク補正法です.この方法では,一番小さい特異値を無理矢理0に変えます.
f:id:rkoichi2001:20180429231233j:plain

特異値によるランク補正では一番小さい特異値を無理矢理0に変えましたが,これをもっと最適に補正する方法が次の方法です.
f:id:rkoichi2001:20180429234035j:plain

2.内部接近法

 初めから基礎行列をランク2となるようにパラメータ化し,そのパラメータ空間内で J を最小にする基礎行列を求める方法です.
ちょっと書くのに疲れたので,あとでアップします....

3.外部接近法

 適当な初期値から出発し,サンプソン誤差 J を最小にするために反復を重ねつつ,かつランク拘束も満たすように収束させる方法です.
f:id:rkoichi2001:20180430000939j:plain

と,,,いうことで,なんだか内容が難しくなってきましたが,実験編に移ります!

参考文献

参考文献1

3次元コンピュータビジョン計算ハンドブック

3次元コンピュータビジョン計算ハンドブック

参考文献2
画像の三次元理解のための最適化計算[Ⅲ]

レイリー商を用いた最小値・最大値問題の解法

レイリー商と呼ばれる下記の数式を最小化することを考えてみます.
f:id:rkoichi2001:20180422170958j:plain

ここで,Aは実対称行列としているので,実数固有値が存在します.直交行列 Q によって A をスペクトル分解したとすると,上記レイリー商の定義式は下記のように変形できます.
f:id:rkoichi2001:20180422173051j:plain

変形後のレイリー商の分子に注目してほしいんですが,最小固有値を λ1,最大固有値を λnとすると分子の値はそれぞれ二つの固有値によって下限・上限が決まることがわかります.
f:id:rkoichi2001:20180422173819j:plain

分子項の下限・上限がはっきりしたので,これをもとにレイリー商を計算すると,
f:id:rkoichi2001:20180422175158j:plain

この問題の見方を少し変えると,条件付き最小値・最大値問題が簡単にとけます.例えば,下記のような問題を考えます.
f:id:rkoichi2001:20180422180233j:plain

上記の例のように,条件付き最小値・最大値問題が簡単にとけるようになります.この考え方を使って最適化問題を解いていきます.

最適化計算2 楕円の当てはめ 実験編

下記のエントリで楕円の当てはめの理論的な部分のエントリをしましたが,実際にパイソンのスクリプトを作って実験しました.
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 法の結果が一番いいですね.
f:id:rkoichi2001:20180429162620p:plain

python を使った楕円のプロットの仕方

別エントリで楕円の当てはめのスクリプトを書いているのですが,どうも楕円のプロットのさせ方がいまいちピンと来なかったのでまとめます.ここで最終的に解決しようとしている問題は

楕円の方程式として f(x, y) = 0 が与えられたときに,どうやって Python を使ってプロットするか?

です.

1.楕円の方程式

中心が (0, 0),長軸の長さ 2a,短軸の長さ 2b の楕円の方程式は下記のように定義されます.
f:id:rkoichi2001:20180408113641j:plain

上記の式をみてもわかるように,y = f(x) の形ではなく,f(x, y) = const の形になっています.そのため,直線をプロットするときのように x を一定の範囲で変化させてその時の y の値を都度プロットするという形をとれません.これでは不便です...

2.楕円の中心が原点で,かつ長軸・短軸が x 軸と y 軸に平行な場合

このケースが一番単純な場合になります.この場合,数式は下記のような形になります.
f:id:rkoichi2001:20180410061525j:plain

媒介変数表示を用いたプロット

基本式のままではプロットするときに不便なので,媒介変数表示を使います.結局のところ方程式を満たすような変数表現を見つければいいだけなので,下記のように表現できます.
f:id:rkoichi2001:20180408114239j:plain

これで,t の値を 0 -> 2pi まで変化させたときの x と y の値を単純にプロットしていけば楕円のプロットができるようになります.下記,サンプルコードとその結果です.

import matplotlib.pyplot as plt
import numpy as np
import math

def generateVecFromEllipse(a, b):
  
  t = np.linspace(0, 2 * math.pi, 101) 
  t = np.reshape(t, (t.shape[0], 1))
  
  xVec = np.zeros((t.shape))
  yVec = np.zeros((t.shape))
  for i in range(t.shape[0]):
    xVec[i] = a * math.cos(t[i])
    yVec[i] = b * math.sin(t[i])
  
  data = np.concatenate((xVec, yVec),  axis=1)
  return data
  
def plotData(dataOrg):
  
  fig, ax = plt.subplots(ncols = 1, figsize=(10, 10))
  plt.xlim(-10, 10)
  plt.ylim(-10, 10)
  ax.plot(dataOrg[:, 0], dataOrg[:, 1])
 
if __name__ == "__main__":
  print("Ellipse Drawing Sample")
  
  # 1. Generating Ellipse Point
  dataEl = generateVecFromEllipse(3, 2)

  # 2. Plot Data
  plotData(dataEl)
  
プロット結果

f:id:rkoichi2001:20180408114841p:plain

3.長軸・短軸が座標軸に対して角度を持つ場合

で,問題はここからです.上記でプロットした楕円は中心が原点にピッタリそろっていて,長軸・短軸が座標2軸に対して平行ですが,こんな理想的な状況はほとんどありえません...よって,ここからは f(x, y) = const の楕円の方程式が与えられたときに,プロットする方法を説明します.まず,長軸・短軸に角度がある場合です.この場合,数式は下記のようになります.
f:id:rkoichi2001:20180410063054j:plain

この場合,楕円をプロットしたいときにはいくつかのステップを踏む必要があります.
1.楕円の長軸・短軸が何度傾いているか調べる.
2.楕円の長軸・短軸の長さを調べる.
3.一番基本となる楕円のプロット(2)で紹介した方法で,2で取得した長軸・短軸の楕円を描画.
4.3で作成した点群を1で取得した角度で回転させる.

上記の4ステップを一つづつ見ていきます.
1.楕円の長軸・短軸が何度傾いているか調べる.
ここでは,長軸・短軸の角度を求めます.一番簡単な方法は二次形式の係数行列 A をスペクトル分解する方法です.
f:id:rkoichi2001:20180410074520j:plain
直交変換行列 T が回転をあらわしているので,これが回転角になります.

2.楕円の長軸・短軸の長さを調べる.
1の結果より,下記のことが言えます.
f:id:rkoichi2001:20180410064749j:plain

3.一番基本となる楕円のプロット(2)で紹介した方法で,2で取得した長軸・短軸の楕円を描画.
基本となる楕円プロットと同じです.

4.3で作成した点群を1で取得した角度で回転させる.
1より求まったθを用いて,生成点群を回転させます.

プロット結果(最後のスクリプトを使って生成してます.)

f:id:rkoichi2001:20180410074119p:plain

4.長軸・短軸が座標軸に対して角度を持ち,かつ楕円の中心が原点でない場合.

最後のパターンです.二次元平面に書かれる楕円をすべて表現するにはこの方程式が必要です.導出は3とほぼ同じですが,点の変換に平行移動が入ってきます.つまり,,,
f:id:rkoichi2001:20180410075258j:plain

この場合も3の場合とほぼ同じですが,一次の項を平方完成する作業が増えます.
1.楕円の長軸・短軸が何度傾いているか調べる.
2.楕円の長軸・短軸の長さを調べる.
3.スペクトル分解で取得した回転行列を代入する.
4.一次の項を平方完成させ,原点からの平行移動ベクトル (a, b) を求める.
5.一番基本となる楕円のプロット(2)で紹介した方法で,2で取得した長軸・短軸の楕円を描画.
6.3で作成した点群を1で取得した角度で回転させる.
7.6で作成した点群を4で取得したベクトル分移動させる.
f:id:rkoichi2001:20180410080626j:plain

上記より,楕円の一般式が求まりました.下記スクリプトです.

# -*- coding: utf-8 -*-

from sympy import *
import sympy
import numpy as np
import matplotlib.pyplot as plt
import math

def generateVecFromEllipse(axis, center, T, rng=[0, 2*math.pi], num=101):
 
  t = np.linspace(rng[0], rng[1], num) 
  t = np.reshape(t, (t.shape[0], 1))
  
  xVec = np.zeros((t.shape))
  yVec = np.zeros((t.shape))
  for i in range(t.shape[0]):
    xVec[i] = axis[0, 0] * math.cos(t[i])
    yVec[i] = axis[1, 0] * math.sin(t[i])
  
  dataTmp = np.concatenate((xVec, yVec),  axis=1)
  dataTmp = T * dataTmp.T
  dataTmp = dataTmp.T
  dataTmp[:, 0] =  dataTmp[:, 0] + center[0, 0]
  dataTmp[:, 1] =  dataTmp[:, 1] + center[1, 0]
  
  return dataTmp

def generateEllipseEqn(axis, center, T):
  
  # 1. Creating Vector with Symbol
  x_ = Symbol('x_')
  y_ = Symbol('y_') 
  x_vec = np.matrix([[x_ - center[0, 0]], [y_ - center[1, 0]]])
  
  # 2. Define Original Ellipse Matrix
  # No tilt
  ElMat = np.matrix([[1/axis[0, 0]**2, 0], [0, 1/axis[1, 0]**2]])
  
  # 3. Rotate Coordinate
  Tx_ = T.T * x_vec

  # 4. Expand Ellipse Eqn.
  fn = sympy.expand((Tx_.T * (ElMat * Tx_))[0, 0] - 1)
  
  A = float(fn.coeff(x_, 2))
  B = float(fn.coeff(x_*y_, 1))
  C = float(fn.coeff(y_, 2))
  D = float(fn.subs([(y_, 0)]).coeff(x_, 1))
  E = float(fn.subs([(x_, 0)]).coeff(y_, 1))
  F = float(fn.subs([(x_, 0), (y_, 0)]))
  
  print("The ellipse eqn you are looking for is :")
  print(fn)
  return A, B, C, D, E, F
  
def getEllipseProperty(A, B, C, D, E, F):
  
  if A < 0:
    A = -A
    B = -B
    C = -C
    D = -D
    E = -E
    F = -F   
    
  # Spectral Decomposition
  M = np.matrix([[A, B/2], [B/2, C]])
  lamdas, v = np.linalg.eigh(M)
  
  # Diagonalize Coeffs Matrix
  DiagA = v.T * M * v  
  
  # Apply translation for 1st order term.
  tmp = np.matrix([D, E]) * v
                  
  # Calculate coefficient in rotated coords
  AA = DiagA[0, 0]
  BA = DiagA[0, 1] + DiagA[1, 0]
  CA = DiagA[1, 1]
  DA = tmp[0, 0]
  EA = tmp[0, 1]
  scale = F - DA**2 / (4*AA) - EA**2 / (4*CA)
  
  # Normalize coeffs wrt to constant term.
  AA = AA / abs(scale)
  BA = BA / abs(scale)
  CA = CA / abs(scale)
  DA = DA / abs(scale)
  EA = EA / abs(scale)
  FA = abs(scale) / abs(scale)
 
  # Ellipse Property Extraction
  a = 1 / math.sqrt(abs(lamdas[0] / scale))
  b = 1 / math.sqrt(abs(lamdas[1] / scale))
  
  T = np.matrix([[v[0, 0], v[0, 1]], [v[1, 0], v[1, 1]]]) 
  trans = v.T * np.matrix([[-DA/(2*AA)], [-EA/(2*CA)]])
 
  valid = True
  if AA * CA < 0:
    valid = False
  
  return valid, np.matrix([[a], [b]]), trans, T
  
def plotData(dataOrg, dataEst):
  
  fig, ax = plt.subplots(ncols = 1, figsize=(10, 10))
  plt.xlim(-10, 10)
  plt.ylim(-10, 10)
  ax.plot(dataOrg[:, 0], dataOrg[:, 1])
  ax.plot(dataEst[:, 0], dataEst[:, 1])
 
if __name__ == "__main__":
  
  print("Ellipse Drawing Sample")

  # Define ellipse property.
  axisXOrg = 3
  axisYOrg = 1
  centerX = 5
  centerY = 3
  rad = math.radians(120)
  
  # Prepare arguments.
  axisOrg = np.matrix([[axisXOrg], [axisYOrg]])
  Rorg = np.matrix([[cos(rad), -sin(rad)], [sin(rad), cos(rad)]])
  centOrg = np.matrix([[centerX], [centerY]])
  
  # 0. Generating Data and Elilpse Equation Coefficient 
  dataOrg = generateVecFromEllipse(axisOrg, centOrg, Rorg)  
  A, B, C, D, E, F = generateEllipseEqn(axisOrg, centOrg, Rorg)
  
  # 1. Extract Elilpse Property from Egn
  valid, axis, centerEst, Rest = getEllipseProperty(A, B, C, D, E, F)
   
  # 2. Generating Ellipse Point and Rotate
  dataEst = generateVecFromEllipse(axis, centerEst, Rest) 

  # 3. Plot Data
  plotData(dataOrg, dataEst)
  
    

実際に実行した結果が下記のようになります.青の楕円がオリジナルで,緑の楕円が楕円の方程式から上記スクリプトを使って生成した点群です.結局同じ楕円なので,きれいに重なります.
f:id:rkoichi2001:20180414100753p:plain