一回目試走会(7/8)に向けて
うーん...来週の説明会&試走会ですが,あと一週間になりましたが,,,ロボット走らせるところまでもっていくのはだいぶ難しそうです.
今回を逃すと,次は9/23でだいぶ後ろになってしまうのでどうにか脱初心者コースを達成したかったんですが...
実際に走らせるところまで達成しようとすると...
1.NVIDIA Jetson のセットアップ
2.NVIDIA Jetson のターゲットコンパイル・動確
3.AMAZON で Jetson が動くバッテリーの購入.
4.ステレオデータを用いた ROS AMCL パッケージの変更
5.Navigation Stackの調整
くらいですかね...どう考えても終わらないなあ...
とりあえず週末頑張ります.次はJetsonの記事をアップします.
ステレオカメラを用いた 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で関連付けたカメラ座標をバンドル調整で最適化する.