環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	20.04
ROS	Noetic
OpenCV	4.2

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

カメラ画像からは多くの情報を得ることが出来ますが、カメラに写っているものと、現実位置の対応が取れなければロボットビジョンといて活用することはできません。ROSにおいてこの2つを結びつける情報がCameraInfoです。

座標系

以下の話では

元画像、補正画像：画像の左上が原点、右方向がu軸正、下方向がu軸正
3D空間：カメラの前方向がz軸正、右方向がx軸正、下方向がy軸正

CameraInfoの中身

以下がCameraInfoの例です。要素数なども含めて基本的にこの形になります。

cam_lecture/config/camera.yaml

image_width: 640
image_height: 480
camera_name: camera
camera_matrix:
  rows: 3
  cols: 3
  data: [318.898697, 0, 311.613177, 0, 319.855587, 291.248501, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.016615, -0.025819, 0.002167, 0.003753, 0]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
  rows: 3
  cols: 4
  data: [315.782593, 0, 314.915759, 0, 0, 318.99234, 293.472369, 0, 0, 0, 1, 0]

image_width/image_height: カメラのが素数を表します。

camera_matrix：元画像のカメラ行列を表す3x3の行列です。説明ではよくKと表されます。実質的には以下の4パラメーターとなります。

K

K =
\begin{pmatrix}
fx & 0 & cx \\
0 & fy & cy \\
0 & 0 & 1 \\
\end{pmatrix}
=
\begin{pmatrix}
318.898697 & 0 & 311.613177 \\
0 & 319.855587 & 291.248501 \\
0 & 0 & 1 \\
\end{pmatrix}

distortion_model: レンズ歪みのモデルを選択します。plumb_bob固定です。

distortion_coefficients：レンズひずみパラメーターと呼ばれます。Dと表される4or5次元の値です。こちらは行列ではなく5次元の値となります。

D

D = [k1, k2, p1, p2, k3] = [0.016615, -0.025819, 0.002167, 0.003753, 0]

rectification_matrix: 回転行列です。イメージとしてカメラの取付誤差などに対応します。Rであらわされます。今回の例では回転しない単位行列が入っています。

K

R =
\begin{pmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{pmatrix}

projection_matrix: 投影行列とも呼ばれる補正画像のカメラ行列です。元画像のカメラ行列と基本は同じですが、平行移動のパラメーターが追加されます。

P

P =
\begin{pmatrix}
fx' & 0 & cx' & tx \\
0 & fy' & cy' & ty \\
0 & 0 & 1 & 0 \\
\end{pmatrix}
=
\begin{pmatrix}
315.782593 & 0 & 314.915759 & 0 \\
0 & 318.99234 & 293.472369 & 0 \\
0 & 0 & 1 & 0 \\
\end{pmatrix}

行列	記号	パラメーター名	種類	説明
K	cx, cy	principal point	内部	元画像の画素座標上でのレンズ中心
K	fx, fy	focal length	内部	元画像の1m先の1mの物が画素座標系でどれぐらいの大きさに見えるか
D	k1, k2, k3	radial distortion	内部	放射方向でのレンズ歪みパラメーター
D	p1, p2	tangential distortion	内部	接戦方向でのレンズ歪みパラメーター
R		rotation matrix	外部	第1カメラと平行になるように補正する行列
P	cx', cy'	principal point	内部	補正画像の画素座標上でのレンズ中心
P	fx', fy'	focal length	内部	補正画像の1m先の1mの物が画素座標系でどれぐらいの大きさに見えるか
P	tx, ty	normalized translation	外部	第1カメラとの平行距離を表すパラメーター

種類の項目は
- 内部：内部パラメーターを表します。カメラ単体のキャリブレーション（チェッカーボードをカメラ前に動かす動作）で推定可能なパラメーターです。
- 外部：外部パラメーターを表します。他のカメラや取付位置との相対位置関係で決まる値です。
K,D,Rは元画像->3D位置の補正に使われるのに対して、Pは補正画像->3D位置の補正に使います。
cx, cyは画素座標上でのレンズ中心（光学中心）の位置を表します。理想的にはwidth/2+0.5, height/2+0.5となりますが、レンズの取付誤差によってずれます。
fx, fyはfocal length(焦点距離)という名前ですが、レンズの焦点距離とは関係がありません。カメラの1m先でu方向に1mずれたものが光学中心から何pixelの位置に見えるかがfx、v方向に1mずれたものの見える位置がfyで、カメラの画角に相当するパラメーターです。
k1,k2,k3はレンズの放射上方向のひずみ（いわゆる樽収差など）を表します。以下のような式になっていてk1>0で樽型収差、K1<0で糸巻き型収差になります。

x(distorted) = x(1+k_1r^2+k_2r^4+k_3r^6) \\
y(distorted) = y(1+k_1r^2+k_2r^4+k_3r^6)

p1,p2は周方向のひずみを表します。
tx,tyはその画像と第1カメラの位置の相対位置を表すのに使います。ステレオカメラでは慣習的に左が第1カメラです。左カメラのcamera_infoではtx=ty=0になりますが、右カメラではここに左カメラとの相対位置に対応するパラメーターが入ります。
- 左右カメラの水平距離がBx[m]である場合tx = -fx' * Bxとなります。

座標の変換

以下の関数で行えます。

元画像上の位置 -> 補正画像上の位置
image_geometry::PinholeCameraModel::rectifyPoint()
補正画像上の位置 -> 元画像上の位置
image_geometry::PinholeCameraModel::unrectifyPoint()
補正画像上の位置 -> 3D上の位置
image_geometry::PinholeCameraModel::projectPixelTo3dRay()を使うと以下のような式の変換をできます。

ray.x = (rect.u - cx - Tx) / fx \\
ray.y = (rect.v - cy - Ty) / fy \\
ray.z = 1.0

3D上の位置 -> 補正画像上の位置
image_geometry::PinholeCameraModel::project3dToPixel()を使うと以下のような式の変換をできます。

rect.u = (fx\times p.x + Tx) / p.z + cx \\
rect.v = (fy\times p.y + Ty) / p.z + cy

参考

image_geometry 画像上の位置の変換
 image_geometry API
OpenCV カメラキャリブレーションと3次元再構成
 sensor_msgs/CameraInfo

目次ページへのリンク

ROS講座の目次へのリンク