2019-12-16

制約のある軌道を生成する方法〜 Constrained Trajectory Generation 〜

なかなか仰々しいタイトルになってしまいました（笑）

つくばチャレンジも終わりSFMに戻ろうかと思っていたのですが，今年のつくばチャレンジではなかなか愛犬が言うことを聞いてくれないこと（＝Plannerの重要性）を痛感し，来年のために Planner 周りの勉強をしてます．で，せっかくなのである程度広く参考文献をあさってみたのですが，なかなかこれが自分には難しく．．．どうやってまとめればいいか．．．と思っている最中なんですが，ひとまず重要そうなトピックを小出しに書いてみることにしました．そこそこ部品ができてきたら大きなまとめを作れればと思ってます．

X．はじめに

　人間が自動車を運転している状況を考えてみると，普通は進みたい軌道があると思います．例えば高速道路を走行しているときは，できれば走行レーンのど真ん中を走りたいですし，おしっこしたくなったら SA に入りたくなります．で，「走行レーンのど真ん中」とか「SAに入るためにレーンチェンジ」とかって，地球の表面を完全に x,y 平面だとすると，軌道は複数の点 (x, y) で定義されます．
　が，実際に運転しているときにこの軌道を頭のなかで計算している人はいないと思います．しかも，実際に車を操作するときに使うインプットって，アクセル（≒加速度）とハンドル（≒角速度）ですよね？つまり，「走行レーンの真ん中を走る」というのは結果でしかなくて，走行レーンの真ん中からちょっとずれたらハンドルを逆に回して走行レーンの真ん中に戻す．という操作を繰り返して走行レーンの真ん中を走っています．
　ロボットを制御するときも同じロジックで，レーンの真ん中からちょっとずれたらフィードバックして．．．という方法で制御することができます．自分は 2013年にETロボコンというコンテストに出場したことがあるのですが，このロボコンのロボットは光センサーを使ったライントレースで目的地まで進んでいきます．このライントレースがここで言うフィードバックのイメージです．ロボットは進む方向も何も全くわかっておらず，自分の直下の黒線との位置関係だけをみて制御をしています．
（下記動画の最初の部分にライントレースしながら進むロボットが出てきます．）

が，この方法だとロボットの慣性が大きくなったり，緻密な制御が求められる場合に対応できなくなります．ETロボコンでも上位チームのロボットはめちゃ早いんですが（笑），速度が上がってくるとラインを追従しきれなくなってコースアウトするロボットも出てきてしまいます．

で，この問題を解決するためには，

「目標出力（走行レーンの真ん中を走る）を満たす入力（アクセル，ハンドル）を事前に計算して，そいつを入力する」

が必要になるわけですが，これが計算できれば，得られた値を入力として使うことによって，ある程度は期待に沿った目標出力が得られます．このエントリでは，目標出力を満たす入力を事前計算する方法をまとめたいと思います．

X．問題設定

ということで，具体的な問題設定をします．

出発点の車の状態

X_start = [x_st, y_st, φ_st, v_st, θ_st]^T

到達点の車の状態

X_end = [x_end, y_end, φ_end, v_end, θ_end]^T

x : 車両の x 座標
y : 車両の y 座標
φ : 車両の向き
v : 車両の速度
θ : ステアリング角度（≒タイヤの切れ角）

上の２つがわかっている状況下で，２つの点をうまく繋いでくれる制御入力 [v, θ]^T を求めます．ここでは，車両がレーンチェンジして停車するマヌーバを考えてみます．下図の点線のラインが，おそらく車両がたどるであろうラインです．

f:id:rkoichi2001:20191207201149j:plain — サンプルの問題設定（レーンチェンジして停車）

X．処理の大きな流れ

軌道生成法の大きなながれをまずクリアにして，それから各パートに入っていきたいと思います．このアルゴリズムで解く問題設定は下記のようになります．

１．制御入力をモデル化して関数として表現する．（v = f(t), θ = g(t)）

２．モデル化関数のパラメータの初期値を決める．（v = a_vt² + b_vt + c_v, θ = a_θt² + b_θt + c_θの，係数 a, b, c のこと．）

３．各時点での制御入力をモデル化関数から求め，数値積分して最終時間での車両状態を求め，到達点からの差分を計算する．（diff = X_end - X(tf)）

４．到達点からの差分を，モデル化関数のパラメータに関して線形化してヤコビアンを計算する．

５．３で求まった目標値からの差分，４で求まったヤコビアンをもとに，ニュートン法を使って修正ベクトルを計算．

６．５で計算した修正ベクトルを使って，パラメータを更新し，３に戻る．

X．運動モデル

実際に入力から出力を計算するためには，制御対象となるロボットのモデルを考えてやる必要があります．自動車の例が一番イメージしやすいかと思ったので，二輪モデルを使って考えてみます．ここでは，入力と目標出力の関係は下記のように設定しました．

入力：速度（v），ステアリング角度（θ）
出力：車両の位置，向き（x, y, φ）

で，アクセルの場合は入力を加速度としたほうがよりイメージに近いかと思ったのですが，論文が速度入力だったので速度入力にしています．あと，ステアリング角とタイヤの切れ角は実際は違いますが，問題が複雑になるだけなので同じとしてます．

f:id:rkoichi2001:20191207193441j:plain — 運動モデル

X．パラメータ化された制御入力（Parameterized Control Input）

今回のアプローチでは数値最適化を使います．なので，何らかの形で制御入力（v, θ）を数式に落としてあげないといけません．論文では，速度入力を時間に関しての台形関数，角速度を時間に関してのスプライン曲線で近似していたのですが，簡単のためにそれぞれを時間に関しての n次多項式としています．

f:id:rkoichi2001:20191207194514j:plain — パラメータ化された制御入力

ちなみに，論文では "Parameterized Vehicle Control" というワードが頻繁に出てきてたんですが，上図のように「入力を何らかの関数でモデル化して，そのパラメータ（上図だと，多項式の係数）として表現すること」なんですね．このイメージがなかなかわかずに「？」してました．

X．数値最適化を用いた最適パラメータの計算

ここからは最適化の導出をします．今回の最適化対象は，

「目標到達点」と「モデルを数値積分することによって求まった到達点」の差分

を最小化することなので，下記のように定式化します．

f:id:rkoichi2001:20191207205218j:plain — 最適化問題の定式化

で，目的関数が定義できたので，あとは多変数のニュートン法を使ってパラメータ修正ベクトルを求めます．

f:id:rkoichi2001:20191207210918j:plain — パラメータ修正ベクトルの導出

次に，上記ノートに出てきたヤコビアンを計算します．このあたりは実際に組み込むときにはライブラリを使うんだとおもいますが，最適化マスターを目指して，今回は泥臭くやります(｀･ω･´)ゞ

f:id:rkoichi2001:20191207212620j:plain — ヤコビアンの導出

X．実装

ということで，実装です．ごちゃごちゃとプロットや設定をしているとこがありますが，重要なとこは２つだけです．

収束計算を実施しているとこ．

