Google Cartographer を使ったマッピング 〜パラメータチューニング編〜
ということで，今年のつくばチャレンジで習得したスキルを書き残しておきたいと思います．
０．マップ作成対象
今年のつくばチャレンジのコースは下記の全長２．Xkmのコースだったんですが，全部のコースマップを一括で作るとなかなか大変だったので，自分は下記の６つに分割しました．
コース２．交差点まで（行き）
コース５．交差点まで
コース６．交差点からゴール（帰り）
で，いつものように gmapping で地図を作ればいいかと思っていたのですが，コース４（研究学園駅前公園）が結構長く，なかなか安定してマップが作成できなかったために Cartographer を使うことにしました．ただ，この Cartographer，なかなかパラメータチューニングが大変でうまく使いこなすまでに時間がかかったので，このエントリではそのあたりを書き残せればと思います．時間があれば理論編も書きたいですが，，，いつものことながら理論系のエントリは時間がかかるので，気が向いたら書きます．
１．参考文献＆ブログ
パラメータチューニングのやり方は， Cartographer の作者の方が作ってくれている下記ドキュメントやブログを見ながら実施すればある程度は理解できると思います．
Cartographer ROS Documentation
Cartographer Documentation
https://buildmedia.readthedocs.org/media/pdf/google-cartographer/latest/google-cartographer.pdf
あと，下記はお世話になったブログです．
ssk0109.hatenablog.com
２．前提・システム構成
このエントリでは，下記の前提でマップ作っています．
2D Lidar が一つ，Odometry（x, y, yaw）有り，IMU無し
主要パラメータファイル一覧
変更が必要となるパラメータファイルは下記のとおりです．
backpack_2d.urdf
各センサーの搭載位置情報が記載された urdf ファイル．今回のエントリでは horizontal_lidar のみ使用．
手順
で，実際にマップを生成するにあたっては，下記の手順で設定・パラメータを変更していきます．
１．センサーの搭載位置情報を backpack_2d.urdf に記載する．
２．Topic 名や構成情報を backpack2d.lua に記載する．
３．Odometry がある程度正しくなるようにパラメータ調整．
４．調整された Odometry を使って Local SLAM のパラメータ調整（trajectory_builder2d.lua）．
５．狙った箇所で正しく Loop が閉じるように Global SLAM のパラメータ調整（pose_graph.lua）．
ステップ１ センサーの搭載位置情報を backpack_2d.urdf に記載
ここの設定はとても大事です！これを忘れるとうまく行きません！
ステップ２ Topic 名や構成情報を backpack2d.lua に記載
Topic 名や構成情報を backpack2d.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 がある程度正しくなるようにパラメータ調整
このステップはちょっと詐欺かもですが，，，，正しいマップを手っ取り早く作成したかったので，自分のオドメトリで軌跡がつながるようにヨーレートのオフセットを事前に計算して使うことにしました．
オフセット調整前
例えば，こんな感じで全く繋がらないオドメトリを．．．
オフセット調整してそこそこつながるようにします．
ステップ４ 調整された 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
※それ以外はデフォルトパラメータのままで動かしてみます．
上図を見てわかるように，デフォルトパラメータだとヨー角の誤差が積算してしまい，一周して元の地点に戻ってきてもマップが繋がらなくなってしまいました．ということで，ヨー角に関しては Local Scan Matcher の結果よりももう少しオドメトリを信じたほうが良さそうです．ひとまず効果代を見るために，パラメータを目一杯変えてみます．
【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000
だいぶいい感じになりました．というか，オドメトリしか使ってない感じになりました（笑）で，図中の吹き出しにも書いたんですが，ヨー角に関してはオドメトリ生成値の重みを強くしたものの，並進に関しては設定をいじらなかったため，２つのオドメトリ間でまだ差分が見られます．ということで，並進もオドメトリを強く信じてみます．
【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000
完全にオドメトリを信じて作ったマップになりました！マップもきれいになったし，めでたいめでたし．．．．だったら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
うーん．あんまり変わらないですね．．．．ということで，次に 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
再訪問したところで 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
８の字走行した部分（図の左側）に関してはループが閉じました．が，走行の最後の部分に関してはまだループが閉じていません．おそらく自分のデータのとり方が良くなかったんですが，一周回って同じところに帰ってきた時点でデータ取得を止めてしまったので，再訪箇所のオーバーラップが少なく，ループが閉じきれてないのかと思われます．ということで，Loop Close をもっと頻繁に発生させて，最後のループが閉じるかどうか実験してみます．
【Before】
POSE_GRAPH::optimize_every_n_nodes = 90
【After】
POSE_GRAPH::optimize_every_n_nodes = 5
うーん．なんかまだ微妙です．
Cartographer では，マップの小領域（Submapと呼ぶみたいです）に対して Scan 結果から得られる尤度を蓄積していき，更新が一定回数以上になったときにその Submap を大域的最適化対象に登録します．この「一定回数以上」のパラメータが下記になるのですが，最適化対象にするタイミングをもっと早めてみます．
【Before】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 90
【After】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10
うーん．ループを誤検知してしまいました．もう少しオドメトリを信じるべきか．．．
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
ということで，上記のパラメータ調整を経て，結構いい感じになったのでこれで良しとします．エンコーダとIMUを使ったヨーレート（赤線）は完全には正確でなく，一周回ってきたときには繋がってませんが，SLAMから生成されるオドメトリ（青色）は一周回った時点できれいにつながりました．
結果
ということで，全マップです！うーん．やっぱり，ループを綴じ込むのはかなり難しいですね．オドメトリを合わせればそこそこまでは行くものの，完全ではないのでループ綴じ込む程ではなく，逆に Local SLAM, Glocal SLAM の寄与を上げるとループじゃないところで誤マッチングが起こり．．．というシーソーゲームをひたすら繰り返してました．
コース１．確認走行区間
デフォルトからの変更パラメータ
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
コース２．交差点まで（この区間は難しく，，，完璧にはできませんでした．）
デフォルトからの変更パラメータ
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
コース３．研究学園駅
デフォルトからの変更パラメータ
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
コース４．研究学園駅前公園（この区間は難しく，，，完璧にはできませんでした．）
デフォルトからの変更パラメータ
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
コース５．交差点まで
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
コース６．交差点からゴール
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
あともう一つ，英語圏（≒アメリカ，ドイツ）からのアクセスをもっと増やせないかな〜という思いもあって，アクセス数が稼げそうなエントリに限って英語版を作ってみることにしました．アクセスが劇的に増えたら，また報告します（笑）
http://daily-tech.hatenablog.com/entry/2019/11/25/062304daily-tech.hatenablog.com
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.
Cartographer ROS Documentation
Cartographer Documentation
https://buildmedia.readthedocs.org/media/pdf/google-cartographer/latest/google-cartographer.pdf
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.
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
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 4. Kenkyu Gakuen Station Park (This interval is difficult and the loop does not get closed completely.)
Modified parameters from Cartographer's default
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
Course 5. Up to 2nd Intersection
Modified parameters from Cartographer's default
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
Course 6. From 1st Intersection to Goal
Modified parameters from Cartographer's default
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
つくばチャレンジ２０１９を終えて．．．
ということで，今年も終わりました．つくばチャレンジ本走行．
全く同じ書き出しでちょうど一年，二年前に同じ記事を書いてます．一年たつの早いですね～．
daily-tech.hatenablog.com
今年の結果
つくばチャレンジの様子はニコニコ動画とか Youtube とかでライブ配信されているのですが，発走前にちょっとしたインタビューをしてくれます．
インタビューア：次の発走は番号 1901 チームイエスマンです．ズバリ今年の目標をおきかせください．
自分 ：今年の目標は「完走」です！
インタビューア：おお！「完走」のお言葉いただきました！ありがとうございます！それでは頑張ってください！
インタビューア：それでは間もなく発走です．
〜10秒後〜
インタビューア：それではスタートです！
からの，，，
まさかの 2m 走行リタイアでした．．．．自分の発走順が２番目ということもあって，スタート地点は結構な観客がいたんですが，「完走します！」ってスタート直前に言っといて，スタート直後にロボットがくるくる回り始め．．．．なかなかの赤っ恥でした（笑）ということで，結果的には走行距離２ｍでフィニッシュでした．
敗因ですが，スタート地点にたくさんの人だかりができており，マップマッチングが全くできなかったことだと思われます．スタート地点の人だかりは昨年（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氏：本走行時のあたたかな声援
なんだか，サクラダファミリアみたいに，「永遠に完成しないロボット」みたいな雰囲気になってしまってますが，今年の結果で今まで以上に火がついた気がします！ということで，今年も言わせてください．
「来年は，完走します(ﾟДﾟ)ﾉ」
Visual Studio Code を使った C++ プログラミング 〜フォーマッターとインデクサー〜
毎回プロジェクトが変わるたびに検索して設定している気がするので，まとめておきます．
インデクサーの設定
インデクサーには GNU Global を使います．Ubuntu 16.04 の時は apt でちょうどいいバージョンをインストールすることができず，ソースをダウンロード＆ビルドしていたんですが，Ubuntu 18.04 の場合はそのままダウンロードすれば OK でした．
1. GNU Global のダウンロード
sudo apt install global
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
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
#includenamespace 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
#includenamespace 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 にする必要がありました．
Stixelを用いた視差画像からのフリースペース計算 〜（理論編２）
ということで，実装編に入る！と思っていたんですが，「どうやって動的計画法を実装するか？」という点を抑えておきたく下記の理論編１に引き続き理論編２のエントリも書くことにしました．
１．参考文献
こちらのエントリでは下記の論文の内容を見ていきますが，論文内容としては「Stixel をどうやって Cuda で実装するか？」です．
- GPU-accelerated real-time stixel computation
画像処理をやる人の端くれとして，一応 Cuda には触ったことはあるのですが，あまりむずかしい並列化はやったことがなく，今回の Stixel の問題は難易度も結構高くていい題材かと思ったので論文を読んでまとめてみることにしました．（が，このエントリでは動的計画法のとこまでしか書いてません．Cuda化のところは実装編で触れます．）
２．問題のおさらい
細かい問題定義は「理論編１」を見ていただくとして，簡単に問題構成のおさらいをしておきます．解こうとしている問題は，「視差画像 D が与えられたときに，尤度が最大となるような Stixel の集合 L を見つける．」ことです．
で，尤度が最大になるような Stixel L の集合をどうやって見つけるか？ですが，簡単に言うと可能な Stixel の組み合わせを総当りで試してみて，その中で尤度が最大になる組み合わせを採用します．この総当り計算を実施するときに，単純に全組み合わせを計算すると計算量が爆発してしまうので，「重複した計算を避ける」ために動的計画法が出てきます．で，動的計画法の話に入る前に，解法に重要な影響を与える２つの前提を書いておきます．
１．隣り合う列は互いに独立である．
３．動的計画法のおさらい
次に，簡単に動的計画法のおさらいをしておきます．この Stixel の問題と類似の問題を見つけたので，別のエントリとして作ってみました．
で，上記エントリの場合の問題設定は下記でした．
ロッド切り出し問題
あなたはコーナンで働いているとします！長〜い金属棒を仕入れて，これを小分けに切断して販売したいですが，金属棒は長さによって販売価格が異なります．そして，販売価格は長さに比例しているとは限りません！このときに，長さ n の金属棒をどのように切断すれば売上を最大化できるでしょうか？
動的計画法の観点では，この問題はとても良く似ています．
一点異なる点として，ロッド切り出し問題の場合は単純に切り出し方法を決めればよかったのに対し，Stixel の最適化計算では Stixel の種類（Sky, Ground, Object）が変数として増えてきます．
４．動的計画法を用いた Stixel の解法
ロッド切り出し問題との類似点がわかったので，同様の考え方で動的計画法を解いてみたいと思います．
４．１．細かな問題設定
下記，論文にある問題設定です．
・Stixel は３種類（Object, Ground, Sky）
・Stixel自体 の表記方法
1. 開始端を row = b，終了端を row = t にもつ，Object の Stixel：obtb = {vb, vt, Object}
2. 開始端を row = b，終了端を row = t にもつ，Sky の Stixel：skytb = {vb, vt, Sky}
3. 開始端を row = b，終了端を row = t にもつ，Ground の Stixel：groundtb = {vb, vt, Ground}
・集約コストの表記方法
1. 終了端を row = k にもち，最後の Stixel が Object である場合の集約コスト： OBk
2. 終了端を row = k にもち，最後の Stixel が Sky である場合の集約コスト： SKk
3. 終了端を row = k にもち，最後の Stixel が Ground である場合の集約コスト： GRk
で，やっぱりわかりにくいので描画します！
４．２．評価式
評価式自体は，理論編１のときに紹介したものがベースになっています．が，，，２つの論文で評価式がちょっと違っていたので，これは実装と突き合わせながら，実装編で確認します．
４．３．動的計画法
で，ようやく評価式ができたとして，動的計画法を用いて最適化問題をときます．
上図を見てもらうとわかると思うのですが，ある画像列に対して高さ h までの集約コストが最適になるものを求めるという問題なので，ロッドの切り出し問題とよく似ています．が，Stixel の種類が３つあるということと，評価式が実質的に２つの項（条件付き確率項，事前確率項）からなるということで，ちょっとだけ問題が複雑になります． この条件を鑑みて，動的計画法の漸化式を書いてみます．
上図に描画したとおり，Stixel の種類が３種になることから下記の点が異なってきます．
・考慮している Stixel (終了端が k) 自体に関して，３つの分岐ができる．
・考慮している Stixel (終了端が k) の一つ前の Stixel に関して，３つの分岐ができる．
ここまでで，動的計画法の漸化式が求まりました．前述したとおり，この論文のメインテーマは「Cudaでどうやって並列化するか？」なのですが，これは実装編でかければと思います．
動的計画法５ ロッド切り出し問題
ちょうど一年ぶりに動的計画法のエントリですが， Stixel を求める問題で動的計画法を使わないといけなかったので，簡単に復習の意味も込めてエントリをつくります．今回のロッド切り出し問題は Stixel を求める問題ととっても似てまして，これがわかれば Stixel 解決にむけて一気に理解が進むかな？と思いまして．
問題定義
ロッド切り出し問題
あなたはコーナンで働いているとします！長〜い金属棒を仕入れて，これを小分けに切断して販売したいですが，金属棒は長さによって販売価格が異なります．そして，販売価格は長さに比例しているとは限りません！このときに，長さ n の金属棒をどのように切断すれば売上を最大化できるでしょうか？
で，上記の問題を解くには金属棒の長さ毎の価格表が必要です．この価格表を下記のように定義します．（ちょっと話はそれますが，確かに長さの短いものは切れ端としていっぱいできそうなので，値段は安くなりそうですよね．）
長さ | １ | ２ | ３ | ４ | ５ | ６ | ７ | ８ | ９ | １０ ーーー|ーーー|ーーー|ーーー|ーーー|ーーーー|ーーーー|ーーーー|ーーーー|ーーーー|ーーー 価格 | １ | ５ | ８ | ９ | １０ | １７ | １７ | ２０ | ２４ | ３０
で，いつもの如く図示します．
で上図を見ていただけるとわかると思うのですが， n = 4 の場合の最適な切り出し方は [2, 2] であることがわかります．
次に，n = 5 の場合の切り出し方法を考えてみたいと思います．で，ここではちょっと考え方を変えて，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 = max1≦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件) を見る
Stixelを用いた視差画像からのフリースペース計算 〜（理論編１）
ということで，今年のつくばチャレンジのマイテーマ「視差画像を用いた障害物検知・回避」です．「障害物検知・回避」のフローを下記の４ステップに分けるとすると，このテーマは下記の２番に該当します．
１．ステレオ画像から視差を計算する．
２．視差画像をセグメンテーションする．<- ココ
３．セグメンテーションした結果をセンシング情報としてグリッドに反映する．
４．更新されたグリッドをもとに障害物を回避する経路を生成する．
１．参考文献
Stixelという概念を作ったのはDaimlerの研究所なんですが，下記の２つがDaimlerのVision Groupから出ているものです．
- The Stixel World - A Compact Medium LevelRepresentation of the 3D-World
- Towards a Global Optimal Multi-Layer Stixel Representation of Dense 3D Data
あと，つくばチャレンジで実際に使うにあたってCuda実装されたソースが必要だったのですが，その関連論文として下記のものを読みました．
- GPU-accelerated real-time stixel computation
２．Stixelって？
で，Stixel って何ぞや？って話なんですが，「ステレオから得た画像を適度にセグメンテーションしたもの」です．．．．っていっても説明になってないので，描画します！
１．まず，ステレオ画像を取得します．
２．次にステレオ画像から視差画像を計算します．
ここで視差画像が求まって，なんとなーくダンボールとか，テレビとかそれらしい感じで視差（≒距離）が出ていることがわかると思うのですが，視差画像って結局ピクセルに割り当てられた情報でしかないので，情報が多すぎます．このケースだと 1280 x 750 ≒ 100万点くらいの情報量になります．ただ，実際にロボットで使うときって，どこが走行できそうかどうか（フリースペース）が一番大切なので，もう少し抽象化してデータ量を減らせればステキです．下記のようなイメージです．
３．Stixel の計算
ということで，実際に Stixel を計算した結果です．結果の絵ではフリースペースではなくて，オブジェクトがあるところに Stixel がでているので，「フリースペースのイメージ」と逆になってしまってます．あと，パラメータチューニングとかがまだ十分でないので，オブジェクトの足元がちゃんと出てませんが，これはおいおい改善していきます．
このエントリでは，参考文献として挙げた論文の理論的な部分を解剖していきます．
３．Stixel の計算フロー
「Towards a Global Optimal Multi-Layer Stixel Representation of Dense 3D Data」を読み解いていきます．で
３．１．Stixel モデル
アルゴリズムとしては視差画像を入力として受け取り， Stixel を出力するわけですが，式展開をしていく中で D, L, Lu, Sn という４つのデータ単位が出てきます．
D : もとの視差画像．
L : D に対する Stixel の集合．
Lu : 列 u の Stixel の集合．
Sn : 列 u の Stixel の集合のうちの n 番目の Stixel．
ちょっと言葉だけだと味気ないので，簡単にこれを図示してみます．
ということで，このアルゴリズムの IN & OUT としては，視差画像 D を入力としてもらったときに，最適な Stixel の集合が詰まった L を計算することです．で，最適な Stixel の集合 L は列毎の最適な Stixel の集合 Lu からなっており，Lu はその列の最適な Stixel から構成されてます．
３．２．ベイズの定理を用いた Stixel の計算
で，「じゃあ，どうやって最適な Stixel を計算するの？」という話なんですが，ここではベイズの定理を用いて最適化する確率式を構築していきます．確率的に考えると，視差画像 D が求まったときに，最適な Stixel の集合 L を求めるということは下記の式を解くことになります．
で，SLAMのときにもやったようにベイズの定理を使って式変形していきます．
この式変形，ロボットの自己位置推定のときにもいっぱい使いましたが，条件付き確率の条件付けする変数を入れ替えることができました．ロボットの自己位置推定のときにも思ったんですが，実際の Input を条件付き確率の条件付けされる側にすることで，入力データを用いて答え合わせ（尤度計算）ができるようになるのがミソなんですかね．
それぞれの項の役割
ベイズの定理を用いて，P (L | D) ~ P(D | L) * P(L) と式展開できることがわかりましたが，各項にはそれぞれ役割があります．
P(D | L) （条件付き確率項）
この項は，仮定された Stixel に対して，与えられた視差画像を用いて ”答え合わせ” をする役割を担っています．
P(L)（事前確率項）
この項は，物理的な条件・仮定を加味した上で，当該の Stixel が起こりうる確率を計算する役割を担っています．見たまんまですが，この項は視差画像には依存していません．
４．条件付き確率項 P(D | L) の計算
うーん．ここからだいぶややこしくなってきますが，まずは条件付き確率項の詳細です．下記の２条件によって，条件付き確率項がある程度展開できることを見ました．
・隣り合う列は互いに独立である．
・それぞれのピクセルの視差の値は互いに独立である．
ここで，PD(dv | sn, v) の項をもう少し具体的に見ていきますが，この項は一つの Stixel を構成する一つのピクセルに対しての確率を計算します．ちょっと図示すると．．．
で，図の中に書いてしまいましたが，ステレオカメラの地表に対する傾きが決まれば，行の位置（v）によって視差がある程度決まります．例えば，下記のような感じです．
Stixel が路面だったとき：視差値 = α (v - v hor)
（α はステレオカメラの傾きによって決まる値，v hor画像中のは水平線の行座標）
Stixel が物体だったとき：視差値 = μ
（μ は物体の視差値．）
ということで，Stixel を仮定してしまえば尤度が求まることがわかります．で，ここからどうやって尤度を計算していくのかという話ですが，この論文では，確率を次のように定義していました．
で，上式では下記の２つの場合を考慮してモデルに組み込んでいます．
「１．ある物体が存在するときに，得られる視差値が有効である確率」
「２．ある物体が存在するときに，得られる視差値が無効である確率」
無効な場合というのは，おそらくオクルージョンとかの場合ですかね．で，ここからはエイヤで１の確率には正規分布を用いて，２の確率には一様分布を用いてモデリングしています．
イメージは上図の感じですかね．仮定した Stixel からあるべき視差値 fn(v) が求まり，これを実際の視差画像と比較することで１ピクセルずつ尤度を計算していく．．．と．地道ですね．ちなみに，正規化項 Anorm の理解は諦めました（笑）
５．事前確率項 P(L) の計算
次に事前確率項 P(L) の計算です．この項で，仮定した Stixel に対してモデリングの仮定を反映し，尤度を計算します．ここで，具体的に「モデリングの仮定」とは，，
- ベイズ情報量規準：１つの列中において，検知される物体は比較的すくない．
- 重力拘束：浮遊している物体はなく，オブジェクト・地表の境界部は地表上に存在する．
- 順序拘束：１つの列中において，上下隣接したセグメントがある場合，上側のセグメントの視差が大きいことが期待される．
やっぱり文章だと味気ないので，図示します．
上記の３つの拘束を尤度計算に取り込むため，隣接する２つの Stixel を使って尤度計算を実施します．隣接する２つの Stixel のうち，上の Stixel が常に下側の Stixel に条件付けされるような形になります．具体的には，，，，
５．１．隣接する Stixel を用いた尤度計算
次に，実際に P(Sn | Sn-1) をどのように計算していくのか見ていきます．Stixel Sn を構成する要素は Sn = {vbn, vtn, cn, fn(v)} であるので，P(Sn | Sn-1) = P(vbn, vtn, cn, fn(v) | vbn-1, vtn-1, cn-1, fn-1(v)) と書けます．論文では，この条件付き確率を下記のように分解していました．
P(Sn | Sn-1)
=
P(vbn, vtn, cn, fn(v) | vbn-1, vtn-1, cn-1, fn-1(v))
=
P(vbn | vtn-1) * P(vtn | vtn-1) * P(cn | vtn-1, cn-1) * P(fn(v) | cn, vtn-1, cn-1, fn-1(v))
うーん．この式変換が完全に数学的に出てくるのか，ちょっと仮定が入っているのかはわからないんですが，４つある項の意味合いは下記のようになります．
４つの項のそれぞれの確率的意味
１．P(vbn | vtn-1) :
隣接する Stixel を考えた場合に，下の Stixel の上端と上の Stixel の下端が隣り合っていること．
２．P(vtn | vtn-1) :
隣接する Stixel を考えた場合に，上の Stixel の長さを評価する項．
３．P(cn | vtn-1, cn-1) :
隣接する Stixel を考えた場合に，下の Stixel のカテゴリを踏まえた上で上の Stixel のカテゴリを評価する項．（下がオブジェクトなら，上に地面が来る確率は高くない etc...）
４．P(fn(v) | cn, vtn-1, cn-1, fn-1(v)) :
重力拘束，順序拘束，地面より下にオブジェクトがくる可能性を評価する項．
この問題では，事前確率項 P(L)，条件付き確率項 P(D | L) ともに事前計算して Look Up Table として保存しておくことで実行時間の増加を抑えることができます．（ただ，カメラの路面に対する角度が変わると，テーブルを再計算しないといけません．坂道になったりするとこの角度は変わってしまうので，都度計算がさけられないような気もします．）
論文では，P(fn(v) | cn, vtn-1, cn-1, fn-1(v)) の項を下記表にテーブルとしてまとめてました．ちなみに，ここで６つ（実質５つ）の変数が新たに導入されてますが，意味合いとしては，，，
⊿Z（⊿d） : オブジェクトの分離条件
２つの異なるオブジェクトとして分離するための条件変数です．「別の」Stixel として評価されるためには，下の Stixel から ±⊿Z 奥行きが離れている必要があります．⊿d は ⊿Zから計算されます．
ε : 重力拘束・順序拘束の緩和条件
例えば重力拘束を考えると，「浮遊している物体はなく，オブジェクト・地表の境界部は地表上に存在する．」になりますが，視差にはノイズも乗ったりするのでちょっとだけ緩和させてあげるために ε を導入します．
pord: 順序拘束を満たさない確率
「隣接する Stixel があった場合，基本的に上の Stixel のほうが遠方にある」というのが順序拘束ですが，この拘束を満たさない確率をこの変数で表現します．
pgrav: 重力拘束を満たさない確率
「地表面と Stixel 下端は接している」というのが重力拘束ですが，この拘束を満たさない確率をこの変数で表現します．
pblg: 地表面よりも下にオブジェクトがある確率
基本的に地表面よりも下にオブジェクトは無いはず．．．ですが，マンホールの蓋が取れてたりとかそういうケースを表現する確率です．
上記２つの変数を導入すると，P(fn(v) | cn, vtn-1, cn-1, fn-1(v)) のテーブルは下記のようになります．ちなみに，Sn は上側の Stixel，Sn-1 は下側の Stixel です．
ふー．．．．いつものことながら時間がかかるなあ．．．次は実装編です！
