360度カメラの全天球画像ビューアーを作る　その３

　360度カメラで撮影した写真を任意の視点からの画像（パノラマ画像）へ変換するビューアーを作ってみようと思います。その最終回です。

　1.360度写真の平面投影
　 2.視点の変更と背面の投影
　3.視点の回転

　前回までの記事でひとまず適当な全天球画像からそれっぽい画像へ変換することはできました。

　今回はその続きです。画像をグルグルまわして、好きな角度の画像を切り出してみようと思います。

回転
3次元座標の回転
ロール・ピッチ・ヨー
Pythonで記述
結果
まとめと今後

回転

　前回までの記事で全天球画像専用のビューアーで見るような画像へ変換することができました。ただ現状はとある一視点だけです。専用ビューアーのように画像をグルグルまわしてみます。

　視点のイメージはコレ。球体に張り付けた全天球画像を球の中から観察するイメージです。

　画像をグルグルまわします。これを視点の位置を変えるのではなく、球上のxyzの座標をまわすことで代替します。

　視点を変えるのではなく、球に張り付いた画像の方をまわすイメージになります。視点は固定で周りの風景がグルグル回ることになります。

3次元座標の回転

　得られる2次元の画像座標が３次元のどこの座標から投影されているかは、前回までの記事のように比較的簡単に計算できます。

　視点と投影面の関係は

　このように定義します。視点をx軸上のとある座標$x_1$とし、撮像面の座標を$x_2$とします。そこに幅2wの面をおいて、そこに投影される球面の座標をxyzとします。この座標を回転させてみます。３次元なのでx,y,z軸それぞれに対して回転が定義できます。

　例えばy軸を回転軸として回転を考えてみます。図にするとこんな感じになります。

　視点とその向きはそのままに、球の方をこんなして回転させたときに得られる画像としては、首を上下に振ったようなものになります。

　上を見上げたり、下を見下げたり。

　xz平面で考えると単純な２次元の回転のアフィン変換です。y軸を回転軸にしているのでyの値は回転前後で変化しません。

　もっともらしい式で表現するとこんな感じになります。回転軸にあたるｙの成分は変化せず、ｘとｚの値がλだけ回転しています。

$$ \left(\begin{array}{c}\cos\lambda & 0 & \sin\lambda \\ 0 & 1 & 0 \\ -\sin\lambda & 0 & \cos\lambda\end{array}\right) \left(\begin{array}{c}x\\ y \\ z\end{array}\right) $$

　同じようにx軸を回転軸にするとこんな感じ。首を横に傾げたイメージ。

　同様に式は

$$ \left(\begin{array}{c}1 & 0 & 0 \\ 0 & \cos\lambda & \sin\lambda \\ 0 & -\sin\lambda & \cos\lambda\end{array}\right) \left(\begin{array}{c}x \\ y \\ z\end{array}\right) $$

　z軸を回転軸にするとこんなイメージ。首を横に振ったイメージ。

　式

$$ \left(\begin{array}{c}\cos\lambda & \sin\lambda & 0 \\ -\sin\lambda & \cos\lambda & 0 \\ 0 & 0 & 1 \end{array}\right) \left(\begin{array}{c}x\\ y \\ z\end{array}\right) $$

　この３つの回転行列を順番に掛け算することで任意の回転補正が可能になります。

　お手本にしているTHETAのビューアーだとx軸の回転はサポートされていないようです。カメラの水平は保たれていることを前提にしているようです。（もしくはジャイロセンサで取って勝手に補正しているのかも。）

ロール・ピッチ・ヨー

　何ぞ呪文のようですが、一般的に回転軸はロール・ピッチ・ヨーの名前で呼ばれているようです。カメラの手振れ補正装置、ジンバルでもそんな感じで呼ばれるのが一般的のようです。

　今回の回転軸で考えると、x軸がロール、y軸がピッチ、z軸がヨーになります。カメラの世界で使われているそれと同じ軸です。

Pythonで記述

　Pythonで記述してみます。詳細は過去の2記事を参考にしてもらえるとわかると思います。

import numpy as np
import cv2
 
##
# @brief 3次元座標群切り出し関数
# @details とある視点からとある距離の平面を通る半径１の球の表面を結んだ交点座標群を切り出す
# @param img_w 画像の幅（分解能）
# @param img_h 画像の高さ（分解能）
# @param senser_size センサに見立てた平面の大きさ（0以上の少数）
# @param view_point 視点位置
# @param senser_point 視点から平面までの距離
# @return x,y,zの座標群
def create_3dmap_from_viewpoint(img_w, img_h, senser_size, view_point, senser_point):
 
    senser_w = senser_size
    senser_h = senser_size * img_h / img_w
      
    x1 = view_point   # 視点の位置
    x2 = view_point + senser_point  #　撮像面の位置(必ず視点より前)
       
    w = np.linspace(-senser_w, senser_w, img_w, endpoint=False)
    h = np.linspace(-senser_h, senser_h, img_h, endpoint=False)
     
    # 配列を対称形にするためのオフセット
    w = w + senser_w / img_w
    h = h + senser_h / img_h
      
    # センサの座標
    ww, hh = np.meshgrid(w, h)
      
    # 直線の式
    a1 = ww / (x2 - x1)
    a2 = hh / (x2 - x1)
    b1 = -a1 * x1
    b2 = -a2 * x1
      
    a = 1 + a1**2 + a2**2
    b = 2 * (a1 * b1 + a2 * b2)
    c = b1**2 + b2**2 - 1
      
    d = (b**2 - 4*a*c) ** (1/2)
      
    # 球面上の3次元座標
    x = (-b + d) / (2 * a)
    y = a1 * x + b1
    z = a2 * x + b2
     
    return x, y, z
 
 