下記の大きなループで収束計算を回しています．

    for i in range(6):

        v_coeffs = params[0: init_v_coeffs.shape[0]]
        w_coeffs = params[init_v_coeffs.shape[0]: params.shape[0]]
        
        # Forward Integration.
        x_tf = b_traj.compute_states(x_init, v_coeffs, w_coeffs, t_st, t_end, dt)

        # Plot Result of Forward Integration.
        pose.plot(x_tf[0,:], x_tf[1,:], label="Iteration Count : " + str(i))

        # Compute Jacobian via Linearization.        
        jaco = jacobian_calc.calc_jacobian(x_init, x_final, v_coeffs, w_coeffs, t_st, t_end, dt)
        
        # Compute Parameter Correction Vector via Newton Methods
        dx = np.append(x_init, x_final, axis=0) - np.append(x_tf[:,0], x_tf[:,-1], axis=0)
        jaco_pseudo_inv = np.linalg.pinv(jaco)
        dp = np.matmul(-jaco_pseudo_inv, dx)
        params = params + dp

重要な箇所としては，

        jacobian_calc.calc_jacobian(x_init, x_final, v_coeffs, w_coeffs, t_st, t_end, dt)

のラインでヤコビアンをつどつど計算して，その後の，

        dx = np.append(x_init, x_final, axis=0) - np.append(x_tf[:,0], x_tf[:,-1], axis=0)
        jaco_pseudo_inv = np.linalg.pinv(jaco)
        dp = np.matmul(-jaco_pseudo_inv, dx)
        params = params + dp

のパートでパラメータ修正ベクトルを計算，パラメータの更新をしています

ヤコビアンを計算しているとこ

下記のパートでヤコビアンを計算しています．

    def calc_jacobian(self, x_0, x_f, v_coeffs, w_coeffs, t_st, t_end, dt):
        jaco = np.zeros([x_0.shape[0] + x_f.shape[0], v_coeffs.shape[0] + w_coeffs.shape[0]])
        params = np.append(v_coeffs, w_coeffs, axis=0)
        x_tgt = np.append(x_0, x_f, axis=0)

        for i in range(jaco.shape[1]):
            d = np.zeros(jaco.shape[1])
            d[i] = self.__ep
            n_params = params - d
            p_params = params + d

            # Numerical Integration
            states = self.__b_traj.compute_states(x_0, n_params[0:v_coeffs.shape[0]], n_params[v_coeffs.shape[0]:params.shape[0]], t_st, t_end, dt)
            x_f_n = np.append(states[:, 0], states[:, -1], axis=0)
            states = self.__b_traj.compute_states(x_0, p_params[0:v_coeffs.shape[0]], p_params[v_coeffs.shape[0]:params.shape[0]], t_st, t_end, dt) 
            x_f_p = np.append(states[:, 0], states[:, -1], axis=0)

            # Differentiation
            diff_n = x_tgt - x_f_n
            diff_p = x_tgt - x_f_p
            jaco[:, i] = (diff_p - diff_n) / (2*self.__ep)

        return jaco

今回のケースでは数値計算でヤコビアンを求めていますが，ヤコビアンの一つの列が一つのパラメータに対する変動成分を表すので，下記のコードをパラメータ数分実行してヤコビアンを作ってます．

            d = np.zeros(jaco.shape[1])
            d[i] = self.__ep
            n_params = params - d
            p_params = params + d

            # Numerical Integration
            states = self.__b_traj.compute_states(x_0, n_params[0:v_coeffs.shape[0]], n_params[v_coeffs.shape[0]:params.shape[0]], t_st, t_end, dt)
            x_f_n = np.append(states[:, 0], states[:, -1], axis=0)
            states = self.__b_traj.compute_states(x_0, p_params[0:v_coeffs.shape[0]], p_params[v_coeffs.shape[0]:params.shape[0]], t_st, t_end, dt) 
            x_f_p = np.append(states[:, 0], states[:, -1], axis=0)

            # Differentiation
            diff_n = x_tgt - x_f_n
            diff_p = x_tgt - x_f_p
            jaco[:, i] = (diff_p - diff_n) / (2*self.__ep)

X．実験１（収束する場合）

　実験結果は次のようになりました！出発点と到着点がそれぞれ色付きの丸で表されてまして，イテレーション毎に軌道が描かれてます．パラメータの初期値はすべて０からスタートさせましたが，このサンプルではいい感じに収束しました．二点，上述の説明と異なる部分がありまして，論文 ”Optimal rough terrain trajectory generation for wheeled mobile robots” では，速度，ステアリング角の初期条件は予め多項式の係数として事前に求めてから最適化を実施していましたが，そのロジックを作る代わりに初期状態と終了状態の両方のベクトルを制約条件として最適化しています．あと，レーンチェンジのマヌーバはステアリング操作が２次の多項式で表現できないため，速度，ステアリング角ともに３次の多項式にしてます．
　走行開始時，走行終了時の両方の境界条件が満たされていることが下記のプロットから解ると思います．

f:id:rkoichi2001:20191215224946p:plain — 軌道生成結果

X．実験２（収束しない場合）

　実験１はいい感じに目標に対して収束していきました．「じゃあ，収束しないときはどうなんの？」という疑問が湧いたので，実験してみます．上の結果を見てわかるとおり，レーンチェンジのマヌーバーはハンドルを左右に切らないと行けないので二次式では表現できません．ここではあえて，速度，ステアリング角を２次式でモデル化して問題を解いてみます．

f:id:rkoichi2001:20191215225226p:plain — 軌道生成結果（失敗版）

上図を見るとわかると思うのですが，目標に対して収束しませんでした．また，開始端，終了端ともに境界条件を満たせておらず，下記のような誤差になりました．
[dx_st, dy_st, dφ_st, dv_st, dθ_st, dx_end, dy_end, dφ_end, dv_end, dθ_end]
=
[ 0, 0, 0, -8.7e-05, -4.1e-01, -1.1e-01, 1.0e-01 -7.0e-01, -1.2e-02, 4.1e-01]

ということで，実際にこのアルゴリズムを使うときは

１．収束に必要であろう回数を定義して，最適化計算を実施．

２．定義回数を超えた時点で境界条件と数値積分の結果を比較し，誤差が閾値内に収まれば生成成功，そうでなければ生成失敗

という感じに作ると良さそうです．

X．実験で使ったコード

Planner の学習過程で作ったサンプルコードはここに入れて行こうと思います．
github.com

X．参考文献

論文

Optimal rough terrain trajectory generation for wheeled mobile robots

お世話になったブログ＆Github

myenigma.hatenablog.com

github.com

2019-11-25

Google Cartographer を使ったマッピング　〜パラメータチューニング編〜

ということで，今年のつくばチャレンジで習得したスキルを書き残しておきたいと思います．

０．マップ作成対象

今年のつくばチャレンジのコースは下記の全長２．Xkmのコースだったんですが，全部のコースマップを一括で作るとなかなか大変だったので，自分は下記の６つに分割しました．

f:id:rkoichi2001:20191115034319p:plain — つくばチャレンジ２０１９のコース

コース１．確認走行区間

f:id:rkoichi2001:20191117074725p:plain — 確認走行区間

コース２．交差点まで（行き）

f:id:rkoichi2001:20191117074755p:plain — 交差点まで

コース３．研究学園駅

f:id:rkoichi2001:20191117074817p:plain — 研究学園駅

