Visual SLAM 14 講座 - ch10 の実践操作とピット回避
0. 実践前の豆知識のご紹介
ビジュアル SLAM (Simultaneous Localization and Mapping) バックエンドは、ビジュアル SLAM の問題を処理するためのアルゴリズムです。Visual SLAMとは、未知の環境において機械が自らの位置特定と地図構築を同時に実現する技術を指します。
ビジュアル SLAM バックエンドのタスクは、ビジュアル SLAM で最適化されたマップとロボットの軌道を維持することです。一般的なビジュアル SLAM バックエンド アルゴリズムには、G2O、ceres などのグラフ最適化ベースの手法と、カルマン フィルター、拡張カルマン フィルターなどのフィルター ベースの手法が含まれます。
ビジュアル SLAM バックエンド アルゴリズムは、センサー データのノイズと不確実性に対処し、アルゴリズムを最適化してエラーを削減し、精度を向上させる必要があります。同時に、ビジュアル SLAM バックエンド アルゴリズムは、大量のデータを迅速かつ効率的に処理し、計算結果をリアルタイムで更新および最適化する必要もあります。
ビジュアル SLAM バックエンド アルゴリズムは、自動運転、ドローン、ロボットなどの分野で幅広い用途に使用されます。
1. 実運用前の準備
- ターミナルでch10フォルダに入り、以下のコマンドを実行してコンパイルします。
mkdir build
cd build
cmake ..
//注意，j8还是其他主要看自己的电脑情况
make -j8
- ビルドファイル内で実行します。
注:作成プロセス中に、警告、しかし私たちの練習のプロセスにはほとんど影響しません。
2. 練習プロセス
2.1 g2o ネイティブ ポーズ グラフ
ビルドでステートメントを実行します。
./pose_graph_g2o_SE3 /home/fighter/slam/slambook2/ch10/sphere.g2o
実行結果:
ファイル result.g2o を生成; ターミナル出力:
read total 2500 vertices, 9799 edges.
optimizing ...
iteration= 0 chi2= 1023011093.967641 time= 0.616354 cumTime= 0.616354 edges= 9799 schur= 0 lambda= 805.622433 levenbergIter= 1
iteration= 1 chi2= 385118688.233187 time= 0.363139 cumTime= 0.979494 edges= 9799 schur= 0 lambda= 537.081622 levenbergIter= 1
iteration= 2 chi2= 166223726.693657 time= 0.350041 cumTime= 1.32953 edges= 9799 schur= 0 lambda= 358.054415 levenbergIter= 1
iteration= 3 chi2= 86610874.269316 time= 0.351792 cumTime= 1.68133 edges= 9799 schur= 0 lambda= 238.702943 levenbergIter= 1
iteration= 4 chi2= 40582782.710190 time= 0.388134 cumTime= 2.06946 edges= 9799 schur= 0 lambda= 159.135295 levenbergIter= 1
iteration= 5 chi2= 15055383.753041 time= 0.377086 cumTime= 2.44655 edges= 9799 schur= 0 lambda= 101.425210 levenbergIter= 1
iteration= 6 chi2= 6715193.487655 time= 0.363641 cumTime= 2.81019 edges= 9799 schur= 0 lambda= 37.664667 levenbergIter= 1
iteration= 7 chi2= 2171949.168382 time= 0.383848 cumTime= 3.19404 edges= 9799 schur= 0 lambda= 12.554889 levenbergIter= 1
iteration= 8 chi2= 740566.827049 time= 0.376922 cumTime= 3.57096 edges= 9799 schur= 0 lambda= 4.184963 levenbergIter= 1
iteration= 9 chi2= 313641.802464 time= 0.367001 cumTime= 3.93796 edges= 9799 schur= 0 lambda= 2.583432 levenbergIter= 1
iteration= 10 chi2= 82659.743578 time= 0.356125 cumTime= 4.29408 edges= 9799 schur= 0 lambda= 0.861144 levenbergIter= 1
iteration= 11 chi2= 58220.369189 time= 0.326571 cumTime= 4.62065 edges= 9799 schur= 0 lambda= 0.287048 levenbergIter= 1
iteration= 12 chi2= 52214.188561 time= 0.34423 cumTime= 4.96488 edges= 9799 schur= 0 lambda= 0.095683 levenbergIter= 1
iteration= 13 chi2= 50948.580336 time= 0.344858 cumTime= 5.30974 edges= 9799 schur= 0 lambda= 0.031894 levenbergIter= 1
iteration= 14 chi2= 50587.776729 time= 0.323255 cumTime= 5.633 edges= 9799 schur= 0 lambda= 0.016436 levenbergIter= 1
iteration= 15 chi2= 50233.038802 time= 0.321105 cumTime= 5.9541 edges= 9799 schur= 0 lambda= 0.010957 levenbergIter= 1
iteration= 16 chi2= 49995.082839 time= 0.321401 cumTime= 6.2755 edges= 9799 schur= 0 lambda= 0.007305 levenbergIter= 1
iteration= 17 chi2= 48876.738967 time= 0.702032 cumTime= 6.97753 edges= 9799 schur= 0 lambda= 0.009298 levenbergIter= 2
iteration= 18 chi2= 48806.625522 time= 0.373531 cumTime= 7.35107 edges= 9799 schur= 0 lambda= 0.006199 levenbergIter= 1
iteration= 19 chi2= 47790.891373 time= 0.764476 cumTime= 8.11554 edges= 9799 schur= 0 lambda= 0.008265 levenbergIter= 2
iteration= 20 chi2= 47713.626582 time= 0.344026 cumTime= 8.45957 edges= 9799 schur= 0 lambda= 0.005510 levenbergIter= 1
iteration= 21 chi2= 46869.323689 time= 0.698816 cumTime= 9.15838 edges= 9799 schur= 0 lambda= 0.007347 levenbergIter= 2
iteration= 22 chi2= 46802.585509 time= 0.359225 cumTime= 9.51761 edges= 9799 schur= 0 lambda= 0.004898 levenbergIter= 1
iteration= 23 chi2= 46128.758041 time= 0.631884 cumTime= 10.1495 edges= 9799 schur= 0 lambda= 0.006489 levenbergIter= 2
iteration= 24 chi2= 46069.133541 time= 0.309911 cumTime= 10.4594 edges= 9799 schur= 0 lambda= 0.004326 levenbergIter= 1
iteration= 25 chi2= 45553.862164 time= 0.622007 cumTime= 11.0814 edges= 9799 schur= 0 lambda= 0.005595 levenbergIter= 2
iteration= 26 chi2= 45511.762616 time= 0.311606 cumTime= 11.393 edges= 9799 schur= 0 lambda= 0.003730 levenbergIter= 1
iteration= 27 chi2= 45122.762999 time= 0.61714 cumTime= 12.0102 edges= 9799 schur= 0 lambda= 0.004690 levenbergIter= 2
iteration= 28 chi2= 45095.174397 time= 0.31117 cumTime= 12.3213 edges= 9799 schur= 0 lambda= 0.003127 levenbergIter= 1
iteration= 29 chi2= 44811.248505 time= 0.608863 cumTime= 12.9302 edges= 9799 schur= 0 lambda= 0.003785 levenbergIter= 2
saving optimization results ...
実際には、レーベンバーグ・マルカート降下法が使用され、反復回数は 30 回に選択されます。
ファイル result.g2o を開きます。
ターミナルで次のコマンドを実行します。
g2o_viewer result.g2o
実行中のグラフは次のとおりです (g2o に付属の頂点とエッジを使用した結果)。
2.2 リー代数におけるポーズグラフの最適化
ビルドでステートメントを実行します。
./pose_graph_g2o_lie /home/fighter/slam/slambook2/ch10/sphere.g2o
実行結果:
ファイル result_lie.g2o を生成; 端末出力:
read total 2500 vertices, 9799 edges.
optimizing ...
iteration= 0 chi2= 674837160.579968 time= 0.419014 cumTime= 0.419014 edges= 9799 schur= 0 lambda= 6658.554263 levenbergIter= 1
iteration= 1 chi2= 234706314.970484 time= 0.307203 cumTime= 0.726217 edges= 9799 schur= 0 lambda= 2219.518088 levenbergIter= 1
iteration= 2 chi2= 142146174.348537 time= 0.306181 cumTime= 1.0324 edges= 9799 schur= 0 lambda= 739.839363 levenbergIter= 1
iteration= 3 chi2= 83834595.145595 time= 0.309102 cumTime= 1.3415 edges= 9799 schur= 0 lambda= 246.613121 levenbergIter= 1
iteration= 4 chi2= 41878079.903257 time= 0.314584 cumTime= 1.65608 edges= 9799 schur= 0 lambda= 82.204374 levenbergIter= 1
iteration= 5 chi2= 16598628.119947 time= 0.306542 cumTime= 1.96263 edges= 9799 schur= 0 lambda= 27.401458 levenbergIter= 1
iteration= 6 chi2= 6137666.739406 time= 0.306009 cumTime= 2.26864 edges= 9799 schur= 0 lambda= 9.133819 levenbergIter= 1
iteration= 7 chi2= 2182986.250589 time= 0.313833 cumTime= 2.58247 edges= 9799 schur= 0 lambda= 3.044606 levenbergIter= 1
iteration= 8 chi2= 732676.668220 time= 0.304348 cumTime= 2.88682 edges= 9799 schur= 0 lambda= 1.014869 levenbergIter= 1
iteration= 9 chi2= 284457.115176 time= 0.305686 cumTime= 3.1925 edges= 9799 schur= 0 lambda= 0.338290 levenbergIter= 1
iteration= 10 chi2= 170796.109734 time= 0.317388 cumTime= 3.50989 edges= 9799 schur= 0 lambda= 0.181974 levenbergIter= 1
iteration= 11 chi2= 145466.315841 time= 0.305792 cumTime= 3.81568 edges= 9799 schur= 0 lambda= 0.060658 levenbergIter= 1
iteration= 12 chi2= 142373.179501 time= 0.347022 cumTime= 4.16271 edges= 9799 schur= 0 lambda= 0.020219 levenbergIter= 1
iteration= 13 chi2= 137485.756901 time= 0.304775 cumTime= 4.46748 edges= 9799 schur= 0 lambda= 0.006740 levenbergIter= 1
iteration= 14 chi2= 131202.175665 time= 0.311505 cumTime= 4.77899 edges= 9799 schur= 0 lambda= 0.002247 levenbergIter= 1
iteration= 15 chi2= 128006.202529 time= 0.30704 cumTime= 5.08603 edges= 9799 schur= 0 lambda= 0.000749 levenbergIter= 1
iteration= 16 chi2= 127587.860945 time= 0.313496 cumTime= 5.39952 edges= 9799 schur= 0 lambda= 0.000250 levenbergIter= 1
iteration= 17 chi2= 127578.599359 time= 0.322269 cumTime= 5.72179 edges= 9799 schur= 0 lambda= 0.000083 levenbergIter= 1
iteration= 18 chi2= 127578.573853 time= 0.326536 cumTime= 6.04833 edges= 9799 schur= 0 lambda= 0.000028 levenbergIter= 1
iteration= 19 chi2= 127578.573840 time= 0.328882 cumTime= 6.37721 edges= 9799 schur= 0 lambda= 0.000018 levenbergIter= 1
iteration= 20 chi2= 127578.573840 time= 0.315369 cumTime= 6.69258 edges= 9799 schur= 0 lambda= 0.000012 levenbergIter= 1
iteration= 21 chi2= 127578.573840 time= 0.308945 cumTime= 7.00152 edges= 9799 schur= 0 lambda= 0.000008 levenbergIter= 1
iteration= 22 chi2= 127578.573840 time= 3.00403 cumTime= 10.0056 edges= 9799 schur= 0 lambda= 296083660142.312988 levenbergIter= 10
saving optimization results ...
23 回の反復後、全体的な誤差は変化せず、実際に最適化アルゴリズムが停止することがわかります。
ファイル result_lie.g2o: を
ターミナルで開きます:
g2o_viewer result_lie.g2o
実行中のグラフは次のとおりです (リー代数を使用してノードと最適化された結果をカスタマイズします)。
ウィンドウの [最適化] ボタンをクリックすると、g2o は最適化に独自の SE3 頂点を使用します。ウィンドウの下のテキスト ボックスに次の内容が表示されます。
loaded result_lie.g2o with 2500 vertices and 9799 measurements
graph is fixed by node 2499
# Using CHOLMOD poseDim -1 landMarkDim -1 blockordering 1
Preparing (no marginalization of Landmarks)
iteration= 0 chi2= 44360.504602 time= 1.01586 cumTime= 1.01586 edges= 9799 schur= 0
iteration= 1 chi2= 44360.466873 time= 0.247692 cumTime= 1.26355 edges= 9799 schur= 0
iteration= 2 chi2= 44360.466872 time= 0.253022 cumTime= 1.51658 edges= 9799 schur= 0
iteration= 3 chi2= 44360.466872 time= 0.247728 cumTime= 1.7643 edges= 9799 schur= 0
iteration= 4 chi2= 44360.466872 time= 0.272316 cumTime= 2.03662 edges= 9799 schur= 0
iteration= 5 chi2= 44360.466872 time= 0.249167 cumTime= 2.28579 edges= 9799 schur= 0
iteration= 6 chi2= 44360.466872 time= 0.248997 cumTime= 2.53478 edges= 9799 schur= 0
iteration= 7 chi2= 44360.466872 time= 0.28726 cumTime= 2.82204 edges= 9799 schur= 0
iteration= 8 chi2= 44360.466872 time= 0.243892 cumTime= 3.06594 edges= 9799 schur= 0
iteration= 9 chi2= 44360.466872 time= 0.245539 cumTime= 3.31148 edges= 9799 schur= 0
3. 発生した問題と解決策
3.1 Pose_graph_g2o_lie の実行中にエラーが発生しました
- 発生するエラーは次のとおりです。
Sophus ensure failed in function 'void Sophus::SO3Base<Derived>::normalize() [with Derived = Sophus::SO3<double>]', file '/usr/local/include/sophus/so3.hpp', line 273.
Quaternion ( 0.706662 4.32706e-17 0.707551 -4.3325e-17) should not be close to zero!
Aborted
理由：
頂点クラス VertexSE3LieAlgebra の読み取り関数 virtual bool read(istream& is) には戻り値がないため、上記のエラーが報告されます。
解決：
関数virtual bool read(istream& is)にreturn trueを追加します。