Visual Odometry @ 大清水公園

前回の投稿から、ほぼほぼ一ヶ月。。。ちょっとサボり気味になってしまいました。。。
7/8の試走会に向けて、大清水公園のマップ作成をしていました。車輪速センサで生成したオドメトリをベースにZEDとSGBMで生成した測距データを二次元に投影することでOGMを
作ろうとしていたんですが、車輪速センサベースのオドメトリだとどうしてもドリフトの影響が大きくなってしまい、耐えられませんでした。

ということで、
1. Visual Odometry でオドメトリを生成し、そのデータをベースにOGMを作成する。
2. 1で精度が足りてない場合、バンドル調整やSLAMを使う。

という2つのアプローチでマップの精度改善に取り組むことに決めたのですが、Visual Odometryが当初思っていたより難しく、ちょっと時間がかかってしまいました。
ベースとなるコードは下記のリンクで、(OpenCV, FAST Feature, Optical Flow Tracking)ソースコードもシンプルだったので簡単に作れるかと思っていたのですが、
E行列の推定にカメラの位置移動が必要で(単純回転だと方程式が退化してしまう。)あることを知らずにずっとハマってました。

Avi Singh's blog

結果としては下記の通りで、緑線が車輪速センサベースの自己位置推定結果、赤線がVisual Odometryの自己位置推定結果でドリフトの影響が取りのぞけました。
明日からは赤線をベースにステレオの測距データを投影します。試走会一回目(7/8)まであと2週間。。。ちょっと自律走行させるとこまでは間に合わなさそうですが。。。
諦めずがんばります。。。

f:id:rkoichi2001:20170626073302p:plain

大清水公園 + SGBM + Cuda ステレオマッチング

ZEDのSDKについてくるステレオのパフォーマンスがどうもあまり良くないため,オープンソースのSGBMを探して使ってみることにしました.
結果,,,ビンゴでした.ZEDのライブラリよりも,遥かに性能が上がりました.

www.youtube.com

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の誤差がセンサーの検知誤差で,こちらは積算されることは無いですが,精度が完璧なセンサーは存在し得ないのでそのセンサー・環境に特有の誤差が乗ってきます.誤差補正を全くせずに,大清水公園で取得したデータをもとにマップを作った結果が下記になります.

f:id:rkoichi2001:20170528165225p:plain

上記マップは,ステレオカメラで取得したデータを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が一定のレンジにあるもののみを使って,花・葉っぱの影響を取り除けるか?

OpenCV 3.0のAPIに一通り目を通す.

Doneしたこと.

ステレオの結果を可視化するモジュールを作成.Done

ホワイトバランス調整モジュールを作成.Done

ステレオのデータを路面に投影し2Dにして,マップを作る.Done

GW の成果

おはようございます.前回のアップからちょっと時間が空いてしまいました...
長かったはずの GW も残念ながら終わってしまい,社会復帰中です・・・・.
GW の成果と今後の TODO を洗い出すためにエントリを残しておきます.

GWの成果

 まず,GWの成果ですが,時間をかけた割には予定していたことの半分もできませんでした.完了したこととしては,

ステレオの視差画像から,ピッチ角を推定するロジックを作成し,ROSノード化.

 ピッチ角推定に関しては,以前から作成していた vDisparity のロジックを使いました.ただ,鳥瞰図変換に関しては角度の推定誤差に対する鳥瞰図の見え方変化が大きすぎて,ピッチ角推定結果を使って鳥瞰図を作成するのが少しむずかしいかもしれません.

原画                   鳥瞰図変換後
f:id:rkoichi2001:20170511061125p:plain f:id:rkoichi2001:20170511061301p:plain

SWEEPの台車取り付けのための部品作成.

 こちらに関しては,,,,前から欲しかったのですが買ってしまいました...3Dプリンタ.割と安いやつ(でないと買えないので...)を選んだのですが,これめちゃめちゃ便利です.今まではちょっとした取付具等も「ホームセンターに買いに行く」「DIY用品で時間をかけて加工」という繰り返しだったのですが,これなら一瞬である程度正確に部品ができます.

購入した3Dプリンタ

SWEEP取り付け後の台車
f:id:rkoichi2001:20170511064611j:plain

