random dot式のStructured Lightの考察

Python

はじめに

本ブログでは、過去にFMCWやdToFなどの測距方式についてまとめてきた。

一方で、構造化光あるいはストラクチャードライト(以下、SL)についてはあまり触れてこなかった。というのも、製品的にはiPhoneをはじめとした製品群の顔認証に採用され、幅広く利用されているし、ある程度成熟した技術といえる。実際にiPhoneのSLを使ったベンチマークの論文(例えば、博物館の所蔵品スキャンJerzy2026)なども存在する。

また、原理的にも結構シンプルで解説記事などが多いため、わざわざ自分が書く気が起きなかった。しかし、自分で検討している中で一つ分かったことがあった。

それは、計算の中身があまりオープンになっていないことだ。MSSLS joint calibrationといったキャリブ手法を書いた論文のソースコードはオープン(githubリンク)になっているが、ブログレベルで平易に解説したものはあまりない。

そこで本記事では、実際のソースコードを示したうえで、実際にOpen CVの計算ママではむずかしった点について追加していこうと思う。

SLの原理

図1. SLの原理

図1にSLの原理図を示す。SLはいくつか手法があるが、今回紹介するのはramdom dot手法のStrctured Lightである。他のSLに興味がある方は例えばturtorial(jason2011)とかを読んでみてほしい。

基本的な計算原理は簡単で、投光機(Tx)の位置を\(x=0\)、受光機(Rx)の位置を\(x=l\)。基準面までの距離を\(Z_0\)、箱上面までの距離を

\begin{equation}
Z_1 = Z_0 + \Delta z
\end{equation}

とする。Txから出るある一本の光線が、基準面では\(x_0\)にあたる(図中の”〇　基準面上の投影点”)としたら、箱面上ではずれた\(x_1\)にあたり(図中の”●　物体面上の投影点”)、この位置は三角形の相似の関係から

\begin{equation}
x_1 = x_0 \frac{Z_1}{Z_0} = x_0 \frac{Z_0-\Delta z}{Z_0}
\end{equation}

と導ける。また

\begin{equation}
\Delta u_{Tx} = x_1 – x_0
\end{equation}

また、受光側はピンホールモデルと見做す(参考：ピンホールカメラモデル)と、Rxの画像位置を\(u\)、受光機のレンズ焦点距離を\(f\)とすると

\begin{equation}
u = f\frac{x-l}{Z}
\end{equation}

となる。ここで、受光機の\(z\)座標は\(l\)であることを利用した。基準面と物体面との画像上の変化\(du_{Rx}\)は

\begin{equation}
\Delta u_{Rx} = f\frac{x-l}{Z_0}- f\frac{x-l}{Z_1}
\end{equation}

となる。ここまでの計算結果から物体上の投影点と仮想基準点(点線〇)の移動量\(\Delta u\)は

\begin{equation}
\Delta u = \Delta u_{Tx} + \Delta u_{Rx}
\end{equation}

と導ける。この移動量\(\Delta u\)は距離\(Z \)の関数で表すことができるので、この移動量を求めることで測距が可能となる。この移動量のことを視差と呼ぶ。また、視差の方向についてはepipolar方向と呼び、epipolar幾何学(参考:【コンピュータビジョン】ネコと学ぶエピポーラ幾何)にしたがって決定される。大雑把に言えば投光機と受光機が結ぶ直線に平行な方向がSLにおけるepipolar方向となる。

視差の求め方

ここまでで、視差というパラメータがわかれば、距離が計算できるといったことを示した。では、この視差を画像上でどうやって判断するかといった計算手法について解説する。

これには、テンプレートマッチングといった手法で画像上での位置ずれを計算する。実際にこのテンプレートマッチングでは様々な手法(参考: 画像認識の手法「テンプレートマッチング」の仕組みなどがわかりやすい)があるが、SLではどの計算式がいいかといわれると一概には言えない。(Censusがよりロバストな測定になるといった報告はある。mingyu2022)

