ROS2でカメラのキャリブレーションを行う


こんにちは、Ponkotsu_TKです。

はじめに

今回はROS2(Foxy)でカメラの歪みを補正したいと思います。

環境は以下の通り。

項目
Ubuntu 20.04
ROS Foxy

 インストールについてはROS1とROS2の環境構築のROS2を参照してください。

インストール

cd ~/ros2_ws/src/
git clone https://github.com/ros-perception/image_pipeline.git
mkdir ~/ros2_ws/ros2_camera_driver/
cd ~/ros2_ws/ros2_camera_driver/
git clone https://github.com/clydemcqueen/opencv_cam.git
git clone https://github.com/ptrmu/ros2_shared.git

foxy
colcon build

foxy コマンドは前回作成した環境作成コマンドです。

歪みの測定

 ROS2で歪みの測定を行います。

チェッカーボードの印刷

3次元空間に形状が既知の物体と画像平面での対応を利用してカメラの内部パラメータを推定します。
詳しく知りたい方は以下の論文を
"A flexible new technique for camera calibration". IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000.

チェッカーボードはpattern.pngをA4サイズで印刷して使いました。

歪みの推定

ターミナルでカメラを起動します。

foxy
ros2 run opencv_cam opencv_cam_main --ros-args -p index:=2 -p width:=1280 -p height:=720

ここで、index:=2/dev/video2のデバイスを参照します。デフォルトでは/dev/video0です。

次に、違うターミナルで

foxy
ros2 run camera_calibration cameracalibrator --size=9x6 --square=0.024 --approximate=0.1 --no-service-check  --ros-args --remap /image:=/image_raw

sizeは交点の数で、squareはマスの1辺の長さです。
詳しく知りたい方は、ターミナルでros2 run camera_calibration cameracalibrator -hを実行してください。

 ターミナルからGUIが生成されたら、測定を開始します。

色々動かして、右上のゲージが増やしてください。その後、CALIBRATEが押し、計算を開始させます。
私の環境では、計算に10分くらい時間がかかりました。

SAVEを押すと、

('Wrote calibration data to', '/tmp/calibrationdata.tar.gz')

とターミナルに出力されるので、解凍&修正をします。

tar -xvzf /tmp/calibrationdata.tar.gz
mv ost.txt camera.ini

ost.txtはiniファイルとして使用できます。
そして、ファイルを開いて修正

vim camera.ini
[camera]       #書き換え前
  ↓
[narrow_stereo] #書き換え後

歪み補正

では、先程作成したパラメータを利用して歪み補正をします。

準備(launchファイル)

歪み補正のlaunchファイルを作成します。

image_proc.launch.py
# Copyright (c) 2020, Open Source Robotics Foundation, Inc.
# All rights reserved.
#
# Software License Agreement (BSD License 2.0)
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of the copyright holder nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

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():
    composable_nodes = [
        ComposableNode(
            package='image_proc',
            plugin='image_proc::DebayerNode',
            name='debayer_node',
        ),
        ComposableNode(
            package='image_proc',
            plugin='image_proc::RectifyNode',
            name='rectify_color_node',
            # Remap subscribers and publishers
            remappings=[
                ('image', '/usb_cam/image_raw'),
                ('camera_info', '/usb_cam/camera_info'),
                ('image_rect', 'image_rect_color')
            ],
        )
    ]

    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='',
        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_container,
        image_processing_container,
        load_composable_nodes,
    ])

これで準備は完了です。

実行

1つ目のターミナルで

foxy
ros2 run opencv_cam opencv_cam_main --ros-args -p index:=2 -p width:=1280 -p height:=720 --remap __ns:=/usb_cam  -p camera_info_path:=${CAMERA_PARAM_PATH}/camera.ini

${CAMERA_PARAM_PATH}ではcamera.iniにパスを通してください。

2つ目のターミナルで

foxy
ros2 launch image_proc.launch.py

3つ目のターミナルで

foxy
ros2 run  image_tools showimage  /image:=/image_rect_color

結果

以下のようになりました。

カメラから補正するプログラムを経由して画像が出力されています。

最後に

ost.txtがiniファイルとして使えたことは覚えておきたいです。

質問や要望などが有りましたら、本サイトの質問かツイッターのDMでお願い致します。

参考

ROS2
ROS講座66 カメラのキャリブレーションを行う
Akira's Study Room
clydemcqueen
opencv
image_pipeline