コース４．研究学園駅前公園

f:id:rkoichi2001:20191117074850p:plain — 研究学園駅前公園

コース５．交差点まで

f:id:rkoichi2001:20191117074934p:plain — 交差点まで

コース６．交差点からゴール（帰り）

f:id:rkoichi2001:20191117075003p:plain — 交差点からゴールまで

で，いつものように gmapping で地図を作ればいいかと思っていたのですが，コース４（研究学園駅前公園）が結構長く，なかなか安定してマップが作成できなかったために Cartographer を使うことにしました．ただ，この Cartographer，なかなかパラメータチューニングが大変でうまく使いこなすまでに時間がかかったので，このエントリではそのあたりを書き残せればと思います．時間があれば理論編も書きたいですが，，，いつものことながら理論系のエントリは時間がかかるので，気が向いたら書きます．

１．参考文献＆ブログ

パラメータチューニングのやり方は， Cartographer の作者の方が作ってくれている下記ドキュメントやブログを見ながら実施すればある程度は理解できると思います．

Cartographer ROS Documentation

https://buildmedia.readthedocs.org/media/pdf/google-cartographer-ros/latest/google-cartographer-ros.pdf

Cartographer Documentation

https://buildmedia.readthedocs.org/media/pdf/google-cartographer/latest/google-cartographer.pdf

あと，下記はお世話になったブログです．
ssk0109.hatenablog.com

ssk0109.hatenablog.com

２．前提・システム構成

このエントリでは，下記の前提でマップ作っています．

2D Lidar が一つ，Odometry（x, y, yaw）有り，IMU無し

f:id:rkoichi2001:20191117093646j:plain — 今回のマップ制作に使用したロボット，改め愛犬．前から

f:id:rkoichi2001:20191117093752j:plain — 今回のマップ制作に使用したロボット，改め愛犬．横から

主要パラメータファイル一覧

変更が必要となるパラメータファイルは下記のとおりです．

backpack_2d.urdf

　各センサーの搭載位置情報が記載された urdf ファイル．今回のエントリでは horizontal_lidar のみ使用．

backpack_2d.lua

　map 作成時の構成情報が記載された lua ファイル．

trajectory_builder2d.lua

　Local SLAM のパラメータが記載された lua ファイル．　　

pose_graph.lua

　Global SLAM のパラメータ（大域的最適化）のパラメータが記載された lua ファイル．

手順

で，実際にマップを生成するにあたっては，下記の手順で設定・パラメータを変更していきます．

１．センサーの搭載位置情報を backpack_2d.urdf に記載する．
２．Topic 名や構成情報を backpack2d.lua に記載する．
３．Odometry がある程度正しくなるようにパラメータ調整．
４．調整された Odometry を使って Local SLAM のパラメータ調整（trajectory_builder2d.lua）．
５．狙った箇所で正しく Loop が閉じるように Global SLAM のパラメータ調整（pose_graph.lua）．

ステップ１　センサーの搭載位置情報を backpack_2d.urdf に記載

ここの設定はとても大事です！これを忘れるとうまく行きません！

f:id:rkoichi2001:20191115061647p:plain — horizontal scanner の位置を設定

ステップ２　Topic 名や構成情報を backpack2d.lua に記載

Topic 名や構成情報を backpack2d.lua に記載します．

f:id:rkoichi2001:20191115054802p:plain — backpack_2d.lua の差分

変更したパラメータですが，
published_frame：今回はオドメトリを使用しているので，オドメトリフレームである odom を指定します．cartographer が生成する map フレームは，ここで指定したフレームを子供に持つ事になります．
provide_odom_frame：true にすると，Local SLAM で生成された自己位置情報が odom_frame として供給されます．自分の場合は別のノードが odom_frame の tf を供給していたので， false にしました．
use_odometry：マップ生成にオドメトリを使うか否か．今回は true です．
num_laser_scans：レーザスキャンセンサーの数．今回は１つです．
num_multi_echo_laser_scans：Echo Laser Scan を生成するレーザスキャンセンサの数．今回は０です．

ステップ３　Odometry がある程度正しくなるようにパラメータ調整

このステップはちょっと詐欺かもですが，，，，正しいマップを手っ取り早く作成したかったので，自分のオドメトリで軌跡がつながるようにヨーレートのオフセットを事前に計算して使うことにしました．

オフセット調整前

例えば，こんな感じで全く繋がらないオドメトリを．．．

f:id:rkoichi2001:20191115064643p:plain — オフセット調整前オドメトリ

オフセット調整してそこそこつながるようにします．

f:id:rkoichi2001:20191115064445p:plain — オフセット調整後オドメトリ

ステップ４　調整された Odometry を使って Local SLAM のパラメータ調整

Local SLAM の調整です．Cartographer のパラメータ調整マニュアルにも有りますが，下記のパラメータを変更して Global SLAM を OFF にして，Local SLAM だけで性能出しします．

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90
【After】
POSE_GRAPH::optimize_every_n_nodes = 0

※それ以外はデフォルトパラメータのままで動かしてみます．

f:id:rkoichi2001:20191115070810p:plain — Local SLAM の結果（デフォルトパラメータ）

上図を見てわかるように，デフォルトパラメータだとヨー角の誤差が積算してしまい，一周して元の地点に戻ってきてもマップが繋がらなくなってしまいました．ということで，ヨー角に関しては Local Scan Matcher の結果よりももう少しオドメトリを信じたほうが良さそうです．ひとまず効果代を見るために，パラメータを目一杯変えてみます．

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

f:id:rkoichi2001:20191115073656p:plain — オドメトリのヨー角を強く信じる設定．

だいぶいい感じになりました．というか，オドメトリしか使ってない感じになりました（笑）で，図中の吹き出しにも書いたんですが，ヨー角に関してはオドメトリ生成値の重みを強くしたものの，並進に関しては設定をいじらなかったため，２つのオドメトリ間でまだ差分が見られます．ということで，並進もオドメトリを強く信じてみます．

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000

f:id:rkoichi2001:20191115075034p:plain — オドメトリのヨー角，並進を強く信じる設定．

完全にオドメトリを信じて作ったマップになりました！マップもきれいになったし，めでたいめでたし．．．．だったらSLAMはいらないわけで．．．大域的にも局所的にもオドメトリには誤差があるので，細かいとこを見ていくとマップが変になってます．例えば，一つの平面をLIDARで取っているにもかかわらず，MAP上には２つの線として現れています．ここからは，，，おそらく微調整が必要になるのかと思います．ということで，もうちょっとスキャンマッチングの結果を信用してみます．

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 500
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000

f:id:rkoichi2001:20191115083542p:plain — スキャンマッチングの結果をもうちょっと信用

うーん．あんまり変わらないですね．．．．ということで，次に Global SLAM の調整をしてみます．

ステップ５　狙った箇所で正しく Loop が閉じるように Global SLAM のパラメータ調整．

まず，ステップ４でOFFにした Global SLAM を ON にします．

【Before】
POSE_GRAPH::optimize_every_n_nodes = 0
【After】
POSE_GRAPH::optimize_every_n_nodes = 90

f:id:rkoichi2001:20191117080829p:plain — Global SLAM を ON にしてマップ生成