そこで、本記事は一例としてSADといった計算例で解説を行う。

Sum of Absolute Difference (SAD)

図2. random dotによる視差計算例

Sum of Absolute Diffrence (SAD)の解説に入る前に、SLの原理なところを少し深堀する。前述した通り、ある距離離れたとき投影するdotは視差分移動をする。ここでまずは簡単のために、平行平面にdotを投影した時(図2(c))を考える。

投光機(Tx)を用いて平面にdotを投影し、ある距離を基準としてキャリブレーションしたときのパターン画像を図2(a)としたときに、ある距離\(Z_1\)離したパターンを受光機(Rx)で受光したときの画像を図2(b)で表す。

このとき、平行平面への投影なため、すべてのdotは同じ視差分だけ移動する。

これを画像ベースで理解するのは難しいため、次のような疑似的なバイナリパターンを考える。

図3. 簡易的な視差画像の再現図
実際の画像(a),(b)の上下左右にはパターンが広がっている

図3の元画像(a)に一致するドットパターンが視差画像中(b)のどこに含まれているかを解いていく。このとき、ある3候補で検討を行ってみる。回答からいうと、一見してわかる通り”候補2″が元画像と一致しており、視差としては3画素分というのが答えになる。では、これを定量的にどのように考えればよいのかを説明していく。

ここで、候補のうちどれがもっとも元画像に近いかを考えてみる。この時の考え方として、SADという考え方がある。これを定性的に説明する。

図4. SADの概念図

図4に示すように、それぞれの候補に対して元画像の分を引いた差分画像を作り出す。例えば、元画像と候補が像の画素値がともに1であるときに差は0となる。一方、元画像/候補の片方のみが0で、片方が1のときに差は±1となる。今回重要なのはあくまで、”違い”が重要なため、±のどちらでもよく、また、計算上打消しが発生しないように絶対値をとる。これらを合計して、どの程度違うかを計算する。

この考え方をSADといい、これを一般的な形で書くと次式で表すことができる。

\begin{equation}
R_{SSD} = \sum_{j=0}^{h-1} \sum_{i=0}^{w-1} | I_{parallex}(x+i,y+j) -I_{original}(i,j) |
\end{equation}

ここで\(h,w\)は縦横の窓サイズで今回の場合は\(h=w=2\)、\(I_{parallex}\)は視差画像の画素値で\(I_{original}\)は元画像の画素値となる。

今回の例では画素値を0と1で表しているため、画素値が一致していれば0、不一致なら絶対値をとって1となる。

したがって、今回の例示の場合はSADを簡単に言うと「元画像と異なっている画素数」と言い換えることができる、

そこで各画素のSAD(差分画像の絶対値の合計値)を見てみると候補1,3は8,5となり、異なる画素をいくつも持っていることがわかる。候補2は0となり異なる画素値はないと言い換えることができる。

今回の例の場合は、SADが最小となる候補は2である。元画像の位置を候補3とすると3画素分左にずれていることがわかる。よって、今回の視差\(\Delta u\)は

\begin{equation}
\Delta u = 3
\end{equation}

となる。このように、元画像のパターンと視差画像内の候補領域を少しずつずらしながらSAD計算し、SADが最小となる位置を探索することで画像内の視差を定量的に求めることができる。

また、画像全体に対してSADを求めていては画像の局所ごとの距離がわからない。そのため、通常はある窓サイズ内で視差計算をおこない、その窓に対応した位置の距離を計算する。

実際の実装

正直、ここが一番つまづいたところである。我らが大正義openCVには、Structured Light APIにはGray code式と位相シフト縞式の2種類しかなく、今回行いたいrandom dot式のAPIは存在しない。(もし、異なっている場合はコメントいただけますと幸いです。)

また、ひとつ前のレイヤーに戻ってテンプレートマッチングで楽ができないかと考えたがOpen CVのreferenceを確認してみても、全画素(厳密には窓サイズを差し引いた分)に対してテンプレートマッチングをしてしまう。

