├── .circleci └── config.yml ├── .dockerignore ├── .gitattributes ├── .gitignore ├── CMakeLists.txt ├── Dockerfile ├── LICENSE ├── Makefile ├── README.md ├── confusion_matrix_example.png ├── docker-compose.yml ├── requirements.txt ├── scripts ├── build.sh ├── download_data.sh ├── ground_truth_comparison.py └── precision_recall.py └── src ├── frame_descriptor.h ├── new_college.cpp └── utils.h /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | 3 | jobs: 4 | build: 5 | docker: 6 | - image: nicolov/simple_slam_loop_closure:0.0.2 7 | steps: 8 | - checkout 9 | - run: 10 | name: download data files 11 | command: scripts/download_data.sh 12 | - run: 13 | name: build 14 | command: scripts/build.sh 15 | - run: 16 | name: run 17 | command: rm -rf out && mkdir out && ./build/new_college 18 | - run: 19 | name: plot confusion matrix 20 | command: ./scripts/ground_truth_comparison.py 21 | - run: 22 | name: plot precision/recall curve 23 | command: ./scripts/precision_recall.py 24 | - store_artifacts: 25 | path: out 26 | 27 | workflows: 28 | version: 2 29 | build: 30 | jobs: 31 | - build 32 | -------------------------------------------------------------------------------- /.dockerignore: -------------------------------------------------------------------------------- 1 | * 2 | !requirements.txt 3 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | *.gz filter=lfs diff=lfs merge=lfs -text 2 | *.jpg filter=lfs diff=lfs merge=lfs -text 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /build 2 | /data 3 | /out 4 | .DS_Store 5 | /env 6 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(simple_slam_loop_closure) 3 | 4 | include(ExternalProject) 5 | 6 | IF(NOT CMAKE_BUILD_TYPE) 7 | SET(CMAKE_BUILD_TYPE Release) 8 | ENDIF() 9 | 10 | MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) 11 | 12 | add_definitions(-std=c++11 -Wall -O3 -march=native) 13 | 14 | find_package(OpenCV 3 REQUIRED) 15 | find_package(Eigen3 REQUIRED) 16 | 17 | include_directories( 18 | ${EIGEN3_INCLUDE_DIR} 19 | ${OpenCV_INCLUDE_DIRS} 20 | ) 21 | 22 | set(DEPENDENCY_DIR ${CMAKE_CURRENT_BINARY_DIR}/dependencies) 23 | set(DEPENDENCY_INSTALL_DIR ${DEPENDENCY_DIR}/install) 24 | 25 | macro(GetDependency name other_dependency) 26 | find_package(${name} QUIET 27 | PATHS ${DEPENDENCY_INSTALL_DIR}) 28 | if(${${name}_FOUND}) 29 | message("${name} library found, using it from the system") 30 | include_directories(${${name}_INCLUDE_DIRS}) 31 | add_custom_target(${name}) 32 | else(${${name}_FOUND}) 33 | message("${name} library not found in the system, it will be downloaded on build") 34 | option(DOWNLOAD_${name}_dependency "Download ${name} dependency" ON) 35 | if(${DOWNLOAD_${name}_dependency}) 36 | ExternalProject_Add(${name} 37 | PREFIX ${DEPENDENCY_DIR} 38 | GIT_REPOSITORY http://github.com/dorian3d/${name} 39 | GIT_TAG v1.1-nonfree 40 | INSTALL_DIR ${DEPENDENCY_INSTALL_DIR} 41 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX= 42 | DEPENDS ${other_dependency}) 43 | else() 44 | message(SEND_ERROR "Please, activate DOWNLOAD_${name}_dependency option or download manually") 45 | endif(${DOWNLOAD_${name}_dependency}) 46 | endif(${${name}_FOUND}) 47 | endmacro(GetDependency) 48 | 49 | GetDependency(DLib "") 50 | GetDependency(DBoW2 DLib) 51 | add_custom_target(Dependencies 52 | ${CMAKE_COMMAND} ${CMAKE_SOURCE_DIR} 53 | DEPENDS DBoW2 DLib) 54 | 55 | include_directories( 56 | ${DEPENDENCY_DIR}/install/include) 57 | 58 | 59 | add_executable(new_college 60 | src/new_college.cpp) 61 | target_link_libraries(new_college 62 | ${OpenCV_LIBS} 63 | ${EIGEN3_LIBS} 64 | ${DLib_LIBS} 65 | ${DBoW2_LIBS}) 66 | add_dependencies(new_college 67 | Dependencies) 68 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:xenial 2 | 3 | ENV DEBIAN_FRONTEND noninteractive 4 | 5 | # Development 6 | RUN apt-get update \ 7 | && apt-get install -y --no-install-recommends \ 8 | build-essential \ 9 | ca-certificates \ 10 | cmake \ 11 | curl \ 12 | git \ 13 | vim \ 14 | wget 15 | 16 | # OpenCV dependencies 17 | RUN apt-get install -y --no-install-recommends \ 18 | libboost-dev \ 19 | libeigen3-dev 20 | 21 | # Build OpenCV 22 | RUN mkdir /tmp/opencv-build 23 | WORKDIR /tmp/opencv-build 24 | 25 | ENV OPENCV_VERSION="3.4.2" 26 | 27 | RUN wget -O opencv_contrib.tar.gz https://github.com/opencv/opencv_contrib/archive/${OPENCV_VERSION}.tar.gz \ 28 | && tar -xzf opencv_contrib.tar.gz \ 29 | && rm -rf opencv_contrib-${OPENCV_VERSION}/modules/datasets 30 | 31 | RUN wget https://github.com/opencv/opencv/archive/${OPENCV_VERSION}.tar.gz \ 32 | && tar -xzf ${OPENCV_VERSION}.tar.gz \ 33 | && cd opencv-${OPENCV_VERSION} \ 34 | && mkdir build \ 35 | && cd build \ 36 | && cmake \ 37 | -DOPENCV_EXTRA_MODULES_PATH=../../opencv_contrib-${OPENCV_VERSION}/modules \ 38 | -DBUILD_TIFF=ON \ 39 | -DBUILD_opencv_java=OFF \ 40 | -DWITH_CUDA=OFF \ 41 | -DWITH_OPENGL=OFF \ 42 | -DWITH_OPENCL=OFF \ 43 | -DWITH_IPP=OFF \ 44 | -DWITH_TBB=OFF \ 45 | -DWITH_EIGEN=ON \ 46 | -DWITH_V4L=OFF \ 47 | -DBUILD_TESTS=OFF \ 48 | -DBUILD_PERF_TESTS=OFF \ 49 | -DCMAKE_BUILD_TYPE=RELEASE \ 50 | .. \ 51 | && make -j$(nproc) \ 52 | && make install \ 53 | && rm -rf /tmp/opencv-build 54 | 55 | RUN apt-get update \ 56 | && apt-get install -y --no-install-recommends \ 57 | python3 \ 58 | python3-pip \ 59 | unzip 60 | 61 | COPY requirements.txt /tmp/requirements.txt 62 | RUN pip3 install -r /tmp/requirements.txt 63 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Nicolo Valigi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | clang-format: 2 | find src/ -iname *.h -o -iname *.cpp | xargs clang-format -style=google -i 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Simple loop closure for Visual SLAM 2 | 3 | [![CircleCI](https://circleci.com/gh/nicolov/simple_slam_loop_closure.svg?style=shield)](https://circleci.com/gh/nicolov/simple_slam_loop_closure) 4 | 5 | 6 | 7 | Possibily the simplest example of loop closure for Visual SLAM. More 8 | information [on my blog](http://nicolovaligi.com/bag-of-words-loop-closure-visual-slam.html). 9 | 10 | As I'm experimenting with alternative approaches for SLAM loop closure, I 11 | wanted a baseline that was reasonably close to state-of-the art approaches. 12 | The approach here is pretty similar to ORB-SLAM's, and uses SURF descriptors 13 | and *bag of words* to translate them to a global image description vector. 14 | 15 | ## The dataset 16 | 17 | For testing, I've used the New College dataset published alongside FAB-MAP. 18 | It's available for download 19 | [here](http://www.ijrr.org/ijrr_2008/volume27-issue6/090961/3_data.htm). It's 20 | ideal for loop-closure testing, since it includes manual place associations 21 | that can be used for evaluation. The `scripts/download_data.sh` will 22 | download the data files (bag of words vocabulary and images) needed to run 23 | the code. 24 | 25 | ## Building with Docker 26 | 27 | You can build and run the code using `docker-compose` and Docker. The Docker 28 | configuration uses a Ubuntu 16.04 base image, and builds OpenCV 3 from source. 29 | 30 | ``` 31 | # Download the data files 32 | ./scripts/download_data.sh 33 | 34 | # Will take ~10 minutes to download and build OpenCV 3 35 | docker-compose build runner 36 | # Enter the docker shell 37 | docker-compose run runner bash 38 | # You're now in a shell inside the Docker container, build and run the code: 39 | ./scripts/build.sh 40 | ./build/new_college ./data/brief_k10L6.voc.gz ./data 41 | ``` 42 | 43 | ## Compatibility 44 | 45 | Only tested on Ubuntu 16.04 LTS with OpenCV3, gcc 5.4.0 46 | 47 | ## Plotting the confusion matrix 48 | 49 | The `ground_truth_comparison.py` plots and compares the loop closures from the 50 | ground truth to the actual results from the code. 51 | -------------------------------------------------------------------------------- /confusion_matrix_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nicolov/simple_slam_loop_closure/465b15127219d716c13df80e10aa130463a033f6/confusion_matrix_example.png -------------------------------------------------------------------------------- /docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: '3' 2 | services: 3 | runner: 4 | build: . 5 | volumes: 6 | - .:/src 7 | working_dir: /src 8 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | cycler==0.10.0 2 | kiwisolver==1.0.1 3 | matplotlib==3.0.2 4 | numpy==1.16.1 5 | pandas==0.24.1 6 | pyparsing==2.3.1 7 | python-dateutil==2.8.0 8 | pytz==2018.9 9 | scipy==1.2.0 10 | seaborn==0.9.0 11 | six==1.12.0 12 | -------------------------------------------------------------------------------- /scripts/build.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | set -euxo pipefail 4 | 5 | rm -rf build 6 | mkdir build 7 | cd build 8 | cmake .. 9 | make -j$(nproc) 10 | -------------------------------------------------------------------------------- /scripts/download_data.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | set -euxo pipefail 4 | 5 | BASE_URL=https://github.com/nicolov/simple_slam_loop_closure/releases/download/0.0.1 6 | 7 | rm -rf data 8 | mkdir data 9 | 10 | wget -O data/surf64_k10L6.voc.gz "${BASE_URL}/surf64_k10L6.voc.gz" 11 | 12 | wget -O data/Images.zip "${BASE_URL}/Images.zip" 13 | (cd data && unzip Images.zip) 14 | 15 | wget -O data/ImageCollectionCoordinates.txt "${BASE_URL}/ImageCollectionCoordinates.txt" 16 | 17 | wget -O data/NewCollegeGroundTruth.mat "${BASE_URL}/NewCollegeGroundTruth.mat" 18 | -------------------------------------------------------------------------------- /scripts/ground_truth_comparison.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import argparse 4 | import os 5 | import sys 6 | 7 | import matplotlib 8 | import seaborn as sns 9 | import matplotlib.pyplot as plt 10 | 11 | import scipy.io as sio 12 | import numpy as np 13 | 14 | if __name__ == "__main__": 15 | parser = argparse.ArgumentParser() 16 | parser.add_argument('--gt-path', 17 | default='data/NewCollegeGroundTruth.mat') 18 | parser.add_argument('--eval-path', 19 | default='out/confusion_matrix.txt') 20 | args = parser.parse_args() 21 | 22 | default_heatmap_kwargs = dict( 23 | xticklabels=False, 24 | yticklabels=False, 25 | square=True, 26 | cbar=False,) 27 | 28 | fig, (ax1, ax2) = plt.subplots(ncols=2) 29 | 30 | # Plot the ground truth 31 | gt_data = sio.loadmat(args.gt_path)['truth'] 32 | sns.heatmap(gt_data[::2, ::2], 33 | ax=ax1, 34 | **default_heatmap_kwargs) 35 | ax1.set_title('Ground truth') 36 | 37 | # Plot the BoW results 38 | bow_data = np.loadtxt(args.eval_path) 39 | # Take the lower triangle only 40 | bow_data = np.tril(bow_data, 0) 41 | sns.heatmap(bow_data, 42 | ax=ax2, 43 | vmax=0.2, 44 | **default_heatmap_kwargs) 45 | ax2.set_title('SURF + BoW') 46 | 47 | # plt.show() 48 | plt.tight_layout() 49 | plt.savefig(args.eval_path.replace('.txt', '.png'), bbox_inches='tight') 50 | -------------------------------------------------------------------------------- /scripts/precision_recall.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import argparse 4 | import os 5 | import sys 6 | 7 | import matplotlib 8 | 9 | import seaborn as sns 10 | import matplotlib.pyplot as plt 11 | 12 | import scipy.io as sio 13 | import numpy as np 14 | 15 | if __name__ == "__main__": 16 | parser = argparse.ArgumentParser() 17 | parser.add_argument('--gt-path', 18 | default='data/NewCollegeGroundTruth.mat') 19 | parser.add_argument('--eval-path', 20 | default='out/confusion_matrix.txt') 21 | args = parser.parse_args() 22 | 23 | gt_data = sio.loadmat(args.gt_path)['truth'][::2, ::2] 24 | 25 | bow_data = np.loadtxt(args.eval_path) 26 | # Take the lower triangle only 27 | bow_data = np.tril(bow_data, -1) 28 | 29 | prec_recall_curve = [] 30 | 31 | for thresh in np.arange(0, 0.09, 0.002): 32 | # precision: fraction of retrieved instances that are relevant 33 | # recall: fraction of relevant instances that are retrieved 34 | true_positives = (bow_data > thresh) & (gt_data == 1) 35 | all_positives = (bow_data > thresh) 36 | 37 | try: 38 | precision = float(np.sum(true_positives)) / np.sum(all_positives) 39 | recall = float(np.sum(true_positives)) / np.sum(gt_data == 1) 40 | 41 | prec_recall_curve.append([thresh, precision, recall]) 42 | except: 43 | break 44 | 45 | prec_recall_curve = np.array(prec_recall_curve) 46 | 47 | plt.plot(prec_recall_curve[:, 1], prec_recall_curve[:, 2]) 48 | 49 | for thresh, prec, rec in prec_recall_curve[5::5]: 50 | plt.annotate( 51 | str(thresh), 52 | xy=(prec, rec), 53 | xytext=(8, 8), 54 | textcoords='offset points') 55 | 56 | plt.xlabel('Precision', fontsize=14) 57 | plt.ylabel('Recall', fontsize=14) 58 | 59 | # plt.show() 60 | plt.tight_layout() 61 | plt.savefig(args.eval_path.replace('.txt', '_prec_recall.png'), bbox_inches='tight') 62 | -------------------------------------------------------------------------------- /src/frame_descriptor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace slc { 9 | struct FrameDescriptor { 10 | /* 11 | * Computes a global representation for an image by using 12 | * the SURF feature descriptor in OpenCV and the bag of 13 | * words approach. 14 | */ 15 | FrameDescriptor(const std::string& vocabulary_path) { 16 | std::cout << "Loading vocabulary from " << vocabulary_path << std::endl; 17 | vocab_.reset(new Surf64Vocabulary(vocabulary_path)); 18 | std::cout << "Loaded vocabulary with " << vocab_->size() << " visual words." 19 | << std::endl; 20 | } 21 | 22 | void extract_surf(const cv::Mat& img, std::vector& keys, 23 | std::vector>& descriptors) { 24 | /* Extracts SURF interest points and their descriptors. */ 25 | 26 | static cv::Ptr surf_detector = 27 | cv::xfeatures2d::SURF::create(400); 28 | 29 | surf_detector->setExtended(false); 30 | 31 | std::vector plain; 32 | surf_detector->detectAndCompute(img, cv::Mat(), keys, plain); 33 | 34 | const int L = surf_detector->descriptorSize(); 35 | descriptors.resize(plain.size() / L); 36 | 37 | unsigned int j = 0; 38 | for (unsigned int i = 0; i < plain.size(); i += L, ++j) { 39 | descriptors[j].resize(L); 40 | std::copy(plain.begin() + i, plain.begin() + i + L, 41 | descriptors[j].begin()); 42 | } 43 | } 44 | 45 | void describe_frame(const cv::Mat& img, DBoW2::BowVector& bow_vec) { 46 | /* Transforms the feature descriptors to a BoW representation 47 | * of the whole image. */ 48 | 49 | std::vector keypoints; 50 | std::vector> descriptors; 51 | 52 | extract_surf(img, keypoints, descriptors); 53 | vocab_->transform(descriptors, bow_vec); 54 | } 55 | 56 | std::unique_ptr vocab_; 57 | }; 58 | } // namespace slc 59 | -------------------------------------------------------------------------------- /src/new_college.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "frame_descriptor.h" 12 | #include "utils.h" 13 | 14 | int main(int argc, char* argv[]) { 15 | std::string vocabulary_path("data/surf64_k10L6.voc.gz"); 16 | slc::FrameDescriptor descriptor(vocabulary_path); 17 | 18 | std::string dataset_folder("data"); 19 | const auto filenames = load_filenames(dataset_folder); 20 | 21 | if (filenames.size() == 0) { 22 | std::cerr << "No images found in " << dataset_folder << "\n"; 23 | exit(1); 24 | } 25 | 26 | std::cerr << "Processing " << filenames.size() << " images\n"; 27 | 28 | // Will hold BoW representations for each frame 29 | std::vector bow_vecs; 30 | bow_vecs.reserve(filenames.size()); 31 | 32 | for (unsigned int img_i = 0; img_i < filenames.size(); img_i++) { 33 | auto img_filename = dataset_folder + "/Images/" + filenames[img_i]; 34 | auto img = cv::imread(img_filename); 35 | 36 | std::cerr << img_filename << "\n"; 37 | 38 | if (img.empty()) { 39 | std::cerr << std::endl << "Failed to load: " << img_filename << std::endl; 40 | exit(1); 41 | } 42 | 43 | // Get a BoW description of the current image 44 | DBoW2::BowVector bow_vec; 45 | descriptor.describe_frame(img, bow_vec); 46 | bow_vecs.push_back(bow_vec); 47 | } 48 | 49 | std::cerr << "\nWriting output...\n"; 50 | 51 | std::string output_path("out/confusion_matrix.txt"); 52 | std::ofstream of; 53 | of.open(output_path); 54 | if (of.fail()) { 55 | std::cerr << "Failed to open output file " << output_path << std::endl; 56 | exit(1); 57 | } 58 | 59 | // Compute confusion matrix 60 | // i.e. the (i, j) element of the matrix contains the distance 61 | // between the BoW representation of frames i and j 62 | for (unsigned int i = 0; i < bow_vecs.size(); i++) { 63 | for (unsigned int j = 0; j < bow_vecs.size(); j++) { 64 | of << descriptor.vocab_->score(bow_vecs[i], bow_vecs[j]) << " "; 65 | } 66 | of << "\n"; 67 | } 68 | 69 | of.close(); 70 | std::cerr << "Output done\n"; 71 | } 72 | -------------------------------------------------------------------------------- /src/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | std::vector load_filenames(const std::string& dir, 6 | const bool skip_even = true) { 7 | std::vector filenames; 8 | 9 | auto index_filename = dir + "/ImageCollectionCoordinates.txt"; 10 | 11 | std::cout << "Opening index from " << index_filename << "\n"; 12 | 13 | std::ifstream f; 14 | f.open(index_filename); 15 | 16 | if (!f) { 17 | throw std::runtime_error("Failed to open index file"); 18 | } 19 | 20 | while (!f.eof()) { 21 | std::string l; 22 | getline(f, l); 23 | 24 | if (!l.empty()) { 25 | std::stringstream ss; 26 | ss << l; 27 | std::string filename; 28 | ss >> filename; 29 | filenames.push_back(filename); 30 | } 31 | 32 | // Discard even-numbered images (for stereo datasets) 33 | if (skip_even) { 34 | std::getline(f, l); 35 | } 36 | } 37 | 38 | return filenames; 39 | } --------------------------------------------------------------------------------