【RealSense】RGB画像の歪みを補正する(D455)

2020.09.14

カフェチームの山本です。

現在、カフェでは、商品を手に取るユーザの、RGB画像とdepth画像を取得するために、Intel製のRealSenseを利用しています。RealSenseの機種の中でも、最近発売されたRealSense D455は、従来の機種に比べ広角(86° × 57°)にRGB画像を撮影でき、広範囲をカバーするのに少ない台数ですませられるため、機器コストや設置の手間を削減できます。

しかし、D455ではRGB画像に歪みがある状態で取得されます。そのため、場合によっては、この歪みを補正する処理を加える必要があります。(以前の機種(D435iなど)では、RGB画像に歪みが無かったため、こうした処理は不要でした。また、D455でも、depth画像には歪みがないため、補正の必要はありません。)

今回は、D455で取得した画像上の座標を、歪みがない場合の座標に補正するために、pyrealsense2を利用する方法をまとめます。

歪みとは

今回の歪みとは、カメラのレンズなどの影響によって、画像が変形してしまうことを指します。この変形は、全体的に膨らんだり、縮んだりすること意味しています(画像の一部分だけ特異的につぶれている、という意味の歪みではありません)。そのため、実際には直線の形状をしていても、撮影した画像上では、曲線になってしまう、ということが発生します。

詳しくは、さまざまなページで解説されているので、そちらをご参照ください。

Distortion (optics)

歪みがない(と仮定した)場合の画像上の座標と、歪みがある場合の画像上の座標の対応関係は、Brown–Conradyモデルという式で、モデル化することができます。上記ページでいうと、「Software correction」の項目が該当します。このモデルでは、画像の中心(正確には光軸)からの距離とパラメータによって、歪みのあり/なしの座標を変換できます。

このパラメータは、カメラの内部パラメータの1つです。

(座標変換については、以前の記事をご参照ください。)

座標変換について調べてみた | Developers.IO

画像の座標を空間の座標に変換する | Developers.IO

内部パラメータの取得方法

RealSenseでは、歪みの補正に必要な、カメラの内部パラメータを取得するための関数が用意されています。コードとしては、以下のようにして取得できます。

import pyrealsense2 as rs

width = 848
height = 480
fps = 30

config = rs.config()
config.enable_stream(rs.stream.color, width, height, rs.format.bgr8, fps)
config.enable_stream(rs.stream.depth, width, height, rs.format.z16, fps)

# ストリーミング開始
pipeline = rs.pipeline()
profile = pipeline.start(config)
depth_intrinsics = rs.video_stream_profile(profile.get_stream(rs.stream.depth)).get_intrinsics()
color_intrinsics = rs.video_stream_profile(profile.get_stream(rs.stream.color)).get_intrinsics()

print("depth_intrinsics")
print(depth_intrinsics)
print()
print("color_intrinsics")
print(color_intrinsics)
print()

出力される結果は、以下のとおりでした。RGB画像とdepth画像それぞれの、カメラの内部パラメータを取得できます。(depthはBrown Conradyが0のみで歪みがなく、RGBは0以外の数値があり歪みがあることが、この結果からもわかります。)

歪みのパラメータとして、5つの数値が出力されています(詳細は次節)。

depth_intrin
[ 848x480  p[420.099 231.147]  f[425.681 425.681]  Brown Conrady [0 0 0 0 0] ]

color_intrin
[ 848x480  p[419.714 246.234]  f[418.757 417.784]  Inverse Brown Conrady [-0.0568841 0.0675002 0.000153208 0.000432398 -0.0216763] ]

歪みの補正方法

先程のページの「Software correction」の項目に書かれているモデル式を実装すれば、取得したパラメータを利用して、歪んだ画像内の座標から、歪みのない画像内の座標に、自分で計算できます。

Distortion (optics)

ただ、pyrealsenseには、この補正用の関数が用意されているため、こちらを使う方が簡単で良さそうです。使う関数としては、pyrealsense2.rs2_deproject_pixel_to_pointを利用できます。この関数自体は、引数として、カメラの内部パラメータのオブジェクト、画像内の座標(x, y)、奥行きを受け取り、カメラを原点とした空間に変換する(射影変換の逆)というものです。

Projection in Intel RealSense SDK 2.0

ソースコードを見ると、変換の途中で、カメラの内部パラメータに含まれる、歪みパラメータを利用して、歪みをなくしていることがわかります。(5つのパラメータの意味はここでわかります)