SLの場合は、epipolar方向にしか視差は発生しないため、探索画素はepipolar方向のみを探索すればいい。これにに対して全画素操作は無駄な計算となる。さらに、画像はかなりのデータ量を持つのでこの計算量もばかにならない。

ここで、chatGPT(GPT-5.6-sol)に解決策を聞いたところ、積分画像による計算を提案されたので、自分の理解がてら実際にこれを解説してみる。ここから提示するコードは基本的に、codex(GPT-5.6-sol)によって生成したコードに対して、レビュー＆加筆＆修正を加えたものになる。

random dotの生成

random dotの生成も地味に厄介で、ただ単にrandomなdotパターンとすると一部にdotが集中したりする。

一方で、格子を設定しその格子内にdotを配置する手法だと周期性が生まれてしまい、それを同一パターンと勘違いしてしまうので格子性を持たせるのもよくない。

import numpy as np

def make_random_dot(shape=(640, 640), dot_count=1600, dot_diameter_px=5, min_distance_px=5, seed=20260817):
    rng = np.random.default_rng(seed)
    height, width = shape
    cell = min_distance_px / np.sqrt(2)
    grid = {}
    points = []

    while len(points) < dot_count:
        point = rng.uniform((dot_diameter_px / 2, dot_diameter_px / 2), (width - dot_diameter_px / 2, height - dot_diameter_px / 2))
        key = tuple(np.floor(point / cell).astype(int))
        near = (grid.get((key[0] + i, key[1] + j)) for i in range(-2, 3) for j in range(-2, 3))

        if all(other is None or np.sum((point - other) ** 2) >= min_distance_px ** 2 for other in near): 
            points.append(point); grid[key] = point

    image = np.zeros(shape, float)
    radius = dot_diameter_px / 2

    for x, y in points:
        x0, x1, y0, y1 = max(0, int(np.floor(x-radius))), min(width, int(np.ceil(x+radius))+1), max(0, int(np.floor(y-radius))), min(height, int(np.ceil(y+radius))+1); yy, xx = np.ogrid[y0:y1, x0:x1]
        image[y0:y1, x0:x1][(xx-x)**2+(yy-y)**2 <= radius**2] = 1

    return image

そこで、作成したプログラムが以上のようになる。ポイントとしてはrandom dotの距離の最小がdot同士で重ならないことのみが条件となっている。

今後はデジタルスペックルパターン生成のOSS(Yong2022)の考え方も取り入れる改善などを考えている。

視差画像の生成

視差画像についてはSLの原理で説明した内容がそのまま使われている。distanceの画像を入れればそのままrandom dotに視差を与えてくれるようなプログラムとなっている。

import numpy as np

def making_parallex(random_dot, distance_image, focal_length_px=1145.9, baseline_mm=60):
    height, width = random_dot.shape
    y, x = np.nonzero(random_dot)
    disparity = focal_length_px * baseline_mm / distance_image[y, x]
    target_x = x - disparity
    x0 = np.floor(target_x).astype(int)
    x1 = x0 + 1
    image = np.zeros_like(random_dot, float)
    valid0 = (x0 >= 0) & (x0 < width)
    valid1 = (x1 >= 0) & (x1 < width)
    np.maximum.at(image, (y[valid0], x0[valid0]), random_dot[y[valid0], x[valid0]] * (x1[valid0] - target_x[valid0]))
    np.maximum.at(image, (y[valid1], x1[valid1]), random_dot[y[valid1], x[valid1]] * (target_x[valid1] - x0[valid1]))
    return image

SAD計算

SAD計算は、愚直にやるなら元画像のある領域に窓サイズ分の画素を抜き出して、視差画像に対して視差ずれがどうなっているかを一つ一つの窓で計算していく。

この場合はepi polar方向の探索と限定しても、画像サイズを\( H,W\)として、窓サイズを\(W_h, W_w\)として

