RaspberryPiで広角カメラを使う+ROSでキャリブレーション

Raspberry Piで広角カメラを使ってみたので、使い方をまとめました。
また、広角カメラで撮影した画像は歪んでいるので、ROSを使って補正（キャリブレーション）し、補正した画像を配信する方法も説明します。

# デバイス、環境
- Raspberry Pi4 Model B
    - Ubuntu Server 18.04
    - ROS melodic
- RPi Camera (G), Fisheye Lens　(広角カメラ)
- Desktop PC
    - Ubuntu 16.04
    - ROS Kinetic

Desktop PCは、後述しますが補正のために使用しました。カメラを使用するだけであればRaspiだけでよいです。

# カメラ詳細
今回使用したカメラは`waveshare`というメーカーの広角カメラ「RPi Camera (G), Fisheye Lens」です。
スイッチサイエンスで購入可能です。
- [スイッチサイエンスのページ](https://www.switch-science.com/catalog/3211/)
- [メーカーページ](https://www.waveshare.com/RPi-Camera-G.htm)
- [seeedのページ](https://www.seeedstudio.com/Raspberry-Pi-Wide-Angle-Camera-Module.html)

## マニュアル
ユーザマニュアルは、メーカーページからダウンロードできます。
- [マニュアル](https://www.waveshare.com/w/upload/e/e2/RPi-Camera-User-Manual-EN.pdf)

## スペック
- 静止画像の解像度: 2592 x 1944
- 最大フレームレート: 30 fps
- 対角画角: 160°
- 水平画角: 120°

通常のカメラの画角は`62.2×48.8`なので、倍以上の画角があります。

## 使い方
使い方はとても簡単で、通常のRaspi専用カメラと同じです。
フラットケーブルで接続した後、`raspistill`コマンドや`raspivid`で画像、動画の撮影が可能です。

# ROSでカメラ画像を取得、表示する
`uvc_camera`でカメラ画像を取得し、`image_view`で表示します。自分の環境では`image_view`をDesktop PC側で実行しました。
![camera_wide.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/293692/45ee07e3-dcb7-42d4-a4ef-e5d6f76c9de9.png)


### Raspi側のlaunchファイル

```
<launch>
  <node name="uvc_camera" pkg="uvc_camera" type="uvc_camera_node">
    <param name="width" type="int" value="2592"/>
    <param name="height" type="int" value="1944"/>
  </node>
</launch>
```

### Desktop側のlaunchファイル

```
<launch>
 <node name="image_view" pkg="image_view" type="image_view" output="screen">
    <remap from="image" to="image_raw"/>
 </node>
</launch>
```

### 結果
画像が上下反転していますが、カメラを上下逆さに取り付けたからです。上下正しく設置すれば、画像は逆さになりません。
周辺部に行くほど、画像が歪んでいるのがわかると思います。この歪みをこれから補正します。
![original.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/293692/a20d0aee-3ccb-0b64-cda3-e4bae4e1e8bc.jpeg)



# ROSでキャリブレーションする
上の画像のように広角カメラの画像は歪んでいます。そこでキャリブレーションを行い、歪み補正をします。
歪み補正は「[ROS講座66 カメラのキャリブレーションを行う](https://qiita.com/srs/items/416aa78f2c679ddb7c52)」を参考にして行いました。

キャリブレーションはけっこう重く計算に時間がかかるため、スペックがより高いDesktop側で行いました。
![camera_calib.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/293692/02e68472-0efe-5281-d33e-e85b32f6b9cb.png)

## インストール
キャリブレーションで使用するROSパッケージをインストールします

```
$ sudo apt-get install -y ros-kinetic-camera-calibration
$ sudo apt-get install -y ros-kinetic-image-proc 
```

## チェッカーボードの印刷
キャリブレーションに使用するチェッカーボードを準備します。
[ROS wiki](http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration?action=AttachFile&do=get&target=check-108.pdf)からチェッカーボードの画像をダウンロードし、印刷します。自分はA3に印刷しました。これを板か何かに貼り付けます。
また、キャリブレーションの際、チェッカーボードの四角の数と大きさが必要になります。四角の数は、上記の画像であれば 8x6 です。四角の大きさはA3に印刷した場合、0.03mでした。

## キャリブレーション
キャリブレーションを実施します。
Raspi側はカメラ画像を表示した時と同じ方法で`uvc_camera`を立ち上げます。
Desktop PC側は`camera_calibration`を立ち上げます。起動時、四角の数、大きさを指定します。

- 1つ目のターミナル

```
$ roscore
```
- 2つ目のターミナル

```
$ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.03 image:=/image_raw
```

実行すると以下のような画面が表示されます。そうしたらチェッカーボードを画像内に収めながら、色々な距離や角度、位置で撮影します。
「CALIBRATE」ボタンが押せるようになったら、このボタンをクリックします。すると、歪み補正用のパラメータの計算が始まります。
計算には数１０分ほどかかります。自分の環境ではだいたい１５分位かかりました。

![calibration.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/293692/71b60f41-7082-e108-2af6-4a8a063265a6.png)


## 結果の保存
`/tmp/calibrationdata.tar.gz`に結果が出力されているので、任意のディレクトリに移動して解凍します。
大量のファイルが出力されますが、`ost.yaml`というのが歪み補正用のパラメータファイルです。

```
$ tar -xvzf /tmp/calibrationdata.tar.gz
```

```ost.yaml
image_width: 2592
image_height: 1944
camera_name: narrow_stereo
camera_matrix:
  rows: 3
  cols: 3
  data: [1374.805968, 0.000000, 1251.269799, 0.000000, 1385.361132, 912.160642, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.292026, 0.057026, 0.000142, 0.002053, 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: [928.518127, 0.000000, 1277.457798, 0.000000, 0.000000, 1123.514282, 877.728323, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
```

# カメラ画像の補正
キャリブレーションで求めた歪み補正用のパラメータを使って、画像の歪みを補正します。
Raspberry Pi側で歪み補正を行い、Desktop側で補正した画像を表示する構成にしました。
![camera_rect.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/293692/915c15e0-1a41-5085-97fa-aeba1029445a.png)

## パラメータファイルをコピー
まず、キャリブレーションで出力された`ost.yaml`をRaspberry Piにコピーします。

## インストール
歪み補正で使用するROSパッケージをインストールします

```
$ sudo apt-get install -y ros-kinetic-image-proc 
```

## 実行
launchファイルを実行します。

- Raspberry Pi側  
`camera_info_url`のvalueには、`ost.yaml`ファイルのパスを記述します。

```
<launch>
  <node name="uvc_camera" pkg="uvc_camera" type="uvc_camera_node">
    <param name="width" type="int" value="2592"/>
    <param name="height" type="int" value="1944"/>
    <param name="camera_info_url" value="file:///home/ubuntu/ros/catkin_ws/file/ost.yaml"/>
  </node>
  <node name="image_proc" pkg="image_proc" type="image_proc" output="screen"/>
</launch>
```

- Desktop PC側  
補正前の画像と補正後の画像の両方を表示します。

```
<launch>
 <node name="image_view" pkg="image_view" type="image_view" output="screen">
    <remap from="image" to="image_raw"/>
 </node>
 <node name="image_rect_view" pkg="image_view" type="image_view" output="screen">
    <remap from="image" to="image_rect_color"/>
 </node>
</launch>
```

## 結果
若干まだ歪んでいますが、先程は歪んでいた画像がかなり補正されています。
![calibrated.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/293692/78cfb42d-1fa3-78c8-367e-a3d79eaa6bc7.png)


# 参考
- [ROS講座66 カメラのキャリブレーションを行う](https://qiita.com/srs/items/416aa78f2c679ddb7c52)