Intel® RealSense™ Cross Platform API: C:/librealsense-2.13.0/include/librealsense2/rsutil.h Source File

// 上記リンクから引用
/* Given pixel coordinates and depth in an image with no distortion or inverse distortion coefficients, compute the corresponding point in 3D space relative to the same camera */
static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics * intrin, const float pixel[2], float depth)
{
    assert(intrin->model != RS2_DISTORTION_MODIFIED_BROWN_CONRADY); // Cannot deproject from a forward-distorted image
    assert(intrin->model != RS2_DISTORTION_FTHETA); // Cannot deproject to an ftheta image
    //assert(intrin->model != RS2_DISTORTION_BROWN_CONRADY); // Cannot deproject to an brown conrady model

    float x = (pixel[0] - intrin->ppx) / intrin->fx;
    float y = (pixel[1] - intrin->ppy) / intrin->fy;
    if(intrin->model == RS2_DISTORTION_INVERSE_BROWN_CONRADY)
    {
        float r2  = x*x + y*y;
        float f = 1 + intrin->coeffs[0]*r2 + intrin->coeffs[1]*r2*r2 + intrin->coeffs[4]*r2*r2*r2;
        float ux = x*f + 2*intrin->coeffs[2]*x*y + intrin->coeffs[3]*(r2 + 2*x*x);
        float uy = y*f + 2*intrin->coeffs[3]*x*y + intrin->coeffs[2]*(r2 + 2*y*y);
        x = ux;
        y = uy;
    }
    point[0] = depth * x;
    point[1] = depth * y;
    point[2] = depth;
}

よって、(変換したい対象である)歪みありの座標を、depthを1に設定して入力することで、補正した座標を取得できます。正確には、この結果はカメラ座標になるため、(歪みなしで)射影変換を再度おこなうことで、歪み無しのスクリーン座標を得られます。

import pyrealsense2 as rs

# 内部パラメータを取得
width = 848
height = 480
fps = 30

config = rs.config()
config.enable_stream(rs.stream.color, width, height, rs.format.bgr8, fps)
config.enable_stream(rs.stream.depth, width, height, rs.format.z16, fps)

# ストリーミング開始
pipeline = rs.pipeline()
profile = pipeline.start(config)
color_intrinsics = rs.video_stream_profile(profile.get_stream(rs.stream.color)).get_intrinsics()

# 歪みを補正(変換)
x, y = 300, 200  # 変換したい座標
depth = 1

pixel = [x, y]
point = rs.rs2_deproject_pixel_to_point(intrinsic, pixel, depth)

# カメラ座標をスクリーン座標に変換(歪みなし)
x_ = int(point[0] * intrinsic.fx + intrinsic.ppx)
y_ = int(point[1] * intrinsic.fy + intrinsic.ppy)

動作の確認

画像上で、変換前(歪みあり)の座標が、変換後(歪みなし)の座標のどこに当たるのかを見てみました。コード(の一部)は以下のようです。画像内の11*11=121点において、変換前を赤、変換後を緑で表示しています。

CAPTURE_WIDTH = 640
CAPTURE_HEIGHT = 480
n_interval = 11

for i in range(n_interval):
    for j in range(n_interval):
        interval_width = CAPTURE_WIDTH / n_interval
        interval_height = CAPTURE_HEIGHT / n_interval
        pixel = [interval_width * (i + 1/2), interval_height * (j + 1/2)]

        # before correcting distortion
        x = int(pixel[0])
        y = int(pixel[1])
        cv2.circle(RGB_image, (x, y), 5, (0, 0, 255), -1) # red

        # after correcting distortion
        point = rs.rs2_deproject_pixel_to_point(intrinsic, pixel, 1)
        x_ = int(point[0] * intrinsic.fx + intrinsic.ppx)
        y_ = int(point[1] * intrinsic.fy + intrinsic.ppy)
        cv2.circle(RGB_image, (x_, y_), 5, (0, 255, 0), -1) # green

RealSenseから640*480で取得した画像で表示すると、以下のようになりました。端にいく程、歪みによって広がって表示されていることがわかります。

まとめ

RealSense D455の歪みを補正するため、pyrealsense2の関数を利用する方法をまとめました。

参考にさせていただいたページ

Distortion (optics)

Projection in Intel RealSense SDK 2.0

pyrealsense2 - pyrealsense2 2.33.1 documentation

Intel® RealSense™ Cross Platform API: C:/librealsense-2.13.0/include/librealsense2/rsutil.h Source File