SWEEPと作成した取り付け器具(スキャナ搭載位置高くね?というつっこみはなしでお願いします..)
f:id:rkoichi2001:20170511064615j:plain

SWEEPの ROS Wrapper を作成し,ノード化.

 SWEEPのWrapperに関しては,Scanseから提供されている下記をベースに必要な変更を入れ込みました.
github.com

ZED Stereo Camera と SWEEP を使って大清水公園でデータ取得.

スタート地点
f:id:rkoichi2001:20170509060855j:plain

ゴール地点
f:id:rkoichi2001:20170509062411j:plain

オドメトリ計測結果
オドメトリに関しては,大清水公園一周程度なら耐えられる時もあるのですが,やはりヨー角誤差が徐々に増えていってしまいなかなか厳しいです.
f:id:rkoichi2001:20170509062123p:plain

 期待の新アイテムSWEEPですが,レンジが思っていたよりもはるかに短く,LIDAR による環境地図の作成がうまくいきませんでした....

scanse.io

 一応うたい文句としては,屋外使用想定で 40m のレンジを実現と書いてあったのですが,屋外で実験してみたところコンクリートの壁で検知距離 5m 程度でした...反射率による違いは確かにあるとしても,検知距離 5m は誤算です...一応,このレーザースキャナで gmapping による環境地図を生成して自律走行させる予定だったのですが,計画変更です...

www.hokuyo-aut.co.jp

北陽電機のレンジが広いセンサーを購入すべきかどうか悩んだのですが,価格が50万前後することもあり,あきらめました.ということで,図らずも画像だけで粘るしかなさそうです...ということで,やっぱりいろいろと予定変更を余儀なくされますね...次のエントリでやることをもう一回整理します.

4月後半と GW の目標・マイルストーン

ついに来週から GW に突入です.自分の予定はと言いますと,,,特段旅行に行くわけでもなく,ロボットと向かいあう事になるかと思います.昨年の経験からすると, GW とお盆休みが受験ならぬつくばチャレンジの天王山になるはずなので,なんとか追い込んで頑張ります.で,以前のエントリにも書きましたが,もう一度 GW 中にやることを反芻します.

とりあえず,直近の目標としては,

試走会一回目(7/8)で確認走行完了する!

です.これを必達目標として考えたときに,4月・5月・6月のマイルストーンを考えると...

4月 足回りの完成と Teb Local Planner を使った軌道生成・追従

  • Teb Local Plannerのコード読み・理解.Done
  • 低速域の制御とオドメトリ精度の向上のために,エンコーダの付け替え.Done
  • オドメトリフレーム内でのロボットの制御.Done
  • Zed Stereo Camera の使い方勉強.Done
  • Zed Stereo Camera の ROS ノード作成.
  • 地図生成用台車(以降,Mapper) への Zed Stereo Camera の取り付け.
  • Sweep Lidar の使い方勉強.
  • Sweep Lidar の ROS ノード作成.
  • Mapper への Sweep Lidar の取り付け.

5月 レーザースキャナ・ステレオカメラを用いた環境地図の作成

  • gmapping のコード読み・理解.
  • gmapping を用いて環境地図が生成できるようにする.
  • 清水公園に mapper を持っていき,Stereo と Lidar のデータ取得.(マップ作成用,自己位置推定用)
  • 取得データから大清水公園の環境地図作成.
  • ステレオカメラの depth 情報を scan に変換し,地図作成に使えないかどうか検討する.

6月 限定区間・静的環境での自律走行トライ

  • amcl のコード読み・理解.
  • 5月取得のデータを用いて,amclを実行.
  • スキャナ・カメラをロボットに搭載し,自律走行トライ.

ひとまず,5月頭に大清水公園で実験データを取得することも目標に,下記のアイテムを重点的に取り組みます.

  • Zed Stereo Camera の ROS ノード作成.
  • 地図生成用台車(以降,Mapper) への Zed Stereo Camera の取り付け.
  • Sweep Lidar の使い方勉強.
  • Sweep Lidar の ROS ノード作成.
  • Mapper への Sweep Lidar の取り付け.
  • gmapping のコード読み・理解.
  • gmapping を用いて環境地図が生成できるようにする.

ということで,Zed Stereo Camera の ROS ノード作成に取り掛かります!