DualfisheyeからEquirectangularに変換を行う

RICOH THETA SのためのDualfisheyeからEquirectangularに変換を行うGstreamerプラグインの変換式を理解するため、xmap, ymapの計算を作り直し。

calcが元々、calc2が作り直したもの。

import numpy as np

def calc():
    COLS = 1280
    ROWS = COLS // 2
    xmap = np.zeros((ROWS, COLS), np.float32)
    ymap = np.zeros((ROWS, COLS), np.float32)
    DST_X = float(COLS)
    DST_Y = DST_X / 2
    SRC_CX1 = DST_X / 4
    SRC_CX2 = DST_X - SRC_CX1
    SRC_CY = DST_X / 4
    SRC_R = 0.884 * DST_X / 4
    SRC_RX = SRC_R * 1.00
    SRC_RY = SRC_R * 1.00
    #
    for y in range(COLS // 2):
        for x in range(COLS):
            ph1 = np.pi * x / DST_Y
            th1 = np.pi * y / DST_Y

            x1 = np.sin(th1) * np.cos(ph1)
            y1 = np.sin(th1) * np.sin(ph1)
            z1 = np.cos(th1)

            ph2 = np.arccos(-x1)
            th2 = (1 if y1 >= 0 else -1) * np.arccos(-z1 / np.sqrt(y1 * y1 + z1 * z1))

            if ph2 < np.pi / 2:
                r0 = ph2 / (np.pi / 2)
                xmap[y,x] = SRC_RX * r0 * np.cos(th2) + SRC_CX1
                ymap[y,x] = SRC_RY * r0 * np.sin(th2) + SRC_CY
            else:
                r0 = (np.pi - ph2) / (np.pi / 2)
                xmap[y,x] = SRC_RX * r0 * np.cos(np.pi - th2) + SRC_CX2
                ymap[y,x] = SRC_RY * r0 * np.sin(np.pi - th2) + SRC_CY
    
    return xmap, ymap

def calc2():
    COLS = 1280
    ROWS = COLS // 2
    xmap = np.zeros((ROWS, COLS), np.float32)
    ymap = np.zeros((ROWS, COLS), np.float32)
    DST_X = float(COLS)
    DST_Y = DST_X / 2
    SRC_CX1 = DST_X / 4
    SRC_CX2 = DST_X - SRC_CX1
    SRC_CY = DST_X / 4
    SRC_R = 0.884 * DST_X / 4
    SRC_RX = SRC_R * 1.00
    SRC_RY = SRC_R * 1.00
    
    # xx 長い方、yy 短い方
    xx, yy = np.meshgrid(range(0, COLS), range(0, COLS // 2))
    ph1 = np.pi * xx / DST_Y # 2piまで
    th1 = np.pi * yy / DST_Y # piまで

    x1 = np.sin(th1) * np.cos(ph1)
    y1 = np.sin(th1) * np.sin(ph1)
    z1 = np.cos(th1)
    
    ph2 = np.arccos(-x1)
    th2 = np.where(y1 >= 0, 1, -1) * np.arccos(-z1 / np.sqrt(y1 * y1 + z1 * z1))
    #th2 = (1 if y1 >= 0 else -1) * np.arccos(-z1 / np.sqrt(y1 * y1 + z1 * z1))
    
    flg = ph2 < np.pi / 2
    ph3 = np.where(flg, ph2, np.pi - ph2)
    th3 = np.where(flg, th2, np.pi - th2)
    cx  = np.where(flg, SRC_CX1, SRC_CX2)
    
    r0 = ph3 / (np.pi / 2)
    xmap = SRC_RX * r0 * np.cos(th3) + cx
    ymap = SRC_RY * r0 * np.sin(th3) + SRC_CY
    
    # if ph2 < np.pi / 2:
    #     r0 = ph2 / (np.pi / 2)
    #     xmap[y,x] = SRC_RX * r0 * np.cos(th2) + SRC_CX1
    #     ymap[y,x] = SRC_RY * r0 * np.sin(th2) + SRC_CY
    # else:
    #     r0 = (np.pi - ph2) / (np.pi / 2)
    #     xmap[y,x] = SRC_RX * r0 * np.cos(np.pi - th2) + SRC_CX2
    #     ymap[y,x] = SRC_RY * r0 * np.sin(np.pi - th2) + SRC_CY
    return xmap, ymap

if分が入るところは、ちょっとややこしいけど、違う部分をnp.whereで括りだせば、それほど式の理解に影響は与えないかな。。。人による？

%%timeit calc()
%%timeit calc2()

10.4 s ± 308 ms per loop (mean ± std. dev. of 7 runs, 1 loop each)
206 ms ± 31.2 ms per loop (mean ± std. dev. of 7 runs, 1 loop each)

速度は全然違うね！

xmap_a, ymap_a = calc()
xmap_b, ymap_b = calc2()
np.allclose(xmap_a, xmap_b), np.allclose(ymap_a, ymap_b)

(True, True)
もちろん同じ結果。

作り直したもの

dualfisheye.py

# Dual-fisheye to Equirectangular Converter using OpenCV remap
import sys
import numpy as np
import cv2

def calc_map(width, height):
    COLS = int(width)
    ROWS = int(height)
    DST_X = float(COLS)
    DST_Y = DST_X / 2
    SRC_CX1 = DST_X / 4
    SRC_CX2 = DST_X - SRC_CX1
    SRC_CY = DST_X / 4
    SRC_R = 0.884 * DST_X / 4
    SRC_RX = SRC_R * 1.00
    SRC_RY = SRC_R * 1.00
    
    # xx 長い方、yy 短い方
    xx, yy = np.meshgrid(range(0, COLS), range(0, COLS // 2))
    ph1 = np.pi * xx / DST_Y # 2piまで
    th1 = np.pi * yy / DST_Y # piまで

    x1 = np.sin(th1) * np.cos(ph1)
    y1 = np.sin(th1) * np.sin(ph1)
    z1 = np.cos(th1)
    
    ph2 = np.arccos(-x1)
    th2 = np.where(y1 >= 0, 1, -1) * np.arccos(-z1 / np.sqrt(y1 * y1 + z1 * z1))
    #th2 = (1 if y1 >= 0 else -1) * np.arccos(-z1 / np.sqrt(y1 * y1 + z1 * z1))
    
    flg = ph2 < np.pi / 2
    ph3 = np.where(flg, ph2, np.pi - ph2)
    th3 = np.where(flg, th2, np.pi - th2)
    cx  = np.where(flg, SRC_CX1, SRC_CX2)
    
    r0 = ph3 / (np.pi / 2)
    xmap = SRC_RX * r0 * np.cos(th3) + cx
    ymap = SRC_RY * r0 * np.sin(th3) + SRC_CY
    
    # if ph2 < np.pi / 2:
    #     r0 = ph2 / (np.pi / 2)
    #     xmap[y,x] = SRC_RX * r0 * np.cos(th2) + SRC_CX1
    #     ymap[y,x] = SRC_RY * r0 * np.sin(th2) + SRC_CY
    # else:
    #     r0 = (np.pi - ph2) / (np.pi / 2)
    #     xmap[y,x] = SRC_RX * r0 * np.cos(np.pi - th2) + SRC_CX2
    #     ymap[y,x] = SRC_RY * r0 * np.sin(np.pi - th2) + SRC_CY
    return xmap.astype(np.float32), ymap.astype(np.float32)



def convert_dualfisheye_to_equirectangular(frmae, xmap, ymap):
    return cv2.remap(frame, xmap, ymap, cv2.INTER_LINEAR, cv2.BORDER_CONSTANT)

if __name__ == '__main__':
    if len(sys.argv) < 3:
        print('Usage: Python3 dualfisheye.py <inputfile> <outputfile>')
        sys.exit(1)
    cap = cv2.VideoCapture(sys.argv[1])
    if not cap.isOpened():
        print('file not opened')
        sys.exit(1)
    fps    = cap.get(cv2.CAP_PROP_FPS)
    height = cap.get(cv2.CAP_PROP_FRAME_HEIGHT)
    width  = cap.get(cv2.CAP_PROP_FRAME_WIDTH)
    fourcc = cv2.VideoWriter_fourcc('m', 'p', '4', 'v')
    
    xmap, ymap = calc_map(width, height)
    out = cv2.VideoWriter(sys.argv[2], fourcc, fps, (int(width), int(height)))
    count = 0
    while(cap.isOpened()):
        ret, frame = cap.read()
        if ret == False:
            break
        cv2.imshow("in", cv2.resize(frame, None, fx=0.3, fy=0.3))
        
        frame = convert_dualfisheye_to_equirectangular(frame, xmap, ymap)
        cv2.imshow("out", cv2.resize(frame, None, fx=0.3, fy=0.3))
        out.write(frame)
        cv2.waitKey(1)

        if count % 30 == 0:
            print('.', end="")
            sys.stdout.flush()
        count += 1

    print()
    cap.release()
    out.release()
    cv2.destroyAllWindows()