diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100755
index 0000000..6d65227
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,28 @@
+project(LittleSLAM)
+
+cmake_minimum_required(VERSION 2.8)
+
+# C++11を使う
+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
new file mode 100755
index 0000000..a612ad9
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,373 @@
+Mozilla Public License Version 2.0
+==================================
+
+1. Definitions
+--------------
+
+1.1. "Contributor"
+    means each individual or legal entity that creates, contributes to
+    the creation of, or owns Covered Software.
+
+1.2. "Contributor Version"
+    means the combination of the Contributions of others (if any) used
+    by a Contributor and that particular Contributor's Contribution.
+
+1.3. "Contribution"
+    means Covered Software of a particular Contributor.
+
+1.4. "Covered Software"
+    means Source Code Form to which the initial Contributor has attached
+    the notice in Exhibit A, the Executable Form of such Source Code
+    Form, and Modifications of such Source Code Form, in each case
+    including portions thereof.
+
+1.5. "Incompatible With Secondary Licenses"
+    means
+
+    (a) that the initial Contributor has attached the notice described
+        in Exhibit B to the Covered Software; or
+
+    (b) that the Covered Software was made available under the terms of
+        version 1.1 or earlier of the License, but not also under the
+        terms of a Secondary License.
+
+1.6. "Executable Form"
+    means any form of the work other than Source Code Form.
+
+1.7. "Larger Work"
+    means a work that combines Covered Software with other material, in
+    a separate file or files, that is not Covered Software.
+
+1.8. "License"
+    means this document.
+
+1.9. "Licensable"
+    means having the right to grant, to the maximum extent possible,
+    whether at the time of the initial grant or subsequently, any and
+    all of the rights conveyed by this License.
+
+1.10. "Modifications"
+    means any of the following:
+
+    (a) any file in Source Code Form that results from an addition to,
+        deletion from, or modification of the contents of Covered
+        Software; or
+
+    (b) any new file in Source Code Form that contains any Covered
+        Software.
+
+1.11. "Patent Claims" of a Contributor
+    means any patent claim(s), including without limitation, method,
+    process, and apparatus claims, in any patent Licensable by such
+    Contributor that would be infringed, but for the grant of the
+    License, by the making, using, selling, offering for sale, having
+    made, import, or transfer of either its Contributions or its
+    Contributor Version.
+
+1.12. "Secondary License"
+    means either the GNU General Public License, Version 2.0, the GNU
+    Lesser General Public License, Version 2.1, the GNU Affero General
+    Public License, Version 3.0, or any later versions of those
+    licenses.
+
+1.13. "Source Code Form"
+    means the form of the work preferred for making modifications.
+
+1.14. "You" (or "Your")
+    means an individual or a legal entity exercising rights under this
+    License. For legal entities, "You" includes any entity that
+    controls, is controlled by, or is under common control with You. For
+    purposes of this definition, "control" means (a) the power, direct
+    or indirect, to cause the direction or management of such entity,
+    whether by contract or otherwise, or (b) ownership of more than
+    fifty percent (50%) of the outstanding shares or beneficial
+    ownership of such entity.
+
+2. License Grants and Conditions
+--------------------------------
+
+2.1. Grants
+
+Each Contributor hereby grants You a world-wide, royalty-free,
+non-exclusive license:
+
+(a) under intellectual property rights (other than patent or trademark)
+    Licensable by such Contributor to use, reproduce, make available,
+    modify, display, perform, distribute, and otherwise exploit its
+    Contributions, either on an unmodified basis, with Modifications, or
+    as part of a Larger Work; and
+
+(b) under Patent Claims of such Contributor to make, use, sell, offer
+    for sale, have made, import, and otherwise transfer either its
+    Contributions or its Contributor Version.
+
+2.2. Effective Date
+
+The licenses granted in Section 2.1 with respect to any Contribution
+become effective for each Contribution on the date the Contributor first
+distributes such Contribution.
+
+2.3. Limitations on Grant Scope
+
+The licenses granted in this Section 2 are the only rights granted under
+this License. No additional rights or licenses will be implied from the
+distribution or licensing of Covered Software under this License.
+Notwithstanding Section 2.1(b) above, no patent license is granted by a
+Contributor:
+
+(a) for any code that a Contributor has removed from Covered Software;
+    or
+
+(b) for infringements caused by: (i) Your and any other third party's
+    modifications of Covered Software, or (ii) the combination of its
+    Contributions with other software (except as part of its Contributor
+    Version); or
+
+(c) under Patent Claims infringed by Covered Software in the absence of
+    its Contributions.
+
+This License does not grant any rights in the trademarks, service marks,
+or logos of any Contributor (except as may be necessary to comply with
+the notice requirements in Section 3.4).
+
+2.4. Subsequent Licenses
+
+No Contributor makes additional grants as a result of Your choice to
+distribute the Covered Software under a subsequent version of this
+License (see Section 10.2) or under the terms of a Secondary License (if
+permitted under the terms of Section 3.3).
+
+2.5. Representation
+
+Each Contributor represents that the Contributor believes its
+Contributions are its original creation(s) or it has sufficient rights
+to grant the rights to its Contributions conveyed by this License.
+
+2.6. Fair Use
+
+This License is not intended to limit any rights You have under
+applicable copyright doctrines of fair use, fair dealing, or other
+equivalents.
+
+2.7. Conditions
+
+Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted
+in Section 2.1.
+
+3. Responsibilities
+-------------------
+
+3.1. Distribution of Source Form
+
+All distribution of Covered Software in Source Code Form, including any
+Modifications that You create or to which You contribute, must be under
+the terms of this License. You must inform recipients that the Source
+Code Form of the Covered Software is governed by the terms of this
+License, and how they can obtain a copy of this License. You may not
+attempt to alter or restrict the recipients' rights in the Source Code
+Form.
+
+3.2. Distribution of Executable Form
+
+If You distribute Covered Software in Executable Form then:
+
+(a) such Covered Software must also be made available in Source Code
+    Form, as described in Section 3.1, and You must inform recipients of
+    the Executable Form how they can obtain a copy of such Source Code
+    Form by reasonable means in a timely manner, at a charge no more
+    than the cost of distribution to the recipient; and
+
+(b) You may distribute such Executable Form under the terms of this
+    License, or sublicense it under different terms, provided that the
+    license for the Executable Form does not attempt to limit or alter
+    the recipients' rights in the Source Code Form under this License.
+
+3.3. Distribution of a Larger Work
+
+You may create and distribute a Larger Work under terms of Your choice,
+provided that You also comply with the requirements of this License for
+the Covered Software. If the Larger Work is a combination of Covered
+Software with a work governed by one or more Secondary Licenses, and the
+Covered Software is not Incompatible With Secondary Licenses, this
+License permits You to additionally distribute such Covered Software
+under the terms of such Secondary License(s), so that the recipient of
+the Larger Work may, at their option, further distribute the Covered
+Software under the terms of either this License or such Secondary
+License(s).
+
+3.4. Notices
+
+You may not remove or alter the substance of any license notices
+(including copyright notices, patent notices, disclaimers of warranty,
+or limitations of liability) contained within the Source Code Form of
+the Covered Software, except that You may alter any license notices to
+the extent required to remedy known factual inaccuracies.
+
+3.5. Application of Additional Terms
+
+You may choose to offer, and to charge a fee for, warranty, support,
+indemnity or liability obligations to one or more recipients of Covered
+Software. However, You may do so only on Your own behalf, and not on
+behalf of any Contributor. You must make it absolutely clear that any
+such warranty, support, indemnity, or liability obligation is offered by
+You alone, and You hereby agree to indemnify every Contributor for any
+liability incurred by such Contributor as a result of warranty, support,
+indemnity or liability terms You offer. You may include additional
+disclaimers of warranty and limitations of liability specific to any
+jurisdiction.
+
+4. Inability to Comply Due to Statute or Regulation
+---------------------------------------------------
+
+If it is impossible for You to comply with any of the terms of this
+License with respect to some or all of the Covered Software due to
+statute, judicial order, or regulation then You must: (a) comply with
+the terms of this License to the maximum extent possible; and (b)
+describe the limitations and the code they affect. Such description must
+be placed in a text file included with all distributions of the Covered
+Software under this License. Except to the extent prohibited by statute
+or regulation, such description must be sufficiently detailed for a
+recipient of ordinary skill to be able to understand it.
+
+5. Termination
+--------------
+
+5.1. The rights granted under this License will terminate automatically
+if You fail to comply with any of its terms. However, if You become
+compliant, then the rights granted under this License from a particular
+Contributor are reinstated (a) provisionally, unless and until such
+Contributor explicitly and finally terminates Your grants, and (b) on an
+ongoing basis, if such Contributor fails to notify You of the
+non-compliance by some reasonable means prior to 60 days after You have
+come back into compliance. Moreover, Your grants from a particular
+Contributor are reinstated on an ongoing basis if such Contributor
+notifies You of the non-compliance by some reasonable means, this is the
+first time You have received notice of non-compliance with this License
+from such Contributor, and You become compliant prior to 30 days after
+Your receipt of the notice.
+
+5.2. If You initiate litigation against any entity by asserting a patent
+infringement claim (excluding declaratory judgment actions,
+counter-claims, and cross-claims) alleging that a Contributor Version
+directly or indirectly infringes any patent, then the rights granted to
+You by any and all Contributors for the Covered Software under Section
+2.1 of this License shall terminate.
+
+5.3. In the event of termination under Sections 5.1 or 5.2 above, all
+end user license agreements (excluding distributors and resellers) which
+have been validly granted by You or Your distributors under this License
+prior to termination shall survive termination.
+
+************************************************************************
+*                                                                      *
+*  6. Disclaimer of Warranty                                           *
+*  -------------------------                                           *
+*                                                                      *
+*  Covered Software is provided under this License on an "as is"       *
+*  basis, without warranty of any kind, either expressed, implied, or  *
+*  statutory, including, without limitation, warranties that the       *
+*  Covered Software is free of defects, merchantable, fit for a        *
+*  particular purpose or non-infringing. The entire risk as to the     *
+*  quality and performance of the Covered Software is with You.        *
+*  Should any Covered Software prove defective in any respect, You     *
+*  (not any Contributor) assume the cost of any necessary servicing,   *
+*  repair, or correction. This disclaimer of warranty constitutes an   *
+*  essential part of this License. No use of any Covered Software is   *
+*  authorized under this License except under this disclaimer.         *
+*                                                                      *
+************************************************************************
+
+************************************************************************
+*                                                                      *
+*  7. Limitation of Liability                                          *
+*  --------------------------                                          *
+*                                                                      *
+*  Under no circumstances and under no legal theory, whether tort      *
+*  (including negligence), contract, or otherwise, shall any           *
+*  Contributor, or anyone who distributes Covered Software as          *
+*  permitted above, be liable to You for any direct, indirect,         *
+*  special, incidental, or consequential damages of any character      *
+*  including, without limitation, damages for lost profits, loss of    *
+*  goodwill, work stoppage, computer failure or malfunction, or any    *
+*  and all other commercial damages or losses, even if such party      *
+*  shall have been informed of the possibility of such damages. This   *
+*  limitation of liability shall not apply to liability for death or   *
+*  personal injury resulting from such party's negligence to the       *
+*  extent applicable law prohibits such limitation. Some               *
+*  jurisdictions do not allow the exclusion or limitation of           *
+*  incidental or consequential damages, so this exclusion and          *
+*  limitation may not apply to You.                                    *
+*                                                                      *
+************************************************************************
+
+8. Litigation
+-------------
+
+Any litigation relating to this License may be brought only in the
+courts of a jurisdiction where the defendant maintains its principal
+place of business and such litigation shall be governed by laws of that
+jurisdiction, without reference to its conflict-of-law provisions.
+Nothing in this Section shall prevent a party's ability to bring
+cross-claims or counter-claims.
+
+9. Miscellaneous
+----------------
+
+This License represents the complete agreement concerning the subject
+matter hereof. If any provision of this License is held to be
+unenforceable, such provision shall be reformed only to the extent
+necessary to make it enforceable. Any law or regulation which provides
+that the language of a contract shall be construed against the drafter
+shall not be used to construe this License against a Contributor.
+
+10. Versions of the License
+---------------------------
+
+10.1. New Versions
+
+Mozilla Foundation is the license steward. Except as provided in Section
+10.3, no one other than the license steward has the right to modify or
+publish new versions of this License. Each version will be given a
+distinguishing version number.
+
+10.2. Effect of New Versions
+
+You may distribute the Covered Software under the terms of the version
+of the License under which You originally received the Covered Software,
+or under the terms of any subsequent version published by the license
+steward.
+
+10.3. Modified Versions
+
+If you create software not governed by this License, and you want to
+create a new license for such software, you may create and use a
+modified version of this License if you rename the license and remove
+any references to the name of the license steward (except to note that
+such modified license differs from this License).
+
+10.4. Distributing Source Code Form that is Incompatible With Secondary
+Licenses
+
+If You choose to distribute Source Code Form that is Incompatible With
+Secondary Licenses under the terms of this version of the License, the
+notice described in Exhibit B of this License must be attached.
+
+Exhibit A - Source Code Form License Notice
+-------------------------------------------
+
+  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 http://mozilla.org/MPL/2.0/.
+
+If it is not possible or desirable to put the notice in a particular
+file, then You may include the notice in a location (such as a LICENSE
+file in a relevant directory) where a recipient would be likely to look
+for such a notice.
+
+You may add additional accurate notices of copyright ownership.
+
+Exhibit B - "Incompatible With Secondary Licenses" Notice
+---------------------------------------------------------
+
+  This Source Code Form is "Incompatible With Secondary Licenses", as
+  defined by the Mozilla Public License, v. 2.0.
diff --git a/README.md b/README.md
new file mode 100755
index 0000000..40c544c
--- /dev/null
+++ 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
new file mode 100755
index 0000000..0136cb7
--- /dev/null
+++ 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
new file mode 100755
index 0000000..38f312f
--- /dev/null
+++ 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);                        // センサ融合する
+}
diff --git a/cui/FrameworkCustomizer.h b/cui/FrameworkCustomizer.h
new file mode 100755
index 0000000..0553bad
--- /dev/null
+++ b/cui/FrameworkCustomizer.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ * 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.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef FRAMEWORK_CUSTOMIZER_H_
+#define FRAMEWORK_CUSTOMIZER_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "RefScanMaker.h" 
+#include "RefScanMakerBS.h" 
+#include "RefScanMakerLM.h" 
+#include "DataAssociator.h" 
+#include "DataAssociatorLS.h" 
+#include "DataAssociatorGT.h" 
+#include "CostFunction.h" 
+#include "CostFunctionED.h" 
+#include "CostFunctionPD.h" 
+#include "PoseOptimizer.h" 
+#include "PoseOptimizerSD.h" 
+#include "PoseOptimizerSL.h" 
+#include "PointCloudMap.h" 
+#include "PointCloudMapBS.h" 
+#include "PointCloudMapGT.h" 
+#include "PointCloudMapLP.h" 
+#include "LoopDetector.h" 
+#include "LoopDetectorSS.h" 
+#include "ScanPointResampler.h"
+#include "ScanPointAnalyser.h"
+#include "PoseEstimatorICP.h" 
+#include "PoseFuser.h" 
+#include "ScanMatcher2D.h" 
+#include "SlamFrontEnd.h" 
+
+class FrameworkCustomizer
+{
+  // フレームワーク改造用の部品
+  RefScanMakerBS rsmBS;
+  RefScanMakerLM rsmLM;
+  DataAssociatorLS dassLS;
+  DataAssociatorGT dassGT;
+  CostFunctionED cfuncED;
+  CostFunctionPD cfuncPD;
+  PoseOptimizerSD poptSD;
+  PoseOptimizerSL poptSL;
+  PointCloudMapBS pcmapBS;
+  PointCloudMapGT pcmapGT;
+  PointCloudMapLP pcmapLP;
+  PointCloudMap *pcmap;            // SlamLauncherで参照するためメンバ変数にする
+  LoopDetector lpdDM;              // ダミー。何もしない
+  LoopDetectorSS lpdSS;
+  ScanPointResampler spres;
+  ScanPointAnalyser spana;
+
+  PoseEstimatorICP poest;
+  PoseFuser pfu;
+  ScanMatcher2D smat;
+  SlamFrontEnd *sfront;
+
+public:
+  FrameworkCustomizer() : pcmap(nullptr) {
+  }
+
+  ~FrameworkCustomizer() {
+  }
+
+  void setSlamFrontEnd(SlamFrontEnd *f) {
+    sfront = f;
+  }
+
+  PointCloudMap *getPointCloudMap() {
+    return(pcmap);
+  }
+
+//////
+
+  void makeFramework();
+  void customizeA();
+  void customizeB();
+  void customizeC();
+  void customizeD();
+  void customizeE();
+  void customizeF();
+  void customizeG();
+  void customizeH();
+  void customizeI();
+};
+
+#endif
diff --git a/cui/MapDrawer.cpp b/cui/MapDrawer.cpp
new file mode 100755
index 0000000..c51b4a0
--- /dev/null
+++ b/cui/MapDrawer.cpp
@@ -0,0 +1,82 @@
+/****************************************************************************
+ * 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 MapDrawer.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "MapDrawer.h"
+
+using namespace std;
+
+////////// Gnuplotによる地図描画 //////////
+
+// 地図と軌跡を描画
+void MapDrawer::drawMapGp(const PointCloudMap &pcmap) {
+  const vector<LPoint2D> &lps = pcmap.globalMap;         // 地図の点群
+  const vector<Pose2D> &poses = pcmap.poses;             // ロボット軌跡
+  drawGp(lps, poses);
+}
+
+// スキャン1個を描画
+void MapDrawer::drawScanGp(const Scan2D &scan) {
+  vector<Pose2D> poses;
+  Pose2D pose;                   // 原点
+  poses.emplace_back(pose);      // drawGpを使うためにvectorに入れる
+  drawGp(scan.lps, poses);
+}
+
+// ロボット軌跡だけを描画
+void MapDrawer::drawTrajectoryGp(const vector<Pose2D> &poses) {
+  vector<LPoint2D> lps;          // drawGpを使うためのダミー（空）
+  drawGp(lps, poses);
+}
+
+//////////
+
+void MapDrawer::drawGp(const vector<LPoint2D> &lps, const vector<Pose2D> &poses, bool flush) {
+  printf("drawGp: lps.size=%lu\n", lps.size());     // 点数の確認用
+
+  // 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");
+
+  // 点群の描画
+  int step1=1;                  // 点の間引き間隔。描画が重いとき大きくする
+  for (size_t i=0; i<lps.size(); i+=step1) {
+    const LPoint2D &lp = lps[i];
+    fprintf(gp, "%lf %lf\n", lp.x, lp.y);    // 点の描画
+  }
+  fprintf(gp, "e\n");
+
+  // ロボット軌跡の描画
+  int step2=10;                      // ロボット位置の間引き間隔
+  for (size_t i=0; i<poses.size(); i+=step2) {
+    const Pose2D &pose = poses[i];
+    double cx = pose.tx;             // 並進位置
+    double cy = pose.ty;
+    double cs = pose.Rmat[0][0];     // 回転角によるcos
+    double sn = pose.Rmat[1][0];     // 回転角によるsin
+
+    // ロボット座標系の位置と向きを描く
+    double dd = 0.4;
+    double x1 = cs*dd;              // ロボット座標系のx軸
+    double y1 = sn*dd;
+    double x2 = -sn*dd;             // ロボット座標系のy軸
+    double y2 = cs*dd;
+    fprintf(gp, "%lf %lf %lf %lf\n", cx, cy, x1, y1);
+    fprintf(gp, "%lf %lf %lf %lf\n", cx, cy, x2, y2);
+  }
+  fprintf(gp, "e\n");
+
+  if (flush)
+    fflush(gp);         // バッファのデータを書き出す。これしないと描画がよくない
+}
diff --git a/cui/MapDrawer.h b/cui/MapDrawer.h
new file mode 100755
index 0000000..4aea023
--- /dev/null
+++ b/cui/MapDrawer.h
@@ -0,0 +1,97 @@
+/****************************************************************************
+ * 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 MapDrawer.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include <stdio.h>
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+#include "PointCloudMap.h"
+
+class MapDrawer
+{
+private:
+  FILE *gp;               // gnuplotへのパイプ
+  double xmin;            // 描画範囲[m]
+  double xmax;
+  double ymin;
+  double ymax;
+  double aspectR;         // xy比
+
+public:
+  MapDrawer() : gp(nullptr), xmin(-10), xmax(10), ymin(-10), ymax(10), aspectR(-1.0) {
+  }
+
+  ~MapDrawer() {
+    finishGnuplot();
+  }
+  
+/////////
+
+  void initGnuplot() {
+#ifdef _WIN32
+    gp = _popen("gnuplot", "w");      // パイプオープン.Windows
+#elif __linux__
+    gp = popen("gnuplot", "w");       // パイプオープン.Linux
+#endif
+  }
+
+  void finishGnuplot() {
+    if (gp != nullptr)
+#ifdef _WIN32
+      _pclose(gp);
+#elif __linux__
+      pclose(gp);
+#endif
+  }
+
+  void setAspectRatio(double a) {
+    aspectR = a;
+    fprintf(gp, "set size ratio %lf\n", aspectR);
+  }
+
+  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) {  // 描画範囲を±xR、±yRにする
+    xmin = -xR;
+    xmax = xR;
+    ymin = -yR; 
+    ymax = yR;
+    fprintf(gp, "set xrange [%lf:%lf]\n", xmin, xmax);
+    fprintf(gp, "set yrange [%lf:%lf]\n", ymin, ymax);
+  }
+
+  void setRange(double xm, double xM, double ym, double yM) {  // 描画範囲を全部指定
+    xmin = xm;
+    xmax = xM;
+    ymin = ym; 
+    ymax = yM;
+    fprintf(gp, "set xrange [%lf:%lf]\n", xmin, xmax);
+    fprintf(gp, "set yrange [%lf:%lf]\n", ymin, ymax);
+  }
+
+////////
+
+  void drawMapGp(const PointCloudMap &pcmap);
+  void drawScanGp(const Scan2D &scan);
+  void drawTrajectoryGp(const std::vector<Pose2D> &poses);
+  void drawGp(const std::vector<LPoint2D> &lps, const std::vector<Pose2D> &poses, bool flush=true);
+};
+
diff --git a/cui/SlamLauncher.cpp b/cui/SlamLauncher.cpp
new file mode 100755
index 0000000..4fc0f01
--- /dev/null
+++ b/cui/SlamLauncher.cpp
@@ -0,0 +1,164 @@
+/****************************************************************************
+ * 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 SlamLauncher.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include <boost/algorithm/string/predicate.hpp>
+#include <boost/timer.hpp>
+#include "SlamLauncher.h"
+#include "ScanPointResampler.h"
+
+using namespace std;                       // C++標準ライブラリの名前空間を使う
+
+//////////
+
+void SlamLauncher::run() {
+  mdrawer.initGnuplot();                   // gnuplot初期化
+  mdrawer.setAspectRatio(-0.9);            // x軸とy軸の比（負にすると中身が一定）
+  
+  size_t cnt = 0;                          // 処理の論理時刻
+  if (startN > 0)
+    skipData(startN);                      // startNまでデータを読み飛ばす
+
+  double totalTime=0, totalTimeDraw=0, totalTimeRead=0;
+  Scan2D scan;
+  bool eof = sreader.loadScan(cnt, scan);  // ファイルからスキャンを1個読み込む
+  boost::timer tim;
+  while(!eof) {
+    if (odometryOnly) {                      // オドメトリによる地図構築（SLAMより優先）
+      if (cnt == 0) {
+        ipose = scan.pose;
+        ipose.calRmat();
+      }
+      mapByOdometry(&scan);
+    }
+    else 
+      sfront.process(scan);                // SLAMによる地図構築
+
+    double t1 = 1000*tim.elapsed();
+
+    if (cnt%drawSkip == 0) {               // drawSkipおきに結果を描画
+      mdrawer.drawMapGp(*pcmap);
+    }
+    double t2 = 1000*tim.elapsed();
+
+    ++cnt;                                 // 論理時刻更新
+    eof = sreader.loadScan(cnt, scan);     // 次のスキャンを読み込む
+
+    double t3 = 1000*tim.elapsed();
+    totalTime = t3;                        // 全体処理時間
+    totalTimeDraw += (t2-t1);              // 描画時間の合計
+    totalTimeRead += (t3-t2);              // ロード時間の合計
+
+    printf("---- SlamLauncher: cnt=%lu ends ----\n", cnt);
+  }
+  sreader.closeScanFile();
+
+  printf("Elapsed time: mapping=%g, drawing=%g, reading=%g\n", (totalTime-totalTimeDraw-totalTimeRead), totalTimeDraw, totalTimeRead);
+  printf("SlamLauncher finished.\n");
+
+  // 処理終了後も描画画面を残すためにsleepで無限ループにする。ctrl-Cで終了。
+  while(true) {
+#ifdef _WIN32
+    Sleep(1000);                            // WindowsではSleep
+#elif __linux__
+    usleep(1000000);                        // Linuxではusleep
+#endif
+  }
+}
+
+// 開始からnum個のスキャンまで読み飛ばす
+void SlamLauncher::skipData(int num) {
+  Scan2D scan;
+  bool eof = sreader.loadScan(0, scan);
+  for (int i=0; !eof && i<num; i++) {       // num個空読みする
+    eof = sreader.loadScan(0, scan);
+  }
+}
+
+///////// オドメトリのよる地図構築 //////////
+
+void SlamLauncher::mapByOdometry(Scan2D *scan) {
+//  Pose2D &pose = scan->pose;               // スキャン取得時のオドメトリ位置
+  Pose2D pose;
+  Pose2D::calRelativePose(scan->pose, ipose, pose);
+  vector<LPoint2D> &lps = scan->lps;       // スキャン点群
+  vector<LPoint2D> glps;                   // 地図座標系での点群
+  for (size_t j=0; j<lps.size(); j++) {
+    LPoint2D &lp = lps[j];
+    LPoint2D glp;
+    pose.globalPoint(lp, glp);             // センサ座標系から地図座標系に変換
+    glps.emplace_back(glp);
+  }
+
+  // 点群地図pcmapにデータを格納
+  pcmap->addPose(pose);
+  pcmap->addPoints(glps);
+  pcmap->makeGlobalMap();
+
+  printf("Odom pose: tx=%g, ty=%g, th=%g\n", pose.tx, pose.ty, pose.th);
+}
+
+////////// スキャン描画 ////////
+
+void SlamLauncher::showScans() {
+  mdrawer.initGnuplot();
+  mdrawer.setRange(6);                     // 描画範囲。スキャンが6m四方の場合
+  mdrawer.setAspectRatio(-0.9);            // x軸とy軸の比（負にすると中身が一定）
+
+  ScanPointResampler spres;
+
+  size_t cnt = 0;                          // 処理の論理時刻
+  if (startN > 0)
+    skipData(startN);                      // startNまでデータを読み飛ばす
+
+  Scan2D scan;
+  bool eof = sreader.loadScan(cnt, scan);
+  while(!eof) {
+//    spres.resamplePoints(&scan);         // コメントアウトはずせば、スキャン点間隔を均一にする。
+ 
+    // 描画間隔をあける
+#ifdef _WIN32
+    Sleep(100);                            // WindowsではSleep
+#elif __linux__
+    usleep(100000);                        // Linuxではusleep
+#endif
+
+    mdrawer.drawScanGp(scan);              // スキャン描画
+
+    printf("---- scan num=%lu ----\n", cnt);
+    eof = sreader.loadScan(cnt, scan);
+    ++cnt;
+  }
+  sreader.closeScanFile();
+  printf("SlamLauncher finished.\n");
+}
+
+//////// スキャン読み込み /////////
+
+bool SlamLauncher::setFilename(char *filename) {
+  bool flag = sreader.openScanFile(filename);        // ファイルをオープン
+
+  return(flag);
+}
+
+////////////
+
+void SlamLauncher::customizeFramework() {
+  fcustom.setSlamFrontEnd(&sfront);
+  fcustom.makeFramework();
+//  fcustom.customizeG();                         // 退化の対処をしない
+//  fcustom.customizeH();                         // 退化の対処をする
+  fcustom.customizeI();                           // ループ閉じ込みをする
+
+  pcmap = fcustom.getPointCloudMap();           // customizeの後にやること
+}
diff --git a/cui/SlamLauncher.h b/cui/SlamLauncher.h
new file mode 100755
index 0000000..7d4b5a8
--- /dev/null
+++ b/cui/SlamLauncher.h
@@ -0,0 +1,77 @@
+/****************************************************************************
+ * 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 SlamLauncher.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef SLAM_LAUNCHER_H_
+#define SLAM_LAUNCHER_H_
+
+#include <vector>
+#ifdef _WIN32
+#include <windows.h>
+#elif __linux__
+#include <unistd.h>
+#endif
+
+#include "SensorDataReader.h"
+#include "PointCloudMap.h"
+#include "SlamFrontEnd.h"
+#include "SlamBackEnd.h"
+#include "MapDrawer.h"
+#include "FrameworkCustomizer.h"
+
+/////////////
+
+class SlamLauncher
+{
+private:
+  int startN;                      // 開始スキャン番号
+  int drawSkip;                    // 描画間隔
+  bool odometryOnly;               // オドメトリによる地図構築か
+  Pose2D ipose;                    // オドメトリ地図構築の補助データ。初期位置の角度を0にする
+
+  Pose2D lidarOffset;              // レーザスキャナとロボットの相対位置
+
+  SensorDataReader sreader;        // ファイルからのセンサデータ読み込み
+  PointCloudMap *pcmap;            // 点群地図
+  SlamFrontEnd sfront;             // SLAMフロントエンド
+  MapDrawer mdrawer;               // gnuplotによる描画
+  FrameworkCustomizer fcustom;     // フレームワークの改造
+
+public:
+  SlamLauncher() : startN(0), drawSkip(10), odometryOnly(false), pcmap(nullptr) {
+  }
+
+  ~SlamLauncher() {
+  }
+
+///////////
+
+  void setStartN(int n) {
+    startN = n;
+  }
+
+  void setOdometryOnly(bool p) {
+    odometryOnly = p;
+  }
+
+///////////
+
+  void run();
+  void showScans();
+  void mapByOdometry(Scan2D *scan);
+  bool setFilename(char *filename);
+  void skipData(int num);
+  void customizeFramework();
+};
+
+#endif
diff --git a/cui/main.cpp b/cui/main.cpp
new file mode 100755
index 0000000..eb00fc4
--- /dev/null
+++ b/cui/main.cpp
@@ -0,0 +1,77 @@
+/****************************************************************************
+ * 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 main.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "SlamLauncher.h"
+
+int main(int argc, char *argv[]) {
+  bool scanCheck=false;              // スキャン表示のみか
+  bool odometryOnly=false;           // オドメトリによる地図構築か
+  char *filename;                    // データファイル名
+  int startN=0;                      // 開始スキャン番号
+
+  if (argc < 2) {
+    printf("Error: too few arguments.\n");
+    return(1);
+  }
+
+  // コマンド引数の処理
+  int idx=1;
+  // コマンドオプションの解釈（'-'のついた引数）
+  if (argv[1][0] == '-') {
+    for (int i=1; ; i++) {
+      char option = argv[1][i];
+      if (option == NULL)
+        break;
+      else if (option == 's')        // スキャン表示のみ
+        scanCheck = true;
+      else if (option == 'o')        // オドメトリによる地図構築
+        odometryOnly = true;
+    }
+    if (argc == 2) {
+      printf("Error: no file name.\n");
+      return(1);
+    }
+    ++idx;
+  }
+  if (argc >= idx+1)                 // '-'ある場合idx=2、ない場合idx=1
+    filename = argv[idx];
+  if (argc == idx+2)                 // argcがidxより2大きければstartNがある
+    startN = atoi(argv[idx+1]);
+  else if (argc >= idx+2) {
+    printf("Error: invalid arguments.\n");
+    return(1);
+  }
+  
+  printf("SlamLauncher: startN=%d, scanCheck=%d, odometryOnly=%d\n", startN, scanCheck, odometryOnly);
+  printf("filename=%s\n", filename);
+
+  // ファイルを開く
+  SlamLauncher sl;
+  bool flag = sl.setFilename(filename);
+  if (!flag)
+    return(1);
+
+  sl.setStartN(startN);              // 開始スキャン番号の設定
+
+  // 処理本体
+  if (scanCheck)
+    sl.showScans();
+  else {                             // スキャン表示以外はSlamLauncher内で場合分け
+    sl.setOdometryOnly(odometryOnly);
+    sl.customizeFramework();
+    sl.run();
+  }
+
+  return(0);
+}
diff --git a/doc/customize.md b/doc/customize.md
new file mode 100755
index 0000000..ecd032d
--- /dev/null
+++ b/doc/customize.md
@@ -0,0 +1,46 @@
+## プログラムのカスタマイズ
+
+LittleSLAMは、大きく、スキャンマッチング、センサ融合、ループ閉じ込みという
+要素技術から構成されています。
+LittleSLAMは、学習用プログラムとして、これらの技術に関して
+いくつかのカスタマイズができるように作られています。
+下表にカスタマイズのタイプを示します。
+それぞれの詳細は、参考書籍[1]を参照してください。
+
+
+| タイプ              | 内容         |
+|:--------------------|:-------------|
+| 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();                   // 使わないのでコメントアウト
+  fcustom.customizeI();                     // このカスタマイズを指定
+
+  pcmap = fcustom.getPointCloudMap();
+}
+
+```  
+
+また、関数customizeX（X=A to I）は、cui/FrameworkCustomizer.cppで定義されています。  
+ユーザが新しいcustomizeXを作って試すことも可能です。
+
+
diff --git a/doc/images/build.png b/doc/images/build.png
new file mode 100755
index 0000000..0ff06c4
Binary files /dev/null and b/doc/images/build.png differ
diff --git a/doc/images/cmake-lnx.png b/doc/images/cmake-lnx.png
new file mode 100755
index 0000000..1f7614f
Binary files /dev/null and b/doc/images/cmake-lnx.png differ
diff --git a/doc/images/cmake.png b/doc/images/cmake.png
new file mode 100755
index 0000000..ef3aefd
Binary files /dev/null and b/doc/images/cmake.png differ
diff --git a/doc/images/cmake1.png b/doc/images/cmake1.png
new file mode 100755
index 0000000..ef3aefd
Binary files /dev/null and b/doc/images/cmake1.png differ
diff --git a/doc/images/command-lnx.png b/doc/images/command-lnx.png
new file mode 100755
index 0000000..35975d0
Binary files /dev/null and b/doc/images/command-lnx.png differ
diff --git a/doc/images/command.png b/doc/images/command.png
new file mode 100755
index 0000000..b364990
Binary files /dev/null and b/doc/images/command.png differ
diff --git a/doc/images/exefile-lnx.png b/doc/images/exefile-lnx.png
new file mode 100755
index 0000000..9aa5c9e
Binary files /dev/null and b/doc/images/exefile-lnx.png differ
diff --git a/doc/images/exefile.png b/doc/images/exefile.png
new file mode 100755
index 0000000..f14e722
Binary files /dev/null and b/doc/images/exefile.png differ
diff --git a/doc/images/folders-lnx.png b/doc/images/folders-lnx.png
new file mode 100755
index 0000000..46effe9
Binary files /dev/null and b/doc/images/folders-lnx.png differ
diff --git a/doc/images/folders.png b/doc/images/folders.png
new file mode 100755
index 0000000..23ede74
Binary files /dev/null and b/doc/images/folders.png differ
diff --git a/doc/images/result-lnx.png b/doc/images/result-lnx.png
new file mode 100755
index 0000000..52aecd0
Binary files /dev/null and b/doc/images/result-lnx.png differ
diff --git a/doc/images/result.png b/doc/images/result.png
new file mode 100755
index 0000000..014d629
Binary files /dev/null and b/doc/images/result.png differ
diff --git a/doc/install-linux.md b/doc/install-linux.md
new file mode 100755
index 0000000..a9dead1
--- /dev/null
+++ b/doc/install-linux.md
@@ -0,0 +1,96 @@
+## LittleSLAMの使い方（Linuxの場合）
+
+### (1) 関連ソフトウェアのインストール
+
+- C++コンパイラ(gcc)、Boost、Eigen、CMake, gnuplot  
+以下のコマンドで、まとめてインストールします。
+
+</code></pre>
+<pre><code> $ sudo apt-get install build-essential cmake libboost-all-dev libeigen3-dev gnuplot gnuplot-x11
+</code></pre>
+
+- p2o  
+[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ファイルを適当なディレクトリに展開します。
+ここでは、たとえば、"\~/LittleSLAM"に展開するとします。
+LittleSLAM-master.zipの中の"LittleSLAM-master"ディレクトリの下の
+4個のディレクトリと3個のファイルを"\~/LittleSLAM"の下にコピーします。  
+(B) gitを使って、リポジトリをcloneします。
+
+- p2oの展開  
+"\~/LittleSLAM"の下にp2oディレクトリを作成します。  
+前述のp2o-master.zipの中のファイル"p2o.h"を"\~/LittleSLAM/p2o"にコピーします。  
+
+- buildディレクトリの作成  
+"\~/LittleSLAM"の下にbuildディレクトリを作成します。  
+ここまでのディレクトリ構成は以下のようになります。
+
+![ディレクトリ構成](images/folders-lnx.png)
+
+- CMakeの実行  
+コンソールで、buildディレクトリに移動し、cmakeを実行します。
+
+</code></pre>
+<pre><code> ~/LittleSLAM$ cd build
+</code></pre>
+<pre><code> ~/LittleSLAM/build$ cmake ..
+</code></pre>
+
+下図にcmakeの実行例を示します。
+
+![cmake](images/cmake-lnx.png)
+
+あるいは、CMakeのGUI版をインストールして、Windowsの場合と同じように
+GUIでCMakeを実行することもできます。
+
+- ビルド  
+コンソールで、buildディレクトリにおいてmakeを実行します。  
+</code></pre>
+<pre><code> ~/LittleSLAM/build$ make
+</code></pre>
+ビルドが成功すると、"\~/LittleSLAM/build/cui"ディレクトリに、実行ファイルLittleSLAMが生成されます。  
+
+![cmake](images/exefile-lnx.png)
+
+### (3) 実行
+
+以下のコマンドで、LittleSLAMを実行します。
+
+</code></pre>
+<pre><code> ./LittleSLAM [-so] データファイル名 [開始スキャン番号]
+</code></pre>
+
+-sオプションを指定すると、スキャンを1個ずつ描画します。各スキャン形状を確認したい場合に
+使います。  
+-oオプションを指定すると、スキャンをオドメトリデータで並べた地図
+（SLAMによる地図ではない）を生成します。  
+オプション指定がなければ、SLAMを実行します。  
+開始スキャン番号を指定すると、その番号までスキャンを読み飛ばしてから実行します。
+
+例として、以下のコマンドでSLAMを実行します。  
+この例では"\~/LittleSLAM/dataset"ディレクトリに"corridor.lsc"というデータファイルが置かれています。  
+</code></pre>
+<pre><code> ~/LittleSLAM/build/cui$ ./LittleSLAM ~/LittleSLAM/dataset/corridor.lsc
+</code></pre>
+
+![cmake](images/command-lnx.png)  
+  
+コマンドを実行すると、LittleSLAMはファイルからデータを読み込んで地図を少しずつ
+構築していきます。その様子がgnuplotに描画されます。  
+最終的に、下図のような地図が生成されます。  
+処理が終わっても、プログラムは終了せず、地図はそのまま表示されています。  
+プログラムを終了するにはCtrl-Cを押してください。
+
+　
+![cmake](images/result-lnx.png)
diff --git a/doc/install-win.md b/doc/install-win.md
new file mode 100755
index 0000000..dd16ba6
--- /dev/null
+++ b/doc/install-win.md
@@ -0,0 +1,131 @@
+## LittleSLAMの使い方 （Windowsの場合）
+
+### (1) 関連ソフトウェアのインストール
+
+- Boost  
+[Boost](http://www.boost.org/)をダウンロードして、適当なフォルダに解凍します。  
+LiitleSLAMでは、Boostのヘッダファイルだけを使用するので、ビルドは必要ありません。
+
+- Eigen3  
+[Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page)を
+ダウンロードして、適当なフォルダに解凍します。  
+Eigenはヘッダファイルだけで使用するライブラリなので、ビルドは必要ありません。
+
+- gnuplot  
+[gnuplot](http://www.gnuplot.info/)をダウンロードしてインストールします。 
+LittleSLAMからは、APIではなく、実行コマンドで呼び出すので、
+Windowsの環境変数Pathにgnuplotのパスを設定しておきます。  
+たとえば、gnuplotをフォルダC:\gnuplotにインストールした場合、"Path=... ;C:\gnuplot\bin; ..."とします。  
+（インストーラが自動で設定してくれることもあります）
+
+- CMake  
+[CMake](https://cmake.org/)をダウンロードしてインストールします。
+
+- p2o  
+[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の指定   
+もし、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より抜粋 --
+
+find_package(Eigen3)  
+IF(NOT EIGEN3_INCLUDE_DIR)          # Eigen3のパスが見つからない
+  set(EIGEN3_INCLUDE_DIR $ENV{EIGEN3_ROOT_DIR})
+ENDIF() 
+```  
+
+- Visual studioの起動  
+"C:\abc\LittleSLAM\build"の下にLittleSLAM.slnができているので、
+それをダブルクリックすると、Visual studioが起動します。
+
+- ビルド  
+下図のように、Visual studioで、Release, x64（64ビットの場合）を指定し、BuildメニューからBuild Solutionを実行します。
+
+![cmake](images/build.png)
+
+
+ビルドが成功すると、"build\cui\Release"フォルダに、実行ファイルLittleSLAM.exeが生成されます。  
+
+![cmake](images/exefile.png)
+
+### (3) 実行
+
+Windowsコマンドプロンプトから以下のコマンドにより、LittleSLAMを実行します。
+
+</code></pre>
+<pre><code> LittleSLAM [-so] データファイル名 [開始スキャン番号]
+</code></pre>
+
+-sオプションを指定すると、スキャンを1個ずつ描画します。各スキャン形状を確認したい場合に
+使います。  
+-oオプションを指定すると、スキャンをオドメトリデータで並べた地図
+（SLAMによる地図ではない）を生成します。  
+オプション指定がなければ、SLAMを実行します。  
+開始スキャン番号を指定すると、その番号までスキャンを読み飛ばしてから実行します。
+
+例として、以下のコマンドでSLAMを実行します。  
+この例では"C:\abc\dataset"フォルダに"corridor.lsc"というデータファイルが置かれています。  
+</code></pre>
+<pre><code> C:\abc\LittleSLAM\build\cui\Release> LittleSLAM C:\abc\dataset\corridor.lsc
+</code></pre>
+
+![cmake](images/command.png)  
+  
+コマンドを実行すると、LittleSLAMはファイルからデータを読み込んで地図を少しずつ
+構築していきます。その様子がgnuplotに描画されます。  
+最終的に、下図のような地図が生成されます。  
+処理が終わっても、プログラムは終了せず、地図はそのまま表示されています。  
+プログラムを終了するにはCtrl-Cを押してください。
+
+
+![cmake](images/result.png)
+
+
diff --git a/framework/CMakeLists.txt b/framework/CMakeLists.txt
new file mode 100755
index 0000000..e645b2a
--- /dev/null
+++ b/framework/CMakeLists.txt
@@ -0,0 +1,66 @@
+PROJECT(framework)
+
+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() 
+
+SET(fw_HDRS
+    MyUtil.h
+    LPoint2D.h
+    Pose2D.h
+    Scan2D.h
+    PointCloudMap.h
+    RefScanMaker.h
+    ScanPointResampler.h
+    ScanPointAnalyser.h
+    PoseEstimatorICP.h
+    PoseOptimizer.h
+    CostFunction.h
+    PoseGraph.h
+    P2oDriver2D.h
+    ScanMatcher2D.h
+    PoseFuser.h
+    CovarianceCalculator.h
+    DataAssociator.h
+    NNGridTable.h
+    SensorDataReader.h
+    SlamFrontEnd.h
+    SlamBackEnd.h
+    LoopDetector.h
+)
+
+SET(fw_SRCS 
+    MyUtil.cpp
+    Pose2D.cpp
+    Scan2D.cpp
+    ScanPointResampler.cpp
+    ScanPointAnalyser.cpp
+    PoseEstimatorICP.cpp
+    PoseGraph.cpp
+    P2oDriver2D.cpp
+    ScanMatcher2D.cpp
+    PoseFuser.cpp
+    CovarianceCalculator.cpp
+    NNGridTable.cpp
+    SensorDataReader.cpp
+    SlamFrontEnd.cpp
+    SlamBackEnd.cpp
+    LoopDetector.cpp
+)
+
+include_directories(
+    ${Boost_INCLUDE_DIR}
+    ${EIGEN3_INCLUDE_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR}/../p2o
+)
+
+link_directories(
+)
+
+ADD_LIBRARY(framework ${fw_SRCS} ${fw_HDRS})
+
diff --git a/framework/CostFunction.h b/framework/CostFunction.h
new file mode 100755
index 0000000..fa3a79d
--- /dev/null
+++ b/framework/CostFunction.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ * 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 CostFunction.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _COST_FUNCTION_H_
+#define _COST_FUNCTION_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+
+class CostFunction
+{
+protected:
+  std::vector<const LPoint2D*> curLps;         // 対応がとれた現在スキャンの点群
+  std::vector<const LPoint2D*> refLps;         // 対応がとれた参照スキャンの点群
+  double evlimit;                              // マッチングで対応がとれたと見なす距離閾値
+  double pnrate;                               // 誤差がevlimit以内で対応がとれた点の比率
+
+public:
+  CostFunction() : evlimit(0), pnrate(0) {
+  }
+
+  ~CostFunction() {
+  }
+
+///////
+
+  void setEvlimit(double e) {
+    evlimit = e;
+  }
+
+  // DataAssociatorで対応のとれた点群cur, refを設定
+  void setPoints(std::vector<const LPoint2D*> &cur, std::vector<const LPoint2D*> &ref) {
+    curLps = cur;
+    refLps = ref;
+  }
+
+  double getPnrate() {
+    return(pnrate);
+  }
+
+///////////
+
+  virtual double calValue(double tx, double ty, double th) = 0;
+
+};
+
+#endif
diff --git a/framework/CovarianceCalculator.cpp b/framework/CovarianceCalculator.cpp
new file mode 100755
index 0000000..217120b
--- /dev/null
+++ b/framework/CovarianceCalculator.cpp
@@ -0,0 +1,240 @@
+/****************************************************************************
+ * 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 CovarianceCalculator.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "CovarianceCalculator.h"
+
+using namespace std;
+
+////////// ICPによる推定値の共分散 /////////
+
+// ICPによるロボット位置の推定値の共分散covを求める。
+// 推定位置pose、現在スキャン点群curLps、参照スキャン点群refLps
+double CovarianceCalculator::calIcpCovariance(const Pose2D &pose, std::vector<const LPoint2D*> &curLps, std::vector<const LPoint2D*> &refLps, Eigen::Matrix3d &cov) {
+  double tx = pose.tx;
+  double ty = pose.ty;
+  double th = pose.th;
+  double a = DEG2RAD(th);
+  vector<double> Jx;                                         // ヤコビ行列のxの列
+  vector<double> Jy;                                         // ヤコビ行列のyの列
+  vector<double> Jt;                                         // ヤコビ行列のthの列
+
+  for (size_t i=0; i<curLps.size(); i++) {
+    const LPoint2D *clp = curLps[i];                         // 現在スキャンの点
+    const LPoint2D *rlp = refLps[i];                         // 参照スキャンの点
+
+    if (rlp->type == ISOLATE)                                // 孤立点は除外
+      continue;
+
+    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);                            // 偏微分（x成分）
+    Jy.push_back((pdy - pd0)/dd);                            // 偏微分（y成分）
+    Jt.push_back((pdt - pd0)/da);                            // 偏微分（th成分）
+  }
+
+  // ヘッセ行列の近似J^TJの計算
+  Eigen::Matrix3d hes = Eigen::Matrix3d::Zero(3,3);          // 近似ヘッセ行列。0で初期化
+  for (size_t i=0; i<Jx.size(); i++) {
+    hes(0,0) += Jx[i]*Jx[i];
+    hes(0,1) += Jx[i]*Jy[i];
+    hes(0,2) += Jx[i]*Jt[i];
+    hes(1,1) += Jy[i]*Jy[i];
+    hes(1,2) += Jy[i]*Jt[i];
+    hes(2,2) += Jt[i]*Jt[i];
+  }
+  // J^TJが対称行列であることを利用
+  hes(1,0) = hes(0,1);
+  hes(2,0) = hes(0,2);
+  hes(2,1) = hes(1,2);
+
+  // 共分散行列は（近似）ヘッセ行列の逆行列
+//  cov = hes.inverse();
+  cov = MyUtil::svdInverse(hes);                              // SVDを使う方が少しよい
+
+  double vals[2], vec1[2], vec2[2];
+  double ratio = calEigen(cov, vals, vec1, vec2);            // 固有値計算して、退化具合を調べる
+
+  // 必要に応じて共分散行列のスケールを調整する
+//  double kk = 1;          // 退化で極端にずれる場合
+  double kk = 0.1;       // 通常
+  cov *= kk;
+
+  return(ratio);
+}
+
+// 垂直距離を用いた観測モデルの式
+double CovarianceCalculator::calPDistance(const LPoint2D *clp, const LPoint2D *rlp, double tx, double ty, double th) {
+  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;           // 座標変換した点からrlpへの垂直距離
+
+  return(pdis);
+}
+
+///////// 運動モデルの計算 /////////
+
+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;                    // 並進速度[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;
+  if (wt < wthre)
+    wt = wthre;
+
+  double dx = vt;
+  double dy = vt;
+  double da = wt;
+
+  Eigen::Matrix3d C1;
+  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;
+
+  // 確認用
+  printf("calMotionCovarianceSimple\n");
+  printf("vt=%g, wt=%g\n", vt, wt);
+  double vals[2], vec1[2], vec2[2];
+  calEigen(cov, vals, vec1, vec2);
+  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));
+}
+
+///////// 運動モデルの計算 /////////
+
+// 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);   // 走行距離
+  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;
+
+ // 累積する場合は、時刻t-1の共分散行列sigmaから、時刻tの共分散行列を計算
+  Eigen::Matrix3d A = Eigen::Matrix3d::Zero(3,3);
+  if (accum) {
+    Eigen::Matrix3d Jxk;
+    calJxk(th, vt, dt, Jxk);
+    A = Jxk*cov*Jxk.transpose();
+  }
+
+  Eigen::Matrix2d Uk;
+  calUk(vt, wt, Uk);
+
+  Eigen::Matrix<double, 3, 2> Juk;
+  calJuk(th, dt, Juk);
+  Eigen::Matrix3d B = Juk*Uk*Juk.transpose();
+
+  cov = A + B;
+
+}
+
+//////////////////////////////////
+
+void CovarianceCalculator::calUk(double vt, double wt, Eigen::Matrix2d &Uk) {
+  Uk << a1*vt*vt, 0,
+        0, a2*wt*wt;
+}
+
+// ロボット姿勢に関するヤコビ行列。vtはロボットの速度、thはロボットの方向(ラジアン)、dtは時間
+void CovarianceCalculator::calJxk(double th, double vt, double dt, Eigen::Matrix3d &Jxk) {
+  double cs = cos(th);
+  double sn = sin(th);
+  Jxk << 1, 0, -vt*dt*sn,
+         0, 1, vt*dt*cs,
+         0, 0, 1;
+}
+
+// 
+void CovarianceCalculator::calJuk(double th, double dt, Eigen::Matrix<double, 3, 2> &Juk) {
+  double cs = cos(th);
+  double sn = sin(th);
+  Juk << dt*cs, 0,
+         dt*sn, 0,
+         0, dt;
+}
+
+////////////////
+
+// 共分散行列covの並進成分だけを固有値分解し、固有値をvalsに、固有ベクトルをvec1とvec2に入れる。
+double CovarianceCalculator::calEigen(const Eigen::Matrix3d &cov, double *vals, double *vec1, double *vec2) {
+  // 並進部分だけ取り出す
+  double cv2[2][2];
+  for (int i=0; i<2; i++) 
+    for (int j=0; j<2; j++) 
+      cv2[i][j] = cov(i,j);
+
+  MyUtil::calEigen2D(cv2, vals, vec1, vec2);        // 固有値分解
+  double ratio = vals[0]/vals[1];
+
+  // 確認用
+  printf("Eigen: ratio=%g, val1=%g, val2=%g\n", ratio, vals[0], vals[1]);
+  printf("Eigen: vec1=(%g, %g), ang=%g\n", vec1[0], vec1[1], RAD2DEG(atan2(vec1[1], vec1[0])));
+
+  return(ratio);
+}
+
+//////////////
+
+// 共分散行列の累積。前回位置の共分散行列prevCovに移動量の共分散行列mcovを加えて、現在位置の共分散行列curCovを求める。
+void CovarianceCalculator::accumulateCovariance(const Pose2D &curPose, const Pose2D &prevPose, const Eigen::Matrix3d &prevCov, const Eigen::Matrix3d &mcov, Eigen::Matrix3d &curCov) {
+  Eigen::Matrix3d J1, J2;
+  J1 << 1, 0, -(curPose.ty - prevPose.ty),
+        0, 1, curPose.tx - prevPose.tx,
+        0, 0, 1;
+
+  double prevCos = cos(DEG2RAD(prevPose.th));
+  double prevSin = sin(DEG2RAD(prevPose.th));
+  J2 << prevCos, -prevSin, 0,
+        prevSin, prevCos, 0,
+        0, 0, 1;
+
+  curCov = J1*prevCov*J1.transpose() + J2*mcov*J2.transpose();
+}
+
+/////////////
+
+// 共分散行列covをposeの角度分だけ回転させる
+void CovarianceCalculator::rotateCovariance(const Pose2D &pose, const Eigen::Matrix3d &cov, Eigen::Matrix3d &icov, bool reverse) {
+  double cs = cos(DEG2RAD(pose.th));            // poseの回転成分thによるcos
+  double sn = sin(DEG2RAD(pose.th));
+  Eigen::Matrix3d J;                            // 回転のヤコビ行列
+  J << cs, -sn, 0,
+       sn, cs, 0,
+       0, 0, 1;
+
+  Eigen::Matrix3d JT = J.transpose();
+
+  if (reverse)
+    icov = JT*cov*J;                              // 逆回転変換
+  else
+    icov = J*cov*JT;                              // 回転変換
+}
diff --git a/framework/CovarianceCalculator.h b/framework/CovarianceCalculator.h
new file mode 100755
index 0000000..9c5f362
--- /dev/null
+++ b/framework/CovarianceCalculator.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ * 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 CovarianceCalculator.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef COVARIANCE_CALCULATOR_H_
+#define COVARIANCE_CALCULATOR_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+
+// ICPによる推定値の共分散、および、オドメトリによる推定値の共分散を計算する。
+class CovarianceCalculator
+{
+private:
+  double dd;                      // 数値微分の刻み
+  double da;                      // 数値微分の刻み
+  double a1;                      // オドメトリ共分散の係数
+  double a2;                      // オドメトリ共分散の係数
+
+public:
+  CovarianceCalculator() : dd(0.00001), da(0.00001) {
+  }
+
+  ~CovarianceCalculator() {
+  }
+  
+  void setAlpha(double a1_, double a2_) {
+    a1 = a1_;
+    a2 = a2_;
+  }
+
+////////
+
+  double calIcpCovariance(const Pose2D &pose, std::vector<const LPoint2D*> &curLps, std::vector<const LPoint2D*> &refLps, Eigen::Matrix3d &cov);
+  double calPDistance(const LPoint2D *clp, const LPoint2D *rlp, double tx, double ty, double th);
+
+  void calMotionCovarianceSimple(const Pose2D &motion, double dT, Eigen::Matrix3d &cov);
+  void calMotionCovariance(double th, double dx, double dy, double dth, double dt, Eigen::Matrix3d &cov, bool accum=false);
+  void calUk(double vt, double wt, Eigen::Matrix2d &Uk);
+  void calJxk(double th, double vt, double dt, Eigen::Matrix3d &Jxk);
+  void calJuk(double th, double dt, Eigen::Matrix<double, 3, 2> &Juk);
+
+  double calEigen(const Eigen::Matrix3d &cov, double *vals, double *vec1, double *vec2);
+
+  static void accumulateCovariance(const Pose2D &curPose, const Pose2D &prevPose, const Eigen::Matrix3d &prevCov, const Eigen::Matrix3d &mcov, Eigen::Matrix3d &curCov);
+  static void rotateCovariance(const Pose2D &pose, const Eigen::Matrix3d &cov, Eigen::Matrix3d &icov, bool reverse=false);
+};
+
+#endif
diff --git a/framework/DataAssociator.h b/framework/DataAssociator.h
new file mode 100755
index 0000000..0be19d2
--- /dev/null
+++ b/framework/DataAssociator.h
@@ -0,0 +1,40 @@
+/****************************************************************************
+ * 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 DataAssociator.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef DATA_ASSOCIATOR_H_
+#define DATA_ASSOCIATOR_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+
+class DataAssociator
+{
+public:
+  std::vector<const LPoint2D*> curLps;            // 対応がとれた現在スキャンの点群
+  std::vector<const LPoint2D*> refLps;            // 対応がとれた参照スキャンの点群
+
+  DataAssociator() {
+  }
+
+  ~DataAssociator() {
+  }
+
+  virtual void setRefBase(const std::vector<LPoint2D> &lps) = 0;
+  virtual double findCorrespondence(const Scan2D *curScan, const Pose2D &predPose) = 0;
+};
+
+#endif
diff --git a/framework/LPoint2D.h b/framework/LPoint2D.h
new file mode 100755
index 0000000..2e54e87
--- /dev/null
+++ b/framework/LPoint2D.h
@@ -0,0 +1,103 @@
+/****************************************************************************
+ * 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 LPoint2D.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef LPOINT2D_H_
+#define LPOINT2D_H_
+
+////////////////////////
+
+struct Vector2D
+{
+  double x,y;
+};
+
+////////////////////////
+
+enum ptype {UNKNOWN=0, LINE=1, CORNER=2, ISOLATE=3};    // 点のタイプ：未知、直線、コーナ、孤立
+
+struct LPoint2D
+{
+  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();
+  }
+
+  LPoint2D(int id, double _x, double _y): x(_x), y(_y) {
+    init();
+    sid = id;
+  }
+ 
+//////////
+
+  void init() {
+    sid = -1;
+    atd = 0;
+    type = UNKNOWN;
+    nx = 0;
+    ny = 0;
+  }
+
+  void setData(int id, double _x, double _y) {
+    init();
+    sid = id;
+    x = _x;
+    y = _y;
+  }
+
+  void setXY(double _x, double _y) {
+    x = _x;
+    y = _y;
+  }
+
+  // rangeとangleからxyを求める(右手系)
+  void calXY(double range, double angle) {
+    double a = DEG2RAD(angle);
+    x = range*cos(a);
+    y = range*sin(a);
+  }
+
+  // rangeとangleからxyを求める(左手系）
+  void calXYi(double range, double angle) {
+    double a = DEG2RAD(angle);
+    x = range*cos(a);
+    y = -range*sin(a);
+  }
+
+  void setSid(int i) {
+    sid = i;
+  }
+
+  void setAtd(double t) {
+    atd = t;
+  }
+
+  void setType(ptype t) {
+    type = t;
+  }
+
+  void setNormal(double x, double y) {
+    nx = x;
+    ny = y;
+  }
+
+};
+
+#endif
diff --git a/framework/LoopDetector.cpp b/framework/LoopDetector.cpp
new file mode 100755
index 0000000..ec793ba
--- /dev/null
+++ b/framework/LoopDetector.cpp
@@ -0,0 +1,25 @@
+/****************************************************************************
+ * 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 LoopDetector.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "LoopDetector.h"
+
+using namespace std;
+
+////////////
+
+// ループ検出のダミー関数
+bool LoopDetector::detectLoop(Scan2D *curScan, Pose2D &curPose, int cnt) {
+  return(false);
+}
+
diff --git a/framework/LoopDetector.h b/framework/LoopDetector.h
new file mode 100755
index 0000000..14bad99
--- /dev/null
+++ b/framework/LoopDetector.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ * 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 LoopDetector.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef LOOP_DETECTOR_H_
+#define LOOP_DETECTOR_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+#include "PoseGraph.h"
+
+///////
+
+// ループアーク設定情報
+struct LoopInfo
+{
+  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) {
+  }
+
+  ~LoopInfo() {
+  }
+  
+  void setArcked(bool t) {
+    arcked = t;
+  }
+};
+
+//////////////
+
+// デバッグ用データ
+struct LoopMatch
+{
+  Scan2D curScan;
+  Scan2D refScan;
+  LoopInfo info;
+
+  LoopMatch() {
+  }
+
+  LoopMatch(Scan2D &cs, Scan2D &rs, LoopInfo &i) {
+    curScan = cs;
+    refScan = rs;
+    info = i;
+  }
+};
+
+////////////
+
+class LoopDetector
+{
+protected:  
+  PoseGraph *pg;                               // ポーズグラフ
+  std::vector<LoopMatch> loopMatches;          // デバッグ用
+
+public:
+  LoopDetector() {
+  }
+
+  ~LoopDetector() {
+  }
+
+////////
+
+  // デバッグ用
+  std::vector<LoopMatch> &getLoopMatches() {
+    return(loopMatches);
+  }
+
+  void setPoseGraph(PoseGraph *p) {
+    pg = p;
+  }
+
+///////
+
+  virtual bool detectLoop(Scan2D *curScan, Pose2D &curPose, int cnt);
+
+};
+
+#endif
diff --git a/framework/MyUtil.cpp b/framework/MyUtil.cpp
new file mode 100755
index 0000000..4d50d98
--- /dev/null
+++ b/framework/MyUtil.cpp
@@ -0,0 +1,131 @@
+/****************************************************************************
+ * 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 MyUtil.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "MyUtil.h"
+#include <Eigen/SVD>
+
+using namespace std;
+using namespace Eigen;
+
+/////////
+
+// 角度の加算。-180から180に正規化
+int MyUtil::add(int a1, int a2) {
+  int sum = a1 + a2;
+  if (sum < -180)
+    sum += 360;
+  else if (sum >= 180)
+    sum -= 360;
+
+  return(sum);
+}
+
+// 角度の加算。-180から180に正規化
+double MyUtil::add(double a1, double a2) {
+  double sum = a1 + a2;
+  if (sum < -180)
+    sum += 360;
+  else if (sum >= 180)
+    sum -= 360;
+
+  return(sum);
+}
+
+// 角度の加算（ラジアン）。-piからpiに正規化
+double MyUtil::addR(double a1, double a2) {
+  double sum = a1 + a2;
+  if (sum < -M_PI)
+    sum += 2*M_PI;
+  else if (sum >= M_PI)
+    sum -= 2*M_PI;
+
+  return(sum);
+}
+
+////////////
+
+// SVDを用いた逆行列計算
+Eigen::Matrix3d MyUtil::svdInverse(const Matrix3d &A) {
+  size_t m = A.rows();
+  size_t n = A.cols();
+
+  JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);
+
+  MatrixXd eU = svd.matrixU();
+  MatrixXd eV = svd.matrixV();
+  VectorXd eS = svd.singularValues();  
+
+  MatrixXd M1(m, n);
+  for (size_t i=0; i<n; i++) {
+//    if (eS[i] < 1.0E-10)                   // AがSingularかどうかのチェック。今回はしない
+//      return;
+    for (size_t j=0; j<n; j++) {
+      M1(i,j) = eU(j,i)/eS[i];
+    }
+  }
+    
+  Matrix3d IA;
+  for (size_t i=0; i<n; i++) {
+    for (size_t j=0; j<n; j++) {
+      IA(i,j) = 0;
+      for (size_t k=0; k<n; k++) 
+        IA(i,j) += eV(i,k)*M1(k,j);
+    }
+  }
+
+  return(IA);
+}
+
+/////////
+
+// 2次正方行列の固有値分解
+void MyUtil::calEigen2D( double (*mat)[2], double *vals, double *vec1, double *vec2) {
+  double a = mat[0][0];
+  double b = mat[0][1];
+  double c = mat[1][0];
+  double d = mat[1][1];
+
+  double B = sqrt((a+d)*(a+d) - 4*(a*d-b*c));
+  double x1 = ((a+d) + B)/2;
+  double x2 = ((a+d) - B)/2;
+  vals[0] = x1;                    // 固有値
+  vals[1] = x2;
+
+  double m00 = a-x1;
+  double m01 = b;
+  double L = sqrt(m00*m00 + m01*m01);
+  vec1[0] = m01/L;                 // 固有ベクトル
+  vec1[1] = -m00/L;
+
+  m00 = a-x2;
+  m01 = b;
+  L = sqrt(m00*m00 + m01*m01);
+  vec2[0] = m01/L;                 // 固有ベクトル
+  vec2[1] = -m00/L;
+
+/*
+  // 検算
+  double ax1 = a*vec1[0] + b*vec1[1];
+  double ax2 = x1*vec1[0];
+  double ay1 = c*vec1[0] + d*vec1[1];
+  double ay2 = x1*vec1[1];
+  printf("ax1=%g, ax2=%g\n", ax1, ax2);
+  printf("ay1=%g, ay2=%g\n", ay1, ay2);
+
+  double prod = vec1[0]*vec2[0] + vec1[1]*vec2[1];
+  printf("prod=%g\n", prod);
+*/
+
+}
+
diff --git a/framework/MyUtil.h b/framework/MyUtil.h
new file mode 100755
index 0000000..5efe027
--- /dev/null
+++ b/framework/MyUtil.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ * 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 MyUtil.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef MY_UTIL_H_
+#define MY_UTIL_H_
+
+#include <cmath>
+#include <Eigen/Core>
+#include <Eigen/Eigen>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979             // 円周率
+#endif
+
+#ifndef NULL
+#define NULL 0                     // 基本的には、C++11のnullptrを使う
+#endif
+
+#define DEG2RAD(x) ((x)*M_PI/180)  // 度からラジアン
+#define RAD2DEG(x) ((x)*180/M_PI)  // ラジアンから度
+
+typedef unsigned char uchar;
+
+//////////
+
+class MyUtil
+{
+public:
+  MyUtil(void) {
+  }
+
+  ~MyUtil(void){
+  }
+
+///////////
+
+  static int add(int a1, int a2);
+  static double add(double a1, double a2);
+  static double addR(double a1, double a2);
+
+  static Eigen::Matrix3d svdInverse(const Eigen::Matrix3d &A);
+  static void calEigen2D( double (*mat)[2], double *vals, double *vec1, double *vec2);
+
+};
+
+#endif
+
diff --git a/framework/NNGridTable.cpp b/framework/NNGridTable.cpp
new file mode 100755
index 0000000..fecc9dc
--- /dev/null
+++ b/framework/NNGridTable.cpp
@@ -0,0 +1,128 @@
+/****************************************************************************
+ * 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 NNGridTable.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "NNGridTable.h"
+
+using namespace std;
+
+////////////
+
+// 格子テーブルにスキャン点lpを登録する
+void NNGridTable::addPoint(const LPoint2D *lp) {
+  // テーブル検索のインデックス計算。まず、対象領域内にあるかチェックする。
+  int xi = static_cast<int>(lp->x/csize) + tsize;
+  if (xi < 0 || xi > 2*tsize)                      // 対象領域の外
+    return;
+  int yi = static_cast<int>(lp->y/csize) + tsize;
+  if (yi < 0 || yi > 2*tsize)                      // 対象領域の外
+    return;
+
+  size_t idx = static_cast<size_t>(yi*(2*tsize +1) + xi);   // テーブルのインデックス
+  table[idx].lps.push_back(lp);                             // 目的のセルに入れる
+}
+
+///////////
+
+// スキャン点clpをpredPoseで座標変換した位置に最も近い点を格子テーブルから見つける
+const LPoint2D *NNGridTable::findClosestPoint(const LPoint2D *clp, const Pose2D &predPose) {
+  LPoint2D glp;                           // clpの予測位置
+  predPose.globalPoint(*clp, glp);         // relPoseで座標変換
+
+  // clpのテーブルインデックス。対象領域内にあるかチェックする。
+  int cxi = static_cast<int>(glp.x/csize) + tsize;
+  if (cxi < 0 || cxi > 2*tsize)
+    return(nullptr);
+  int cyi = static_cast<int>(glp.y/csize) + tsize;
+  if (cyi < 0 || cyi > 2*tsize)
+    return(nullptr);
+
+  size_t pn=0;                            // 探したセル内の点の総数。確認用
+  double dmin=1000000;
+  const LPoint2D *lpmin = nullptr;        // 最も近い点（目的の点）
+  double dthre=0.2;                       // これより遠い点は除外する[m]
+  int R=static_cast<int>(dthre/csize);
+
+  // ±R四方を探す
+  for (int i=-R; i<=R; i++) {
+    int yi = cyi+i;                       // cyiから広げる
+    if (yi < 0 || yi > 2*tsize)
+      continue;
+    for (int j=-R; j<=R; j++) {
+      int xi = cxi+j;                     // cxiから広げる
+      if (xi < 0 || xi > 2*tsize)
+        continue;
+
+      size_t idx = yi*(2*tsize+1) + xi;             // テーブルインデックス
+      NNGridCell &cell = table[idx];                // そのセル
+      vector<const LPoint2D*> &lps = cell.lps;      // セルがもつスキャン点群
+      for (size_t k=0; k<lps.size(); k++) {
+        const LPoint2D *lp = lps[k];
+        double d = (lp->x - glp.x)*(lp->x - glp.x) + (lp->y - glp.y)*(lp->y - glp.y);
+
+        if (d <= dthre*dthre && d < dmin) {         // dthre内で距離が最小となる点を保存
+          dmin = d;
+          lpmin = lp;
+        }
+      }
+      pn += lps.size();
+    }
+  }
+//  printf("pn=%d\n", pn);                 // 探したセル内の点の総数。確認用
+
+  return(lpmin);
+}
+
+////////////
+
+// 格子テーブルの各セルの代表点を作ってpsに格納する。
+void NNGridTable::makeCellPoints(int nthre, vector<LPoint2D> &ps) {
+  // 現状はセル内の各点のスキャン番号の平均をとる。
+  // スキャン番号の最新値をとる場合は、その部分のコメントをはずし、
+  // 平均とる場合（2行）をコメントアウトする。
+
+  size_t nn=0;                           // テーブル内の全セル数。確認用
+  for (size_t i=0; i<table.size(); i++) {
+    vector<const LPoint2D*> &lps = table[i].lps;      // セルのスキャン点群
+    nn += lps.size();
+    if (lps.size() >= nthre) {           // 点数がnthreより多いセルだけ処理する
+      double gx=0, gy=0;                 // 点群の重心位置
+      double nx=0, ny=0;                 // 点群の法線ベクトルの平均
+      int sid=0;
+      for (size_t j=0; j<lps.size(); j++) {
+        const LPoint2D *lp = lps[j];
+        gx += lp->x;                     // 位置を累積
+        gy += lp->y;
+        nx += lp->nx;                    // 法線ベクトル成分を累積
+        ny += lp->ny;
+        sid += lp->sid;                  // スキャン番号の平均とる場合
+//        if (lp->sid > sid)             // スキャン番号の最新値とる場合
+//          sid = lp->sid;
+//        printf("sid=%d\n", lp->sid);
+      }
+      gx /= lps.size();                  // 平均
+      gy /= lps.size();
+      double L = sqrt(nx*nx + ny*ny);
+      nx /=  L;                          // 平均（正規化）
+      ny /=  L;
+      sid /= lps.size();                 // スキャン番号の平均とる場合
+
+      LPoint2D newLp(sid, gx, gy);       // セルの代表点を生成
+      newLp.setNormal(nx, ny);           // 法線ベクトル設定
+      newLp.setType(LINE);               // タイプは直線にする
+      ps.emplace_back(newLp);            // psに追加
+    }
+  }
+
+//  printf("nn=%d\n", nn);               // テーブル内の全セル数。確認用
+}
diff --git a/framework/NNGridTable.h b/framework/NNGridTable.h
new file mode 100755
index 0000000..bc8ce71
--- /dev/null
+++ b/framework/NNGridTable.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ * 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 NNGridTable.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _NN_GRID_TABLE_H_
+#define _NN_GRID_TABLE_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "Pose2D.h"
+
+struct NNGridCell
+{
+  std::vector<const LPoint2D*> lps;         // このセルに格納されたスキャン点群
+
+  void clear() {
+    lps.clear();                            // 空にする
+  }
+};
+
+///////
+
+// 格子テーブル
+class NNGridTable
+{
+private:
+  double csize;                       // セルサイズ[m]
+  double rsize;                       // 対象領域のサイズ[m]。正方形の1辺の半分。
+  int tsize;                          // テーブルサイズの半分
+  std::vector<NNGridCell> table;      // テーブル本体
+
+public:
+  NNGridTable() : csize(0.05), rsize(40){            // セル5cm、対象領域40x2m四方
+    tsize = static_cast<int>(rsize/csize);           // テーブルサイズの半分
+    size_t w = static_cast<int>(2*tsize+1);          // テーブルサイズ
+    table.resize(w*w);                               // 領域確保
+    clear();                                         // tableの初期化
+  }
+
+  ~NNGridTable() {
+  }
+  
+  void clear() {
+    for (size_t i=0; i<table.size(); i++)
+      table[i].clear();                         // 各セルを空にする
+  }
+  
+////////////
+
+  void addPoint(const LPoint2D *lp);
+  const LPoint2D *findClosestPoint(const LPoint2D *clp, const Pose2D &predPose);
+  void makeCellPoints(int nthre, std::vector<LPoint2D> &ps);
+};
+
+#endif
+
diff --git a/framework/P2oDriver2D.cpp b/framework/P2oDriver2D.cpp
new file mode 100755
index 0000000..c4a9c7f
--- /dev/null
+++ b/framework/P2oDriver2D.cpp
@@ -0,0 +1,64 @@
+/****************************************************************************
+ * 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 P2oDriver2D.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "P2oDriver2D.h"
+#include "p2o.h"
+
+using namespace std;
+
+////////
+
+// kslamを用いてポーズグラフpgをポーズ調整し、その結果のロボット軌跡をnewPosesに格納する。
+void P2oDriver2D::doP2o( PoseGraph &pg, vector<Pose2D> &newPoses, int N) {
+  vector<PoseNode*> &nodes = pg.nodes;                        // ポーズノード
+  vector<PoseArc*> &arcs = pg.arcs;                           // ポーズアーク
+
+  // ポーズノードをp2o用に変換
+  vector<p2o::Pose2D> pnodes;                                  // p2oのポーズノード集合
+  for (size_t i=0; i<nodes.size(); i++) {
+    PoseNode *node = nodes[i];
+    Pose2D pose = node->pose;                                  // ノードの位置
+    pnodes.push_back(p2o::Pose2D(pose.tx, pose.ty, DEG2RAD(pose.th)));   // 位置だけ入れる
+  }
+
+
+  // ポーズアークをkslam用に変換
+  p2o::Con2DVec pcons;                             // p2oのポーズアーク集合
+  for (size_t i=0; i<arcs.size(); i++) {
+    PoseArc *arc = arcs[i];
+    PoseNode *src = arc->src;
+    PoseNode *dst = arc->dst;
+    Pose2D &relPose = arc->relPose;
+    p2o::Con2D con;
+    con.id1 = src->nid;
+    con.id2 = dst->nid;
+    con.t = p2o::Pose2D(relPose.tx, relPose.ty, DEG2RAD(relPose.th));
+    for (int k=0; k<3; k++)
+      for (int m=0; m<3; m++)
+        con.info(k, m) = arc->inf(k,m);
+    pcons.push_back(con);
+  }
+
+//  printf("knodes.size=%lu, kcons.size=%lu\n", knodes.size(), kcons.size());    // 確認用
+
+  p2o::Optimizer2D opt;                                          // p2oインスタンス
+  std::vector<p2o::Pose2D> result = opt.optimizePath(pnodes, pcons, N);  // N回実行
+
+   // 結果をnewPoseに格納する
+  for (size_t i=0; i<result.size(); i++) {
+    p2o::Pose2D newPose = result[i];                                      // i番目のノードの修正された位置
+    Pose2D pose(newPose.x, newPose.y, RAD2DEG(newPose.th));
+    newPoses.emplace_back(pose);
+  }
+}
diff --git a/framework/P2oDriver2D.h b/framework/P2oDriver2D.h
new file mode 100755
index 0000000..08b1500
--- /dev/null
+++ b/framework/P2oDriver2D.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ * 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 P2oDriver2D.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef P2O_DRIVER2D_H_
+#define P2O_DRIVER2D_H_
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+
+#include "MyUtil.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+#include "PoseGraph.h"
+
+//////////
+
+// ポーズグラフ最適化ライブラリkslamを起動する。
+class P2oDriver2D
+{
+public:
+
+  P2oDriver2D() {
+  }
+
+  ~P2oDriver2D() {
+  }
+
+///////
+
+  void doP2o( PoseGraph &graph, std::vector<Pose2D> &newPoses, int N);
+
+};
+
+#endif
diff --git a/framework/PointCloudMap.h b/framework/PointCloudMap.h
new file mode 100755
index 0000000..6a1e77a
--- /dev/null
+++ b/framework/PointCloudMap.h
@@ -0,0 +1,73 @@
+/****************************************************************************
+ * 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 PointCloudMap.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef POINT_CLOUD_MAP_H_
+#define POINT_CLOUD_MAP_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+
+// 点群地図の基底クラス
+class PointCloudMap
+{
+public:
+  static const int MAX_POINT_NUM=1000000;          // globalMapの最大点数
+
+  int nthre;                                       // 格子テーブルセル点数閾値(GTとLPのみ）
+
+  std::vector<Pose2D> poses;                       // ロボット軌跡
+  Pose2D lastPose;                                 // 最後に推定したロボット位置
+  Scan2D lastScan;                                 // 最後に処理したスキャン
+
+  std::vector<LPoint2D> globalMap;                 // 全体地図。間引き後の点
+  std::vector<LPoint2D> localMap;                  // 現在位置近傍の局所地図。スキャンマッチングに使う
+
+  PointCloudMap() : nthre(1) {
+    globalMap.reserve(MAX_POINT_NUM);              // 最初に確保
+  }
+
+  ~PointCloudMap() {
+  }
+
+///////
+
+  void setNthre(int n) {
+    nthre = n;
+  }
+
+  void setLastPose(const Pose2D &p) {
+    lastPose = p;
+  }
+
+  Pose2D getLastPose() const {
+    return(lastPose);
+  }
+
+  void setLastScan(const Scan2D &s) {
+    lastScan = s;
+  }
+
+/////////////
+
+  virtual void addPose(const Pose2D &p) = 0;
+  virtual void addPoints(const std::vector<LPoint2D> &lps) = 0;
+  virtual void makeGlobalMap() = 0;
+  virtual void makeLocalMap() = 0;
+  virtual void remakeMaps(const std::vector<Pose2D> &newPoses) = 0;
+};
+
+#endif
diff --git a/framework/Pose2D.cpp b/framework/Pose2D.cpp
new file mode 100755
index 0000000..65be1ca
--- /dev/null
+++ b/framework/Pose2D.cpp
@@ -0,0 +1,87 @@
+/****************************************************************************
+ * 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 Pose2D.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "Pose2D.h"
+
+// グローバル座標系での点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];  // 回転の逆行列
+  double y = dx*Rmat[0][1] + dy*Rmat[1][1];
+  return LPoint2D(p.sid, x, y);
+}
+
+////////
+
+// 自分（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);
+}
+
+// 自分（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;
+}
+
+///////
+
+// 基準座標系bposeから見た現座標系nposeの相対位置relPoseを求める（Inverse compounding operator）
+void Pose2D::calRelativePose(const Pose2D &npose, const Pose2D &bpose, Pose2D &relPose) {
+  const double (*R0)[2] = bpose.Rmat;           // 基準座標系
+  const double (*R1)[2] = npose.Rmat;           // 現座標系
+  double (*R2)[2] = relPose.Rmat;               // 相対位置
+
+  // 並進
+  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;
+  else if (th >= 180)
+    th -= 360;
+  relPose.th = th;
+
+  relPose.calRmat();
+}
+
+// 基準座標系bposeから相対位置relPoseだけ進んだ、座標系nposeを求める（Compounding operator）
+void Pose2D::calGlobalPose(const Pose2D &relPose, const Pose2D &bpose, Pose2D &npose) {
+  const double (*R0)[2] = bpose.Rmat;           // 基準座標系
+  const double (*R1)[2] = relPose.Rmat;         // 相対位置
+  double (*R2)[2] = npose.Rmat;                 // 新座標系
+
+  // 並進
+  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;
+
+  // 角度
+  double th = bpose.th + relPose.th;
+  if (th < -180)
+    th += 360;
+  else if (th >= 180)
+    th -= 360;
+  npose.th = th;
+
+  npose.calRmat();
+}
diff --git a/framework/Pose2D.h b/framework/Pose2D.h
new file mode 100755
index 0000000..c33518a
--- /dev/null
+++ b/framework/Pose2D.h
@@ -0,0 +1,113 @@
+/****************************************************************************
+ * 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 Pose2D.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _POSE2D_H_
+#define _POSE2D_H_
+
+#include "MyUtil.h"
+#include "LPoint2D.h"
+
+/////////
+
+struct Pose2D
+{
+  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++) {
+      for(int j=0;j<2;j++) {
+        Rmat[i][j] = (i==j)? 1.0:0.0;
+      }
+    }
+  }
+
+  Pose2D(double tx, double ty, double th) {
+    this->tx = tx;
+    this->ty = ty;
+    this->th = th;
+    calRmat();
+  }
+
+  Pose2D(double mat[2][2], double tx, double ty, double th) {
+    for(int i=0;i<2;i++) {
+      for(int j=0;j<2;j++) {
+        Rmat[i][j] = mat[i][j];
+      }
+    }
+    this->tx = tx;
+    this->ty = ty;
+    this->th = th;
+  }
+
+/////////////////
+
+  void reset() {
+    tx = ty = th = 0;
+    calRmat();
+  }
+
+  void setVal(double x, double y, double a) {
+    tx = x;
+    ty = y;
+    th = a;
+    calRmat();
+  }
+
+  void calRmat() {
+    double a = DEG2RAD(th);
+    Rmat[0][0] = Rmat[1][1] = cos(a);
+    Rmat[1][0] = sin(a);
+    Rmat[0][1] = -Rmat[1][0];
+  }
+
+  void setTranslation(double tx, double ty) {
+    this->tx = tx;
+    this->ty = ty;
+  }
+
+  void setAngle(double th) {
+    this->th = th;
+  }
+
+///////////
+
+  LPoint2D relativePoint(const LPoint2D &p) const;
+  LPoint2D globalPoint(const LPoint2D &p) const;
+  void globalPoint(const LPoint2D &pi, LPoint2D &po) const;
+
+  static void calRelativePose(const Pose2D &npose, const Pose2D &bpose, Pose2D &relPose);
+  static void calGlobalPose(const Pose2D &relPose, const Pose2D &bpose, Pose2D &npose);
+  
+};
+
+///////
+
+struct PoseCov
+{
+  Pose2D pose;
+  Eigen::Matrix3d cov;
+
+  PoseCov() {
+  }
+
+  PoseCov(Pose2D &p, Eigen::Matrix3d &c) {
+    pose = p;
+    cov = c;
+  }
+};
+
+#endif
\ No newline at end of file
diff --git a/framework/PoseEstimatorICP.cpp b/framework/PoseEstimatorICP.cpp
new file mode 100755
index 0000000..80fde8d
--- /dev/null
+++ b/framework/PoseEstimatorICP.cpp
@@ -0,0 +1,72 @@
+/****************************************************************************
+ * 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 PoseEstimatorICP.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include <boost/timer.hpp>
+#include "PoseEstimatorICP.h"
+
+using namespace std;
+
+//////////////
+
+// 初期値initPoseを与えて、ICPによりロボット位置の推定値estPoseを求める
+double PoseEstimatorICP::estimatePose(Pose2D &initPose, Pose2D &estPose){
+  boost::timer tim;
+
+  double evmin = HUGE_VAL;             // コスト最小値。初期値は大きく
+  double evthre = 0.000001;            // コスト変化閾値。変化量がこれ以下なら繰り返し終了
+//  double evthre = 0.00000001;            // コスト変化閾値。変化量がこれ以下なら繰り返し終了
+  popt->setEvthre(evthre);
+  popt->setEvlimit(0.2);               // evlimitは外れ値の閾値[m]
+
+  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は振動対策
+    if (i > 0)
+      evold = ev;
+    double mratio = dass->findCorrespondence(curScan, pose);      // データ対応づけ
+    Pose2D newPose;
+    popt->setPoints(dass->curLps, dass->refLps);                  // 対応結果を渡す
+    ev = popt->optimizePose(pose, newPose);                       // その対応づけにおいてロボット位置の最適化
+    pose = newPose;
+
+    if (ev < evmin) {                                             // コスト最小結果を保存
+      poseMin = newPose;
+      evmin = ev;
+    }
+
+//    printf("dass.curLps.size=%lu, dass.refLps.size=%lu\n", dass->curLps.size(), dass->refLps.size());
+//    printf("mratio=%g\n", mratio);
+//    printf("i=%d: ev=%g, evold=%g\n", i, ev, evold);
+  }
+
+  pnrate = popt->getPnrate();
+  usedNum = dass->curLps.size();
+
+  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);      // 確認用
+
+  double t1 = 1000*tim.elapsed();
+  printf("PoseEstimatorICP: t1=%g\n", t1);                 // 処理時間
+
+  if (evmin < HUGE_VAL)
+    totalError += evmin;                                   // 誤差合計
+  totalTime += t1;                                         // 処理時間合計
+  printf("totalError=%g, totalTime=%g\n", totalError, totalTime);    // 確認用
+
+  return(evmin);
+}
diff --git a/framework/PoseEstimatorICP.h b/framework/PoseEstimatorICP.h
new file mode 100755
index 0000000..6e97880
--- /dev/null
+++ b/framework/PoseEstimatorICP.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ * 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 PoseEstimatorICP.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _POSEESTIMATOR_ICP_H_
+#define _POSEESTIMATOR_ICP_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+#include "PoseOptimizer.h"
+#include "DataAssociator.h"
+
+//////
+
+class PoseEstimatorICP
+{
+private:
+  const Scan2D *curScan;       // 現在スキャン
+  size_t usedNum;              // ICPに使われた点数。LoopDetectorで信頼性チェックに使う
+  double pnrate;               // 正しく対応づけされた点の比率
+  
+  PoseOptimizer *popt;         // 最適化クラス
+  DataAssociator *dass;        // データ対応づけクラス
+
+public:
+  double totalError;           // 誤差合計
+  double totalTime;            // 処理時間合計
+
+public:
+
+  PoseEstimatorICP() : usedNum(0), pnrate(0), totalError(0), totalTime(0) {
+  }
+
+  ~PoseEstimatorICP() {
+  }
+
+///////
+
+  void setPoseOptimizer(PoseOptimizer *p) {
+    popt = p;
+  }
+
+  void setDataAssociator(DataAssociator *d) {
+    dass = d;
+  }
+     
+  double getPnrate() {
+    return(pnrate);
+  }
+
+  size_t getUsedNum() {
+    return(usedNum);
+  }
+  
+  void setScanPair(const Scan2D *c, const Scan2D *r) {
+    curScan = c;
+    dass->setRefBase(r->lps);           // データ対応づけのために参照スキャン点を登録
+  }
+
+  void setScanPair(const Scan2D *c, const std::vector<LPoint2D> &refLps) {
+    curScan = c;
+    dass->setRefBase(refLps);           // データ対応づけのために参照スキャン点を登録
+  }
+
+////////////
+
+  double estimatePose(Pose2D &initPose, Pose2D &estPose);
+};
+
+#endif
diff --git a/framework/PoseFuser.cpp b/framework/PoseFuser.cpp
new file mode 100755
index 0000000..f767437
--- /dev/null
+++ b/framework/PoseFuser.cpp
@@ -0,0 +1,123 @@
+/****************************************************************************
+ * 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 PoseFuser.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "PoseFuser.h"
+
+using namespace std;
+
+////////////// 逐次SLAM用のセンサ融合 ////////////////
+
+// 逐次SLAMでのICPとオドメトリの推定移動量を融合する。dassに参照スキャンを入れておくこと。covに移動量の共分散行列が入る。
+double PoseFuser::fusePose(Scan2D *curScan, const Pose2D &estPose, const Pose2D &odoMotion, const Pose2D &lastPose, Pose2D &fusedPose, Eigen::Matrix3d &fusedCov) {
+  // ICPの共分散
+  dass->findCorrespondence(curScan, estPose);                                      // 推定位置estPoseで現在スキャン点群と参照スキャン点群の対応づけ
+  double ratio = cvc.calIcpCovariance(estPose, dass->curLps, dass->refLps, ecov);  // ここで得られるのは、地図座標系での位置の共分散
+
+  // オドメトリの位置と共分散。速度運動モデルを使うと、短期間では共分散が小さすぎるため、簡易版で大きめに計算する
+  Pose2D predPose;                                                                 // 予測位置
+  Pose2D::calGlobalPose(odoMotion, lastPose, predPose);                            // 直前位置lastPoseに移動量を加えて予測位置を計算
+  Eigen::Matrix3d mcovL;
+  double dT=0.1;
+  cvc.calMotionCovarianceSimple(odoMotion, dT, mcovL);                             // オドメトリで得た移動量の共分散（簡易版）
+  CovarianceCalculator::rotateCovariance(estPose, mcovL, mcov);                    // 現在位置estPoseで回転させて、地図座標系での共分散mcovを得る
+
+  // 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つの正規分布の融合
+
+  fusedPose.setVal(mu[0], mu[1], RAD2DEG(mu[2]));                                  // 融合した移動量を格納
+
+  totalCov = fusedCov;
+
+  // 確認用
+  printf("fusePose\n");
+  double vals[2], vec1[2], vec2[2];
+  printf("ecov: det=%g, ", ecov.determinant());
+  cvc.calEigen(ecov, vals, vec1, vec2);
+  printf("mcov: det=%g, ", mcov.determinant());
+  cvc.calEigen(mcov, vals, vec1, vec2);
+  printf("fusedCov: det=%g, ", fusedCov.determinant());
+  cvc.calEigen(fusedCov, vals, vec1, vec2);
+
+  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("fusedPose: tx=%g, ty=%g, th=%g\n", fusedPose.tx, fusedPose.ty, fusedPose.th);
+
+  return(ratio);
+}
+
+void PoseFuser::calOdometryCovariance(const Pose2D &odoMotion, const Pose2D &lastPose, Eigen::Matrix3d &mcov) {
+  Eigen::Matrix3d mcovL;
+  double dT=0.1;
+  cvc.calMotionCovarianceSimple(odoMotion, dT, mcovL);                             // オドメトリで得た移動量の共分散（簡易版）
+  CovarianceCalculator::rotateCovariance(lastPose, mcovL, mcov);                   // 直前位置lastPoseで回転させて、位置の共分散mcovを得る
+}
+
+/////// ガウス分布の融合 ///////
+
+// 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) {
+  // 共分散行列の融合
+  Eigen::Matrix3d IC1 = MyUtil::svdInverse(cv1);
+  Eigen::Matrix3d IC2 = MyUtil::svdInverse(cv2);
+  Eigen::Matrix3d IC = IC1 + IC2;
+  cv = MyUtil::svdInverse(IC);
+
+  // 角度の補正。融合時に連続性を保つため。
+  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;
+
+  // 平均の融合
+  Eigen::Vector3d nu1 = IC1*mu11;
+  Eigen::Vector3d nu2 = IC2*mu2;
+  Eigen::Vector3d nu3 = nu1 + nu2;
+  mu = cv*nu3;
+
+  // 角度の補正。(-pi, pi)に収める
+  if (mu(2) > M_PI) 
+    mu(2) -= 2*M_PI;
+  else if (mu(2) < -M_PI)
+    mu(2) += 2*M_PI;
+
+  // 係数部の計算
+  Eigen::Vector3d W1 = IC1*mu11;
+  Eigen::Vector3d W2 = IC2*mu2;
+  Eigen::Vector3d W = IC*mu;
+  double A1 = mu1.dot(W1);
+  double A2 = mu2.dot(W2);
+  double A = mu.dot(W);
+  double K = A1+A2-A;
+
+/*
+  printf("cv1: det=%g\n", cv1.determinant());
+  printMatrix(cv1);
+  printf("cv2: det=%g\n", cv2.determinant());
+  printMatrix(cv2);
+  printf("cv: det=%g\n", cv.determinant());
+  printMatrix(cv);
+*/
+
+  return(K);
+}
+
+void PoseFuser::printMatrix(const Eigen::Matrix3d &mat) {
+  for (int i=0; i<3; i++) 
+    printf("%g %g %g\n", mat(i,0), mat(i,1), mat(i,2));
+}
diff --git a/framework/PoseFuser.h b/framework/PoseFuser.h
new file mode 100755
index 0000000..e9fb080
--- /dev/null
+++ b/framework/PoseFuser.h
@@ -0,0 +1,75 @@
+/****************************************************************************
+ * 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 PoseFuser.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef POSE_FUSER_H_
+#define POSE_FUSER_H_
+
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+#include "DataAssociator.h"
+#include "CovarianceCalculator.h"
+
+// センサ融合器。ICPとオドメトリの推定値を融合する。
+class PoseFuser
+{
+public:
+  Eigen::Matrix3d ecov;                      // ICPの共分散行列
+  Eigen::Matrix3d mcov;                      // オドメトリの共分散行列
+  Eigen::Matrix3d totalCov;
+  
+  DataAssociator *dass;                      // データ対応づけ器
+  CovarianceCalculator cvc;                 // 共分散計算器
+
+public:
+  PoseFuser() {
+  }
+
+  ~PoseFuser() {
+  }
+
+/////
+
+  void setDataAssociator(DataAssociator *d) {
+    dass = d;
+  }
+
+  void setRefScan(const Scan2D *refScan) {
+    dass->setRefBase(refScan->lps);
+  }
+
+  void setRefLps(const std::vector<LPoint2D> &refLps) {
+    dass->setRefBase(refLps);
+  }
+
+  // ICPの共分散行列の計算。setRefLpsの後に行うこと。
+  double calIcpCovariance(const Pose2D &estMotion, const Scan2D *curScan, Eigen::Matrix3d &cov) {
+    dass->findCorrespondence(curScan, estMotion);
+
+    // ICPの共分散。ここで得られるのは、世界座標系での共分散
+    double ratio = cvc.calIcpCovariance(estMotion, dass->curLps, dass->refLps, cov);
+    return(ratio);
+  }
+
+//////////
+
+  double fusePose(Scan2D *curScan, const Pose2D &estPose, const Pose2D &odoMotion, const Pose2D &lastPose, Pose2D &fusedPose, Eigen::Matrix3d &cov);
+  void calOdometryCovariance(const Pose2D &odoMotion, const Pose2D &lastPose, Eigen::Matrix3d &mcov);
+  double fuse(const Eigen::Vector3d &mu1, const Eigen::Matrix3d &cv1,  const Eigen::Vector3d &mu2, const Eigen::Matrix3d &cv2, Eigen::Vector3d &mu, Eigen::Matrix3d &cv);
+  void printMatrix(const Eigen::Matrix3d &mat);
+
+};
+
+#endif
diff --git a/framework/PoseGraph.cpp b/framework/PoseGraph.cpp
new file mode 100755
index 0000000..2f7c0c5
--- /dev/null
+++ b/framework/PoseGraph.cpp
@@ -0,0 +1,108 @@
+/****************************************************************************
+ * 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 PoseGraph.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "PoseGraph.h"
+
+using namespace std;
+
+//////////// グラフ生成 ////////////
+
+// ポーズグラフにノード追加
+PoseNode *PoseGraph::addNode(const Pose2D &pose) {
+  PoseNode *n1 = allocNode();                // ノード生成
+  addNode(n1, pose);                         // ポーズグラフにノード追加
+
+  return(n1);
+}
+
+// ポーズグラフにノード追加
+void PoseGraph::addNode(PoseNode *n1, const Pose2D &pose) {
+  n1->setNid((int)nodes.size());             // ノードID付与。ノードの通し番号と同じ
+  n1->setPose(pose);                         // ロボット位置を設定
+  nodes.push_back(n1);                       // nodesの最後に追加
+}
+
+// ノードID(nid)からノード実体を見つける
+PoseNode *PoseGraph::findNode(int nid) {
+  for (size_t i=0; i<nodes.size(); i++) {    // 単純に線形探索
+    PoseNode *n = nodes[i];
+    if (n->nid == nid)                       // nidが一致したら見つけた
+      return(n);
+  }
+
+  return(nullptr);
+}
+
+//////////
+
+// ポーズグラフにアークを追加する
+void PoseGraph::addArc(PoseArc *arc) {
+  arc->src->addArc(arc);                   // 始点ノードにarcを追加
+  arc->dst->addArc(arc);                   // 終点ノードにarcを追加
+  arcs.push_back(arc);                     // arcsの最後にarcを追加
+}
+
+// 始点ノードsrcNidと終点ノードdstNidの間にアークを生成する
+PoseArc *PoseGraph::makeArc(int srcNid, int dstNid, const Pose2D &relPose, const Eigen::Matrix3d &cov) {
+//  Eigen::Matrix3d inf = cov.inverse();         // infはcovの逆行列
+  Eigen::Matrix3d inf = MyUtil::svdInverse(cov);            // infはcovの逆行列
+
+  PoseNode *src = nodes[srcNid];                // 始点ノード
+  PoseNode *dst = nodes[dstNid];                // 終点ノード
+
+  PoseArc *arc = allocArc();                    // アークの生成
+  arc->setup(src, dst, relPose, inf);      // relPoseは計測による相対位置
+
+  return(arc);
+}
+
+// 始点ノードがsrcNid、終点ノードがdstNidであるアークを返す
+PoseArc *PoseGraph::findArc(int srcNid, int dstNid) {
+  for (size_t i=0; i<arcs.size(); i++) {
+    PoseArc *a = arcs[i];
+    if (a->src->nid == srcNid && a->dst->nid == dstNid)
+      return(a);
+  }
+  return(nullptr);
+}
+
+////////////////
+
+// 確認用
+void PoseGraph::printNodes() {
+  printf("--- printNodes ---\n");
+  printf("nodes.size=%lu\n", nodes.size());
+  for (size_t i=0; i<nodes.size(); i++) {
+    PoseNode *node = nodes[i];
+    printf("i=%lu: nid=%d, tx=%g, ty=%g, th=%g\n", i, node->nid, node->pose.tx, node->pose.ty, node->pose.th);
+
+    for (size_t j=0; j<node->arcs.size(); j++) {
+      PoseArc *a = node->arcs[j];
+      printf("arc j=%lu: srcId=%d, dstId=%d, src=%p, dst=%p\n", j, a->src->nid, a->dst->nid, a->src, a->dst);
+    }
+  }
+}
+
+// 確認用
+void PoseGraph::printArcs() {
+  printf("--- printArcs ---\n");
+  printf("arcs.size=%lu\n", arcs.size());
+  for (size_t j=0; j<arcs.size(); j++) {
+    PoseArc *a = arcs[j];
+    double dis = (a->src->pose.tx - a->dst->pose.tx)*(a->src->pose.tx - a->dst->pose.tx) + (a->src->pose.ty - a->dst->pose.ty)*(a->src->pose.ty - a->dst->pose.ty);
+
+    Pose2D &rpose = a->relPose;
+    printf("j=%lu, srcId=%d, dstId=%d, tx=%g, ty=%g, th=%g\n", j, a->src->nid, a->dst->nid, rpose.tx, rpose.ty, rpose.th);
+  }
+}
diff --git a/framework/PoseGraph.h b/framework/PoseGraph.h
new file mode 100755
index 0000000..231ab37
--- /dev/null
+++ b/framework/PoseGraph.h
@@ -0,0 +1,164 @@
+/****************************************************************************
+ * 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 PoseGraph.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef POSE_GRAPH_H_
+#define POSE_GRAPH_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "Pose2D.h"
+
+struct PoseArc;
+
+/////////
+
+// ポーズグラフの頂点
+struct PoseNode
+{
+  int nid;                       // ノードID。PoseGraphのnodesのインデックス（通し番号）
+  Pose2D pose;                   // このノードのロボット位置
+  std::vector<PoseArc*> arcs;    // このノードにつながるアーク
+
+  PoseNode(): nid(-1) {
+  }
+
+  PoseNode(const Pose2D &p) : nid(-1) {
+    pose = p;
+  }
+
+  ~PoseNode() {
+  }
+
+//////
+
+  void init() {
+    nid = -1;
+    arcs.clear();
+  }
+
+  void setPose(const Pose2D &p) {
+    pose = p;
+  }
+  
+  void setNid(int n) {
+    nid = n;
+  }
+
+  void addArc(PoseArc *a) {
+    arcs.push_back(a);
+  }
+
+};
+
+////////
+
+// ポーズグラフの辺
+struct PoseArc
+{
+  PoseNode *src;                      // このアークの始点側のノード
+  PoseNode *dst;                      // このアークの終点側のノード
+  Pose2D relPose;                     // このアークのもつ相対位置(計測値)
+  Eigen::Matrix3d inf;                // 情報行列
+
+  PoseArc(void) : src(nullptr), dst(nullptr){
+  }
+
+  PoseArc(PoseNode *s, PoseNode *d, Pose2D &rel, const Eigen::Matrix3d _inf) {
+    setup(s, d, rel, _inf);
+  }
+  
+  ~PoseArc(void) {
+  }
+
+/////////
+
+  void setup(PoseNode *s, PoseNode *d, const Pose2D &rel, const Eigen::Matrix3d _inf) {
+    src = s;
+    dst = d;
+    relPose = rel;
+    inf = _inf;
+  }
+
+};
+
+////////// ポーズグラフ //////////
+
+class PoseGraph
+{
+private:
+  static const int POOL_SIZE=100000;
+  std::vector<PoseNode> nodePool;     // ノード生成用のメモリプール
+  std::vector<PoseArc> arcPool;       // アーク生成用のメモリプール
+
+public:
+  std::vector<PoseNode*> nodes;       // ノードの集合
+  std::vector<PoseArc*> arcs;         // アークの集合。アークは片方向のみもつ
+
+  PoseGraph() {
+    nodePool.reserve(POOL_SIZE);      // メモリプールの領域を最初に確保。vectorはサイズが変わると中身が移動するのでこうしないと危険
+    arcPool.reserve(POOL_SIZE);
+  }
+
+  ~PoseGraph() {
+  }
+  
+//////////////
+  
+  void reset() {
+    nodes.clear();
+    arcs.clear();
+    nodePool.clear();
+    arcPool.clear();
+  }
+
+  // ノードの生成
+  PoseNode *allocNode() {
+    if (nodePool.size() >= POOL_SIZE) {
+      printf("Error: exceeds nodePool capacity\n");
+      return(nullptr);
+    }
+   
+    PoseNode node;
+    nodePool.emplace_back(node);      // メモリプールに追加して、それを参照する。
+    return(&(nodePool.back()));
+  }
+
+  // アークの生成
+  PoseArc *allocArc() {
+    if (arcPool.size() >= POOL_SIZE) {
+      printf("Error: exceeds arcPool capacity\n");
+      return(nullptr);
+    }
+
+    PoseArc arc;
+    arcPool.emplace_back(arc);       // メモリプールに追加して、それを参照する。
+    return(&(arcPool.back()));
+  }
+
+//////////////
+
+  PoseNode *addNode(const Pose2D &pose);
+  void addNode(PoseNode *n1, const Pose2D &pose);
+  PoseNode *findNode(int nid);
+
+  void addArc(PoseArc *arc);
+  PoseArc *makeArc(int srcNid, int dstNid, const Pose2D &relPose, const Eigen::Matrix3d &cov);
+  PoseArc *findArc(int srcNid, int dstNid);
+
+  void printNodes();
+  void printArcs();
+
+};
+
+#endif
diff --git a/framework/PoseOptimizer.h b/framework/PoseOptimizer.h
new file mode 100755
index 0000000..962e15d
--- /dev/null
+++ b/framework/PoseOptimizer.h
@@ -0,0 +1,76 @@
+/****************************************************************************
+ * 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 PoseOptimizer.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _POSE_OPTIMIZER_H_
+#define _POSE_OPTIMIZER_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "CostFunction.h"
+
+class PoseOptimizer
+{
+public:
+  int allN;             // 繰り返し総数。テスト用
+  double sum;           // 残差合計。テスト用
+
+protected:
+  double evthre;                // コスト変化閾値。変化量がこれ以下なら繰り返し終了
+  double dd;                    // 数値微分の刻み（並進）
+  double da;                    // 数値微分の刻み（回転）
+
+  CostFunction *cfunc;          // コスト関数
+
+public:
+  PoseOptimizer(): evthre(0.000001), dd(0.00001), da(0.00001), cfunc(nullptr) {
+    allN=0; sum=0;
+  }
+
+  ~PoseOptimizer() {
+  }
+
+/////
+
+  void setCostFunction(CostFunction *f) {
+    cfunc = f;
+  }
+
+  void setEvlimit(double l) {
+    cfunc->setEvlimit(l);
+  }
+
+  void setPoints(std::vector<const LPoint2D*> &curLps, std::vector<const LPoint2D*> &refLps) {
+    cfunc->setPoints(curLps, refLps);
+  }
+
+  void setEvthre(double inthre) {
+    this->evthre = inthre;
+  }
+  double getPnrate() {
+    return (cfunc->getPnrate());
+  }
+
+  void setDdDa(double d, double a){
+    dd = d;
+    da = a;
+  }
+
+////////
+
+  virtual double optimizePose(Pose2D &initPose, Pose2D &estPose) = 0;
+};
+
+#endif 
diff --git a/framework/RefScanMaker.h b/framework/RefScanMaker.h
new file mode 100755
index 0000000..8350d48
--- /dev/null
+++ b/framework/RefScanMaker.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ * 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 RefScanMaker.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _REF_SCAN_MAKER_H_
+#define _REF_SCAN_MAKER_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+#include "PointCloudMap.h"
+
+class RefScanMaker
+{
+protected:
+  const PointCloudMap *pcmap;           // 点群地図
+  Scan2D refScan;                       // 参照スキャン本体。これを外に提供
+
+public:
+  RefScanMaker() : pcmap(nullptr) {
+  }
+
+  ~RefScanMaker() {
+  }
+
+  void setPointCloudMap(const PointCloudMap *p) {
+    pcmap = p;
+  }
+
+  virtual const Scan2D *makeRefScan() = 0;
+
+};
+
+#endif
diff --git a/framework/Scan2D.cpp b/framework/Scan2D.cpp
new file mode 100755
index 0000000..8edb9a5
--- /dev/null
+++ b/framework/Scan2D.cpp
@@ -0,0 +1,21 @@
+/****************************************************************************
+ * 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 Scan2D.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "Scan2D.h"
+
+using namespace std;
+
+double Scan2D::MAX_SCAN_RANGE = 6;
+double Scan2D::MIN_SCAN_RANGE = 0.1;
+
diff --git a/framework/Scan2D.h b/framework/Scan2D.h
new file mode 100755
index 0000000..b8c35a0
--- /dev/null
+++ b/framework/Scan2D.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ * 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 Scan2D.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef SCAN2D_H_
+#define SCAN2D_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+
+//////////
+
+// スキャン
+struct Scan2D
+{
+  static double MAX_SCAN_RANGE;              // スキャン点の距離値上限[m]
+  static double MIN_SCAN_RANGE;              // スキャン点の距離値下限[m]
+
+  int sid;                                   // スキャンid
+  Pose2D pose;                               // スキャン取得時のオドメトリ値
+  std::vector<LPoint2D> lps;                 // スキャン点群
+
+  Scan2D() : sid(0) {
+  }
+
+  ~Scan2D() {
+  }
+
+///////
+
+  void setSid(int s) {
+    sid = s;
+  }
+
+  void setLps (const std::vector<LPoint2D> &ps) {
+    lps = ps;
+  }
+  
+  void setPose(Pose2D &p) {
+    pose = p;
+  }
+
+};
+
+#endif
diff --git a/framework/ScanMatcher2D.cpp b/framework/ScanMatcher2D.cpp
new file mode 100755
index 0000000..1955a9d
--- /dev/null
+++ b/framework/ScanMatcher2D.cpp
@@ -0,0 +1,153 @@
+/****************************************************************************
+ * 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 ScanMatcher2D.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "ScanMatcher2D.h"
+
+using namespace std;
+
+/////////
+
+// スキャンマッチングの実行
+bool ScanMatcher2D::matchScan(Scan2D &curScan) {
+  ++cnt;
+
+  printf("----- ScanMatcher2D: cnt=%d start -----\n", cnt);
+
+  // spresが設定されていれば、スキャン点間隔を均一化する
+  if (spres != nullptr)
+    spres->resamplePoints(&curScan);
+
+  // spanaが設定されていれば、スキャン点の法線を計算する
+  if (spana != nullptr)
+    spana->analysePoints(curScan.lps);
+
+  // 最初のスキャンは単に地図に入れるだけ
+  if (cnt == 0) {
+    growMap(curScan, initPose);
+    prevScan = curScan;                      // 直前スキャンの設定
+    return(true);
+  }
+
+  // Scanに入っているオドメトリ値を用いて移動量を計算する
+  Pose2D odoMotion;                                                   // オドメトリに基づく移動量
+  Pose2D::calRelativePose(curScan.pose, prevScan.pose, odoMotion);    // 前スキャンとの相対位置が移動量
+
+  Pose2D lastPose = pcmap->getLastPose();                        // 直前位置
+  Pose2D predPose;                                               // オドメトリによる予測位置
+  Pose2D::calGlobalPose(odoMotion, lastPose, predPose);          // 直前位置に移動量を加えて予測位置を得る
+
+  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による推定位置
+  double score = estim->estimatePose(predPose, estPose);         // 予測位置を初期値にしてICPを実行
+  size_t usedNum = estim->getUsedNum();
+
+  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 (successful) {
+      Pose2D fusedPose;                       // 融合結果
+      Eigen::Matrix3d fusedCov;               // センサ融合後の共分散
+      pfu->setRefScan(refScan);
+      // センサ融合器pfuで、ICP結果とオドメトリ値を融合する
+      double ratio = pfu->fusePose(&curScan, estPose, odoMotion, lastPose, fusedPose, fusedCov);
+      estPose = fusedPose;
+      cov = fusedCov;
+      printf("ratio=%g. Pose fused.\n", ratio);     // ratioは退化度。確認用
+
+      // 共分散を累積する
+      Eigen::Matrix3d covL;               // 移動量の共分散
+      CovarianceCalculator::rotateCovariance(lastPose, fusedCov, covL, true);          // 移動量の共分散に変換
+      Eigen::Matrix3d tcov;                // 累積後の共分散
+      CovarianceCalculator::accumulateCovariance(lastPose, estPose, totalCov, covL, tcov);
+      totalCov = tcov;
+    }
+    else {                                   // ICP成功でなければ、オドメトリによる予測位置を使う
+      estPose = predPose;
+      pfu->calOdometryCovariance(odoMotion, lastPose, cov);       // covはオドメトリ共分散だけ
+    }
+  }
+  else {
+    if (!successful)
+      estPose = predPose;
+  }
+
+  growMap(curScan, estPose);               // 地図にスキャン点群を追加
+  prevScan = curScan;                      // 直前スキャンの設定
+
+  // 確認用
+//  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);     // 確認用
+  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));
+
+  // 共分散の保存（確認用）
+//  PoseCov pcov(estPose, cov);
+//  PoseCov pcov(estPose, totalCov);
+//  PoseCov pcov(estPose, pfu->mcov);
+  PoseCov pcov(estPose, pfu->ecov);
+  poseCovs.emplace_back(pcov);
+
+  // 累積走行距離の計算（確認用）
+  Pose2D estMotion;                                                    // 推定移動量
+  Pose2D::calRelativePose(estPose, lastPose, estMotion);
+  atd += sqrt(estMotion.tx*estMotion.tx + estMotion.ty*estMotion.ty); 
+  printf("atd=%g\n", atd);
+
+  return(successful);
+}
+
+////////////////////
+
+// 現在スキャンを追加して、地図を成長させる
+void ScanMatcher2D::growMap(const Scan2D &scan, const Pose2D &pose) {
+  const vector<LPoint2D> &lps = scan.lps;                // スキャン点群(ロボット座標系)
+  const double (*R)[2] = pose.Rmat;                      // 推定したロボット位置
+  double tx = pose.tx;
+  double ty = pose.ty;
+
+  vector<LPoint2D> scanG;                                // 地図座標系での点群
+  for(size_t i=0; i<lps.size(); i++) {
+    const LPoint2D &lp = lps[i];
+    if (lp.type == ISOLATE)                              // 孤立点（法線なし）は除外
+      continue;
+    double x = R[0][0]*lp.x + R[0][1]*lp.y + tx;         // 地図座標系に変換
+    double y = R[1][0]*lp.x + R[1][1]*lp.y + ty;
+    double nx = R[0][0]*lp.nx + R[0][1]*lp.ny;           // 法線ベクトルも変換
+    double ny = R[1][0]*lp.nx + R[1][1]*lp.ny;
+
+    LPoint2D mlp(cnt, x, y);                             // 新規に点を生成
+    mlp.setNormal(nx, ny);
+    mlp.setType(lp.type);
+    scanG.emplace_back(mlp);                             // mlpはvector内にコピーされる
+  }
+
+  // 点群地図pcmapに登録
+  pcmap->addPose(pose);
+  pcmap->addPoints(scanG);
+  pcmap->setLastPose(pose);
+  pcmap->setLastScan(scan);          // 参照スキャン用に保存
+  pcmap->makeLocalMap();             // 局所地図を生成
+  
+  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
new file mode 100755
index 0000000..88b5406
--- /dev/null
+++ b/framework/ScanMatcher2D.h
@@ -0,0 +1,121 @@
+/****************************************************************************
+ * 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 ScanMatcher2D.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef SCAN_MATCHER2D_H_
+#define SCAN_MATCHER2D_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+#include "PointCloudMap.h"
+#include "RefScanMaker.h"
+#include "ScanPointResampler.h"
+#include "ScanPointAnalyser.h"
+#include "PoseEstimatorICP.h"
+#include "PoseFuser.h"
+
+// ICPを用いてスキャンマッチングを行う
+class ScanMatcher2D
+{
+private:
+  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<PoseCov> 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) {
+  }
+
+  ~ScanMatcher2D() {
+  }
+
+/////// フレームワークの改造箇所 ////////
+
+  void setPoseEstimator(PoseEstimatorICP *p) {
+    estim = p;
+  }
+
+  void setPoseFuser(PoseFuser *p) {
+    pfu = p;
+  }
+
+  void setScanPointResampler(ScanPointResampler *s) {
+    spres = s;
+  }
+
+  void setScanPointAnalyser(ScanPointAnalyser *s) {
+    spana = s;
+  }
+
+   void setRefScanMaker(RefScanMaker *r) {
+     rsm = r;
+     if (pcmap != nullptr)
+       rsm->setPointCloudMap(pcmap);
+   }
+   
+  void setPointCloudMap(PointCloudMap *m) {
+    pcmap = m;
+    if (rsm != nullptr)
+      rsm->setPointCloudMap(pcmap);
+  }
+
+///////
+
+  void reset() {
+    cnt = -1;
+  }
+
+  void setInitPose(Pose2D &p) {
+    initPose = p;
+  }
+
+  void setDgCheck(bool t) {
+    dgcheck = t;
+  }
+
+  Eigen::Matrix3d &getCovariance() {
+    return(cov);
+  }
+
+  // デバッグ用
+  std::vector<PoseCov> &getPoseCovs() {
+    return(poseCovs);
+  }
+  
+//////////
+
+  bool matchScan(Scan2D &scan);
+  void growMap(const Scan2D &scan, const Pose2D &pose);
+
+};
+
+#endif
diff --git a/framework/ScanPointAnalyser.cpp b/framework/ScanPointAnalyser.cpp
new file mode 100755
index 0000000..7865c64
--- /dev/null
+++ b/framework/ScanPointAnalyser.cpp
@@ -0,0 +1,90 @@
+/****************************************************************************
+ * 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 ScanPointAnalyser.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "ScanPointAnalyser.h"
+
+using namespace std;
+  
+const double ScanPointAnalyser::FPDMIN = 0.06;      // ScanPointResampler.dthrSとずらす
+const double ScanPointAnalyser::FPDMAX = 1.0;
+
+///////////
+
+// スキャン点の法線ベクトルを求める。また、直線、コーナ、孤立の場合分けをする。
+void ScanPointAnalyser::analysePoints(vector<LPoint2D> &lps) {
+  for (int i=0; i<lps.size(); i++) {
+    LPoint2D &lp = lps[i];                        // スキャン点
+    ptype type;
+    Vector2D nL, nR, normal;
+    bool flagL = calNormal(i, lps, -1, nL);        // nLはlpと左側の点で求めた法線ベクトル
+    bool flagR = calNormal(i, lps, 1, nR);         // nRはlpと右側の点で求めた法線ベクトル
+    nR.x = -nR.x;                                 // 符号をnLと合せる
+    nR.y = -nR.y;
+    if (flagL) {
+      if (flagR) {                                     // 左右両側で法線ベクトルが計算可能
+        if (fabs(nL.x*nR.x + nL.y*nR.y) >= costh) {    // 両側の法線が平行に近い
+          type = LINE;                                 // 直線とみなす
+        }
+        else {                                         // 平行から遠ければ、コーナ点とみなす
+          type = CORNER;
+        }
+        // 左右両側の法線ベクトルの平均
+        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 {                       // 左側しか法線ベクトルがとれなかった
+        type = LINE;
+        normal = nL;
+      }
+    }
+    else {
+      if (flagR) {                 // 右側しか法線ベクトルがとれなかった
+        type = LINE;
+        normal = nR;
+      }
+      else {                       // 両側とも法線ベクトルがとれなかった
+        type = ISOLATE;            // 孤立点とみなす
+        normal.x = INVALID;
+        normal.y = INVALID;
+      }
+    }
+      
+    lp.setNormal(normal.x, normal.y);
+    lp.setType(type);
+  }
+}
+  
+  // 注目点cpの両側の点が、cpからdmin以上dmax以下の場合に、法線を計算する。
+bool ScanPointAnalyser::calNormal(int idx, const vector<LPoint2D> &lps, int dir, Vector2D &normal){
+  const LPoint2D &cp = lps[idx];                          // 注目点
+  for (int i=idx+dir; i>=0 && i<lps.size(); i+=dir) {
+    const LPoint2D &lp = lps[i];                          // cpのdir（左か右）側の点
+    double dx = lp.x - cp.x;
+    double dy = lp.y - cp.y;
+    double d = sqrt(dx*dx + dy*dy);
+    if (d>=FPDMIN && d<=FPDMAX) {                         // cpとlpの距離dが適切なら法線計算
+      normal.x = dy/d;
+      normal.y = -dx/d;
+      return(true);
+    }
+      
+    if (d > FPDMAX)                                       // もはやどんどん離れるので、途中でやめる
+      break;
+  }
+
+  return(false);
+}
diff --git a/framework/ScanPointAnalyser.h b/framework/ScanPointAnalyser.h
new file mode 100755
index 0000000..d34931a
--- /dev/null
+++ b/framework/ScanPointAnalyser.h
@@ -0,0 +1,48 @@
+/****************************************************************************
+ * 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 ScanPointAnalyser.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef SCAN_ANALYSER_H_
+#define SCAN_ANALYSER_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Scan2D.h"
+
+
+class ScanPointAnalyser
+{
+private:
+  static const double FPDMIN;               // 隣接点との最小距離[m]。これより小さいと誤差が大きくなるので法線計算に使わない。
+  static const double FPDMAX;               // 隣接点との最大距離[m]。これより大きいと不連続とみなして法線計算に使わない。
+  static const int CRTHRE = 45;             // 法線方向変化の閾値[度]。これより大きいとコーナ点とみなす。
+  static const int INVALID = -1;
+  double costh;                             // 左右の法線方向の食い違いの閾値
+
+public:
+  ScanPointAnalyser() : costh(cos(DEG2RAD(CRTHRE))) {
+  }
+
+  ~ScanPointAnalyser() {
+  }
+
+//////////
+  
+  void analysePoints(std::vector<LPoint2D> &lps);
+  bool calNormal(int idx, const std::vector<LPoint2D> &lps, int dir, Vector2D &normal);
+
+};
+
+#endif
+
diff --git a/framework/ScanPointResampler.cpp b/framework/ScanPointResampler.cpp
new file mode 100755
index 0000000..615e764
--- /dev/null
+++ b/framework/ScanPointResampler.cpp
@@ -0,0 +1,76 @@
+/****************************************************************************
+ * 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 ScanPointResampler.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "ScanPointResampler.h"
+
+using namespace std;
+
+/////////
+
+void ScanPointResampler::resamplePoints(Scan2D *scan) {
+  vector<LPoint2D> &lps = scan->lps;           // スキャン点群
+  if (lps.size() == 0)
+    return;
+
+  vector<LPoint2D> newLps;                     // リサンプル後の点群
+
+  dis = 0;                                     // disは累積距離
+  LPoint2D lp = lps[0];
+  LPoint2D prevLp = lp;
+  LPoint2D np(lp.sid, lp.x, lp.y);
+  newLps.emplace_back(np);                     // 最初の点は入れる
+  for (size_t i=1; i<lps.size(); i++) {
+    lp = lps[i];                               // スキャン点
+    bool inserted=false;
+
+    bool exist = findInterpolatePoint(lp, prevLp, np, inserted);
+
+    if (exist) {                               // 入れる点がある
+      newLps.emplace_back(np);                 // 新しい点npを入れる
+      prevLp = np;                             // npが直前点になる
+      dis = 0;                                 // 累積距離をリセット
+      if (inserted)                            // lpの前で補間点を入れたので、lpをもう一度やる
+        i--;
+    }
+    else
+      prevLp = lp;                             // 今のlpが直前点になる
+  }
+
+  scan->setLps(newLps);
+
+  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と直前点ppの距離
+  if (dis+L < dthreS) {                     // 予測累積距離(dis+L)がdthreSより小さい点は削除
+    dis += L;                               // disに加算
+    return(false);
+  }
+  else if (dis+L >= dthreL) {               // 予測累積距離がdthreLより大きい点は補間せず、そのまま残す
+    np.setData(cp.sid, cp.x, cp.y);
+  }
+  else {                                    // 予測累積距離がdthreSを超えたら、dthreSになるように補間する
+    double ratio = (dthreS-dis)/L;
+    double x2 = dx*ratio + pp.x;            // 少し伸ばして距離がdthreSになる位置
+    double y2 = dy*ratio + pp.y;
+    np.setData(cp.sid, x2, y2);
+    inserted = true;                        // cpより前にnpを入れたというフラグ
+  }
+ 
+  return(true);
+}
+
diff --git a/framework/ScanPointResampler.h b/framework/ScanPointResampler.h
new file mode 100755
index 0000000..5cc6af4
--- /dev/null
+++ b/framework/ScanPointResampler.h
@@ -0,0 +1,51 @@
+/****************************************************************************
+ * 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 ScanPointResampler.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef SCAN_POINT_RESAMPLER_H_
+#define SCAN_POINT_RESAMPLER_H_
+
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Scan2D.h"
+
+
+class ScanPointResampler
+{
+private:
+  double dthreS;                     // 点の距離間隔[m]
+  double dthreL;                     // 点の距離閾値[m]。この間隔を超えたら補間しない
+  double dis;                        // 累積距離。作業用
+
+public:
+  ScanPointResampler() : dthreS(0.05), dthreL(0.25), dis(0) {
+  }
+
+  ~ScanPointResampler() {
+  }
+
+///////
+
+  void setDthre(int s, int l) {
+    dthreS = s;
+    dthreL = l;
+  }  
+
+////////
+
+  void resamplePoints(Scan2D *scan);
+  bool findInterpolatePoint(const LPoint2D &cp, const LPoint2D &pp, LPoint2D &np, bool &inserted);
+};
+
+#endif
diff --git a/framework/SensorDataReader.cpp b/framework/SensorDataReader.cpp
new file mode 100755
index 0000000..d253916
--- /dev/null
+++ b/framework/SensorDataReader.cpp
@@ -0,0 +1,80 @@
+/****************************************************************************
+ * 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 SensorDataReader.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "SensorDataReader.h"
+
+using namespace std;
+
+// ファイルからスキャンを1個読む
+bool SensorDataReader::loadScan(size_t cnt, Scan2D &scan) {
+  bool isScan=false;
+  while (!inFile.eof() && !isScan) {     // スキャンを読むまで続ける
+    isScan = loadLaserScan(cnt, scan);
+  }
+
+  if (isScan) 
+    return(false);                       // まだファイルが続くという意味
+  else
+    return(true);                        // ファイルが終わったという意味
+}
+
+//////////////
+
+// ファイルから項目1個を読む。読んだ項目がスキャンならtrueを返す。
+bool SensorDataReader::loadLaserScan(size_t cnt, Scan2D &scan) {
+  string type;                           // ファイル内の項目ラベル
+  inFile >> type;
+  if (type == "LASERSCAN") {             // スキャンの場合
+    scan.setSid(cnt);
+
+    int sid, sec, nsec;
+    inFile >> sid >> sec >> nsec;        // これらは使わない
+
+    vector<LPoint2D> lps;
+    int pnum;                            // スキャン点数
+    inFile >> pnum;
+    lps.reserve(pnum);
+    for (int i=0; i<pnum; i++) {
+      float angle, range;
+      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) {         // わざと退化を起こしやすく
+        continue;
+      }
+
+      LPoint2D lp;
+      lp.setSid(cnt);                    // スキャン番号はcnt（通し番号）にする
+      lp.calXY(range, angle);            // angle,rangeから点の位置xyを計算
+      lps.emplace_back(lp);
+    }
+    scan.setLps(lps);
+
+    // スキャンに対応するオドメトリ情報
+    Pose2D &pose = scan.pose;
+    inFile >> pose.tx >> pose.ty;
+    double th;
+    inFile >> th;
+    pose.setAngle(RAD2DEG(th));          // オドメトリ角度はラジアンなので度にする
+    pose.calRmat();
+
+    return(true);
+  }
+  else {                                 // スキャン以外の場合
+    string line;
+    getline(inFile, line);               // 読み飛ばす
+
+    return(false);
+  }
+}
diff --git a/framework/SensorDataReader.h b/framework/SensorDataReader.h
new file mode 100755
index 0000000..1b6a39d
--- /dev/null
+++ b/framework/SensorDataReader.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ * 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 SensorDataReader.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef SENSOR_DATA_READER_H_
+#define SENSOR_DATA_READER_H_
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include "MyUtil.h"
+#include "LPoint2D.h"
+#include "Pose2D.h"
+#include "Scan2D.h"
+
+/////////
+
+class SensorDataReader
+{
+private:
+  int angleOffset;                      // レーザスキャナとロボットの向きのオフセット
+  std::ifstream inFile;                 // データファイル
+
+public:
+  SensorDataReader() : angleOffset(180) {
+  }
+
+  ~SensorDataReader() {
+  }
+
+////////
+
+  bool openScanFile(const char *filepath) {
+    inFile.open(filepath);
+    if (!inFile.is_open()) {
+      std::cerr << "Error: cannot open file " << filepath << std::endl;
+      return(false);
+    }
+
+    return(true);
+  }
+
+  void closeScanFile() {
+    inFile.close();
+  }
+
+  void setAngleOffset(int o) {
+     angleOffset = o;
+  }
+
+//////////
+
+  bool loadScan(size_t cnt, Scan2D &scan);
+  bool loadLaserScan(size_t cnt, Scan2D &scan);
+};
+
+#endif
diff --git a/framework/SlamBackEnd.cpp b/framework/SlamBackEnd.cpp
new file mode 100755
index 0000000..4ebe70c
--- /dev/null
+++ b/framework/SlamBackEnd.cpp
@@ -0,0 +1,48 @@
+/****************************************************************************
+ * 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 SlamBackEnd.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "SlamBackEnd.h"
+#include "P2oDriver2D.h"
+
+using namespace std;
+
+////////// ポーズ調整 //////////
+
+Pose2D SlamBackEnd::adjustPoses() {
+//  pg->printArcs();
+//  pg->printNodes();
+
+  newPoses.clear();
+
+  P2oDriver2D p2o;
+  p2o.doP2o(*pg, newPoses, 5);                 // 5回くり返す
+
+  return(newPoses.back());
+}
+
+/////////////////////////////
+
+void SlamBackEnd::remakeMaps() {
+  // PoseGraphの修正
+  vector<PoseNode*> &pnodes = pg->nodes;      // ポーズノード
+  for (size_t i=0; i<newPoses.size(); i++) {
+    Pose2D &npose = newPoses[i];
+    PoseNode *pnode = pnodes[i];              // ノードはロボット位置と1:1対応
+    pnode->setPose(npose);                    // 各ノードの位置を更新
+  }
+  printf("newPoses.size=%lu, nodes.size=%lu\n", newPoses.size(), pnodes.size());
+
+  // PointCloudMapの修正
+  pcmap->remakeMaps(newPoses);
+}
diff --git a/framework/SlamBackEnd.h b/framework/SlamBackEnd.h
new file mode 100755
index 0000000..c40965b
--- /dev/null
+++ b/framework/SlamBackEnd.h
@@ -0,0 +1,54 @@
+/****************************************************************************
+ * 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 SlamBackEnd.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef SLAM_BACK_END_H_
+#define SLAM_BACK_END_H_
+
+#include <vector>
+#include "PointCloudMap.h"
+#include "PoseGraph.h"
+
+////////
+
+class SlamBackEnd
+{
+private:
+  std::vector<Pose2D> newPoses;            // ポーズ調整後の姿勢
+  PointCloudMap *pcmap;                    // 点群地図
+  PoseGraph *pg;                           // ポーズグラフ
+
+public:
+  SlamBackEnd() {
+  }
+
+  ~SlamBackEnd() {
+  }
+
+//////////
+
+  void setPointCloudMap(PointCloudMap *m) {
+    pcmap = m;
+  }
+
+  void setPoseGraph(PoseGraph *g) {
+    pg = g;
+  }
+  
+//////////
+
+  Pose2D adjustPoses();
+  void remakeMaps(); 
+};
+
+#endif
diff --git a/framework/SlamFrontEnd.cpp b/framework/SlamFrontEnd.cpp
new file mode 100755
index 0000000..b78b71b
--- /dev/null
+++ b/framework/SlamFrontEnd.cpp
@@ -0,0 +1,110 @@
+/****************************************************************************
+ * 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 SlamFrontEnd.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "SlamFrontEnd.h"
+
+using namespace std;
+
+/////////
+
+// 初期化
+void SlamFrontEnd::init() {
+  smat->reset();
+  smat->setPointCloudMap(pcmap);
+  sback.setPointCloudMap(pcmap);
+}
+
+///////////
+
+// 現在スキャンscanを処理する。
+void SlamFrontEnd::process(Scan2D &scan) {
+  if (cnt == 0) 
+    init();                                       // 開始時に初期化
+
+  // スキャンマッチング
+  smat->matchScan(scan);
+
+  Pose2D curPose = pcmap->getLastPose();          // これはスキャンマッチングで推定した現在のロボット位置
+  
+  // ポーズグラフにオドメトリアークを追加
+  if (cnt == 0) {                                 // 最初はノードを置くだけ。
+    pg->addNode(curPose);
+  }
+  else {                                          // 次からはノードを追加して、オドメトリアークを張る
+    Eigen::Matrix3d &cov = smat->getCovariance();
+    makeOdometryArc(curPose, cov);
+  }
+
+  if (cnt%keyframeSkip==0) {                             // キーフレームのときだけ行う
+    if (cnt == 0)
+      pcmap->setNthre(1);                                // cnt=0のときは地図が小さいのでサンプリング多くする
+    else
+      pcmap->setNthre(5);
+    pcmap->makeGlobalMap();                              // 点群地図の全体地図を生成
+  }
+
+  // ループ閉じ込み
+  if (cnt > keyframeSkip && cnt%keyframeSkip==0) {       // キーフレームのときだけ行う
+    bool flag = lpd->detectLoop(&scan, curPose, cnt);    // ループ検出を起動
+    if (flag) {
+      sback.adjustPoses();                               // ループが見つかったらポーズ調整
+      sback.remakeMaps();                                // 地図やポーズグラフの修正
+    }
+  }
+
+  printf("pcmap.size=%lu\n", pcmap->globalMap.size());   // 確認用
+
+  countLoopArcs();            // 確認用
+
+  ++cnt;
+}
+
+////////////
+
+// オドメトリアークの生成
+bool SlamFrontEnd::makeOdometryArc(Pose2D &curPose, const Eigen::Matrix3d &fusedCov) {
+  if (pg->nodes.size() == 0)                             // 念のためのチェック
+    return(false);
+  PoseNode *lastNode = pg->nodes.back();                 // 直前ノード
+  PoseNode *curNode = pg->addNode(curPose);              // ポーズグラフに現在ノードを追加
+
+  // 直前ノードと現在ノードの間にオドメトリアークを張る
+  Pose2D &lastPose = lastNode->pose;
+  Pose2D relPose;
+  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);     // 移動量の共分散に変換
+  PoseArc *arc = pg->makeArc(lastNode->nid, curNode->nid, relPose, cov);     // アークの生成
+  pg->addArc(arc);                                                           // ポーズグラフにアークを追加
+
+  return(true);
+}
+
+////////////
+
+// ループアーク数を数える。確認用
+void SlamFrontEnd::countLoopArcs() {
+  vector<PoseArc*> &parcs = pg->arcs;       // 全ポーズアーク
+  int an=0;                                 // ループアーク数
+  for (size_t i=0; i<parcs.size(); i++) {
+    PoseArc *a = parcs[i];
+    PoseNode *src = a->src;
+    PoseNode *dst = a->dst;
+    if (src->nid != dst->nid-1)             // オドメトリアークは始点と終点が連番になっている
+      ++an;                                 // オドメトリアークでなければループアーク
+  }
+  printf("loopArcs.size=%d\n", an);         // 確認用
+}
diff --git a/framework/SlamFrontEnd.h b/framework/SlamFrontEnd.h
new file mode 100755
index 0000000..7f942be
--- /dev/null
+++ b/framework/SlamFrontEnd.h
@@ -0,0 +1,105 @@
+/****************************************************************************
+ * 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 SlamFrontEnd.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef SLAM_FRONT_END_H_
+#define SLAM_FRONT_END_H_
+
+#include <vector>
+#include <boost/circular_buffer.hpp>
+#include "PointCloudMap.h"
+#include "ScanMatcher2D.h"
+#include "PoseGraph.h"
+#include "LoopDetector.h"
+#include "SlamBackEnd.h"
+
+////////
+
+// SLAMフロントエンド。ロボット位置推定、地図生成、ループ閉じ込みを取り仕切る。
+class SlamFrontEnd
+{
+private:
+  int cnt;                               // 論理時刻
+  int keyframeSkip;                      // キーフレーム間隔
+
+  PointCloudMap *pcmap;                  // 点群地図
+  PoseGraph *pg;                         // ポーズグラフ
+  ScanMatcher2D *smat;                   // スキャンマッチング
+  LoopDetector *lpd;                     // ループ検出器
+  SlamBackEnd sback;                     // SLAMバックエンド
+
+public:
+  SlamFrontEnd()  : cnt(0), keyframeSkip(10), smat(nullptr), lpd(nullptr) {
+    pg = new PoseGraph();
+    sback.setPoseGraph(pg);
+  }
+
+  ~SlamFrontEnd() {
+    delete pg;
+  }
+
+///////
+
+  void setScanMatcher(ScanMatcher2D *s) {
+    smat = s;
+  }
+
+  void setLoopDetector(LoopDetector *l) {
+    lpd = l;
+    lpd->setPoseGraph(pg);
+  }
+
+  void setPointCloudMap(PointCloudMap *p) {
+    pcmap = p;
+  }
+
+  void setRefScanMaker(RefScanMaker *r) {
+    smat->setRefScanMaker(r);
+  }
+
+  PointCloudMap *getPointCloudMap() {
+    return(pcmap);
+  }
+
+  PoseGraph *getPoseGraph() {
+    return(pg);
+  }
+
+  int getCnt() {
+    return(cnt);
+  }
+
+  void setDgCheck(bool p){
+    smat->setDgCheck(p);
+  }
+
+  // デバッグ用
+  std::vector<LoopMatch> &getLoopMatches() {
+    return(lpd->getLoopMatches());
+  }
+
+  // デバッグ用
+  std::vector<PoseCov> &getPoseCovs() {
+    return(smat->getPoseCovs());
+  }
+
+/////////
+
+  void init();
+  void process(Scan2D &scan);
+  bool makeOdometryArc(Pose2D &curPose, const Eigen::Matrix3d &cov);
+
+  void countLoopArcs();
+};
+
+#endif
diff --git a/hook/CMakeLists.txt b/hook/CMakeLists.txt
new file mode 100755
index 0000000..ccce295
--- /dev/null
+++ b/hook/CMakeLists.txt
@@ -0,0 +1,54 @@
+PROJECT(hook)
+
+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() 
+
+SET(hk_HDRS
+    RefScanMakerBS.h
+    RefScanMakerLM.h
+    CostFunctionED.h
+    CostFunctionPD.h
+    PoseOptimizerSD.h
+    PoseOptimizerSL.h
+    DataAssociatorLS.h
+    DataAssociatorGT.h
+    PointCloudMapBS.h
+    PointCloudMapGT.h
+    PointCloudMapLP.h
+    LoopDetectorSS.h
+)
+
+SET(hk_SRCS 
+    RefScanMakerBS.cpp
+    RefScanMakerLM.cpp
+    CostFunctionED.cpp
+    CostFunctionPD.cpp
+    PoseOptimizerSD.cpp
+    PoseOptimizerSL.cpp
+    DataAssociatorLS.cpp
+    DataAssociatorGT.cpp
+    PointCloudMapBS.cpp
+    PointCloudMapGT.cpp
+    PointCloudMapLP.cpp
+    LoopDetectorSS.cpp
+)
+
+include_directories(
+    ${Boost_INCLUDE_DIR}
+    ${EIGEN3_INCLUDE_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR}/../framework
+)
+
+link_directories(
+)
+
+ADD_LIBRARY(hook ${hk_SRCS} ${hk_HDRS})
+
+target_link_libraries(hook framework)
+
diff --git a/hook/CostFunctionED.cpp b/hook/CostFunctionED.cpp
new file mode 100755
index 0000000..c1c3240
--- /dev/null
+++ b/hook/CostFunctionED.cpp
@@ -0,0 +1,52 @@
+/****************************************************************************
+ * 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 CostFunctionED.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "CostFunctionED.h"
+
+using namespace std;
+
+// 点間距離による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; i<curLps.size(); i++) {
+    const LPoint2D *clp = curLps[i];             // 現在スキャンの点
+    const LPoint2D *rlp = refLps[i];             // clpに対応する参照スキャンの点
+
+    double cx = clp->x;
+    double cy = clp->y;
+    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);     // 点間距離
+
+    if (edis <= evlimit*evlimit)
+      ++pn;                                      // 誤差が小さい点の数
+
+    error += edis;                               // 各点の誤差を累積
+
+    ++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);     // 確認用
+
+  error *= 100;                                  // 評価値が小さくなりすぎないよう100かける。
+
+  return(error);
+}
diff --git a/hook/CostFunctionED.h b/hook/CostFunctionED.h
new file mode 100755
index 0000000..bab412f
--- /dev/null
+++ b/hook/CostFunctionED.h
@@ -0,0 +1,32 @@
+/****************************************************************************
+ * 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 CostFunctionED.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _COST_FUNCTION_ED_H_
+#define _COST_FUNCTION_ED_H_
+
+#include "CostFunction.h"
+
+class CostFunctionED : public CostFunction
+{
+public:
+  CostFunctionED() {
+  }
+
+  ~CostFunctionED() {
+  }
+
+  virtual double calValue(double tx, double ty, double th);
+};
+
+#endif
diff --git a/hook/CostFunctionPD.cpp b/hook/CostFunctionPD.cpp
new file mode 100755
index 0000000..364b1ef
--- /dev/null
+++ b/hook/CostFunctionPD.cpp
@@ -0,0 +1,56 @@
+/****************************************************************************
+ * 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 CostFunctionPD.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "CostFunctionPD.h"
+
+using namespace std;
+
+// 垂直距離によるコスト関数
+double CostFunctionPD::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; i<curLps.size(); i++) {
+    const LPoint2D *clp = curLps[i];             // 現在スキャンの点
+    const LPoint2D *rlp = refLps[i];             // clpに対応する参照スキャンの点
+
+    if (rlp->type != LINE)                       // 直線上の点でなければ使わない
+      continue;
+ 
+    double cx = clp->x;
+    double cy = clp->y;
+    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 er = pdis*pdis;
+    if (er <= evlimit*evlimit)
+      ++pn;                                      // 誤差が小さい点の数
+
+    error += er;                                 // 各点の誤差を累積
+    ++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);     // 確認用
+
+  error *= 100;                                  // 評価値が小さくなりすぎないよう100かける。
+
+  return(error);
+}
diff --git a/hook/CostFunctionPD.h b/hook/CostFunctionPD.h
new file mode 100755
index 0000000..c66e66b
--- /dev/null
+++ b/hook/CostFunctionPD.h
@@ -0,0 +1,32 @@
+/****************************************************************************
+ * 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 CostFunctionPD.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _COST_FUNCTION_PD_H_
+#define _COST_FUNCTION_PD_H_
+
+#include "CostFunction.h"
+
+class CostFunctionPD : public CostFunction
+{
+public:
+  CostFunctionPD() {
+  }
+
+  ~CostFunctionPD() {
+  }
+
+  virtual double calValue(double tx, double ty, double th);
+};
+
+#endif
diff --git a/hook/DataAssociatorGT.cpp b/hook/DataAssociatorGT.cpp
new file mode 100755
index 0000000..83910b2
--- /dev/null
+++ b/hook/DataAssociatorGT.cpp
@@ -0,0 +1,46 @@
+/****************************************************************************
+ * 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 DataAssociatorGT.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include <boost/timer.hpp>
+#include "DataAssociatorGT.h"
+
+using namespace std;
+
+
+// 現在スキャンcurScanの各スキャン点をpredPoseで座標変換した位置に最も近い点を見つける
+double DataAssociatorGT::findCorrespondence(const Scan2D *curScan, const Pose2D &predPose) {
+  boost::timer tim;                                 // 処理時間測定用
+
+  curLps.clear();                                   // 対応づけ現在スキャン点群を空にする
+  refLps.clear();                                   // 対応づけ参照スキャン点群を空にする
+
+  for (size_t i=0; i<curScan->lps.size(); i++) {
+    const LPoint2D *clp = &(curScan->lps[i]);       // 現在スキャンの点。ポインタで。
+
+    // 格子テーブルにより最近傍点を求める。格子テーブル内に距離閾値dthreがあることに注意。
+    const LPoint2D *rlp = nntab.findClosestPoint(clp, predPose);
+
+    if (rlp != nullptr) {
+      curLps.push_back(clp);                        // 最近傍点があれば登録
+      refLps.push_back(rlp);
+    }
+  }
+
+  double ratio = (1.0*curLps.size())/curScan->lps.size();         // 対応がとれた点の比率
+
+//  double t1 = 1000*tim.elapsed();                   // 処理時間
+//  printf("Elapsed time: dassGT=%g\n", t1);
+
+  return(ratio);
+}
diff --git a/hook/DataAssociatorGT.h b/hook/DataAssociatorGT.h
new file mode 100755
index 0000000..a4789c9
--- /dev/null
+++ b/hook/DataAssociatorGT.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ * 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 DataAssociatorGT.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef DATA_ASSOCIATOR_GT_H_
+#define DATA_ASSOCIATOR_GT_H_
+
+#include "DataAssociator.h"
+#include "NNGridTable.h"
+
+// 格子テーブルを用いて、現在スキャンと参照スキャン間の点の対応づけを行う
+class DataAssociatorGT : public DataAssociator
+{
+private:
+  NNGridTable nntab;                        // 格子テーブル
+  
+public:
+  DataAssociatorGT() {
+  }
+
+  ~DataAssociatorGT() {
+  }
+  
+  // 参照スキャンの点rlpsをポインタにしてnntabに入れる
+  virtual void setRefBase(const std::vector<LPoint2D> &rlps) {
+    nntab.clear();
+    for (size_t i=0; i<rlps.size(); i++) 
+      nntab.addPoint(&rlps[i]);              // ポインタにして格納
+  }
+
+/////////
+
+  virtual double findCorrespondence(const Scan2D *curScan, const Pose2D &predPose);
+};
+
+#endif
diff --git a/hook/DataAssociatorLS.cpp b/hook/DataAssociatorLS.cpp
new file mode 100755
index 0000000..4332476
--- /dev/null
+++ b/hook/DataAssociatorLS.cpp
@@ -0,0 +1,57 @@
+/****************************************************************************
+ * 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 DataAssociatorLS.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include <boost/timer.hpp>
+#include "DataAssociatorLS.h"
+
+using namespace std;
+
+// 現在スキャンcurScanの各スキャン点に対応する点をbaseLpsから見つける
+double DataAssociatorLS::findCorrespondence(const Scan2D *curScan, const Pose2D &predPose) {
+  boost::timer tim;                                 // 処理時間測定用
+
+  double dthre = 0.2;                               // これより遠い点は除外する[m]
+  curLps.clear();                                   // 対応づけ現在スキャン点群を空にする
+  refLps.clear();                                   // 対応づけ参照スキャン点群を空にする
+  for (size_t i=0; i<curScan->lps.size(); i++) {
+    const LPoint2D *clp = &(curScan->lps[i]);       // 現在スキャンの点。ポインタで。
+
+    // スキャン点lpをpredPoseで座標変換した位置に最も近い点を見つける
+    LPoint2D glp;                                   // clpの予測位置
+    predPose.globalPoint(*clp, glp);                // predPoseで座標変換
+
+    double dmin = HUGE_VAL;                         // 距離最小値
+    const LPoint2D *rlpmin = nullptr;               // 最も近い点
+    for (size_t j=0; j<baseLps.size(); j++) {
+      const LPoint2D *rlp = baseLps[j];             // 参照スキャン点
+      double d = (glp.x - rlp->x)*(glp.x - rlp->x) + (glp.y - rlp->y)*(glp.y - rlp->y);
+      if (d <= dthre*dthre && d < dmin) {           // dthre内で距離が最小となる点を保存
+        dmin = d;
+        rlpmin = rlp;
+      }
+    }
+    if (rlpmin != nullptr) {                        // 最近傍点があれば登録
+      curLps.push_back(clp);
+      refLps.push_back(rlpmin);
+    }
+  }
+  
+  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();                               // 処理時間
+//  printf("Elapsed time: dassLS=%g\n", t1);
+
+  return(ratio);
+}
diff --git a/hook/DataAssociatorLS.h b/hook/DataAssociatorLS.h
new file mode 100755
index 0000000..61f0c7e
--- /dev/null
+++ b/hook/DataAssociatorLS.h
@@ -0,0 +1,45 @@
+/****************************************************************************
+ * 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 DataAssociatorLS.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef DATA_ASSOCIATOR_LS_H_
+#define DATA_ASSOCIATOR_LS_H_
+
+#include "DataAssociator.h"
+
+// 線形探索を用いて、現在スキャンと参照スキャン間の点の対応づけを行う
+class DataAssociatorLS : public DataAssociator
+{
+private:
+  std::vector<const LPoint2D*> baseLps;              // 参照スキャンの点を格納しておく。作業用
+
+public:
+  DataAssociatorLS() {
+  }
+
+  ~DataAssociatorLS() {
+  }
+
+  // 参照スキャンの点rlpsをポインタにしてbaseLpsに入れる
+  virtual void setRefBase(const std::vector<LPoint2D> &rlps) {
+    baseLps.clear();
+    for (size_t i=0; i<rlps.size(); i++)
+      baseLps.push_back(&rlps[i]);                // ポインタにして格納
+  }
+
+/////////
+
+  virtual double findCorrespondence(const Scan2D *curScan, const Pose2D &predPose);
+};
+
+#endif
diff --git a/hook/LoopDetectorSS.cpp b/hook/LoopDetectorSS.cpp
new file mode 100755
index 0000000..c618cd2
--- /dev/null
+++ b/hook/LoopDetectorSS.cpp
@@ -0,0 +1,206 @@
+/****************************************************************************
+ * 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 LoopDetectorSS.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "LoopDetectorSS.h"
+
+using namespace std;
+
+////////////
+
+// ループ検出
+// 現在位置curPoseに近く、現在スキャンcurScanに形が一致する場所をロボット軌跡から見つけてポーズアークを張る。
+bool LoopDetectorSS::detectLoop(Scan2D *curScan, Pose2D &curPose, int cnt) {
+  printf("-- detectLoop -- \n");
+
+  // 最も近い部分地図を探す
+  double atd = pcmap->atd;                             // 現在の実際の累積走行距離
+  double atdR = 0;                                     // 下記の処理で軌跡をなぞる時の累積走行距離
+  const vector<Submap> &submaps = pcmap->submaps;      // 部分地図
+  const vector<Pose2D> &poses = pcmap->poses;          // ロボット軌跡
+  double dmin=HUGE_VAL;                                // 前回訪問点までの距離の最小値
+  size_t imin=0, jmin=0;                               // 距離最小の前回訪問点のインデックス
+  Pose2D prevP;                                        // 直前のロボット位置
+  for (size_t i=0; i<submaps.size()-1; i++) {          // 現在の部分地図以外を探す
+    const Submap &submap = submaps[i];                 // i番目の部分地図
+    for (size_t j=submap.cntS; j<=submap.cntE; j++) {  // 部分地図の各ロボット位置について
+      Pose2D p = poses[j];                             // ロボット位置
+      atdR += sqrt((p.tx - prevP.tx)*(p.tx - prevP.tx) + (p.ty - prevP.ty)*(p.ty - prevP.ty));
+      if (atd-atdR < atdthre) {                        // 現在位置までの走行距離が短いとループとみなさず、もうやめる
+        i = submaps.size();                            // これで外側のループからも抜ける
+        break;
+      }
+      prevP = p;
+
+      double d = (curPose.tx - p.tx)*(curPose.tx - p.tx) + (curPose.ty - p.ty)*(curPose.ty - p.ty);
+      if (d < dmin) {                                  // 現在位置とpとの距離がこれまでの最小か
+        dmin = d;
+        imin = i;                                      // 候補となる部分地図のインデックス
+        jmin = j;                                      // 前回訪問点のインデックス
+      }
+//      printf("i=%lu, j=%lu: atd=%g, atdR=%g, atdthre=%g\n", i, j, atd, atdR, atdthre);             // 確認用
+    }
+  }
+
+  printf("dmin=%g, radius=%g, imin=%lu, jmin=%lu\n", sqrt(dmin), radius, imin, jmin);  // 確認用
+
+  if (dmin > radius*radius)                            // 前回訪問点までの距離が遠いとループ検出しない
+    return(false);
+
+  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);
+
+  // 再訪点の位置を求める
+  Pose2D revisitPose;
+  bool flag = estimateRevisitPose(curScan, refSubmap.mps, curPose, revisitPose);
+//  bool flag = estimateRelativePose(curScan, refSubmap.mps, initPose, revisitPose);
+
+  if (flag) {                                          // ループを検出した
+    Eigen::Matrix3d icpCov;                                                  // ICPの共分散
+    double ratio = pfu->calIcpCovariance(revisitPose, curScan, icpCov);      // ICPの共分散を計算
+
+    LoopInfo info;                                     // ループ検出結果
+    info.pose = revisitPose;                           // ループアーク情報に再訪点位置を設定
+    info.cov = icpCov;                                 // ループアーク情報に共分散を設定。
+    info.curId = cnt;                                  // 現在位置のノードid
+    info.refId = static_cast<int>(jmin);               // 前回訪問点のノードid
+    makeLoopArc(info);                                 // ループアーク生成
+
+    // 確認用
+    Scan2D refScan;
+    Pose2D spose = poses[refSubmap.cntS];
+    refScan.setSid(info.refId);
+    refScan.setLps(refSubmap.mps);
+    refScan.setPose(spose);
+    LoopMatch lm(*curScan, refScan, info);
+    loopMatches.emplace_back(lm);
+    printf("curId=%d, refId=%d\n", info.curId, info.refId);
+  }
+
+  return(flag);
+}
+
+//////////
+
+// 前回訪問点(refId)を始点ノード、現在位置(curId)を終点ノードにして、ループアークを生成する。
+void LoopDetectorSS::makeLoopArc(LoopInfo &info) {
+  if (info.arcked)                                             // infoのアークはすでに張ってある
+    return;
+  info.setArcked(true);
+
+  Pose2D srcPose = pcmap->poses[info.refId];                   // 前回訪問点の位置
+  Pose2D dstPose(info.pose.tx, info.pose.ty, info.pose.th);    // 再訪点の位置
+  Pose2D relPose;
+  Pose2D::calRelativePose(dstPose, srcPose, relPose);          // ループアークの拘束
+
+  // アークの拘束は始点ノードからの相対位置なので、共分散をループアークの始点ノード座標系に変換
+  Eigen::Matrix3d cov;
+  CovarianceCalculator::rotateCovariance(srcPose, info.cov, cov, true);    // 共分散の逆回転
+
+  PoseArc *arc = pg->makeArc(info.refId, info.curId, relPose, cov);        // ループアーク生成
+  pg->addArc(arc);                                                         // ループアーク登録
+
+  // 確認用
+  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);
+  printf("relPose: tx=%g, ty=%g, th=%g\n", relPose.tx, relPose.ty, relPose.th);
+  PoseNode *src = pg->findNode(info.refId);
+  PoseNode *dst = pg->findNode(info.curId);
+  Pose2D relPose2;
+  Pose2D::calRelativePose(dst->pose, src->pose, relPose2);
+  printf("relPose2: tx=%g, ty=%g, th=%g\n", relPose2.tx, relPose2.ty, relPose2.th);
+}
+
+//////////
+
+// 現在スキャンcurScanと部分地図の点群refLpsでICPを行い、再訪点の位置を求める。
+bool LoopDetectorSS::estimateRevisitPose(const Scan2D *curScan, const vector<LPoint2D> &refLps, const Pose2D &initPose, Pose2D &revisitPose) {
+  dass->setRefBase(refLps);                              // データ対応づけ器に参照点群を設定
+  cfunc->setEvlimit(0.2);                                // コスト関数の誤差閾値
+
+  printf("initPose: tx=%g, ty=%g, th=%g\n", initPose.tx, initPose.ty, initPose.th);       // 確認用
+
+  size_t usedNumMin = 50; 
+//  size_t usedNumMin = 100;
+
+  // 初期位置initPoseの周囲をしらみつぶしに調べる。
+  // 効率化のため、ICPは行わず、各位置で単純にマッチングスコアを調べる。
+  double rangeT = 1;                                     // 並進の探索範囲[m]
+  double rangeA = 45;                                    // 回転の探索範囲[度]
+  double dd = 0.2;                                       // 並進の探索間隔[m]
+  double da = 2;                                         // 回転の探索間隔[度]
+  double pnrateMax=0;
+  vector<double> pnrates;
+  double scoreMin=1000;
+  vector<double> scores;
+  vector<Pose2D> 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);   // 位置poseでデータ対応づけ
+        size_t usedNum = dass->curLps.size();
+//        printf("usedNum=%lu, mratio=%g\n", usedNum, mratio);          // 確認用
+        if (usedNum < usedNumMin || mratio < 0.9)        // 対応率が悪いと飛ばす
+          continue;
+        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);  // 確認用
+//          printf("score=%g, pnrate=%g\n", score, pnrate);                    // 確認用
+        }
+      }
+    }
+  }
+  printf("candidates.size=%lu\n", candidates.size());                           // 確認用
+  if (candidates.size() == 0)
+    return(false);
+
+  // 候補位置candidatesの中から最もよいものをICPで選ぶ
+  Pose2D best;                                              // 最良候補
+  double smin=1000000;                                      // ICPスコア最小値
+  estim->setScanPair(curScan, refLps);                      // ICPにスキャン設定
+  for (size_t i=0; i<candidates.size(); i++) {
+    Pose2D p = candidates[i];                               // 候補位置
+    printf("score=%g\n", scores[i]);    // 確認用
+    Pose2D estP;
+    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);    // 確認用
+    }
+  }
+
+  // 最小スコアが閾値より小さければ見つけた
+  if (smin <= scthre) {
+    revisitPose = best;
+    return(true);
+  }
+
+  return(false);
+}
diff --git a/hook/LoopDetectorSS.h b/hook/LoopDetectorSS.h
new file mode 100755
index 0000000..9d3d660
--- /dev/null
+++ b/hook/LoopDetectorSS.h
@@ -0,0 +1,77 @@
+/****************************************************************************
+ * 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 LoopDetectorSS.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef LOOP_DETECTOR_SS_H_
+#define LOOP_DETECTOR_SS_H_
+
+#include "LoopDetector.h"
+#include "PointCloudMapLP.h"
+#include "DataAssociator.h"
+#include "PoseEstimatorICP.h"
+#include "PoseFuser.h"
+
+
+////////////
+
+class LoopDetectorSS : public LoopDetector
+{
+private:
+  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) {
+  }
+
+  ~LoopDetectorSS() {
+  }
+
+/////////
+
+  void setPoseEstimator(PoseEstimatorICP *p) {
+    estim = p;
+  }
+
+  void setPoseFuser(PoseFuser *p) {
+    pfu = p;
+  }
+
+  void setDataAssociator(DataAssociator *d) {
+    dass = d;
+  }
+
+  void setCostFunction(CostFunction *f) {
+    cfunc = f;
+  }
+
+  void setPointCloudMap(PointCloudMapLP *p) {
+    pcmap = p;
+  }
+
+//////////
+
+  virtual bool detectLoop(Scan2D *curScan, Pose2D &curPose, int cnt);
+  void makeLoopArc(LoopInfo &info);
+  bool estimateRevisitPose(const Scan2D *curScan, const std::vector<LPoint2D> &refLps, const Pose2D &initPose, Pose2D &revisitPose);
+
+};
+
+#endif
diff --git a/hook/PointCloudMapBS.cpp b/hook/PointCloudMapBS.cpp
new file mode 100755
index 0000000..ec10b19
--- /dev/null
+++ b/hook/PointCloudMapBS.cpp
@@ -0,0 +1,49 @@
+/****************************************************************************
+ * 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 PointCloudMapBS.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "PointCloudMapBS.h"
+
+using namespace std;
+
+///////////
+
+// ロボット位置の追加
+void PointCloudMapBS::addPose(const Pose2D &p) {
+  poses.emplace_back(p);
+}
+
+// スキャン点群の追加
+void PointCloudMapBS::addPoints(const vector<LPoint2D> &lps) {
+  int skip=5;                                       // さすがに重いので、1/5に間引く
+//  int skip=10;                                       // さすがに重いので、1/10に間引く
+  for (size_t i=0; i<lps.size(); i+=skip) {
+    globalMap.emplace_back(lps[i]);                 // 全体地図に追加するだけ
+  }
+}
+
+// 全体地図生成。すでにできているので何もしない
+void PointCloudMapBS::makeGlobalMap(){
+  printf("globalMap.size=%lu\n", globalMap.size());   // 確認用
+}
+
+// 局所地図生成。ダミー
+void PointCloudMapBS::makeLocalMap(){
+//  localMap = globalMap;
+}
+
+////////
+
+// ダミー
+void PointCloudMapBS::remakeMaps(const vector<Pose2D> &newPoses) {
+}
diff --git a/hook/PointCloudMapBS.h b/hook/PointCloudMapBS.h
new file mode 100755
index 0000000..8a3b23d
--- /dev/null
+++ b/hook/PointCloudMapBS.h
@@ -0,0 +1,39 @@
+/****************************************************************************
+ * 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 PointCloudMapBS.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _POINT_CLOUD_MAP_BS_H_
+#define _POINT_CLOUD_MAP_BS_H_
+
+#include "PointCloudMap.h"
+
+// スキャン点をすべて保存する点群地図
+class PointCloudMapBS : public PointCloudMap
+{
+public:
+  PointCloudMapBS() {
+  }
+
+  ~PointCloudMapBS() {
+  }
+
+////////
+
+  virtual void addPose(const Pose2D &p);
+  virtual void addPoints(const std::vector<LPoint2D> &lps);
+  virtual void makeGlobalMap();
+  virtual void makeLocalMap();
+  virtual void remakeMaps(const std::vector<Pose2D> &newPoses);
+};
+
+#endif
diff --git a/hook/PointCloudMapGT.cpp b/hook/PointCloudMapGT.cpp
new file mode 100755
index 0000000..5d19814
--- /dev/null
+++ b/hook/PointCloudMapGT.cpp
@@ -0,0 +1,63 @@
+/****************************************************************************
+ * 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 PointCloudMapGT.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "PointCloudMapGT.h"
+
+using namespace std;
+
+///////////
+
+// ロボット位置の追加
+void PointCloudMapGT::addPose(const Pose2D &p) {
+  poses.emplace_back(p);
+}
+
+// 格子テーブルの各セルの代表点を求めてspsに格納する
+void PointCloudMapGT::subsamplePoints(vector<LPoint2D> &sps) {
+  nntab.clear();                            // 格子テーブルの初期化
+  for (size_t i=0; i<allLps.size(); i++) 
+    nntab.addPoint(&(allLps[i]));           // 全点を格子テーブルに登録
+
+  nntab.makeCellPoints(nthre, sps);         // nthre点以上あるセルから代表点を得る
+
+  printf("allLps.size=%lu, sps.size=%lu\n", allLps.size(), sps.size());  // 確認用
+}
+
+/////////
+
+// スキャン点群を追加
+void PointCloudMapGT::addPoints(const vector<LPoint2D> &lps) {
+  for (size_t i=0; i<lps.size(); i++)
+    allLps.emplace_back(lps[i]);
+}
+
+// 全体地図の生成
+void PointCloudMapGT::makeGlobalMap(){
+  globalMap.clear();
+  subsamplePoints(globalMap);         // 格子テーブルの各セルの代表点から全体地図を作る
+
+  printf("GT: globalMap.size=%lu\n", globalMap.size());    // 確認用
+}
+
+// 局所地図の生成。全体地図をそのまま使う
+void PointCloudMapGT::makeLocalMap(){
+  localMap = globalMap;
+  printf("GT: localMap.size=%lu\n", localMap.size());
+}
+
+////////
+
+// ダミー
+void PointCloudMapGT::remakeMaps(const vector<Pose2D> &newPoses) {
+}
diff --git a/hook/PointCloudMapGT.h b/hook/PointCloudMapGT.h
new file mode 100755
index 0000000..f0614a3
--- /dev/null
+++ b/hook/PointCloudMapGT.h
@@ -0,0 +1,49 @@
+/****************************************************************************
+ * 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 PointCloudMapGT.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef POINT_CLOUD_MAP_GT_H_
+#define POINT_CLOUD_MAP_GT_H_
+
+#include <boost/unordered_map.hpp>
+#include "PointCloudMap.h"
+#include "NNGridTable.h"
+
+///////////
+
+// 格子テーブルを用いた点群地図
+class PointCloudMapGT : public PointCloudMap
+{
+public:
+  std::vector<LPoint2D> allLps;             // 全スキャン点群
+  NNGridTable nntab;                        // 格子テーブル
+
+public:
+  PointCloudMapGT() {
+    allLps.reserve(MAX_POINT_NUM);          // 最初に確保
+  }
+
+  ~PointCloudMapGT() {
+  }
+
+/////////////
+
+  virtual void addPose(const Pose2D &p);
+  virtual void addPoints(const std::vector<LPoint2D> &lps);
+  virtual void makeGlobalMap();
+  virtual void makeLocalMap();
+  void subsamplePoints(std::vector<LPoint2D> &sps);
+  virtual void remakeMaps(const std::vector<Pose2D> &newPoses);
+};
+
+#endif
diff --git a/hook/PointCloudMapLP.cpp b/hook/PointCloudMapLP.cpp
new file mode 100755
index 0000000..0cbd846
--- /dev/null
+++ b/hook/PointCloudMapLP.cpp
@@ -0,0 +1,161 @@
+/****************************************************************************
+ * 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 PointCloudMapLP.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "PointCloudMapLP.h"
+#include "NNGridTable.h"
+
+using namespace std;
+
+double PointCloudMapLP::atdThre = 10;
+
+///////////
+
+// 格子テーブルを用いて、部分地図の代表点を得る
+vector<LPoint2D> Submap::subsamplePoints(int nthre) {
+  NNGridTable nntab;                     // 格子テーブル
+  for (size_t i=0; i<mps.size(); i++) {
+    LPoint2D &lp = mps[i];
+    nntab.addPoint(&lp);                 // 全点を登録
+  }
+
+  vector<LPoint2D> sps;
+  nntab.makeCellPoints(nthre, sps);      // nthre個以上のセルの代表点をspsに入れる
+  printf("mps.size=%lu, sps.size=%lu\n", mps.size(), sps.size());
+
+  return(sps);
+}
+
+/////////
+
+// ロボット位置の追加
+void PointCloudMapLP::addPose(const Pose2D &p) {
+  // 累積走行距離(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));
+  }
+  else {
+    atd += sqrt(p.tx*p.tx + p.ty*p.ty);
+  }
+
+  poses.emplace_back(p);
+}
+
+// スキャン点の追加
+void PointCloudMapLP::addPoints(const vector<LPoint2D> &lps) {
+  Submap &curSubmap = submaps.back();              // 現在の部分地図
+  if (atd - curSubmap.atdS >= atdThre ) {          // 累積走行距離が閾値を超えたら新しい部分地図に変える
+    size_t size = poses.size();
+    curSubmap.cntE = size-1;                       // 部分地図の最後のスキャン番号
+    curSubmap.mps = curSubmap.subsamplePoints(nthre); // 終了した部分地図は代表点のみにする（軽量化）
+
+    Submap submap(atd, size);                      // 新しい部分地図
+    submap.addPoints(lps);                         // スキャン点群の登録
+    submaps.emplace_back(submap);                  // 部分地図を追加
+  }
+  else {
+    curSubmap.addPoints(lps);                      // 現在の部分地図に点群を追加
+  }
+}
+
+// 全体地図の生成。局所地図もここでいっしょに作った方が速い
+void PointCloudMapLP::makeGlobalMap(){
+  globalMap.clear();                               // 初期化
+  localMap.clear();
+  // 現在以外のすでに確定した部分地図から点を集める
+  for (size_t i=0; i<submaps.size()-1; i++) {
+    Submap &submap = submaps[i];                   // 部分地図
+    vector<LPoint2D> &mps = submap.mps;            // 部分地図の点群。代表点だけになっている
+    for (size_t j=0; j<mps.size(); j++) {
+      globalMap.emplace_back(mps[j]);              // 全体地図には全点入れる
+    }
+    if (i == submaps.size()-2) {                   // 局所地図には最後の部分地図だけ入れる
+      for (size_t j=0; j<mps.size(); j++) {
+        localMap.emplace_back(mps[j]);
+      }
+    }
+  }
+
+  // 現在の部分地図の代表点を全体地図と局所地図に入れる
+  Submap &curSubmap = submaps.back();              // 現在の部分地図
+  vector<LPoint2D> sps = curSubmap.subsamplePoints(nthre);  // 代表点を得る
+  for (size_t i=0; i<sps.size(); i++) {
+    globalMap.emplace_back(sps[i]);
+    localMap.emplace_back(sps[i]);
+  }
+
+  // 以下は確認用
+  printf("curSubmap.atd=%g, atd=%g, sps.size=%lu\n", curSubmap.atdS, atd, sps.size());
+  printf("submaps.size=%lu, globalMap.size=%lu\n", submaps.size(), globalMap.size());
+}
+
+// 局所地図の生成
+void PointCloudMapLP::makeLocalMap(){
+  localMap.clear();                                // 初期化
+  if (submaps.size() >= 2) {
+    Submap &submap = submaps[submaps.size()-2];    // 直前の部分地図だけ使う
+    vector<LPoint2D> &mps = submap.mps;            // 部分地図の点群。代表点だけになっている
+    for (size_t i=0; i<mps.size(); i++) {
+      localMap.emplace_back(mps[i]);
+    }
+  }
+
+  // 現在の部分地図の代表点を局所地図に入れる
+  Submap &curSubmap = submaps.back();              // 現在の部分地図
+  vector<LPoint2D> sps = curSubmap.subsamplePoints(nthre);  // 代表点を得る
+  for (size_t i=0; i<sps.size(); i++) {
+    localMap.emplace_back(sps[i]);
+  }
+
+  printf("localMap.size=%lu\n", localMap.size());   // 確認用
+}
+
+//////////
+
+// ポーズ調整後のロボット軌跡newPoseを用いて、地図を再構築する
+void PointCloudMapLP::remakeMaps(const vector<Pose2D> &newPoses){
+  // 各部分地図内の点の位置を修正する
+  for (size_t i=0; i<submaps.size(); i++) {
+    Submap &submap = submaps[i];
+    vector<LPoint2D> &mps = submap.mps;                // 部分地図の点群。現在地図以外は代表点になっている
+    for (size_t j=0; j<mps.size(); j++) {
+      LPoint2D &mp = mps[j];
+      size_t idx = mp.sid;                             // 点のスキャン番号
+      if (idx >= poses.size()) {                       // 不正なスキャン番号（あったらバグ）
+        continue;
+      }
+
+      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);        // 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;     // 法線ベクトルもoldPoseでセンサ座標系に変換
+      double ny = R1[0][1]*mp.nx + R1[1][1]*mp.ny;
+      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();                                     // 部分地図から全体地図と局所地図を生成
+
+  for (size_t i=0; i<poses.size(); i++) {              // posesをポーズ調整後の値に更新
+    poses[i] = newPoses[i];
+  }
+  lastPose = newPoses.back();
+}
diff --git a/hook/PointCloudMapLP.h b/hook/PointCloudMapLP.h
new file mode 100755
index 0000000..998918b
--- /dev/null
+++ b/hook/PointCloudMapLP.h
@@ -0,0 +1,91 @@
+/****************************************************************************
+ * 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 PointCloudMapLP.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef POINT_CLOUD_MAP_LP_H_
+#define POINT_CLOUD_MAP_LP_H_
+
+#include <boost/unordered_map.hpp>
+#include "PointCloudMap.h"
+
+///////////
+
+// 部分地図
+struct Submap
+{
+  double atdS;                              // 部分地図の始点での累積走行距離
+  size_t cntS;                              // 部分地図の最初のスキャン番号
+  size_t cntE;                              // 部分地図の最後のスキャン番号
+
+  std::vector<LPoint2D> mps;                // 部分地図内のスキャン点群
+
+  Submap() : atdS(0), cntS(0), cntE(-1) {
+  }
+
+  Submap(double a, size_t s) : cntE(-1) {
+    atdS = a;
+    cntS = s;
+  }
+
+  void addPoints(const std::vector<LPoint2D> &lps) {
+    for (size_t i=0; i<lps.size(); i++)
+      mps.emplace_back(lps[i]);
+  }
+
+  std::vector<LPoint2D> subsamplePoints(int nthre);
+};
+
+///////////
+
+// 部分地図から構成される点群地図
+class PointCloudMapLP : public PointCloudMap
+{
+public:
+  static double atdThre;                    // 部分地図の区切りとなる累積走行距離(atd)[m]
+  double atd;                               // 現在の累積走行距離(accumulated travel distance)
+  std::vector<Submap> submaps;              // 部分地図
+
+public:
+  PointCloudMapLP() {
+    Submap submap;
+    submaps.emplace_back(submap);           // 最初の部分地図を作っておく
+  }
+
+  ~PointCloudMapLP() {
+  }
+
+//////////
+
+  Pose2D getLastPose() const {
+    return(lastPose);
+  }
+
+  void setLastPose(Pose2D &p) {
+    lastPose = p;
+  }
+
+  std::vector<Submap> &getSubmaps() {
+    return(submaps);
+  }
+
+/////////////
+
+  virtual void addPose(const Pose2D &p);
+  virtual void addPoints(const std::vector<LPoint2D> &lps);
+  virtual void makeGlobalMap();
+  virtual void makeLocalMap();
+  virtual void remakeMaps(const std::vector<Pose2D> &newPoses);
+
+};
+
+#endif
diff --git a/hook/PoseOptimizerSD.cpp b/hook/PoseOptimizerSD.cpp
new file mode 100755
index 0000000..1b22058
--- /dev/null
+++ b/hook/PoseOptimizerSD.cpp
@@ -0,0 +1,68 @@
+/****************************************************************************
+ * 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 PoseOptimizerSD.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "PoseOptimizerSD.h"
+
+using namespace std;
+
+////////
+
+// データ対応づけ固定のもと、初期値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;         // コスト最小の解
+  double evmin = HUGE_VAL;                     // コストの最小値
+  double evold = evmin;                        // 1つ前のコスト値。収束判定に使う
+
+  double ev = cfunc->calValue(tx, ty, th);     // コスト計算
+  int nn=0;                                    // 繰り返し回数。確認用
+  double kk=0.00001;                           // 最急降下法のステップ幅係数
+  while (abs(evold-ev) > evthre) {             // 収束判定。1つ前の値との変化が小さいと終了
+    nn++;
+    evold = ev;
+
+    // 数値計算による偏微分
+    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;
+
+    // 微分係数にkkをかけてステップ幅にする
+    double dx = -kk*dEtx;
+    double dy = -kk*dEty;
+    double dth = -kk*dEth;
+    tx += dx;  ty += dy;  th += dth;            // ステップ幅を加えて次の探索位置を決める
+
+    ev = cfunc->calValue(tx, ty, th);           // その位置でコスト計算
+
+    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));         // 確認用
+  }
+
+  ++allN;
+  if (allN > 0 && evmin < 100) 
+    sum += evmin;
+//  printf("allN=%d, evmin=%g, avg=%g\n", allN, evmin, (sum/allN));         // 確認用
+
+//  printf("nn=%d, ev=%g\n", nn, ev);         // 確認用
+
+  estPose.setVal(txmin, tymin, thmin);          // 最小値を与える解を保存
+
+  return(evmin);
+}
diff --git a/hook/PoseOptimizerSD.h b/hook/PoseOptimizerSD.h
new file mode 100755
index 0000000..ab5987a
--- /dev/null
+++ b/hook/PoseOptimizerSD.h
@@ -0,0 +1,35 @@
+/****************************************************************************
+ * 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 PoseOptimizerSD.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _POSE_OPTIMIZER_SD_H_
+#define _POSE_OPTIMIZER_SD_H_
+
+#include "PoseOptimizer.h"
+
+// 最急降下法でコスト関数を最小化する
+class PoseOptimizerSD : public PoseOptimizer
+{
+public:
+  PoseOptimizerSD() {
+  }
+
+  ~PoseOptimizerSD() {
+  }
+
+/////
+
+  virtual double optimizePose(Pose2D &initPose, Pose2D &estPose);
+};
+
+#endif 
diff --git a/hook/PoseOptimizerSL.cpp b/hook/PoseOptimizerSL.cpp
new file mode 100755
index 0000000..dae5121
--- /dev/null
+++ b/hook/PoseOptimizerSL.cpp
@@ -0,0 +1,101 @@
+/****************************************************************************
+ * 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 PoseOptimizerSL.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include <boost/math/tools/minima.hpp>
+#include "PoseOptimizerSL.h"
+
+using namespace std;
+
+////////
+
+// データ対応づけ固定のもと、初期値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;           // コスト最小の解
+  double evmin = HUGE_VAL;                       // コストの最小値
+  double evold = evmin;                          // 1つ前のコスト値。収束判定に使う
+  Pose2D pose, dir;
+
+  double ev = cfunc->calValue(tx, ty, th);       // コスト計算
+  int nn=0;                                      // 繰り返し回数。確認用
+  while (abs(evold-ev) > evthre) {               // 収束判定。値の変化が小さいと終了
+    nn++;
+    evold = ev;
+
+    // 数値計算による偏微分
+    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;              // いったん次の探索位置を決める
+
+    // ブレント法による直線探索
+    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);             // 求めた位置でコスト計算
+
+    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));         // 確認用
+  }
+  ++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);         // 確認用
+
+//  printf("nn=%d, evmin=%g\n", nn, evmin);       // 確認用
+
+  estPose.setVal(txmin, tymin, thmin);            // 最小値を与える解を保存
+
+  return(evmin);
+}
+
+////////// Line search ///////////
+
+// boostライブラリのブレント法で直線探索を行う。
+// poseを始点に、dp方向にどれだけ進めばよいかステップ幅を見つける。
+double PoseOptimizerSL::search(double ev0, Pose2D &pose, Pose2D &dp) {
+  int bits = numeric_limits<double>::digits;         // 探索精度
+  boost::uintmax_t maxIter=40;                       // 最大繰り返し回数。経験的に決める
+  pair<double, double> result = 
+    boost::math::tools::brent_find_minima(
+    [this, &pose, &dp](double tt) {return (objFunc(tt, pose, dp));},
+    -2.0, 2.0, bits, maxIter);                       // 探索範囲(-2.0,2.0)
+
+  double t = result.first;                           // 求めるステップ幅
+  double v = result.second;                          // 求める最小値
+
+  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);
+}  
+
+// 直線探索の目的関数。ttがステップ幅
+double PoseOptimizerSL::objFunc(double tt, Pose2D &pose, Pose2D &dp) {
+  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);             // コスト関数値
+
+  return(v);
+}
diff --git a/hook/PoseOptimizerSL.h b/hook/PoseOptimizerSL.h
new file mode 100755
index 0000000..4f6cb3e
--- /dev/null
+++ b/hook/PoseOptimizerSL.h
@@ -0,0 +1,37 @@
+/****************************************************************************
+ * 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 PoseOptimizerSL.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _POSE_OPTIMIZER_SL_H_
+#define _POSE_OPTIMIZER_SL_H_
+
+#include "PoseOptimizer.h"
+
+// 直線探索つきの最急降下法でコスト関数を最小化する
+class PoseOptimizerSL : public PoseOptimizer
+{
+public:
+  PoseOptimizerSL() {
+  }
+
+  ~PoseOptimizerSL() {
+  }
+
+/////
+
+  virtual double optimizePose(Pose2D &initPose, Pose2D &estPose);
+  double search(double ev0, Pose2D &pose, Pose2D &dp);
+  double objFunc(double tt, Pose2D &pose, Pose2D &dp);
+};
+
+#endif 
diff --git a/hook/RefScanMakerBS.cpp b/hook/RefScanMakerBS.cpp
new file mode 100755
index 0000000..7c80e20
--- /dev/null
+++ b/hook/RefScanMakerBS.cpp
@@ -0,0 +1,43 @@
+/****************************************************************************
+ * 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 RefScanMakerBS.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "RefScanMakerBS.h"
+
+using namespace std;
+
+const Scan2D *RefScanMakerBS::makeRefScan() {
+  vector<LPoint2D> &refLps = refScan.lps;         // 参照スキャンの点群のコンテナ
+  refLps.clear();
+
+  Pose2D lastPose = pcmap->getLastPose();         // 点群地図に保存した最後の推定位置
+  double (*R)[2] = lastPose.Rmat;
+  double tx = lastPose.tx;
+  double ty = lastPose.ty;
+
+  // 点群地図に保存した最後のスキャンを参照スキャンにする
+  const vector<LPoint2D> &lps = pcmap->lastScan.lps;
+  for (size_t i=0; i<lps.size(); i++) {
+    const LPoint2D &mp = lps[i];                  // 参照スキャンの点
+
+    // スキャンはロボット座標系なので、地図座標系に変換
+    LPoint2D rp;
+    rp.x = R[0][0]*mp.x + R[0][1]*mp.y + tx;      // 点の位置
+    rp.y = R[1][0]*mp.x + R[1][1]*mp.y + ty;
+    rp.nx = R[0][0]*mp.nx + R[0][1]*mp.ny;        // 法線ベクトル
+    rp.ny = R[1][0]*mp.nx + R[1][1]*mp.ny;
+    refLps.emplace_back(rp);
+  }
+
+  return(&refScan);
+}
diff --git a/hook/RefScanMakerBS.h b/hook/RefScanMakerBS.h
new file mode 100755
index 0000000..4bf49c4
--- /dev/null
+++ b/hook/RefScanMakerBS.h
@@ -0,0 +1,32 @@
+/****************************************************************************
+ * 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 RefScanMakerBS.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _REF_SCAN_MAKER_BS_H_
+#define _REF_SCAN_MAKER_BS_H_
+
+#include "RefScanMaker.h"
+
+class RefScanMakerBS : public RefScanMaker
+{
+public:
+  RefScanMakerBS() {
+  }
+  ~RefScanMakerBS() {
+  }
+
+  virtual const Scan2D *makeRefScan();
+
+};
+
+#endif
diff --git a/hook/RefScanMakerLM.cpp b/hook/RefScanMakerLM.cpp
new file mode 100755
index 0000000..bed31f1
--- /dev/null
+++ b/hook/RefScanMakerLM.cpp
@@ -0,0 +1,31 @@
+/****************************************************************************
+ * 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 RefScanMakerLM.cpp
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#include "RefScanMakerLM.h"
+
+using namespace std;
+
+
+const Scan2D *RefScanMakerLM::makeRefScan() {
+  vector<LPoint2D> &refLps = refScan.lps;         // 参照スキャンの点群のコンテナ
+  refLps.clear();
+
+  const vector<LPoint2D> &localMap = pcmap->localMap;  // 点群地図の局所地図
+  for (size_t i=0; i<localMap.size(); i++) {
+    const LPoint2D &rp = localMap[i];
+    refLps.emplace_back(rp);                           // 単にコピー
+  }
+
+  return(&refScan);
+}
diff --git a/hook/RefScanMakerLM.h b/hook/RefScanMakerLM.h
new file mode 100755
index 0000000..deef66f
--- /dev/null
+++ b/hook/RefScanMakerLM.h
@@ -0,0 +1,32 @@
+/****************************************************************************
+ * 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 RefScanMakerLM.h
+ * @author Masahiro Tomono
+ ****************************************************************************/
+
+#ifndef _REF_SCAN_MAKER_LM_H_
+#define _REF_SCAN_MAKER_LM_H_
+
+#include "RefScanMaker.h"
+
+class RefScanMakerLM : public RefScanMaker
+{
+public:
+  RefScanMakerLM() {
+  }
+  ~RefScanMakerLM() {
+  }
+
+  virtual const Scan2D *makeRefScan();
+
+};
+
+#endif