diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100755 index 0000000..a19ce3f --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,6 @@ + 2018-03-26 Masahiro Tomono + + * syundo0730さんの提案にもとづき、*.cpp, *.h, *.mdのファイルエンコーディングをS-JISからUTF-8-BOMに変更した。 + * CMakeLists.txtは、日本語コメントを削除し、ASCIIとした。 + （Ubuntu14.04のcmakeでエラーが出るのを防ぐため） + diff --git a/CMakeLists.txt b/CMakeLists.txt index 6d65227..c545275 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,6 @@ project(LittleSLAM) cmake_minimum_required(VERSION 2.8) -# C++11g if(UNIX) include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) @@ -16,11 +15,6 @@ if(UNIX) endif() endif() -if(UNIX) # gccSHIFT-JISǂނ - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -finput-charset=cp932") - SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fexec-charset=cp932") -endif() - SET(CMAKE_BUILD_TYPE "Release") add_subdirectory(cui cui) diff --git a/README.md b/README.md index 40c544c..f5216b1 100755 --- a/README.md +++ b/README.md @@ -1,24 +1,24 @@ -# LittleSLAM +# LittleSLAM -## LittleSLAMɂ‚ +## LittleSLAMについて -LittleSLAḾASLAMwKpvOłB -2D[UXLĩf[^iXLjƃIhgf[^i[t@C͂A -{bgʒűOՂ2D_Qn}gnuplotɏo͂܂B +LittleSLAMは、SLAM学習用プログラムです。 +2Dレーザスキャナのデータ（スキャン）とオドメトリデータを格納したファイルを入力し、 +ロボット位置の軌跡と2D点群地図をgnuplot上に出力します。 -LittleSLAḾAXL}b`OɊÂʒuA[UXLiƃIhg̃ZTZA -Graph-based SLAMɊÂ[v‚݂Ȃǂ̗vfZp\Ă܂B +LittleSLAMは、スキャンマッチングに基づく位置合せ、レーザスキャナとオドメトリのセンサ融合、 +Graph-based SLAMに基づくループ閉じ込みなどの要素技術から構成されています。 -LittleSLAM͎Ql[1]̋ނƂčꂽvOłA -킩₷D悵ăVvȃASY̗pĂ܂B -̂߁AtXybNSLAMvOƔׂƐ\͗܂A -e̗͂₷ȂĂ܂B +LittleSLAMは参考書籍[1]の教材として作られたプログラムであり、 +わかりやすさを優先してシンプルなアルゴリズムを採用しています。 +そのため、フルスペックのSLAMプログラムと比べると性能は落ちますが、 +内容の理解はしやすくなっています。 -## s‹ +## 実行環境 -LittleSLAM̓vO~OC++ŋLqĂ܂B -mFs‹͉L̂̂łB64rbgłłB +LittleSLAMはプログラミング言語C++で記述されています。 +動作を確認した実行環境は下記のものです。いずれも64ビット版です。 | OS | C++ | |:--:|:---:| @@ -27,60 +27,60 @@ LittleSLAM | Linux Ubuntu 14.04 LTS | gcc 4.8.4| | Linux Ubuntu 16.04 LTS | gcc 5.4.0| -32rbgOSł̓mF͂ĂȂ̂ŁAKvȏꍇ͂ŎĂB +32ビットOSでの動作確認はしていないので、必要な場合はご自分で試してください。 -## Kvȃ\tgEFA +## 必要なソフトウェア -LittleSLAM̎sɂ́AL̃\tgEFAKvłB +LittleSLAMの実行には、下記のソフトウェアが必要です。 -| \tgEFA | e | o[W | +| ソフトウェア | 内容 | バージョン | |:------------:|:----:|:----------:| -| Boost | C++ėpCu |1.58.0 | -| Eigen3 | `㐔Cu|3.2.4 | -| gnuplot | Ot`c[ |5.0 | -| CMake | rhxc[ |3.2.2 | -| p2o | Graph-based SLAM\o|beta | +| Boost | C++汎用ライブラリ |1.58.0 | +| Eigen3 | 線形代数ライブラリ|3.2.4 | +| gnuplot | グラフ描画ツール |5.0 | +| CMake | ビルド支援ツール |3.2.2 | +| p2o | Graph-based SLAMソルバ|beta | -o[WLittleSLAM̊JŎgp̂łAmȏł͂܂B -ȏ̃o[WłΒʏ͓삵܂B -ȉ̃o[Wł삷”\͂܂B +バージョンはLittleSLAMの開発で使用したものであり、明確な条件ではありません。 +これ以上のバージョンであれば通常は動作します。 +これ以下のバージョンでも動作する可能性はあります。 -## g +## 使い方 -- Windowsł̎g[](doc/install-win.md) +- Windowsでの使い方は[こちら](doc/install-win.md) -- Linuxł̎g[](doc/install-linux.md) +- Linuxでの使い方は[こちら](doc/install-linux.md) -## f[^Zbg +## データセット -p6‚̃f[^t@CpӂĂ܂B\Ɉꗗ܂B -[](https://furo.org/software/little_slam/dataset.zip)_E[hł܂B +実験用に6個のデータファイルを用意しています。下表に一覧を示します。 +[ここ](https://furo.org/software/little_slam/dataset.zip)からダウンロードできます。 -| t@C | e | +| ファイル名 | 内容 | |:--------------------|:-------------| -| corridor.lsc | LiPꃋ[vj | -| hall.lsc | LԁiPꃋ[vj | -| corridor-degene.lsc | Liމj | -| hall-degene.lsc | Lԁiމj | -| corridor-loops.lsc | Lid[vj | -| hall-loops.lsc | Lԁid[vj | +| corridor.lsc | 廊下（単一ループ） | +| hall.lsc | 広間（単一ループ） | +| corridor-degene.lsc | 廊下（退化） | +| hall-degene.lsc | 広間（退化） | +| corridor-loops.lsc | 廊下（多重ループ） | +| hall-loops.lsc | 広間（多重ループ） | -## JX^}CY +## カスタマイズ -LittleSLAM͊wKpvOłA{`炢‚̉ǂo -悤ɃJX^}CYł܂B -ڍׂ[](doc/customize.md)QƂĂB +LittleSLAMは学習用プログラムであり、基本形からいくつかの改良を経て +完成するようにカスタマイズできます。 +詳細は[こちら](doc/customize.md)を参照してください。 -## Ql +## 参考書籍 -L̏ЂSLAM̉łBSLAM̈ʓIȉƂƂɁA -̗ƂLittleSLAMނɗpÃ\[XR[h̏ڍׂĂ܂B +下記の書籍はSLAMの解説書です。SLAMの一般的な解説をするとともに、 +具体例としてLittleSLAMを教材に用い、そのソースコードの詳細を説明しています。 -[1] F[TAuSLAM -- {bg̎Ȉʒuƒn}\z̋ZpvAI[ЁA2018N +[1] 友納正裕、「SLAM入門 -- ロボットの自己位置推定と地図構築の技術」、オーム社、2018年 -## CZX +## ライセンス -- LittleSLAḾAMPL-2.0CZXɂƂÂĂ܂B +- LittleSLAMは、MPL-2.0ライセンスにもとづいています。 diff --git a/cui/FrameworkCustomizer.cpp b/cui/FrameworkCustomizer.cpp index 38f312f..60e4a56 100755 --- a/cui/FrameworkCustomizer.cpp +++ b/cui/FrameworkCustomizer.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -16,12 +16,12 @@ using namespace std; -// t[[N̊{ݒ +// フレームワークの基本部分を設定 void FrameworkCustomizer::makeFramework() { smat.setPoseEstimator(&poest); smat.setPoseFuser(&pfu); - // LoopDetectorSS͎gi܂Ă̂ŁAœ + // LoopDetectorSSは使う部品が決まっているので、ここで入れる lpdSS.setPoseEstimator(&poest); lpdSS.setPoseFuser(&pfu); lpdSS.setDataAssociator(&dassGT); @@ -31,16 +31,16 @@ void FrameworkCustomizer::makeFramework() { sfront->setScanMatcher(&smat); } -/////// p +/////// 実験用 -// t[[N{\ +// フレームワーク基本構成 void FrameworkCustomizer::customizeA() { - pcmap = &pcmapBS; // SXL_ۑ_Qn} - RefScanMaker *rsm = &rsmBS; // OXLQƃXLƂ - DataAssociator *dass = &dassLS; // `Tɂf[^ΉÂ - CostFunction *cfunc = &cfuncED; // [NbhRXg֐Ƃ - PoseOptimizer *popt = &poptSD; // ŋ}~@ɂœK - LoopDetector *lpd = &lpdDM; // _~[̃[vo + pcmap = &pcmapBS; // 全スキャン点を保存する点群地図 + RefScanMaker *rsm = &rsmBS; // 直前スキャンを参照スキャンとする + DataAssociator *dass = &dassLS; // 線形探索によるデータ対応づけ + CostFunction *cfunc = &cfuncED; // ユークリッド距離をコスト関数とする + PoseOptimizer *popt = &poptSD; // 最急降下法による最適化 + LoopDetector *lpd = &lpdDM; // ダミーのループ検出 popt->setCostFunction(cfunc); poest.setDataAssociator(dass); @@ -50,17 +50,17 @@ void FrameworkCustomizer::customizeA() { smat.setRefScanMaker(rsm); sfront->setLoopDetector(lpd); sfront->setPointCloudMap(pcmap); - sfront->setDgCheck(false); // ZTZȂ + sfront->setDgCheck(false); // センサ融合しない } -// iqe[u̗p +// 格子テーブルの利用 void FrameworkCustomizer::customizeB() { - pcmap = &pcmapGT; // iqe[uŊǗ_Qn} - RefScanMaker *rsm = &rsmLM; // Ǐn}QƃXLƂ - DataAssociator *dass = &dassLS; // `Tɂf[^ΉÂ - CostFunction *cfunc = &cfuncED; // [NbhRXg֐Ƃ - PoseOptimizer *popt = &poptSD; // ŋ}~@ɂœK - LoopDetector *lpd = &lpdDM; // _~[̃[vo + pcmap = &pcmapGT; // 格子テーブルで管理する点群地図 + RefScanMaker *rsm = &rsmLM; // 局所地図を参照スキャンとする + DataAssociator *dass = &dassLS; // 線形探索によるデータ対応づけ + CostFunction *cfunc = &cfuncED; // ユークリッド距離をコスト関数とする + PoseOptimizer *popt = &poptSD; // 最急降下法による最適化 + LoopDetector *lpd = &lpdDM; // ダミーのループ検出 popt->setCostFunction(cfunc); poest.setDataAssociator(dass); @@ -70,17 +70,17 @@ void FrameworkCustomizer::customizeB() { smat.setRefScanMaker(rsm); sfront->setLoopDetector(lpd); sfront->setPointCloudMap(pcmap); - sfront->setDgCheck(false); // ZTZȂ + sfront->setDgCheck(false); // センサ融合しない } -// ŋ}~@̌ɒTs +// 最急降下法の後に直線探索を行う void FrameworkCustomizer::customizeC() { - pcmap = &pcmapGT; // iqe[uŊǗ_Qn} - RefScanMaker *rsm = &rsmLM; // Ǐn}QƃXLƂ - DataAssociator *dass = &dassLS; // `Tɂf[^ΉÂ - CostFunction *cfunc = &cfuncED; // [NbhRXg֐Ƃ - PoseOptimizer *popt = &poptSL; // ŋ}~@ƒTɂœK - LoopDetector *lpd = &lpdDM; // _~[̃[vo + pcmap = &pcmapGT; // 格子テーブルで管理する点群地図 + RefScanMaker *rsm = &rsmLM; // 局所地図を参照スキャンとする + DataAssociator *dass = &dassLS; // 線形探索によるデータ対応づけ + CostFunction *cfunc = &cfuncED; // ユークリッド距離をコスト関数とする + PoseOptimizer *popt = &poptSL; // 最急降下法と直線探索による最適化 + LoopDetector *lpd = &lpdDM; // ダミーのループ検出 popt->setCostFunction(cfunc); poest.setDataAssociator(dass); @@ -90,17 +90,17 @@ void FrameworkCustomizer::customizeC() { smat.setRefScanMaker(rsm); sfront->setLoopDetector(lpd); sfront->setPointCloudMap(pcmap); - sfront->setDgCheck(false); // ZTZȂ + sfront->setDgCheck(false); // センサ融合しない } -// f[^ΉÂiqe[uōs +// データ対応づけを格子テーブルで行う void FrameworkCustomizer::customizeD() { - pcmap = &pcmapGT; // iqe[uŊǗ_Qn} - RefScanMaker *rsm = &rsmLM; // Ǐn}QƃXLƂ - DataAssociator *dass = &dassGT; // iqe[uɂf[^ΉÂ - CostFunction *cfunc = &cfuncED; // [NbhRXg֐Ƃ - PoseOptimizer *popt = &poptSL; // ŋ}~@ƒTɂœK - LoopDetector *lpd = &lpdDM; // _~[̃[vo + pcmap = &pcmapGT; // 格子テーブルで管理する点群地図 + RefScanMaker *rsm = &rsmLM; // 局所地図を参照スキャンとする + DataAssociator *dass = &dassGT; // 格子テーブルによるデータ対応づけ + CostFunction *cfunc = &cfuncED; // ユークリッド距離をコスト関数とする + PoseOptimizer *popt = &poptSL; // 最急降下法と直線探索による最適化 + LoopDetector *lpd = &lpdDM; // ダミーのループ検出 popt->setCostFunction(cfunc); poest.setDataAssociator(dass); @@ -110,17 +110,17 @@ void FrameworkCustomizer::customizeD() { smat.setRefScanMaker(rsm); sfront->setLoopDetector(lpd); sfront->setPointCloudMap(pcmap); - sfront->setDgCheck(false); // ZTZȂ + sfront->setDgCheck(false); // センサ融合しない } -// XL_Ԋuψꉻlj +// スキャン点間隔均一化を追加 void FrameworkCustomizer::customizeE() { - pcmap = &pcmapGT; // iqe[uŊǗ_Qn} - RefScanMaker *rsm = &rsmLM; // Ǐn}QƃXLƂ - DataAssociator *dass = &dassGT; // iqe[uɂf[^ΉÂ - CostFunction *cfunc = &cfuncED; // [NbhRXg֐Ƃ - PoseOptimizer *popt = &poptSL; // ŋ}~@ƒTɂœK - LoopDetector *lpd = &lpdDM; // _~[̃[vo + pcmap = &pcmapGT; // 格子テーブルで管理する点群地図 + RefScanMaker *rsm = &rsmLM; // 局所地図を参照スキャンとする + DataAssociator *dass = &dassGT; // 格子テーブルによるデータ対応づけ + CostFunction *cfunc = &cfuncED; // ユークリッド距離をコスト関数とする + PoseOptimizer *popt = &poptSL; // 最急降下法と直線探索による最適化 + LoopDetector *lpd = &lpdDM; // ダミーのループ検出 popt->setCostFunction(cfunc); poest.setDataAssociator(dass); @@ -128,20 +128,20 @@ void FrameworkCustomizer::customizeE() { pfu.setDataAssociator(dass); smat.setPointCloudMap(pcmap); smat.setRefScanMaker(rsm); - smat.setScanPointResampler(&spres); // XL_Ԋuψꉻ + smat.setScanPointResampler(&spres); // スキャン点間隔均一化 sfront->setLoopDetector(lpd); sfront->setPointCloudMap(pcmap); - sfront->setDgCheck(false); // ZTZȂ + sfront->setDgCheck(false); // センサ融合しない } -// XL_̖@vZlj +// スキャン点の法線計算を追加 void FrameworkCustomizer::customizeF() { - pcmap = &pcmapGT; // iqe[uŊǗ_Qn} - RefScanMaker *rsm = &rsmLM; // Ǐn}QƃXLƂ - DataAssociator *dass = &dassGT; // iqe[uɂf[^ΉÂ - CostFunction *cfunc = &cfuncPD; // RXg֐Ƃ - PoseOptimizer *popt = &poptSL; // ŋ}~@ƒTɂœK - LoopDetector *lpd = &lpdDM; // _~[̃[vo + pcmap = &pcmapGT; // 格子テーブルで管理する点群地図 + RefScanMaker *rsm = &rsmLM; // 局所地図を参照スキャンとする + DataAssociator *dass = &dassGT; // 格子テーブルによるデータ対応づけ + CostFunction *cfunc = &cfuncPD; // 垂直距離をコスト関数とする + PoseOptimizer *popt = &poptSL; // 最急降下法と直線探索による最適化 + LoopDetector *lpd = &lpdDM; // ダミーのループ検出 popt->setCostFunction(cfunc); poest.setDataAssociator(dass); @@ -149,20 +149,20 @@ void FrameworkCustomizer::customizeF() { pfu.setDataAssociator(dass); smat.setPointCloudMap(pcmap); smat.setRefScanMaker(rsm); - smat.setScanPointAnalyser(&spana); // XL_̖@vZ + smat.setScanPointAnalyser(&spana); // スキャン点の法線計算 sfront->setLoopDetector(lpd); sfront->setPointCloudMap(pcmap); - sfront->setDgCheck(false); // ZTZȂ + sfront->setDgCheck(false); // センサ融合しない } -// XL_ԊuψꉻƖ@vZlj +// スキャン点間隔均一化と法線計算を追加 void FrameworkCustomizer::customizeG() { - pcmap = &pcmapGT; // iqe[uŊǗ_Qn} - RefScanMaker *rsm = &rsmLM; // Ǐn}QƃXLƂ - DataAssociator *dass = &dassGT; // iqe[uɂf[^ΉÂ - CostFunction *cfunc = &cfuncPD; // RXg֐Ƃ - PoseOptimizer *popt = &poptSL; // ŋ}~@ƒTɂœK - LoopDetector *lpd = &lpdDM; // _~[̃[vo + pcmap = &pcmapGT; // 格子テーブルで管理する点群地図 + RefScanMaker *rsm = &rsmLM; // 局所地図を参照スキャンとする + DataAssociator *dass = &dassGT; // 格子テーブルによるデータ対応づけ + CostFunction *cfunc = &cfuncPD; // 垂直距離をコスト関数とする + PoseOptimizer *popt = &poptSL; // 最急降下法と直線探索による最適化 + LoopDetector *lpd = &lpdDM; // ダミーのループ検出 popt->setCostFunction(cfunc); poest.setDataAssociator(dass); @@ -170,22 +170,22 @@ void FrameworkCustomizer::customizeG() { pfu.setDataAssociator(dass); smat.setPointCloudMap(pcmap); smat.setRefScanMaker(rsm); - smat.setScanPointResampler(&spres); // XL_Ԋuψꉻ - smat.setScanPointAnalyser(&spana); // XL_̖@vZ + smat.setScanPointResampler(&spres); // スキャン点間隔均一化 + smat.setScanPointAnalyser(&spana); // スキャン点の法線計算 sfront->setLoopDetector(lpd); sfront->setPointCloudMap(pcmap); - sfront->setDgCheck(false); // ZTZȂ + sfront->setDgCheck(false); // センサ融合しない } -// ZTZlj +// センサ融合を追加 void FrameworkCustomizer::customizeH() { -// pcmap = &pcmapGT; // iqe[uŊǗ_Qn} - pcmap = &pcmapLP; // n}ƂɊǗ_Qn}BcIƂ̔rp - RefScanMaker *rsm = &rsmLM; // Ǐn}QƃXLƂ - DataAssociator *dass = &dassGT; // iqe[uɂf[^ΉÂ - CostFunction *cfunc = &cfuncPD; // RXg֐Ƃ - PoseOptimizer *popt = &poptSL; // ŋ}~@ƒTɂœK - LoopDetector *lpd = &lpdDM; // _~[̃[vo +// pcmap = &pcmapGT; // 格子テーブルで管理する点群地図 + pcmap = &pcmapLP; // 部分地図ごとに管理する点群地図。cIとの比較用 + RefScanMaker *rsm = &rsmLM; // 局所地図を参照スキャンとする + DataAssociator *dass = &dassGT; // 格子テーブルによるデータ対応づけ + CostFunction *cfunc = &cfuncPD; // 垂直距離をコスト関数とする + PoseOptimizer *popt = &poptSL; // 最急降下法と直線探索による最適化 + LoopDetector *lpd = &lpdDM; // ダミーのループ検出 popt->setCostFunction(cfunc); poest.setDataAssociator(dass); @@ -197,17 +197,17 @@ void FrameworkCustomizer::customizeH() { smat.setScanPointAnalyser(&spana); sfront->setLoopDetector(lpd); sfront->setPointCloudMap(pcmap); - sfront->setDgCheck(true); // ZTZ + sfront->setDgCheck(true); // センサ融合する } -// ZTZƃ[v‚݂lj +// センサ融合とループ閉じ込みを追加 void FrameworkCustomizer::customizeI() { - pcmap = &pcmapLP; // n}ƂɊǗ_Qn} - RefScanMaker *rsm = &rsmLM; // Ǐn}QƃXLƂ - DataAssociator *dass = &dassGT; // iqe[uɂf[^ΉÂ - CostFunction *cfunc = &cfuncPD; // RXg֐Ƃ - PoseOptimizer *popt = &poptSL; // ŋ}~@ƒTɂœK - LoopDetector *lpd = &lpdSS; // n}p[vo + pcmap = &pcmapLP; // 部分地図ごとに管理する点群地図 + RefScanMaker *rsm = &rsmLM; // 局所地図を参照スキャンとする + DataAssociator *dass = &dassGT; // 格子テーブルによるデータ対応づけ + CostFunction *cfunc = &cfuncPD; // 垂直距離をコスト関数とする + PoseOptimizer *popt = &poptSL; // 最急降下法と直線探索による最適化 + LoopDetector *lpd = &lpdSS; // 部分地図を用いたループ検出 popt->setCostFunction(cfunc); poest.setDataAssociator(dass); @@ -219,5 +219,5 @@ void FrameworkCustomizer::customizeI() { smat.setScanPointAnalyser(&spana); sfront->setLoopDetector(lpd); sfront->setPointCloudMap(pcmap); - sfront->setDgCheck(true); // ZTZ + sfront->setDgCheck(true); // センサ融合する } diff --git a/cui/FrameworkCustomizer.h b/cui/FrameworkCustomizer.h index 0553bad..8d0d1cc 100755 --- a/cui/FrameworkCustomizer.h +++ b/cui/FrameworkCustomizer.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -44,7 +44,7 @@ class FrameworkCustomizer { - // t[[Np̕i + // フレームワーク改造用の部品 RefScanMakerBS rsmBS; RefScanMakerLM rsmLM; DataAssociatorLS dassLS; @@ -56,8 +56,8 @@ class FrameworkCustomizer PointCloudMapBS pcmapBS; PointCloudMapGT pcmapGT; PointCloudMapLP pcmapLP; - PointCloudMap *pcmap; // SlamLauncherŎQƂ邽߃oϐɂ - LoopDetector lpdDM; // _~[BȂ + PointCloudMap *pcmap; // SlamLauncherで参照するためメンバ変数にする + LoopDetector lpdDM; // ダミー。何もしない LoopDetectorSS lpdSS; ScanPointResampler spres; ScanPointAnalyser spana; diff --git a/cui/MapDrawer.cpp b/cui/MapDrawer.cpp index c51b4a0..9158cb3 100755 --- a/cui/MapDrawer.cpp +++ b/cui/MapDrawer.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -16,61 +16,61 @@ using namespace std; -////////// Gnuplotɂn}` ////////// +////////// Gnuplotによる地図描画 ////////// -// n}ƋOՂ` +// 地図と軌跡を描画 void MapDrawer::drawMapGp(const PointCloudMap &pcmap) { - const vector &lps = pcmap.globalMap; // n}̓_Q - const vector &poses = pcmap.poses; // {bgO + const vector &lps = pcmap.globalMap; // 地図の点群 + const vector &poses = pcmap.poses; // ロボット軌跡 drawGp(lps, poses); } -// XL1‚` +// スキャン1個を描画 void MapDrawer::drawScanGp(const Scan2D &scan) { vector poses; - Pose2D pose; // _ - poses.emplace_back(pose); // drawGpg߂vectorɓ + Pose2D pose; // 原点 + poses.emplace_back(pose); // drawGpを使うためにvectorに入れる drawGp(scan.lps, poses); } -// {bgOՂ` +// ロボット軌跡だけを描画 void MapDrawer::drawTrajectoryGp(const vector &poses) { - vector lps; // drawGpg߂̃_~[ij + vector lps; // drawGpを使うためのダミー（空） drawGp(lps, poses); } ////////// void MapDrawer::drawGp(const vector &lps, const vector &poses, bool flush) { - printf("drawGp: lps.size=%lu\n", lps.size()); // _̊mFp + printf("drawGp: lps.size=%lu\n", lps.size()); // 点数の確認用 - // gnuplotݒ + // gnuplot設定 fprintf(gp, "set multiplot\n"); // fprintf(gp, "plot '-' w p pt 7 ps 0.1, '-' with vector\n"); fprintf(gp, "plot '-' w p pt 7 ps 0.1 lc rgb 0x0, '-' with vector\n"); - // _Q̕` - int step1=1; // _̊ԈԊuB`悪dƂ傫 + // 点群の描画 + int step1=1; // 点の間引き間隔。描画が重いとき大きくする for (size_t i=0; i &lps, const vector &poses, fprintf(gp, "e\n"); if (flush) - fflush(gp); // obt@̃f[^oBꂵȂƕ`悪悭Ȃ + fflush(gp); // バッファのデータを書き出す。これしないと描画がよくない } diff --git a/cui/MapDrawer.h b/cui/MapDrawer.h index 4aea023..67bb2fd 100755 --- a/cui/MapDrawer.h +++ b/cui/MapDrawer.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -23,12 +23,12 @@ class MapDrawer { private: - FILE *gp; // gnuplotւ̃pCv - double xmin; // `͈[m] + FILE *gp; // gnuplotへのパイプ + double xmin; // 描画範囲[m] double xmax; double ymin; double ymax; - double aspectR; // xy + double aspectR; // xy比 public: MapDrawer() : gp(nullptr), xmin(-10), xmax(10), ymin(-10), ymax(10), aspectR(-1.0) { @@ -42,9 +42,9 @@ class MapDrawer void initGnuplot() { #ifdef _WIN32 - gp = _popen("gnuplot", "w"); // pCvI[v.Windows + gp = _popen("gnuplot", "w"); // パイプオープン.Windows #elif __linux__ - gp = popen("gnuplot", "w"); // pCvI[v.Linux + gp = popen("gnuplot", "w"); // パイプオープン.Linux #endif } @@ -62,14 +62,14 @@ class MapDrawer fprintf(gp, "set size ratio %lf\n", aspectR); } - void setRange(double R) { // `͈͂Rlɂ + void setRange(double R) { // 描画範囲をR四方にする xmin = ymin = -R; xmax = ymax = R; fprintf(gp, "set xrange [%lf:%lf]\n", xmin, xmax); fprintf(gp, "set yrange [%lf:%lf]\n", ymin, ymax); } - void setRange(double xR, double yR) { // `͈͂}xRA}yRɂ + void setRange(double xR, double yR) { // 描画範囲を±xR、±yRにする xmin = -xR; xmax = xR; ymin = -yR; @@ -78,7 +78,7 @@ class MapDrawer fprintf(gp, "set yrange [%lf:%lf]\n", ymin, ymax); } - void setRange(double xm, double xM, double ym, double yM) { // `͈͂Sw + void setRange(double xm, double xM, double ym, double yM) { // 描画範囲を全部指定 xmin = xm; xmax = xM; ymin = ym; diff --git a/cui/SlamLauncher.cpp b/cui/SlamLauncher.cpp index 4fc0f01..5fdc22f 100755 --- a/cui/SlamLauncher.cpp +++ b/cui/SlamLauncher.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -17,24 +17,24 @@ #include "SlamLauncher.h" #include "ScanPointResampler.h" -using namespace std; // C++WCu̖OԂg +using namespace std; // C++標準ライブラリの名前空間を使う ////////// void SlamLauncher::run() { - mdrawer.initGnuplot(); // gnuplot - mdrawer.setAspectRatio(-0.9); // xy̔iɂƒgj + mdrawer.initGnuplot(); // gnuplot初期化 + mdrawer.setAspectRatio(-0.9); // x軸とy軸の比（負にすると中身が一定） - size_t cnt = 0; // ̘_ + size_t cnt = 0; // 処理の論理時刻 if (startN > 0) - skipData(startN); // startN܂Ńf[^ǂݔ΂ + skipData(startN); // startNまでデータを読み飛ばす double totalTime=0, totalTimeDraw=0, totalTimeRead=0; Scan2D scan; - bool eof = sreader.loadScan(cnt, scan); // t@CXL1“ǂݍ + bool eof = sreader.loadScan(cnt, scan); // ファイルからスキャンを1個読み込む boost::timer tim; while(!eof) { - if (odometryOnly) { // Ihgɂn}\ziSLAMDj + if (odometryOnly) { // オドメトリによる地図構築（SLAMより優先） if (cnt == 0) { ipose = scan.pose; ipose.calRmat(); @@ -42,22 +42,22 @@ void SlamLauncher::run() { mapByOdometry(&scan); } else - sfront.process(scan); // SLAMɂn}\z + sfront.process(scan); // SLAMによる地図構築 double t1 = 1000*tim.elapsed(); - if (cnt%drawSkip == 0) { // drawSkipɌʂ` + if (cnt%drawSkip == 0) { // drawSkipおきに結果を描画 mdrawer.drawMapGp(*pcmap); } double t2 = 1000*tim.elapsed(); - ++cnt; // _XV - eof = sreader.loadScan(cnt, scan); // ̃XLǂݍ + ++cnt; // 論理時刻更新 + eof = sreader.loadScan(cnt, scan); // 次のスキャンを読み込む double t3 = 1000*tim.elapsed(); - totalTime = t3; // S̏ - totalTimeDraw += (t2-t1); // `掞Ԃ̍v - totalTimeRead += (t3-t2); // [hԂ̍v + totalTime = t3; // 全体処理時間 + totalTimeDraw += (t2-t1); // 描画時間の合計 + totalTimeRead += (t3-t2); // ロード時間の合計 printf("---- SlamLauncher: cnt=%lu ends ----\n", cnt); } @@ -66,41 +66,41 @@ void SlamLauncher::run() { printf("Elapsed time: mapping=%g, drawing=%g, reading=%g\n", (totalTime-totalTimeDraw-totalTimeRead), totalTimeDraw, totalTimeRead); printf("SlamLauncher finished.\n"); - // I`ʂc߂sleepŖ[vɂBctrl-CŏIB + // 処理終了後も描画画面を残すためにsleepで無限ループにする。ctrl-Cで終了。 while(true) { #ifdef _WIN32 - Sleep(1000); // WindowsłSleep + Sleep(1000); // WindowsではSleep #elif __linux__ - usleep(1000000); // Linuxłusleep + usleep(1000000); // Linuxではusleep #endif } } -// Jnnum‚̃XL܂œǂݔ΂ +// 開始からnum個のスキャンまで読み飛ばす void SlamLauncher::skipData(int num) { Scan2D scan; bool eof = sreader.loadScan(0, scan); - for (int i=0; !eof && ipose; // XL擾̃Ihgʒu +// Pose2D &pose = scan->pose; // スキャン取得時のオドメトリ位置 Pose2D pose; Pose2D::calRelativePose(scan->pose, ipose, pose); - vector &lps = scan->lps; // XL_Q - vector glps; // n}Wnł̓_Q + vector &lps = scan->lps; // スキャン点群 + vector glps; // 地図座標系での点群 for (size_t j=0; jaddPose(pose); pcmap->addPoints(glps); pcmap->makeGlobalMap(); @@ -108,32 +108,32 @@ void SlamLauncher::mapByOdometry(Scan2D *scan) { printf("Odom pose: tx=%g, ty=%g, th=%g\n", pose.tx, pose.ty, pose.th); } -////////// XL` //////// +////////// スキャン描画 //////// void SlamLauncher::showScans() { mdrawer.initGnuplot(); - mdrawer.setRange(6); // `͈́BXL6ml̏ꍇ - mdrawer.setAspectRatio(-0.9); // xy̔iɂƒgj + mdrawer.setRange(6); // 描画範囲。スキャンが6m四方の場合 + mdrawer.setAspectRatio(-0.9); // x軸とy軸の比（負にすると中身が一定） ScanPointResampler spres; - size_t cnt = 0; // ̘_ + size_t cnt = 0; // 処理の論理時刻 if (startN > 0) - skipData(startN); // startN܂Ńf[^ǂݔ΂ + skipData(startN); // startNまでデータを読み飛ばす Scan2D scan; bool eof = sreader.loadScan(cnt, scan); while(!eof) { -// spres.resamplePoints(&scan); // RgAEg͂΁AXL_ԊuψɂB +// spres.resamplePoints(&scan); // コメントアウトはずせば、スキャン点間隔を均一にする。 - // `Ԋu + // 描画間隔をあける #ifdef _WIN32 - Sleep(100); // WindowsłSleep + Sleep(100); // WindowsではSleep #elif __linux__ - usleep(100000); // Linuxłusleep + usleep(100000); // Linuxではusleep #endif - mdrawer.drawScanGp(scan); // XL` + mdrawer.drawScanGp(scan); // スキャン描画 printf("---- scan num=%lu ----\n", cnt); eof = sreader.loadScan(cnt, scan); @@ -143,10 +143,10 @@ void SlamLauncher::showScans() { printf("SlamLauncher finished.\n"); } -//////// XLǂݍ ///////// +//////// スキャン読み込み ///////// bool SlamLauncher::setFilename(char *filename) { - bool flag = sreader.openScanFile(filename); // t@CI[v + bool flag = sreader.openScanFile(filename); // ファイルをオープン return(flag); } @@ -156,9 +156,9 @@ bool SlamLauncher::setFilename(char *filename) { void SlamLauncher::customizeFramework() { fcustom.setSlamFrontEnd(&sfront); fcustom.makeFramework(); -// fcustom.customizeG(); // މ̑ΏȂ -// fcustom.customizeH(); // މ̑Ώ - fcustom.customizeI(); // [v‚݂ +// fcustom.customizeG(); // 退化の対処をしない +// fcustom.customizeH(); // 退化の対処をする + fcustom.customizeI(); // ループ閉じ込みをする - pcmap = fcustom.getPointCloudMap(); // customizěɂ邱 + pcmap = fcustom.getPointCloudMap(); // customizeの後にやること } diff --git a/cui/SlamLauncher.h b/cui/SlamLauncher.h index 7d4b5a8..afc35c1 100755 --- a/cui/SlamLauncher.h +++ b/cui/SlamLauncher.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -34,18 +34,18 @@ class SlamLauncher { private: - int startN; // JnXLԍ - int drawSkip; // `Ԋu - bool odometryOnly; // Ihgɂn}\z - Pose2D ipose; // Ihgn}\z̕⏕f[^Bʒůpx0ɂ + int startN; // 開始スキャン番号 + int drawSkip; // 描画間隔 + bool odometryOnly; // オドメトリによる地図構築か + Pose2D ipose; // オドメトリ地図構築の補助データ。初期位置の角度を0にする - Pose2D lidarOffset; // [UXLiƃ{bg̑Έʒu + Pose2D lidarOffset; // レーザスキャナとロボットの相対位置 - SensorDataReader sreader; // t@C̃ZTf[^ǂݍ - PointCloudMap *pcmap; // _Qn} - SlamFrontEnd sfront; // SLAMtgGh - MapDrawer mdrawer; // gnuplotɂ` - FrameworkCustomizer fcustom; // t[[N̉ + SensorDataReader sreader; // ファイルからのセンサデータ読み込み + PointCloudMap *pcmap; // 点群地図 + SlamFrontEnd sfront; // SLAMフロントエンド + MapDrawer mdrawer; // gnuplotによる描画 + FrameworkCustomizer fcustom; // フレームワークの改造 public: SlamLauncher() : startN(0), drawSkip(10), odometryOnly(false), pcmap(nullptr) { diff --git a/cui/main.cpp b/cui/main.cpp index eb00fc4..778c59a 100755 --- a/cui/main.cpp +++ b/cui/main.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -15,27 +15,27 @@ #include "SlamLauncher.h" int main(int argc, char *argv[]) { - bool scanCheck=false; // XL\݂̂ - bool odometryOnly=false; // Ihgɂn}\z - char *filename; // f[^t@C - int startN=0; // JnXLԍ + bool scanCheck=false; // スキャン表示のみか + bool odometryOnly=false; // オドメトリによる地図構築か + char *filename; // データファイル名 + int startN=0; // 開始スキャン番号 if (argc < 2) { printf("Error: too few arguments.\n"); return(1); } - // R}h̏ + // コマンド引数の処理 int idx=1; - // R}hIvV̉߁i'-'̂‚j + // コマンドオプションの解釈（'-'のついた引数） if (argv[1][0] == '-') { for (int i=1; ; i++) { char option = argv[1][i]; if (option == NULL) break; - else if (option == 's') // XL\̂ + else if (option == 's') // スキャン表示のみ scanCheck = true; - else if (option == 'o') // Ihgɂn}\z + else if (option == 'o') // オドメトリによる地図構築 odometryOnly = true; } if (argc == 2) { @@ -44,9 +44,9 @@ int main(int argc, char *argv[]) { } ++idx; } - if (argc >= idx+1) // '-'ꍇidx=2AȂꍇidx=1 + if (argc >= idx+1) // '-'ある場合idx=2、ない場合idx=1 filename = argv[idx]; - if (argc == idx+2) // argcidx2傫startN + if (argc == idx+2) // argcがidxより2大きければstartNがある startN = atoi(argv[idx+1]); else if (argc >= idx+2) { printf("Error: invalid arguments.\n"); @@ -56,18 +56,18 @@ int main(int argc, char *argv[]) { printf("SlamLauncher: startN=%d, scanCheck=%d, odometryOnly=%d\n", startN, scanCheck, odometryOnly); printf("filename=%s\n", filename); - // t@CJ + // ファイルを開く SlamLauncher sl; bool flag = sl.setFilename(filename); if (!flag) return(1); - sl.setStartN(startN); // JnXLԍ̐ݒ + sl.setStartN(startN); // 開始スキャン番号の設定 - // { + // 処理本体 if (scanCheck) sl.showScans(); - else { // XL\ȊOSlamLauncherŏꍇ + else { // スキャン表示以外はSlamLauncher内で場合分け sl.setOdometryOnly(odometryOnly); sl.customizeFramework(); sl.run(); diff --git a/doc/customize.md b/doc/customize.md index ecd032d..babb0c6 100755 --- a/doc/customize.md +++ b/doc/customize.md @@ -1,46 +1,46 @@ -## vÕJX^}CY +## プログラムのカスタマイズ -LittleSLAḾA傫AXL}b`OAZTZA[v‚݂Ƃ -vfZp\Ă܂B -LittleSLAḾAwKpvOƂāA̋ZpɊւ -‚̃JX^}CYł悤ɍĂ܂B -\ɃJX^}CỸ^Cv܂B -ꂼ̏ڍׂ́AQl[1]QƂĂB +LittleSLAMは、大きく、スキャンマッチング、センサ融合、ループ閉じ込みという +要素技術から構成されています。 +LittleSLAMは、学習用プログラムとして、これらの技術に関して +いくつかのカスタマイズができるように作られています。 +下表にカスタマイズのタイプを示します。 +それぞれの詳細は、参考書籍[1]を参照してください。 -| ^Cv | e | +| タイプ | 内容 | |:--------------------|:-------------| -| customizeA | XL}b`O{` | -| customizeB | XL}b`Onj`1 | -| customizeC | XL}b`Onj`2 | -| customizeD | XL}b`Onj`3 | -| customizeE | XL}b`Onj`4 | -| customizeF | XL}b`Onj`5 | -| customizeG | XL}b`Onj`6 | -| customizeH | ZTZɂމ̑Ώ | -| customizeI | [v‚ | - - -JX^}CỸ^Cv́ASlamLauncher.cpp -֐customizeFramework̒ŁAL̂悤Ɏw肵܂B -\̃^Cv̂܂܊֐łAw肵֐āA -ȊO̓RgAEg܂B -ftHǵAcustomizeIɂȂĂ܂B +| customizeA | スキャンマッチング基本形 | +| customizeB | スキャンマッチング改良形1 | +| customizeC | スキャンマッチング改良形2 | +| customizeD | スキャンマッチング改良形3 | +| customizeE | スキャンマッチング改良形4 | +| customizeF | スキャンマッチング改良形5 | +| customizeG | スキャンマッチング改良形6 | +| customizeH | センサ融合による退化の対処 | +| customizeI | ループ閉じ込み | + + +カスタマイズのタイプは、SlamLauncher.cppの +関数customizeFrameworkの中で、下記のように指定します。 +表のタイプがそのまま関数名であり、指定したい関数を書いて、 +それ以外はコメントアウトします。 +デフォルトは、customizeIになっています。 ```C++ void SlamLauncher::customizeFramework() { fcustom.setSlamFrontEnd(&sfront); fcustom.makeFramework(); -// fcustom.customizeG(); // gȂ̂ŃRgAEg - fcustom.customizeI(); // ̃JX^}CYw +// fcustom.customizeG(); // 使わないのでコメントアウト + fcustom.customizeI(); // このカスタマイズを指定 pcmap = fcustom.getPointCloudMap(); } ``` -܂A֐customizeXiX=A to Ij́Acui/FrameworkCustomizer.cppŒ`Ă܂B -[UVcustomizeXĎƂ”\łB +また、関数customizeX（X=A to I）は、cui/FrameworkCustomizer.cppで定義されています。 +ユーザが新しいcustomizeXを作って試すことも可能です。 diff --git a/doc/install-linux.md b/doc/install-linux.md index a9dead1..c2377a2 100755 --- a/doc/install-linux.md +++ b/doc/install-linux.md @@ -1,45 +1,45 @@ -## LittleSLAM̎giLinux̏ꍇj +## LittleSLAMの使い方（Linuxの場合） -### (1) ֘A\tgEFÃCXg[ +### (1) 関連ソフトウェアのインストール -- C++RpC(gcc)ABoostAEigenACMake, gnuplot -ȉ̃R}hŁA܂Ƃ߂ăCXg[܂B +- C++コンパイラ(gcc)、Boost、Eigen、CMake, gnuplot +以下のコマンドで、まとめてインストールします。 
 $ sudo apt-get install build-essential cmake libboost-all-dev libeigen3-dev gnuplot gnuplot-x11
