first commit

sweetquiet · Mar 2, 2018 · d5ae1d4 · d5ae1d4
commit d5ae1d4
Show file tree

Hide file tree

Showing 90 changed files with 6,447 additions and 0 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -0,0 +1,28 @@
+project(LittleSLAM)
+
+cmake_minimum_required(VERSION 2.8)
+
+# C++11‚ðŽg‚¤
+if(UNIX)
+  include(CheckCXXCompilerFlag)
+  CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+  CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+  if(COMPILER_SUPPORTS_CXX11)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+  elseif(COMPILER_SUPPORTS_CXX0X)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+  else()
+    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Use a different C++ compiler.")
+  endif()
+endif()
+
+if(UNIX)   # gcc‚ÅSHIFT-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)
+add_subdirectory(framework framework)
+add_subdirectory(hook hook)
diff --git a/LICENSE b/LICENSE
diff --git a/README.md b/README.md
@@ -0,0 +1,86 @@
+# LittleSLAM
+
+## LittleSLAMについて
+
+LittleSLAMは、SLAM学習用プログラムです。
+2Dレーザスキャナのデータ（スキャン）とオドメトリデータを格納したファイルを入力し、
+ロボット位置の軌跡と2D点群地図をgnuplot上に出力します。
+
+LittleSLAMは、スキャンマッチングに基づく位置合せ、レーザスキャナとオドメトリのセンサ融合、
+Graph-based SLAMに基づくループ閉じ込みなどの要素技術から構成されています。
+
+LittleSLAMは参考書籍[1]の教材として作られたプログラムであり、
+わかりやすさを優先してシンプルなアルゴリズムを採用しています。
+そのため、フルスペックのSLAMプログラムと比べると性能は落ちますが、
+内容の理解はしやすくなっています。
+
+
+## 実行環境
+
+LittleSLAMはプログラミング言語C++で記述されています。
+動作を確認した実行環境は下記のものです。いずれも64ビット版です。
+
+| OS | C++ |
+|:--:|:---:|
+| Windows 7 | Visual C++ 2013 (Visual Studio Community 2013)|
+| Windows 10 | Visual C++ 2015 (Visual Studio Community 2015)|
+| Linux Ubuntu 14.04 LTS | gcc 4.8.4|
+| Linux Ubuntu 16.04 LTS | gcc 5.4.0|
+
+32ビットOSでの動作確認はしていないので、必要な場合はご自分で試してください。
+
+
+## 必要なソフトウェア
+
+LittleSLAMの実行には、下記のソフトウェアが必要です。
+
+| ソフトウェア | 内容 | バージョン |
+|:------------:|:----:|:----------:|
+| Boost        | C++汎用ライブラリ |1.58.0 |
+| Eigen3       | 線形代数ライブラリ|3.2.4 |
+| gnuplot      | グラフ描画ツール  |5.0 |
+| CMake        | ビルド支援ツール  |3.2.2 |
+| p2o          | Graph-based SLAMソルバ|beta |
+
+バージョンはLittleSLAMの開発で使用したものであり、明確な条件ではありません。
+これ以上のバージョンであれば通常は動作します。
+これ以下のバージョンでも動作する可能性はあります。
+
+## 使い方
+
+- Windowsでの使い方は[こちら](doc/install-win.md)
+
+- Linuxでの使い方は[こちら](doc/install-linux.md)
+
+## データセット
+
+実験用に6個のデータファイルを用意しています。下表に一覧を示します。
+[ここ](https://furo.org/software/little_slam/dataset.zip)からダウンロードできます。
+
+
+| ファイル名          | 内容         |
+|:--------------------|:-------------|
+| corridor.lsc        | 廊下（単一ループ） |
+| hall.lsc            | 広間（単一ループ） |
+| corridor-degene.lsc | 廊下（退化） |
+| hall-degene.lsc     | 広間（退化） |
+| corridor-loops.lsc  | 廊下（多重ループ） |
+| hall-loops.lsc      | 広間（多重ループ） |
+
+## カスタマイズ
+
+LittleSLAMは学習用プログラムであり、基本形からいくつかの改良を経て
+完成するようにカスタマイズできます。  
+詳細は[こちら](doc/customize.md)を参照してください。
+
+## 参考書籍
+
+下記の書籍はSLAMの解説書です。SLAMの一般的な解説をするとともに、
+具体例としてLittleSLAMを教材に用い、そのソースコードの詳細を説明しています。
+
+[1] 友納正裕、「SLAM入門 -- ロボットの自己位置推定と地図構築の技術」、オーム社、2018年  
+
+## ライセンス
+
+- LittleSLAMは、MPL-2.0ライセンスにもとづいています。
+
diff --git a/cui/CMakeLists.txt b/cui/CMakeLists.txt
@@ -0,0 +1,45 @@
+project(cui)
+
+cmake_minimum_required(VERSION 2.8)
+
+find_package(Boost REQUIRED)
+
+find_package(Eigen3)
+IF(NOT EIGEN3_INCLUDE_DIR)
+  set(EIGEN3_INCLUDE_DIR $ENV{EIGEN3_ROOT_DIR})
+ENDIF() 
+
+include_directories(
+ 	${Boost_INCLUDE_DIR}
+        ${EIGEN3_INCLUDE_DIR}
+	${CMAKE_CURRENT_SOURCE_DIR}
+	${CMAKE_CURRENT_SOURCE_DIR}/../framework
+	${CMAKE_CURRENT_SOURCE_DIR}/../hook
+)
+
+link_directories(
+)
+
+set(CUI_SRCS
+    main.cpp
+    SlamLauncher.cpp
+    FrameworkCustomizer.cpp
+    MapDrawer.cpp
+)
+
+set(CUI_HDRS
+    SlamLauncher.h
+    FrameworkCustomizer.h
+    MapDrawer.h
+)
+
+add_executable(LittleSLAM
+    ${CUI_SRCS}
+    ${CUI_HDRS}
+)
+
+target_link_libraries(LittleSLAM
+  framework
+  hook
+)
+
diff --git a/cui/FrameworkCustomizer.cpp b/cui/FrameworkCustomizer.cpp
@@ -0,0 +1,223 @@
+/****************************************************************************
+ * LittleSLAM: 2D-Laser SLAM for educational use
+ * Copyright (C) 2017-2018 Masahiro Tomono
+ * Copyright (C) 2018 Future Robotics Technology Center (fuRo),
+ *                    Chiba Institute of Technology.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at https://mozilla.org/MPL/2.0/.
+ *
+ * @file FrameworkCustomizer.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "FrameworkCustomizer.h"
+
+using namespace std;
+
+// フレームワークの基本部分を設定
+void FrameworkCustomizer::makeFramework() {
+  smat.setPoseEstimator(&poest);
+  smat.setPoseFuser(&pfu);
+
+  // LoopDetectorSSは使う部品が決まっているので、ここで入れる
+  lpdSS.setPoseEstimator(&poest);
+  lpdSS.setPoseFuser(&pfu);
+  lpdSS.setDataAssociator(&dassGT);
+  lpdSS.setCostFunction(&cfuncPD);
+  lpdSS.setPointCloudMap(&pcmapLP);
+
+  sfront->setScanMatcher(&smat);
+}
+
+/////// 実験用
+
+// フレームワーク基本構成
+void FrameworkCustomizer::customizeA() {
+  pcmap = &pcmapBS;                                // 全スキャン点を保存する点群地図
+  RefScanMaker *rsm = &rsmBS;                      // 直前スキャンを参照スキャンとする
+  DataAssociator *dass = &dassLS;                  // 線形探索によるデータ対応づけ
+  CostFunction *cfunc = &cfuncED;                  // ユークリッド距離をコスト関数とする
+  PoseOptimizer *popt = &poptSD;                   // 最急降下法による最適化
+  LoopDetector *lpd = &lpdDM;                      // ダミーのループ検出
+
+  popt->setCostFunction(cfunc);
+  poest.setDataAssociator(dass);
+  poest.setPoseOptimizer(popt);
+  pfu.setDataAssociator(dass);
+  smat.setPointCloudMap(pcmap);
+  smat.setRefScanMaker(rsm);
+  sfront->setLoopDetector(lpd);
+  sfront->setPointCloudMap(pcmap);
+  sfront->setDgCheck(false);                       // センサ融合しない
+}
+
+// 格子テーブルの利用
+void FrameworkCustomizer::customizeB() {
+  pcmap = &pcmapGT;                                // 格子テーブルで管理する点群地図
+  RefScanMaker *rsm = &rsmLM;                      // 局所地図を参照スキャンとする
+  DataAssociator *dass = &dassLS;                  // 線形探索によるデータ対応づけ
+  CostFunction *cfunc = &cfuncED;                  // ユークリッド距離をコスト関数とする
+  PoseOptimizer *popt = &poptSD;                   // 最急降下法による最適化
+  LoopDetector *lpd = &lpdDM;                      // ダミーのループ検出
+
+  popt->setCostFunction(cfunc);
+  poest.setDataAssociator(dass);
+  poest.setPoseOptimizer(popt);
+  pfu.setDataAssociator(dass);
+  smat.setPointCloudMap(pcmap);
+  smat.setRefScanMaker(rsm);
+  sfront->setLoopDetector(lpd);
+  sfront->setPointCloudMap(pcmap);
+  sfront->setDgCheck(false);                       // センサ融合しない
+}
+
+// 最急降下法の後に直線探索を行う
+void FrameworkCustomizer::customizeC() {
+  pcmap = &pcmapGT;                                // 格子テーブルで管理する点群地図
+  RefScanMaker *rsm = &rsmLM;                      // 局所地図を参照スキャンとする
+  DataAssociator *dass = &dassLS;                  // 線形探索によるデータ対応づけ
+  CostFunction *cfunc = &cfuncED;                  // ユークリッド距離をコスト関数とする
+  PoseOptimizer *popt = &poptSL;                   // 最急降下法と直線探索による最適化
+  LoopDetector *lpd = &lpdDM;                      // ダミーのループ検出
+
+  popt->setCostFunction(cfunc);
+  poest.setDataAssociator(dass);
+  poest.setPoseOptimizer(popt);
+  pfu.setDataAssociator(dass);
+  smat.setPointCloudMap(pcmap);
+  smat.setRefScanMaker(rsm);
+  sfront->setLoopDetector(lpd);
+  sfront->setPointCloudMap(pcmap);
+  sfront->setDgCheck(false);                       // センサ融合しない
+}
+
+// データ対応づけを格子テーブルで行う
+void FrameworkCustomizer::customizeD() {
+  pcmap = &pcmapGT;                                // 格子テーブルで管理する点群地図
+  RefScanMaker *rsm = &rsmLM;                      // 局所地図を参照スキャンとする
+  DataAssociator *dass = &dassGT;                  // 格子テーブルによるデータ対応づけ
+  CostFunction *cfunc = &cfuncED;                  // ユークリッド距離をコスト関数とする
+  PoseOptimizer *popt = &poptSL;                   // 最急降下法と直線探索による最適化
+  LoopDetector *lpd = &lpdDM;                      // ダミーのループ検出
+
+  popt->setCostFunction(cfunc);
+  poest.setDataAssociator(dass);
+  poest.setPoseOptimizer(popt);
+  pfu.setDataAssociator(dass);
+  smat.setPointCloudMap(pcmap);
+  smat.setRefScanMaker(rsm);
+  sfront->setLoopDetector(lpd);
+  sfront->setPointCloudMap(pcmap);
+  sfront->setDgCheck(false);                       // センサ融合しない
+}
+
+// スキャン点間隔均一化を追加
+void FrameworkCustomizer::customizeE() {
+  pcmap = &pcmapGT;                                // 格子テーブルで管理する点群地図
+  RefScanMaker *rsm = &rsmLM;                      // 局所地図を参照スキャンとする
+  DataAssociator *dass = &dassGT;                  // 格子テーブルによるデータ対応づけ
+  CostFunction *cfunc = &cfuncED;                  // ユークリッド距離をコスト関数とする
+  PoseOptimizer *popt = &poptSL;                   // 最急降下法と直線探索による最適化
+  LoopDetector *lpd = &lpdDM;                      // ダミーのループ検出
+
+  popt->setCostFunction(cfunc);
+  poest.setDataAssociator(dass);
+  poest.setPoseOptimizer(popt);
+  pfu.setDataAssociator(dass);
+  smat.setPointCloudMap(pcmap);
+  smat.setRefScanMaker(rsm);
+  smat.setScanPointResampler(&spres);              // スキャン点間隔均一化
+  sfront->setLoopDetector(lpd);
+  sfront->setPointCloudMap(pcmap);
+  sfront->setDgCheck(false);                       // センサ融合しない
+}
+
+// スキャン点の法線計算を追加
+void FrameworkCustomizer::customizeF() {
+  pcmap = &pcmapGT;                                // 格子テーブルで管理する点群地図
+  RefScanMaker *rsm = &rsmLM;                      // 局所地図を参照スキャンとする
+  DataAssociator *dass = &dassGT;                  // 格子テーブルによるデータ対応づけ
+  CostFunction *cfunc = &cfuncPD;                  // 垂直距離をコスト関数とする
+  PoseOptimizer *popt = &poptSL;                   // 最急降下法と直線探索による最適化
+  LoopDetector *lpd = &lpdDM;                      // ダミーのループ検出
+
+  popt->setCostFunction(cfunc);
+  poest.setDataAssociator(dass);
+  poest.setPoseOptimizer(popt);
+  pfu.setDataAssociator(dass);
+  smat.setPointCloudMap(pcmap);
+  smat.setRefScanMaker(rsm);
+  smat.setScanPointAnalyser(&spana);               // スキャン点の法線計算
+  sfront->setLoopDetector(lpd);
+  sfront->setPointCloudMap(pcmap);
+  sfront->setDgCheck(false);                       // センサ融合しない
+}
+
+// スキャン点間隔均一化と法線計算を追加
+void FrameworkCustomizer::customizeG() {
+  pcmap = &pcmapGT;                                // 格子テーブルで管理する点群地図
+  RefScanMaker *rsm = &rsmLM;                      // 局所地図を参照スキャンとする
+  DataAssociator *dass = &dassGT;                  // 格子テーブルによるデータ対応づけ
+  CostFunction *cfunc = &cfuncPD;                  // 垂直距離をコスト関数とする
+  PoseOptimizer *popt = &poptSL;                   // 最急降下法と直線探索による最適化
+  LoopDetector *lpd = &lpdDM;                      // ダミーのループ検出
+
+  popt->setCostFunction(cfunc);
+  poest.setDataAssociator(dass);
+  poest.setPoseOptimizer(popt);
+  pfu.setDataAssociator(dass);
+  smat.setPointCloudMap(pcmap);
+  smat.setRefScanMaker(rsm);
+  smat.setScanPointResampler(&spres);              // スキャン点間隔均一化
+  smat.setScanPointAnalyser(&spana);               // スキャン点の法線計算
+  sfront->setLoopDetector(lpd);
+  sfront->setPointCloudMap(pcmap);
+  sfront->setDgCheck(false);                       // センサ融合しない
+}
+
+// センサ融合を追加
+void FrameworkCustomizer::customizeH() {
+//  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);
+  poest.setPoseOptimizer(popt);
+  pfu.setDataAssociator(dass);
+  smat.setPointCloudMap(pcmap);
+  smat.setRefScanMaker(rsm);
+  smat.setScanPointResampler(&spres);
+  smat.setScanPointAnalyser(&spana);
+  sfront->setLoopDetector(lpd);
+  sfront->setPointCloudMap(pcmap);
+  sfront->setDgCheck(true);                        // センサ融合する
+}
+
+// センサ融合とループ閉じ込みを追加
+void FrameworkCustomizer::customizeI() {
+  pcmap = &pcmapLP;                                // 部分地図ごとに管理する点群地図
+  RefScanMaker *rsm = &rsmLM;                      // 局所地図を参照スキャンとする
+  DataAssociator *dass = &dassGT;                  // 格子テーブルによるデータ対応づけ
+  CostFunction *cfunc = &cfuncPD;                  // 垂直距離をコスト関数とする
+  PoseOptimizer *popt = &poptSL;                   // 最急降下法と直線探索による最適化
+  LoopDetector *lpd = &lpdSS;                      // 部分地図を用いたループ検出
+
+  popt->setCostFunction(cfunc);
+  poest.setDataAssociator(dass);
+  poest.setPoseOptimizer(popt);
+  pfu.setDataAssociator(dass);
+  smat.setPointCloudMap(pcmap);
+  smat.setRefScanMaker(rsm);
+  smat.setScanPointResampler(&spres);
+  smat.setScanPointAnalyser(&spana);
+  sfront->setLoopDetector(lpd);
+  sfront->setPointCloudMap(pcmap);
+  sfront->setDgCheck(true);                        // センサ融合する
+}