Zed Stereo Camera 事始め2
前回のエントリからの続きです．前回のエントリまでで，とりあえずサンプルコードが動くところまではできたので，今回はいよいよサンプルコードを実行します．
提供されているサンプルコード一覧
- ZED_Camera_Control : カメラの制御変数変更や簡単な動作確認．
- ZED_Positional_Tracking : カメラの位置トラッキング
- ZED_SVO_Recording : ステレオを用いた Visual Odometry
- ZED_Depth_Sensing : 視差マップ生成
- ZED_Spatial_Mapping : 空間マッピング
- ZED_with_OpenCV : OpenCV を用いた画像の表示
ということで，視差マップの生成をまず試してみました．
原画
視差マップ
うーん．撮影している風景が近距離すぎるのか，思ったほどでもないような．．．OpenCVの SGBM とそんなに大きく変わらないような気もします．
実際に外で撮影してどのくらい綺麗に Depth が取れるかどうかが大事なのでもう少しいじってみます．いずれにしても，同期レベルの高い2つのフレームを取得
できるようになったことは進歩かと思います．
Zed Stereo Camera 事始め1
ということで，足回りの目処が立ったので Zed Stereo Camera の環境構築にとりかかります．
Stereo Camera に付属している SDK に視差マップを生成してくれるソフトが付いているのですが，どうやらこれが NVIDIA の GPU がないと動かないみたいで．．．
ロボットにつけている Intel NUC には GPU がついてないので，どうしようかな．．．．と思ってます．ひとまず， NVIDIA の GPU が付いているノートパソコンに環境を作って動かすことにしました．
1.ZED SDK のダウンロード
まず， ZED の SDK を下記リンクからダウンロードします．
ZED SDK 1.2 - Developer Center
3.Cuda8.0 のダウンロード
4.Cuda8.0 のインストール
下記ドキュメントの必要なとこだけ拾っていけば，割とすんなりとインストールできました．
docs.nvidia.com
５．OpenCV3.1 のビルド・インストール
最後， OpenCV3.1 のダウンロードとインストールです．ROSとの兼ね合いもあるので，個別の OpenCV のシステムディスクへのインストールは避けたく，ローカルに適当なインストールフォルダを作ってそこに共有ライブラリのパスを設定します．
OpenCV のダウンロード，3.1.0へのブランチ切り替え，opencv_contribのダウンロード
$ git clone https://github.com/opencv/opencv.git opencv-3.1.0 $ git checkout 3.1.0 $ git clone https://github.com/opencv/opencv_contrib.git opencv_contrib
なお，Cuda8.0との兼ね合いで一部ソースコードを変更しないとビルドが通りませんでした．
"/modules/cudalegacy/src/graphcuts.cpp"のL46の記述を下記のように変更します．
Before :
#if !defined (HAVE_CUDA) || defined (CUDA_DISABLER)
After :
// GraphCut has been removed in NPP 8.0 +#if !defined (HAVE_CUDA) || defined (CUDA_DISABLER) || (CUDART_VERSION >= 8000)
cmake のコンフィグレーション設定
cmake -DCMAKE_INSTALL_PREFIX=/installpath/reps/opencv-3.1.0/install -DBUILD_EXAMPLES=ON -DINSTALL_C_EXAMPLES=ON -DWITH_OPENGL=ON -DWITH_V4L=ON -DWITH_QT=ON -DOPENCV_EXTRA_MODULES_PATH=/installpath/reps/opencv-3.1.0/opencv_contrib/modules ..
ビルド実行 & インストール
make -j8 make install
Linux コマンド ldd, ldconfig
Linux のコマンドのお勉強です．（すごい基礎的で恐縮ですが．．．）
ldd : 共有ライブラリへの依存関係参照．
使おうとしている実行ファイルが共有ライブラリを用いている際，実行時に動的リンクされます．つまり，実行時にメモリに読み込まれて使われます．このコマンドでは，リンクしているライブラリの一覧を調べることができます．例えば，今 ZED Stereo Camera の導入をしているんですが，その一つの実行ファイルがリンクしている共有ライブラリを調べたければ，下記のように ldd を実行すればわかります．ちょっとしたコマンドに対して ldd をやって見ると，以下に多くのライブラリが使われているのかがわかりますね．
$ ldd ZED_Camera_Control
linux-vdso.so.1 => (0x00007ffc63fe7000)
libsl_zed.so => /usr/local/zed/lib/libsl_zed.so (0x00007f48f4b53000)
libsl_core.so => /usr/local/zed/lib/libsl_core.so (0x00007f48f4912000)
・・・・
$ ldd /bin/ls
linux-vdso.so.1 => (0x00007ffef4d88000)
libselinux.so.1 => /lib/x86_64-linux-gnu/libselinux.so.1 (0x00007f0275917000)
libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x00007f027554e000)
libpcre.so.3 => /lib/x86_64-linux-gnu/libpcre.so.3 (0x00007f02752dd000)
libdl.so.2 => /lib/x86_64-linux-gnu/libdl.so.2 (0x00007f02750d9000)
/lib64/ld-linux-x86-64.so.2 (0x0000561ed5bde000)
libpthread.so.0 => /lib/x86_64-linux-gnu/libpthread.so.0 (0x00007f0274ebc000)
ldconfig : 共有ライブラリを探索するパスの設定．
実行時に動的リンクされるライブラリの場所をシステムに把握させておく必要があります．このコマンドでは，共有ライブラリを探索するパスを指定できます．ライブラリパスは /etc/ld.so.conf.d に conf ファイルを作成し，作成後に ldconfig コマンドを実行すればライブラリパスとして設定してくれるみたいです．今回はお試しに自分のローカルで OpenCV 3.1.0 をビルドしたので，そのパスを/etc/ld.so.conf.d/opencv.confというファイルを作成してそこに記述しました．
ROS Teb Local Planner + Arduino Due + Rotary Encoder
ということで，前回のエントリから早くも一週間．．．
ようやくエンコーダ周りの基板作成・マイコン実装変更・ROS対応が完了しました．長かった．．．．
エンコーダの解像度が上がったことで，低速での制御が可能になったことと，オドメトリの精度がアップしました．
あと，二つの Arduino Leonard を一つの Arduino Due にまとめたことで，ロボットのスペースがだいぶすっきりしました．
空いたスペースに，NVIDIA の Jetson TX2 を搭載して画像処理をしたいな～．なんて，新しいもの試したいいつものパターンにはまりつつありますが，これは時間があれば．．．
組み込みシステムの開発者キットとモジュール | NVIDIA Jetson | NVIDIA
先ほどようやく Teb Local Planner で軌道生成するところが完了し，ひとまず足回りは完了です．
上記ビデオのリンクですが，指定した複数の地点に対して Teb Local Planner を使って軌道生成してます．一つの緑のガムテープのマス目が 50cm なので，割と厳しめの経路になっているかと．要求しているパスポイントは，下記のとおりです．
(x, y, theta) : (0, 0, 0) -> (0.5, 0, 0) -> (1.0, 0.5, 0) -> (1.5 0.0, -90deg) -> (1.0 -0.5, -180deg)
経路生成とその追従に関しては，カメラやLidarなどのセンサーとくっつけたときにまだまだ課題が出てくると思うのですが，ひとまず明日からは Zed Stereo Camera をパソコンで使えるようにします．
Arduino Due + Rotary Encoder
前回のエントリから２週間弱が経過してしまい，，，当初の目標をちょっとデコミットしてしまいました．．．がそんなときもあります．ただ，何もやっていなかったわけではなく，，，あらたな問題を解決しています（笑）
実は，TebLocalPlannerの軌道生成に使うにあたって，使っていたエンコーダの分解能が荒すぎることが発覚し，いまこの付け替えをしてます．なんだか素朴な感じで恥ずかしいのですが，もともと田宮のラジコンをベースに使っていたため，ラジコンのタイヤにネオジム磁石を張り付けてこれをホールセンサで読み取ることでオドメトリを計算してました．
タイヤとネオジム磁石
ホールセンサ
で，タイヤの直径が12cmで，一周37.68cmになります．一秒間に一周したとしたら大体40cm/sになるのですが，張り付けた磁石の数が20個なので50msに一回パルスが立つか立たないかという計算になります．このくらいの速度が出ていれば問題ないのですが，停止状態から滑らかに速度を立ち上げる場合，低速域でもある程度速度コントロールができないといけないのですが，ここがどうしても荒くなってしまって最終位置精度を高くできませんでした．
ということで，思い切ってエンコーダを付け替えることにしました．購入したエンコーダは結構いいやつで，OMRONの産業用のものです．
が，付け替えようと思った矢先，マイコンの割込みのピンが足りないことが発覚．．．結局マイコン周りも大幅に改修しないといけなくなってしまいました．．．マイコンは Arduino Due を聖地（笑）秋葉原まで行って購入してきました．UNOやLeonardと違ってAVRではなくARMを使っているとのことで，クロック数も上がっていたのでマイコンを一つに集約しました．
平日は朝仕事前に作業をしていたのですが，ハードをいじりだすと作業毎にいちいち部屋を片付けられないのでリビングが荒れ放題です（笑）．独身万歳ですね．
今週末でハードの取り付け・マイコン周りのＳＷの整理を終わらせたいのですが．．．頑張ります．制御のキメが細かくなったらまたアップします．
つくばチャレンジ2017への参加のお勧め
ついに来てしまいました．．．．そうですつくばチャレンジ２０１７の参加募集開始です．
昨年ＳＩ２０１６に参加し，つくばチャレンジ２０１６の取り組みを終えてから早くも半年．．．それなりに前もって動いては来ましたが，まあなかなか思うようには進まないもんですね．
今年は３回目の参加ということで，そろそろ結果出したいです．
とりあえず，自戒の意味も込めて，２月７日に書いた今年の目標を反芻します．
- １．月ごとに目標・プランニングをしてブログで公表する．
週ごとに達成内容をブログで残す．（備忘録・整理目的）
自分の知識不足もあり，年単位のプランニングがやっぱり難しいので，月ごとに取り組む内容を決めて，ブログで公表します．で，週ごとの成果をブログで公表します．
- ２．まず，できる限りありものを使って，走るところまで持っていく．
フレームワークとかライブラリとかの使い方を勉強するのがめんどくさくて，昨年はもっとシンプルなものを自分で作れないかと思っていたのですが，まずはありものをできる限りパクる・再利用するという方針で作り上げていきたいと思います．
２月・３月でROS Navigation Stackを理解して動かすところまでを目標にやってきました．ちょっと手直しが発生したりして（ここは後のエントリで書きます．）完全には終わらなかったのですが，それっぽく動くところまでは来ました．
本番（２０１７年１１月）に完走目指す！となると，かなりハードルが高いのは痛いほどわかっていますが，目標は高く！ということで，それを目標に考えるととりあえず一回目の説明会の時点で「確認走行区間の自律走行達成」を実現しないと苦しくなるかと思います．これを目標にざっくりスケジュールを組むと．．．
４月 足回りの完成とTebLocalPlannerを使ったローカルプランニング
後のエントリで書きますが，TebLocalPlannerを使うにあたってエンコーダの分解能が荒すぎて細かい制御ができなかったため高いロータリーエンコーダを買いました．．．取り付けにするにあたって，Arduinoのピンが足りないことが発覚し，，，，結局マイコンも新しくしました．今取り付けしてますが，今月はこの足回りをかっちりさせることを最低限の目標にしてます．あとは，Zed Stereo Camera が届いたので，こいつの使い方をマスターします．
５月 レーザスキャナ・ステレオカメラを用いたマッピング
SWEEPを発注したのですが，5月には届いているはず．．．ということで，５月はレーザースキャナ・ステレオカメラを用いてマッピングします．できればゴールデンウィークに大清水公園に行ってデータを取ってこられれば．
６月 限定区間・静的環境での自律走行トライ
５月に作成したマップに対してＡＭＣＬを用いてローカリゼーションを実施して，少なくとも静的な環境では自律走行できるようにします．
７月 試走会一回目
一回目試走会．今年はここでの確認走行完了を目指します！
ということで，皆様応援よろしくお願いします！！！
Gauss Newton 法，Levenberg-Marquardt 法 in Python
ちょっと前回の投稿からアップが遅れてしまいましたが，さぼっていたわけではなく．．．
引き続き Teb Planner のパラメータ調整と解析をしてたんですが，もう少し詳しく理解したかったんでソースを読んでみました．
で，気づいたんですが，LSD SLAM にしても， Teb Planner にしても，バンドル調整にしても，最近よく出てくる（少なくとも自分の周りで）技術は，結局非線形方程式のパラメータ最適化をしているなと．
で，この非線形方程式の最適化なんですが，あまりちゃんと勉強したことが今までなかったのでちょっとハードルが高かったのですが，せっかくの機会なのでちょっとかじってみることにしました．
読んだ本は，有名な下記の本です．さすが有名なだけあって，わかりやすかったですね．
- 作者: 金谷健一
- 出版社/メーカー: 共立出版
- 発売日: 2005/09/01
- メディア: 単行本
- 購入: 29人 クリック: 424回
- この商品を含むブログ (41件) を見る
なかなかちゃんと理解するのに時間がかかったんですが，根本的なアイデアとしては一次元のニュートン法が出発点になるんですね．
でも，高次元になるとヘッセ行列とかがいっぱい出てきて，途端に難しくなります．．．下記，簡単な例ですが，Pythonで非線形方程式のパラメータ推定をやってみました．
1. Gauss Newton 法
import numpy as np
import numpy.linalg as linalg
import random
# Function is
# f(x, y) = u0*x^3 + u1*x^2*(y^2 + u2*y)
def f(x, y, z, u):
return u[0] * x**3 + u[1] * x**2 * (y**2 + u[2] * y) - z
def df_du0(x, y, u):
return x**3
def df_du1(x, y, u):
return x**2 * (y**2 + u[2] * y)
def df_du2(x, y, u):
return u[1] * x**2 * y
def calculateDeltaU(H_sum, J_sum):
return linalg.lstsq(H_sum, J_sum)
def jacobian(x, y, z, u):
nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]])
return f(x, y, z, u) * nablaF
def hessian(x, y, u):
nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]])
return nablaF * nablaF.T
def createDataSet(x, y, u):
z = []
for i in range(len(x)):
noise = random.random() * 0.01
z.append(f(x[i], y[i], 0, u) + noise)
return z
def gaussNewton(x, y, z, u0, eps):
u = np.array(u0).reshape((3, 1)).astype(float)
oldDeltaU = np.zeros((3, 1))
deltaU = np.zeros((3, 1))
while(True):
J_sum = np.zeros((3, 1))
H_sum = np.zeros((3, 3))
for i in range(len(x)):
H_sum += hessian(x[i], y[i], u)
J_sum += jacobian(x[i], y[i], z[i], u)
oldDeltaU = deltaU
deltaU = calculateDeltaU(H_sum, J_sum)[0]
u -= deltaU
if (linalg.norm(deltaU[0] - oldDeltaU) < eps):
break
return u
if __name__ == "__main__":
trueU = [0.8, 0.8, 0.8]
u0 = [1.5, 1.5, 15]
x = [1 for i in range(10)]
y = [i for i in range(10)]
z = createDataSet(x, y, trueU)
u = gaussNewton(x, y, z, u0, 0.00001)
print("Estimated u : ", u)
2. Levenberg-Marquardt 法
# -*- coding: utf-8 -*-
import numpy as np
import numpy.linalg as linalg
import random
# Function is
# f(x, y) = u0*x^3 + u1*x^2*(y^2 + u2*y)
def f(x, y, z, u):
return u[0] * x**3 + u[1] * x**2 * (y**2 + u[2] * y) - z
def J(x, y, z, u):
totalJ = 0
for i in range(len(x)):
totalJ += (f(x[i], y[i], z[i], u))**2
return 0.5 * totalJ
def df_du0(x, y, u):
return x**3
def df_du1(x, y, u):
return x**2 * (y**2 + u[2] * y)
def df_du2(x, y, u):
return u[1] * x**2 * y
def calculateDeltaU(H_sum, J_sum):
return linalg.lstsq(H_sum, J_sum)
def jacobian(x, y, z, u):
nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]])
return f(x, y, z, u) * nablaF
def hessian(x, y, u):
nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]])
return nablaF * nablaF.T
def createDataSet(x, y, u):
z = []
for i in range(len(x)):
noise = random.random() * 0.01
z.append(f(x[i], y[i], 0, u) + noise)
#z.append(f(x[i], y[i], 0, u))
return z
def levenbergMarquardt(x, y, z, u0, eps):
c = 0.0001
u = np.array(u0).reshape((3, 1)).astype(float)
tmpu = u
oldDeltaU = np.zeros((3, 1))
deltaU = np.zeros((3, 1))
newJ = J(x, y, z, u0)
oldJ = newJ
while(True):
J_sum = np.zeros((3, 1))
H_sum = np.zeros((3, 3))
for i in range(len(x)):
H_sum += hessian(x[i], y[i], u)
J_sum += jacobian(x[i], y[i], z[i], u)
H_sum = H_sum + c * H_sum * np.identity(3)
tmpDeltaU = calculateDeltaU(H_sum, J_sum)[0]
tmpu -= tmpDeltaU
oldJ = newJ
newJ = J(x, y, z, u)
if (newJ > oldJ):
c = 10 * c
tmpu = u
tmpDeltaU = deltaU
else:
c = 0.1 * c
u = tmpu
oldDeltaU = deltaU
deltaU = tmpDeltaU
if (linalg.norm(deltaU[0] - oldDeltaU) < eps):
break
return u, newJ
if __name__ == "__main__":
trueU = [0.8, 0.8, 0.8]
u0 = [10, 10, 20]
x = [1 for i in range(10)]
y = [0.5 * i for i in range(10)]
z = createDataSet(x, y, trueU)
u, Jval = levenbergMarquardt(x, y, z, u0, 0.000001)
print("Estimated u : {0}, J : {1}".format(u, Jval))