再訪問したところで Loop Close してほしいのですが，デフォルトパラメータのままではそれほどループが閉じませんでした．ということで，パラメータを変更してループが閉じやすくします．

【Before】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e4
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e5

【After】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e9
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e10

f:id:rkoichi2001:20191117084435p:plain — Loop Close しやすくした結果

８の字走行した部分（図の左側）に関してはループが閉じました．が，走行の最後の部分に関してはまだループが閉じていません．おそらく自分のデータのとり方が良くなかったんですが，一周回って同じところに帰ってきた時点でデータ取得を止めてしまったので，再訪箇所のオーバーラップが少なく，ループが閉じきれてないのかと思われます．ということで，Loop Close をもっと頻繁に発生させて，最後のループが閉じるかどうか実験してみます．

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90

【After】
POSE_GRAPH::optimize_every_n_nodes = 5

うーん．なんかまだ微妙です．

f:id:rkoichi2001:20191117085541p:plain — 頻繁に Loop Close 走らせた結果

Cartographer では，マップの小領域（Submapと呼ぶみたいです）に対して Scan 結果から得られる尤度を蓄積していき，更新が一定回数以上になったときにその Submap を大域的最適化対象に登録します．この「一定回数以上」のパラメータが下記になるのですが，最適化対象にするタイミングをもっと早めてみます．

【Before】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 90

【After】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

f:id:rkoichi2001:20191117090401p:plain — 間違った Loop Closure を検出

うーん．ループを誤検知してしまいました．もう少しオドメトリを信じるべきか．．．
Cartographer のポーズ最適化では， Local SLAM のときと同様に，オドメトリ，Local SLAM の結果に対して重みを設定することができるのですが，もう少しオドメトリを信じる設定にしてみます．

【Before】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e5
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e5

【After】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7

f:id:rkoichi2001:20191117094137p:plain — 一応完成！

ということで，上記のパラメータ調整を経て，結構いい感じになったのでこれで良しとします．エンコーダとIMUを使ったヨーレート（赤線）は完全には正確でなく，一周回ってきたときには繋がってませんが，SLAMから生成されるオドメトリ（青色）は一周回った時点できれいにつながりました．

結果

ということで，全マップです！うーん．やっぱり，ループを綴じ込むのはかなり難しいですね．オドメトリを合わせればそこそこまでは行くものの，完全ではないのでループ綴じ込む程ではなく，逆に Local SLAM, Glocal SLAM の寄与を上げるとループじゃないところで誤マッチングが起こり．．．というシーソーゲームをひたすら繰り返してました．

コース１．確認走行区間

f:id:rkoichi2001:20191117190041p:plain — コース１のマップ

デフォルトからの変更パラメータ
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e9
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e10
POSE_GRAPH::optimize_every_n_nodes = 5
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 500
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

コース２．交差点まで（この区間は難しく，，，完璧にはできませんでした．）

f:id:rkoichi2001:20191117190556p:plain — コース２のマップ．

デフォルトからの変更パラメータ
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e9
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 100
POSE_GRAPH::constraint_builder::ceres_scan_matcher::rotation_weight = 100
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
POSE_GRAPH::optimization_problem::global_sampling_ratio = 0.3
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 100000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 100000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

コース３．研究学園駅

f:id:rkoichi2001:20191117190751p:plain — コース３のマップ．

コース４．研究学園駅前公園（この区間は難しく，，，完璧にはできませんでした．）

f:id:rkoichi2001:20191117191016p:plain — コース４のマップ

デフォルトからの変更パラメータ
POSE_GRAPH::constraint_builder::constraint_builder::min_score = 0.60
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e6
POSE_GRAPH::constraint_builder::fast_correlative_scan_matcher::linear_search_window = 20
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 1
POSE_GRAPH::optimization_problem::huber_scale = 0.1e1
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e8
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e8

コース５．交差点まで

f:id:rkoichi2001:20191117191218p:plain — コース５のマップ．

POSE_GRAPH::constraint_builder::constraint_builder::min_score = 0.60
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e6
POSE_GRAPH::constraint_builder::fast_correlative_scan_matcher::linear_search_window = 20
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 1
POSE_GRAPH::optimization_problem::huber_scale = 0.1e1
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e8
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e8
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 40

コース６．交差点からゴール

f:id:rkoichi2001:20191117191340p:plain — コース６のマップ．

あともう一つ，英語圏（≒アメリカ，ドイツ）からのアクセスをもっと増やせないかな〜という思いもあって，アクセス数が稼げそうなエントリに限って英語版を作ってみることにしました．アクセスが劇的に増えたら，また報告します（笑）

http://daily-tech.hatenablog.com/entry/2019/11/25/062304daily-tech.hatenablog.com

2019-11-25

Mapping with Google Cartographer ~ Parameter Tuning ~

I would like to summarize the skills I learned from this year's Tsukuba Challenge. This blog post is about mapping with Google Cartographer.

0. Mapping Target

The total length of Tsukuba Challenge 2019 is 2.X km. Since it is quite difficult to do SLAM for all the course at once, I divided it into the following 6.

Course 1. Confirmation Section

Course 2. Up to 1st Intersection

Course 3. Kenkyu Gakuen Station

Course 4. Kenkyu Gakuen Station Park

Course 5. Up to 2nd Intersection

Course 6. From 1st Intersection to Goal

I thought that I should make a map with gmapping as usual, but I decided to use Cartographer because Course 4 (Kenkyu Gakuen Station Park) was quite long and I couldn't create a stable map. It is quite difficult to tune parameters for Cartographer, and it took a long time for me. So I decided to create this entry as "Tuning manual with actual examples". I also wanted to write down theory that is used inside it, but this would take so much time, so,,, I will give it a shot when I have time.

1. References & Useful Blog Posts

How to tune parameters are roughly described in the following manual that are documented by the developer. I also referenced the following 2 blog posts.

2. Preconditions and System Configuration.

The maps shown in this entry are created with the following preconditions.

One 2d Lidar, Odometry (x, y, yaw), No IMU.

3. Parameter files subject to update.

The frollowing 4 files are necessary to be modified for 2d slam cases. (My case.)

backpack_2d.urdf

Urdf file that describes sensor configuration. Only horizontal_lidar is used for this entry.

backpack_2d.lua

Lua file that describes system configuration. (Topic, Frame etc ...)

trajectory_builder2d.lua

Lua file that describes parameters for Local SLAM.

pose_graph.lua

Lua file that describes parameters for Glocal SLAM (Loop Closure).

4. Procedure

Change the settings and parameters according to the following procesure.

1. Update sendor mounting position information described in backpack_2d.urdf.
2. Update topic name and system configuration described in backpack2d.lua.
3. Tune parameters so that odometry is correct to some extent.
4. Tune local SLAM parameters with already tuned odometry. (trajectory_builder2d.lua)
5. Tune global SLAM parameters so that loop closes correctly at the target location.

Step 1 : Update sendor mounting position information described in backpack_2d.urdf.

The setting here is very important! If you forget this, it will not work!

Step 2 : Update topic name and system configuration described in backpack2d.lua

