ROS 2でCRANE+ V2を動かしてみた技術情報・開発日誌

ROS 2 カメラのキャリブレーション方法の紹介

キャリブレーション実行中 ROS 2でCRANE+ V2を動かしてみた
キャリブレーション実行中

こんにちは、アールティの加藤です。

この記事ではROS 2パッケージを利用してWebカメラのキャリブレーションを行う方法を紹介します。また、カメラのキャリブレーションの仕組みについても簡単な解説を行います。

カメラのキャリブレーションについて

カメラのキャリブレーションとは、カメラの外部パラメータや内部パラメータ、画像の歪みのパラメータなどを事前に求めて、撮影した画像を正しく補正することを指します。

キャリブレーションが必要な例として、アームロボットCRANE+ V2の色検出サンプルを紹介します。

CRANE+ V2 ROS 2パッケージに色検出サンプルが追加されました
こんにちは。アールティの加藤です。 アールティが販売するアームロボットCRANE+ V2のROS 2パッケージに、特定の色の物体を検出して掴むサンプルプログラムを追加しました。本記事では色検出サンプルの詳細や実際の動作についてご紹介します...

こちらのサンプルではロボットの頭上に取り付けた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'))
)
crane_plus/crane_plus_examples/launch/demo.launch.py at master · rt-net/crane_plus
CRANE+ V2 ROS 2 Packages. Contribute to rt-net/crane_plus development by creating an account on GitHub.

以上でキャリブレーションは完了です。

カメラのキャリブレーションの仕組み

ここからはキャリブレーションにおいてどのようなパラメータを求めているのか、数式を用いて紹介します。

三次元座標を二次元平面へ投影

カメラにおいて、ワールド座標系の位置 \( ( 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

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

キャリブレーションのサンプルのNotebook

https://rt-net.jp/mobility/archives/17588

パラメータファイルの中身の確認

それでは実際にキャリブレーションで求めたパラメータを確認してみます。下記のパラメータの一覧が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もご覧ください。

カメラのキャリブレーション - RT Corporation Software Tutorials
株式会社アールティのオンラインソフトウェアチュートリアルです。製品をより便利に使うためのソフトウェアの使い方を紹介しています。

参考資料

Navigation 2 : Camera Calibration

OpenCV : Camera Calibration and 3D Reconstruction

MathWorks : カメラのキャリブレーションとは

ROS講座132 CameraInfoを理解する

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