- p2o -[p2o](https://github.com/furo-org/p2o)GithubTCgJ܂Bȉ̂ǂ炩̕@p2o_E[h܂B -(A) Githubʂ"Clone or download"{^āA"Download ZIP"IA -p2o-master.zip_E[h܂Bzipt@C̓WJ@͌q܂B -(B) gitgāA|Wgclone܂B +[p2o](https://github.com/furo-org/p2o)のGithubサイトを開きます。以下のどちらかの方法でp2oをダウンロードします。 +(A) Github画面の"Clone or download"ボタンを押して、"Download ZIP"を選択し、 +p2o-master.zipをダウンロードします。zipファイルの展開方法は後述します。 +(B) gitを使って、リポジトリをcloneします。 -### (2) LittleSLAM̃CXg[ +### (2) LittleSLAMのインストール -- LittleSLAM̓WJ -[LittleSLAM](https://github.com/furo-org/LittleSLAM)GithubTCgJ܂B -ȉ̂ǂ炩̕@LittleSLAM_E[h܂B -(A) Githubʂ"Clone or download"{^āA"Download ZIP"IA -LittleSLAM-master.zip_E[h܂B -āAzipt@CKȃfBNgɓWJ܂B -ł́AƂ΁A"\~/LittleSLAM"ɓWJƂ܂B -LittleSLAM-master.zip̒"LittleSLAM-master"fBNg̉ -4‚̃fBNg3‚̃t@C"\~/LittleSLAM"̉ɃRs[܂B -(B) gitgāA|Wgclone܂B +- LittleSLAMの展開 +[LittleSLAM](https://github.com/furo-org/LittleSLAM)のGithubサイトを開きます。 +以下のどちらかの方法でLittleSLAMをダウンロードします。 +(A) Github画面の"Clone or download"ボタンを押して、"Download ZIP"を選択し、 +LittleSLAM-master.zipをダウンロードします。 +そして、このzipファイルを適当なディレクトリに展開します。 +ここでは、たとえば、"\~/LittleSLAM"に展開するとします。 +LittleSLAM-master.zipの中の"LittleSLAM-master"ディレクトリの下の +4個のディレクトリと3個のファイルを"\~/LittleSLAM"の下にコピーします。 +(B) gitを使って、リポジトリをcloneします。 -- p2o̓WJ -"\~/LittleSLAM"̉p2ofBNg쐬܂B -Oqp2o-master.zip̒̃t@C"p2o.h""\~/LittleSLAM/p2o"ɃRs[܂B +- p2oの展開 +"\~/LittleSLAM"の下にp2oディレクトリを作成します。 +前述のp2o-master.zipの中のファイル"p2o.h"を"\~/LittleSLAM/p2o"にコピーします。 -- buildfBNg̍쐬 -"\~/LittleSLAM"̉buildfBNg쐬܂B -܂ł̃fBNg\͈ȉ̂悤ɂȂ܂B +- buildディレクトリの作成 +"\~/LittleSLAM"の下にbuildディレクトリを作成します。 +ここまでのディレクトリ構成は以下のようになります。 -![fBNg\](images/folders-lnx.png) +![ディレクトリ構成](images/folders-lnx.png) -- CMake̎s -R\[ŁAbuildfBNgɈړAcmakes܂B +- CMakeの実行 +コンソールで、buildディレクトリに移動し、cmakeを実行します。 
 ~/LittleSLAM$ cd build
@@ -47,50 +47,50 @@ LittleSLAM-master.zip
 
 ~/LittleSLAM/build$ cmake ..
 

 
-}cmake̎s܂B
+下図にcmakeの実行例を示します。
 
 ![cmake](images/cmake-lnx.png)
 
