最適化計算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