ステレオカメラを用いた OGM の作成2 @ 大清水公園
やりました!やりましたぞ!くらいのテンションで今日の朝5時にコードをいじってました.
Visual Odometryを使ってセンサドリフトの影響を除去し,そのオドメトリ結果を用いてステレオカメラのデータを使ってOGMを作る.
ということをやってましたが,今日一応できました.やっぱり,バンドル調整とか全体最適化みたいなことをやってないので,部分的にちょっと怪しい
とこはあるのですが,一応マップとしては使えるのではないかと.これでだめだと,ちょっと辛いです.
ステレオデータをマッピングした結果
Google Earthから取った大清水公園のルート
Google Earthの赤線は自分がランダムで引いたものなので,実際のロボットの軌道とはちょっと違いますが,木の本数とか歩道脇の花壇の段差とかはある程度拾えていると思います.
これで,大清水公園の全体地図ができたので,今度は実際にロボットが無人走行しているときにデータを取得して,そのデータが「この地図のどこと一番似ているか?」ということことを
利用して「この地図の中で,ロボットはどこを走っているか?」ということをはっきりさせます.
やっていることはGoogle Carもおなじなんですよ!(恥ずかしくなるくらい素朴ですが(笑))
明日からは,その時の時で取ったデータを,上記の地図にどうマッチングさせるかの部分を作っていきます.
Visual Odometry @ 大清水公園
前回の投稿から、ほぼほぼ一ヶ月。。。ちょっとサボり気味になってしまいました。。。
7/8の試走会に向けて、大清水公園のマップ作成をしていました。車輪速センサで生成したオドメトリをベースにZEDとSGBMで生成した測距データを二次元に投影することでOGMを
作ろうとしていたんですが、車輪速センサベースのオドメトリだとどうしてもドリフトの影響が大きくなってしまい、耐えられませんでした。
ということで、
1. Visual Odometry でオドメトリを生成し、そのデータをベースにOGMを作成する。
2. 1で精度が足りてない場合、バンドル調整やSLAMを使う。
という2つのアプローチでマップの精度改善に取り組むことに決めたのですが、Visual Odometryが当初思っていたより難しく、ちょっと時間がかかってしまいました。
ベースとなるコードは下記のリンクで、(OpenCV, FAST Feature, Optical Flow Tracking)ソースコードもシンプルだったので簡単に作れるかと思っていたのですが、
E行列の推定にカメラの位置移動が必要で(単純回転だと方程式が退化してしまう。)あることを知らずにずっとハマってました。
結果としては下記の通りで、緑線が車輪速センサベースの自己位置推定結果、赤線がVisual Odometryの自己位置推定結果でドリフトの影響が取りのぞけました。
明日からは赤線をベースにステレオの測距データを投影します。試走会一回目(7/8)まであと2週間。。。ちょっと自律走行させるとこまでは間に合わなさそうですが。。。
諦めずがんばります。。。
大清水公園 + SGBM + Cuda ステレオマッチング
ZEDのSDKについてくるステレオのパフォーマンスがどうもあまり良くないため,オープンソースのSGBMを探して使ってみることにしました.
結果,,,ビンゴでした.ZEDのライブラリよりも,遥かに性能が上がりました.
SGM実装
GitHub - dhernandez0/sgm: Semi-Global Matching on the GPU
Stixel実装
GitHub - dhernandez0/stixels: GPU-accelerated real-time stixel computation
上記のリンク先の実装はどちらもCuda化されていて,ちゃんと整理されていたので簡単にラッパーを作るだけで動きました.
本当はStixelのラッパーも作って実験するつもりだったのですが,ちょっと間に合わなかったのでまた明日.
Stixelの地面からの生え際だけを環境地図に取り込んでOGMを作るというのが直近の目標です.
週末までにはアップできると思うので,お待ち下さい!!
ステレオカメラを使ったOGMの作成
金曜朝のエントリで少し書きましたが,ステレオカメラを使ったマップを作成中です.
「GWの取り組み」エントリで書いたとおり,スキャナの性能が思いのほか高くなく,,,,結局カメラでなんとかしようとしています.
地図作成ですが,いろいろと研究されて洗礼されたテクニックが存在すると思うのですが,自分の浅い理解ではつまるところ下記の手順でやればよいのかと思います.
1.地図作成する領域(例えば大清水公園)の中で,ロボットの相対位置を正確に把握すること.
2.定期的にロボットの周辺環境を検知して,検知結果を1の情報を用いで領域の中に落としこむこと.
ここで,普通は1にも2にも誤差が含まれてしまいます.1の誤差がオドメトリ誤差で,計測値を積算していくなかでどうしても膨らんでいきます.2の誤差がセンサーの検知誤差で,こちらは積算されることは無いですが,精度が完璧なセンサーは存在し得ないのでそのセンサー・環境に特有の誤差が乗ってきます.誤差補正を全くせずに,大清水公園で取得したデータをもとにマップを作った結果が下記になります.
上記マップは,ステレオカメラで取得したデータを3次元座標に置き換えて,地表から30cm以上,150cm未満のエリアに存在する物体をプロットしたものです.見てあきらかな用に,ヨー角誤差が膨らみ,全体として地図がゆがんでいます.大清水公園を一周した地図なので,スタートとゴール地点でループが閉じないと行けないのですが,完全一致するところまで来ていません.
ただ,地図に写っている木の本数等はある程度一致していて,なんとなく環境の特徴を拾っているようにも見えます.
で,このままではちょっとローカリゼーションするのが難しそうなので,トライアンドエラーで地図を改善します.
ロボットの相対位置を正確に把握するためのアプローチとして,,,
1.VOを作ってみる.
FindEssentialMatを使って,GW取得データでVOを作る.
2.バンドル調整で最適化摩る.
VOで関連付けたカメラ座標をバンドル調整で最適化する.
センサーの誤差を抑えるためのアプローチとして,,,
1.Disparity画像に対してStixelを計算し,フリースペース内外境界のみでマップを作成する.
ちょうど金曜の朝にStixel作成のアルゴリズムを公開しているgithubのリポジトリを見つけたので,次のエントリではStixelを使ってみます.
CMake を使った自作ライブラリの作成
おはようございます.前回のエントリから2週間近く間が空いてしまい,,,若干デコミット気味になってしまいました.
作業の方は相変わらず手探りですが,
1. CMakeをつかったライブラリの作成.
2. ホワイトバランス調整のプログラム作成.
3. ステレオカメラを使ったOGMの作成.
の3つに取り組んでます.1に関しては,コードを作っていくうえで,ROSだとどうしても規模が大きくて(roscore, rviz立ち上げ等々...)デバッグしたりするのが面倒で,
まずは小さく作動するライブラリを作って,それをラップしてROSで使うことにしました.いまちょうど仕事の方でもソフトの整理的なことを少しやってまして,あとあと振り
返って使えるものにしたかったので,CMakeの使い方を少し勉強しました.
一応,下記のCMakeLists.txtに必要なものが全部入っているかと思います.(自分の使用用途範囲無いですが...)
あと,エディタにはEclipseを使っているんですが,ソースフォルダと並列にビルドフォルダを配置しないと行けないとのことで,下記のようなフォルダ構成にしました.
robotics_library
|
|-algo
|-vision
|-ground_estimation
|-src
|-include
|-build
cmake_minimum_required(VERSION 2.8) project(rosbag_reader) include(CheckCXXCompilerFlag) #set(CMAKE_BUILD_TYPE "Debug") # Specify dll name set(LIB_NAME "rosbag_reader") # Specify Src for Library # ライブラリのソースコード set(LIB_SRC "rosbag_reader.cpp") # Specify Header for Library set(LIB_HEADER "rosbag_reader.h") # Specify Src for Sample Exe (ライブラリの作成時のデバッグ用に,小さいテストプログラムを毎回作ってます.) set(LIB_SAMPLE "main.cpp") # Specify Library Type set(LIB_TYPE "SHARED") (静的ライブラリ or 共有ライブラリ) # Specify External Library # (rosbag ファイル等にアクセスしてログを使いたい時があるのですが,catkinをつかうと複雑になってしまう気がして,,,必要なものだけ入れます.) set(ROS_INCLUDE_DIRS "/opt/ros/kinetic/include") include_directories(${ROS_INCLUDE_DIRS}) set(ROS_LIB_DIRS "/opt/ros/kinetic/lib") link_directories(${ROS_LIB_DIRS}) set(BOOST_INCLUDE_DIRS "/usr/include/boost/") include_directories(${BOOST_INCLUDE_DIRS}) set(BOOST_LIB_DIRS "/usr/lib/x86_64-linux-gnu/") link_directories(${BOOST_LIB_DIRS}) # Specify install place. set(INSTALL_DIR "/home/koichi/workspace/koichi_robotics_lib/install") # Option for Release Build. set(CMAKE_CXX_FLAGS_RELEASE "-Wall -O2") # Oprion for Debug Build. set(CMAKE_CXX_FLAGS_DEBUG "-g") if(${LIB_TYPE} MATCHES "STATIC") set(LIB_TYPE_ARC "ARCHIVE") else() set(LIB_TYPE_ARC "LIBRARY") endif() # Set Build Type. if(NOT_CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE "Release") add_definitions(-DRelease) message(STATUS "Build type not specified. Defaulting to Release.") endif(NOT_CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} CACHE STRING "") message(CMAKE_BUILD_TYPE : "${CMAKE_BUILD_TYPE}") if(${CMAKE_BUILD_TYPE} MATCHES "Release") message(Build Type (Release) : "${CMAKE_BUILD_TYPE}") add_definitions(-DRelease) else(${CMAKE_BUILD_TYPE} MATCHES "Debug") message(Build Type (Debug) : "${CMAKE_BUILD_TYPE}") add_definitions(-DDebug) endif() # Check CXX compiler. Abort build if the build system does not support cxx11 CHECK_CXX_COMPILER_FLAG(-std=c++11 COMPILER_SUPPORTS_CXX11) if(COMPILER_SUPPORTS_CXX11) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") else() message(FATAL_ERROR "${CMAKE_CXX_COMPILER} has no C++11 support. Build abort. Please check.") endif() # Register OpenCV Library. # ビルド・ローカルインストールした OpenCV を使用.できるだけシステムへのインストールをしない方向でオープンソースを使ってます. set(OpenCV_DIR "/home/koichi/workspace/reps/opencv-3.1.0/install/share/OpenCV/") find_package(OpenCV) message(CMAKE_CURRENT_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") # Build and generate lib add_library(${LIB_NAME} ${LIB_TYPE} ${LIB_SRC}) target_include_directories("${LIB_NAME}" PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include) target_link_libraries("${LIB_NAME}" ${OpenCV_LIBS} rosbag rosbag_storage rostime) # Build and generate exe file. link_directories(${INSTALL_DIR}/lib) add_executable("${LIB_NAME}_test" ${LIB_SAMPLE}) target_include_directories("${LIB_NAME}_test" PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../include ${INSTALL_DIR}/include) target_link_libraries("${LIB_NAME}_test" ${OpenCV_LIBS} ${LIB_NAME} rosbag cv_bridge cpp_common roscpp_serialization) # Specify Install Folder # 以下,インストールフォルダへのライブラリ・ヘッダファイルのインストール.このフォルダに対して,ROSのラッパー作成時にリンクします. install(TARGETS "${LIB_NAME}" ${LIB_TYPE_ARC} DESTINATION "${INSTALL_DIR}/lib") install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/../include/${LIB_HEADER} DESTINATION "${INSTALL_DIR}/include")
ほんとに雑記になってしまいましたが,出勤時間が来たので(笑),ステレオカメラを使ったマップの作成は今晩か明日に!
今後のやること メモ
ブログというよりメモ帳代わりのエントリ.今後のTODO整理用.
ピッチ角推定の精度を上げる.
vDisparity からの直線抽出の精度を上げる.
取得した画像に対して,SIFT特徴量を試してみる.
SIFT特徴量でタグ付けして,検索できるようにする.
別の日時に撮影した画像に対して計算したSIFT特徴量で,前の画像を検索できるか試す.
Map座標系とOdometry座標系を紐付ける練習
特定の色のターゲットに向かってロボットを進ませるようにする.
特定の色(白)のラインをトレースするようにする.
ステレオのデータを路面に投影し2Dにして,rvizで可視化する.
ステレオからGrid Mapを作成するために,Laser Scan に変換して gmapping に入力してみる.
Mapを作成するときに,生のOdometryだと誤差が乗るため,SFMとBAが使えるかどうか試してみる.
走行可能エリアを色識別
画像に強めのフィルタをかける.
HSV変換して,色毎に分ける.
走行可能エリアを直線変換をして見つける.
画像に強めのフィルタをかける.
元の画像を膨張・縮小処理してハフ変換する.
ハフ変換する?
VOを作ってみる.
FindEssentialMatを使って,GW取得データでVOを作る.
VOで関連付けたカメラ座標をバンドル調整で最適化する.
SFMで環境地図を作るアプローチが可能かどうか確認する.
PIRFを調べてみる.
rosbag record の HDD 記録が遅い理由を調査する.
Disparity を取得して,一定の面積あたりの分散を見てみる.
元の画素をLBP変換してみたらどうなる?
画像撮影時に,路面がはっきりと出る露光時間でシャッターを切る.
ブログを読んで,SLAM・SFMの違いを見る.
Map作成にSIFT特徴量を使うときに,HUEが一定のレンジにあるもののみを使って,花・葉っぱの影響を取り除けるか?
GW の成果
おはようございます.前回のアップからちょっと時間が空いてしまいました...
長かったはずの GW も残念ながら終わってしまい,社会復帰中です・・・・.
GW の成果と今後の TODO を洗い出すためにエントリを残しておきます.
GWの成果
まず,GWの成果ですが,時間をかけた割には予定していたことの半分もできませんでした.完了したこととしては,
ステレオの視差画像から,ピッチ角を推定するロジックを作成し,ROSノード化.
ピッチ角推定に関しては,以前から作成していた vDisparity のロジックを使いました.ただ,鳥瞰図変換に関しては角度の推定誤差に対する鳥瞰図の見え方変化が大きすぎて,ピッチ角推定結果を使って鳥瞰図を作成するのが少しむずかしいかもしれません.
原画 鳥瞰図変換後
SWEEPの台車取り付けのための部品作成.
こちらに関しては,,,,前から欲しかったのですが買ってしまいました...3Dプリンタ.割と安いやつ(でないと買えないので...)を選んだのですが,これめちゃめちゃ便利です.今まではちょっとした取付具等も「ホームセンターに買いに行く」「DIY用品で時間をかけて加工」という繰り返しだったのですが,これなら一瞬である程度正確に部品ができます.
購入した3Dプリンタ
★24時間以内発送★QIDI TECH シングル押出機 3Dプリンター 新モデル:X-1 無料の1 PLAフィラメントと, 公式の旗艦店【その日配達、日曜日は除く】
- 出版社/メーカー: QIDIテクノロジー
- メディア:
- この商品を含むブログを見る
SWEEP取り付け後の台車
SWEEPと作成した取り付け器具(スキャナ搭載位置高くね?というつっこみはなしでお願いします..)
SWEEPの ROS Wrapper を作成し,ノード化.
SWEEPのWrapperに関しては,Scanseから提供されている下記をベースに必要な変更を入れ込みました.
github.com
ZED Stereo Camera と SWEEP を使って大清水公園でデータ取得.
スタート地点
ゴール地点
オドメトリ計測結果
オドメトリに関しては,大清水公園一周程度なら耐えられる時もあるのですが,やはりヨー角誤差が徐々に増えていってしまいなかなか厳しいです.
期待の新アイテムSWEEPですが,レンジが思っていたよりもはるかに短く,LIDAR による環境地図の作成がうまくいきませんでした....
一応うたい文句としては,屋外使用想定で 40m のレンジを実現と書いてあったのですが,屋外で実験してみたところコンクリートの壁で検知距離 5m 程度でした...反射率による違いは確かにあるとしても,検知距離 5m は誤算です...一応,このレーザースキャナで gmapping による環境地図を生成して自律走行させる予定だったのですが,計画変更です...
北陽電機のレンジが広いセンサーを購入すべきかどうか悩んだのですが,価格が50万前後することもあり,あきらめました.ということで,図らずも画像だけで粘るしかなさそうです...ということで,やっぱりいろいろと予定変更を余儀なくされますね...次のエントリでやることをもう一回整理します.