こんにちは、アールティの加藤です。
この記事ではROS 2パッケージを利用してWebカメラのキャリブレーションを行う方法を紹介します。また、カメラのキャリブレーションの仕組みについても簡単な解説を行います。
カメラのキャリブレーションについて
カメラのキャリブレーションとは、カメラの外部パラメータや内部パラメータ、画像の歪みのパラメータなどを事前に求めて、撮影した画像を正しく補正することを指します。
キャリブレーションが必要な例として、アームロボットCRANE+ V2の色検出サンプルを紹介します。
こちらのサンプルではロボットの頭上に取り付けたWebカメラを使用して特定の色を検出して把持を行います。しかし、下図のようにカメラ画像とロボットのモデルにズレが発生していると、物体の把持に失敗する場合があります。
キャリブレーション前（ズレが大きい）
キャリブレーションを行うと、このようなカメラ画像のズレを小さくできます。
キャリブレーション後（ズレが小さい）
ROS 2パッケージのインストール
下記のコマンドを順番に実行して、カメラのキャリブレーションを行うためのROS 2パッケージをインストールします。
sudo apt install ros-humble-camera-calibration-parsers
sudo apt install ros-humble-camera-info-manager
sudo apt install ros-humble-launch-testing-ament-cmake
sudo apt install ros-humble-usb-cam
sudo apt install ros-humble-image-pipeline以上でインストールは完了です。
チェスボードの印刷
カメラのキャリブレーションでは、白黒のチェスボードを撮影することでパラメータを求めます。次の画像のように、A4サイズの紙にチェスボードを印刷してください。また、印刷した紙は画用紙やバインダーなどの硬い板に貼り付けるとキャリブレーションが行いやすいです。
チェスボード
キャリブレーションの実行
カメラのキャリブレーションを行います。
まず、下記のコマンドでWebカメラを起動します。video_deviceは使用するカメラのデバイス名を指定してください。
ros2 run usb_cam usb_cam_node_exe --ros-args --remap video_device:=/dev/video0次に、下記のコマンドでキャリブレーションを実行します。sizeはチェスボードの交点の数を指定し、squareはチェスボードの1マスの大きさ（単位はメートル）を指定してください。
ros2 run camera_calibration cameracalibrator --size 6x9 --square 0.022 --ros-args -r image:=/image_rawキャリブレーションの実行に成功すると、次の画像のようなウィンドウが表示されます。
キャリブレーションを実行
キャリブレーション中はチェスボードを様々な位置と姿勢で撮影することで、ウィンドウの右上に表示されている4つのゲージを進めることができます。カメラから見て、Xは左右方向、Yは上下方向、Sizeは前後方向、Skewは傾きを表しています。しばらくチェスボードを動かし続けると4つのゲージが緑色になります。その後「CALIBRATE」ボタンも緑色になるので、ボタンを押してから少し待ちます。最後に「SAVE」ボタンを押せばパラメータファイルを保存できます。
キャリブレーション完了
パラメータファイルは/tmpディレクトリの中にcalibrationdata.tar.gzとして保存されます。
このファイルは下記のコマンドで展開できます。
tar -xvf /tmp/calibrationdata.tar.gz展開したost.yamlファイルをカメラ起動時に読み込むとキャリブレーション結果を反映できます。例としてCRANE+ V2のカメラサンプルでは下記のようにlaunchファイル内でost.yaml（camera_info.yamlにファイル名を変更）を読み込んでいます。
camera_info_file = 'file://' + get_package_share_directory(
'crane_plus_examples') + '/config/camera_info.yaml'
usb_cam_node = Node(
package='usb_cam',
executable='usb_cam_node_exe',
parameters=[
{'video_device': LaunchConfiguration('video_device')},
{'frame_id': 'camera_color_optical_frame'},
{'camera_info_url': camera_info_file},
{'pixel_format': 'yuyv2rgb'}
],
condition=IfCondition(LaunchConfiguration('use_camera'))
)以上でキャリブレーションは完了です。
カメラのキャリブレーションの仕組み
ここからはキャリブレーションにおいてどのようなパラメータを求めているのか、数式を用いて紹介します。
三次元座標を二次元平面へ投影
カメラにおいて、ワールド座標系の位置 \( ( X, Y, Z ) \) を画像平面上（スクリーン座標系）の位置 \( ( u, v ) \) に変換するには、以下のような式を利用します。この式において、 \( \boldsymbol{A} \) は内部パラメータ、 \( \begin{bmatrix}\boldsymbol{R} | \boldsymbol{t}\end{bmatrix} \) は外部パラメータと呼びます。
\begin{align*}
s
\boldsymbol{m^{\prime}}
=
\boldsymbol{A}
\begin{bmatrix}
\boldsymbol{R} | \boldsymbol{t}
\end{bmatrix}
\boldsymbol{M^{\prime}}
\end{align*}
内部パラメータと外部パラメータは以下のように表されます。 \( c_x, c_y \) は画像平面の中心、 \( f_x, f_y \) は焦点距離です。また、外部パラメータは回転行列と並進のベクトルによって構成されています。
\begin{align*}
s
\begin{bmatrix}
u \\
v \\
1
\end{bmatrix}
=
\begin{bmatrix}
f_x & 0 & c_x \\
0 & f_y & c_y \\
0 & 0 & 1 \\
\end{bmatrix}
\begin{bmatrix}
r_{11} & r_{12} & r_{13} & t_1 \\
r_{21} & r_{22} & r_{23} & t_2 \\
r_{31} & r_{32} & r_{33} & t_3 \\
\end{bmatrix}
\begin{bmatrix}
X \\
Y \\
Z \\
1
\end{bmatrix}
\end{align*}
ワールド座標系の位置 \( ( X, Y, Z ) \) に外部パラメータの行列をかけることでカメラ座標系の位置 \( ( X_{C}, Y_{C}, Z_{C} ) \) に変換できます。さらに、カメラ座標系の位置 \( ( X_{C}, Y_{C}, Z_{C} ) \) に内部パラメータの行列をかけることで画像平面上（スクリーン座標系）の位置 \( ( u, v ) \) に変換できます。
pinhole_camera_model
画像引用元 : Camera Calibration and 3D Reconstruction
レンズによる画像の歪み
カメラのレンズによって画像が歪む場合、原因は主に二つあります。一つは半径方向の歪みで、これはレンズの中心に比べて外側のほうが光の屈折率が高いために生じます。もう一つは円周方向の歪みで、これはレンズと画像平面が完璧な平行状態ではないために生じます。カメラのキャリブレーションは、これらの歪みに対して有効です。
画像の歪みを含めると、三次元座標を画像平面へ投影する式は以下のようになります。ただし、\( z \neq 0 \) です。
\begin{align*}
\begin{bmatrix}
x \\
y \\
z
\end{bmatrix}
&=
\boldsymbol{R}
\begin{bmatrix}
X \\
Y \\
Z
\end{bmatrix}
+
\boldsymbol{t}
\end{align*}
\begin{align*}
x^{\prime} &= x / z \\
y^{\prime} &= y / z \\
r^{2} &= x^{\prime 2} + y^{\prime 2}
\end{align*}
\begin{align*}
x^{\prime\prime} &= x^{\prime} ( 1+k_1 r^{2}+k_2 r^{4}+k_3 r^{6} )
+ 2 p_1 x^{\prime} y^{\prime} + p_2(r^{2}+2x^{\prime 2}) \\
y^{\prime\prime} &= y^{\prime} ( 1+k_1 r^{2}+k_2 r^{4}+k_3 r^{6} )
+ p_1(r^{2}+2y^{\prime 2}) + 2 p_2 x^{\prime} y^{\prime}
\end{align*}
\begin{align*}
u &= f_x * x^{\prime \prime} + c_x \\
v &= f_y * y^{\prime \prime} + c_y
\end{align*}
このとき、\( k_1 \) と \( k_2 \) 、 \( k_3 \) は半径方向の歪み係数、\( p_1 \) と \( p_2 \) は円周方向の歪み係数です。
カメラのキャリブレーションでは、既知のチェスボードを撮影することで外部パラメータと内部パラメータ、歪み係数を求めることができます。
さらに詳しく知りたい方へ
カメラのキャリブレーションについてさらに詳しく知りたい方は下記の記事をご覧ください。こちらの記事ではOpenCVを使用したカメラキャリブレーションについて詳しく解説されています。
キャリブレーションのサンプルのNotebook
パラメータファイルの中身の確認
それでは実際にキャリブレーションで求めたパラメータを確認してみます。下記のパラメータの一覧がost.yaml（camera_info.yaml）の中身です。
image_width: 640
image_height: 480
camera_name: narrow_stereo
camera_matrix:
rows: 3
cols: 3
data: [505.527843, 0.000000, 323.470665, 0.000000, 506.716878, 250.383936, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [0.009293, -0.052158, 0.001870, -0.000132, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
projection_matrix:
rows: 3
cols: 4
data: [502.349548, 0.000000, 322.979503, 0.000000, 0.000000, 505.240051, 250.568902, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]ここではcamera_matrixが内部パラメータの行列を表しています。
\begin{align*}
\boldsymbol{A}
=
\begin{bmatrix}
f_x & 0 & c_x \\
0 & f_y & c_y \\
0 & 0 & 1 \\
\end{bmatrix}
=
\begin{bmatrix}
505.527843 & 0 & 323.470665 \\
0 & 506.716878 & 250.383936 \\
0 & 0 & 1 \\
\end{bmatrix}
\end{align*}
例として\( c_x, c_y \)に着目すると、カメラの解像度が 640 x 480 なので理想的には \( c_x = 320.5, c_y = 240.5 \) となりますが、今回求めた値は \( c_x = 323.470665, c_y = 250.383936 \) となっています。このズレはレンズとセンサの取り付け位置の誤差によって生じています。
おわりに
この記事ではROS 2のImage Pipelineパッケージを利用してWebカメラのキャリブレーションを行う方法を紹介しました。また、キャリブレーションの仕組みを数式を使って簡単に解説しました。カメラのキャリブレーションは必要不可欠というわけではありませんが、画像処理などを行う場合は重要になってくると思いますので、ぜひお試しください。
アームロボットCRANE+ V2のカメラのキャリブレーションについては下記のSoftware Tutorialsもご覧ください。
参考資料
Navigation 2 : Camera Calibration
