今回はステレオカメラです。この機能自体はROSのパッケージを導入すれば実現可能ですが、よい結果を得るためにはキャリブレーション（校正）をしっかりと行うことがステレオ性能を左右する重要な要素となります。
今回はステレオカメラを良好に動作させるためのキャリブレーション方法について紹介いたします。
ROSを使ったステレオカメラキャリブレーション
今回はWebカメラ２台を使い、ステレオカメラを構築してみます。これにより、精度は低いものの第６回で使用したPMD CamBoard pico flexxのように周囲の情報を３次元の点群として得ることが可能です。
ステレオカメラは、２つのカメラを平行に並べ、このカメラの視差から奥行きを測定するものです。視差を使うあたり人間の認識方法に似ていますね。
ステレオカメラのキャリブレーション
ステレオカメラのキャリブレーション手順は、ROSの手順通り進めましょう。順を追って作業していきましょう。まずはカメラ関係のパッケージを導入しておきましょう。
sudo apt-get install -y ros-indigo-uvc-camera sudo apt-get install -y ros-indigo-image-* sudo apt-get install -y ros-indigo-rqt-image-view sudo apt-get install -y ros-indigo-camera-calibration sudo apt-get install -y ros-indigo-stereo-image-proc
今回は次のカメラを２つ購入しました。
BUFFALO マイク内蔵500万画素WEBカメラ FullHD1080p対応モデル ブラック BSW50KM02BK
他にも、同じくバッファローの安いWebカメラBSWHD06Mを2台用意して試しました。
２台のカメラは、ずれないように固定する必要があります。今回は殻割りして基板をアクリル板の上に絶妙に固定してみました。
カメラをUbuntuに接続します。接続確認してみましょう。
lsusb Bus 001 Device 013: ID 0458:708c KYE Systems Corp. (Mouse Systems) Genius WideCam F100 Bus 001 Device 014: ID 0458:708c KYE Systems Corp. (Mouse Systems) Genius WideCam F100
テストします。
$ roscore $ rosrun uvc_camera uvc_camera_node _device:=/dev/video1 $ rosrun image_view image_view image:=/image_raw
画面にカメラ画像が表示されることを確認します。
uvc_stereo_nodeで右カメラと左カメラをまとめて起動することが出来ます。
$ rosrun uvc_camera uvc_stereo_node _left/device:=/dev/video1 _right/device:=/dev/video2
しかし起動すると
terminate called after throwing an instance of 'std::runtime_error' what(): unable to start capture 中止 (コアダンプ)
となり、エラーで終了してしまいます。これはUSBハブにカメラ２台を接続していたためでした。帯域が足りずにエラーになってしまうようです。
パソコンのUSBポートに直接接続すれば問題なく起動出来ました。
しかしまだ問題が発生。uvc_stereo_nodeを使うと、何故かうまく動作しません。５秒に１回ぐらいしか配信されない状況です。ただ、これはPC再起動で直りました。
それではカメラ２台をまとめて起動します。
rosrun uvc_camera uvc_stereo_node _left/device:=/dev/video1 _right/device:=/dev/video2 _fps:=30その後、次のコマンドでキャリブレーションします。
rosrun camera_calibration cameracalibrator.py --size 10x7 --square 0.024 right:=/right/image_raw left:=/left/image_raw left_camera:=/left right_camera:=/right
まず、左右のカメラが像が表示されます。最初に重要なのは「カメラのピントを固定する」という点です。キャリブレーション前にピントをしっかりと合わせておきましょう。このWebカメラの場合、リングをまわすとピントが変更されます。またキャリブレーション後にピントがずれてしまうとこれも意味がありません。注意しましょう。
ピントが調整できたら、次はチェッカーボードの出番です。白と黒のマスがならんだ図形でカメラの歪み補正を行います。
ここやここなどからPDFをダウンロード、印刷して、歪まない板などに貼り付けて利用します。今回はA3用紙に印刷し、厚めのアクリル板に貼り付けて利用しました。
10x7はチェッカーボードのマスの数-1です。交差する点の数ということです。
0.024はチェッカーボードのマスの辺の長さです。印刷後にノギスなどで測ります。単位はmです。
CALIBRATEボタンを押すと開始されます。チェッカーボードを持ってカメラにかざします。認識されると、虹色に丸とラインが引かれて認識されたことがわかります。
ゆっくりと回転させたり、上下左右に移動させます。また遠く、近くに動かしたり、角度を変化させたりすると、X、Y、Size、Skewのメーターが伸びてきます。
十分な画像が集まれば、CALIBRATEボタンが押せるようになります。押すと計算が始まります。
計算が終わるとSAVEとCOMMITボタンも押せるようになります。SAVEボタンで別途バックアップを保存することもできます。
('Wrote calibration data to', '/tmp/calibrationdata.tar.gz')
COMMITを押すと、データを保存して終了です。
再度、uvc_stereo_nodeを起動してみます。すると
[ INFO] [1472012767.076202308]: using default calibration URL [ INFO] [1472012767.076319059]: camera calibration URL: file:///home/hiroakikaneda/.ros/camera_info/left_camera.yaml [ INFO] [1472012767.079758680]: using default calibration URL [ INFO] [1472012767.079800185]: camera calibration URL: file:///home/hiroakikaneda/.ros/camera_info/right_camera.yaml
と表示され、キャリブレーション用データが読み込まれたことがわかります。
ステレオイメージの取得
まず、ステレオカメラを起動します。ROS_NAMESPACE=stereo rosrun uvc_camera uvc_stereo_node _left/device:=/dev/video1 _right/device:=/dev/video2
$ ROS_NAMESPACE=stereo rosrun uvc_camera uvc_stereo_node _left/vice:=/dev/video1 _right/device:=/dev/video2 [ INFO] [1477891411.823536447]: using default calibration URL [ INFO] [1477891411.823680665]: camera calibration URL: file:///home/my_robo/.ros/camera_info/left_camera.yaml [ INFO] [1477891411.825454630]: using default calibration URL [ INFO] [1477891411.825513301]: camera calibration URL: file:///home/my_robo/.ros/camera_info/right_camera.yaml opening /dev/video1 pixfmt 0 = 'YUYV' desc = 'YUV 4:2:2 (YUYV)' discrete: 1280x720: 1/8 discrete: 800x600: 1/15 1/8 discrete: 640x480: 1/30 1/20 1/15 1/8 discrete: 640x360: 1/30 1/20 1/15 1/8 discrete: 352x288: 1/30 1/20 1/15 1/8 discrete: 320x240: 1/30 1/20 1/15 1/8 discrete: 176x144: 1/30 1/20 1/15 1/8 discrete: 160x120: 1/30 1/20 1/15 1/8 pixfmt 1 = 'MJPG' desc = 'MJPEG' discrete: 1280x960: 1/12 discrete: 1280x720: 1/30 1/24 1/20 1/16 discrete: 800x600: 1/30 1/24 1/20 1/16 discrete: 640x480: 1/30 1/24 1/20 1/16 discrete: 640x360: 1/30 1/20 1/15 1/8 discrete: 352x288: 1/30 1/24 1/20 1/15 discrete: 320x240: 1/30 1/20 1/15 1/8 discrete: 176x144: 1/30 1/20 1/15 1/8 discrete: 160x120: 1/30 1/24 1/20 1/16 int (Brightness, 0, id = 980900): -255 to 255 (1) int (Contrast, 0, id = 980901): 0 to 30 (1) int (Saturation, 0, id = 980902): 0 to 127 (1) int (Hue, 0, id = 980903): -16000 to 16000 (1) bool (White Balance Temperature, Auto, 0, id = 98090c): 0 to 1 (1) int (Gamma, 0, id = 980910): 20 to 250 (1) menu (Power Line Frequency, 0, id = 980918): 0 to 2 (1) 0: Disabled 1: 50 Hz 2: 60 Hz int (White Balance Temperature, 16, id = 98091a): 2500 to 7000 (1) int (Sharpness, 0, id = 98091b): 0 to 64 (1) int (Backlight Compensation, 0, id = 98091c): 0 to 2 (1) opening /dev/video2 pixfmt 0 = 'YUYV' desc = 'YUV 4:2:2 (YUYV)' discrete: 1280x720: 1/8 discrete: 800x600: 1/15 1/8 discrete: 640x480: 1/30 1/20 1/15 1/8 discrete: 640x360: 1/30 1/20 1/15 1/8 discrete: 352x288: 1/30 1/20 1/15 1/8 discrete: 320x240: 1/30 1/20 1/15 1/8 discrete: 176x144: 1/30 1/20 1/15 1/8 discrete: 160x120: 1/30 1/20 1/15 1/8 pixfmt 1 = 'MJPG' desc = 'MJPEG' discrete: 1280x960: 1/12 discrete: 1280x720: 1/30 1/24 1/20 1/16 discrete: 800x600: 1/30 1/24 1/20 1/16 discrete: 640x480: 1/30 1/24 1/20 1/16 discrete: 640x360: 1/30 1/20 1/15 1/8 discrete: 352x288: 1/30 1/24 1/20 1/15 discrete: 320x240: 1/30 1/20 1/15 1/8 discrete: 176x144: 1/30 1/20 1/15 1/8 discrete: 160x120: 1/30 1/24 1/20 1/16 int (Brightness, 0, id = 980900): -255 to 255 (1) int (Contrast, 0, id = 980901): 0 to 30 (1) int (Saturation, 0, id = 980902): 0 to 127 (1) int (Hue, 0, id = 980903): -16000 to 16000 (1) bool (White Balance Temperature, Auto, 0, id = 98090c): 0 to 1 (1) int (Gamma, 0, id = 980910): 20 to 250 (1) menu (Power Line Frequency, 0, id = 980918): 0 to 2 (1) 0: Disabled 1: 50 Hz 2: 60 Hz int (White Balance Temperature, 16, id = 98091a): 2500 to 7000 (1) int (Sharpness, 0, id = 98091b): 0 to 64 (1) int (Backlight Compensation, 0, id = 98091c): 0 to 2 (1)
次にステレオイメージを作成します。
ROS_NAMESPACE=stereo rosrun stereo_image_proc stereo_image_proc
上記を実行後rostpoic listすると、/stereo_image_proc/のトピックが配信されていることが確認できます。
$ rostopic list /rosout /rosout_agg 中略 /stereo/stereo_image_proc/parameter_descriptions /stereo/stereo_image_proc/parameter_updates /stereo/stereo_image_proc_debayer_left/parameter_descriptions /stereo/stereo_image_proc_debayer_left/parameter_updates /stereo/stereo_image_proc_debayer_right/parameter_descriptions /stereo/stereo_image_proc_debayer_right/parameter_updates /stereo/stereo_image_proc_rectify_color_left/parameter_descriptions /stereo/stereo_image_proc_rectify_color_left/parameter_updates /stereo/stereo_image_proc_rectify_color_right/parameter_descriptions /stereo/stereo_image_proc_rectify_color_right/parameter_updates /stereo/stereo_image_proc_rectify_mono_left/parameter_descriptions /stereo/stereo_image_proc_rectify_mono_left/parameter_updates /stereo/stereo_image_proc_rectify_mono_right/parameter_descriptions /stereo/stereo_image_proc_rectify_mono_right/parameter_updates
作成されたデプス画像の確認
stereo_image_procrosrun image_view stereo_view stereo:=/stereo image:=image_rect_color _approximate_sync:=True
以上でステレオイメージが取得できました。
では、調整にあたり、対象物を決めておくとよいでしょう。今回は、本と2本のコーヒー缶を使いました。
これらがカメラのほぼ正面にくる様に配置します。また、カメラから対象物までの画像をメジャーで測定しておきましょう。
今回、後ろの本までの距離が約1.170m
手前のコーヒー缶が約0.900m
中央のコーヒー缶はその中間に位置しています。
次のコマンドでリアルタイムでパラメータを調整することが可能です。
rosrun rqt_reconfigure rqt_reconfigure
stereo_image_procを選択し、パラメータを変更してみましょう。stereo_algorithはアルゴリズムを変更できますBM(ブロックマッチング)とSGBM（セミグローバルブロックマッチング）が選べます。
このアルゴリズムでは差分絶対値和ウィンドウを用い左右の画像の間で対応点を見つけるものです。
アルゴリズムを理解しておかないと、パラメータの調整は難しいと思います。このアルゴリズムは次のように処理を行います。
１．画像の明るさを正規化しテクスチャを強調するプリフィルタ処理。
２．差分絶対値和ウィンドウを用いて水平なエピポーラ線に沿って対応点を探索
３．不良な対応点を削除するために事後フィルタリング処理。
パラメータの意味は次の通りです。
prefilter_size ： プリフィルタ処理。上記の１の処理です。サイズは5〜21の間ぐらいで調整します。
prefilter_cap ： およそ31ぐらいの値で調整します。小さければ処理は速くなりますがノイズが目立つようになります。
correlation_window_size ： 絶対値の差の合計（SAD）を用いて対応点探索を行います。5,7, … ,21間で調整します。
min_disparity ： 最小視差です。
disparity range ： 最大視差と最小視差のレンジ
uniqueness_ratio ： 事後フィルタリングで誤ったマッチを除去するためのパラメータ
texture_shrshold ： テクスチャがない領域を無視します。
speckle_size ： 視差変化ウィンドウ。除去されるスペックルの最大面積です。大きくしすない程度に調整してノイズを低減させます
speckle_range ： 許容される視差範囲。ウィンドウ内の変化の許容範囲です。
スレテオマッチングの手順としては、まず重要な値である、ウィンドウサイズと視差のレンジを決め、それから他のパラメータで微調整する、という流れが効率的です。
今回は、correlation_window_size を、５、１５、 ２１と変更し、disparity range を48、 96、 128と変更して画面キャプチャを取得してみました。
ではキャプチャしたこれらの画像を並べて見ましょう。
correlation_window_size=5、disparity range=48
correlation_window_size=15、disparity range=96
correlation_window_size=15、disparity range=128
correlation_window_size=21、disparity range=48
correlation_window_size=21、disparity range=96
correlation_window_size=21、disparity range=128
このように並べて見ることで、ウィンドウサイズ・視差レンジを少しずつ変更した場合に最も精度の出ている組み合わせがどれなのかがよくわかります。
今回作成したステレオカメラではcorrelation_window_size=15、disparity range=128を基本とし、他のパラメータを微調整していくこととしました。
あとは、speckle_sizeを微調整して、ノイズが発生しない程度に微調整したり、texture_shrsholdを調整してノイズを低減させたり、uniqueness_ratio（事後フィルタリングで誤ったマッチを除去するためのパラメータ）の値をあげてノイズを低減させたりと微調整してください。各パラメータはあまり大きく動かさない方がよいでしょう。微調整し、最もよい結果が出るように調整すれば、カメラのキャリブレーションは完成です。
RVizで使用してみる
それでは実際にどれぐらいの精度が出ているのかRVizで見てみましょう。ほぼ正面から見た図です。
斜め上から見た図です。位置関係が分かりますでしょうか。
上部から見た図です。ほぼ、実際の配置とおなじ位置に表示されていることが確認できます。
では本当に正しく距離を図れているのか確認してみましょう。
RVizではメジャー機能により長さを図ることができます。カメラから、奥には位置した本までの距離をはかってみます。
ちょっと曲がってしまいましたが、測定結果は約1.151mということで、誤差は0.019m。ご覧のとおりなかなかの精度が出ていることが分かると思います。
まとめると、ステレオカメラキャリブレーションのポイントとしては
・2つのカメラをしっかり固定する。
・事前にピンと合わせを終わらせておく。
・事前に距離を計り、出力されたPointCloudのデータの距離感とあっているかチェックする。
となります。
ステレオカメラはキャリブレーションやパラメータ調整が面倒ではありますが、一度調整さえしてしまえば、比較的広域にわたる範囲の距離をデータ化することができます。この奥行き情報を使うことで、ロボットは外界の様子を確認しながら、例えば障害物を避けながら目的の場所へ移動する、といった事が可能になるわけです。
今回はWebカメラを用いましたが、画像がぼやけ気味となります。また、ロボットが激しく動く場合に像がブレてぼやけるという現象が発生します。このとき、通常のWebカメラでは画像がゆがむ現象が発生します。これを解決するため高速動作かつグローバルシャッターを搭載したカメラを選定します。この様な問題を解決したカメラを選択する事で、さらなる精度向上を行うことが可能です。次のステップとして、もう少し高性能なカメラで試してみる予定です。
ROSで始めるロボティクス（6） ー ROSでデプスカメラPMD CamBoard pico flexxを使う
ROSで始めるロボティクス（7） ー ロボットのための二次元mapを作る
ROSで始めるロボティクス（8） ー ロボットのナビゲーションを行う
ROSで始めるロボティクス（9） ー ROSを使ったステレオカメラキャリブレーションROSで始めるロボティクス（7） ー ロボットのための二次元mapを作る
ROSで始めるロボティクス（8） ー ロボットのナビゲーションを行う
参考文献：
詳解 OpenCV
詳解 OpenCV
記載されている会社名、および商品名等は、各社の商標または登録商標です。
作成されたデプス画像の確認
返信削除の項目したのコマンドが間違っています。
これでは動作しないと思います。