##
# @brief 3次元回転座標を取得する関数
# @details 3次元座標群を原点から指定された角度で回転させる
# @param x x座標群
# @param y y座標群
# @param z z座標群
# @param roll roll角（度単位）
# @param pitch pitch角（度単位）
# @param yaw yaw角（度単位）
# @return 回転後の3次元座標
def rotate_3d(x, y, z, roll, pitch, yaw):
 
    # 回転量
    roll = roll * np.pi / 180
    pitch = pitch * np.pi / 180
    yaw = yaw * np.pi / 180
     
    # 3次元の回転行列
    mtx1 = np.array([[1, 0, 0],
                     [0, np.cos(roll), np.sin(roll)],
                     [0, -np.sin(roll), np.cos(roll)]])
      
    mtx2 = np.array([[np.cos(pitch), 0, -np.sin(pitch)],
                     [0, 1, 0],
                     [np.sin(pitch), 0, np.cos(pitch)]])
      
    mtx3 = np.array([[np.cos(yaw), np.sin(yaw), 0],
                     [-np.sin(yaw), np.cos(yaw), 0],
                     [0, 0, 1]])
      
    # 回転行列の積
    mtx4 = np.dot(mtx3, np.dot(mtx2, mtx1))
     
    # 座標の行列計算
    xd = mtx4[0][0] * x + mtx4[0][1] * y + mtx4[0][2] * z
    yd = mtx4[1][0] * x + mtx4[1][1] * y + mtx4[1][2] * z
    zd = mtx4[2][0] * x + mtx4[2][1] * y + mtx4[2][2] * z
      
    return xd, yd, zd
 
 
##
# @brief 緯度経度群を取得する関数
# @details 半径1の球上の3次元座標群を緯度経度表記に変換する
# @param x x座標群
# @param y y座標群
# @param z z座標群
# @return 緯度、経度群（ラジアン）
def conv_equirectangularmap(x, y, z):
     
    # 緯度・経度へ変換
    phi = np.arcsin(z)
    theta = np.arcsin(np.clip(y / np.cos(phi), -1, 1))
     
    theta = np.where((x<0) & (y<0), -np.pi-theta, theta)
    theta = np.where((x<0) & (y>0),  np.pi-theta, theta)
 
    return phi, theta
 
 
##
# @brief 緯度経度情報にしたがって画像を変換する関数
# @details 1:2で格納されている画像から、そのx,y座標を緯度経度と見立てて対応する座標変換を行う
# @param img 元となる360度画像（1:2）
# @param phi 緯度（縦方向　ラジアン）
# @param theta 経度（横方向　ラジアン）
# @return 変換された画像
def remap_equirectangular(img, phi, theta, interpolation=cv2.INTER_CUBIC, borderMode=cv2.BORDER_WRAP):
     
    img_h, img_w = img.shape[:2]
     
    # 画像座標へ正規化（座標を画素位置に戻すため0.5オフセット）
    phi = (phi * img_h / np.pi + img_h / 2).astype(np.float32) - 0.5
    theta = (theta * img_w / (2 * np.pi) + img_w / 2).astype(np.float32) - 0.5
 
    return cv2.remap(img, theta, phi, interpolation, borderMode = borderMode)
 
img = cv2.imread("src.jpg")
x, y, z = create_3dmap_from_viewpoint(1920, 1080, 0.75, -1.2, 0.5)
x, y, z = rotate_3d(x, y, z, 45, 45, 45)
phi, theta = conv_equirectangularmap(x,y,z)
out = remap_equirectangular(img, phi, theta)
cv2.imwrite("dst.jpg", out)

　だいぶ長くなってきましたが、前回までのコードに対して、回転行列の演算が追加されただけです。なので今回のポイントは

# 回転量
roll = 45.0 * np.pi / 180
pitch = 45.0 * np.pi / 180
yaw = 45.0 * np.pi / 180
 
# 3次元の回転行列
mtx1 = np.array([[1, 0, 0],
                 [0, np.cos(roll), np.sin(roll)],
                 [0, -np.sin(roll), np.cos(roll)]])
 
mtx2 = np.array([[np.cos(pitch), 0, -np.sin(pitch)],
                 [0, 1, 0],
                 [np.sin(pitch), 0, np.cos(pitch)]])
 
mtx3 = np.array([[np.cos(yaw), np.sin(yaw), 0],
                 [-np.sin(yaw), np.cos(yaw), 0],
                 [0, 0, 1]])
 
# 回転行列の積
mtx4 = np.dot(mtx3, np.dot(mtx2, mtx1))

　この部分。任意の向きへ画像をぐるっとまわすために、ヨー・ピッチ・ロールの3つの角度を持つ行列の積をとっています。上記で説明したまんまのコードです。この3×3の回転行列を得られたxyzの3次元座標に対して乗じることで、座標を球面上回転させます。この例では適当にすべて45度まわしています。この行列を

# 座標の行列計算
xd = mtx4[0][0] * x + mtx4[0][1] * y + mtx4[0][2] * z
yd = mtx4[1][0] * x + mtx4[1][1] * y + mtx4[1][2] * z
zd = mtx4[2][0] * x + mtx4[2][1] * y + mtx4[2][2] * z