Regarding the modified parameters,,,,
published_frame：Since odometry is used this time, specify odometry frame as "odom". The map frame generated by cartographer will have this frame as the child.
provide_odom_frame：If true, transform between map frame and localized position by local SLAM is supplied as odom_frame. In my case, since another node supplied tf of odom_frame, I set it to false.
use_odometry：Whether to use odometry for map generation. This time, set it to true.
num_laser_scans：Number of laser scan sensors. This time, set it to one.
num_multi_echo_laser_scans：Number of echo laser scan sensor. This time, set it to zero.

Step 3 : Tune parameters so that odometry is correct to some extent.

This step may be a bit tricky, but I wanted to quickly create a correct map, so I decided to tune the yaw rate offset in advance so that the odometry trajectory gets roughly connected.

Before offset adjustment

For example, odometry not connected at all like this....

Adjust offset so that it connects properly.

Step 4 : Tune local SLAM parameters with already tuned odometry.

Adjustment of Local SLAM. At first, turning off global SLAM by the following parameter, tune and improve local SLAM performance as described in "Cartographer parameter tuning manual".

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90
【After】
POSE_GRAPH::optimize_every_n_nodes = 0

Default settings are used except for the above modification.

As you can see from the above figure, the default parameter causes the yaw angle error to accumulate, and the map does not connect even if you return to the original point after going around. So, it seems better to believe in odometry a little more than the result of Local Scan Matcher for yaw angle. Let's change the parameters as much as possible and see the result.

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

As for the rotation, the map became match better. Since I only increased the weight of odometry for rotation and not for translation, there is still a translation offset between encoder based odometry (red) and slam base odometry (purple).

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000

A map made is completely based on encode odometry! Even though the map seems decent, if you take a look at it closely there are some errors, since odometry contains errors both in locally and globally. For example, even though one plane is taken with LIDAR, it appears as two lines on the MAP. From here, I think that fine tuning is probably necessary. So, let's trust the scan matching result a little more.

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 500
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000

Well. It doesn't change much. . . . So, let's go to next step,,, adjustment of Global SLAM.

Step 5 : Tune global SLAM parameters so that loop closes correctly at the target location.

Enable the global SLAM that was disabled in step 4.

【Before】
POSE_GRAPH::optimize_every_n_nodes = 0
【After】
POSE_GRAPH::optimize_every_n_nodes = 90

I would like a loop to get closed when it re-visited the same place, but it did not happen with the default parameters. So let's modify parameters for loop closure to happen.

【Before】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e4
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e5

【After】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e9
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e10

The loop closed for the part of figure 8 (left side of the figure). However, the loop is not yet closed for the last part of the run. This is probably because the way I collect the data was not good. Since I stopped recording data immediately after I returned to the same place after going around the circle, there were not enough overlap around the revisited places. To complement this, let's modify parameter to make loop closure occur more easily and frequently, and see if this achieve correct loop closure.

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90

【After】
POSE_GRAPH::optimize_every_n_nodes = 5

Mmm.... No drastic change....

Cartographer accumulates the likelihood obtained from the scan results. This likelihood is accumulated for a small chunk of the map, called "Submap", and registers the submap as a global optimization target when the number of updates exceeds a certain number. This "certain number" parameter is as follows, and let's lower the threshold to achieve earlier registration of submap for global optimization.

【Before】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 90

【After】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

Well,,, wrong loop closure detected... Probably I should believe encoder based odometry a little more.
In Cartographer's pose optimization, as with Local SLAM, you can set weights for the results of encoder based odometry and scan matching based odometry. Let's trust encoder based odometry a little more.

【Before】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e5
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e5

【After】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7

After several parameter adjustment, the generated map seems to be OK. The encoder based odometry (red line) does not completely close when it goes around, but the odometry line generated from SLAM (purple) is connected smoothly where the loop closes.

Result

Result of all maps shown below! After all, it is quite difficult to achieve loop closure. If you increase weight on the encoder based odometry, the map becomes decent. However it is not perfect enough to achieve loop closure, and conversely, if you increase the contribution of local and global SLAM, a false match occurs in a place that is not a loop. . . I just repeated the seesaw game.

Course 1. Conrimation Section

Course 2. Up to 1st Intersection (This interval is difficult and the loop does not get closed completely.)

Modified parameters from Cartographer's default

POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e9
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 100
POSE_GRAPH::constraint_builder::ceres_scan_matcher::rotation_weight = 100
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
POSE_GRAPH::optimization_problem::global_sampling_ratio = 0.3
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 100000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 100000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

Course 3. Kenkyu Gakuen Station

Modified parameter from Cartographer's default

Course 4. Kenkyu Gakuen Station Park (This interval is difficult and the loop does not get closed completely.)

Modified parameters from Cartographer's default

Course 5. Up to 2nd Intersection

Modified parameters from Cartographer's default

Course 6. From 1st Intersection to Goal

Modified parameters from Cartographer's default

2019-11-21

つくばチャレンジ２０１９を終えて．．．

ということで，今年も終わりました．つくばチャレンジ本走行．
全く同じ書き出しでちょうど一年，二年前に同じ記事を書いてます．一年たつの早いですね～．
daily-tech.hatenablog.com

今年の結果

つくばチャレンジの様子はニコニコ動画とか Youtube とかでライブ配信されているのですが，発走前にちょっとしたインタビューをしてくれます．

インタビューア：次の発走は番号 1901 チームイエスマンです．ズバリ今年の目標をおきかせください．
自分　　　　　：今年の目標は「完走」です！
インタビューア：おお！「完走」のお言葉いただきました！ありがとうございます！それでは頑張ってください！
インタビューア：それでは間もなく発走です．

〜10秒後〜

インタビューア：それではスタートです！

からの，，，
まさかの 2m 走行リタイアでした．．．．自分の発走順が２番目ということもあって，スタート地点は結構な観客がいたんですが，「完走します！」ってスタート直前に言っといて，スタート直後にロボットがくるくる回り始め．．．．なかなかの赤っ恥でした（笑）ということで，結果的には走行距離２ｍでフィニッシュでした．

f:id:rkoichi2001:20191114052119p:plain

敗因ですが，スタート地点にたくさんの人だかりができており，マップマッチングが全くできなかったことだと思われます．スタート地点の人だかりは昨年（2018）に難なくクリアしていたことからノーマークだったんですが，今年は特に出走順が早かったこともあってスタート地点の観客が多く，ちょっと環境的にも異なってしまっていたのかと思われます．

実は，ひょっとすると１０%くらいの確率で今年は完走できるんじゃないかと思っていた節もあって，この結果にはなかなか凹みました（笑）まあ，まぐれ狙いだと本番一発勝負では全く通用しないということですね．反省＆精進します m(_ _;)m

今年の成果

といことで，９月時点の下記エントリで書いていた３つの目標のうち，
daily-tech.hatenablog.com

１．Stereo Camera を用いて障害物検知・回避を実装する．（LIDARは自己位置推定のみ！にできればいいなあ．．．）（未完）

　-> アルゴリズムを動かすところまではいったものの，Planner に繋いでコースで動作検証をする時間を取れず，結局今年も Lidar での参加．

２．move_base, teb_local_planner の内部（アーキテクチャ・アルゴリズム）を理解して，手を入れられるようにする．（未完）

　-> 信号機前での待ち行列（隊列）ロジックなど一部手を入れたものの，未完．

