この記事は公開されてから1年以上経過しています。情報が古い可能性がありますので、ご注意ください。
カフェチームの山本です。
現在、カフェでは、商品を手に取るユーザの、RGB画像とdepth画像を取得するために、Intel製のRealSenseを利用しています。RealSenseの機種の中でも、最近発売されたRealSense D455は、従来の機種に比べ広角（86° × 57°）にRGB画像を撮影でき、広範囲をカバーするのに少ない台数ですませられるため、機器コストや設置の手間を削減できます。
しかし、D455ではRGB画像に歪みがある状態で取得されます。そのため、場合によっては、この歪みを補正する処理を加える必要があります。（以前の機種（D435iなど）では、RGB画像に歪みが無かったため、こうした処理は不要でした。また、D455でも、depth画像には歪みがないため、補正の必要はありません。）
今回は、D455で取得した画像上の座標を、歪みがない場合の座標に補正するために、pyrealsense2を利用する方法をまとめます。
歪みとは
今回の歪みとは、カメラのレンズなどの影響によって、画像が変形してしまうことを指します。この変形は、全体的に膨らんだり、縮んだりすること意味しています（画像の一部分だけ特異的につぶれている、という意味の歪みではありません）。そのため、実際には直線の形状をしていても、撮影した画像上では、曲線になってしまう、ということが発生します。
詳しくは、さまざまなページで解説されているので、そちらをご参照ください。
歪みがない（と仮定した）場合の画像上の座標と、歪みがある場合の画像上の座標の対応関係は、Brown–Conradyモデルという式で、モデル化することができます。上記ページでいうと、「Software correction」の項目が該当します。このモデルでは、画像の中心（正確には光軸）からの距離とパラメータによって、歪みのあり/なしの座標を変換できます。
このパラメータは、カメラの内部パラメータの1つです。
(座標変換については、以前の記事をご参照ください。)
画像の座標を空間の座標に変換する | 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」の項目に書かれているモデル式を実装すれば、取得したパラメータを利用して、歪んだ画像内の座標から、歪みのない画像内の座標に、自分で計算できます。
ただ、pyrealsenseには、この補正用の関数が用意されているため、こちらを使う方が簡単で良さそうです。使う関数としては、pyrealsense2.rs2_deproject_pixel_to_pointを利用できます。この関数自体は、引数として、カメラの内部パラメータのオブジェクト、画像内の座標（x, y）、奥行きを受け取り、カメラを原点とした空間に変換する（射影変換の逆）というものです。
Projection in Intel RealSense SDK 2.0
ソースコードを見ると、変換の途中で、カメラの内部パラメータに含まれる、歪みパラメータを利用して、歪みをなくしていることがわかります。（5つのパラメータの意味はここでわかります）
// 上記リンクから引用
/* 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) # greenRealSenseから640*480で取得した画像で表示すると、以下のようになりました。端にいく程、歪みによって広がって表示されていることがわかります。
まとめ
RealSense D455の歪みを補正するため、pyrealsense2の関数を利用する方法をまとめました。
参考にさせていただいたページ
Projection in Intel RealSense SDK 2.0
2
