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

以前、Raspberry Pi4 + ROS 1で広角カメラを使い、その画像のキャリブレーションまで行う手順（[link](https://qiita.com/hiro-han/items/f6e943b7f614db16a06f)）をまとめましたが、Raspberry Pi4 + ROS 2で同じことをしたので、防備録を兼ねて手順をまとめます。

ROS 2の環境構築はできているものとします。

# デバイス、環境
## Raspi / PC
- Raspberry Pi4 Model B
    - Ubuntu Server 22.04
    - ROS 2 humble
    - RPi Camera (G), Fisheye Lens　(広角カメラ)
- Desktop PC
    - Ubuntu 22.04
    - ROS 2 humble

自分はDesktop PCでrviz2を使っての動作確認とキャリブレーションを行いました。
Raspberry Piで表示とキャリブレーションを行うことも可能なので、必須ではありません。

## 広角カメラ
使用したカメラはwaveshareというメーカーの広角カメラ「RPi Camera (G), Fisheye Lens」です。
スイッチサイエンスで購入可能です。
- [スイッチサイエンスのページ](https://www.switch-science.com/products/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で画像、動画の撮影が可能です。

# カメラ画像の表示
カメラからの画像の取得には、**v4l2_camera**を使用しました。
- [ROS Index](https://index.ros.org/r/v4l2_camera/#humble)
- [Github](https://github.com/tier4/ros2_v4l2_camera)

## インストール、ビルド
インストールは`apt`で可能ですが、自分の環境では`apt`でインストールして実行すると以下のようなエラーがでてしまいました。（「*height * step != size  or*」以降の数値は画像の高さ、幅によって変わります。）
```
[v4l2_camera_node-1] terminate called after throwing an instance of 'cv_bridge::Exception'
[v4l2_camera_node-1]   what():  Image is wrongly formed: height * step != size  or  1944 * 5184 != 10119168
[ERROR] [v4l2_camera_node-1]: process has died [pid 9414, exit code -6, cmd '/opt/ros/humble/lib/v4l2_camera/v4l2_camera_node --ros-args -r __node:=v4l2_camera_node -r __ns:=/v4l2_camera --params-file /tmp/launch_params_bfvkzl13'].
```

そのため、ソースコードを直接ビルドしました。
なおGithubリポジトリには`galactic`ブランチはあるものの`humble`ブランチはなかったため`galactic`ブランチを使用しました。

```
$ cd (ROSワークスペース)/src
$ git clone https://github.com/tier4/ros2_v4l2_camera.git
$ cd (ROSワークスペース)
$ colcon build --packages-select v4l2_camera
$ . install/setup.bash
```
## 実行
launchファイルを用意し、実行しました。

``` camera_laucnh.py
import launch
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    return LaunchDescription([
        
        launch.actions.LogInfo(
            msg="Camera."
        ),
        
        # Node for USB Camera
        Node(
            package         = 'v4l2_camera',
            namespace       = 'v4l2_camera',
            executable      = 'v4l2_camera_node',
            name            = 'v4l2_camera_node',
            parameters      = [{'image_size': [2592, 1944]}],
            ),
    ])
```

```
$ ros2 launch camera_laucnh.py
```

`/v4l2_camera/image_raw`というトピック名で出力されるはずなので、`ros2 topic list`でこのトピックがあること、また、rviz2デコのトピックを選択し、カメラ画像が表示されれば成功です。

![Screenshot from 2024-08-15 20-47-56.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/293692/7d5e7c59-d239-1678-fb54-1596fa010d91.png)


## 圧縮画像の出力
圧縮した画像を出力することも可能です。その場合は以下をインストールしてください。
```
$ sudo apt install -y ros-humble-image-transport
$ sudo apt install -y ros-humble-image-transport-plugins
```

インストール後、上記と同じ手順で実行すると`/v4l2_camera_mod/image_raw/compressed`というトピックが増え、圧縮画像が出力されます。

# キャリブレーション
このカメラは魚眼レンズが使われているので、上の画像のように画像が歪んでいるので、この歪みを補正します。

## 準備
キャリブレーションに使用するチェスボードを印刷し、板などに貼りつけます。
自分は[これ](https://github.com/opencv/opencv/blob/master/samples/data/chessboard.png)を印刷しました。

## キャリブレーションアプリのインストール
キャリブレーションを行うROSアプリをインストールします。

```
$ sudo apt install -y ros-humble-camera-calibration
```

## キャリブレーション実行
`v4l2_camera`を実行し、さらにキャリブレーションアプリを実行します。

```
$ ros2 run camera_calibration cameracalibrator --size 9x6 --square 0.025 image:=/v4l2_camera/image_raw camera:=/v4l2_camera
```

実行するとキャリブレーションウィンドウと、そこにカメラ画像が表示されます。カメラでチェスボードを写し、それが認識されると以下のような表示がされます。

![Screenshot from 2024-08-15 20-56-00.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/293692/4377f5a7-9cc5-6e4a-d67c-53a1a748ced1.png)


`X`, `Y`, `Size`, `Krew`のそれぞれのゲージが緑になり`CALIBRATE`ボタンが緑になって押下できる状態になるまで、位置や大きさなどを変えます。
`CALIBRATE`ボタンを押すと、歪み補正用のパラメータの計算が行われ、完了したら`SAVE`ボタンを押下します。
結果は`/tmp/calibrationdata.tar.gz`に出力されます。

## キャリブレーション結果の編集
`calibrationdata.tar.gz`を展開すると`ost.yaml`というファイルがあり、歪み補正用のパラメータが記載されています。
自分の環境ではこれをそのまま使用すると実行エラーとなったため、ファイル内の`camera_name`をデフォルトの`narrow_stereo`から`mmal_service_16.1`に変更しました。
変更後の値は`v4l2_camera`にカメラ名を出力するログを追加して確認しました。

```ros2_v4l2_camera/src/v4l2_camera.cpp

V4L2Camera::V4L2Camera(rclcpp::NodeOptions const & options)
: rclcpp::Node{"v4l2_camera", options},
  canceled_{false}
{

  ...(略)...

  cinfo_ = std::make_shared<camera_info_manager::CameraInfoManager>(this, camera_->getCameraName());
  RCLCPP_INFO(get_logger(), "camera name : %s\n", camera_->getCameraName().c_str());  // <-- 追加したログ出力
```

# 歪み補正
## 歪み補正アプリのインストール
```
$ sudo apt install -y ros-humble-image-pipeline
```
この`image_pipeline`に含まれる`image_proc`を使用します。

## launchファイルの準備
`image_proc`を実行するlaunchファイルを用意します。
このlaunchファイルは、Githubリポジトリにあるサンプル（[Link](https://github.com/ros-perception/image_pipeline/blob/rolling/image_proc/launch/image_proc.launch.py)）をもとにして作成しました。

``` image_rect.py
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import LaunchConfigurationEquals
from launch.conditions import LaunchConfigurationNotEquals
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode


def generate_launch_description():

    arg_namespace = DeclareLaunchArgument(
        name='namespace', default_value='',
        description=('namespace for all components loaded')
    )

    composable_nodes = [
        ComposableNode(
            package='image_proc',
            plugin='image_proc::RectifyNode',
            name='rectify_mono_node',
            namespace=LaunchConfiguration('namespace'),
            # Remap subscribers and publishers
            remappings=[
                ('image', '/v4l2_camera/image_raw'),
                ('camera_info', '/v4l2_camera/camera_info')
            ],
        )
    ]

    arg_container = DeclareLaunchArgument(
        name='container', default_value='',
        description=(
            'Name of an existing node container to load launched nodes into. '
            'If unset, a new container will be created.'
        )
    )

    # If an existing container is not provided, start a container and load nodes into it
    image_processing_container = ComposableNodeContainer(
        condition=LaunchConfigurationEquals('container', ''),
        name='image_proc_container',
        namespace=LaunchConfiguration('namespace'),
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=composable_nodes,
        output='screen'
    )

    # If an existing container name is provided, load composable nodes into it
    # This will block until a container with the provided name is available and nodes are loaded
    load_composable_nodes = LoadComposableNodes(
        condition=LaunchConfigurationNotEquals('container', ''),
        composable_node_descriptions=composable_nodes,
        target_container=LaunchConfiguration('container'),
    )

    return LaunchDescription([
        arg_namespace,
        arg_container,
        image_processing_container,
        load_composable_nodes,
    ])
```

次に`v4l2_camera`の実行時に歪み補正ファイルを読み込むようにlaunchファイルを変更します。なお、以下の例では`ost.yaml`を`camera_calib.yaml`にリネームし、launchファイルと同じ階層に置いています。

``` camera_laucnh.py
import launch
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    camera_file = launch.substitutions.LaunchConfiguration(
        'params', default=['file://', launch.substitutions.ThisLaunchFileDir(), '/camera_calib.yaml'])

    return LaunchDescription([
        
        launch.actions.LogInfo(
            msg="Camera."
        ),
        
        # Node for USB Camera
        Node(
            package         = 'v4l2_camera',
            namespace       = 'v4l2_camera',
            executable      = 'v4l2_camera_node',
            name            = 'v4l2_camera_node',
            parameters      = [{'image_size': [2592, 1944], 'camera_info_url': camera_file}],
            ),
    ])
```

## 歪み補正の実行
2つのlaunchファイルを実行します。

```
$ ros2 launch launch/camera_launch.py
```
```
$ ros2 launch launch/image_rect.py
```

補正後の画像は`/image_rect`というトピック名で出力されます。表示してみると歪みが補正されていることがわかります。
上が補正前の画像、下が補正後の画像です。
（この画像では補正しきれず一部歪んでいますが、何度かキャリブレーションを試したらいい感じに補正できるパラメータを出力できました。）

![Screenshot from 2024-08-15 20-51-00.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/293692/ae7e9abd-cee0-1708-6e7e-8b409f8009f5.png)


# 参考：　画像サイズによる遅延時間
Raspberry Pi4でカメラ画像の取得と歪み補正を行った場合にどれくらい遅延が発生するかを、カメラ画像のサイズを
- 最大サイズ（2592x1944）、
- 1/2(1296, 972)
- 1/3(864, 648)
- 1/4(648, 486)

と変更し確認しました。
不要な負荷を無くすため、rviz2はPCで実行しました。また、Raspberry Pi4とPCの間は有線LANで接続しています。

遅延はタイマーをカメラで撮影しビューアに表示された時間とどれくらい差があるかで測定したので、あまり正確ではありませんが参考にはなると思います。

| 画像サイズ | 歪み補正 | 画像圧縮 | 遅延(秒) |
| - | - | - | - |
| 2592 x 1944 | なし | なし | 2.6 |
| 1296 x 972 | なし | なし | 0.3 |
| 864 x 648 | なし | なし | 0.2 |
| 648 x 486 | なし | なし | 0.1 |
| 2592 x 1944 | あり | なし | 3.2 |
| 1296 x 972 | あり | なし | 0.3 |
| 864 x 648 | あり | なし | 0.3 |
| 648 x 486 | あり | なし | 0.1 |
| 2592 x 1944 | あり | あり | 4.2 |
| 1296 x 972 | あり | あり | 0.4 |
| 864 x 648 | あり | あり | 0.3 |
| 648 x 486 | あり | あり | 0.2 |

フルサイズ（2592 x 1944）だと、かなり遅延がありますが、1/2以下だと遅延は小さくなっていました。
1/2より小さい場合は歪み補正による遅延はそれほど大きくありませんでした。
また画像圧縮による効果は、有線LANを使っているためかほぼない、あるいは圧縮による負荷の方が影響ありそうに思えます。

自分はラジコンに載せて使用する予定なので、(648 x 486)でないと遅延が目立ってしまい使えない結果となってしまいました。