-邢́ACMakeGUIłCXg[āAWindows̏ꍇƓ悤
-GUICMakes邱Ƃł܂B
+あるいは、CMakeのGUI版をインストールして、Windowsの場合と同じように
+GUIでCMakeを実行することもできます。
 
-- rh  
-R\[ŁAbuildfBNgɂmakes܂B  
+- ビルド  
+コンソールで、buildディレクトリにおいてmakeを実行します。  
 
 ~/LittleSLAM/build$ make
-rhƁA"\~/LittleSLAM/build/cui"fBNgɁAst@CLittleSLAM܂B +ビルドが成功すると、"\~/LittleSLAM/build/cui"ディレクトリに、実行ファイルLittleSLAMが生成されます。 ![cmake](images/exefile-lnx.png) -### (3) s +### (3) 実行 -ȉ̃R}hŁALittleSLAMs܂B +以下のコマンドで、LittleSLAMを実行します。 -
 ./LittleSLAM [-so] f[^t@C [JnXLԍ]
+
 ./LittleSLAM [-so] データファイル名 [開始スキャン番号]
 

 
--sIvVw肷ƁAXL1‚•`悵܂BeXL`mFꍇ
-g܂B  
--oIvVw肷ƁAXLIhgf[^ŕׂn}
-iSLAMɂn}ł͂Ȃj𐶐܂B  
-IvVw肪Ȃ΁ASLAMs܂B  
-JnXLԍw肷ƁA̔ԍ܂ŃXLǂݔ΂Ăs܂B
+-sオプションを指定すると、スキャンを1個ずつ描画します。各スキャン形状を確認したい場合に
+使います。  
+-oオプションを指定すると、スキャンをオドメトリデータで並べた地図
+（SLAMによる地図ではない）を生成します。  
+オプション指定がなければ、SLAMを実行します。  
+開始スキャン番号を指定すると、その番号までスキャンを読み飛ばしてから実行します。
 
-ƂāAȉ̃R}hSLAMs܂B  
-̗ł"\~/LittleSLAM/dataset"fBNg"corridor.lsc"Ƃf[^t@CuĂ܂B  
+例として、以下のコマンドでSLAMを実行します。  
+この例では"\~/LittleSLAM/dataset"ディレクトリに"corridor.lsc"というデータファイルが置かれています。  
 
 ~/LittleSLAM/build/cui$ ./LittleSLAM ~/LittleSLAM/dataset/corridor.lsc
![cmake](images/command-lnx.png) -R}hsƁALittleSLAM̓t@Cf[^ǂݍŒn} -\zĂ܂B̗lqgnuplotɕ`悳܂B -ŏIIɁA}̂悤Ȓn}܂B -IĂAvO͏IAn}͂̂܂ܕ\Ă܂B -vOIɂCtrl-CĂB +コマンドを実行すると、LittleSLAMはファイルからデータを読み込んで地図を少しずつ +構築していきます。その様子がgnuplotに描画されます。 +最終的に、下図のような地図が生成されます。 +処理が終わっても、プログラムは終了せず、地図はそのまま表示されています。 +プログラムを終了するにはCtrl-Cを押してください。 -@ +　 ![cmake](images/result-lnx.png) diff --git a/doc/install-win.md b/doc/install-win.md index dd16ba6..3812e7b 100755 --- a/doc/install-win.md +++ b/doc/install-win.md @@ -1,129 +1,129 @@ -## LittleSLAM̎g iWindows̏ꍇj +## LittleSLAMの使い方 （Windowsの場合） -### (1) ֘A\tgEFÃCXg[ +### (1) 関連ソフトウェアのインストール - Boost -[Boost](http://www.boost.org/)_E[hāAKȃtH_ɉ𓀂܂B -LiitleSLAMł́ABoost̃wb_t@Cgp̂ŁArh͕Kv܂B +[Boost](http://www.boost.org/)をダウンロードして、適当なフォルダに解凍します。 +LiitleSLAMでは、Boostのヘッダファイルだけを使用するので、ビルドは必要ありません。 - Eigen3 -[Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page) -_E[hāAKȃtH_ɉ𓀂܂B -Eigen̓wb_t@CŎgp郉CuȂ̂ŁArh͕Kv܂B +[Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page)を +ダウンロードして、適当なフォルダに解凍します。 +Eigenはヘッダファイルだけで使用するライブラリなので、ビルドは必要ありません。 - gnuplot -[gnuplot](http://www.gnuplot.info/)_E[hăCXg[܂B -LittleSLAḾAAPIł͂ȂAsR}hŌĂяôŁA -Windows̊‹ϐPathgnuplot̃pXݒ肵Ă܂B -Ƃ΁AgnuplottH_C:\gnuplotɃCXg[ꍇA"Path=... ;C:\gnuplot\bin; ..."Ƃ܂B -iCXg[Őݒ肵Ă邱Ƃ܂j +[gnuplot](http://www.gnuplot.info/)をダウンロードしてインストールします。 +LittleSLAMからは、APIではなく、実行コマンドで呼び出すので、 +Windowsの環境変数Pathにgnuplotのパスを設定しておきます。 +たとえば、gnuplotをフォルダC:\gnuplotにインストールした場合、"Path=... ;C:\gnuplot\bin; ..."とします。 +（インストーラが自動で設定してくれることもあります） - CMake -[CMake](https://cmake.org/)_E[hăCXg[܂B +[CMake](https://cmake.org/)をダウンロードしてインストールします。 - p2o -[p2o](https://github.com/furo-org/p2o)GithubTCgJ܂Bȉ̂ǂ炩̕@p2o_E[h܂B -(A) Githubʂ"Clone or download"{^āA"Download ZIP"IA -p2o-master.zip_E[h܂Bzipt@C̓WJ@͌q܂B -(B) gitgāA|Wgclone܂B - -### (2) LittleSLAM̃CXg[ - -- LittleSLAM̓WJ -[LittleSLAM](https://github.com/furo-org/LittleSLAM)GithubTCgJ܂B -ȉ̂ǂ炩̕@LittleSLAM_E[h܂B -(A) Githubʂ"Clone or download"{^āA"Download ZIP"IA -LittleSLAM-master.zip_E[h܂B -āAzipt@CKȃtH_ɓWJ܂B -ł́AƂ΁A"C:\abc\LittleSLAM"ɓWJƂ܂B -"abc"̓[U߂Cӂ̃tH_łB -LittleSLAM-master.zip̒"LittleSLAM-master"tH_̉ -4‚̃tH_3‚̃t@C"C:\abc\LittleSLAM"̉ɃRs[܂B -(B) gitgāA|Wgclone܂B - -- p2o̓WJ -"C:\abc\LittleSLAM"̉"p2o"tH_쐬܂B -Oqp2o-master.zip̒̃t@C"p2o.h""C:\abc\LittleSLAM\p2o"̉ɃRs[܂B - -- buildtH_̍쐬 -"C:\abc\LittleSLAM"̉buildtH_쐬܂B -܂ł̃tH_\͈ȉ̂悤ɂȂ܂B - -![tH_\](images/folders.png) - -- CMake̎s -CMake(GUI)sāALittleSLAM.sln𐶐܂B -܂A"Where is the source code""Where to buid the binaries"ɉ}̃tH_w肵܂B -ɁA Configure{^܂B -LittleSLAMɑ΂ď߂CMakesꍇA}̂悤C++RpC𕷂̂ŁA -gpĂC++RpCw肵A"Use default native compliers"IāAFinish{^܂B -āAxAConfigure{^AŌGenerate{^܂B +[p2o](https://github.com/furo-org/p2o)のGithubサイトを開きます。以下のどちらかの方法でp2oをダウンロードします。 +(A) Github画面の"Clone or download"ボタンを押して、"Download ZIP"を選択し、 +p2o-master.zipをダウンロードします。zipファイルの展開方法は後述します。 +(B) gitを使って、リポジトリをcloneします。 + +### (2) LittleSLAMのインストール + +- LittleSLAMの展開 +[LittleSLAM](https://github.com/furo-org/LittleSLAM)のGithubサイトを開きます。 +以下のどちらかの方法でLittleSLAMをダウンロードします。 +(A) Github画面の"Clone or download"ボタンを押して、"Download ZIP"を選択し、 +LittleSLAM-master.zipをダウンロードします。 +そして、このzipファイルを適当なフォルダに展開します。 +ここでは、たとえば、"C:\abc\LittleSLAM"に展開するとします。 +"abc"はユーザが決める任意のフォルダです。 +LittleSLAM-master.zipの中の"LittleSLAM-master"フォルダの下の +4個のフォルダと3個のファイルを"C:\abc\LittleSLAM"の下にコピーします。 +(B) gitを使って、リポジトリをcloneします。 + +- p2oの展開 +"C:\abc\LittleSLAM"の下に"p2o"フォルダを作成します。 +前述のp2o-master.zipの中のファイル"p2o.h"を"C:\abc\LittleSLAM\p2o"の下にコピーします。 + +- buildフォルダの作成 +"C:\abc\LittleSLAM"の下にbuildフォルダを作成します。 +ここまでのフォルダ構成は以下のようになります。 + +![フォルダ構成](images/folders.png) + +- CMakeの実行 +CMake(GUI)を実行して、LittleSLAM.slnを生成します。 +まず、"Where is the source code"欄および"Where to buid the binaries"欄に下図のフォルダを指定します。 +次に、 Configureボタンを押します。 +LittleSLAMに対して初めてCMakeを実行する場合、下図のようにC++コンパイラを聞かれるので、 +使用しているC++コンパイラを指定し、"Use default native compliers"を選択して、Finishボタンを押します。 +そして、もう一度、Configureボタンを押し、最後にGenerateボタンを押します。 ![cmake](images/cmake.png) -- Eigen3̎w -ACMakeEigen3̏ꏊiEIGEN3_INCLUDE_DIRj‚ꂸɃG[oꍇ́A -̂ꂩsāACMakeċNConfigureGenerate蒼ĂB -(A) Windows̃VXe‹ϐEIGEN3_ROOT_DIRljāA -Eigen3WJtH_ݒ肵܂B -ƁA}̂悤ɁA"C:\abc\LittleSLAM"cui, framework, hook̊eCMakeLists.txt̒ŁA -EIGEN3_ROOT_DIRŎw肳ꂽtH_EIGEN3_INCLUDE_DIRɐݒ肳܂B -(B) eCMakeLists.txtEigen3̃tH_Őݒ肵܂B -Ƃ΁AEigen3"C:\eigen"ɓWJꍇ́A} -$ENV{EIGEN3_ROOT_DIR}̕C:\eigenɏ܂B +- Eigen3の指定 +もし、CMakeがEigen3の場所（EIGEN3_INCLUDE_DIR）を見つけられずにエラーが出た場合は、 +次のいずれかを行って、CMakeを再起動してConfigureとGenerateをやり直してください。 +(A) Windowsのシステム環境変数にEIGEN3_ROOT_DIRを追加して、 +そこにEigen3を展開したフォルダを設定します。 +すると、下図のように、"C:\abc\LittleSLAM"下のcui, framework, hookの各CMakeLists.txtの中で、 +EIGEN3_ROOT_DIRで指定されたフォルダがEIGEN3_INCLUDE_DIRに設定されます。 +(B) 各CMakeLists.txtのEigen3のフォルダを手で設定します。 +たとえば、Eigen3を"C:\eigen"に展開した場合は、下図の +$ENV{EIGEN3_ROOT_DIR}の部分をC:\eigenに書き換えます。 ``` --- CMakeLists.txt蔲 -- +-- CMakeLists.txtより抜粋 -- find_package(Eigen3) -IF(NOT EIGEN3_INCLUDE_DIR) # Eigen3̃pX‚Ȃ +IF(NOT EIGEN3_INCLUDE_DIR) # Eigen3のパスが見つからない set(EIGEN3_INCLUDE_DIR $ENV{EIGEN3_ROOT_DIR}) ENDIF() ``` -- Visual studiőN -"C:\abc\LittleSLAM\build"̉LittleSLAM.slnłĂ̂ŁA -_uNbNƁAVisual studioN܂B +- Visual studioの起動 +"C:\abc\LittleSLAM\build"の下にLittleSLAM.slnができているので、 +それをダブルクリックすると、Visual studioが起動します。 -- rh -}̂悤ɁAVisual studioŁARelease, x64i64rbg̏ꍇjw肵ABuildj[Build Solutions܂B +- ビルド +下図のように、Visual studioで、Release, x64（64ビットの場合）を指定し、BuildメニューからBuild Solutionを実行します。 ![cmake](images/build.png) -rhƁA"build\cui\Release"tH_ɁAst@CLittleSLAM.exe܂B +ビルドが成功すると、"build\cui\Release"フォルダに、実行ファイルLittleSLAM.exeが生成されます。 ![cmake](images/exefile.png) -### (3) s +### (3) 実行 -WindowsR}hvvgȉ̃R}hɂALittleSLAMs܂B +Windowsコマンドプロンプトから以下のコマンドにより、LittleSLAMを実行します。 -
 LittleSLAM [-so] f[^t@C [JnXLԍ]
+
 LittleSLAM [-so] データファイル名 [開始スキャン番号]
 

 
--sIvVw肷ƁAXL1‚•`悵܂BeXL`mFꍇ
-g܂B  
--oIvVw肷ƁAXLIhgf[^ŕׂn}
-iSLAMɂn}ł͂Ȃj𐶐܂B  
-IvVw肪Ȃ΁ASLAMs܂B  
-JnXLԍw肷ƁA̔ԍ܂ŃXLǂݔ΂Ăs܂B
+-sオプションを指定すると、スキャンを1個ずつ描画します。各スキャン形状を確認したい場合に
+使います。  
+-oオプションを指定すると、スキャンをオドメトリデータで並べた地図
+（SLAMによる地図ではない）を生成します。  
+オプション指定がなければ、SLAMを実行します。  
+開始スキャン番号を指定すると、その番号までスキャンを読み飛ばしてから実行します。
 
-ƂāAȉ̃R}hSLAMs܂B  
-̗ł"C:\abc\dataset"tH_"corridor.lsc"Ƃf[^t@CuĂ܂B  
+例として、以下のコマンドでSLAMを実行します。  
+この例では"C:\abc\dataset"フォルダに"corridor.lsc"というデータファイルが置かれています。  
 
 C:\abc\LittleSLAM\build\cui\Release> LittleSLAM C:\abc\dataset\corridor.lsc
![cmake](images/command.png) -R}hsƁALittleSLAM̓t@Cf[^ǂݍŒn} -\zĂ܂B̗lqgnuplotɕ`悳܂B -ŏIIɁA}̂悤Ȓn}܂B -IĂAvO͏IAn}͂̂܂ܕ\Ă܂B -vOIɂCtrl-CĂB +コマンドを実行すると、LittleSLAMはファイルからデータを読み込んで地図を少しずつ +構築していきます。その様子がgnuplotに描画されます。 +最終的に、下図のような地図が生成されます。 +処理が終わっても、プログラムは終了せず、地図はそのまま表示されています。 +プログラムを終了するにはCtrl-Cを押してください。 ![cmake](images/result.png) diff --git a/framework/CostFunction.h b/framework/CostFunction.h index fa3a79d..13a4962 100755 --- a/framework/CostFunction.h +++ b/framework/CostFunction.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -24,10 +24,10 @@ class CostFunction { protected: - std::vector curLps; // ΉƂꂽ݃XL̓_Q - std::vector refLps; // ΉƂꂽQƃXL̓_Q - double evlimit; // }b`OőΉƂꂽƌȂ臒l - double pnrate; // 덷evlimitȓőΉƂꂽ_̔䗦 + std::vector curLps; // 対応がとれた現在スキャンの点群 + std::vector refLps; // 対応がとれた参照スキャンの点群 + double evlimit; // マッチングで対応がとれたと見なす距離閾値 + double pnrate; // 誤差がevlimit以内で対応がとれた点の比率 public: CostFunction() : evlimit(0), pnrate(0) { @@ -42,7 +42,7 @@ class CostFunction evlimit = e; } - // DataAssociatorőΉ̂Ƃꂽ_Qcur, refݒ + // DataAssociatorで対応のとれた点群cur, refを設定 void setPoints(std::vector &cur, std::vector &ref) { curLps = cur; refLps = ref; diff --git a/framework/CovarianceCalculator.cpp b/framework/CovarianceCalculator.cpp index 217120b..03dd6ad 100755 --- a/framework/CovarianceCalculator.cpp +++ b/framework/CovarianceCalculator.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -16,38 +16,38 @@ using namespace std; -////////// ICPɂ鐄l̋U ///////// +////////// ICPによる推定値の共分散 ///////// -// ICPɂ郍{bgʒu̐l̋Ucov߂B -// ʒuposeA݃XL_QcurLpsAQƃXL_QrefLps +// ICPによるロボット位置の推定値の共分散covを求める。 +// 推定位置pose、現在スキャン点群curLps、参照スキャン点群refLps double CovarianceCalculator::calIcpCovariance(const Pose2D &pose, std::vector &curLps, std::vector &refLps, Eigen::Matrix3d &cov) { double tx = pose.tx; double ty = pose.ty; double th = pose.th; double a = DEG2RAD(th); - vector Jx; // Rrsx̗ - vector Jy; // Rrsy̗ - vector Jt; // Rrsth̗ + vector Jx; // ヤコビ行列のxの列 + vector Jy; // ヤコビ行列のyの列 + vector Jt; // ヤコビ行列のthの列 for (size_t i=0; itype == ISOLATE) // Ǘ_͏O + if (rlp->type == ISOLATE) // 孤立点は除外 continue; - double pd0 = calPDistance(clp, rlp, tx, ty, a); // RXg֐l - double pdx = calPDistance(clp, rlp, tx+dd, ty, a); // xςRXg֐l - double pdy = calPDistance(clp, rlp, tx, ty+dd, a); // yςRXg֐l - double pdt = calPDistance(clp, rlp, tx, ty, a+da); // thςRXg֐l + double pd0 = calPDistance(clp, rlp, tx, ty, a); // コスト関数値 + double pdx = calPDistance(clp, rlp, tx+dd, ty, a); // xを少し変えたコスト関数値 + double pdy = calPDistance(clp, rlp, tx, ty+dd, a); // yを少し変えたコスト関数値 + double pdt = calPDistance(clp, rlp, tx, ty, a+da); // thを少し変えたコスト関数値 - Jx.push_back((pdx - pd0)/dd); // Δixj - Jy.push_back((pdy - pd0)/dd); // Δiyj - Jt.push_back((pdt - pd0)/da); // Δithj + Jx.push_back((pdx - pd0)/dd); // 偏微分（x成分） + Jy.push_back((pdy - pd0)/dd); // 偏微分（y成分） + Jt.push_back((pdt - pd0)/da); // 偏微分（th成分） } - // wbZs̋ߎJ^TJ̌vZ - Eigen::Matrix3d hes = Eigen::Matrix3d::Zero(3,3); // ߎwbZsB0ŏ + // ヘッセ行列の近似J^TJの計算 + Eigen::Matrix3d hes = Eigen::Matrix3d::Zero(3,3); // 近似ヘッセ行列。0で初期化 for (size_t i=0; ix - sin(th)*clp->y + tx; // clp𐄒ʒuōWϊ + double x = cos(th)*clp->x - sin(th)*clp->y + tx; // clpを推定位置で座標変換 double y = sin(th)*clp->x + cos(th)*clp->y + ty; - double pdis = (x - rlp->x)*rlp->nx + (y - rlp->y)*rlp->ny; // Wϊ_rlpւ̐ + double pdis = (x - rlp->x)*rlp->nx + (y - rlp->y)*rlp->ny; // 座標変換した点からrlpへの垂直距離 return(pdis); } -///////// ^f̌vZ ///////// +///////// 運動モデルの計算 ///////// void CovarianceCalculator::calMotionCovarianceSimple(const Pose2D &motion, double dT, Eigen::Matrix3d &cov) { - double dis = sqrt(motion.tx*motion.tx + motion.ty*motion.ty); // ړ - double vt = dis/dT; // ix[m/s] - double wt = DEG2RAD(motion.th)/dT; // px[rad/s] - double vthre = 0.02; // vt̉lB0ɂȂꍇ̑Ώ - double wthre = 0.05; // wt̉l + double dis = sqrt(motion.tx*motion.tx + motion.ty*motion.ty); // 移動距離 + double vt = dis/dT; // 並進速度[m/s] + double wt = DEG2RAD(motion.th)/dT; // 角速度[rad/s] + double vthre = 0.02; // vtの下限値。同期ずれで0になる場合の対処 + double wthre = 0.05; // wtの下限値 if (vt < vthre) vt = vthre; @@ -104,18 +104,18 @@ void CovarianceCalculator::calMotionCovarianceSimple(const Pose2D &motion, doubl double da = wt; Eigen::Matrix3d C1; - C1.setZero(); // Ίpvf - C1(0,0) = 0.001*dx*dx; // ix - C1(1,1) = 0.005*dy*dy; // iy -// C1(2,2) = 0.005*da*da; // ] - C1(2,2) = 0.05*da*da; // ] - - // XP[ -// double kk = 100; // Ihĝꂪ傫ꍇ - double kk = 1; // ʏ + C1.setZero(); // 対角要素だけ入れる + C1(0,0) = 0.001*dx*dx; // 並進成分x + C1(1,1) = 0.005*dy*dy; // 並進成分y +// C1(2,2) = 0.005*da*da; // 回転成分 + C1(2,2) = 0.05*da*da; // 回転成分 + + // スケール調整 +// double kk = 100; // オドメトリのずれが大きい場合 + double kk = 1; // 通常 cov = kk*C1; - // mFp + // 確認用 printf("calMotionCovarianceSimple\n"); printf("vt=%g, wt=%g\n", vt, wt); double vals[2], vec1[2], vec2[2]; @@ -123,22 +123,22 @@ void CovarianceCalculator::calMotionCovarianceSimple(const Pose2D &motion, doubl printf("cov : %g %g %g %g %g %g\n", cov(0,0), cov(0,1), cov(0,2), cov(1,1), cov(1,2), cov(2,2)); } -///////// ^f̌vZ ///////// +///////// 運動モデルの計算 ///////// -// 1t[̑sɂ덷BdT1t[̎ԁBmotion͂̊Ԃ̈ړʁB +// 1フレーム分の走行による誤差。dTは1フレームの時間。motionはその間の移動量。 void CovarianceCalculator::calMotionCovariance(double th, double dx, double dy, double dth, double dt, Eigen::Matrix3d &cov, bool accum) { setAlpha(1, 5); - double dis = sqrt(dx*dx + dy*dy); // s - double vt = dis/dt; // ix[m/s] - double wt = dth/dt; // px[rad/s] - double vthre = 0.001; // vt̉lB^C~Oɂ0ɂȂ̂hB - double wthre = 0.01; // wt̉l + double dis = sqrt(dx*dx + dy*dy); // 走行距離 + double vt = dis/dt; // 並進速度[m/s] + double wt = dth/dt; // 角速度[rad/s] + double vthre = 0.001; // vtの下限値。タイミングにより0になるのを防ぐ。 + double wthre = 0.01; // wtの下限値 if (vt < vthre) vt = vthre; if (wt < wthre) wt = wthre; - // ݐςꍇ́At-1̋UssigmaAt̋UsvZ + // 累積する場合は、時刻t-1の共分散行列sigmaから、時刻tの共分散行列を計算 Eigen::Matrix3d A = Eigen::Matrix3d::Zero(3,3); if (accum) { Eigen::Matrix3d Jxk; @@ -164,7 +164,7 @@ void CovarianceCalculator::calUk(double vt, double wt, Eigen::Matrix2d &Uk) { 0, a2*wt*wt; } -// {bgpɊւ郄RrsBvt̓{bg̑xAth̓{bg̕(WA)Adt͎ +// ロボット姿勢に関するヤコビ行列。vtはロボットの速度、thはロボットの方向(ラジアン)、dtは時間 void CovarianceCalculator::calJxk(double th, double vt, double dt, Eigen::Matrix3d &Jxk) { double cs = cos(th); double sn = sin(th); @@ -184,18 +184,18 @@ void CovarianceCalculator::calJuk(double th, double dt, Eigen::Matrix curLps; // ΉƂꂽ݃XL̓_Q - std::vector refLps; // ΉƂꂽQƃXL̓_Q + std::vector curLps; // 対応がとれた現在スキャンの点群 + std::vector refLps; // 対応がとれた参照スキャンの点群 DataAssociator() { } diff --git a/framework/LPoint2D.h b/framework/LPoint2D.h index 2e54e87..db75bfa 100755 --- a/framework/LPoint2D.h +++ b/framework/LPoint2D.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -24,17 +24,17 @@ struct Vector2D //////////////////////// -enum ptype {UNKNOWN=0, LINE=1, CORNER=2, ISOLATE=3}; // _̃^CvFmAAR[iAǗ +enum ptype {UNKNOWN=0, LINE=1, CORNER=2, ISOLATE=3}; // 点のタイプ：未知、直線、コーナ、孤立 struct LPoint2D { - int sid; // t[ԍiXLԍj - double x; // ʒux - double y; // ʒuy - double nx; // @xNg - double ny; // @xNg - double atd; // ݐϑs(accumulated travel distance) - ptype type; // _̃^Cv + int sid; // フレーム番号（スキャン番号） + double x; // 位置x + double y; // 位置y + double nx; // 法線ベクトル + double ny; // 法線ベクトル + double atd; // 累積走行距離(accumulated travel distance) + ptype type; // 点のタイプ LPoint2D() : sid(-1), x(0), y(0) { init(); @@ -67,14 +67,14 @@ struct LPoint2D y = _y; } - // rangeanglexy߂(En) + // rangeとangleからxyを求める(右手系) void calXY(double range, double angle) { double a = DEG2RAD(angle); x = range*cos(a); y = range*sin(a); } - // rangeanglexy߂(nj + // rangeとangleからxyを求める(左手系） void calXYi(double range, double angle) { double a = DEG2RAD(angle); x = range*cos(a); diff --git a/framework/LoopDetector.cpp b/framework/LoopDetector.cpp index ec793ba..064d493 100755 --- a/framework/LoopDetector.cpp +++ b/framework/LoopDetector.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -18,7 +18,7 @@ using namespace std; //////////// -// [võ_~[֐ +// ループ検出のダミー関数 bool LoopDetector::detectLoop(Scan2D *curScan, Pose2D &curPose, int cnt) { return(false); } diff --git a/framework/LoopDetector.h b/framework/LoopDetector.h index 14bad99..682d317 100755 --- a/framework/LoopDetector.h +++ b/framework/LoopDetector.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -24,15 +24,15 @@ /////// -// [vA[Nݒ +// ループアーク設定情報 struct LoopInfo { - bool arcked; // łɃ|[YA[N𒣂 - int curId; // ݃L[t[idiXLj - int refId; // QƃL[t[idiXLC܂́CLocalGridMap2Dj - Pose2D pose; // ݃L[t[QƃL[t[Ƀ}b`O[opiGridx[X̏ꍇ͋tj - double score; // ICP}b`OXRA - Eigen::Matrix3d cov; // U + bool arcked; // すでにポーズアークを張ったか + int curId; // 現在キーフレームid（スキャン） + int refId; // 参照キーフレームid（スキャン，または，LocalGridMap2D） + Pose2D pose; // 現在キーフレームが参照キーフレームにマッチするグローバル姿勢（Gridベースの場合は逆） + double score; // ICPマッチングスコア + Eigen::Matrix3d cov; // 共分散 LoopInfo() : arcked(false), curId(-1), refId(-1), score(-1) { } @@ -47,7 +47,7 @@ struct LoopInfo ////////////// -// fobOpf[^ +// デバッグ用データ struct LoopMatch { Scan2D curScan; @@ -69,8 +69,8 @@ struct LoopMatch class LoopDetector { protected: - PoseGraph *pg; // |[YOt - std::vector loopMatches; // fobOp + PoseGraph *pg; // ポーズグラフ + std::vector loopMatches; // デバッグ用 public: LoopDetector() { @@ -81,7 +81,7 @@ class LoopDetector //////// - // fobOp + // デバッグ用 std::vector &getLoopMatches() { return(loopMatches); } diff --git a/framework/MyUtil.cpp b/framework/MyUtil.cpp index 4d50d98..5c7ea14 100755 --- a/framework/MyUtil.cpp +++ b/framework/MyUtil.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -20,7 +20,7 @@ using namespace Eigen; ///////// -// px̉ZB-180180ɐK +// 角度の加算。-180から180に正規化 int MyUtil::add(int a1, int a2) { int sum = a1 + a2; if (sum < -180) @@ -31,7 +31,7 @@ int MyUtil::add(int a1, int a2) { return(sum); } -// px̉ZB-180180ɐK +// 角度の加算。-180から180に正規化 double MyUtil::add(double a1, double a2) { double sum = a1 + a2; if (sum < -180) @@ -42,7 +42,7 @@ double MyUtil::add(double a1, double a2) { return(sum); } -// px̉ZiWAjB-pipiɐK +// 角度の加算（ラジアン）。-piからpiに正規化 double MyUtil::addR(double a1, double a2) { double sum = a1 + a2; if (sum < -M_PI) @@ -55,7 +55,7 @@ double MyUtil::addR(double a1, double a2) { //////////// -// SVDptsvZ +// SVDを用いた逆行列計算 Eigen::Matrix3d MyUtil::svdInverse(const Matrix3d &A) { size_t m = A.rows(); size_t n = A.cols(); @@ -68,7 +68,7 @@ Eigen::Matrix3d MyUtil::svdInverse(const Matrix3d &A) { MatrixXd M1(m, n); for (size_t i=0; i #ifndef M_PI -#define M_PI 3.14159265358979 // ~ +#define M_PI 3.14159265358979 // 円周率 #endif #ifndef NULL -#define NULL 0 // {Iɂ́AC++11nullptrg +#define NULL 0 // 基本的には、C++11のnullptrを使う #endif -#define DEG2RAD(x) ((x)*M_PI/180) // x烉WA -#define RAD2DEG(x) ((x)*180/M_PI) // WAx +#define DEG2RAD(x) ((x)*M_PI/180) // 度からラジアン +#define RAD2DEG(x) ((x)*180/M_PI) // ラジアンから度 typedef unsigned char uchar; diff --git a/framework/NNGridTable.cpp b/framework/NNGridTable.cpp index fecc9dc..a1f99f6 100755 --- a/framework/NNGridTable.cpp +++ b/framework/NNGridTable.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -18,28 +18,28 @@ using namespace std; //////////// -// iqe[uɃXL_lpo^ +// 格子テーブルにスキャン点lpを登録する void NNGridTable::addPoint(const LPoint2D *lp) { - // e[ũCfbNXvZB܂AΏۗ̈ɂ邩`FbNB + // テーブル検索のインデックス計算。まず、対象領域内にあるかチェックする。 int xi = static_cast(lp->x/csize) + tsize; - if (xi < 0 || xi > 2*tsize) // Ώۗ̈̊O + if (xi < 0 || xi > 2*tsize) // 対象領域の外 return; int yi = static_cast(lp->y/csize) + tsize; - if (yi < 0 || yi > 2*tsize) // Ώۗ̈̊O + if (yi < 0 || yi > 2*tsize) // 対象領域の外 return; - size_t idx = static_cast(yi*(2*tsize +1) + xi); // e[ũCfbNX - table[idx].lps.push_back(lp); // ړĨZɓ + size_t idx = static_cast(yi*(2*tsize +1) + xi); // テーブルのインデックス + table[idx].lps.push_back(lp); // 目的のセルに入れる } /////////// -// XL_clppredPoseōWϊʒuɍł߂_iqe[u猩‚ +// スキャン点clpをpredPoseで座標変換した位置に最も近い点を格子テーブルから見つける const LPoint2D *NNGridTable::findClosestPoint(const LPoint2D *clp, const Pose2D &predPose) { - LPoint2D glp; // clp̗\ʒu - predPose.globalPoint(*clp, glp); // relPoseōWϊ + LPoint2D glp; // clpの予測位置 + predPose.globalPoint(*clp, glp); // relPoseで座標変換 - // clp̃e[uCfbNXBΏۗ̈ɂ邩`FbNB + // clpのテーブルインデックス。対象領域内にあるかチェックする。 int cxi = static_cast(glp.x/csize) + tsize; if (cxi < 0 || cxi > 2*tsize) return(nullptr); @@ -47,30 +47,30 @@ const LPoint2D *NNGridTable::findClosestPoint(const LPoint2D *clp, const Pose2D if (cyi < 0 || cyi > 2*tsize) return(nullptr); - size_t pn=0; // TZ̓_̑BmFp + size_t pn=0; // 探したセル内の点の総数。確認用 double dmin=1000000; - const LPoint2D *lpmin = nullptr; // ł߂_iړI̓_j - double dthre=0.2; // 艓_͏O[m] + const LPoint2D *lpmin = nullptr; // 最も近い点（目的の点） + double dthre=0.2; // これより遠い点は除外する[m] int R=static_cast(dthre/csize); - // }RlT + // ±R四方を探す for (int i=-R; i<=R; i++) { - int yi = cyi+i; // cyiL + int yi = cyi+i; // cyiから広げる if (yi < 0 || yi > 2*tsize) continue; for (int j=-R; j<=R; j++) { - int xi = cxi+j; // cxiL + int xi = cxi+j; // cxiから広げる if (xi < 0 || xi > 2*tsize) continue; - size_t idx = yi*(2*tsize+1) + xi; // e[uCfbNX - NNGridCell &cell = table[idx]; // ̃Z - vector &lps = cell.lps; // ZƒXL_Q + size_t idx = yi*(2*tsize+1) + xi; // テーブルインデックス + NNGridCell &cell = table[idx]; // そのセル + vector &lps = cell.lps; // セルがもつスキャン点群 for (size_t k=0; kx - glp.x)*(lp->x - glp.x) + (lp->y - glp.y)*(lp->y - glp.y); - if (d <= dthre*dthre && d < dmin) { // dthreŋŏƂȂ_ۑ + if (d <= dthre*dthre && d < dmin) { // dthre内で距離が最小となる点を保存 dmin = d; lpmin = lp; } @@ -78,51 +78,51 @@ const LPoint2D *NNGridTable::findClosestPoint(const LPoint2D *clp, const Pose2D pn += lps.size(); } } -// printf("pn=%d\n", pn); // TZ̓_̑BmFp +// printf("pn=%d\n", pn); // 探したセル内の点の総数。確認用 return(lpmin); } //////////// -// iqe[ůeZ̑\_psɊi[B +// 格子テーブルの各セルの代表点を作ってpsに格納する。 void NNGridTable::makeCellPoints(int nthre, vector &ps) { - // ̓Z̊e_̃XLԍ̕ςƂB - // XLԍ̍ŐVlƂꍇ́Ã̕Rg͂A - // ςƂꍇi2sjRgAEgB + // 現状はセル内の各点のスキャン番号の平均をとる。 + // スキャン番号の最新値をとる場合は、その部分のコメントをはずし、 + // 平均とる場合（2行）をコメントアウトする。 - size_t nn=0; // e[ȗSZBmFp + size_t nn=0; // テーブル内の全セル数。確認用 for (size_t i=0; i &lps = table[i].lps; // Z̃XL_Q + vector &lps = table[i].lps; // セルのスキャン点群 nn += lps.size(); - if (lps.size() >= nthre) { // _nthre葽Z - double gx=0, gy=0; // _Q̏dSʒu - double nx=0, ny=0; // _Q̖@xNg̕ + if (lps.size() >= nthre) { // 点数がnthreより多いセルだけ処理する + double gx=0, gy=0; // 点群の重心位置 + double nx=0, ny=0; // 点群の法線ベクトルの平均 int sid=0; for (size_t j=0; jx; // ʒuݐ + gx += lp->x; // 位置を累積 gy += lp->y; - nx += lp->nx; // @xNgݐ + nx += lp->nx; // 法線ベクトル成分を累積 ny += lp->ny; - sid += lp->sid; // XLԍ̕ςƂꍇ -// if (lp->sid > sid) // XLԍ̍ŐVlƂꍇ + sid += lp->sid; // スキャン番号の平均とる場合 +// if (lp->sid > sid) // スキャン番号の最新値とる場合 // sid = lp->sid; // printf("sid=%d\n", lp->sid); } - gx /= lps.size(); // + gx /= lps.size(); // 平均 gy /= lps.size(); double L = sqrt(nx*nx + ny*ny); - nx /= L; // ρiKj + nx /= L; // 平均（正規化） ny /= L; - sid /= lps.size(); // XLԍ̕ςƂꍇ + sid /= lps.size(); // スキャン番号の平均とる場合 - LPoint2D newLp(sid, gx, gy); // Z̑\_𐶐 - newLp.setNormal(nx, ny); // @xNgݒ - newLp.setType(LINE); // ^Cv͒ɂ - ps.emplace_back(newLp); // psɒlj + LPoint2D newLp(sid, gx, gy); // セルの代表点を生成 + newLp.setNormal(nx, ny); // 法線ベクトル設定 + newLp.setType(LINE); // タイプは直線にする + ps.emplace_back(newLp); // psに追加 } } -// printf("nn=%d\n", nn); // e[ȗSZBmFp +// printf("nn=%d\n", nn); // テーブル内の全セル数。確認用 } diff --git a/framework/NNGridTable.h b/framework/NNGridTable.h index bc8ce71..f0afd35 100755 --- a/framework/NNGridTable.h +++ b/framework/NNGridTable.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -21,30 +21,30 @@ struct NNGridCell { - std::vector lps; // ̃ZɊi[ꂽXL_Q + std::vector lps; // このセルに格納されたスキャン点群 void clear() { - lps.clear(); // ɂ + lps.clear(); // 空にする } }; /////// -// iqe[u +// 格子テーブル class NNGridTable { private: - double csize; // ZTCY[m] - double rsize; // Ώۗ̈̃TCY[m]B`1ӂ̔B - int tsize; // e[uTCY̔ - std::vector table; // e[u{ + double csize; // セルサイズ[m] + double rsize; // 対象領域のサイズ[m]。正方形の1辺の半分。 + int tsize; // テーブルサイズの半分 + std::vector table; // テーブル本体 public: - NNGridTable() : csize(0.05), rsize(40){ // Z5cmAΏۗ̈40x2ml - tsize = static_cast(rsize/csize); // e[uTCY̔ - size_t w = static_cast(2*tsize+1); // e[uTCY - table.resize(w*w); // ̈m - clear(); // tablȅ + NNGridTable() : csize(0.05), rsize(40){ // セル5cm、対象領域40x2m四方 + tsize = static_cast(rsize/csize); // テーブルサイズの半分 + size_t w = static_cast(2*tsize+1); // テーブルサイズ + table.resize(w*w); // 領域確保 + clear(); // tableの初期化 } ~NNGridTable() { @@ -52,7 +52,7 @@ class NNGridTable void clear() { for (size_t i=0; i &newPoses, int N) { - vector &nodes = pg.nodes; // |[Ym[h - vector &arcs = pg.arcs; // |[YA[N + vector &nodes = pg.nodes; // ポーズノード + vector &arcs = pg.arcs; // ポーズアーク - // |[Ym[hp2opɕϊ - vector pnodes; // p2õ|[Ym[hW + // ポーズノードをp2o用に変換 + vector pnodes; // p2oのポーズノード集合 for (size_t i=0; ipose; // m[ḧʒu - pnodes.push_back(p2o::Pose2D(pose.tx, pose.ty, DEG2RAD(pose.th))); // ʒu + Pose2D pose = node->pose; // ノードの位置 + pnodes.push_back(p2o::Pose2D(pose.tx, pose.ty, DEG2RAD(pose.th))); // 位置だけ入れる } - // |[YA[Nkslampɕϊ - p2o::Con2DVec pcons; // p2õ|[YA[NW + // ポーズアークをkslam用に変換 + p2o::Con2DVec pcons; // p2oのポーズアーク集合 for (size_t i=0; isrc; @@ -50,14 +50,14 @@ void P2oDriver2D::doP2o( PoseGraph &pg, vector &newPoses, int N) { pcons.push_back(con); } -// printf("knodes.size=%lu, kcons.size=%lu\n", knodes.size(), kcons.size()); // mFp +// printf("knodes.size=%lu, kcons.size=%lu\n", knodes.size(), kcons.size()); // 確認用 - p2o::Optimizer2D opt; // p2oCX^X - std::vector result = opt.optimizePath(pnodes, pcons, N); // Ns + p2o::Optimizer2D opt; // p2oインスタンス + std::vector result = opt.optimizePath(pnodes, pcons, N); // N回実行 - // ʂnewPoseɊi[ + // 結果をnewPoseに格納する for (size_t i=0; i poses; // {bgO - Pose2D lastPose; // Ōɐ肵{bgʒu - Scan2D lastScan; // ŌɏXL + std::vector poses; // ロボット軌跡 + Pose2D lastPose; // 最後に推定したロボット位置 + Scan2D lastScan; // 最後に処理したスキャン - std::vector globalMap; // S̒n}BԈ̓_ - std::vector localMap; // ݈ʒuߖT̋Ǐn}BXL}b`OɎg + std::vector globalMap; // 全体地図。間引き後の点 + std::vector localMap; // 現在位置近傍の局所地図。スキャンマッチングに使う PointCloudMap() : nthre(1) { - globalMap.reserve(MAX_POINT_NUM); // ŏɊm + globalMap.reserve(MAX_POINT_NUM); // 最初に確保 } ~PointCloudMap() { diff --git a/framework/Pose2D.cpp b/framework/Pose2D.cpp index 65be1ca..66836be 100755 --- a/framework/Pose2D.cpp +++ b/framework/Pose2D.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -14,25 +14,25 @@ #include "Pose2D.h" -// O[oWnł̓_pAiPose2Dj̋ǏWnɕϊ +// グローバル座標系での点pを、自分（Pose2D）の局所座標系に変換 LPoint2D Pose2D::relativePoint(const LPoint2D &p) const { double dx = p.x - tx; double dy = p.y - ty; - double x = dx*Rmat[0][0] + dy*Rmat[1][0]; // ]̋ts + double x = dx*Rmat[0][0] + dy*Rmat[1][0]; // 回転の逆行列 double y = dx*Rmat[0][1] + dy*Rmat[1][1]; return LPoint2D(p.sid, x, y); } //////// -// iPose2Dj̋ǏWnł̓_pAO[oWnɕϊ +// 自分（Pose2D）の局所座標系での点pを、グローバル座標系に変換 LPoint2D Pose2D::globalPoint(const LPoint2D &p) const { double x = Rmat[0][0]*p.x + Rmat[0][1]*p.y + tx; double y = Rmat[1][0]*p.x + Rmat[1][1]*p.y + ty; return LPoint2D(p.sid, x, y); } -// iPose2Dj̋ǏWnł̓_pAO[oWnɕϊpoɓ +// 自分（Pose2D）の局所座標系での点pを、グローバル座標系に変換してpoに入れる void Pose2D::globalPoint(const LPoint2D &pi, LPoint2D &po) const { po.x = Rmat[0][0]*pi.x + Rmat[0][1]*pi.y + tx; po.y = Rmat[1][0]*pi.x + Rmat[1][1]*pi.y + ty; @@ -40,19 +40,19 @@ void Pose2D::globalPoint(const LPoint2D &pi, LPoint2D &po) const { /////// -// Wnbpose猩WnnposȇΈʒurelPose߂iInverse compounding operatorj +// 基準座標系bposeから見た現座標系nposeの相対位置relPoseを求める（Inverse compounding operator） void Pose2D::calRelativePose(const Pose2D &npose, const Pose2D &bpose, Pose2D &relPose) { - const double (*R0)[2] = bpose.Rmat; // Wn - const double (*R1)[2] = npose.Rmat; // Wn - double (*R2)[2] = relPose.Rmat; // Έʒu + const double (*R0)[2] = bpose.Rmat; // 基準座標系 + const double (*R1)[2] = npose.Rmat; // 現座標系 + double (*R2)[2] = relPose.Rmat; // 相対位置 - // i + // 並進 double dx = npose.tx - bpose.tx; double dy = npose.ty - bpose.ty; relPose.tx = R0[0][0]*dx + R0[1][0]*dy; relPose.ty = R0[0][1]*dx + R0[1][1]*dy; - // ] + // 回転 double th = npose.th - bpose.th; if (th < -180) th += 360; @@ -63,19 +63,19 @@ void Pose2D::calRelativePose(const Pose2D &npose, const Pose2D &bpose, Pose2D &r relPose.calRmat(); } -// Wnbpose瑊ΈʒurelPosei񂾁AWnnpose߂iCompounding operatorj +// 基準座標系bposeから相対位置relPoseだけ進んだ、座標系nposeを求める（Compounding operator） void Pose2D::calGlobalPose(const Pose2D &relPose, const Pose2D &bpose, Pose2D &npose) { - const double (*R0)[2] = bpose.Rmat; // Wn - const double (*R1)[2] = relPose.Rmat; // Έʒu - double (*R2)[2] = npose.Rmat; // VWn + const double (*R0)[2] = bpose.Rmat; // 基準座標系 + const double (*R1)[2] = relPose.Rmat; // 相対位置 + double (*R2)[2] = npose.Rmat; // 新座標系 - // i + // 並進 double tx = relPose.tx; double ty = relPose.ty; npose.tx = R0[0][0]*tx + R0[0][1]*ty + bpose.tx; npose.ty = R0[1][0]*tx + R0[1][1]*ty + bpose.ty; - // px + // 角度 double th = bpose.th + relPose.th; if (th < -180) th += 360; diff --git a/framework/Pose2D.h b/framework/Pose2D.h index c33518a..97447ab 100755 --- a/framework/Pose2D.h +++ b/framework/Pose2D.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -22,10 +22,10 @@ struct Pose2D { - double tx; // ix - double ty; // iy - double th; // ]p(x) - double Rmat[2][2]; // p̉]s + double tx; // 並進x + double ty; // 並進y + double th; // 回転角(度) + double Rmat[2][2]; // 姿勢の回転行列 Pose2D() : tx(0), ty(0), th(0) { for(int i=0;i<2;i++) { diff --git a/framework/PoseEstimatorICP.cpp b/framework/PoseEstimatorICP.cpp index 80fde8d..06fd2cb 100755 --- a/framework/PoseEstimatorICP.cpp +++ b/framework/PoseEstimatorICP.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -19,30 +19,30 @@ using namespace std; ////////////// -// linitPose^āAICPɂ胍{bgʒu̐lestPose߂ +// 初期値initPoseを与えて、ICPによりロボット位置の推定値estPoseを求める double PoseEstimatorICP::estimatePose(Pose2D &initPose, Pose2D &estPose){ boost::timer tim; - double evmin = HUGE_VAL; // RXgŏlBl͑傫 - double evthre = 0.000001; // RXgω臒lBωʂȉȂJԂI -// double evthre = 0.00000001; // RXgω臒lBωʂȉȂJԂI + double evmin = HUGE_VAL; // コスト最小値。初期値は大きく + double evthre = 0.000001; // コスト変化閾値。変化量がこれ以下なら繰り返し終了 +// double evthre = 0.00000001; // コスト変化閾値。変化量がこれ以下なら繰り返し終了 popt->setEvthre(evthre); - popt->setEvlimit(0.2); // evlimit͊Ol臒l[m] + popt->setEvlimit(0.2); // evlimitは外れ値の閾値[m] - double ev = 0; // RXg - double evold = evmin; // 1‘O̒lB̂߂ɎgB + double ev = 0; // コスト + double evold = evmin; // 1つ前の値。収束判定のために使う。 Pose2D pose = initPose; Pose2D poseMin = initPose; - for (int i=0; abs(evold-ev) > evthre && i<100; i++) { // i<100͐U΍ + for (int i=0; abs(evold-ev) > evthre && i<100; i++) { // i<100は振動対策 if (i > 0) evold = ev; - double mratio = dass->findCorrespondence(curScan, pose); // f[^ΉÂ + double mratio = dass->findCorrespondence(curScan, pose); // データ対応づけ Pose2D newPose; - popt->setPoints(dass->curLps, dass->refLps); // Ήʂn - ev = popt->optimizePose(pose, newPose); // ̑ΉÂɂă{bgʒu̍œK + popt->setPoints(dass->curLps, dass->refLps); // 対応結果を渡す + ev = popt->optimizePose(pose, newPose); // その対応づけにおいてロボット位置の最適化 pose = newPose; - if (ev < evmin) { // RXgŏʂۑ + if (ev < evmin) { // コスト最小結果を保存 poseMin = newPose; evmin = ev; } @@ -58,15 +58,15 @@ double PoseEstimatorICP::estimatePose(Pose2D &initPose, Pose2D &estPose){ estPose = poseMin; printf("finalError=%g, pnrate=%g\n", evmin, pnrate); - printf("estPose: tx=%g, ty=%g, th=%g\n", pose.tx, pose.ty, pose.th); // mFp + printf("estPose: tx=%g, ty=%g, th=%g\n", pose.tx, pose.ty, pose.th); // 確認用 double t1 = 1000*tim.elapsed(); - printf("PoseEstimatorICP: t1=%g\n", t1); // + printf("PoseEstimatorICP: t1=%g\n", t1); // 処理時間 if (evmin < HUGE_VAL) - totalError += evmin; // 덷v - totalTime += t1; // ԍv - printf("totalError=%g, totalTime=%g\n", totalError, totalTime); // mFp + totalError += evmin; // 誤差合計 + totalTime += t1; // 処理時間合計 + printf("totalError=%g, totalTime=%g\n", totalError, totalTime); // 確認用 return(evmin); } diff --git a/framework/PoseEstimatorICP.h b/framework/PoseEstimatorICP.h index 6e97880..d7020a7 100755 --- a/framework/PoseEstimatorICP.h +++ b/framework/PoseEstimatorICP.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -28,16 +28,16 @@ class PoseEstimatorICP { private: - const Scan2D *curScan; // ݃XL - size_t usedNum; // ICPɎgꂽ_BLoopDetectorŐM`FbNɎg - double pnrate; // ΉÂꂽ_̔䗦 + const Scan2D *curScan; // 現在スキャン + size_t usedNum; // ICPに使われた点数。LoopDetectorで信頼性チェックに使う + double pnrate; // 正しく対応づけされた点の比率 - PoseOptimizer *popt; // œKNX - DataAssociator *dass; // f[^ΉÂNX + PoseOptimizer *popt; // 最適化クラス + DataAssociator *dass; // データ対応づけクラス public: - double totalError; // 덷v - double totalTime; // ԍv + double totalError; // 誤差合計 + double totalTime; // 処理時間合計 public: @@ -67,12 +67,12 @@ class PoseEstimatorICP void setScanPair(const Scan2D *c, const Scan2D *r) { curScan = c; - dass->setRefBase(r->lps); // f[^ΉÂ̂߂ɎQƃXL_o^ + dass->setRefBase(r->lps); // データ対応づけのために参照スキャン点を登録 } void setScanPair(const Scan2D *c, const std::vector &refLps) { curScan = c; - dass->setRefBase(refLps); // f[^ΉÂ̂߂ɎQƃXL_o^ + dass->setRefBase(refLps); // データ対応づけのために参照スキャン点を登録 } //////////// diff --git a/framework/PoseFuser.cpp b/framework/PoseFuser.cpp index f767437..99c6843 100755 --- a/framework/PoseFuser.cpp +++ b/framework/PoseFuser.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -16,33 +16,33 @@ using namespace std; -////////////// SLAMp̃ZTZ //////////////// +////////////// 逐次SLAM用のセンサ融合 //////////////// -// SLAMłICPƃIhg̐ړʂZBdassɎQƃXLĂƁBcovɈړʂ̋Us񂪓B +// 逐次SLAMでのICPとオドメトリの推定移動量を融合する。dassに参照スキャンを入れておくこと。covに移動量の共分散行列が入る。 double PoseFuser::fusePose(Scan2D *curScan, const Pose2D &estPose, const Pose2D &odoMotion, const Pose2D &lastPose, Pose2D &fusedPose, Eigen::Matrix3d &fusedCov) { - // ICP̋U - dass->findCorrespondence(curScan, estPose); // ʒuestPoseŌ݃XL_QƎQƃXL_Q̑ΉÂ - double ratio = cvc.calIcpCovariance(estPose, dass->curLps, dass->refLps, ecov); // œ̂́An}Wnł̈ʒűU + // ICPの共分散 + dass->findCorrespondence(curScan, estPose); // 推定位置estPoseで現在スキャン点群と参照スキャン点群の対応づけ + double ratio = cvc.calIcpCovariance(estPose, dass->curLps, dass->refLps, ecov); // ここで得られるのは、地図座標系での位置の共分散 - // Ihg̈ʒuƋUBx^fgƁAZԂł͋U邽߁AȈՔłő傫߂ɌvZ - Pose2D predPose; // \ʒu - Pose2D::calGlobalPose(odoMotion, lastPose, predPose); // OʒulastPoseɈړʂė\ʒuvZ + // オドメトリの位置と共分散。速度運動モデルを使うと、短期間では共分散が小さすぎるため、簡易版で大きめに計算する + Pose2D predPose; // 予測位置 + Pose2D::calGlobalPose(odoMotion, lastPose, predPose); // 直前位置lastPoseに移動量を加えて予測位置を計算 Eigen::Matrix3d mcovL; double dT=0.1; - cvc.calMotionCovarianceSimple(odoMotion, dT, mcovL); // Ihgœړʂ̋UiȈՔŁj - CovarianceCalculator::rotateCovariance(estPose, mcovL, mcov); // ݈ʒuestPoseʼn]āAn}Wnł̋Umcov𓾂 + cvc.calMotionCovarianceSimple(odoMotion, dT, mcovL); // オドメトリで得た移動量の共分散（簡易版） + CovarianceCalculator::rotateCovariance(estPose, mcovL, mcov); // 現在位置estPoseで回転させて、地図座標系での共分散mcovを得る - // ecov, mcov, covƂɁAlastPose_ƂǏWnł̒l - Eigen::Vector3d mu1(estPose.tx, estPose.ty, DEG2RAD(estPose.th)); // ICPɂ鐄l - Eigen::Vector3d mu2(predPose.tx, predPose.ty, DEG2RAD(predPose.th)); // Ihgɂ鐄l + // ecov, mcov, covともに、lastPoseを原点とした局所座標系での値 + Eigen::Vector3d mu1(estPose.tx, estPose.ty, DEG2RAD(estPose.th)); // ICPによる推定値 + Eigen::Vector3d mu2(predPose.tx, predPose.ty, DEG2RAD(predPose.th)); // オドメトリによる推定値 Eigen::Vector3d mu; - fuse(mu1, ecov, mu2, mcov, mu, fusedCov); // 2‚̐Kz̗Z + fuse(mu1, ecov, mu2, mcov, mu, fusedCov); // 2つの正規分布の融合 - fusedPose.setVal(mu[0], mu[1], RAD2DEG(mu[2])); // Zړʂi[ + fusedPose.setVal(mu[0], mu[1], RAD2DEG(mu[2])); // 融合した移動量を格納 totalCov = fusedCov; - // mFp + // 確認用 printf("fusePose\n"); double vals[2], vec1[2], vec2[2]; printf("ecov: det=%g, ", ecov.determinant()); @@ -62,41 +62,41 @@ double PoseFuser::fusePose(Scan2D *curScan, const Pose2D &estPose, const Pose2D void PoseFuser::calOdometryCovariance(const Pose2D &odoMotion, const Pose2D &lastPose, Eigen::Matrix3d &mcov) { Eigen::Matrix3d mcovL; double dT=0.1; - cvc.calMotionCovarianceSimple(odoMotion, dT, mcovL); // Ihgœړʂ̋UiȈՔŁj - CovarianceCalculator::rotateCovariance(lastPose, mcovL, mcov); // OʒulastPoseʼn]āAʒűUmcov𓾂 + cvc.calMotionCovarianceSimple(odoMotion, dT, mcovL); // オドメトリで得た移動量の共分散（簡易版） + CovarianceCalculator::rotateCovariance(lastPose, mcovL, mcov); // 直前位置lastPoseで回転させて、位置の共分散mcovを得る } -/////// KEXz̗Z /////// +/////// ガウス分布の融合 /////// -// 2‚̐KzZBmu͕ρAcv͋UB +// 2つの正規分布を融合する。muは平均、cvは共分散。 double PoseFuser::fuse(const Eigen::Vector3d &mu1, const Eigen::Matrix3d &cv1, const Eigen::Vector3d &mu2, const Eigen::Matrix3d &cv2, Eigen::Vector3d &mu, Eigen::Matrix3d &cv) { - // Us̗Z + // 共分散行列の融合 Eigen::Matrix3d IC1 = MyUtil::svdInverse(cv1); Eigen::Matrix3d IC2 = MyUtil::svdInverse(cv2); Eigen::Matrix3d IC = IC1 + IC2; cv = MyUtil::svdInverse(IC); - // px̕␳BZɘAۂ‚߁B - Eigen::Vector3d mu11 = mu1; // ICP̕Ihgɍ + // 角度の補正。融合時に連続性を保つため。 + Eigen::Vector3d mu11 = mu1; // ICPの方向をオドメトリに合せる double da = mu2(2) - mu1(2); if (da > M_PI) mu11(2) += 2*M_PI; else if (da < -M_PI) mu11(2) -= 2*M_PI; - // ς̗Z + // 平均の融合 Eigen::Vector3d nu1 = IC1*mu11; Eigen::Vector3d nu2 = IC2*mu2; Eigen::Vector3d nu3 = nu1 + nu2; mu = cv*nu3; - // px̕␳B(-pi, pi)Ɏ߂ + // 角度の補正。(-pi, pi)に収める if (mu(2) > M_PI) mu(2) -= 2*M_PI; else if (mu(2) < -M_PI) mu(2) += 2*M_PI; - // W̌vZ + // 係数部の計算 Eigen::Vector3d W1 = IC1*mu11; Eigen::Vector3d W2 = IC2*mu2; Eigen::Vector3d W = IC*mu; diff --git a/framework/PoseFuser.h b/framework/PoseFuser.h index e9fb080..ac62fd6 100755 --- a/framework/PoseFuser.h +++ b/framework/PoseFuser.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -22,16 +22,16 @@ #include "DataAssociator.h" #include "CovarianceCalculator.h" -// ZTZBICPƃIhg̐lZB +// センサ融合器。ICPとオドメトリの推定値を融合する。 class PoseFuser { public: - Eigen::Matrix3d ecov; // ICP̋Us - Eigen::Matrix3d mcov; // Ihg̋Us + Eigen::Matrix3d ecov; // ICPの共分散行列 + Eigen::Matrix3d mcov; // オドメトリの共分散行列 Eigen::Matrix3d totalCov; - DataAssociator *dass; // f[^ΉÂ - CovarianceCalculator cvc; // UvZ + DataAssociator *dass; // データ対応づけ器 + CovarianceCalculator cvc; // 共分散計算器 public: PoseFuser() { @@ -54,11 +54,11 @@ class PoseFuser dass->setRefBase(refLps); } - // ICP̋UšvZBsetRefLpšɍsƁB + // ICPの共分散行列の計算。setRefLpsの後に行うこと。 double calIcpCovariance(const Pose2D &estMotion, const Scan2D *curScan, Eigen::Matrix3d &cov) { dass->findCorrespondence(curScan, estMotion); - // ICP̋UBœ̂́AEWnł̋U + // ICPの共分散。ここで得られるのは、世界座標系での共分散 double ratio = cvc.calIcpCovariance(estMotion, dass->curLps, dass->refLps, cov); return(ratio); } diff --git a/framework/PoseGraph.cpp b/framework/PoseGraph.cpp index 2f7c0c5..428d92a 100755 --- a/framework/PoseGraph.cpp +++ b/framework/PoseGraph.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -16,28 +16,28 @@ using namespace std; -//////////// Ot //////////// +//////////// グラフ生成 //////////// -// |[YOtɃm[hlj +// ポーズグラフにノード追加 PoseNode *PoseGraph::addNode(const Pose2D &pose) { - PoseNode *n1 = allocNode(); // m[h - addNode(n1, pose); // |[YOtɃm[hlj + PoseNode *n1 = allocNode(); // ノード生成 + addNode(n1, pose); // ポーズグラフにノード追加 return(n1); } -// |[YOtɃm[hlj +// ポーズグラフにノード追加 void PoseGraph::addNode(PoseNode *n1, const Pose2D &pose) { - n1->setNid((int)nodes.size()); // m[hIDt^Bm[h̒ʂԍƓ - n1->setPose(pose); // {bgʒuݒ - nodes.push_back(n1); // nodes̍Ōɒlj + n1->setNid((int)nodes.size()); // ノードID付与。ノードの通し番号と同じ + n1->setPose(pose); // ロボット位置を設定 + nodes.push_back(n1); // nodesの最後に追加 } -// m[hID(nid)m[ĥ‚ +// ノードID(nid)からノード実体を見つける PoseNode *PoseGraph::findNode(int nid) { - for (size_t i=0; inid == nid) // nidv猩‚ + if (n->nid == nid) // nidが一致したら見つけた return(n); } @@ -46,28 +46,28 @@ PoseNode *PoseGraph::findNode(int nid) { ////////// -// |[YOtɃA[Nlj +// ポーズグラフにアークを追加する void PoseGraph::addArc(PoseArc *arc) { - arc->src->addArc(arc); // n_m[harclj - arc->dst->addArc(arc); // I_m[harclj - arcs.push_back(arc); // arcs̍Ōarclj + arc->src->addArc(arc); // 始点ノードにarcを追加 + arc->dst->addArc(arc); // 終点ノードにarcを追加 + arcs.push_back(arc); // arcsの最後にarcを追加 } -// n_m[hsrcNidƏI_m[hdstNid̊ԂɃA[N𐶐 +// 始点ノードsrcNidと終点ノードdstNidの間にアークを生成する PoseArc *PoseGraph::makeArc(int srcNid, int dstNid, const Pose2D &relPose, const Eigen::Matrix3d &cov) { -// Eigen::Matrix3d inf = cov.inverse(); // infcov̋ts - Eigen::Matrix3d inf = MyUtil::svdInverse(cov); // infcov̋ts +// Eigen::Matrix3d inf = cov.inverse(); // infはcovの逆行列 + Eigen::Matrix3d inf = MyUtil::svdInverse(cov); // infはcovの逆行列 - PoseNode *src = nodes[srcNid]; // n_m[h - PoseNode *dst = nodes[dstNid]; // I_m[h + PoseNode *src = nodes[srcNid]; // 始点ノード + PoseNode *dst = nodes[dstNid]; // 終点ノード - PoseArc *arc = allocArc(); // A[N̐ - arc->setup(src, dst, relPose, inf); // relPose͌vɂ鑊Έʒu + PoseArc *arc = allocArc(); // アークの生成 + arc->setup(src, dst, relPose, inf); // relPoseは計測による相対位置 return(arc); } -// n_m[hsrcNidAI_m[hdstNidłA[NԂ +// 始点ノードがsrcNid、終点ノードがdstNidであるアークを返す PoseArc *PoseGraph::findArc(int srcNid, int dstNid) { for (size_t i=0; i arcs; // ̃m[hɂ‚ȂA[N + int nid; // ノードID。PoseGraphのnodesのインデックス（通し番号） + Pose2D pose; // このノードのロボット位置 + std::vector arcs; // このノードにつながるアーク PoseNode(): nid(-1) { } @@ -63,13 +63,13 @@ struct PoseNode //////// -// |[YOt̕ +// ポーズグラフの辺 struct PoseArc { - PoseNode *src; // ̃A[N̎n_̃m[h - PoseNode *dst; // ̃A[N̏I_̃m[h - Pose2D relPose; // ̃A[N̂‘Έʒu(vl) - Eigen::Matrix3d inf; // s + PoseNode *src; // このアークの始点側のノード + PoseNode *dst; // このアークの終点側のノード + Pose2D relPose; // このアークのもつ相対位置(計測値) + Eigen::Matrix3d inf; // 情報行列 PoseArc(void) : src(nullptr), dst(nullptr){ } @@ -92,21 +92,21 @@ struct PoseArc }; -////////// |[YOt ////////// +////////// ポーズグラフ ////////// class PoseGraph { private: static const int POOL_SIZE=100000; - std::vector nodePool; // m[hp̃v[ - std::vector arcPool; // A[Np̃v[ + std::vector nodePool; // ノード生成用のメモリプール + std::vector arcPool; // アーク生成用のメモリプール public: - std::vector nodes; // m[h̏W - std::vector arcs; // A[N̏WBA[N͕Е݂̂ + std::vector nodes; // ノードの集合 + std::vector arcs; // アークの集合。アークは片方向のみもつ PoseGraph() { - nodePool.reserve(POOL_SIZE); // v[̗̈ŏɊmہBvector̓TCYςƒgړ̂łȂƊ댯 + nodePool.reserve(POOL_SIZE); // メモリプールの領域を最初に確保。vectorはサイズが変わると中身が移動するのでこうしないと危険 arcPool.reserve(POOL_SIZE); } @@ -122,7 +122,7 @@ class PoseGraph arcPool.clear(); } - // m[h̐ + // ノードの生成 PoseNode *allocNode() { if (nodePool.size() >= POOL_SIZE) { printf("Error: exceeds nodePool capacity\n"); @@ -130,11 +130,11 @@ class PoseGraph } PoseNode node; - nodePool.emplace_back(node); // v[ɒljāAQƂB + nodePool.emplace_back(node); // メモリプールに追加して、それを参照する。 return(&(nodePool.back())); } - // A[N̐ + // アークの生成 PoseArc *allocArc() { if (arcPool.size() >= POOL_SIZE) { printf("Error: exceeds arcPool capacity\n"); @@ -142,7 +142,7 @@ class PoseGraph } PoseArc arc; - arcPool.emplace_back(arc); // v[ɒljāAQƂB + arcPool.emplace_back(arc); // メモリプールに追加して、それを参照する。 return(&(arcPool.back())); } diff --git a/framework/PoseOptimizer.h b/framework/PoseOptimizer.h index 962e15d..f1d03ed 100755 --- a/framework/PoseOptimizer.h +++ b/framework/PoseOptimizer.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -24,15 +24,15 @@ class PoseOptimizer { public: - int allN; // JԂBeXgp - double sum; // cvBeXgp + int allN; // 繰り返し総数。テスト用 + double sum; // 残差合計。テスト用 protected: - double evthre; // RXgω臒lBωʂȉȂJԂI - double dd; // l̍݁iij - double da; // l̍݁i]j + double evthre; // コスト変化閾値。変化量がこれ以下なら繰り返し終了 + double dd; // 数値微分の刻み（並進） + double da; // 数値微分の刻み（回転） - CostFunction *cfunc; // RXg֐ + CostFunction *cfunc; // コスト関数 public: PoseOptimizer(): evthre(0.000001), dd(0.00001), da(0.00001), cfunc(nullptr) { diff --git a/framework/RefScanMaker.h b/framework/RefScanMaker.h index 8350d48..addf0dd 100755 --- a/framework/RefScanMaker.h +++ b/framework/RefScanMaker.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -25,8 +25,8 @@ class RefScanMaker { protected: - const PointCloudMap *pcmap; // _Qn} - Scan2D refScan; // QƃXL{́BOɒ + const PointCloudMap *pcmap; // 点群地図 + Scan2D refScan; // 参照スキャン本体。これを外に提供 public: RefScanMaker() : pcmap(nullptr) { diff --git a/framework/Scan2D.cpp b/framework/Scan2D.cpp index 8edb9a5..2ab5518 100755 --- a/framework/Scan2D.cpp +++ b/framework/Scan2D.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), diff --git a/framework/Scan2D.h b/framework/Scan2D.h index b8c35a0..3feaa8d 100755 --- a/framework/Scan2D.h +++ b/framework/Scan2D.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -22,15 +22,15 @@ ////////// -// XL +// スキャン struct Scan2D { - static double MAX_SCAN_RANGE; // XL_̋l[m] - static double MIN_SCAN_RANGE; // XL_̋l[m] + static double MAX_SCAN_RANGE; // スキャン点の距離値上限[m] + static double MIN_SCAN_RANGE; // スキャン点の距離値下限[m] - int sid; // XLid - Pose2D pose; // XL擾̃Ihgl - std::vector lps; // XL_Q + int sid; // スキャンid + Pose2D pose; // スキャン取得時のオドメトリ値 + std::vector lps; // スキャン点群 Scan2D() : sid(0) { } diff --git a/framework/ScanMatcher2D.cpp b/framework/ScanMatcher2D.cpp index 1955a9d..64c63b9 100755 --- a/framework/ScanMatcher2D.cpp +++ b/framework/ScanMatcher2D.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -18,71 +18,71 @@ using namespace std; ///////// -// XL}b`O̎s +// スキャンマッチングの実行 bool ScanMatcher2D::matchScan(Scan2D &curScan) { ++cnt; printf("----- ScanMatcher2D: cnt=%d start -----\n", cnt); - // spresݒ肳Ă΁AXL_Ԋuψꉻ + // spresが設定されていれば、スキャン点間隔を均一化する if (spres != nullptr) spres->resamplePoints(&curScan); - // spanaݒ肳Ă΁AXL_̖@vZ + // spanaが設定されていれば、スキャン点の法線を計算する if (spana != nullptr) spana->analysePoints(curScan.lps); - // ŏ̃XL͒Pɒn}ɓ邾 + // 最初のスキャンは単に地図に入れるだけ if (cnt == 0) { growMap(curScan, initPose); - prevScan = curScan; // OXL̐ݒ + prevScan = curScan; // 直前スキャンの設定 return(true); } - // ScanɓĂIhglpĈړʂvZ - Pose2D odoMotion; // IhgɊÂړ - Pose2D::calRelativePose(curScan.pose, prevScan.pose, odoMotion); // OXLƂ̑Έʒuړ + // Scanに入っているオドメトリ値を用いて移動量を計算する + Pose2D odoMotion; // オドメトリに基づく移動量 + Pose2D::calRelativePose(curScan.pose, prevScan.pose, odoMotion); // 前スキャンとの相対位置が移動量 - Pose2D lastPose = pcmap->getLastPose(); // Oʒu - Pose2D predPose; // Ihgɂ\ʒu - Pose2D::calGlobalPose(odoMotion, lastPose, predPose); // OʒuɈړʂė\ʒu𓾂 + Pose2D lastPose = pcmap->getLastPose(); // 直前位置 + Pose2D predPose; // オドメトリによる予測位置 + Pose2D::calGlobalPose(odoMotion, lastPose, predPose); // 直前位置に移動量を加えて予測位置を得る - const Scan2D *refScan = rsm->makeRefScan(); // QƃXL̐ - estim->setScanPair(&curScan, refScan); // ICPɃXLݒ + const Scan2D *refScan = rsm->makeRefScan(); // 参照スキャンの生成 + estim->setScanPair(&curScan, refScan); // ICPにスキャンを設定 printf("curScan.size=%lu, refScan.size=%lu\n", curScan.lps.size(), refScan->lps.size()); - Pose2D estPose; // ICPɂ鐄ʒu - double score = estim->estimatePose(predPose, estPose); // \ʒulɂICPs + Pose2D estPose; // ICPによる推定位置 + double score = estim->estimatePose(predPose, estPose); // 予測位置を初期値にしてICPを実行 size_t usedNum = estim->getUsedNum(); - bool successful; // XL}b`Oɐǂ - if (score <= scthre && usedNum >= nthre) // XRA臒l菬ΐƂ + bool successful; // スキャンマッチングに成功したかどうか + if (score <= scthre && usedNum >= nthre) // スコアが閾値より小さければ成功とする successful = true; else successful = false; printf("score=%g, usedNum=%lu, successful=%d\n", score, usedNum, successful); - if (dgcheck) { // މ̑Ώꍇ + if (dgcheck) { // 退化の対処をする場合 if (successful) { - Pose2D fusedPose; // Z - Eigen::Matrix3d fusedCov; // ZTZ̋U + Pose2D fusedPose; // 融合結果 + Eigen::Matrix3d fusedCov; // センサ融合後の共分散 pfu->setRefScan(refScan); - // ZTZpfuŁAICPʂƃIhglZ + // センサ融合器pfuで、ICP結果とオドメトリ値を融合する double ratio = pfu->fusePose(&curScan, estPose, odoMotion, lastPose, fusedPose, fusedCov); estPose = fusedPose; cov = fusedCov; - printf("ratio=%g. Pose fused.\n", ratio); // ratio͑މxBmFp + printf("ratio=%g. Pose fused.\n", ratio); // ratioは退化度。確認用 - // Uݐς - Eigen::Matrix3d covL; // ړʂ̋U - CovarianceCalculator::rotateCovariance(lastPose, fusedCov, covL, true); // ړʂ̋Uɕϊ - Eigen::Matrix3d tcov; // ݐό̋U + // 共分散を累積する + Eigen::Matrix3d covL; // 移動量の共分散 + CovarianceCalculator::rotateCovariance(lastPose, fusedCov, covL, true); // 移動量の共分散に変換 + Eigen::Matrix3d tcov; // 累積後の共分散 CovarianceCalculator::accumulateCovariance(lastPose, estPose, totalCov, covL, tcov); totalCov = tcov; } - else { // ICPłȂ΁AIhgɂ\ʒug + else { // ICP成功でなければ、オドメトリによる予測位置を使う estPose = predPose; - pfu->calOdometryCovariance(odoMotion, lastPose, cov); // cov̓IhgU + pfu->calOdometryCovariance(odoMotion, lastPose, cov); // covはオドメトリ共分散だけ } } else { @@ -90,26 +90,26 @@ bool ScanMatcher2D::matchScan(Scan2D &curScan) { estPose = predPose; } - growMap(curScan, estPose); // n}ɃXL_Qlj - prevScan = curScan; // OXL̐ݒ + growMap(curScan, estPose); // 地図にスキャン点群を追加 + prevScan = curScan; // 直前スキャンの設定 - // mFp + // 確認用 // printf("lastPose: tx=%g, ty=%g, th=%g\n", lastPose.tx, lastPose.ty, lastPose.th); - printf("predPose: tx=%g, ty=%g, th=%g\n", predPose.tx, predPose.ty, predPose.th); // mFp + printf("predPose: tx=%g, ty=%g, th=%g\n", predPose.tx, predPose.ty, predPose.th); // 確認用 printf("estPose: tx=%g, ty=%g, th=%g\n", estPose.tx, estPose.ty, estPose.th); printf("cov: %g, %g, %g, %g\n", totalCov(0,0), totalCov(0,1), totalCov(1,0), totalCov(1,1)); printf("mcov: %g, %g, %g, %g\n", pfu->mcov(0,0), pfu->mcov(0,1), pfu->mcov(1,0), pfu->mcov(1,1)); printf("ecov: %g, %g, %g, %g\n", pfu->ecov(0,0), pfu->ecov(0,1), pfu->ecov(1,0), pfu->ecov(1,1)); - // U̕ۑimFpj + // 共分散の保存（確認用） // PoseCov pcov(estPose, cov); // PoseCov pcov(estPose, totalCov); // PoseCov pcov(estPose, pfu->mcov); PoseCov pcov(estPose, pfu->ecov); poseCovs.emplace_back(pcov); - // ݐϑšvZimFpj - Pose2D estMotion; // ړ + // 累積走行距離の計算（確認用） + Pose2D estMotion; // 推定移動量 Pose2D::calRelativePose(estPose, lastPose, estMotion); atd += sqrt(estMotion.tx*estMotion.tx + estMotion.ty*estMotion.ty); printf("atd=%g\n", atd); @@ -119,35 +119,35 @@ bool ScanMatcher2D::matchScan(Scan2D &curScan) { //////////////////// -// ݃XLljāAn}𐬒 +// 現在スキャンを追加して、地図を成長させる void ScanMatcher2D::growMap(const Scan2D &scan, const Pose2D &pose) { - const vector &lps = scan.lps; // XL_Q({bgWn) - const double (*R)[2] = pose.Rmat; // 肵{bgʒu + const vector &lps = scan.lps; // スキャン点群(ロボット座標系) + const double (*R)[2] = pose.Rmat; // 推定したロボット位置 double tx = pose.tx; double ty = pose.ty; - vector scanG; // n}Wnł̓_Q + vector scanG; // 地図座標系での点群 for(size_t i=0; iaddPose(pose); pcmap->addPoints(scanG); pcmap->setLastPose(pose); - pcmap->setLastScan(scan); // QƃXLpɕۑ - pcmap->makeLocalMap(); // Ǐn}𐶐 + pcmap->setLastScan(scan); // 参照スキャン用に保存 + pcmap->makeLocalMap(); // 局所地図を生成 - printf("ScanMatcher: estPose: tx=%g, ty=%g, th=%g\n", pose.tx, pose.ty, pose.th); // mFp + printf("ScanMatcher: estPose: tx=%g, ty=%g, th=%g\n", pose.tx, pose.ty, pose.th); // 確認用 } diff --git a/framework/ScanMatcher2D.h b/framework/ScanMatcher2D.h index 88b5406..a768ef6 100755 --- a/framework/ScanMatcher2D.h +++ b/framework/ScanMatcher2D.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -27,29 +27,29 @@ #include "PoseEstimatorICP.h" #include "PoseFuser.h" -// ICPpăXL}b`Os +// ICPを用いてスキャンマッチングを行う class ScanMatcher2D { private: - int cnt; // _BXLԍɑΉ - Scan2D prevScan; // 1‘ÕXL - Pose2D initPose; // n}̌_̈ʒuBʏ(0,0,0) - - double scthre; // XRA臒lB傫ICPsƂ݂Ȃ - double nthre; // gp_臒lB菬ICPsƂ݂Ȃ - double atd; // ݐϑsBmFp - bool dgcheck; // މ邩 - - PoseEstimatorICP *estim; // {bgʒu - PointCloudMap *pcmap; // _Qn} - ScanPointResampler *spres; // XL_Ԋuψꉻ - ScanPointAnalyser *spana; // XL_@vZ - RefScanMaker *rsm; // QƃXL - PoseFuser *pfu; // ZTZ - Eigen::Matrix3d cov; // {bgړʂ̋Us - Eigen::Matrix3d totalCov; // {bgʒűUs - - std::vector poseCovs; // fobOp + int cnt; // 論理時刻。スキャン番号に対応 + Scan2D prevScan; // 1つ前のスキャン + Pose2D initPose; // 地図の原点の位置。通常(0,0,0) + + double scthre; // スコア閾値。これより大きいとICP失敗とみなす + double nthre; // 使用点数閾値。これより小さいとICP失敗とみなす + double atd; // 累積走行距離。確認用 + bool dgcheck; // 退化処理をするか + + PoseEstimatorICP *estim; // ロボット位置推定器 + PointCloudMap *pcmap; // 点群地図 + ScanPointResampler *spres; // スキャン点間隔均一化 + ScanPointAnalyser *spana; // スキャン点法線計算 + RefScanMaker *rsm; // 参照スキャン生成 + PoseFuser *pfu; // センサ融合器 + Eigen::Matrix3d cov; // ロボット移動量の共分散行列 + Eigen::Matrix3d totalCov; // ロボット位置の共分散行列 + + std::vector poseCovs; // デバッグ用 public: ScanMatcher2D() : cnt(-1), scthre(1.0), nthre(50), dgcheck(false), atd(0), pcmap(nullptr), spres(nullptr), spana(nullptr), estim(nullptr), rsm(nullptr), pfu(nullptr) { @@ -58,7 +58,7 @@ class ScanMatcher2D ~ScanMatcher2D() { } -/////// t[[N̉ӏ //////// +/////// フレームワークの改造箇所 //////// void setPoseEstimator(PoseEstimatorICP *p) { estim = p; @@ -106,7 +106,7 @@ class ScanMatcher2D return(cov); } - // fobOp + // デバッグ用 std::vector &getPoseCovs() { return(poseCovs); } diff --git a/framework/ScanPointAnalyser.cpp b/framework/ScanPointAnalyser.cpp index 7865c64..1c2a811 100755 --- a/framework/ScanPointAnalyser.cpp +++ b/framework/ScanPointAnalyser.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -16,48 +16,48 @@ using namespace std; -const double ScanPointAnalyser::FPDMIN = 0.06; // ScanPointResampler.dthrSƂ炷 +const double ScanPointAnalyser::FPDMIN = 0.06; // ScanPointResampler.dthrSとずらす const double ScanPointAnalyser::FPDMAX = 1.0; /////////// -// XL_̖@xNg߂B܂AAR[iAǗ̏ꍇB +// スキャン点の法線ベクトルを求める。また、直線、コーナ、孤立の場合分けをする。 void ScanPointAnalyser::analysePoints(vector &lps) { for (int i=0; i= costh) { // ̖@sɋ߂ - type = LINE; // Ƃ݂Ȃ + if (flagR) { // 左右両側で法線ベクトルが計算可能 + if (fabs(nL.x*nR.x + nL.y*nR.y) >= costh) { // 両側の法線が平行に近い + type = LINE; // 直線とみなす } - else { // s牓΁AR[i_Ƃ݂Ȃ + else { // 平行から遠ければ、コーナ点とみなす type = CORNER; } - // E̖@xNg̕ + // 左右両側の法線ベクトルの平均 double dx = nL.x+nR.x; double dy = nL.y+nR.y; double L = sqrt(dx*dx + dy*dy); normal.x = dx/L; normal.y = dy/L; } - else { // @xNgƂȂ + else { // 左側しか法線ベクトルがとれなかった type = LINE; normal = nL; } } else { - if (flagR) { // E@xNgƂȂ + if (flagR) { // 右側しか法線ベクトルがとれなかった type = LINE; normal = nR; } - else { // Ƃ@xNgƂȂ - type = ISOLATE; // Ǘ_Ƃ݂Ȃ + else { // 両側とも法線ベクトルがとれなかった + type = ISOLATE; // 孤立点とみなす normal.x = INVALID; normal.y = INVALID; } @@ -68,21 +68,21 @@ void ScanPointAnalyser::analysePoints(vector &lps) { } } - // ړ_cp̗̓_Acpdminȏdmaxȉ̏ꍇɁA@vZB + // 注目点cpの両側の点が、cpからdmin以上dmax以下の場合に、法線を計算する。 bool ScanPointAnalyser::calNormal(int idx, const vector &lps, int dir, Vector2D &normal){ - const LPoint2D &cp = lps[idx]; // ړ_ + const LPoint2D &cp = lps[idx]; // 注目点 for (int i=idx+dir; i>=0 && i=FPDMIN && d<=FPDMAX) { // cplp̋dK؂Ȃ@vZ + if (d>=FPDMIN && d<=FPDMAX) { // cpとlpの距離dが適切なら法線計算 normal.x = dy/d; normal.y = -dx/d; return(true); } - if (d > FPDMAX) // ͂ǂǂ񗣂̂ŁArł߂ + if (d > FPDMAX) // もはやどんどん離れるので、途中でやめる break; } diff --git a/framework/ScanPointAnalyser.h b/framework/ScanPointAnalyser.h index d34931a..1d757fa 100755 --- a/framework/ScanPointAnalyser.h +++ b/framework/ScanPointAnalyser.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -24,11 +24,11 @@ class ScanPointAnalyser { private: - static const double FPDMIN; // אړ_Ƃ̍ŏ[m]B菬ƌ덷傫Ȃ̂Ŗ@vZɎgȂB - static const double FPDMAX; // אړ_Ƃ̍ő勗[m]B傫ƕsAƂ݂ȂĖ@vZɎgȂB - static const int CRTHRE = 45; // @ω臒l[x]B傫ƃR[i_Ƃ݂ȂB + static const double FPDMIN; // 隣接点との最小距離[m]。これより小さいと誤差が大きくなるので法線計算に使わない。 + static const double FPDMAX; // 隣接点との最大距離[m]。これより大きいと不連続とみなして法線計算に使わない。 + static const int CRTHRE = 45; // 法線方向変化の閾値[度]。これより大きいとコーナ点とみなす。 static const int INVALID = -1; - double costh; // E̖@̐HႢ臒l + double costh; // 左右の法線方向の食い違いの閾値 public: ScanPointAnalyser() : costh(cos(DEG2RAD(CRTHRE))) { diff --git a/framework/ScanPointResampler.cpp b/framework/ScanPointResampler.cpp index 615e764..dad73a4 100755 --- a/framework/ScanPointResampler.cpp +++ b/framework/ScanPointResampler.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -19,56 +19,56 @@ using namespace std; ///////// void ScanPointResampler::resamplePoints(Scan2D *scan) { - vector &lps = scan->lps; // XL_Q + vector &lps = scan->lps; // スキャン点群 if (lps.size() == 0) return; - vector newLps; // Tv̓_Q + vector newLps; // リサンプル後の点群 - dis = 0; // dis͗ݐϋ + dis = 0; // disは累積距離 LPoint2D lp = lps[0]; LPoint2D prevLp = lp; LPoint2D np(lp.sid, lp.x, lp.y); - newLps.emplace_back(np); // ŏ̓_͓ + newLps.emplace_back(np); // 最初の点は入れる for (size_t i=1; isetLps(newLps); - printf("lps.size=%lu, newLps.size=%lu\n", lps.size(), newLps.size()); // mFp + printf("lps.size=%lu, newLps.size=%lu\n", lps.size(), newLps.size()); // 確認用 } bool ScanPointResampler::findInterpolatePoint(const LPoint2D &cp, const LPoint2D &pp, LPoint2D &np, bool &inserted) { double dx = cp.x - pp.x; double dy = cp.y - pp.y; - double L = sqrt(dx*dx+dy*dy); // ݓ_cpƒO_pp̋ - if (dis+L < dthreS) { // \ݐϋ(dis+L)dthreS菬_͍폜 - dis += L; // disɉZ + double L = sqrt(dx*dx+dy*dy); // 現在点cpと直前点ppの距離 + if (dis+L < dthreS) { // 予測累積距離(dis+L)がdthreSより小さい点は削除 + dis += L; // disに加算 return(false); } - else if (dis+L >= dthreL) { // \ݐϋdthreL傫_͕ԂÂ܂܎c + else if (dis+L >= dthreL) { // 予測累積距離がdthreLより大きい点は補間せず、そのまま残す np.setData(cp.sid, cp.x, cp.y); } - else { // \ݐϋdthreS𒴂AdthreSɂȂ悤ɕԂ + else { // 予測累積距離がdthreSを超えたら、dthreSになるように補間する double ratio = (dthreS-dis)/L; - double x2 = dx*ratio + pp.x; // L΂ċdthreSɂȂʒu + double x2 = dx*ratio + pp.x; // 少し伸ばして距離がdthreSになる位置 double y2 = dy*ratio + pp.y; np.setData(cp.sid, x2, y2); - inserted = true; // cpOnpꂽƂtO + inserted = true; // cpより前にnpを入れたというフラグ } return(true); diff --git a/framework/ScanPointResampler.h b/framework/ScanPointResampler.h index 5cc6af4..eedfbd9 100755 --- a/framework/ScanPointResampler.h +++ b/framework/ScanPointResampler.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -24,9 +24,9 @@ class ScanPointResampler { private: - double dthreS; // _̋Ԋu[m] - double dthreL; // _̋臒l[m]B̊Ԋu𒴂ԂȂ - double dis; // ݐϋBƗp + double dthreS; // 点の距離間隔[m] + double dthreL; // 点の距離閾値[m]。この間隔を超えたら補間しない + double dis; // 累積距離。作業用 public: ScanPointResampler() : dthreS(0.05), dthreL(0.25), dis(0) { diff --git a/framework/SensorDataReader.cpp b/framework/SensorDataReader.cpp index d253916..d958eb5 100755 --- a/framework/SensorDataReader.cpp +++ b/framework/SensorDataReader.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -16,64 +16,64 @@ using namespace std; -// t@CXL1“ǂ +// ファイルからスキャンを1個読む bool SensorDataReader::loadScan(size_t cnt, Scan2D &scan) { bool isScan=false; - while (!inFile.eof() && !isScan) { // XLǂނ܂ő + while (!inFile.eof() && !isScan) { // スキャンを読むまで続ける isScan = loadLaserScan(cnt, scan); } if (isScan) - return(false); // ܂t@CƂӖ + return(false); // まだファイルが続くという意味 else - return(true); // t@CIƂӖ + return(true); // ファイルが終わったという意味 } ////////////// -// t@C獀1‚ǂށBǂ񂾍ڂXLȂtrueԂB +// ファイルから項目1個を読む。読んだ項目がスキャンならtrueを返す。 bool SensorDataReader::loadLaserScan(size_t cnt, Scan2D &scan) { - string type; // t@C̍ڃx + string type; // ファイル内の項目ラベル inFile >> type; - if (type == "LASERSCAN") { // XL̏ꍇ + if (type == "LASERSCAN") { // スキャンの場合 scan.setSid(cnt); int sid, sec, nsec; - inFile >> sid >> sec >> nsec; // ͎gȂ + inFile >> sid >> sec >> nsec; // これらは使わない vector lps; - int pnum; // XL_ + int pnum; // スキャン点数 inFile >> pnum; lps.reserve(pnum); for (int i=0; i> angle >> range; // XL_̕ʂƋ - angle += angleOffset; // [UXLi̕ItZbgl + inFile >> angle >> range; // スキャン点の方位と距離 + angle += angleOffset; // レーザスキャナの方向オフセットを考慮 if (range <= Scan2D::MIN_SCAN_RANGE || range >= Scan2D::MAX_SCAN_RANGE) { -// if (range <= Scan2D::MIN_SCAN_RANGE || range >= 3.5) { // 킴ƑމN₷ +// if (range <= Scan2D::MIN_SCAN_RANGE || range >= 3.5) { // わざと退化を起こしやすく continue; } LPoint2D lp; - lp.setSid(cnt); // XLԍcntiʂԍjɂ - lp.calXY(range, angle); // angle,range_̈ʒuxyvZ + lp.setSid(cnt); // スキャン番号はcnt（通し番号）にする + lp.calXY(range, angle); // angle,rangeから点の位置xyを計算 lps.emplace_back(lp); } scan.setLps(lps); - // XLɑΉIhg + // スキャンに対応するオドメトリ情報 Pose2D &pose = scan.pose; inFile >> pose.tx >> pose.ty; double th; inFile >> th; - pose.setAngle(RAD2DEG(th)); // Ihgpx̓WAȂ̂œxɂ + pose.setAngle(RAD2DEG(th)); // オドメトリ角度はラジアンなので度にする pose.calRmat(); return(true); } - else { // XLȊȌꍇ + else { // スキャン以外の場合 string line; - getline(inFile, line); // ǂݔ΂ + getline(inFile, line); // 読み飛ばす return(false); } diff --git a/framework/SensorDataReader.h b/framework/SensorDataReader.h index 1b6a39d..dbee10f 100755 --- a/framework/SensorDataReader.h +++ b/framework/SensorDataReader.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -28,8 +28,8 @@ class SensorDataReader { private: - int angleOffset; // [UXLiƃ{bǧ̃ItZbg - std::ifstream inFile; // f[^t@C + int angleOffset; // レーザスキャナとロボットの向きのオフセット + std::ifstream inFile; // データファイル public: SensorDataReader() : angleOffset(180) { diff --git a/framework/SlamBackEnd.cpp b/framework/SlamBackEnd.cpp index 4ebe70c..efd1c22 100755 --- a/framework/SlamBackEnd.cpp +++ b/framework/SlamBackEnd.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -17,7 +17,7 @@ using namespace std; -////////// |[Y ////////// +////////// ポーズ調整 ////////// Pose2D SlamBackEnd::adjustPoses() { // pg->printArcs(); @@ -26,7 +26,7 @@ Pose2D SlamBackEnd::adjustPoses() { newPoses.clear(); P2oDriver2D p2o; - p2o.doP2o(*pg, newPoses, 5); // 5񂭂Ԃ + p2o.doP2o(*pg, newPoses, 5); // 5回くり返す return(newPoses.back()); } @@ -34,15 +34,15 @@ Pose2D SlamBackEnd::adjustPoses() { ///////////////////////////// void SlamBackEnd::remakeMaps() { - // PoseGraph̏C - vector &pnodes = pg->nodes; // |[Ym[h + // PoseGraphの修正 + vector &pnodes = pg->nodes; // ポーズノード for (size_t i=0; isetPose(npose); // em[ḧʒuXV + PoseNode *pnode = pnodes[i]; // ノードはロボット位置と1:1対応 + pnode->setPose(npose); // 各ノードの位置を更新 } printf("newPoses.size=%lu, nodes.size=%lu\n", newPoses.size(), pnodes.size()); - // PointCloudMap̏C + // PointCloudMapの修正 pcmap->remakeMaps(newPoses); } diff --git a/framework/SlamBackEnd.h b/framework/SlamBackEnd.h index c40965b..de70a27 100755 --- a/framework/SlamBackEnd.h +++ b/framework/SlamBackEnd.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -24,9 +24,9 @@ class SlamBackEnd { private: - std::vector newPoses; // |[Y̎p - PointCloudMap *pcmap; // _Qn} - PoseGraph *pg; // |[YOt + std::vector newPoses; // ポーズ調整後の姿勢 + PointCloudMap *pcmap; // 点群地図 + PoseGraph *pg; // ポーズグラフ public: SlamBackEnd() { diff --git a/framework/SlamFrontEnd.cpp b/framework/SlamFrontEnd.cpp index b78b71b..3a9f7d5 100755 --- a/framework/SlamFrontEnd.cpp +++ b/framework/SlamFrontEnd.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -18,7 +18,7 @@ using namespace std; ///////// -// +// 初期化 void SlamFrontEnd::init() { smat->reset(); smat->setPointCloudMap(pcmap); @@ -27,84 +27,84 @@ void SlamFrontEnd::init() { /////////// -// ݃XLscanB +// 現在スキャンscanを処理する。 void SlamFrontEnd::process(Scan2D &scan) { if (cnt == 0) - init(); // Jnɏ + init(); // 開始時に初期化 - // XL}b`O + // スキャンマッチング smat->matchScan(scan); - Pose2D curPose = pcmap->getLastPose(); // ̓XL}b`OŐ肵݂̃{bgʒu + Pose2D curPose = pcmap->getLastPose(); // これはスキャンマッチングで推定した現在のロボット位置 - // |[YOtɃIhgA[Nlj - if (cnt == 0) { // ŏ̓m[huB + // ポーズグラフにオドメトリアークを追加 + if (cnt == 0) { // 最初はノードを置くだけ。 pg->addNode(curPose); } - else { // ̓m[hljāAIhgA[N𒣂 + else { // 次からはノードを追加して、オドメトリアークを張る Eigen::Matrix3d &cov = smat->getCovariance(); makeOdometryArc(curPose, cov); } - if (cnt%keyframeSkip==0) { // L[t[̂Ƃs + if (cnt%keyframeSkip==0) { // キーフレームのときだけ行う if (cnt == 0) - pcmap->setNthre(1); // cnt=0̂Ƃ͒n}̂ŃTvO + pcmap->setNthre(1); // cnt=0のときは地図が小さいのでサンプリング多くする else pcmap->setNthre(5); - pcmap->makeGlobalMap(); // _Qn}̑S̒n}𐶐 + pcmap->makeGlobalMap(); // 点群地図の全体地図を生成 } - // [v‚ - if (cnt > keyframeSkip && cnt%keyframeSkip==0) { // L[t[̂Ƃs - bool flag = lpd->detectLoop(&scan, curPose, cnt); // [voN + // ループ閉じ込み + if (cnt > keyframeSkip && cnt%keyframeSkip==0) { // キーフレームのときだけ行う + bool flag = lpd->detectLoop(&scan, curPose, cnt); // ループ検出を起動 if (flag) { - sback.adjustPoses(); // [v‚|[Y - sback.remakeMaps(); // n}|[YOt̏C + sback.adjustPoses(); // ループが見つかったらポーズ調整 + sback.remakeMaps(); // 地図やポーズグラフの修正 } } - printf("pcmap.size=%lu\n", pcmap->globalMap.size()); // mFp + printf("pcmap.size=%lu\n", pcmap->globalMap.size()); // 確認用 - countLoopArcs(); // mFp + countLoopArcs(); // 確認用 ++cnt; } //////////// -// IhgA[N̐ +// オドメトリアークの生成 bool SlamFrontEnd::makeOdometryArc(Pose2D &curPose, const Eigen::Matrix3d &fusedCov) { - if (pg->nodes.size() == 0) // Ô߂̃`FbN + if (pg->nodes.size() == 0) // 念のためのチェック return(false); - PoseNode *lastNode = pg->nodes.back(); // Om[h - PoseNode *curNode = pg->addNode(curPose); // |[YOtɌ݃m[hlj + PoseNode *lastNode = pg->nodes.back(); // 直前ノード + PoseNode *curNode = pg->addNode(curPose); // ポーズグラフに現在ノードを追加 - // Om[hƌ݃m[h̊ԂɃIhgA[N𒣂 + // 直前ノードと現在ノードの間にオドメトリアークを張る Pose2D &lastPose = lastNode->pose; Pose2D relPose; - Pose2D::calRelativePose(curPose, lastPose, relPose); // ݈ʒuƒOʒȗΈʒuiړʁǰvZ + Pose2D::calRelativePose(curPose, lastPose, relPose); // 現在位置と直前位置の相対位置（移動量）の計算 printf("sfront: lastPose: tx=%g, ty=%g, th=%g\n", lastPose.tx, lastPose.ty, lastPose.th); Eigen::Matrix3d cov; - CovarianceCalculator::rotateCovariance(lastPose, fusedCov, cov, true); // ړʂ̋Uɕϊ - PoseArc *arc = pg->makeArc(lastNode->nid, curNode->nid, relPose, cov); // A[N̐ - pg->addArc(arc); // |[YOtɃA[Nlj + CovarianceCalculator::rotateCovariance(lastPose, fusedCov, cov, true); // 移動量の共分散に変換 + PoseArc *arc = pg->makeArc(lastNode->nid, curNode->nid, relPose, cov); // アークの生成 + pg->addArc(arc); // ポーズグラフにアークを追加 return(true); } //////////// -// [vA[N𐔂BmFp +// ループアーク数を数える。確認用 void SlamFrontEnd::countLoopArcs() { - vector &parcs = pg->arcs; // S|[YA[N - int an=0; // [vA[N + vector &parcs = pg->arcs; // 全ポーズアーク + int an=0; // ループアーク数 for (size_t i=0; isrc; PoseNode *dst = a->dst; - if (src->nid != dst->nid-1) // IhgA[N͎n_ƏI_AԂɂȂĂ - ++an; // IhgA[NłȂ΃[vA[N + if (src->nid != dst->nid-1) // オドメトリアークは始点と終点が連番になっている + ++an; // オドメトリアークでなければループアーク } - printf("loopArcs.size=%d\n", an); // mFp + printf("loopArcs.size=%d\n", an); // 確認用 } diff --git a/framework/SlamFrontEnd.h b/framework/SlamFrontEnd.h index 7f942be..3f47df2 100755 --- a/framework/SlamFrontEnd.h +++ b/framework/SlamFrontEnd.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -25,18 +25,18 @@ //////// -// SLAMtgGhB{bgʒuAn}A[v‚݂d؂B +// SLAMフロントエンド。ロボット位置推定、地図生成、ループ閉じ込みを取り仕切る。 class SlamFrontEnd { private: - int cnt; // _ - int keyframeSkip; // L[t[Ԋu + int cnt; // 論理時刻 + int keyframeSkip; // キーフレーム間隔 - PointCloudMap *pcmap; // _Qn} - PoseGraph *pg; // |[YOt - ScanMatcher2D *smat; // XL}b`O - LoopDetector *lpd; // [vo - SlamBackEnd sback; // SLAMobNGh + PointCloudMap *pcmap; // 点群地図 + PoseGraph *pg; // ポーズグラフ + ScanMatcher2D *smat; // スキャンマッチング + LoopDetector *lpd; // ループ検出器 + SlamBackEnd sback; // SLAMバックエンド public: SlamFrontEnd() : cnt(0), keyframeSkip(10), smat(nullptr), lpd(nullptr) { @@ -83,12 +83,12 @@ class SlamFrontEnd smat->setDgCheck(p); } - // fobOp + // デバッグ用 std::vector &getLoopMatches() { return(lpd->getLoopMatches()); } - // fobOp + // デバッグ用 std::vector &getPoseCovs() { return(smat->getPoseCovs()); } diff --git a/hook/CostFunctionED.cpp b/hook/CostFunctionED.cpp index c1c3240..dc708b0 100755 --- a/hook/CostFunctionED.cpp +++ b/hook/CostFunctionED.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -16,37 +16,37 @@ using namespace std; -// _ԋɂICP̃RXg֐ +// 点間距離によるICPのコスト関数 double CostFunctionED::calValue(double tx, double ty, double th) { double a = DEG2RAD(th); double error=0; int pn=0; int nn=0; for (size_t i=0; ix; double cy = clp->y; - double x = cos(a)*cx - sin(a)*cy + tx; // clpQƃXL̍Wnɕϊ + double x = cos(a)*cx - sin(a)*cy + tx; // clpを参照スキャンの座標系に変換 double y = sin(a)*cx + cos(a)*cy + ty; - double edis = (x - rlp->x)*(x - rlp->x) + (y - rlp->y)*(y - rlp->y); // _ԋ + double edis = (x - rlp->x)*(x - rlp->x) + (y - rlp->y)*(y - rlp->y); // 点間距離 if (edis <= evlimit*evlimit) - ++pn; // 덷_̐ + ++pn; // 誤差が小さい点の数 - error += edis; // e_̌덷ݐ + error += edis; // 各点の誤差を累積 ++nn; } - error = (nn>0)? error/nn : HUGE_VAL; // ςƂBL_0ȂAlHUGE_VAL - pnrate = 1.0*pn/nn; // 덷_̔䗦 + error = (nn>0)? error/nn : HUGE_VAL; // 平均をとる。有効点数が0なら、値はHUGE_VAL + pnrate = 1.0*pn/nn; // 誤差が小さい点の比率 -// printf("CostFunctionED: error=%g, pnrate=%g, evlimit=%g\n", error, pnrate, evlimit); // mFp +// printf("CostFunctionED: error=%g, pnrate=%g, evlimit=%g\n", error, pnrate, evlimit); // 確認用 - error *= 100; // ]lȂ肷Ȃ悤100B + error *= 100; // 評価値が小さくなりすぎないよう100かける。 return(error); } diff --git a/hook/CostFunctionED.h b/hook/CostFunctionED.h index bab412f..8ba0144 100755 --- a/hook/CostFunctionED.h +++ b/hook/CostFunctionED.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), diff --git a/hook/CostFunctionPD.cpp b/hook/CostFunctionPD.cpp index 364b1ef..11ebfe1 100755 --- a/hook/CostFunctionPD.cpp +++ b/hook/CostFunctionPD.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -16,7 +16,7 @@ using namespace std; -// ɂRXg֐ +// 垂直距離によるコスト関数 double CostFunctionPD::calValue(double tx, double ty, double th) { double a = DEG2RAD(th); @@ -24,33 +24,33 @@ double CostFunctionPD::calValue(double tx, double ty, double th) { int pn=0; int nn=0; for (size_t i=0; itype != LINE) // ̓_łȂΎgȂ + if (rlp->type != LINE) // 直線上の点でなければ使わない continue; double cx = clp->x; double cy = clp->y; - double x = cos(a)*cx - sin(a)*cy + tx; // clpQƃXL̍Wnɕϊ + double x = cos(a)*cx - sin(a)*cy + tx; // clpを参照スキャンの座標系に変換 double y = sin(a)*cx + cos(a)*cy + ty; - double pdis = (x - rlp->x)*rlp->nx + (y - rlp->y)*rlp->ny; // + double pdis = (x - rlp->x)*rlp->nx + (y - rlp->y)*rlp->ny; // 垂直距離 double er = pdis*pdis; if (er <= evlimit*evlimit) - ++pn; // 덷_̐ + ++pn; // 誤差が小さい点の数 - error += er; // e_̌덷ݐ + error += er; // 各点の誤差を累積 ++nn; } - error = (nn>0)? error/nn : HUGE_VAL; // L_0ȂAlHUGE_VAL - pnrate = 1.0*pn/nn; // 덷_̔䗦 + error = (nn>0)? error/nn : HUGE_VAL; // 有効点数が0なら、値はHUGE_VAL + pnrate = 1.0*pn/nn; // 誤差が小さい点の比率 -// printf("CostFunctionPD: error=%g, pnrate=%g, evlimit=%g\n", error, pnrate, evlimit); // mFp +// printf("CostFunctionPD: error=%g, pnrate=%g, evlimit=%g\n", error, pnrate, evlimit); // 確認用 - error *= 100; // ]lȂ肷Ȃ悤100B + error *= 100; // 評価値が小さくなりすぎないよう100かける。 return(error); } diff --git a/hook/CostFunctionPD.h b/hook/CostFunctionPD.h index c66e66b..287bcd9 100755 --- a/hook/CostFunctionPD.h +++ b/hook/CostFunctionPD.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), diff --git a/hook/DataAssociatorGT.cpp b/hook/DataAssociatorGT.cpp index 83910b2..57861a0 100755 --- a/hook/DataAssociatorGT.cpp +++ b/hook/DataAssociatorGT.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -18,28 +18,28 @@ using namespace std; -// ݃XLcurScan̊eXL_predPoseōWϊʒuɍł߂_‚ +// 現在スキャンcurScanの各スキャン点をpredPoseで座標変換した位置に最も近い点を見つける double DataAssociatorGT::findCorrespondence(const Scan2D *curScan, const Pose2D &predPose) { - boost::timer tim; // ԑp + boost::timer tim; // 処理時間測定用 - curLps.clear(); // ΉÂ݃XL_Qɂ - refLps.clear(); // ΉÂQƃXL_Qɂ + curLps.clear(); // 対応づけ現在スキャン点群を空にする + refLps.clear(); // 対応づけ参照スキャン点群を空にする for (size_t i=0; ilps.size(); i++) { - const LPoint2D *clp = &(curScan->lps[i]); // ݃XL̓_B|C^ŁB + const LPoint2D *clp = &(curScan->lps[i]); // 現在スキャンの点。ポインタで。 - // iqe[uɂŋߖT_߂Biqe[uɋ臒ldthre邱ƂɒӁB + // 格子テーブルにより最近傍点を求める。格子テーブル内に距離閾値dthreがあることに注意。 const LPoint2D *rlp = nntab.findClosestPoint(clp, predPose); if (rlp != nullptr) { - curLps.push_back(clp); // ŋߖT_Γo^ + curLps.push_back(clp); // 最近傍点があれば登録 refLps.push_back(rlp); } } - double ratio = (1.0*curLps.size())/curScan->lps.size(); // ΉƂꂽ_̔䗦 + double ratio = (1.0*curLps.size())/curScan->lps.size(); // 対応がとれた点の比率 -// double t1 = 1000*tim.elapsed(); // +// double t1 = 1000*tim.elapsed(); // 処理時間 // printf("Elapsed time: dassGT=%g\n", t1); return(ratio); diff --git a/hook/DataAssociatorGT.h b/hook/DataAssociatorGT.h index a4789c9..651cf00 100755 --- a/hook/DataAssociatorGT.h +++ b/hook/DataAssociatorGT.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -18,11 +18,11 @@ #include "DataAssociator.h" #include "NNGridTable.h" -// iqe[upāA݃XLƎQƃXLԂ̓_̑ΉÂs +// 格子テーブルを用いて、現在スキャンと参照スキャン間の点の対応づけを行う class DataAssociatorGT : public DataAssociator { private: - NNGridTable nntab; // iqe[u + NNGridTable nntab; // 格子テーブル public: DataAssociatorGT() { @@ -31,11 +31,11 @@ class DataAssociatorGT : public DataAssociator ~DataAssociatorGT() { } - // QƃXL̓_rlps|C^ɂnntabɓ + // 参照スキャンの点rlpsをポインタにしてnntabに入れる virtual void setRefBase(const std::vector &rlps) { nntab.clear(); for (size_t i=0; ilps.size(); i++) { - const LPoint2D *clp = &(curScan->lps[i]); // ݃XL̓_B|C^ŁB + const LPoint2D *clp = &(curScan->lps[i]); // 現在スキャンの点。ポインタで。 - // XL_lppredPoseōWϊʒuɍł߂_‚ - LPoint2D glp; // clp̗\ʒu - predPose.globalPoint(*clp, glp); // predPoseōWϊ + // スキャン点lpをpredPoseで座標変換した位置に最も近い点を見つける + LPoint2D glp; // clpの予測位置 + predPose.globalPoint(*clp, glp); // predPoseで座標変換 - double dmin = HUGE_VAL; // ŏl - const LPoint2D *rlpmin = nullptr; // ł߂_ + double dmin = HUGE_VAL; // 距離最小値 + const LPoint2D *rlpmin = nullptr; // 最も近い点 for (size_t j=0; jx)*(glp.x - rlp->x) + (glp.y - rlp->y)*(glp.y - rlp->y); - if (d <= dthre*dthre && d < dmin) { // dthreŋŏƂȂ_ۑ + if (d <= dthre*dthre && d < dmin) { // dthre内で距離が最小となる点を保存 dmin = d; rlpmin = rlp; } } - if (rlpmin != nullptr) { // ŋߖT_Γo^ + if (rlpmin != nullptr) { // 最近傍点があれば登録 curLps.push_back(clp); refLps.push_back(rlpmin); } } - double ratio = (1.0*curLps.size())/curScan->lps.size(); // ΉƂꂽ_̔䗦 + double ratio = (1.0*curLps.size())/curScan->lps.size(); // 対応がとれた点の比率 // printf("ratio=%g, clps.size=%lu\n", ratio, curScan->lps.size()); -// double t1 = 1000*tim.elapsed(); // +// double t1 = 1000*tim.elapsed(); // 処理時間 // printf("Elapsed time: dassLS=%g\n", t1); return(ratio); diff --git a/hook/DataAssociatorLS.h b/hook/DataAssociatorLS.h index 61f0c7e..843805a 100755 --- a/hook/DataAssociatorLS.h +++ b/hook/DataAssociatorLS.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -17,11 +17,11 @@ #include "DataAssociator.h" -// `TpāA݃XLƎQƃXLԂ̓_̑ΉÂs +// 線形探索を用いて、現在スキャンと参照スキャン間の点の対応づけを行う class DataAssociatorLS : public DataAssociator { private: - std::vector baseLps; // QƃXL̓_i[ĂBƗp + std::vector baseLps; // 参照スキャンの点を格納しておく。作業用 public: DataAssociatorLS() { @@ -30,11 +30,11 @@ class DataAssociatorLS : public DataAssociator ~DataAssociatorLS() { } - // QƃXL̓_rlps|C^ɂbaseLpsɓ + // 参照スキャンの点rlpsをポインタにしてbaseLpsに入れる virtual void setRefBase(const std::vector &rlps) { baseLps.clear(); for (size_t i=0; iatd; // ݂̎ۂ̗ݐϑs - double atdR = 0; // L̏ŋOՂȂ鎞̗ݐϑs - const vector &submaps = pcmap->submaps; // n} - const vector &poses = pcmap->poses; // {bgO - double dmin=HUGE_VAL; // OK_܂ł̋̍ŏl - size_t imin=0, jmin=0; // ŏ̑OK_̃CfbNX - Pose2D prevP; // Õ{bgʒu - for (size_t i=0; iatd; // 現在の実際の累積走行距離 + double atdR = 0; // 下記の処理で軌跡をなぞる時の累積走行距離 + const vector &submaps = pcmap->submaps; // 部分地図 + const vector &poses = pcmap->poses; // ロボット軌跡 + double dmin=HUGE_VAL; // 前回訪問点までの距離の最小値 + size_t imin=0, jmin=0; // 距離最小の前回訪問点のインデックス + Pose2D prevP; // 直前のロボット位置 + for (size_t i=0; i radius*radius) // OK_܂ł̋ƃ[voȂ + if (dmin > radius*radius) // 前回訪問点までの距離が遠いとループ検出しない return(false); - Submap &refSubmap = pcmap->submaps[imin]; // ł߂n}QƃXLɂ + Submap &refSubmap = pcmap->submaps[imin]; // 最も近い部分地図を参照スキャンにする const Pose2D &initPose = poses[jmin]; printf("curPose: tx=%g, ty=%g, th=%g\n", curPose.tx, curPose.ty, curPose.th); printf("initPose: tx=%g, ty=%g, th=%g\n", initPose.tx, initPose.ty, initPose.th); - // ĖK_̈ʒu߂ + // 再訪点の位置を求める Pose2D revisitPose; bool flag = estimateRevisitPose(curScan, refSubmap.mps, curPose, revisitPose); // bool flag = estimateRelativePose(curScan, refSubmap.mps, initPose, revisitPose); - if (flag) { // [vo - Eigen::Matrix3d icpCov; // ICP̋U - double ratio = pfu->calIcpCovariance(revisitPose, curScan, icpCov); // ICP̋UvZ + if (flag) { // ループを検出した + Eigen::Matrix3d icpCov; // ICPの共分散 + double ratio = pfu->calIcpCovariance(revisitPose, curScan, icpCov); // ICPの共分散を計算 - LoopInfo info; // [vo - info.pose = revisitPose; // [vA[NɍĖK_ʒuݒ - info.cov = icpCov; // [vA[NɋUݒB - info.curId = cnt; // ݈ʒũm[hid - info.refId = static_cast(jmin); // OK_̃m[hid - makeLoopArc(info); // [vA[N + LoopInfo info; // ループ検出結果 + info.pose = revisitPose; // ループアーク情報に再訪点位置を設定 + info.cov = icpCov; // ループアーク情報に共分散を設定。 + info.curId = cnt; // 現在位置のノードid + info.refId = static_cast(jmin); // 前回訪問点のノードid + makeLoopArc(info); // ループアーク生成 - // mFp + // 確認用 Scan2D refScan; Pose2D spose = poses[refSubmap.cntS]; refScan.setSid(info.refId); @@ -94,25 +94,25 @@ bool LoopDetectorSS::detectLoop(Scan2D *curScan, Pose2D &curPose, int cnt) { ////////// -// OK_(refId)n_m[hA݈ʒu(curId)I_m[hɂāA[vA[N𐶐B +// 前回訪問点(refId)を始点ノード、現在位置(curId)を終点ノードにして、ループアークを生成する。 void LoopDetectorSS::makeLoopArc(LoopInfo &info) { - if (info.arcked) // infõA[N͂łɒĂ + if (info.arcked) // infoのアークはすでに張ってある return; info.setArcked(true); - Pose2D srcPose = pcmap->poses[info.refId]; // OK_̈ʒu - Pose2D dstPose(info.pose.tx, info.pose.ty, info.pose.th); // ĖK_̈ʒu + Pose2D srcPose = pcmap->poses[info.refId]; // 前回訪問点の位置 + Pose2D dstPose(info.pose.tx, info.pose.ty, info.pose.th); // 再訪点の位置 Pose2D relPose; - Pose2D::calRelativePose(dstPose, srcPose, relPose); // [vA[N̍S + Pose2D::calRelativePose(dstPose, srcPose, relPose); // ループアークの拘束 - // A[N̍S͎n_m[h̑ΈʒuȂ̂ŁAU[vA[N̎n_m[hWnɕϊ + // アークの拘束は始点ノードからの相対位置なので、共分散をループアークの始点ノード座標系に変換 Eigen::Matrix3d cov; - CovarianceCalculator::rotateCovariance(srcPose, info.cov, cov, true); // Űt] + CovarianceCalculator::rotateCovariance(srcPose, info.cov, cov, true); // 共分散の逆回転 - PoseArc *arc = pg->makeArc(info.refId, info.curId, relPose, cov); // [vA[N - pg->addArc(arc); // [vA[No^ + PoseArc *arc = pg->makeArc(info.refId, info.curId, relPose, cov); // ループアーク生成 + pg->addArc(arc); // ループアーク登録 - // mFp + // 確認用 printf("makeLoopArc: pose arc added\n"); printf("srcPose: tx=%g, ty=%g, th=%g\n", srcPose.tx, srcPose.ty, srcPose.th); printf("dstPose: tx=%g, ty=%g, th=%g\n", dstPose.tx, dstPose.ty, dstPose.th); @@ -126,77 +126,77 @@ void LoopDetectorSS::makeLoopArc(LoopInfo &info) { ////////// -// ݃XLcurScanƕn}̓_QrefLpsICPsAĖK_̈ʒu߂B +// 現在スキャンcurScanと部分地図の点群refLpsでICPを行い、再訪点の位置を求める。 bool LoopDetectorSS::estimateRevisitPose(const Scan2D *curScan, const vector &refLps, const Pose2D &initPose, Pose2D &revisitPose) { - dass->setRefBase(refLps); // f[^ΉÂɎQƓ_Qݒ - cfunc->setEvlimit(0.2); // RXg֐̌덷臒l + dass->setRefBase(refLps); // データ対応づけ器に参照点群を設定 + cfunc->setEvlimit(0.2); // コスト関数の誤差閾値 - printf("initPose: tx=%g, ty=%g, th=%g\n", initPose.tx, initPose.ty, initPose.th); // mFp + printf("initPose: tx=%g, ty=%g, th=%g\n", initPose.tx, initPose.ty, initPose.th); // 確認用 size_t usedNumMin = 50; // size_t usedNumMin = 100; - // ʒuinitPose݂̎͂‚ԂɒׂB - // ̂߁AICP͍s킸AeʒuŒPɃ}b`OXRA𒲂ׂB - double rangeT = 1; // i̒T͈[m] - double rangeA = 45; // ]̒T͈[x] - double dd = 0.2; // i̒TԊu[m] - double da = 2; // ]̒TԊu[x] + // 初期位置initPoseの周囲をしらみつぶしに調べる。 + // 効率化のため、ICPは行わず、各位置で単純にマッチングスコアを調べる。 + double rangeT = 1; // 並進の探索範囲[m] + double rangeA = 45; // 回転の探索範囲[度] + double dd = 0.2; // 並進の探索間隔[m] + double da = 2; // 回転の探索間隔[度] double pnrateMax=0; vector pnrates; double scoreMin=1000; vector scores; - vector candidates; // XRÂ悢ʒu - for (double dy=-rangeT; dy<=rangeT; dy+=dd) { // iy̒TJԂ - double y = initPose.ty + dy; // ʒuɕψʕdy - for (double dx=-rangeT; dx<=rangeT; dx+=dd) { // ix̒TJԂ - double x = initPose.tx + dx; // ʒuɕψʕdx - for (double dth=-rangeA; dth<=rangeA; dth+=da) { // ]̒TJԂ - double th = MyUtil::add(initPose.th, dth); // ʒuɕψʕdth + vector candidates; // スコアのよい候補位置 + for (double dy=-rangeT; dy<=rangeT; dy+=dd) { // 並進yの探索繰り返し + double y = initPose.ty + dy; // 初期位置に変位分dyを加える + for (double dx=-rangeT; dx<=rangeT; dx+=dd) { // 並進xの探索繰り返し + double x = initPose.tx + dx; // 初期位置に変位分dxを加える + for (double dth=-rangeA; dth<=rangeA; dth+=da) { // 回転の探索繰り返し + double th = MyUtil::add(initPose.th, dth); // 初期位置に変位分dthを加える Pose2D pose(x, y, th); - double mratio = dass->findCorrespondence(curScan, pose); // ʒuposeŃf[^ΉÂ + double mratio = dass->findCorrespondence(curScan, pose); // 位置poseでデータ対応づけ size_t usedNum = dass->curLps.size(); -// printf("usedNum=%lu, mratio=%g\n", usedNum, mratio); // mFp - if (usedNum < usedNumMin || mratio < 0.9) // ΉƔ΂ +// printf("usedNum=%lu, mratio=%g\n", usedNum, mratio); // 確認用 + if (usedNum < usedNumMin || mratio < 0.9) // 対応率が悪いと飛ばす continue; - cfunc->setPoints(dass->curLps, dass->refLps); // RXg֐ɓ_Qݒ - double score = cfunc->calValue(x, y, th); // RXgli}b`OXRAj - double pnrate = cfunc->getPnrate(); // ڍׂȓ_̑Ή -// printf("score=%g, pnrate=%g\n", score, pnrate); // mFp + cfunc->setPoints(dass->curLps, dass->refLps); // コスト関数に点群を設定 + double score = cfunc->calValue(x, y, th); // コスト値（マッチングスコア） + double pnrate = cfunc->getPnrate(); // 詳細な点の対応率 +// printf("score=%g, pnrate=%g\n", score, pnrate); // 確認用 if (pnrate > 0.8) { candidates.emplace_back(pose); if (score < scoreMin) scoreMin = score; scores.push_back(score); -// printf("pose: tx=%g, ty=%g, th=%g\n", pose.tx, pose.ty, pose.th); // mFp -// printf("score=%g, pnrate=%g\n", score, pnrate); // mFp +// printf("pose: tx=%g, ty=%g, th=%g\n", pose.tx, pose.ty, pose.th); // 確認用 +// printf("score=%g, pnrate=%g\n", score, pnrate); // 確認用 } } } } - printf("candidates.size=%lu\n", candidates.size()); // mFp + printf("candidates.size=%lu\n", candidates.size()); // 確認用 if (candidates.size() == 0) return(false); - // ʒucandidates̒ł悢̂ICPőI - Pose2D best; // ŗnj - double smin=1000000; // ICPXRAŏl - estim->setScanPair(curScan, refLps); // ICPɃXLݒ + // 候補位置candidatesの中から最もよいものをICPで選ぶ + Pose2D best; // 最良候補 + double smin=1000000; // ICPスコア最小値 + estim->setScanPair(curScan, refLps); // ICPにスキャン設定 for (size_t i=0; iestimatePose(p, estP); // ICPŃ}b`Oʒu߂ - double pnrate = estim->getPnrate(); // ICPł̓_̑Ή - size_t usedNum = estim->getUsedNum(); // ICPŎgp_ - if (score < smin && pnrate >= 0.9 && usedNum >= usedNumMin) { // [vo͏ + double score = estim->estimatePose(p, estP); // ICPでマッチング位置を求める + double pnrate = estim->getPnrate(); // ICPでの点の対応率 + size_t usedNum = estim->getUsedNum(); // ICPで使用した点数 + if (score < smin && pnrate >= 0.9 && usedNum >= usedNumMin) { // ループ検出は条件厳しく smin = score; best = estP; - printf("smin=%g, pnrate=%g, usedNum=%lu\n", smin, pnrate, usedNum); // mFp + printf("smin=%g, pnrate=%g, usedNum=%lu\n", smin, pnrate, usedNum); // 確認用 } } - // ŏXRA臒l菬Ό‚ + // 最小スコアが閾値より小さければ見つけた if (smin <= scthre) { revisitPose = best; return(true); diff --git a/hook/LoopDetectorSS.h b/hook/LoopDetectorSS.h index 9d3d660..633cd94 100755 --- a/hook/LoopDetectorSS.h +++ b/hook/LoopDetectorSS.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -27,15 +27,15 @@ class LoopDetectorSS : public LoopDetector { private: - double radius; // Ta[m]i݈ʒuƍĖK_̋臒lj - double atdthre; // ݐϑs̍臒l[m] - double scthre; // ICPXRA臒l - - PointCloudMapLP *pcmap; // _Qn} - CostFunction *cfunc; // RXg֐(ICPƂ͕ʂɎg) - PoseEstimatorICP *estim; // {bgʒu(ICP) - DataAssociator *dass; // f[^ΉÂ - PoseFuser *pfu; // ZTZ + double radius; // 探索半径[m]（現在位置と再訪点の距離閾値） + double atdthre; // 累積走行距離の差の閾値[m] + double scthre; // ICPスコアの閾値 + + PointCloudMapLP *pcmap; // 点群地図 + CostFunction *cfunc; // コスト関数(ICPとは別に使う) + PoseEstimatorICP *estim; // ロボット位置推定器(ICP) + DataAssociator *dass; // データ対応づけ器 + PoseFuser *pfu; // センサ融合器 public: LoopDetectorSS() : radius(4), atdthre(10), scthre(0.2) { diff --git a/hook/PointCloudMapBS.cpp b/hook/PointCloudMapBS.cpp index ec10b19..5fb0b5f 100755 --- a/hook/PointCloudMapBS.cpp +++ b/hook/PointCloudMapBS.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -18,32 +18,32 @@ using namespace std; /////////// -// {bgʒu̒lj +// ロボット位置の追加 void PointCloudMapBS::addPose(const Pose2D &p) { poses.emplace_back(p); } -// XL_Q̒lj +// スキャン点群の追加 void PointCloudMapBS::addPoints(const vector &lps) { - int skip=5; // ɏd̂ŁA1/5ɊԈ -// int skip=10; // ɏd̂ŁA1/10ɊԈ + int skip=5; // さすがに重いので、1/5に間引く +// int skip=10; // さすがに重いので、1/10に間引く for (size_t i=0; i &newPoses) { } diff --git a/hook/PointCloudMapBS.h b/hook/PointCloudMapBS.h index 8a3b23d..6462e21 100755 --- a/hook/PointCloudMapBS.h +++ b/hook/PointCloudMapBS.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -17,7 +17,7 @@ #include "PointCloudMap.h" -// XL_ׂĕۑ_Qn} +// スキャン点をすべて保存する点群地図 class PointCloudMapBS : public PointCloudMap { public: diff --git a/hook/PointCloudMapGT.cpp b/hook/PointCloudMapGT.cpp index 5d19814..60dd5f8 100755 --- a/hook/PointCloudMapGT.cpp +++ b/hook/PointCloudMapGT.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -18,39 +18,39 @@ using namespace std; /////////// -// {bgʒu̒lj +// ロボット位置の追加 void PointCloudMapGT::addPose(const Pose2D &p) { poses.emplace_back(p); } -// iqe[ůeZ̑\_߂spsɊi[ +// 格子テーブルの各セルの代表点を求めてspsに格納する void PointCloudMapGT::subsamplePoints(vector &sps) { - nntab.clear(); // iqe[ȕ + nntab.clear(); // 格子テーブルの初期化 for (size_t i=0; i &lps) { for (size_t i=0; i &newPoses) { } diff --git a/hook/PointCloudMapGT.h b/hook/PointCloudMapGT.h index f0614a3..5e3c09d 100755 --- a/hook/PointCloudMapGT.h +++ b/hook/PointCloudMapGT.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -21,16 +21,16 @@ /////////// -// iqe[up_Qn} +// 格子テーブルを用いた点群地図 class PointCloudMapGT : public PointCloudMap { public: - std::vector allLps; // SXL_Q - NNGridTable nntab; // iqe[u + std::vector allLps; // 全スキャン点群 + NNGridTable nntab; // 格子テーブル public: PointCloudMapGT() { - allLps.reserve(MAX_POINT_NUM); // ŏɊm + allLps.reserve(MAX_POINT_NUM); // 最初に確保 } ~PointCloudMapGT() { diff --git a/hook/PointCloudMapLP.cpp b/hook/PointCloudMapLP.cpp index 0cbd846..fb2bc67 100755 --- a/hook/PointCloudMapLP.cpp +++ b/hook/PointCloudMapLP.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -21,16 +21,16 @@ double PointCloudMapLP::atdThre = 10; /////////// -// iqe[upāAn}̑\_𓾂 +// 格子テーブルを用いて、部分地図の代表点を得る vector Submap::subsamplePoints(int nthre) { - NNGridTable nntab; // iqe[u + NNGridTable nntab; // 格子テーブル for (size_t i=0; i sps; - nntab.makeCellPoints(nthre, sps); // nthreˆȏ̃Z̑\_spsɓ + nntab.makeCellPoints(nthre, sps); // nthre個以上のセルの代表点をspsに入れる printf("mps.size=%lu, sps.size=%lu\n", mps.size(), sps.size()); return(sps); @@ -38,9 +38,9 @@ vector Submap::subsamplePoints(int nthre) { ///////// -// {bgʒu̒lj +// ロボット位置の追加 void PointCloudMapLP::addPose(const Pose2D &p) { - // ݐϑs(atd)̌vZ + // 累積走行距離(atd)の計算 if (poses.size() > 0) { Pose2D pp = poses.back(); atd += sqrt((p.tx - pp.tx)*(p.tx - pp.tx) + (p.ty - pp.ty)*(p.ty - pp.ty)); @@ -52,109 +52,109 @@ void PointCloudMapLP::addPose(const Pose2D &p) { poses.emplace_back(p); } -// XL_̒lj +// スキャン点の追加 void PointCloudMapLP::addPoints(const vector &lps) { - Submap &curSubmap = submaps.back(); // ݂̕n} - if (atd - curSubmap.atdS >= atdThre ) { // ݐϑs臒l𒴂Vn}ɕς + Submap &curSubmap = submaps.back(); // 現在の部分地図 + if (atd - curSubmap.atdS >= atdThre ) { // 累積走行距離が閾値を超えたら新しい部分地図に変える size_t size = poses.size(); - curSubmap.cntE = size-1; // n}̍Ō̃XLԍ - curSubmap.mps = curSubmap.subsamplePoints(nthre); // In}͑\_݂̂ɂiyʉj + curSubmap.cntE = size-1; // 部分地図の最後のスキャン番号 + curSubmap.mps = curSubmap.subsamplePoints(nthre); // 終了した部分地図は代表点のみにする（軽量化） - Submap submap(atd, size); // Vn} - submap.addPoints(lps); // XL_Q̓o^ - submaps.emplace_back(submap); // n}lj + Submap submap(atd, size); // 新しい部分地図 + submap.addPoints(lps); // スキャン点群の登録 + submaps.emplace_back(submap); // 部分地図を追加 } else { - curSubmap.addPoints(lps); // ݂̕n}ɓ_Qlj + curSubmap.addPoints(lps); // 現在の部分地図に点群を追加 } } -// S̒n}̐BǏn}łɍ +// 全体地図の生成。局所地図もここでいっしょに作った方が速い void PointCloudMapLP::makeGlobalMap(){ - globalMap.clear(); // + globalMap.clear(); // 初期化 localMap.clear(); - // ݈ȊÔłɊm肵n}_W߂ + // 現在以外のすでに確定した部分地図から点を集める for (size_t i=0; i &mps = submap.mps; // n}̓_QB\_ɂȂĂ + Submap &submap = submaps[i]; // 部分地図 + vector &mps = submap.mps; // 部分地図の点群。代表点だけになっている for (size_t j=0; j sps = curSubmap.subsamplePoints(nthre); // \_𓾂 + // 現在の部分地図の代表点を全体地図と局所地図に入れる + Submap &curSubmap = submaps.back(); // 現在の部分地図 + vector sps = curSubmap.subsamplePoints(nthre); // 代表点を得る for (size_t i=0; i= 2) { - Submap &submap = submaps[submaps.size()-2]; // O̕n}g - vector &mps = submap.mps; // n}̓_QB\_ɂȂĂ + Submap &submap = submaps[submaps.size()-2]; // 直前の部分地図だけ使う + vector &mps = submap.mps; // 部分地図の点群。代表点だけになっている for (size_t i=0; i sps = curSubmap.subsamplePoints(nthre); // \_𓾂 + // 現在の部分地図の代表点を局所地図に入れる + Submap &curSubmap = submaps.back(); // 現在の部分地図 + vector sps = curSubmap.subsamplePoints(nthre); // 代表点を得る for (size_t i=0; i &newPoses){ - // en}̓_̈ʒuC + // 各部分地図内の点の位置を修正する for (size_t i=0; i &mps = submap.mps; // n}̓_QBݒn}ȊO͑\_ɂȂĂ + vector &mps = submap.mps; // 部分地図の点群。現在地図以外は代表点になっている for (size_t j=0; j= poses.size()) { // sȃXLԍioOj + size_t idx = mp.sid; // 点のスキャン番号 + if (idx >= poses.size()) { // 不正なスキャン番号（あったらバグ） continue; } - const Pose2D &oldPose = poses[idx]; // mpɑΉÂ{bgʒu - const Pose2D &newPose = newPoses[idx]; // mpɑΉV{bgʒu + const Pose2D &oldPose = poses[idx]; // mpに対応する古いロボット位置 + const Pose2D &newPose = newPoses[idx]; // mpに対応する新しいロボット位置 const double (*R1)[2] = oldPose.Rmat; const double (*R2)[2] = newPose.Rmat; - LPoint2D lp1 = oldPose.relativePoint(mp); // oldPosempZTWnɕϊ - LPoint2D lp2 = newPose.globalPoint(lp1); // newPoseŃ|[Y̒n}Wnɕϊ + LPoint2D lp1 = oldPose.relativePoint(mp); // oldPoseでmpをセンサ座標系に変換 + LPoint2D lp2 = newPose.globalPoint(lp1); // newPoseでポーズ調整後の地図座標系に変換 mp.x = lp2.x; mp.y = lp2.y; - double nx = R1[0][0]*mp.nx + R1[1][0]*mp.ny; // @xNgoldPoseŃZTWnɕϊ + double nx = R1[0][0]*mp.nx + R1[1][0]*mp.ny; // 法線ベクトルもoldPoseでセンサ座標系に変換 double ny = R1[0][1]*mp.nx + R1[1][1]*mp.ny; - double nx2 = R2[0][0]*nx + R2[0][1]*ny; // @xNgnewPoseŃ|[Y̒n}Wnɕϊ + double nx2 = R2[0][0]*nx + R2[0][1]*ny; // 法線ベクトルもnewPoseでポーズ調整後の地図座標系に変換 double ny2 = R2[1][0]*nx + R2[1][1]*ny; mp.setNormal(nx2, ny2); } } - makeGlobalMap(); // n}S̒n}ƋǏn}𐶐 + makeGlobalMap(); // 部分地図から全体地図と局所地図を生成 - for (size_t i=0; i mps; // n}̃XL_Q + std::vector mps; // 部分地図内のスキャン点群 Submap() : atdS(0), cntS(0), cntE(-1) { } @@ -47,18 +47,18 @@ struct Submap /////////// -// n}\_Qn} +// 部分地図から構成される点群地図 class PointCloudMapLP : public PointCloudMap { public: - static double atdThre; // n}̋؂ƂȂݐϑs(atd)[m] - double atd; // ݂̗ݐϑs(accumulated travel distance) - std::vector submaps; // n} + static double atdThre; // 部分地図の区切りとなる累積走行距離(atd)[m] + double atd; // 現在の累積走行距離(accumulated travel distance) + std::vector submaps; // 部分地図 public: PointCloudMapLP() { Submap submap; - submaps.emplace_back(submap); // ŏ̕n}Ă + submaps.emplace_back(submap); // 最初の部分地図を作っておく } ~PointCloudMapLP() { diff --git a/hook/PoseOptimizerSD.cpp b/hook/PoseOptimizerSD.cpp index 1b22058..5003b82 100755 --- a/hook/PoseOptimizerSD.cpp +++ b/hook/PoseOptimizerSD.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -18,51 +18,51 @@ using namespace std; //////// -// f[^ΉÂŒ̂ƁAlinitPose^ă{bgʒu̐lestPose߂ +// データ対応づけ固定のもと、初期値initPoseを与えてロボット位置の推定値estPoseを求める double PoseOptimizerSD::optimizePose(Pose2D &initPose, Pose2D &estPose) { double th = initPose.th; double tx = initPose.tx; double ty = initPose.ty; - double txmin=tx, tymin=ty, thmin=th; // RXgŏ̉ - double evmin = HUGE_VAL; // RXg̍ŏl - double evold = evmin; // 1‘ÕRXglBɎg + double txmin=tx, tymin=ty, thmin=th; // コスト最小の解 + double evmin = HUGE_VAL; // コストの最小値 + double evold = evmin; // 1つ前のコスト値。収束判定に使う - double ev = cfunc->calValue(tx, ty, th); // RXgvZ - int nn=0; // JԂ񐔁BmFp - double kk=0.00001; // ŋ}~@̃XebvW - while (abs(evold-ev) > evthre) { // B1‘O̒lƂ̕ωƏI + double ev = cfunc->calValue(tx, ty, th); // コスト計算 + int nn=0; // 繰り返し回数。確認用 + double kk=0.00001; // 最急降下法のステップ幅係数 + while (abs(evold-ev) > evthre) { // 収束判定。1つ前の値との変化が小さいと終了 nn++; evold = ev; - // lvZɂΔ + // 数値計算による偏微分 double dEtx = (cfunc->calValue(tx+dd, ty, th) - ev)/dd; double dEty = (cfunc->calValue(tx, ty+dd, th) - ev)/dd; double dEth = (cfunc->calValue(tx, ty, th+da) - ev)/da; - // WkkăXebvɂ + // 微分係数にkkをかけてステップ幅にする double dx = -kk*dEtx; double dy = -kk*dEty; double dth = -kk*dEth; - tx += dx; ty += dy; th += dth; // XebvĎ̒Tʒu߂ + tx += dx; ty += dy; th += dth; // ステップ幅を加えて次の探索位置を決める - ev = cfunc->calValue(tx, ty, th); // ̈ʒuŃRXgvZ + ev = cfunc->calValue(tx, ty, th); // その位置でコスト計算 - if (ev < evmin) { // ev܂ł̍ŏȂXV + if (ev < evmin) { // evがこれまでの最小なら更新 evmin = ev; txmin = tx; tymin = ty; thmin = th; } -// printf("nn=%d, ev=%g, evold=%g, abs(evold-ev)=%g\n", nn, ev, evold, abs(evold-ev)); // mFp +// printf("nn=%d, ev=%g, evold=%g, abs(evold-ev)=%g\n", nn, ev, evold, abs(evold-ev)); // 確認用 } ++allN; if (allN > 0 && evmin < 100) sum += evmin; -// printf("allN=%d, evmin=%g, avg=%g\n", allN, evmin, (sum/allN)); // mFp +// printf("allN=%d, evmin=%g, avg=%g\n", allN, evmin, (sum/allN)); // 確認用 -// printf("nn=%d, ev=%g\n", nn, ev); // mFp +// printf("nn=%d, ev=%g\n", nn, ev); // 確認用 - estPose.setVal(txmin, tymin, thmin); // ŏl^ۑ + estPose.setVal(txmin, tymin, thmin); // 最小値を与える解を保存 return(evmin); } diff --git a/hook/PoseOptimizerSD.h b/hook/PoseOptimizerSD.h index ab5987a..6a8340d 100755 --- a/hook/PoseOptimizerSD.h +++ b/hook/PoseOptimizerSD.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -17,7 +17,7 @@ #include "PoseOptimizer.h" -// ŋ}~@ŃRXg֐ŏ +// 最急降下法でコスト関数を最小化する class PoseOptimizerSD : public PoseOptimizer { public: diff --git a/hook/PoseOptimizerSL.cpp b/hook/PoseOptimizerSL.cpp index dae5121..9b21447 100755 --- a/hook/PoseOptimizerSL.cpp +++ b/hook/PoseOptimizerSL.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -19,83 +19,83 @@ using namespace std; //////// -// f[^ΉÂŒ̂ƁAlinitPose^ă{bgʒu̐lestPose߂ +// データ対応づけ固定のもと、初期値initPoseを与えてロボット位置の推定値estPoseを求める double PoseOptimizerSL::optimizePose(Pose2D &initPose, Pose2D &estPose) { double th = initPose.th; double tx = initPose.tx; double ty = initPose.ty; - double txmin=tx, tymin=ty, thmin=th; // RXgŏ̉ - double evmin = HUGE_VAL; // RXg̍ŏl - double evold = evmin; // 1‘ÕRXglBɎg + double txmin=tx, tymin=ty, thmin=th; // コスト最小の解 + double evmin = HUGE_VAL; // コストの最小値 + double evold = evmin; // 1つ前のコスト値。収束判定に使う Pose2D pose, dir; - double ev = cfunc->calValue(tx, ty, th); // RXgvZ - int nn=0; // JԂ񐔁BmFp - while (abs(evold-ev) > evthre) { // Bl̕ωƏI + double ev = cfunc->calValue(tx, ty, th); // コスト計算 + int nn=0; // 繰り返し回数。確認用 + while (abs(evold-ev) > evthre) { // 収束判定。値の変化が小さいと終了 nn++; evold = ev; - // lvZɂΔ + // 数値計算による偏微分 double dx = (cfunc->calValue(tx+dd, ty, th) - ev)/dd; double dy = (cfunc->calValue(tx, ty+dd, th) - ev)/dd; double dth = (cfunc->calValue(tx, ty, th+da) - ev)/da; - tx += dx; ty += dy; th += dth; // 񎟂̒Tʒu߂ + tx += dx; ty += dy; th += dth; // いったん次の探索位置を決める - // ug@ɂ钼T - pose.tx = tx; pose.ty = ty; pose.th = th; // TJn_ - dir.tx = dx; dir.ty = dy; dir.th = dth; // T - search(ev, pose, dir); // Ts - tx = pose.tx; ty = pose.ty; th = pose.th; // Tŋ߂ʒu + // ブレント法による直線探索 + pose.tx = tx; pose.ty = ty; pose.th = th; // 探索開始点 + dir.tx = dx; dir.ty = dy; dir.th = dth; // 探索方向 + search(ev, pose, dir); // 直線探索実行 + tx = pose.tx; ty = pose.ty; th = pose.th; // 直線探索で求めた位置 - ev = cfunc->calValue(tx, ty, th); // ߂ʒuŃRXgvZ + ev = cfunc->calValue(tx, ty, th); // 求めた位置でコスト計算 - if (ev < evmin) { // RXg܂ł̍ŏȂXV + if (ev < evmin) { // コストがこれまでの最小なら更新 evmin = ev; txmin = tx; tymin = ty; thmin = th; } -// printf("nn=%d, ev=%g, evold=%g, abs(evold-ev)=%g\n", nn, ev, evold, abs(evold-ev)); // mFp +// printf("nn=%d, ev=%g, evold=%g, abs(evold-ev)=%g\n", nn, ev, evold, abs(evold-ev)); // 確認用 } ++allN; if (allN > 0 && evmin < 100) sum += evmin; -// printf("allN=%d, nn=%d, evmin=%g, avg=%g, evthre=%g\n", allN, nn, evmin, (sum/allN), evthre); // mFp +// printf("allN=%d, nn=%d, evmin=%g, avg=%g, evthre=%g\n", allN, nn, evmin, (sum/allN), evthre); // 確認用 -// printf("nn=%d, evmin=%g\n", nn, evmin); // mFp +// printf("nn=%d, evmin=%g\n", nn, evmin); // 確認用 - estPose.setVal(txmin, tymin, thmin); // ŏl^ۑ + estPose.setVal(txmin, tymin, thmin); // 最小値を与える解を保存 return(evmin); } ////////// Line search /////////// -// boostCũug@ŒTsB -// posen_ɁAdpɂǂꂾi߂΂悢Xebv‚B +// boostライブラリのブレント法で直線探索を行う。 +// poseを始点に、dp方向にどれだけ進めばよいかステップ幅を見つける。 double PoseOptimizerSL::search(double ev0, Pose2D &pose, Pose2D &dp) { - int bits = numeric_limits::digits; // Tx - boost::uintmax_t maxIter=40; // őJԂ񐔁BoIɌ߂ + int bits = numeric_limits::digits; // 探索精度 + boost::uintmax_t maxIter=40; // 最大繰り返し回数。経験的に決める pair result = boost::math::tools::brent_find_minima( [this, &pose, &dp](double tt) {return (objFunc(tt, pose, dp));}, - -2.0, 2.0, bits, maxIter); // T͈(-2.0,2.0) + -2.0, 2.0, bits, maxIter); // 探索範囲(-2.0,2.0) - double t = result.first; // ߂Xebv - double v = result.second; // ߂ŏl + double t = result.first; // 求めるステップ幅 + double v = result.second; // 求める最小値 - pose.tx = pose.tx + t*dp.tx; // ߂ŏposeɊi[ + pose.tx = pose.tx + t*dp.tx; // 求める最小解をposeに格納 pose.ty = pose.ty + t*dp.ty; pose.th = MyUtil::add(pose.th, t*dp.th); return(v); } -// T̖ړI֐BttXebv +// 直線探索の目的関数。ttがステップ幅 double PoseOptimizerSL::objFunc(double tt, Pose2D &pose, Pose2D &dp) { - double tx = pose.tx + tt*dp.tx; // posedptti + double tx = pose.tx + tt*dp.tx; // poseからdp方向にttだけ進む double ty = pose.ty + tt*dp.ty; double th = MyUtil::add(pose.th, tt*dp.th); - double v = cfunc->calValue(tx, ty, th); // RXg֐l + double v = cfunc->calValue(tx, ty, th); // コスト関数値 return(v); } diff --git a/hook/PoseOptimizerSL.h b/hook/PoseOptimizerSL.h index 4f6cb3e..4fffecb 100755 --- a/hook/PoseOptimizerSL.h +++ b/hook/PoseOptimizerSL.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -17,7 +17,7 @@ #include "PoseOptimizer.h" -// T‚̍ŋ}~@ŃRXg֐ŏ +// 直線探索つきの最急降下法でコスト関数を最小化する class PoseOptimizerSL : public PoseOptimizer { public: diff --git a/hook/RefScanMakerBS.cpp b/hook/RefScanMakerBS.cpp index 7c80e20..1a8832f 100755 --- a/hook/RefScanMakerBS.cpp +++ b/hook/RefScanMakerBS.cpp @@ -1,4 +1,4 @@ -/**************************************************************************** +/**************************************************************************** * LittleSLAM: 2D-Laser SLAM for educational use * Copyright (C) 2017-2018 Masahiro Tomono * Copyright (C) 2018 Future Robotics Technology Center (fuRo), @@ -17,24 +17,24 @@ using namespace std; const Scan2D *RefScanMakerBS::makeRefScan() { - vector &refLps = refScan.lps; // QƃXL̓_Q̃Rei + vector &refLps = refScan.lps; // 参照スキャンの点群のコンテナ refLps.clear(); - Pose2D lastPose = pcmap->getLastPose(); // _Qn}ɕۑŌ̐ʒu + Pose2D lastPose = pcmap->getLastPose(); // 点群地図に保存した最後の推定位置 double (*R)[2] = lastPose.Rmat; double tx = lastPose.tx; double ty = lastPose.ty; - // _Qn}ɕۑŌ̃XLQƃXLɂ + // 点群地図に保存した最後のスキャンを参照スキャンにする const vector &lps = pcmap->lastScan.lps; for (size_t i=0; i &refLps = refScan.lps; // QƃXL̓_Q̃Rei + vector &refLps = refScan.lps; // 参照スキャンの点群のコンテナ refLps.clear(); - const vector &localMap = pcmap->localMap; // _Qn}̋Ǐn} + const vector &localMap = pcmap->localMap; // 点群地図の局所地図 for (size_t i=0; i