\begin{equation}
R_{SSD} = \sum_{j=0}^{W_h-1} \sum_{i=0}^{W_w-1} | I_{parallex}(x+i,y+j) -I_{original}(i,j) |
\end{equation}

となり、ひとつの計算で\(\mathrm{O}(W_hW_w)\)。これが全画素に対して行うので、画素数分の\(H,W\)(厳密にはwindowサイズ分減っているが…)。また、epipolar方向にずらして計算をおこなっていくので、その距離候補分\(D\)が効いてくる。

よって、最終的な計算オーダーは\(\mathrm{O}(DHWW_hW_w)\)

これを実装したコードが以下のようになる。

def sad(image1, image2, dx, dy, window):
    x, y, width, height = window; h, w = image1.shape
    x0, x1 = max(x, dx, 0), min(x + width, w, w + dx)
    y0, y1 = max(y, dy, 0), min(y + height, h, h + dy)

    if x1 > x0 and y1 > y0:
        return np.mean(np.abs(image1[y0:y1, x0:x1] - image2[y0 - dy:y1 - dy, x0 - dx:x1 - dx]))  
    else:
        return np.inf

一方でこれは計算オーダーが大きすぎるので、計算を軽くしたものを解説していく。

まずは、視差候補ごとに、画像全体の絶対差画像を作る。

\begin{equation}
I_{AD} = |I_{parallex}(x,y) -I_{original}(x,y)|
\end{equation}

この時点で計算オーダーは\(\mathrm{O}(D)\)その後、窓サイズ\(W_h, W_w\)に分ける必要があるが、ここで使われるのが画像の平滑化で使われるbox filterである。box filiterは窓サイズの分だけそれぞれの位置の合計値を計算できる。また、box filterは隣り合った画素の共有合計値を再利用するため、畳み込み積分より早い計算になっている。

また、全窓の平滑化計算は\(\mathrm{O}(HW)\)で与えられるので、最終的な計算オーダーは\(\mathrm{O}(DHW)\)となる。

def fast_sad(reference, target, depth_candidates_mm, window_size, focal_length_px, baseline_mm):
    reference = np.asarray(reference, np.float32)
    target = np.asarray(target, np.float32)
    height, width = reference.shape
    out_h, out_w = height-window_size+1, width-window_size+1
    depths = np.asarray(depth_candidates_mm, np.float32)
    disparities = np.rint(focal_length_px*baseline_mm/depths).astype(np.int32)
    keep = np.r_[True, disparities[1:] != disparities[:-1]]
    depths, disparities = depths[keep], disparities[keep]
    best_cost = np.full((out_h, out_w), np.inf, np.float32)
    best_depth = np.full((out_h, out_w), np.nan, np.float32)
    difference = np.zeros_like(reference, np.float32); x = np.arange(out_w, dtype=np.float32)

    for depth, dx in zip(depths, disparities):
        if dx <= 0 or dx >= width: continue
        difference.fill(0)
        np.subtract(reference[:, dx:], target[:, :-dx], out=difference[:, dx:]) #A-B
        np.abs(difference, out=difference)                                      #|A-B|
        sad_sum = cv2.boxFilter(difference, cv2.CV_32F, (window_size, window_size), anchor=(0, 0), normalize=False, borderType=cv2.BORDER_CONSTANT)[:out_h, :out_w]
        count = (window_size*np.clip(x+window_size-dx, 0, window_size))[None, :]
        sad_cost = np.divide(sad_sum, count, out=np.full_like(sad_sum, np.inf), where=count>0)
        update = sad_cost < best_cost
        best_cost[update] = sad_cost[update]
        best_depth[update] = depth

    return best_depth, best_cost, depths, disparities

ここまで作成したSADを比較すると以下の表のようになる。

比較SADFast SAD
時間計算量\(\mathrm{O}(DHWW_hW_w)\)\(\mathrm{O}(DHW)\)
Python側のSAD計算呼び出し約1.35億回91回のループ
窓内計算毎回31×31を再計算隣接窓で局所和を共有
メモリ小さいが非常に遅い\(\mathrm{O}(HW)\)、数十MB程度