３．子分の理解の促進．（完？）

　-> PC 立ち上げ，自分の PC でロボット動作，マップ作成，ウェイポイント作成までできるようになりました．来年は Planner 担当です！

と，どのアイテムも完了！というステータスには至りませんでした．今回の体たらくに関しましては，ひとえに私の不徳の致すところであり，面目次第もございません (´・ω・`)

システムのこととか，ROSの復習内容とかはまたおいおい備忘録的にブログで上げたいと思います．

Special Thanks

今年もいろいろとつくばチャレンジ有志の方々，関連企業の方々に助けていただきました．ありがとうございました！
つくばチャレンジ実行委員会の皆様：全般的に
北陽電機株式会社：UTM-30LX-EW 貸出
i-Cart Mini プロジェクトの皆様：ハードウェアの流用
元会社同僚のC氏：安全管理責任者のお願い
元会社同僚のS氏：本走行時のあたたかな声援

なんだか，サクラダファミリアみたいに，「永遠に完成しないロボット」みたいな雰囲気になってしまってますが，今年の結果で今まで以上に火がついた気がします！ということで，今年も言わせてください．

「来年は，完走します(ﾟДﾟ)ﾉ」

f:id:rkoichi2001:20191114055508j:plain — 決戦の朝！

f:id:rkoichi2001:20191121042933j:plain — 昨年に引き続き，安全管理責任者をお願いしたC氏

f:id:rkoichi2001:20191121043009j:plain — 完走したときの予行演習 with 子分

2019-10-25

Visual Studio Code を使った C++ プログラミング　〜フォーマッターとインデクサー〜

毎回プロジェクトが変わるたびに検索して設定している気がするので，まとめておきます．

インデクサーの設定

インデクサーには GNU Global を使います．Ubuntu 16.04 の時は apt でちょうどいいバージョンをインストールすることができず，ソースをダウンロード＆ビルドしていたんですが，Ubuntu 18.04 の場合はそのままダウンロードすれば OK でした．

1. GNU Global のダウンロード

sudo apt install global

2. vscode の Extensions から，C/C++ GNU Global をダウンロード．

f:id:rkoichi2001:20191014104348p:plain — vscode extensions

3. vscode の再起動

一応再起動しておきます．

4. GNU Global による GTAG の作成

ctrl + alt + P でコマンドパレットを開き，"Global: Rebuild Gtags Database" を選択．

ここまで来れば，ジャンプしたいメソッドやクラス名にカーソルを合わせて F12 を押下すれば，該当のソース・ヘッダに飛んでくれると思います．

フォーマッターの設定

次にフォーマッターの設定です．ここでは，Clang-Format を使います．clang-format としてデフォルトでインストールされるソフトのバージョンが古い場合が結構あり，ハマりポイントなので，ちょっと新しめの clang-format をインストールします．

1. clang-format のダウンロード

今回は clang-format-6.0 をインストールしました．

sudo apt install clang-format-6.0

2. vscode の Extensions から，Clang-Format をダウンロード

f:id:rkoichi2001:20191014105555p:plain — vscode extensions

3. vscode の再起動

一応再起動しておきます．

4. settings.json の設定

{
    "editor.defaultFormatter": "xaver.clang-format",
    "clang-format.executable": "/usr/bin/clang-format-7",
    "clang-format.language.cpp.style": "{ BasedOnStyle: Google, IndentWidth: 2, ColumnLimit: 80, DerivePointerAlignment: false, PointerAlignment: Left }",
    "[cpp]": {
        "editor.tabSize": 2
    },
    "editor.formatOnSave": true
}

細かい設定は，下記のブログに解説付きで書いてくれてました！が，vscode の extension が全部対応してない可能性もあるような気がします．
yasuharu519.hatenablog.com

上記の設定をすると，

Before

#include 

namespace test_test_test {
namespace hoge_hoge_hoge {
void test_hoge_func(const char *p);
}
}

void test_test_test::hoge_hoge_hoge::test_hoge_func(const char *p) {}

#include 
int main()
{
    std::cout << "Hello World" << std::endl;
    return 0;
}

After

#include 

namespace test_test_test {
namespace hoge_hoge_hoge {
void test_hoge_func(const char* p);
}
}  // namespace test_test_test

void test_test_test::hoge_hoge_hoge::
    test_hoge_func(const char* p) {}

#include 
int main() {
  std::cout << "Hello World"
            << std::endl;
  return 0;
}

な感じになります．PointerAlignment の設定がちょっとハマったのですが，Google の Style で DerivePointerAlignment が true になっているようなので，これを明示的に false にする必要がありました．

2019-10-23

Stixelを用いた視差画像からのフリースペース計算　〜（理論編２）

ということで，実装編に入る！と思っていたんですが，「どうやって動的計画法を実装するか？」という点を抑えておきたく下記の理論編１に引き続き理論編２のエントリも書くことにしました．

daily-tech.hatenablog.com

１．参考文献

こちらのエントリでは下記の論文の内容を見ていきますが，論文内容としては「Stixel をどうやって Cuda で実装するか？」です．

GPU-accelerated real-time stixel computation

画像処理をやる人の端くれとして，一応 Cuda には触ったことはあるのですが，あまりむずかしい並列化はやったことがなく，今回の Stixel の問題は難易度も結構高くていい題材かと思ったので論文を読んでまとめてみることにしました．（が，このエントリでは動的計画法のとこまでしか書いてません．Cuda化のところは実装編で触れます．）

２．問題のおさらい

細かい問題定義は「理論編１」を見ていただくとして，簡単に問題構成のおさらいをしておきます．解こうとしている問題は，「視差画像 D が与えられたときに，尤度が最大となるような Stixel の集合 L を見つける．」ことです．

f:id:rkoichi2001:20191013102429j:plain — 理論編１のおさらい

で，尤度が最大になるような Stixel L の集合をどうやって見つけるか？ですが，簡単に言うと可能な Stixel の組み合わせを総当りで試してみて，その中で尤度が最大になる組み合わせを採用します．この総当り計算を実施するときに，単純に全組み合わせを計算すると計算量が爆発してしまうので，「重複した計算を避ける」ために動的計画法が出てきます．で，動的計画法の話に入る前に，解法に重要な影響を与える２つの前提を書いておきます．

１．隣り合う列は互いに独立である．

２．それぞれのピクセルの視差の値は互いに独立である．

上の２つの前提をおいたおかげで，Stixel を求める問題は列・ピクセルの問題に簡単化することができます．これを図示すると，下記のようになります．

f:id:rkoichi2001:20191011082741j:plain — ２つの前提による問題の簡略化

３．動的計画法のおさらい

次に，簡単に動的計画法のおさらいをしておきます．この Stixel の問題と類似の問題を見つけたので，別のエントリとして作ってみました．

daily-tech.hatenablog.com

で，上記エントリの場合の問題設定は下記でした．

ロッド切り出し問題
あなたはコーナンで働いているとします！長〜い金属棒を仕入れて，これを小分けに切断して販売したいですが，金属棒は長さによって販売価格が異なります．そして，販売価格は長さに比例しているとは限りません！このときに，長さ n の金属棒をどのように切断すれば売上を最大化できるでしょうか？

動的計画法の観点では，この問題はとても良く似ています．

f:id:rkoichi2001:20191012223942j:plain — ロッド切り出し問題と Stixel の最適化問題の比較

一点異なる点として，ロッド切り出し問題の場合は単純に切り出し方法を決めればよかったのに対し，Stixel の最適化計算では Stixel の種類（Sky, Ground, Object）が変数として増えてきます．

４．動的計画法を用いた Stixel の解法

ロッド切り出し問題との類似点がわかったので，同様の考え方で動的計画法を解いてみたいと思います．

４．１．細かな問題設定

下記，論文にある問題設定です．

・Stixel は３種類（Object, Ground, Sky）

・Stixel自体の表記方法

　1. 開始端を row = b，終了端を row = t にもつ，Object の Stixel：ob^t_b = {v_b, v_t, Object}
　2. 開始端を row = b，終了端を row = t にもつ，Sky の Stixel：sky^t_b = {v_b, v_t, Sky}
　3. 開始端を row = b，終了端を row = t にもつ，Ground の Stixel：ground^t_b = {v_b, v_t, Ground}

・集約コストの表記方法

　1. 終了端を row = k にもち，最後の Stixel が Object である場合の集約コスト： OB^k
　2. 終了端を row = k にもち，最後の Stixel が Sky である場合の集約コスト： SK^k
　3. 終了端を row = k にもち，最後の Stixel が Ground である場合の集約コスト： GR^k

で，やっぱりわかりにくいので描画します！

f:id:rkoichi2001:20191012234143j:plain — 問題設定

４．２．評価式

評価式自体は，理論編１のときに紹介したものがベースになっています．が，，，２つの論文で評価式がちょっと違っていたので，これは実装と突き合わせながら，実装編で確認します．

４．３．動的計画法

で，ようやく評価式ができたとして，動的計画法を用いて最適化問題をときます．

f:id:rkoichi2001:20191013001443j:plain — 終了端を k にもつ最適な集約コスト

上図を見てもらうとわかると思うのですが，ある画像列に対して高さ h までの集約コストが最適になるものを求めるという問題なので，ロッドの切り出し問題とよく似ています．が，Stixel の種類が３つあるということと，評価式が実質的に２つの項（条件付き確率項，事前確率項）からなるということで，ちょっとだけ問題が複雑になります．この条件を鑑みて，動的計画法の漸化式を書いてみます．

f:id:rkoichi2001:20191013004451j:plain — 終了端を k にもつ最適な集約コスト（Stixel の種類を考慮）

上図に描画したとおり，Stixel の種類が３種になることから下記の点が異なってきます．

・考慮している Stixel (終了端が k) 自体に関して，３つの分岐ができる．

・考慮している Stixel (終了端が k) の一つ前の Stixel に関して，３つの分岐ができる．

ここまでで，動的計画法の漸化式が求まりました．前述したとおり，この論文のメインテーマは「Cudaでどうやって並列化するか？」なのですが，これは実装編でかければと思います．

2019-10-23

動的計画法５　ロッド切り出し問題

ちょうど一年ぶりに動的計画法のエントリですが， Stixel を求める問題で動的計画法を使わないといけなかったので，簡単に復習の意味も込めてエントリをつくります．今回のロッド切り出し問題は Stixel を求める問題ととっても似てまして，これがわかれば Stixel 解決にむけて一気に理解が進むかな？と思いまして．

問題定義

ロッド切り出し問題
あなたはコーナンで働いているとします！長〜い金属棒を仕入れて，これを小分けに切断して販売したいですが，金属棒は長さによって販売価格が異なります．そして，販売価格は長さに比例しているとは限りません！このときに，長さ n の金属棒をどのように切断すれば売上を最大化できるでしょうか？

で，上記の問題を解くには金属棒の長さ毎の価格表が必要です．この価格表を下記のように定義します．（ちょっと話はそれますが，確かに長さの短いものは切れ端としていっぱいできそうなので，値段は安くなりそうですよね．）

長さ　|　１　|　２　|　３　|　４　|　　５　|　　６　|　　７　|　　８　|　　９　|　１０
ーーー|ーーー|ーーー|ーーー|ーーー|ーーーー|ーーーー|ーーーー|ーーーー|ーーーー|ーーー
価格　|　１　|　５　|　８　|　９　|　１０　|　１７　|　１７　|　２０　|　２４　|　３０

で，いつもの如く図示します．

f:id:rkoichi2001:20191012164226j:plain — n = 4 の場合のロッド切り出し問題

で上図を見ていただけるとわかると思うのですが， n = 4 の場合の最適な切り出し方は [2, 2] であることがわかります．

次に，n = 5 の場合の切り出し方法を考えてみたいと思います．で，ここではちょっと考え方を変えて，n = 5 のロッドに対して一回だけカットする（もしくは全くカットしない）パターンを書き出してみます．

f:id:rkoichi2001:20191012170710j:plain — n = 5 のロッドに対して，一度だけ切り込みを入れた（もしくは入れなかった場合）場合

上図からわかると思うのですが，結局 n = 5 の問題の解はそれより小さい問題の最適解の組み合わせになります．全くカットしなかった場合は新しいパターンになりますが，これはテーブルから一発で求めることができ，どこか一箇所にでも切り込みを入れるとそれよりも小さい問題の最適解の足し算になります．．．．

ということで，再帰的に求める方法がわかりそうです！

１．再帰的に求める方法を考える

では，いつものように「魔法の関数」を考えます．

get_optimal_cut(price, length)
price : ロッドの長さに応じた価格表
length : 最適価格を求めたいロッドの長さ

で，上記の関数がうまく動くとします．とすると，n = 5 の場合の関数のコールイメージは下記のようになります．

長さ = 5 のロッドの最適切り出し．．．
カットしなかった場合：p[n]  <- テーブルを直接参照
１，４ で分割：get_optimal_price(price, 1) + get_optimal_price(price, 4)
２，３ で分割：get_optimal_price(price, 2) + get_optimal_price(price, 3)
３，２ で分割：get_optimal_price(price, 3) + get_optimal_price(price, 2)
４，１ で分割：get_optimal_price(price, 4) + get_optimal_price(price, 1)

上記の組み合わせから最適なものをえらぶので．．．

max(
p[n], 
get_optimal_price(price, 1) + get_optimal_price(price, 4), 
get_optimal_price(price, 2) + get_optimal_price(price, 3),
get_optimal_price(price, 3) + get_optimal_price(price, 2),
get_optimal_price(price, 4) + get_optimal_price(price, 1),
)

となります．実際には（１，４）と（４，１）の切り出しは等価なのでもっと計算を減らせると思うのですが，ひとまずこのまま進みます．上記の関数をコードに落とすと，下記のようになります．

price_tbl = [1, 5, 8, 9, 10, 17, 17, 20, 24, 30]

def rod_cutting(price, length):

    # Base Case
    if (length == 1):
        return price[length - 1]
    
    # No Cutting Case.
    opt_price = price[length - 1]
    for i in range(length - 1):
        left_length = i + 1
        right_length = length - left_length
        opt_price = max(opt_price, rod_cutting(price, left_length) + rod_cutting(price, right_length))

    return opt_price

if __name__ == '__main__':

    length = 4
    opt_price = rod_cutting(price_tbl, length)
    print('Optimal Price for length {0} : {1}'.format(length, opt_price))

２．再帰関数の分岐から漸化式を作る

ということで，ここまでくればあとは機械的に行けそうです．漸化式は，下記のようになります．opt_price[i] は長さ i のロッドの最適価格です．

no_cut_price = price_tbl[i]　　　：切り込みを入れないときの値段
cut_price = max_1≦k≦i-1(opt_price[k] + opt_price[i - k])　　　：切り込みを入れたときの最適値段
opt_price[i] = max(no_cut_price, cut_price)　　　：最終的な最適値段

３．漸化式を使って，ボトムアップで表を埋めていく

漸化式が出来上がったので，長さが小さいロッドから表を埋めていけば，DPでこの問題を解くことができます．最終的な実装は下記のようになります．下記の実装では，最適値段だけでなくどの切り方をすれば最適値段が求まるかも出力しています．

price_tbl = [1, 5, 8, 9, 10, 17, 17, 20, 24, 30]
opt_price_tbl = [-1 for elem in price_tbl]
cut_from_left = [-1 for elem in price_tbl]

def rod_cutting_with_dp(price, length, opt_price, cut_from_left):

    # Compute Optimal Price for Rod of Length i
    for i in range(length):

        # Base Case
        if (i == 0):
            opt_price_tbl[0] = price_tbl[0]
            cut_from_left[0] = 1
            continue

        # No Cutting Case.
        opt_price = price_tbl[i]
        cut_from_left[i] = i + 1

        for j in range(i):
            left_length = j + 1
            right_length = i + 1 - left_length

            new_price = opt_price_tbl[left_length - 1] + opt_price_tbl[right_length - 1]
            if (opt_price < new_price):
                opt_price = new_price
                cut_from_left[i] = j + 1

        opt_price_tbl[i] = opt_price

    return opt_price_tbl, cut_from_left


if __name__ == '__main__':

    length = 10
    opt_price_tbl, cut_from_left_tbl = rod_cutting_with_dp(price_tbl, length, opt_price_tbl, cut_from_left)
    print('Optimal Price Table : {0}'.format(opt_price_tbl))
    print('Optimal Cut Table   : {0}'.format(cut_from_left_tbl))

ということで，このことを踏まえて Stixel に戻ります！

参考文献

何冊本もってんだよ！？って話ですが，積読本の中の一つです（笑）

アルゴリズムイントロダクション第3版総合版：世界標準MIT教科書

作者: Thomas H. Cormen,Clifford Stein,Ronald L. Rivest,Charles E. Leiserson
出版社/メーカー: 近代科学社
発売日: 2018/01/09
メディア: Kindle版
この商品を含むブログ (4件) を見る

X．はじめに

「目標出力（走行レーンの真ん中を走る）を満たす入力（アクセル，ハンドル）を事前に計算して，そいつを入力する」

X．問題設定

出発点の車の状態

到達点の車の状態

X．処理の大きな流れ

１．制御入力をモデル化して関数として表現する．（v = f(t), θ = g(t)）

２．モデル化関数のパラメータの初期値を決める．（v = av*t2 + bv*t + cv, θ = aθ*t2 + bθ*t + cθの，係数 a, b, c のこと．）

３．各時点での制御入力をモデル化関数から求め，数値積分して最終時間での車両状態を求め，到達点からの差分を計算する．（diff = X_end - X(tf)）

４．到達点からの差分を，モデル化関数のパラメータに関して線形化してヤコビアンを計算する．

５．３で求まった目標値からの差分，４で求まったヤコビアンをもとに，ニュートン法を使って修正ベクトルを計算．

６．５で計算した修正ベクトルを使って，パラメータを更新し，３に戻る．

X．運動モデル

X．パラメータ化された制御入力（Parameterized Control Input）

X．数値最適化を用いた最適パラメータの計算

「目標到達点」と「モデルを数値積分することによって求まった到達点」の差分

X．実装

収束計算を実施しているとこ．

ヤコビアンを計算しているとこ

X．実験１（収束する場合）

X．実験２（収束しない場合）

１．収束に必要であろう回数を定義して，最適化計算を実施．

２．定義回数を超えた時点で境界条件と数値積分の結果を比較し，誤差が閾値内に収まれば生成成功，そうでなければ生成失敗

X．実験で使ったコード

X．参考文献

論文

お世話になったブログ＆Github

０．マップ作成対象

コース１．確認走行区間

コース２．交差点まで（行き）

コース３．研究学園駅

コース４．研究学園駅前公園

コース５．交差点まで

コース６．交差点からゴール（帰り）

１．参考文献＆ブログ

Cartographer ROS Documentation

Cartographer Documentation

２．前提・システム構成

2D Lidar が一つ，Odometry（x, y, yaw）有り，IMU無し

主要パラメータファイル一覧

backpack_2d.urdf

backpack_2d.lua

trajectory_builder2d.lua

pose_graph.lua

手順

ステップ１ センサーの搭載位置情報を backpack_2d.urdf に記載

ステップ２ Topic 名や構成情報を backpack2d.lua に記載

ステップ３ Odometry がある程度正しくなるようにパラメータ調整

オフセット調整前

ステップ４ 調整された Odometry を使って Local SLAM のパラメータ調整

ステップ５ 狙った箇所で正しく Loop が閉じるように Global SLAM のパラメータ調整．

結果

コース１．確認走行区間

コース２．交差点まで（この区間は難しく，，，完璧にはできませんでした．）

コース３．研究学園駅

コース４．研究学園駅前公園（この区間は難しく，，，完璧にはできませんでした．）

コース５．交差点まで

コース６．交差点からゴール

0. Mapping Target

1. References & Useful Blog Posts

Cartographer ROS Documentation

Cartographer Documentation

Usefule Blog Entries.

2. Preconditions and System Configuration.

One 2d Lidar, Odometry (x, y, yaw), No IMU.

3. Parameter files subject to update.

backpack_2d.urdf

backpack_2d.lua

trajectory_builder2d.lua

pose_graph.lua

4. Procedure

Step 1 : Update sendor mounting position information described in backpack_2d.urdf.

Step 2 : Update topic name and system configuration described in backpack2d.lua

Step 3 : Tune parameters so that odometry is correct to some extent.

Before offset adjustment

Step 4 : Tune local SLAM parameters with already tuned odometry.

Default settings are used except for the above modification.

Step 5 : Tune global SLAM parameters so that loop closes correctly at the target location.

Result

Course 1. Conrimation Section

２．モデル化関数のパラメータの初期値を決める．（v = a_vt² + b_vt + c_v, θ = a_θt² + b_θt + c_θの，係数 a, b, c のこと．）

ステップ１　センサーの搭載位置情報を backpack_2d.urdf に記載

ステップ２　Topic 名や構成情報を backpack2d.lua に記載

ステップ３　Odometry がある程度正しくなるようにパラメータ調整

ステップ４　調整された Odometry を使って Local SLAM のパラメータ調整

ステップ５　狙った箇所で正しく Loop が閉じるように Global SLAM のパラメータ調整．

・Stixel自体の表記方法