全実装について

ここまでの実装のポイントを踏まえた、全コードを以下に示す。Aという立体物に対して測距を行っている結果となる。

# 1. 基本パラメータとrandom dot・視差画像の生成
import time
import cv2
import numpy as np
import matplotlib.pyplot as plt

def make_random_dot(shape=(640, 640), dot_count=1600, dot_diameter_px=5, min_distance_px=5, seed=20260817):
    rng = np.random.default_rng(seed)
    height, width = shape
    cell = min_distance_px / np.sqrt(2)
    grid = {}
    points = []

    while len(points) < dot_count:
        point = rng.uniform((dot_diameter_px / 2, dot_diameter_px / 2), (width - dot_diameter_px / 2, height - dot_diameter_px / 2))
        key = tuple(np.floor(point / cell).astype(int))
        near = (grid.get((key[0] + i, key[1] + j)) for i in range(-2, 3) for j in range(-2, 3))

        if all(other is None or np.sum((point - other) ** 2) >= min_distance_px ** 2 for other in near): 
            points.append(point); grid[key] = point

    image = np.zeros(shape, float)
    radius = dot_diameter_px / 2

    for x, y in points:
        x0, x1, y0, y1 = max(0, int(np.floor(x-radius))), min(width, int(np.ceil(x+radius))+1), max(0, int(np.floor(y-radius))), min(height, int(np.ceil(y+radius))+1); yy, xx = np.ogrid[y0:y1, x0:x1]
        image[y0:y1, x0:x1][(xx-x)**2+(yy-y)**2 <= radius**2] = 1

    return image

def making_parallex(random_dot, distance_image, focal_length_px=1145.9, baseline_mm=60):
    height, width = random_dot.shape
    y, x = np.nonzero(random_dot)
    disparity = focal_length_px * baseline_mm / distance_image[y, x]
    target_x = x - disparity
    x0 = np.floor(target_x).astype(int)
    x1 = x0 + 1
    image = np.zeros_like(random_dot, float)
    valid0 = (x0 >= 0) & (x0 < width)
    valid1 = (x1 >= 0) & (x1 < width)
    np.maximum.at(image, (y[valid0], x0[valid0]), random_dot[y[valid0], x[valid0]] * (x1[valid0] - target_x[valid0]))
    np.maximum.at(image, (y[valid1], x1[valid1]), random_dot[y[valid1], x[valid1]] * (target_x[valid1] - x0[valid1]))
    return image
    
def fast_sad(reference, target, depth_candidates_mm, window_size, focal_length_px, baseline_mm):
    reference = np.asarray(reference, np.float32)
    target = np.asarray(target, np.float32)
    height, width = reference.shape
    out_h, out_w = height-window_size+1, width-window_size+1
    depths = np.asarray(depth_candidates_mm, np.float32)
    disparities = np.rint(focal_length_px*baseline_mm/depths).astype(np.int32)
    keep = np.r_[True, disparities[1:] != disparities[:-1]]
    depths, disparities = depths[keep], disparities[keep]
    best_cost = np.full((out_h, out_w), np.inf, np.float32)
    best_depth = np.full((out_h, out_w), np.nan, np.float32)
    difference = np.zeros_like(reference, np.float32); x = np.arange(out_w, dtype=np.float32)

    for depth, dx in zip(depths, disparities):
        if dx <= 0 or dx >= width: continue
        difference.fill(0)
        np.subtract(reference[:, dx:], target[:, :-dx], out=difference[:, dx:]) #A-B
        np.abs(difference, out=difference)                                      #|A-B|
        sad_sum = cv2.boxFilter(difference, cv2.CV_32F, (window_size, window_size), anchor=(0, 0), normalize=False, borderType=cv2.BORDER_CONSTANT)[:out_h, :out_w]
        count = (window_size*np.clip(x+window_size-dx, 0, window_size))[None, :]
        sad_cost = np.divide(sad_sum, count, out=np.full_like(sad_sum, np.inf), where=count>0)
        update = sad_cost < best_cost
        best_cost[update] = sad_cost[update]
        best_depth[update] = depth

    return best_depth, best_cost, depths, disparities
    
WIDTH, HEIGHT, WINDOW_SIZE = 1200, 1100, 31
FOCAL_LENGTH_PX, BASELINE_MM = 1145.9, 60
DEPTH_CANDIDATES_MM = np.arange(150, 601, 5, dtype=np.float32)
A_DISTANCE_MM, BACKGROUND_DISTANCE_MM = 450.0, 500.0

random_dot = make_random_dot((HEIGHT, WIDTH), 16000, 5, 5, 20260817).astype(np.float32)
yy, xx = np.indices((HEIGHT, WIDTH)); xn = (xx-WIDTH/2)/(WIDTH/2)
yn = yy/(HEIGHT-1)
half_width = 0.04+0.42*yn

mask_a = ((np.abs(xn)<half_width)&~((yn>0.16)&(np.abs(xn)<half_width-0.075)))|((yn>0.54)&(yn<0.64)&(np.abs(xn)<half_width))

true_distance = np.where(mask_a, A_DISTANCE_MM, BACKGROUND_DISTANCE_MM).astype(np.float32)
parallax_image = making_parallex(random_dot, true_distance, FOCAL_LENGTH_PX, BASELINE_MM).astype(np.float32)

print(f'image={WIDTH}x{HEIGHT}, window={WINDOW_SIZE}x{WINDOW_SIZE}, candidates={len(DEPTH_CANDIDATES_MM)} ({DEPTH_CANDIDATES_MM[0]:.0f}-{DEPTH_CANDIDATES_MM[-1]:.0f} mm, 5 mm step)')

# 2. 15-60 cmを5 mm刻みで測距
start = time.perf_counter()
depth_map, best_sad, used_depths, used_disparities = fast_sad(random_dot, parallax_image, DEPTH_CANDIDATES_MM, WINDOW_SIZE, FOCAL_LENGTH_PX, BASELINE_MM)
elapsed = time.perf_counter()-start
print(f'output shape={depth_map.shape}, evaluated={len(used_depths)} candidates, elapsed={elapsed:.2f} s')
print(f'disparity range={used_disparities.min()}-{used_disparities.max()} px')

# 3. 入力画像・正解距離・SAD測距結果の可視化
half = WINDOW_SIZE//2
truth_window = true_distance[half:HEIGHT-half, half:WIDTH-half]
valid = np.isfinite(depth_map); error = np.where(valid, depth_map-truth_window, np.nan)
mae = np.nanmean(np.abs(error)); exact = np.mean(np.isclose(depth_map[valid], truth_window[valid])); within_5 = np.mean(np.abs(error[valid]) <= 5)

fig, axes = plt.subplots(2, 3, figsize=(18, 10))
axes[0,0].imshow(random_dot, cmap='gray'); axes[0,0].set_title('random dot')
axes[0,1].imshow(parallax_image, cmap='gray'); axes[0,1].set_title('parallax image')
im0 = axes[0,2].imshow(truth_window, cmap='turbo', vmin=150, vmax=600)
axes[0,2].set_title('ground truth [mm]'); fig.colorbar(im0, ax=axes[0,2])
im1 = axes[1,0].imshow(depth_map, cmap='turbo', vmin=150, vmax=600)
axes[1,0].set_title('fast SAD depth [mm]'); fig.colorbar(im1, ax=axes[1,0])
im2 = axes[1,1].imshow(error, cmap='coolwarm', vmin=-25, vmax=25)
axes[1,1].set_title('depth error [mm]'); fig.colorbar(im2, ax=axes[1,1])

#for ax in axes.flat[:5]: 
#    ax.axis('off')

plt.tight_layout()
plt.show()

問題なく、SLの計算が実装できていることが確認できる。

コメント

タイトルとURLをコピーしました