├── .gitignore ├── .gmmloc_https.install ├── .gmmloc_ssh.install ├── .travis.yml ├── LICENSE ├── README.md ├── gmmloc ├── CMakeLists.txt ├── include │ └── gmmloc │ │ ├── common │ │ ├── common.h │ │ ├── eigen_stl_types.h │ │ └── eigen_types.h │ │ ├── config.h │ │ ├── cv │ │ ├── orb_extractor.h │ │ ├── orb_matcher.h │ │ ├── orb_vocabulary.h │ │ └── pinhole_camera.h │ │ ├── global.h │ │ ├── gmm │ │ ├── factors.h │ │ ├── gaussian.h │ │ ├── gaussian_mixture.h │ │ └── gmm_utils.h │ │ ├── gmmloc.h │ │ ├── init_config.hpp │ │ ├── modules │ │ ├── localization.h │ │ └── tracking.h │ │ ├── types │ │ ├── feature.h │ │ ├── frame.h │ │ ├── keyframe.h │ │ ├── map.h │ │ └── mappoint.h │ │ ├── utils │ │ ├── cv_utils.h │ │ ├── dataloader.h │ │ ├── math_utils.h │ │ ├── nanoflann.hpp │ │ ├── protobuf_utils.h │ │ └── timing.h │ │ └── visualization │ │ ├── campose_visualizer.h │ │ ├── gmm_visualizer.h │ │ └── visualizer.h ├── node │ └── gmmloc_node.cpp ├── package.xml ├── proto │ └── gmmloc │ │ └── GMM.proto └── src │ ├── config.cpp │ ├── cv │ ├── orb_extractor.cpp │ ├── orb_matcher.cpp │ ├── orb_vocabulary.cpp │ └── pinhole_camera.cpp │ ├── global.cpp │ ├── gmm │ ├── factors.cpp │ ├── gaussian.cpp │ ├── gaussian_mixture.cpp │ └── gmm_utils.cpp │ ├── gmmloc.cpp │ ├── gmmloc_opt.cpp │ ├── modules │ ├── localization.cpp │ ├── localization_gmm.cpp │ ├── localization_opt.cpp │ ├── tracking.cpp │ └── tracking_opt.cpp │ ├── types │ ├── frame.cpp │ ├── keyframe.cpp │ ├── map.cpp │ └── mappoint.cpp │ ├── utils │ ├── cv_utils.cpp │ ├── dataloader.cpp │ ├── math_utils.cpp │ ├── protobuf_utils.cpp │ └── timing.cpp │ └── visualization │ ├── campose_visualizer.cpp │ ├── gmm_visualizer.cpp │ └── visualizer.cpp ├── gmmloc_ros ├── CMakeLists.txt ├── cfg │ ├── euroc_rect.yaml │ ├── v1.yaml │ ├── v2.yaml │ └── viz.rviz ├── data │ ├── gt │ │ ├── V1_01_easy.csv │ │ ├── V1_02_medium.csv │ │ ├── V1_03_difficult.csv │ │ ├── V2_01_easy.csv │ │ ├── V2_02_medium.csv │ │ └── V2_03_difficult.csv │ ├── gt_sync │ │ ├── V1_01_easy.txt │ │ ├── V1_02_medium.txt │ │ ├── V1_03_difficult.txt │ │ ├── V2_01_easy.txt │ │ ├── V2_02_medium.txt │ │ └── V2_03_difficult.txt │ └── map │ │ ├── v1.gmm │ │ └── v2.gmm ├── launch │ ├── v1.launch │ ├── v2.launch │ └── viz.launch ├── package.xml ├── scripts │ ├── evaluate_euroc.sh │ └── evo_euroc.py └── voc │ └── ORBvoc.bin └── orb_dbow2 ├── CMakeLists.txt ├── LICENSE.txt ├── README.txt ├── include └── orb_dbow2 │ ├── dbow2 │ ├── BowVector.h │ ├── FClass.h │ ├── FORB.h │ ├── FeatureVector.h │ ├── ScoringObject.h │ └── TemplatedVocabulary.h │ └── dutils │ ├── Random.h │ ├── Timestamp.h │ └── config.h ├── package.xml └── src ├── dbow2 ├── BowVector.cpp ├── FORB.cpp ├── FeatureVector.cpp └── ScoringObject.cpp └── dutils ├── Random.cpp └── Timestamp.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | gmmloc_ros/expr/* 3 | todo 4 | 5 | # Prerequisites 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | -------------------------------------------------------------------------------- /.gmmloc_https.install: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: deps/catkin_simple 3 | uri: https://github.com/catkin/catkin_simple.git 4 | - git: 5 | local-name: deps/eigen_catkin 6 | uri: https://github.com/ethz-asl/eigen_catkin.git 7 | - git: 8 | local-name: deps/gflags_catkin 9 | uri: https://github.com/ethz-asl/gflags_catkin.git 10 | - git: 11 | local-name: deps/glog_catkin 12 | uri: https://github.com/ethz-asl/glog_catkin.git 13 | - git: 14 | local-name: deps/protobuf_catkin 15 | uri: https://github.com/ethz-asl/protobuf_catkin.git 16 | - git: 17 | local-name: deps/g2o_catkin 18 | uri: https://github.com/hyhuang1995/g2o_catkin.git 19 | -------------------------------------------------------------------------------- /.gmmloc_ssh.install: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: deps/catkin_simple 3 | uri: git@github.com:catkin/catkin_simple.git 4 | - git: 5 | local-name: deps/eigen_catkin 6 | uri: git@github.com:ethz-asl/eigen_catkin.git 7 | - git: 8 | local-name: deps/gflags_catkin 9 | uri: git@github.com:ethz-asl/gflags_catkin.git 10 | - git: 11 | local-name: deps/glog_catkin 12 | uri: git@github.com:ethz-asl/glog_catkin.git 13 | - git: 14 | local-name: deps/protobuf_catkin 15 | uri: git@github.com:ethz-asl/protobuf_catkin.git 16 | - git: 17 | local-name: deps/g2o_catkin 18 | uri: git@github.com:hyhuang1995/g2o_catkin.git 19 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | dist: bionic 2 | sudo: required 3 | cache: 4 | - apt 5 | env: 6 | global: 7 | - ROS_CI_DESKTOP="bionic" 8 | - ROS_DISTRO="melodic" 9 | - CI_SOURCE_PATH=$(pwd) 10 | 11 | before_install: 12 | - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" 13 | - sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 14 | - sudo apt-get update 15 | - sudo apt-get install dpkg 16 | - sudo apt-get install -y libeigen3-dev libqglviewer-dev-qt5 libopencv-dev libsuitesparse-dev 17 | - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool 18 | - sudo apt-get install python-wstool python-catkin-tools ros-$ROS_DISTRO-desktop ros-melodic-cmake-modules 19 | - source /opt/ros/$ROS_DISTRO/setup.bash 20 | 21 | install: 22 | - mkdir -p ~/catkin_ws/src 23 | - cd ~/catkin_ws/ 24 | 25 | # init catkin 26 | - catkin init 27 | - catkin config --extend /opt/ros/melodic 28 | - catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 29 | - catkin config --merge-devel 30 | 31 | # create symbolic 32 | - cd src 33 | - ln -s $CI_SOURCE_PATH ./gmmloc 34 | # install deps 35 | - wstool init . ./gmmloc/.gmmloc_https.install 36 | - wstool update 37 | 38 | before_script: 39 | - echo $(pwd) 40 | script: 41 | - catkin build gmmloc_ros 42 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # GMMLoc 2 | 3 | [![Build Status](https://travis-ci.org/HyHuang1995/gmmloc.svg?branch=master)](https://travis-ci.org/github/HyHuang1995/gmmloc) 4 | [![LICENSE](https://img.shields.io/badge/license-GPL%20(%3E%3D%202)-informational)](https://github.com/HyHuang1995/gmmloc/blob/master/LICENSE) 5 | 6 | Dense Map Based Visual Localization. [[project]](https://sites.google.com/view/gmmloc/) 7 | 8 | ## Paper and Video 9 | 10 | Related publication: 11 | ```latex 12 | @article{huang2020gmmloc, 13 | title={GMMLoc: Structure Consistent Visual Localization with Gaussian Mixture Models}, 14 | author={Huang, Huaiyang and Ye, Haoyang and Sun, Yuxiang and Liu, Ming}, 15 | journal={IEEE Robotics and Automation Letters}, 16 | volume={5}, 17 | number={4}, 18 | pages={5043--5050}, 19 | year={2020}, 20 | publisher={IEEE} 21 | } 22 | ``` 23 | 24 | Demo videos: 25 | 26 | v103 27 | gmmloc 29 | 30 | ## Prerequisites 31 | 32 | We have tested this library in Ubuntu 18.04. Prerequisites for installation: 33 | 34 | 1. [ROS](http://wiki.ros.org/melodic/Installation) (melodic) 35 | 36 | 2. [OpenCV3](https://docs.opencv.org/3.4.11/d7/d9f/tutorial_linux_install.html) 37 | ``` 38 | apt-get install libopencv-dev 39 | ``` 40 | 3. miscs: 41 | ``` 42 | apt-get install python-wstool python-catkin-tools 43 | ``` 44 | 4. [evo](https://github.com/MichaelGrupp/evo) (optional) 45 | ``` 46 | pip install evo --upgrade --no-binary evo 47 | ``` 48 | 49 | ## Installation 50 | Initialize a workspace: 51 | 52 | ``` 53 | mkdir -p /EXAMPLE/CATKIN/WORK_SPACE 54 | cd /EXAMPLE/CATKIN/WORK_SPACE 55 | 56 | mkdir src 57 | catkin init 58 | catkin config --extend /opt/ros/melodic 59 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 60 | catkin config --merge-devel 61 | ``` 62 | 63 | Clone the code: 64 | ``` 65 | cd src 66 | git clone git@github.com:hyhuang1995/gmmloc.git 67 | ``` 68 | 69 | If using SSH keys for github, prepare the dependencies via: 70 | ``` 71 | wstool init . ./gmmloc/gmmloc_ssh.rosinstall 72 | wstool update 73 | ``` 74 | 75 | or using https instead: 76 | ``` 77 | wstool init . ./gmmloc/gmmloc_https.rosinstall 78 | wstool update 79 | ``` 80 | 81 | Compile with: 82 | ``` 83 | catkin build gmmloc_ros 84 | ``` 85 | 86 | ## Running Examples 87 | We provide examples on EuRoC Vicon Room sequences. For example, to run the demo on V1_03_difficult: 88 | 89 | 1. Download the [sequence](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) (ASL Format) 90 | 91 | 2. Replace the [**/PATH/TO/EUROC/DATASET/**](https://github.com/HyHuang1995/gmmloc/blob/770eadc99229eff17f2f613e969e4e9c10499496/gmmloc_ros/launch/v1.launch#L25) in [v1.launch](https://github.com/HyHuang1995/gmmloc/blob/master/gmmloc_ros/launch/v1.launch) with where the sequence is decompressed: 92 | ``` 93 | 94 | ``` 95 | 96 | 3. Launch: 97 | ``` 98 | roslaunch v1.launch seq:=V1_03_difficult 99 | ``` 100 | 101 | ## Evaluation 102 | If evo is installed, we provide script for evaluating on Vicon Room sequences. 103 | ``` 104 | roscd gmmloc_ros 105 | ./scripts/evaluate_euroc.sh 106 | ``` 107 | and the results would be saved to **gmmloc_ros/expr**. 108 | By default, we follow the evaluation protocol of [DSO](https://vision.in.tum.de/research/vslam/dso) to perform evaluation without multi-threading. If you would like to run the script in online mode, uncomment [this line](https://github.com/HyHuang1995/gmmloc/blob/770eadc99229eff17f2f613e969e4e9c10499496/gmmloc_ros/scripts/evaluate_euroc.sh#L60) in the script: 109 | ``` 110 | rosparam set /gmmloc/online True 111 | ``` 112 | 113 | ## Credits 114 | 115 | Our implementation is built on top of [ORB-SLAM2](https://github.com/raulmur/ORB_SLAM2), we thank Raul et al. for their great work. 116 | -------------------------------------------------------------------------------- /gmmloc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | project(gmmloc) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | add_definitions("${CMAKE_CXX_FLAGS} -Wall -msse3 -std=c++17 -O3 -march=native -Wno-deprecated-declarations") 8 | 9 | set(PROTO_DEFNS proto/gmmloc/GMM.proto) 10 | set(PROTO_LIBRARIES "") 11 | find_package(protobuf_catkin QUIET) 12 | if (protobuf_catkin_FOUND) 13 | message(STATUS "Using protobuf_catkin") 14 | PROTOBUF_CATKIN_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${PROTO_DEFNS}) 15 | set(PROTO_LIBRARIES ${protobuf_catkin_LIBRARIES}) 16 | else() 17 | message(STATUS "Using system protobuf") 18 | find_package(Protobuf REQUIRED) 19 | PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${PROTO_DEFNS}) 20 | set(PROTO_LIBRARIES ${PROTOBUF_LIBRARIES}) 21 | endif() 22 | 23 | find_package(OpenCV 3.0 REQUIRED) 24 | # find_package(fmt) 25 | # find_package(PCL REQUIRED COMPONENTS io) 26 | 27 | include_directories( 28 | ${PROJECT_SOURCE_DIR}/include 29 | ) 30 | 31 | cs_add_library(${PROJECT_NAME}_proto 32 | ${PROTO_SRCS} 33 | ) 34 | set_target_properties(${PROJECT_NAME}_proto PROPERTIES PUBLIC_HEADER "${PROTO_HDRS}") 35 | target_include_directories(${PROJECT_NAME}_proto 36 | PUBLIC ${CMAKE_CURRENT_BINARY_DIR}) 37 | target_link_libraries(${PROJECT_NAME}_proto ${PROTO_LIBRARIES}) 38 | 39 | cs_add_library(${PROJECT_NAME} SHARED 40 | 41 | src/gmmloc.cpp 42 | src/gmmloc_opt.cpp 43 | 44 | src/global.cpp 45 | src/config.cpp 46 | 47 | src/modules/tracking.cpp 48 | src/modules/tracking_opt.cpp 49 | src/modules/localization.cpp 50 | src/modules/localization_opt.cpp 51 | 52 | src/gmm/factors.cpp 53 | src/gmm/gaussian.cpp 54 | src/gmm/gaussian_mixture.cpp 55 | src/gmm/gmm_utils.cpp 56 | 57 | src/cv/orb_extractor.cpp 58 | src/cv/orb_vocabulary.cpp 59 | src/cv/orb_matcher.cpp 60 | src/cv/pinhole_camera.cpp 61 | 62 | src/types/frame.cpp 63 | src/types/keyframe.cpp 64 | src/types/map.cpp 65 | src/types/mappoint.cpp 66 | 67 | src/utils/dataloader.cpp 68 | src/utils/protobuf_utils.cpp 69 | src/utils/timing.cpp 70 | src/utils/math_utils.cpp 71 | src/utils/cv_utils.cpp 72 | 73 | src/visualization/campose_visualizer.cpp 74 | src/visualization/gmm_visualizer.cpp 75 | src/visualization/visualizer.cpp 76 | ) 77 | 78 | target_link_libraries(${PROJECT_NAME} 79 | ${OpenCV_LIBS} 80 | ${PROJECT_NAME}_proto 81 | ) 82 | 83 | cs_add_executable(gmmloc_node node/gmmloc_node.cpp) 84 | target_link_libraries(gmmloc_node ${PROJECT_NAME}) 85 | 86 | install(TARGETS ${PROJECT_NAME}_proto 87 | LIBRARY DESTINATION "lib" 88 | PUBLIC_HEADER DESTINATION "include" 89 | ) 90 | cs_install() 91 | cs_export( 92 | DEPENDS OpenCV 93 | ) 94 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/common/common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include "eigen_stl_types.h" 28 | #include "eigen_types.h" 29 | 30 | #include 31 | 32 | namespace gmmloc { 33 | 34 | using g2o::SE3Quat; 35 | 36 | using SE3QuatPtr = std::shared_ptr; 37 | 38 | using SE3QuatConstPtr = std::shared_ptr; 39 | } -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/common/eigen_stl_types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /// @file 4 | /// This file contains definitions for using Eigen with the STL. 5 | /// See http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html. 6 | /// @see eigen_types.h 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | namespace gmmloc { 18 | 19 | /// A std::map that uses Eigen::aligned_allocator so that the 20 | /// contained types may be fixed-size Eigen values. 21 | template 22 | using eigen_aligned_std_map = 23 | std::map, 24 | Eigen::aligned_allocator>>; 25 | 26 | /// A std::unordered_map that uses Eigen::aligned_allocator so that the 27 | /// contained types may be fixed-size Eigen values. 28 | template 29 | using eigen_aligned_std_unordered_map = 30 | std::unordered_map, std::equal_to, 31 | Eigen::aligned_allocator>>; 32 | 33 | /// A std::vector that uses Eigen::aligned_allocator so that the contained 34 | /// types may be fixed-size Eigen values. 35 | template 36 | using eigen_aligned_std_vector = std::vector>; 37 | 38 | } // namespace gmmloc 39 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/common/eigen_types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace gmmloc { 6 | using scalar_t = double; 7 | 8 | /// The empty column vector (zero rows, one column), templated on scalar type. 9 | template using Vector0 = Eigen::Matrix; 10 | 11 | /// A column vector of size 1 (that is, a scalar), templated on scalar type. 12 | template using Vector1 = Eigen::Matrix; 13 | using Vector1d = Eigen::Matrix; 14 | using Vector1f = Eigen::Matrix; 15 | using Vector1i = Eigen::Matrix; 16 | using Vector1s = Eigen::Matrix; 17 | 18 | /// A column vector of size 2, templated on scalar type. 19 | template using Vector2 = Eigen::Matrix; 20 | // using Vector2d = Eigen::Vector 21 | using Eigen::Vector2d; 22 | using Eigen::Vector2f; 23 | using Eigen::Vector2i; 24 | using Vector2s = Eigen::Matrix; 25 | 26 | /// A column vector of size 3, templated on scalar type. 27 | template using Vector3 = Eigen::Matrix; 28 | using Eigen::Vector3d; 29 | using Eigen::Vector3f; 30 | using Eigen::Vector3i; 31 | using Vector3s = Eigen::Matrix; 32 | 33 | /// A column vector of size 4, templated on scalar type. 34 | template using Vector4 = Eigen::Matrix; 35 | using Eigen::Vector4d; 36 | using Eigen::Vector4f; 37 | using Eigen::Vector4i; 38 | using Vector4s = Eigen::Matrix; 39 | 40 | /// A column vector of size 6. 41 | template using Vector5 = Eigen::Matrix; 42 | using Vector5d = Eigen::Matrix; 43 | using Vector5f = Eigen::Matrix; 44 | using Vector5i = Eigen::Matrix; 45 | using Vector5s = Eigen::Matrix; 46 | 47 | /// A column vector of size 6. 48 | template using Vector6 = Eigen::Matrix; 49 | using Vector6d = Eigen::Matrix; 50 | using Vector6f = Eigen::Matrix; 51 | using Vector6i = Eigen::Matrix; 52 | using Vector6s = Eigen::Matrix; 53 | 54 | /// A column vector templated on the number of rows. 55 | template 56 | using Vector = Eigen::Matrix; 57 | 58 | /// A column vector of any size, templated on scalar type. 59 | template 60 | using VectorX = Eigen::Matrix; 61 | 62 | /// A vector of dynamic size templated on scalar type, up to a maximum of 6 63 | /// elements. 64 | template 65 | using VectorUpTo6 = Eigen::Matrix; 66 | 67 | /// A row vector of size 2, templated on scalar type. 68 | template using RowVector2 = Eigen::Matrix; 69 | using Eigen::RowVector2d; 70 | using Eigen::RowVector2f; 71 | using Eigen::RowVector2i; 72 | using RowVector2s = Eigen::Matrix; 73 | 74 | /// A row vector of size 3, templated on scalar type. 75 | template using RowVector3 = Eigen::Matrix; 76 | using Eigen::RowVector3d; 77 | using Eigen::RowVector3f; 78 | using Eigen::RowVector3i; 79 | using RowVector3s = Eigen::Matrix; 80 | 81 | /// A row vector of size 4, templated on scalar type. 82 | template using RowVector4 = Eigen::Matrix; 83 | using Eigen::RowVector4d; 84 | using Eigen::RowVector4f; 85 | using Eigen::RowVector4i; 86 | using RowVector4s = Eigen::Matrix; 87 | 88 | /// A row vector of size 6. 89 | template using RowVector6 = Eigen::Matrix; 90 | using RowVector6d = Eigen::Matrix; 91 | using RowVector6f = Eigen::Matrix; 92 | using RowVector6i = Eigen::Matrix; 93 | using RowVector6s = Eigen::Matrix; 94 | 95 | /// A row vector templated on the number of columns. 96 | template 97 | using RowVector = Eigen::Matrix; 98 | 99 | /// A row vector of any size, templated on scalar type. 100 | template 101 | using RowVectorX = Eigen::Matrix; 102 | 103 | /// A matrix of 2 rows and 2 columns, templated on scalar type. 104 | template using Matrix2 = Eigen::Matrix; 105 | using Eigen::Matrix2d; 106 | using Eigen::Matrix2f; 107 | using Eigen::Matrix2i; 108 | using Matrix2s = Eigen::Matrix; 109 | 110 | /// A matrix of 3 rows and 3 columns, templated on scalar type. 111 | template using Matrix3 = Eigen::Matrix; 112 | using Eigen::Matrix3d; 113 | using Eigen::Matrix3f; 114 | using Eigen::Matrix3i; 115 | using Matrix3s = Eigen::Matrix; 116 | 117 | /// A matrix of 4 rows and 4 columns, templated on scalar type. 118 | template using Matrix4 = Eigen::Matrix; 119 | using Eigen::Matrix4d; 120 | using Eigen::Matrix4f; 121 | using Eigen::Matrix4i; 122 | using Matrix4s = Eigen::Matrix; 123 | 124 | /// A matrix of 6 rows and 6 columns, templated on scalar type. 125 | template using Matrix6 = Eigen::Matrix; 126 | using Matrix6d = Eigen::Matrix; 127 | using Matrix6f = Eigen::Matrix; 128 | using Matrix6i = Eigen::Matrix; 129 | using Matrix6s = Eigen::Matrix; 130 | 131 | /// A matrix of 2 rows, dynamic columns, templated on scalar type. 132 | template 133 | using Matrix2X = Eigen::Matrix; 134 | 135 | /// A matrix of 3 rows, dynamic columns, templated on scalar type. 136 | template 137 | using Matrix3X = Eigen::Matrix; 138 | 139 | /// A matrix of 4 rows, dynamic columns, templated on scalar type. 140 | template 141 | using Matrix4X = Eigen::Matrix; 142 | 143 | /// A matrix of 6 rows, dynamic columns, templated on scalar type. 144 | template 145 | using Matrix6X = Eigen::Matrix; 146 | 147 | /// A matrix of dynamic size, templated on scalar type. 148 | template 149 | using MatrixX = Eigen::Matrix; 150 | 151 | /// A matrix of dynamic size templated on scalar type, up to a maximum of 6 rows 152 | /// and 6 columns. Rectangular matrices, with different number of rows and 153 | /// columns, are allowed. 154 | template 155 | using MatrixUpTo6 = 156 | Eigen::Matrix; 157 | 158 | /// A quaternion templated on scalar type. 159 | template using Quaternion = Eigen::Quaternion; 160 | using Eigen::Quaterniond; 161 | using Eigen::Quaternionf; 162 | using Quaternions = Eigen::Quaternion; 163 | 164 | /// An AngleAxis templated on scalar type. 165 | template using AngleAxis = Eigen::AngleAxis; 166 | using Eigen::AngleAxisd; 167 | using Eigen::AngleAxisf; 168 | using AngleAxiss = Eigen::AngleAxis; 169 | 170 | /// An Isometry templated on scalar type. 171 | template 172 | using Isometry3 = Eigen::Transform; 173 | using Eigen::Isometry3d; 174 | using Eigen::Isometry3f; 175 | using Isometry3s = Eigen::Transform; 176 | 177 | /// A translation in 3D templated on scalar type. 178 | template using Translation3 = Eigen::Translation; 179 | 180 | } // namespace gmmloc 181 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/config.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace gmmloc { 8 | 9 | namespace common { 10 | 11 | extern std::string voc_path; 12 | 13 | extern std::string data_path; 14 | 15 | extern std::string output_path; 16 | 17 | extern std::string gt_path; 18 | 19 | extern std::string rect_config; 20 | 21 | extern std::string gmm_path; 22 | 23 | extern bool online; 24 | 25 | extern bool verbose; 26 | 27 | extern bool viewer; 28 | 29 | } // namespace common 30 | 31 | namespace gmmmap { 32 | 33 | extern double neighbor_dist_thresh; 34 | 35 | } // namespace gmmmap 36 | 37 | namespace camera { 38 | extern float fx, fy, cx, cy; 39 | 40 | extern float fx_inv, fy_inv; 41 | 42 | extern float k1, k2, p1, p2, k3; 43 | 44 | extern int width, height; 45 | 46 | extern float fps; 47 | 48 | extern float bf; 49 | 50 | extern bool do_rectify; 51 | 52 | extern bool do_equalization; 53 | } // namespace camera 54 | 55 | namespace frame { 56 | 57 | const int grid_cols = 64, grid_rows = 48; 58 | 59 | extern float num_grid_col_inv, num_grid_row_inv; 60 | 61 | extern const int num_levels; 62 | extern const float scale_factor, scale_factor_log; 63 | 64 | extern std::vector scale_factors; 65 | extern std::vector scale_factors_inv; 66 | extern std::vector sigma2; 67 | extern std::vector sigma2_inv; 68 | 69 | extern int num_features; 70 | 71 | extern float th_depth; 72 | 73 | } // namespace frame 74 | 75 | namespace loc { 76 | 77 | extern bool tri_check_deg; 78 | 79 | extern bool tri_use_stereo; 80 | 81 | extern float tri_lambda2; 82 | 83 | extern bool tri_check_str_chi2; 84 | 85 | extern float tri_str_thresh; 86 | 87 | extern bool ba_verbose; 88 | 89 | extern float ba_lambda2; 90 | 91 | extern bool ba_first_as_prior; 92 | 93 | extern double ba_prior_sigma_trans; 94 | 95 | extern double ba_prior_sigma_rot; 96 | 97 | } // namespace loc 98 | 99 | namespace viewer { 100 | 101 | extern int traj_length; 102 | 103 | } // namespace viewer 104 | 105 | } // namespace gmmloc 106 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/cv/orb_extractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University 5 | * of Zaragoza) For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | namespace gmmloc { 30 | 31 | class ExtractorNode { 32 | public: 33 | ExtractorNode() : bNoMore(false) {} 34 | 35 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, 36 | ExtractorNode &n4); 37 | 38 | std::vector vKeys; 39 | cv::Point2i UL, UR, BL, BR; 40 | std::list::iterator lit; 41 | bool bNoMore; 42 | }; 43 | 44 | class ORBextractor { 45 | public: 46 | using Ptr = std::unique_ptr; 47 | 48 | using ConstPtr = std::unique_ptr; 49 | 50 | enum { HARRIS_SCORE = 0, FAST_SCORE = 1 }; 51 | 52 | // ORBextractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST, 53 | // int minThFAST); 54 | 55 | explicit ORBextractor(int nfeatures); 56 | 57 | void init(); 58 | 59 | ORBextractor() = delete; 60 | 61 | ~ORBextractor() = default; 62 | 63 | // Compute the ORB features and descriptors on an image. 64 | // ORB are dispersed on the image using an octree. 65 | // Mask is ignored in the current implementation. 66 | void detect(const cv::Mat &image, const cv::Mat &mask, 67 | std::vector &keypoints, cv::Mat &descriptors); 68 | 69 | int inline GetLevels() { return nlevels; } 70 | 71 | float inline GetScaleFactor() { return scaleFactor; } 72 | 73 | std::vector inline GetScaleFactors() { return mvScaleFactor; } 74 | 75 | std::vector inline GetInverseScaleFactors() { 76 | return mvInvScaleFactor; 77 | } 78 | 79 | std::vector inline GetScaleSigmaSquares() { return mvLevelSigma2; } 80 | 81 | std::vector inline GetInverseScaleSigmaSquares() { 82 | return mvInvLevelSigma2; 83 | } 84 | 85 | std::vector mvImagePyramid; 86 | 87 | protected: 88 | void ComputePyramid(cv::Mat image); 89 | void 90 | ComputeKeyPointsOctTree(std::vector> &allKeypoints); 91 | std::vector 92 | DistributeOctTree(const std::vector &vToDistributeKeys, 93 | const int &minX, const int &maxX, const int &minY, 94 | const int &maxY, const int &nFeatures, const int &level); 95 | 96 | void 97 | ComputeKeyPointsOld(std::vector> &allKeypoints); 98 | std::vector pattern; 99 | 100 | int nfeatures; 101 | double scaleFactor; 102 | int nlevels; 103 | int iniThFAST; 104 | int minThFAST; 105 | 106 | std::vector mnFeaturesPerLevel; 107 | 108 | std::vector umax; 109 | 110 | std::vector mvScaleFactor; 111 | std::vector mvInvScaleFactor; 112 | std::vector mvLevelSigma2; 113 | std::vector mvInvLevelSigma2; 114 | }; 115 | 116 | } // namespace gmmloc 117 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/cv/orb_matcher.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "../types/frame.h" 8 | #include "../types/keyframe.h" 9 | #include "../types/mappoint.h" 10 | 11 | namespace gmmloc { 12 | 13 | class ORBmatcher { 14 | public: 15 | ORBmatcher(float nnratio = 0.6, bool check_ori = true); 16 | 17 | // Computes the Hamming distance between two ORB descriptors 18 | static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b); 19 | 20 | // Search matches between Frame keypoints and projected MapPoints. Returns 21 | // number of matches Used to track the local map (Tracking) 22 | int searchByProjection(Frame &frame, const std::vector &mappts, 23 | const std::vector &stats, 24 | const float th = 3); 25 | 26 | int searchByProjection(Frame &CurrentFrame, const Frame &LastFrame, 27 | const float th, const bool bMono); 28 | 29 | int searchByBoW(KeyFrame *kf_ptr, Frame &F, 30 | std::vector &vpMapPointMatches); 31 | 32 | int searchForTriangulation( 33 | KeyFrame *kf1_ptr, KeyFrame *kf2_ptr, 34 | std::vector> &vMatchedPairs, 35 | const bool bOnlyStereo); 36 | 37 | public: 38 | static const int TH_LOW; 39 | static const int TH_HIGH; 40 | static const int HISTO_LENGTH; 41 | 42 | protected: 43 | bool checkEpipolarDist(const Feature &kp1, const Feature &kp2, 44 | const Matrix3d &fmat, const KeyFrame *kf_ptr); 45 | 46 | float computeRadiusByViewingCos(const float &viewCos); 47 | 48 | void computeThreeMaxima(std::vector *histo, const int L, int &ind1, 49 | int &ind2, int &ind3); 50 | 51 | float nn_ratio_; 52 | bool check_orientation_; 53 | }; 54 | 55 | } // namespace gmmloc 56 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/cv/orb_vocabulary.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "orb_dbow2/dbow2/FORB.h" 6 | #include "orb_dbow2/dbow2/TemplatedVocabulary.h" 7 | 8 | #include "../types/frame.h" 9 | #include "../types/keyframe.h" 10 | 11 | namespace gmmloc { 12 | 13 | class ORBVocabulary { 14 | private: 15 | using voc_type_t = 16 | DBoW2::TemplatedVocabulary; 17 | 18 | static std::unique_ptr voc_; 19 | 20 | public: 21 | using Ptr = std::unique_ptr; 22 | 23 | ORBVocabulary() = delete; 24 | 25 | ORBVocabulary(const ORBVocabulary &voc) = delete; 26 | 27 | static bool initialize(const std::string &voc_file); 28 | 29 | static void transform(Frame *frame); 30 | 31 | static void transform(KeyFrame *frame); 32 | }; 33 | 34 | } // namespace gmmloc 35 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/global.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include 5 | #include 6 | 7 | namespace gmmloc { 8 | 9 | namespace global { 10 | 11 | extern std::atomic_bool pause; 12 | extern std::atomic_bool step; 13 | extern std::atomic_bool stop; 14 | } // namespace global 15 | 16 | } // namespace gmmloc -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/gmm/factors.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "g2o/core/base_binary_edge.h" 4 | #include "g2o/core/base_unary_edge.h" 5 | #include "g2o/core/base_vertex.h" 6 | #include "g2o/types/sba/types_sba.h" 7 | #include "g2o/types/sba/types_six_dof_expmap.h" 8 | #include "g2o/types/slam3d/se3_ops.h" 9 | 10 | #include 11 | 12 | #include "gaussian.h" 13 | 14 | namespace g2o { 15 | 16 | // Projection using focal_length in x and y directions 17 | class EdgeSE3QuatPrior : public BaseUnaryEdge<6, SE3Quat, VertexSE3Expmap> { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | EdgeSE3QuatPrior() = default; 22 | 23 | virtual void setMeasurement(const SE3Quat &m) { 24 | _measurement = m; 25 | _inverseMeasurement = m.inverse(); 26 | } 27 | 28 | void computeError(); 29 | 30 | virtual void linearizeOplus(); 31 | 32 | bool read(std::istream &is) {} 33 | 34 | bool write(std::ostream &os) const {} 35 | 36 | SE3Quat _inverseMeasurement; 37 | }; 38 | 39 | // Projection using focal_length in x and y directions 40 | class EdgePt2Gaussian : public BaseUnaryEdge<3, Vector3, VertexSBAPointXYZ> { 41 | public: 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 43 | 44 | EdgePt2Gaussian() = default; 45 | 46 | void computeError(); 47 | 48 | virtual void linearizeOplus(); 49 | 50 | bool read(std::istream &is) {} 51 | 52 | bool write(std::ostream &os) const {} 53 | 54 | const gmmloc::GaussianComponent *comp_; 55 | }; 56 | 57 | // Projection using focal_length in x and y directions 58 | class EdgePt2GaussianDeg : public BaseUnaryEdge<1, double, VertexSBAPointXYZ> { 59 | public: 60 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 61 | 62 | EdgePt2GaussianDeg() = default; 63 | 64 | void computeError(); 65 | 66 | virtual void linearizeOplus(); 67 | 68 | bool read(std::istream &is) {} 69 | 70 | bool write(std::ostream &os) const {} 71 | 72 | Vector3 normal_, mean_; 73 | }; 74 | 75 | // Projection using focal_length in x and y directions 76 | class EdgeProjectXYZOnly 77 | : public BaseUnaryEdge<2, Eigen::Vector2d, VertexSBAPointXYZ> { 78 | public: 79 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 80 | 81 | EdgeProjectXYZOnly() = default; 82 | 83 | void computeError(); 84 | 85 | bool isDepthPositive() { 86 | // const VertexSE3Expmap *v1 = 87 | // static_cast(_vertices[1]); 88 | // const VertexSBAPointXYZ *v2 = 89 | // static_cast(_vertices[0]); 90 | const VertexSBAPointXYZ *v2 = 91 | static_cast(_vertices[0]); 92 | 93 | return (rot_c_w_ * v2->estimate() + t_c_w_)(2) > 0.0; 94 | // return (v1->estimate().map(v2->estimate()))(2) > 0.0; 95 | } 96 | 97 | bool read(std::istream &is) {} 98 | 99 | bool write(std::ostream &os) const {} 100 | 101 | virtual void linearizeOplus(); 102 | 103 | inline Vector2 cam_project(const Vector3 &trans_xyz) const; 104 | 105 | Eigen::Quaterniond rot_c_w_; 106 | Eigen::Vector3d t_c_w_; 107 | 108 | double fx, fy, cx, cy; 109 | }; 110 | 111 | // Projection using focal_length in x and y directions 112 | class EdgeProjectXYZOnlyStereo 113 | : public BaseUnaryEdge<3, Eigen::Vector3d, VertexSBAPointXYZ> { 114 | public: 115 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 116 | 117 | EdgeProjectXYZOnlyStereo() = default; 118 | 119 | void computeError(); 120 | 121 | bool isDepthPositive() { 122 | const VertexSBAPointXYZ *v2 = 123 | static_cast(_vertices[0]); 124 | 125 | return (rot_c_w_ * v2->estimate() + t_c_w_)(2) > 0.0; 126 | } 127 | 128 | bool read(std::istream &is) {} 129 | 130 | bool write(std::ostream &os) const {} 131 | 132 | virtual void linearizeOplus(); 133 | 134 | inline Vector3 cam_project(const Vector3 &trans_xyz) const; 135 | 136 | Eigen::Quaterniond rot_c_w_; 137 | Eigen::Vector3d t_c_w_; 138 | 139 | double fx, fy, cx, cy, bf; 140 | }; 141 | 142 | } // namespace g2o 143 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/gmm/gaussian.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "../common/common.h" 8 | 9 | #include "../cv/pinhole_camera.h" 10 | 11 | namespace gmmloc { 12 | constexpr int Dim = 3; 13 | 14 | class GaussianComponent { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | using Ptr = GaussianComponent *; 19 | using ConstPtr = const GaussianComponent *; 20 | 21 | struct NeighbourInfo { 22 | double dist; 23 | double idx; 24 | Ptr ptr; 25 | }; 26 | 27 | using Vec = Eigen::Matrix; 28 | using Mat = Eigen::Matrix; 29 | 30 | GaussianComponent(const Vec &mean, const Mat &cov) : mean_(mean), cov_(cov) { 31 | cov_inv_ = cov_.inverse(); 32 | 33 | det_ = cov_.determinant(); 34 | 35 | det_sqrt_ = sqrt(det_); 36 | det_sqrt_inv_ = 1.0 / det_sqrt_; 37 | 38 | decompose(); 39 | } 40 | 41 | ~GaussianComponent() = default; 42 | 43 | void decompose(); 44 | 45 | double predict(const Vec &feature); 46 | 47 | double predictLog(const Vec &feature); 48 | 49 | double predictFull(const Vec &feature); 50 | 51 | double chi2(const Vec &feature); 52 | 53 | inline double MDist2(const Eigen::Vector3d ¢re) { 54 | Eigen::Vector3d delta = centre - mean_; 55 | return delta.transpose() * cov_inv_ * delta; 56 | } 57 | 58 | const Mat &cov() const { return cov_; } 59 | 60 | const Mat &cov_inv() const { return cov_inv_; } 61 | 62 | const Vec &mean() const { return mean_; } 63 | 64 | const double &det() const { return det_; } 65 | 66 | friend std::ostream &operator<<(std::ostream &os, 67 | const GaussianComponent &hh); 68 | 69 | public: 70 | std::vector nbs_; 71 | 72 | Vec mean_; 73 | 74 | Mat cov_, cov_inv_; 75 | 76 | Matrix3d sqrt_info_; 77 | 78 | Vec scale_; 79 | 80 | Mat axis_; 81 | 82 | Quaterniond rot_; 83 | 84 | double det_; 85 | 86 | double det_sqrt_; 87 | double det_sqrt_inv_; 88 | 89 | bool is_degenerated = false; 90 | bool is_salient = false; 91 | }; 92 | 93 | class GaussianComponent2d { 94 | public: 95 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 96 | 97 | using Ptr = std::shared_ptr; 98 | 99 | using ConstPtr = std::shared_ptr; 100 | 101 | GaussianComponent2d() = delete; 102 | 103 | GaussianComponent2d(const GaussianComponent2d &g) = delete; 104 | 105 | GaussianComponent2d &operator=(const GaussianComponent2d &g) = delete; 106 | 107 | GaussianComponent2d(const Vector2d &mean, const Matrix2d &cov) 108 | : mean_(mean), cov_(cov) { 109 | cov_inv_ = cov_.inverse(); 110 | 111 | det_ = cov_.determinant(); 112 | 113 | det_sqrt_ = sqrt(det_); 114 | det_sqrt_inv_ = 1.0 / det_sqrt_; 115 | 116 | decompose(); 117 | } 118 | 119 | ~GaussianComponent2d() = default; 120 | 121 | void decompose(); 122 | 123 | double MDist2(Eigen::Vector2d centre) { 124 | Eigen::Vector2d delta = centre - mean_; 125 | return delta.transpose() * cov_inv_ * delta; 126 | } 127 | 128 | const Matrix2d &cov() const { return cov_; } 129 | 130 | const Matrix2d &cov_inv() const { return cov_inv_; } 131 | 132 | const Vector2d &mean() const { return mean_; } 133 | 134 | const Vector2d &scale() const { return scale_; } 135 | 136 | const double &theta() const { return theta_; } 137 | 138 | const double &det() const { return det_; } 139 | 140 | public: 141 | uint32_t id_ = 0; 142 | 143 | GaussianComponent *parent_ = nullptr; 144 | 145 | double proj_d_ = 0.0; 146 | 147 | Vector2d mean_; 148 | 149 | Matrix2d cov_, cov_inv_; 150 | 151 | Vector2d scale_; 152 | 153 | Matrix2d axis_; 154 | 155 | // Quaterniond rot_; 156 | double theta_; 157 | 158 | double det_; 159 | 160 | double det_sqrt_; 161 | double det_sqrt_inv_; 162 | }; 163 | 164 | // TODO: wrapper for dynamic memory allocation 165 | using GaussianMixture = std::vector; 166 | 167 | } // namespace gmmloc 168 | 169 | // #include "gmm.hpp" 170 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/gmm/gaussian_mixture.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../types/feature.h" 4 | 5 | #include "gaussian.h" 6 | 7 | #include "../utils/nanoflann.hpp" 8 | 9 | namespace gmmloc { 10 | 11 | using GaussianComponents = std::vector; 12 | 13 | using GaussianComponents2d = std::vector; 14 | 15 | struct FLANNPoints3d { 16 | 17 | struct Point3 { 18 | double x, y, z; 19 | }; 20 | std::vector pts; 21 | inline FLANNPoints3d(const GaussianComponents &comp_) { 22 | Point3 pt; 23 | for (size_t i = 0; i < comp_.size(); i++) { 24 | pt.x = comp_[i]->mean().x(); 25 | pt.y = comp_[i]->mean().y(); 26 | pt.z = comp_[i]->mean().z(); 27 | pts.push_back(pt); 28 | } 29 | } 30 | 31 | inline size_t kdtree_get_point_count() const { return pts.size(); } 32 | 33 | inline double kdtree_distance(const double *p1, const size_t idx_p2, 34 | size_t /*size*/) const { 35 | const double d0 = p1[0] - pts[idx_p2].x; 36 | const double d1 = p1[1] - pts[idx_p2].y; 37 | const double d2 = p1[2] - pts[idx_p2].z; 38 | return d0 * d0 + d1 * d1 + d2 * d2; 39 | } 40 | 41 | inline double kdtree_get_pt(const size_t idx, const size_t dim) const { 42 | if (dim == 0) 43 | return pts[idx].x; 44 | else if (dim == 1) 45 | return pts[idx].y; 46 | else 47 | return pts[idx].z; 48 | } 49 | 50 | template bool kdtree_get_bbox(BBOX & /* bb */) const { 51 | return false; 52 | } 53 | }; 54 | 55 | struct FLANNPoints2d { 56 | 57 | struct Point2 { 58 | double x, y; 59 | }; 60 | std::vector pts; 61 | inline FLANNPoints2d(const GaussianComponents2d &comp_) { 62 | Point2 pt; 63 | for (size_t i = 0; i < comp_.size(); i++) { 64 | pt.x = comp_[i]->mean().x(); 65 | pt.y = comp_[i]->mean().y(); 66 | pts.push_back(pt); 67 | } 68 | } 69 | 70 | inline size_t kdtree_get_point_count() const { return pts.size(); } 71 | 72 | inline double kdtree_distance(const double *p1, const size_t idx_p2, 73 | size_t /*size*/) const { 74 | const double d0 = p1[0] - pts[idx_p2].x; 75 | const double d1 = p1[1] - pts[idx_p2].y; 76 | return d0 * d0 + d1 * d1; 77 | } 78 | 79 | inline double kdtree_get_pt(const size_t idx, const size_t dim) const { 80 | if (dim == 0) 81 | return pts[idx].x; 82 | // else if (dim == 1) 83 | // return pts[idx].y; 84 | else 85 | return pts[idx].y; 86 | } 87 | 88 | template bool kdtree_get_bbox(BBOX & /* bb */) const { 89 | return false; 90 | } 91 | }; 92 | 93 | using KDTreePoints2d = nanoflann::KDTreeSingleIndexAdaptor< 94 | nanoflann::L2_Simple_Adaptor, FLANNPoints2d, 2>; 95 | using KDTreePoints3d = nanoflann::KDTreeSingleIndexAdaptor< 96 | nanoflann::L2_Simple_Adaptor, FLANNPoints3d, 3>; 97 | 98 | class GMM { 99 | 100 | public: 101 | using Ptr = std::shared_ptr; 102 | 103 | using ConstPtr = std::shared_ptr; 104 | 105 | // using Ptr = GMM *; 106 | 107 | // using ConstPtr = const GMM *; 108 | 109 | public: 110 | explicit GMM(const GaussianComponents &components); 111 | 112 | GMM(/* args */) = delete; 113 | 114 | GMM(const GMM &other) = delete; 115 | 116 | void operator=(const GMM &) = delete; 117 | 118 | ~GMM(); 119 | 120 | void renderViewProb(const Quaterniond &rot_c_w, const Vector3d &t_c_w); 121 | 122 | void renderView(const Quaterniond &rot_c_w, const Vector3d &t_c_w); 123 | 124 | void renderViewCovarianceCheck(const Quaterniond &rot_c_w, 125 | const Vector3d &t_c_w); 126 | 127 | void renderView(const Quaterniond &rot_c_w, const Vector3d &t_c_w, 128 | std::vector indices, bool sort_by_depth = false); 129 | 130 | std::unique_ptr genKDTree2d(); 131 | 132 | void searchCorrespondence(const std::vector &kpts, 133 | GaussianComponents2d &comps); 134 | 135 | void searchCorrespondence(const std::vector &kpts, 136 | std::vector &comps, 137 | int num = 5); 138 | 139 | void visualize2d(cv::Mat &viz_img) const; 140 | 141 | public: 142 | // get set functions 143 | void setCamera(PinholeCamera::Ptr cam) { camera_ = cam; } 144 | 145 | const GaussianComponents &getComponents() const { return components_; } 146 | 147 | GaussianComponent::ConstPtr getComponent3d(size_t idx) const { 148 | return components_[idx]; 149 | } 150 | 151 | const GaussianComponents2d &getComponents2d() const { return components2d_; } 152 | 153 | size_t countComponents() { return components_.size(); } 154 | 155 | private: 156 | PinholeCamera::Ptr camera_ = nullptr; 157 | 158 | uint32_t num_; 159 | GaussianComponents components_; 160 | 161 | GaussianComponents2d components2d_; 162 | 163 | KDTreePoints3d *kdtree3d_ = nullptr; 164 | FLANNPoints3d *pts3d_ = nullptr; 165 | 166 | public: 167 | void buildKDTree(); 168 | 169 | void queryPoint(const Eigen::Vector3d &pt, std::vector &res); 170 | }; 171 | 172 | } // namespace gmmloc 173 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/gmm/gmm_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "gaussian.h" 4 | #include "gaussian_mixture.h" 5 | 6 | namespace gmmloc { 7 | 8 | class GMMUtility { 9 | public: 10 | static double KLDivergence(const GaussianComponent &g0, 11 | const GaussianComponent &g1); 12 | 13 | static bool loadGMMModel(const std::string &file_path, GMM::Ptr &model); 14 | 15 | static bool saveGMMModel(const std::string &file_path, GMM::Ptr &model, 16 | bool clear_file = true); 17 | 18 | template static double BHCoefficient(const T &g0, const T &g1); 19 | 20 | static GaussianComponent2d::Ptr 21 | projectGaussian(const GaussianComponent &comp, PinholeCamera::Ptr cam, 22 | const Eigen::Quaterniond &rot_c_w, 23 | const Eigen::Vector3d &t_c_w); 24 | 25 | // GaussianComponent2d::Ptr KLDivergence(const GaussianComponent &g0, 26 | // const GaussianComponent &g1); 27 | }; 28 | 29 | // TODO: dimensionality? 30 | template 31 | double GMMUtility::BHCoefficient(const T &g0, const T &g1) { 32 | 33 | auto &&cov0 = g0.cov(); 34 | auto &&cov1 = g1.cov(); 35 | // auto &&cov1_inv = g1.cov_inv(); 36 | 37 | auto cov = (cov0 + cov1) / 2.0; 38 | 39 | auto &&mu0 = g0.mean(); 40 | auto &&mu1 = g1.mean(); 41 | decltype(mu0) delta = mu1 - mu0; 42 | 43 | double d0 = delta.transpose() * cov.inverse() * delta; 44 | d0 /= 8.0; 45 | double d1 = log(cov.determinant() / sqrt(g0.det() * g1.det())) / 2.0; 46 | 47 | // auto d2 = log(cov1.determinant() / cov0.determinant()); 48 | 49 | double dist = d0 + d1; 50 | 51 | return dist; 52 | } 53 | 54 | } // namespace gmmloc 55 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/gmmloc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "modules/localization.h" 9 | #include "modules/tracking.h" 10 | 11 | #include "visualization/visualizer.h" 12 | 13 | #include "gmm/gaussian_mixture.h" 14 | 15 | #include "utils/dataloader.h" 16 | #include "utils/cv_utils.h" 17 | 18 | #include "cv/orb_extractor.h" 19 | 20 | namespace gmmloc { 21 | 22 | class GMMLoc { 23 | public: 24 | explicit GMMLoc(ros::NodeHandle &nh); 25 | 26 | ~GMMLoc(); 27 | 28 | void spin(); 29 | 30 | Frame *processFrame(DataFrame::Ptr data); 31 | 32 | bool needNewKeyFrame(const TrackStat &stat); 33 | 34 | KeyFrame *processNewKeyFrame(Frame *frame); 35 | 36 | KeyFrame *processKeyFrame(Frame *frame, bool is_first = false); 37 | 38 | void createMapPointsFromStereo(Frame *frame, KeyFrame *kf_ptr, 39 | bool check_depth = true); 40 | 41 | void associateMapElements(KeyFrame *kf); 42 | 43 | static StrOptStat optimizePoint(const Eigen::Vector3d &pt3d, 44 | const Feature &kp, const SE3Quat &Tcw, 45 | GaussianComponent::ConstPtr comp, 46 | const double proj_z); 47 | 48 | GaussianComponent::ConstPtr checkMapAssociation(Vector3d &pt3d, KeyFrame *kf, 49 | size_t idx); 50 | 51 | void initialize(); 52 | 53 | void stop(); 54 | 55 | ros::NodeHandle nh_; 56 | 57 | bool initialized_ = false; 58 | 59 | cv::Mat im_left_, im_right_; 60 | 61 | Frame *curr_frame_ = nullptr, *last_frame_ = nullptr; 62 | KeyFrame *curr_keyframe_; 63 | 64 | Map *map_ = nullptr; 65 | 66 | Rectify *recter_ = nullptr; 67 | 68 | // TOOD: smart pointer? 69 | ORBextractor *extractor_left_ = nullptr, *extractor_right_ = nullptr; 70 | Tracking *tracker_ = nullptr; 71 | 72 | Localization *localizer_ = nullptr; 73 | 74 | ViewerGMMLoc *viewer_ = nullptr; 75 | 76 | PinholeCamera *camera_; 77 | 78 | // model 79 | GMM::Ptr gmm_model_ = nullptr; 80 | 81 | // loader 82 | Dataloader *loader_; 83 | 84 | std::unique_ptr thread_loc_ = nullptr; 85 | std::unique_ptr thread_viewer_ = nullptr; 86 | 87 | atomic_bool pause_; 88 | 89 | eigen_aligned_std_vector rot_gt_; 90 | eigen_aligned_std_vector trans_gt_; 91 | }; 92 | 93 | } // namespace gmmloc 94 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/init_config.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "config.h" 8 | 9 | namespace gmmloc { 10 | 11 | void initParameters(ros::NodeHandle &nh) { 12 | 13 | #define GPARAM(x, y) \ 14 | do { \ 15 | if (!nh.getParam(x, y)) { \ 16 | LOG(WARNING) << "retrive pararm " #x " error!"; \ 17 | } \ 18 | } while (0) 19 | 20 | GPARAM("gt_path", common::gt_path); 21 | GPARAM("vocabulary_path", common::voc_path); 22 | GPARAM("data_path", common::data_path); 23 | GPARAM("output_path", common::output_path); 24 | 25 | GPARAM("rect_config", common::rect_config); 26 | 27 | GPARAM("gmm_path", common::gmm_path); 28 | GPARAM("online", common::online); 29 | GPARAM("verbose", common::verbose); 30 | GPARAM("viewer", common::viewer); 31 | 32 | GPARAM("map/neighbor_dist_thresh", gmmmap::neighbor_dist_thresh); 33 | 34 | GPARAM("camera/bf", camera::bf); 35 | GPARAM("camera/fx", camera::fx); 36 | GPARAM("camera/fy", camera::fy); 37 | GPARAM("camera/cx", camera::cx); 38 | GPARAM("camera/cy", camera::cy); 39 | GPARAM("camera/fps", camera::fps); 40 | GPARAM("camera/width", camera::width); 41 | GPARAM("camera/height", camera::height); 42 | GPARAM("camera/do_rectify", camera::do_rectify); 43 | GPARAM("camera/do_equalization", camera::do_equalization); 44 | 45 | // processing 46 | { 47 | camera::fx_inv = 1.0f / camera::fx; 48 | camera::fy_inv = 1.0f / camera::fy; 49 | 50 | frame::num_grid_col_inv = 51 | static_cast(frame::grid_cols) / camera::width; 52 | 53 | frame::num_grid_row_inv = 54 | static_cast(frame::grid_rows) / camera::height; 55 | } 56 | 57 | // frame 58 | GPARAM("frame/th_depth", frame::th_depth); 59 | GPARAM("frame/num_features", frame::num_features); 60 | { 61 | frame::th_depth = camera::bf * frame::th_depth / camera::fx; 62 | 63 | frame::scale_factors.resize(frame::num_levels); 64 | frame::scale_factors_inv.resize(frame::num_levels); 65 | frame::sigma2.resize(frame::num_levels); 66 | frame::sigma2_inv.resize(frame::num_levels); 67 | frame::scale_factors[0] = 1.0f; 68 | frame::sigma2[0] = 1.0f; 69 | frame::scale_factors_inv[0] = 1.0f; 70 | frame::sigma2_inv[0] = 1.0f; 71 | for (int i = 1; i < frame::num_levels; i++) { 72 | frame::scale_factors[i] = 73 | frame::scale_factors[i - 1] * frame::scale_factor; 74 | frame::sigma2[i] = frame::scale_factors[i] * frame::scale_factors[i]; 75 | 76 | frame::scale_factors_inv[i] = 1.0f / frame::scale_factors[i]; 77 | frame::sigma2_inv[i] = 1.0f / frame::sigma2[i]; 78 | } 79 | } 80 | 81 | GPARAM("loc/tri_check_deg", loc::tri_check_deg); 82 | GPARAM("loc/tri_use_stereo", loc::tri_use_stereo); 83 | GPARAM("loc/tri_lambda2", loc::tri_lambda2); 84 | GPARAM("loc/tri_check_str_chi2", loc::tri_check_str_chi2); 85 | GPARAM("loc/tri_str_thresh", loc::tri_str_thresh); 86 | 87 | GPARAM("loc/ba_lambda2", loc::ba_lambda2); 88 | GPARAM("loc/ba_verbose", loc::ba_verbose); 89 | 90 | GPARAM("loc/ba_first_as_prior", loc::ba_first_as_prior); 91 | 92 | #undef GPARAM 93 | } 94 | 95 | void printParameters() {} 96 | 97 | } // namespace gmmloc 98 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/modules/localization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "../types/keyframe.h" 7 | #include "../types/map.h" 8 | #include "../types/mappoint.h" 9 | 10 | #include "../gmm/gaussian_mixture.h" 11 | 12 | namespace gmmloc { 13 | 14 | class Localization { 15 | public: 16 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | Localization(); 19 | 20 | void spinOnce(); 21 | 22 | void spin(); 23 | 24 | void insertKeyFrame(KeyFrame *kf_ptr); 25 | 26 | void interruptBA(); 27 | 28 | void setModel(const GMM::Ptr &gmm) { gmm_model_ = gmm; } 29 | 30 | void setMap(Map *map) { map_ = map; } 31 | 32 | int countKFsInQueue(); 33 | 34 | public: 35 | void stop() { shutdown_ = true; } 36 | 37 | std::atomic_bool shutdown_; 38 | std::atomic_bool is_finished_; 39 | std::atomic_bool is_idle_; 40 | 41 | protected: 42 | bool checkNewKeyFrames(); 43 | 44 | void processNewKeyFrame(); 45 | 46 | void removeMapPoints(); 47 | 48 | void searchInNeighbors(); 49 | 50 | int fuseObservations(KeyFrame *kf, const std::vector &mappts, 51 | const float th = 3.0f); 52 | 53 | void removeKeyFrames(); 54 | 55 | protected: 56 | void createMapPoints(); 57 | 58 | GaussianComponent *optimizeTriangulationVec(Vector3d &x3d, KeyFrame *kf_ptr, 59 | size_t idx1, size_t idx2); 60 | 61 | void jointOptimization(KeyFrame *kf_ptr, bool *pbStopFlag, Map *pMap); 62 | 63 | protected: 64 | std::list keyframe_queue_; 65 | 66 | KeyFrame *curr_kf_; 67 | 68 | std::list candidate_mappts_; 69 | 70 | std::mutex mutex_kf_insertion_; 71 | 72 | bool flag_abort_ba_ = false; 73 | 74 | protected: 75 | GMM::Ptr gmm_model_; 76 | 77 | Map *map_ = nullptr; 78 | }; 79 | 80 | } // namespace gmmloc 81 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/modules/tracking.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "../cv/orb_extractor.h" 7 | #include "../cv/orb_vocabulary.h" 8 | 9 | #include "../types/frame.h" 10 | #include "../utils/dataloader.h" 11 | 12 | #include "localization.h" 13 | 14 | namespace gmmloc { 15 | 16 | struct TrackStat { 17 | bool res; 18 | int num_match_inliers; 19 | int num_ref_matches; 20 | float ratio_map; 21 | }; 22 | 23 | class Tracking { 24 | 25 | public: 26 | Tracking(); 27 | 28 | ~Tracking() = default; 29 | 30 | void initialize(Frame *init_frame); 31 | 32 | TrackStat track(Frame *frame); 33 | 34 | public: 35 | void updateLastFrame(); 36 | 37 | void createTemporalPoints(); 38 | 39 | void clearTemporalPoints(); 40 | 41 | int trackWithMotionModel(); 42 | 43 | int trackKeyFrame(); 44 | 45 | void updateLocalMap(); 46 | 47 | void searchLocalPoints(); 48 | 49 | int trackLocalMap(); 50 | 51 | int optimizeCurrentPose(); 52 | 53 | public: 54 | TrackStat stat_; 55 | 56 | Frame *curr_frame_ = nullptr, *last_frame_ = nullptr; 57 | 58 | // for temporal tracking 59 | std::list temp_pts_; 60 | 61 | KeyFrame *ref_keyframe_ = nullptr; 62 | 63 | std::vector local_keyframes_; 64 | std::vector local_mappoints_; 65 | 66 | std::vector mappoints_proj_stat; 67 | }; 68 | 69 | } // namespace gmmloc 70 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/types/feature.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../common/common.h" 4 | 5 | namespace gmmloc { 6 | 7 | struct Feature { 8 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | 10 | Feature(const cv::KeyPoint &kp, const cv::Mat &_desc) 11 | : uv(kp.pt.x, kp.pt.y), desc(_desc.clone()), size(kp.size), 12 | responce(kp.response), angle(kp.angle), octave(kp.octave), depth(-1.0f), 13 | u_right(-1.0f) 14 | 15 | {} 16 | 17 | inline double error(const Vector2d &obs) const { 18 | return (uv - obs).squaredNorm(); 19 | } 20 | 21 | inline double error(const Vector3d &obs) const { 22 | if (u_right < 0.0f) { 23 | return error(Vector2d(obs.x(), obs.y())); 24 | } else { 25 | Vector3d uvr(uv.x(), uv.y(), u_right); 26 | return (uvr - obs).squaredNorm(); 27 | } 28 | } 29 | 30 | Vector2d uv; 31 | 32 | cv::Mat desc; 33 | 34 | float size = 0.0f; 35 | float responce = 0.0f; 36 | float angle = -1.0f; 37 | int octave = 0; 38 | 39 | float depth = -1.0f; 40 | float u_right = -1.0f; 41 | }; 42 | 43 | } // namespace gmmloc 44 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/types/frame.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "feature.h" 6 | #include "keyframe.h" 7 | #include "mappoint.h" 8 | 9 | #include "orb_dbow2/dbow2/BowVector.h" 10 | #include "orb_dbow2/dbow2/FeatureVector.h" 11 | 12 | #include "../cv/pinhole_camera.h" 13 | 14 | #include "../config.h" 15 | 16 | #include 17 | 18 | namespace gmmloc { 19 | 20 | class MapPoint; 21 | class KeyFrame; 22 | 23 | class Frame { 24 | public: 25 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 26 | 27 | using Ptr = std::shared_ptr; 28 | 29 | using ConstPtr = std::shared_ptr; 30 | 31 | Frame() = delete; 32 | explicit Frame(int a); 33 | 34 | // Copy constructor. 35 | Frame(const Frame &frame); 36 | 37 | ~Frame() = default; 38 | 39 | void computeStereoMatches(const std::vector &img_pyr_left, 40 | const std::vector &img_pyr_right); 41 | 42 | void computeStereoMatches(std::vector &kps_right, 43 | const cv::Mat &desc_right, 44 | const std::vector &img_pyr_left, 45 | const std::vector &img_pyr_right); 46 | 47 | std::vector getFeaturesInArea(const float &x, const float &y, 48 | const float &r, const int minLevel = -1, 49 | const int maxLevel = -1) const; 50 | 51 | void assignFeaturesToGrid(); 52 | 53 | public: 54 | const SE3Quat getTcw() const { return *T_c_w_; } 55 | 56 | const SE3Quat getTwc() const { return *T_w_c_; } 57 | 58 | void setTcw(const SE3Quat &T_c_w) { 59 | T_c_w_->setRotation(T_c_w.rotation()); 60 | T_c_w_->setTranslation(T_c_w.translation()); 61 | *T_w_c_ = T_c_w_->inverse(); 62 | } 63 | 64 | void setTwc(const SE3Quat &T_w_c) { 65 | T_w_c_->setRotation(T_w_c.rotation()); 66 | T_w_c_->setTranslation(T_w_c.translation()); 67 | *T_c_w_ = T_w_c_->inverse(); 68 | } 69 | 70 | bool project3(const Vector3d &pt, Vector2d *uv); 71 | 72 | bool project3(const Vector3d &pt, Vector3d *uvr); 73 | 74 | bool unproject3(size_t idx, Vector3d *pt3d); 75 | 76 | public: 77 | static long unsigned int next_idx_; ///< Next Frame id. 78 | long unsigned int idx_; ///< Current Frame id. 79 | 80 | bool is_keyframe_ = false; 81 | 82 | // Reference Keyframe. 83 | KeyFrame *ref_keyframe_ = nullptr; 84 | 85 | double timestamp_; 86 | 87 | float mbf, mb; 88 | 89 | size_t num_feats_; 90 | std::vector features_; 91 | 92 | std::vector mappoints_; 93 | std::vector is_outlier_; 94 | 95 | DBoW2::BowVector bow_vec_; 96 | DBoW2::FeatureVector feat_vec_; 97 | 98 | PinholeCamera *camera_ = nullptr; 99 | 100 | std::vector grid_[frame::grid_cols][frame::grid_rows]; 101 | 102 | // protected: 103 | SE3QuatPtr T_c_w_, T_w_c_, T_c_r_; 104 | }; 105 | 106 | } // namespace gmmloc 107 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/types/keyframe.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "orb_dbow2/dbow2/BowVector.h" 7 | #include "orb_dbow2/dbow2/FeatureVector.h" 8 | 9 | #include "frame.h" 10 | #include "mappoint.h" 11 | 12 | #include "../gmm/gaussian_mixture.h" 13 | 14 | namespace gmmloc { 15 | 16 | class MapPoint; 17 | class Frame; 18 | class Map; 19 | 20 | struct ErrorResult { 21 | bool is_projection_valid = true; 22 | 23 | bool is_stereo = false; 24 | 25 | double err = 0.0; 26 | }; 27 | 28 | class KeyFrame { 29 | friend class Map; 30 | 31 | public: 32 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | 34 | KeyFrame(Frame &F); 35 | 36 | void updateConnections(); 37 | 38 | void addConnection(KeyFrame *kf_ptr, const int &weight); 39 | 40 | void removeConnection(KeyFrame *kf_ptr); 41 | 42 | std::vector getVectorCovisibleKeyFrames(); 43 | std::vector getBestCovisibilityKeyFrames(const int &N); 44 | std::vector getCovisiblesByWeight(const int &w); 45 | 46 | void updateBestCovisibles(); 47 | KeyFrame *getBestCovisibilityKeyFrame(); 48 | 49 | void addObservation(MapPoint *mappt, const size_t &idx); 50 | 51 | void removeObservation(const size_t &idx); 52 | 53 | void removeObservation(MapPoint *mappt); 54 | 55 | void replaceObservation(const size_t &idx, MapPoint *mappt); 56 | 57 | std::vector getMapPoints(); 58 | 59 | int countMapPoints(const int &minum_obs_); 60 | 61 | MapPoint *getMapPoint(const size_t &idx); 62 | 63 | bool project3(const Vector3d &pt, Vector2d *uv); 64 | 65 | bool project3(const Vector3d &pt, Vector3d *uvr); 66 | 67 | bool unproject3(size_t idx, Vector3d *pt3d); 68 | 69 | ErrorResult projectionError(const Vector3d &pt, size_t idx); 70 | 71 | std::vector getFeaturesInArea(const float &x, const float &y, 72 | const float &r) const; 73 | 74 | public: 75 | static long unsigned int next_idx_; 76 | long unsigned int idx_; 77 | const long unsigned int frame_idx_; 78 | 79 | const double timestamp_; 80 | 81 | std::atomic_bool not_valid_; 82 | 83 | PinholeCamera *camera_ = nullptr; 84 | 85 | // Variables used by the local mapping 86 | long unsigned int ba_local_kf_ = 0; 87 | long unsigned int fixed_kf_idx_ba = 0; 88 | 89 | const float mbf, mb; 90 | 91 | const size_t num_feats_; 92 | 93 | const std::vector features_; 94 | 95 | DBoW2::BowVector bow_vec_; 96 | DBoW2::FeatureVector feat_vec_; 97 | 98 | public: 99 | const SE3Quat getTcw(); 100 | 101 | const SE3Quat getTwc(); 102 | 103 | void setTcw(const SE3Quat &T_c_w); 104 | 105 | void setTwc(const SE3Quat &T_w_c); 106 | 107 | protected: 108 | SE3QuatPtr T_c_w_, T_w_c_; 109 | 110 | std::vector mappoints_; 111 | 112 | std::vector>> grid_; 113 | 114 | std::unordered_map map_frame_weights_; 115 | std::vector ordered_keyframes_; 116 | std::vector ordered_weights_; 117 | 118 | KeyFrame *best_cov_kf_ = nullptr; 119 | 120 | std::shared_mutex mutex_pose_; 121 | std::mutex mutex_connections_; 122 | std::mutex mutex_attr_; 123 | 124 | public: 125 | std::vector comps_; 126 | }; 127 | 128 | } // namespace gmmloc 129 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/types/map.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "frame.h" 7 | 8 | #include "keyframe.h" 9 | #include "mappoint.h" 10 | 11 | #include "../gmm/gaussian_mixture.h" 12 | 13 | namespace gmmloc { 14 | 15 | class MapPoint; 16 | class KeyFrame; 17 | class Frame; 18 | 19 | // basic frame information for visualization and analysis 20 | struct FrameInfo { 21 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | double timestamp; 24 | 25 | KeyFrame *ref = nullptr; 26 | 27 | SE3QuatPtr Trc; 28 | }; 29 | 30 | struct StrOptStat { 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | bool res = false; 33 | double chi2_proj, chi2_str; 34 | Vector3d pt_est; 35 | }; 36 | 37 | enum StrOptResult { 38 | 39 | }; 40 | 41 | class Map { 42 | public: 43 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 | 45 | Map() = default; 46 | 47 | void addKeyFrame(KeyFrame *kf_ptr); 48 | 49 | void addObservation(MapPoint *mappt); 50 | 51 | void insertMapPoint(MapPoint *mappt); 52 | 53 | void insertKeyFrame(KeyFrame *kf); 54 | 55 | void removeMapPoint(MapPoint *mappt); 56 | 57 | void removeKeyFrame(KeyFrame *kf); 58 | 59 | void updateLastFrame(Frame *last_frame); 60 | 61 | void updateFrameInfo(Frame *curr_frame); 62 | 63 | void replaceMapPoint(MapPoint *mp_src, MapPoint *mp_tgt); 64 | 65 | void setGMMMap(GMM::Ptr model) { gmm_map_ = model; } 66 | 67 | // for visualization 68 | std::vector getAllKeyFrames(); 69 | std::vector getAllMapPoints(); 70 | 71 | long unsigned int countMapPoints(); 72 | long unsigned countKeyFrames(); 73 | 74 | void clear(); 75 | 76 | void summarize(); 77 | 78 | void setTrajectory(const eigen_aligned_std_vector &rot, 79 | const eigen_aligned_std_vector &trans); 80 | 81 | protected: 82 | std::set mappoints_; 83 | std::set keyframes_; 84 | 85 | std::mutex mutex_info_; 86 | 87 | std::vector frame_info_; 88 | std::vector gt_traj_; 89 | 90 | long unsigned int max_kf_idx_ = 0; 91 | 92 | std::mutex mutex_map_; 93 | 94 | GMM::Ptr gmm_map_ = nullptr; 95 | }; 96 | 97 | } // namespace gmmloc 98 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/types/mappoint.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "frame.h" 9 | #include "keyframe.h" 10 | #include "map.h" 11 | 12 | #include "../gmm/gaussian_mixture.h" 13 | #include "../gmm/gaussian.h" 14 | 15 | namespace gmmloc { 16 | 17 | class KeyFrame; 18 | class Map; 19 | class Frame; 20 | 21 | struct ProjStat { 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | Vector3d uvr; 25 | 26 | double dist; 27 | 28 | double view_cos; 29 | 30 | double scale_pred; 31 | }; 32 | 33 | class MapPoint { 34 | friend class Map; 35 | 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | enum MapPointType { 39 | FromDepth = 0, 40 | FromDepthGMM = 1, 41 | 42 | FromTriMono = 2, 43 | FromTriMonoGMM = 3, 44 | 45 | FromTriStereo = 4, 46 | FromTriStereoGMM = 5 47 | }; 48 | 49 | MapPoint(const Vector3d &pos, KeyFrame *ref_kf); 50 | 51 | MapPoint(const Vector3d &pos, Frame *frame, const int &idx); 52 | 53 | Vector3d getPosition(); 54 | 55 | void setPosition(const Vector3d &pt3d); 56 | 57 | Vector3d getNormal(); 58 | 59 | void addObservation(KeyFrame *kf_ptr, size_t idx); 60 | 61 | bool removeObservation(KeyFrame *kf_ptr); 62 | 63 | std::unordered_map getObservations(); 64 | 65 | int countObservations(); 66 | 67 | bool checkObservation(KeyFrame *kf_ptr); 68 | 69 | int getIndexInKeyFrame(KeyFrame *kf_ptr); 70 | 71 | void computeDistinctiveDescriptors(); 72 | 73 | cv::Mat getDescriptor(); 74 | 75 | void updateNormalAndDepth(); 76 | 77 | bool checkScaleAndVisible(const Vector3d &t_w_c, ProjStat &stat); 78 | 79 | void replace(MapPoint *mappt); 80 | 81 | MapPoint *getReplaced(); 82 | 83 | public: 84 | long unsigned int idx_; ///< Global ID for MapPoint 85 | static long unsigned int next_idx_; 86 | const long int ref_idx_ = -1; 87 | const long int frame_idx_; 88 | int num_obs_ = 0; 89 | 90 | bool is_in_view_; 91 | 92 | long unsigned int last_visible_idx_ = 0; 93 | 94 | long unsigned int ba_local_kf_ = 0; 95 | long unsigned int fuse_tgt_kf_ = 0; 96 | 97 | std::atomic_bool not_valid_; 98 | 99 | static std::mutex global_mutex_; 100 | static std::mutex global_mutex_idx; 101 | 102 | std::atomic_int num_visible_; 103 | std::atomic_int num_found_; 104 | 105 | protected: 106 | Vector3d pos_; 107 | Vector3d normal_; 108 | 109 | std::unordered_map observations_; 110 | 111 | cv::Mat desc_; 112 | 113 | KeyFrame *ref_kf_ = nullptr; 114 | 115 | MapPoint *ptr_replaced_ = nullptr; 116 | 117 | float min_dist_ = 0.0f, max_dist_ = 0.0f; 118 | 119 | std::shared_mutex mutex_pos_; 120 | std::shared_mutex mutex_attr_; 121 | 122 | public: 123 | // associated component 124 | std::vector asscociations_; 125 | MapPointType type_; 126 | }; 127 | 128 | } // namespace gmmloc 129 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/utils/cv_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace gmmloc { 6 | 7 | class Rectify { 8 | public: 9 | Rectify(const std::string &config); 10 | 11 | void doRectifyL(const cv::Mat &img_src, cv::Mat &img_rec); 12 | 13 | void doRectifyR(const cv::Mat &img_src, cv::Mat &img_rec); 14 | 15 | cv::Mat M1l, M2l, M1r, M2r; 16 | }; 17 | 18 | } // namespace gmmloc 19 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/utils/dataloader.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include "../common/eigen_stl_types.h" 11 | #include "../common/eigen_types.h" 12 | 13 | namespace gmmloc { 14 | 15 | enum class DataType : std::uint8_t { 16 | Mono = 1, 17 | Stereo = 1 << 1, 18 | Depth = 1 << 2, 19 | IMU = 1 << 3, 20 | Lidar = 1 << 4, 21 | Odom = 1 << 5, 22 | 23 | GT = 1 << 7 24 | 25 | }; 26 | 27 | inline DataType operator|(DataType a, DataType b) { 28 | return static_cast(static_cast(a) | 29 | static_cast(b)); 30 | } 31 | 32 | inline uint8_t operator&(DataType a, DataType b) { 33 | return static_cast(a) & static_cast(b); 34 | } 35 | 36 | struct DataFrame { 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | 39 | uint32_t idx = 0; 40 | 41 | using Ptr = std::shared_ptr; 42 | 43 | using ConstPtr = std::shared_ptr; 44 | 45 | cv::Mat mono, depth; 46 | double timestamp; 47 | 48 | Vector3d t_w_c; 49 | Quaterniond rot_w_c; 50 | }; 51 | 52 | class Dataloader { 53 | public: 54 | using Ptr = std::unique_ptr; 55 | 56 | using ConstPtr = std::unique_ptr; 57 | 58 | Dataloader() = default; 59 | 60 | Dataloader(const std::string &str, DataType cfg); 61 | 62 | virtual ~Dataloader() = default; 63 | 64 | virtual DataFrame::Ptr getNextFrame() = 0; 65 | 66 | virtual DataFrame::Ptr getFrameByIndex(size_t idx) = 0; 67 | 68 | virtual bool getTrajectory(eigen_aligned_std_vector &rot, 69 | eigen_aligned_std_vector &trans); 70 | 71 | size_t getSize() const { return num_; } 72 | 73 | protected: 74 | size_t num_ = 0, idx_ = 0; 75 | DataType cfg_; 76 | 77 | std::string base_path_; 78 | 79 | std::vector mono_file_, depth_file_; 80 | std::vector time_stamp_; 81 | 82 | eigen_aligned_std_vector rot_; 83 | eigen_aligned_std_vector trans_; 84 | }; 85 | 86 | class DataloaderEuRoC : public Dataloader { 87 | public: 88 | using Ptr = std::shared_ptr; 89 | 90 | using ConstPtr = std::shared_ptr; 91 | 92 | DataloaderEuRoC(const std::string &str, const std::string >_path, 93 | DataType cfg); 94 | 95 | ~DataloaderEuRoC() = default; 96 | 97 | DataFrame::Ptr getNextFrame() override; 98 | 99 | DataFrame::Ptr getFrameByIndex(size_t idx) override; 100 | 101 | private: 102 | void loadTrajectory(const std::string &traj_file); 103 | 104 | void loadImages(const std::string &ass_file); 105 | }; 106 | 107 | } // namespace gmmloc -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/utils/math_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../common/common.h" 4 | 5 | namespace gmmloc { 6 | 7 | class MathUtils { 8 | public: 9 | static Matrix3d skew(const Vector3d &vec); 10 | 11 | static Matrix3d computeEssentialMatrix(const SE3Quat &Tcw1, 12 | const SE3Quat &Tcw2 13 | 14 | ); 15 | 16 | static Matrix3d computeFundamentalMatrix(const SE3Quat &Tcw1, 17 | const Matrix3d &K1, 18 | const SE3Quat &Tcw2, 19 | const Matrix3d &K2 20 | 21 | ); 22 | }; 23 | 24 | } // namespace gmmloc 25 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/utils/protobuf_utils.h: -------------------------------------------------------------------------------- 1 | // adapted from voxblox 2 | 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace gmmloc { 13 | 14 | namespace pb { 15 | 16 | bool readProtoMsgCountFromStream(std::fstream *stream_in, uint32_t *message_count, 17 | uint32_t *byte_offset); 18 | 19 | bool writeProtoMsgCountToStream(uint32_t message_count, 20 | std::fstream *stream_out); 21 | 22 | bool readProtoMsgFromStream(std::fstream *stream_in, 23 | google::protobuf::Message *message, 24 | uint32_t *byte_offset); 25 | 26 | bool writeProtoMsgToStream(const google::protobuf::Message &message, 27 | std::fstream *stream_out); 28 | 29 | } // namespace pb 30 | 31 | } // namespace gmmloc 32 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/utils/timing.h: -------------------------------------------------------------------------------- 1 | // adapted from voxblox 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | 17 | namespace gmmloc { 18 | namespace timing { 19 | 20 | template class Accumulator { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | Accumulator() 25 | : window_samples_(0), totalsamples_(0), window_sum_(0), sum_(0), 26 | min_(std::numeric_limits::max()), 27 | max_(std::numeric_limits::min()) {} 28 | 29 | void Add(T sample) { 30 | if (window_samples_ < N) { 31 | samples_[window_samples_++] = sample; 32 | window_sum_ += sample; 33 | } else { 34 | T &oldest = samples_[window_samples_++ % N]; 35 | window_sum_ += sample - oldest; 36 | oldest = sample; 37 | } 38 | sum_ += sample; 39 | ++totalsamples_; 40 | if (sample > max_) { 41 | max_ = sample; 42 | } 43 | if (sample < min_) { 44 | min_ = sample; 45 | } 46 | } 47 | 48 | int TotalSamples() const { return totalsamples_; } 49 | 50 | double Sum() const { return sum_; } 51 | 52 | double Mean() const { return sum_ / totalsamples_; } 53 | 54 | double RollingMean() const { 55 | return window_sum_ / std::min(window_samples_, N); 56 | } 57 | 58 | double Max() const { return max_; } 59 | 60 | double Min() const { return min_; } 61 | 62 | double LazyVariance() const { 63 | if (window_samples_ == 0) { 64 | return 0.0; 65 | } 66 | double var = 0; 67 | double mean = RollingMean(); 68 | for (int i = 0; i < std::min(window_samples_, N); ++i) { 69 | var += (samples_[i] - mean) * (samples_[i] - mean); 70 | } 71 | var /= std::min(window_samples_, N); 72 | return var; 73 | } 74 | 75 | private: 76 | int window_samples_; 77 | int totalsamples_; 78 | Total window_sum_; 79 | Total sum_; 80 | T min_; 81 | T max_; 82 | T samples_[N]; 83 | }; 84 | 85 | struct TimerMapValue { 86 | TimerMapValue() {} 87 | 88 | /// Create an accumulator with specified window size. 89 | Accumulator acc_; 90 | }; 91 | 92 | /** 93 | * A class that has the timer interface but does nothing. Swapping this in in 94 | * place of the Timer class (say with a typedef) should allow one to disable 95 | * timing. Because all of the functions are inline, they should just disappear. 96 | */ 97 | class DummyTimer { 98 | public: 99 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 100 | 101 | explicit DummyTimer(size_t /*handle*/, bool /*constructStopped*/ = false) {} 102 | explicit DummyTimer(std::string const & /*tag*/, 103 | bool /*constructStopped*/ = false) {} 104 | ~DummyTimer() {} 105 | 106 | void Start() {} 107 | void Stop() {} 108 | bool IsTiming() { return false; } 109 | }; 110 | 111 | class Timer { 112 | public: 113 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 114 | 115 | explicit Timer(size_t handle, bool constructStopped = false); 116 | explicit Timer(std::string const &tag, bool constructStopped = false); 117 | ~Timer(); 118 | 119 | void Start(); 120 | void Stop(); 121 | bool IsTiming() const; 122 | 123 | private: 124 | std::chrono::time_point time_; 125 | 126 | bool timing_; 127 | size_t handle_; 128 | }; 129 | 130 | class Timing { 131 | public: 132 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 133 | 134 | typedef std::map map_t; 135 | friend class Timer; 136 | // Definition of static functions to query the timers. 137 | static size_t GetHandle(std::string const &tag); 138 | static std::string GetTag(size_t handle); 139 | static double GetTotalSeconds(size_t handle); 140 | static double GetTotalSeconds(std::string const &tag); 141 | static double GetMeanSeconds(size_t handle); 142 | static double GetMeanSeconds(std::string const &tag); 143 | static size_t GetNumSamples(size_t handle); 144 | static size_t GetNumSamples(std::string const &tag); 145 | static double GetVarianceSeconds(size_t handle); 146 | static double GetVarianceSeconds(std::string const &tag); 147 | static double GetMinSeconds(size_t handle); 148 | static double GetMinSeconds(std::string const &tag); 149 | static double GetMaxSeconds(size_t handle); 150 | static double GetMaxSeconds(std::string const &tag); 151 | static double GetHz(size_t handle); 152 | static double GetHz(std::string const &tag); 153 | static void Print(std::ostream &out); 154 | static std::string Print(); 155 | static std::string SecondsToTimeString(double seconds); 156 | static void Reset(); 157 | static const map_t &GetTimers() { return Instance().tagMap_; } 158 | 159 | private: 160 | void AddTime(size_t handle, double seconds); 161 | 162 | static Timing &Instance(); 163 | 164 | Timing(); 165 | ~Timing(); 166 | 167 | // using list_t = std::vector; 168 | using list_t = 169 | std::vector>; 170 | 171 | list_t timers_; 172 | map_t tagMap_; 173 | size_t maxTagLength_; 174 | std::mutex mutex_; 175 | }; 176 | 177 | #if ENABLE_MSF_TIMING 178 | typedef Timer DebugTimer; 179 | #else 180 | typedef DummyTimer DebugTimer; 181 | #endif 182 | 183 | } // namespace timing 184 | } // namespace gmmloc 185 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/visualization/campose_visualizer.h: -------------------------------------------------------------------------------- 1 | // adapted from vins-mono 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace gmmloc { 13 | class CameraPoseVisualizer { 14 | public: 15 | using Ptr = std::shared_ptr; 16 | 17 | using ConstPtr = std::shared_ptr; 18 | 19 | public: 20 | std::string m_marker_ns; 21 | 22 | CameraPoseVisualizer(float r, float g, float b, float a); 23 | 24 | CameraPoseVisualizer(const CameraPoseVisualizer &cam) = delete; 25 | 26 | void setImageBoundaryColor(float r, float g, float b, float a = 1.0); 27 | void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0); 28 | void setScale(double s); 29 | void setLineWidth(double width); 30 | 31 | void addKFPose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q); 32 | void addPose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q); 33 | void reset(); 34 | 35 | void publish(ros::Publisher &pub, const std_msgs::Header &header); 36 | void addEdge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1); 37 | void addLoopEdge(const Eigen::Vector3d &p0, const Eigen::Vector3d &p1); 38 | 39 | private: 40 | std::vector m_markers; 41 | std_msgs::ColorRGBA m_image_boundary_color; 42 | std_msgs::ColorRGBA m_optical_center_connector_color; 43 | double m_scale; 44 | double m_line_width; 45 | 46 | static const Eigen::Vector3d imlt; 47 | static const Eigen::Vector3d imlb; 48 | static const Eigen::Vector3d imrt; 49 | static const Eigen::Vector3d imrb; 50 | static const Eigen::Vector3d oc; 51 | static const Eigen::Vector3d lt0; 52 | static const Eigen::Vector3d lt1; 53 | static const Eigen::Vector3d lt2; 54 | }; 55 | 56 | } // namespace gmmloc 57 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/visualization/gmm_visualizer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "../gmm/gaussian_mixture.h" 7 | 8 | namespace gmmloc { 9 | 10 | class GMMVisualizer { 11 | 12 | public: 13 | using Ptr = std::unique_ptr; 14 | 15 | using ConstPtr = std::unique_ptr; 16 | 17 | // GMMVisualizer(const GaussianMixture& model, ros::NodeHandle& nh); 18 | explicit GMMVisualizer(GMM::ConstPtr model, 19 | const std::string topic_name = "gmm_model", 20 | const std::string frame_id = "map", 21 | const double cov_factor = 3); 22 | 23 | ~GMMVisualizer() = default; 24 | 25 | void republish(GMM::ConstPtr model); 26 | 27 | void republish(GMM::ConstPtr model, const std::vector& indices); 28 | 29 | public: 30 | std::string frame_id_; 31 | ros::Publisher pub_; 32 | 33 | visualization_msgs::MarkerArrayPtr msg_; 34 | 35 | double cov_factor_; 36 | }; 37 | 38 | } // namespace gmmloc 39 | -------------------------------------------------------------------------------- /gmmloc/include/gmmloc/visualization/visualizer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "../types/map.h" 6 | 7 | #include "campose_visualizer.h" 8 | #include "gmm_visualizer.h" 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | namespace gmmloc { 16 | 17 | class TrajectoryViewer { 18 | public: 19 | using Ptr = std::unique_ptr; 20 | 21 | using ConstPtr = std::unique_ptr; 22 | 23 | public: 24 | TrajectoryViewer() = delete; 25 | 26 | TrajectoryViewer(const std::string &name, ros::NodeHandle &nh); 27 | 28 | ~TrajectoryViewer() = default; 29 | 30 | void visualize(int start = 0, int end = -1); 31 | 32 | // static nav_msgs:: 33 | static geometry_msgs::PoseStamped pose2msg(const SE3Quat &pose); 34 | 35 | std::string name_; 36 | ros::Publisher pub_; 37 | nav_msgs::Path::Ptr msg_ = nullptr; 38 | 39 | std::vector traj_; 40 | }; 41 | 42 | class ViewerGMMLoc { 43 | protected: 44 | std::atomic_bool stop_, finished_; 45 | 46 | public: 47 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | public: 50 | ViewerGMMLoc(GMM::Ptr map, ros::NodeHandle &nh); 51 | 52 | ros::NodeHandle nh_; 53 | 54 | bool stop() { 55 | stop_ = true; 56 | return stop_; 57 | } 58 | 59 | bool isFinished() { return finished_; } 60 | 61 | void spin(); 62 | 63 | void switchKey(char key); 64 | 65 | void drawKeyFrames(); 66 | 67 | void drawMapPoints(); 68 | 69 | void setMap(Map *pMap) { map_ = pMap; } 70 | 71 | void broadcastTF(); 72 | 73 | void setTransform(const Quaterniond &rot, const Vector3d &trans, 74 | const std::string &name = "camera"); 75 | 76 | void setTrajectory(const eigen_aligned_std_vector &rot, 77 | const eigen_aligned_std_vector &trans, 78 | const std::string &name = "camera"); 79 | 80 | void publishTrajectories(); 81 | 82 | void setImage(const cv::Mat &img); 83 | 84 | private: 85 | CameraPoseVisualizer::Ptr pose_viz_ = nullptr; 86 | Map *map_ = nullptr; 87 | 88 | ros::Publisher kf_pub_; 89 | ros::Publisher mp_pub_; 90 | 91 | sensor_msgs::PointCloud2::Ptr mappoints_ = nullptr; 92 | 93 | std::mutex mutex_pose_, mutex_gt_; 94 | 95 | std::unordered_map traj_records_; 96 | 97 | std::unordered_map tf_records_; 98 | 99 | GMMVisualizer::Ptr gmm_visualizer_ = nullptr; 100 | 101 | std::mutex mutex_img_; 102 | cv::Mat image_; 103 | }; 104 | 105 | } // namespace gmmloc 106 | -------------------------------------------------------------------------------- /gmmloc/node/gmmloc_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "gmmloc/gmmloc.h" 4 | 5 | using namespace std; 6 | 7 | int main(int argc, char **argv) { 8 | google::InitGoogleLogging(argv[0]); 9 | google::ParseCommandLineFlags(&argc, &argv, false); 10 | 11 | ros::init(argc, argv, "gmmloc"); 12 | 13 | ros::NodeHandle nh("~"); 14 | 15 | gmmloc::GMMLoc system(nh); 16 | 17 | system.spin(); 18 | 19 | // stop all threads 20 | system.stop(); 21 | 22 | return 0; 23 | } 24 | -------------------------------------------------------------------------------- /gmmloc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gmmloc 4 | 0.0.0 5 | gmmloc ros package 6 | 7 | hyhuang 8 | 9 | GPL-v3 10 | 11 | catkin 12 | catkin_simple 13 | 14 | glog_catkin 15 | gflags_catkin 16 | g2o_catkin 17 | eigen_catkin 18 | orb_dbow2 19 | protobuf_catkin 20 | 21 | roscpp 22 | tf2 23 | tf2_ros 24 | 25 | -------------------------------------------------------------------------------- /gmmloc/proto/gmmloc/GMM.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | package gmmloc; 4 | 5 | message ComponentProto { 6 | 7 | optional bool is_degenerated = 1; 8 | 9 | optional bool is_salient = 2; 10 | 11 | repeated double mean = 3 [packed=true]; 12 | 13 | repeated double covariance = 4 [packed=true]; 14 | } 15 | 16 | message GMMProto{ 17 | 18 | required int32 num = 1; 19 | 20 | repeated ComponentProto components = 2; 21 | } -------------------------------------------------------------------------------- /gmmloc/src/config.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/config.h" 2 | 3 | #include 4 | 5 | namespace gmmloc { 6 | 7 | namespace common { 8 | 9 | std::string voc_path; 10 | 11 | std::string data_path; 12 | 13 | std::string gt_path; 14 | 15 | std::string output_path; 16 | 17 | std::string rect_config; 18 | 19 | std::string gmm_path; 20 | 21 | bool online = false; 22 | 23 | bool verbose = false; 24 | 25 | bool viewer = false; 26 | 27 | } // namespace common 28 | 29 | namespace camera { 30 | 31 | float fx, fy, cx, cy; 32 | 33 | float fx_inv, fy_inv; 34 | 35 | float k1 = 0.0f, k2 = 0.0f, p1 = 0.0f, p2 = 0.0f, k3 = 0.0f; 36 | 37 | int width = 0, height = 0; 38 | 39 | float fps; 40 | 41 | float bf; 42 | 43 | bool do_rectify = false; 44 | 45 | bool do_equalization = false; 46 | 47 | } // namespace camera 48 | 49 | namespace frame { 50 | 51 | float num_grid_col_inv, num_grid_row_inv; 52 | 53 | const int num_levels = 8; 54 | 55 | const float scale_factor = 1.2; 56 | 57 | const float scale_factor_log = log(scale_factor); 58 | 59 | std::vector scale_factors; 60 | std::vector scale_factors_inv; 61 | std::vector sigma2; 62 | std::vector sigma2_inv; 63 | 64 | float th_depth = 35.0f; 65 | 66 | int num_features = 1000; 67 | 68 | } // namespace frame 69 | 70 | namespace gmmmap { 71 | 72 | double neighbor_dist_thresh = 0.5; 73 | 74 | } // namespace gmmmap 75 | 76 | namespace loc { 77 | 78 | bool tri_use_stereo = false; 79 | 80 | bool tri_check_deg = false; 81 | 82 | float tri_lambda2 = 100.0f; 83 | 84 | bool tri_check_str_chi2 = false; 85 | 86 | float tri_str_thresh = 0.1; 87 | 88 | bool ba_verbose = false; 89 | 90 | float ba_lambda2 = 100.0f; 91 | 92 | bool ba_first_as_prior = false; 93 | 94 | } // namespace loc 95 | 96 | namespace viewer { 97 | 98 | int traj_length = 200; 99 | 100 | } // namespace viewer 101 | 102 | } // namespace gmmloc 103 | -------------------------------------------------------------------------------- /gmmloc/src/cv/orb_vocabulary.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "gmmloc/cv/orb_vocabulary.h" 3 | 4 | #include "gmmloc/common/common.h" 5 | 6 | namespace gmmloc { 7 | 8 | using namespace std; 9 | 10 | ORBVocabulary::Ptr ORBVocabulary::voc_ = nullptr; 11 | 12 | bool ORBVocabulary::initialize(const string &file) { 13 | if (!voc_) { 14 | voc_ = unique_ptr(new voc_type_t); 15 | } 16 | 17 | return voc_->loadFromBinaryFile(file); 18 | } 19 | 20 | void ORBVocabulary::transform(Frame *frame) { 21 | CHECK_NOTNULL(voc_); 22 | 23 | vector vec_desc; 24 | for (size_t i = 0; i < frame->num_feats_; i++) { 25 | vec_desc.push_back(frame->features_[i].desc); 26 | } 27 | 28 | voc_->transform(vec_desc, frame->bow_vec_, frame->feat_vec_, 4); 29 | } 30 | 31 | void ORBVocabulary::transform(KeyFrame *frame) { 32 | CHECK_NOTNULL(voc_); 33 | 34 | vector vec_desc; 35 | for (size_t i = 0; i < frame->num_feats_; i++) { 36 | vec_desc.push_back(frame->features_[i].desc); 37 | } 38 | 39 | voc_->transform(vec_desc, frame->bow_vec_, frame->feat_vec_, 4); 40 | } 41 | 42 | } // namespace gmmloc -------------------------------------------------------------------------------- /gmmloc/src/cv/pinhole_camera.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/cv/pinhole_camera.h" 2 | 3 | #include "gmmloc/common/common.h" 4 | 5 | namespace gmmloc { 6 | 7 | const double PinholeCamera::kMinimumDepth = 0.0; 8 | 9 | PinholeCamera::PinholeCamera(const Eigen::Vector4d &intrinsics, 10 | uint32_t image_width, uint32_t image_height) 11 | : intrinsics_(intrinsics), width_(image_width), height_(image_height) {} 12 | 13 | PinholeCamera::PinholeCamera(double focallength_cols, double focallength_rows, 14 | double imagecenter_cols, double imagecenter_rows, 15 | uint32_t image_width, uint32_t image_height) 16 | : intrinsics_(focallength_cols, focallength_rows, imagecenter_cols, 17 | imagecenter_rows), 18 | width_(image_width), height_(image_height) {} 19 | 20 | void PinholeCamera::unproject3(const Eigen::Vector2d &uv, const double &z, 21 | Eigen::Vector3d *pt3d) const { 22 | CHECK_NOTNULL(pt3d); 23 | CHECK_EQ(intrinsics_.size(), kNumOfParams) << "intrinsics: invalid size!"; 24 | 25 | const auto &fu = intrinsics_[0]; 26 | const auto &fv = intrinsics_[1]; 27 | const auto &cu = intrinsics_[2]; 28 | const auto &cv = intrinsics_[3]; 29 | 30 | (*pt3d)[0] = z * (uv[0] - cu) / fu; 31 | (*pt3d)[1] = z * (uv[1] - cv) / fv; 32 | (*pt3d)[2] = z; 33 | } 34 | 35 | void PinholeCamera::unproject3(const Eigen::Quaterniond &rot_w_c, 36 | const Eigen::Vector3d &t_w_c, 37 | const Eigen::Vector2d &uv, double z, 38 | Eigen::Vector3d *pt3d) const { 39 | Eigen::Vector3d ptc; 40 | unproject3(uv, z, &ptc); 41 | 42 | (*pt3d) = rot_w_c * ptc + t_w_c; 43 | } 44 | 45 | const ProjectionResult 46 | PinholeCamera::project3(const Eigen::Vector3d &point_3d, 47 | Eigen::Vector2d *out_keypoint) const { 48 | CHECK_NOTNULL(out_keypoint); 49 | CHECK_EQ(intrinsics_.size(), kNumOfParams) << "intrinsics: invalid size!"; 50 | 51 | const auto &fu = intrinsics_[0]; 52 | const auto &fv = intrinsics_[1]; 53 | const auto &cu = intrinsics_[2]; 54 | const auto &cv = intrinsics_[3]; 55 | 56 | auto rz = static_cast(1.0) / point_3d[2]; 57 | 58 | Eigen::Vector2d keypoint; 59 | keypoint[0] = point_3d[0] * rz; 60 | keypoint[1] = point_3d[1] * rz; 61 | 62 | (*out_keypoint)[0] = fu * keypoint[0] + cu; 63 | (*out_keypoint)[1] = fv * keypoint[1] + cv; 64 | 65 | return evaluateProjectionResult(*out_keypoint, point_3d); 66 | } 67 | 68 | const ProjectionResult PinholeCamera::project3( 69 | const Eigen::Vector3d &point_3d, Eigen::Vector2d *out_keypoint, 70 | Eigen::Matrix *out_jacobian_point, 71 | Eigen::Matrix *out_jacobian_intrinsics) const { 72 | 73 | CHECK_NOTNULL(out_keypoint); 74 | 75 | const double &fu = intrinsics_[0]; 76 | const double &fv = intrinsics_[1]; 77 | const double &cu = intrinsics_[2]; 78 | const double &cv = intrinsics_[3]; 79 | 80 | // Project the point. 81 | const double &x = point_3d[0]; 82 | const double &y = point_3d[1]; 83 | const double &z = point_3d[2]; 84 | 85 | const double rz = 1.0 / z; 86 | (*out_keypoint)[0] = x * rz; 87 | (*out_keypoint)[1] = y * rz; 88 | 89 | // jacobian point 90 | if (out_jacobian_point) { 91 | // Jacobian including distortion 92 | const double rz2 = rz * rz; 93 | 94 | const double duf_dx = fu * rz; 95 | // const double duf_dy = 0.0; 96 | const double duf_dz = -fu * x * rz2; 97 | // const double dvf_dx = 0.0; 98 | const double dvf_dy = fv * rz; 99 | const double dvf_dz = -fv * y * rz2; 100 | 101 | (*out_jacobian_point) << duf_dx, 0.0, duf_dz, 0.0, dvf_dy, dvf_dz; 102 | } 103 | 104 | // Calculate the Jacobian w.r.t to the intrinsic parameters, if requested. 105 | if (out_jacobian_intrinsics) { 106 | out_jacobian_intrinsics->resize(2, kNumOfParams); 107 | const double duf_dfu = (*out_keypoint)[0]; 108 | const double duf_dfv = 0.0; 109 | const double duf_dcu = 1.0; 110 | const double duf_dcv = 0.0; 111 | const double dvf_dfu = 0.0; 112 | const double dvf_dfv = (*out_keypoint)[1]; 113 | const double dvf_dcu = 0.0; 114 | const double dvf_dcv = 1.0; 115 | 116 | (*out_jacobian_intrinsics) << duf_dfu, duf_dfv, duf_dcu, duf_dcv, dvf_dfu, 117 | dvf_dfv, dvf_dcu, dvf_dcv; 118 | } 119 | 120 | // Normalized image plane to camera plane. 121 | (*out_keypoint)[0] = fu * (*out_keypoint)[0] + cu; 122 | (*out_keypoint)[1] = fv * (*out_keypoint)[1] + cv; 123 | 124 | return evaluateProjectionResult(*out_keypoint, point_3d); 125 | } 126 | 127 | inline const ProjectionResult 128 | PinholeCamera::evaluateProjectionResult(const Eigen::Vector2d &keypoint, 129 | const Eigen::Vector3d &point_3d) const { 130 | 131 | // Eigen::Matrix kp = keypoint; 132 | const bool visibility = isKeypointVisible(keypoint); 133 | 134 | if (visibility && (point_3d[2] > kMinimumDepth)) 135 | return ProjectionResult(ProjectionResult::Status::KEYPOINT_VISIBLE); 136 | else if (!visibility && (point_3d[2] > kMinimumDepth)) 137 | return ProjectionResult( 138 | ProjectionResult::Status::KEYPOINT_OUTSIDE_IMAGE_BOX); 139 | else if (point_3d[2] < 0.0) 140 | return ProjectionResult(ProjectionResult::Status::POINT_BEHIND_CAMERA); 141 | else 142 | return ProjectionResult(ProjectionResult::Status::PROJECTION_INVALID); 143 | } 144 | 145 | bool PinholeCamera::isKeypointVisible(const Eigen::Vector2d &keypoint) const { 146 | return keypoint[0] >= static_cast(0.0) && 147 | keypoint[1] >= static_cast(0.0) && 148 | keypoint[0] < static_cast(width_) && 149 | keypoint[1] < static_cast(height_); 150 | } 151 | } // namespace gmmloc -------------------------------------------------------------------------------- /gmmloc/src/global.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "gmmloc/global.h" 3 | 4 | namespace gmmloc { 5 | 6 | namespace global { 7 | 8 | std::atomic_bool pause(false); 9 | std::atomic_bool step(false); 10 | std::atomic_bool stop(false); 11 | 12 | } // namespace global 13 | 14 | } // namespace gmmloc -------------------------------------------------------------------------------- /gmmloc/src/gmm/factors.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/gmm/factors.h" 2 | 3 | namespace g2o { 4 | 5 | void EdgePt2Gaussian::computeError() { 6 | const VertexSBAPointXYZ *v1 = 7 | static_cast(_vertices[0]); 8 | 9 | // Vector3 obs(_measurement); 10 | _error = comp_->sqrt_info_.transpose() * (v1->estimate() - comp_->mean_); 11 | } 12 | 13 | void EdgePt2Gaussian::linearizeOplus() { 14 | // _jacobianOplusXi = normal_.transpose(); 15 | 16 | _jacobianOplusXi = comp_->sqrt_info_.transpose(); 17 | } 18 | 19 | void EdgeSE3QuatPrior::computeError() { 20 | const VertexSE3Expmap *v1 = 21 | static_cast(_vertices[0]); 22 | 23 | const SE3Quat &Tj = v1->estimate(); 24 | 25 | SE3Quat delta = _inverseMeasurement * Tj; 26 | 27 | _error = delta.log(); 28 | } 29 | 30 | void EdgeSE3QuatPrior::linearizeOplus() { 31 | // _jacobianOplusXi = normal_.transpose(); 32 | const VertexSE3Expmap *v1 = 33 | static_cast(_vertices[0]); 34 | 35 | const SE3Quat &Tj = v1->estimate(); 36 | 37 | SE3Quat delta = _inverseMeasurement * Tj; 38 | 39 | Vector6 dvec = delta.log(); 40 | 41 | Eigen::Matrix Jr = Eigen::Matrix::Zero(); 42 | const Eigen::Matrix3d &&phi_s = skew(dvec.head<3>()); 43 | const Eigen::Matrix3d &&luo_s = skew(dvec.tail<3>()); 44 | Jr.block(0, 0, 3, 3) = phi_s; 45 | Jr.block(3, 3, 3, 3) = phi_s; 46 | Jr.block(0, 3, 3, 3) = luo_s; 47 | Jr *= 0.5; 48 | Jr = Eigen::Matrix::Identity() + Jr; 49 | 50 | _jacobianOplusXi = Jr * Tj.inverse().adj(); 51 | 52 | // Jr.block(0, 0, ) 53 | } 54 | 55 | void EdgePt2GaussianDeg::computeError() { 56 | const VertexSBAPointXYZ *v1 = 57 | static_cast(_vertices[0]); 58 | // Vector3 obs(_measurement); 59 | _error = normal_.transpose() * (v1->estimate() - mean_); 60 | } 61 | 62 | void EdgePt2GaussianDeg::linearizeOplus() { 63 | _jacobianOplusXi = normal_.transpose(); 64 | } 65 | 66 | inline Vector2 project2d(const Vector3 &v) { 67 | return Vector2(v(0) / v(2), v(1) / v(2)); 68 | } 69 | 70 | Vector2 EdgeProjectXYZOnly::cam_project(const Vector3 &trans_xyz) const { 71 | Vector2 proj = project2d(trans_xyz); 72 | 73 | return Vector2(proj[0] * fx + cx, proj[1] * fy + cy); 74 | } 75 | 76 | void EdgeProjectXYZOnly::computeError() { 77 | const VertexSBAPointXYZ *v1 = 78 | static_cast(_vertices[0]); 79 | 80 | Vector2 obs(_measurement); 81 | _error = obs - cam_project(rot_c_w_ * v1->estimate() + t_c_w_); 82 | } 83 | 84 | void EdgeProjectXYZOnly::linearizeOplus() { 85 | // VertexSE3Expmap *vj = static_cast(_vertices[1]); 86 | // SE3Quat T(vj->estimate()); 87 | VertexSBAPointXYZ *vi = static_cast(_vertices[0]); 88 | Vector3 xyz = vi->estimate(); 89 | 90 | Vector3 xyz_trans = rot_c_w_ * xyz + t_c_w_; 91 | 92 | double x = xyz_trans[0]; 93 | double y = xyz_trans[1]; 94 | double z = xyz_trans[2]; 95 | double z_2 = z * z; 96 | 97 | Eigen::Matrix tmp; 98 | tmp(0, 0) = fx; 99 | tmp(0, 1) = 0; 100 | tmp(0, 2) = -x / z * fx; 101 | 102 | tmp(1, 0) = 0; 103 | tmp(1, 1) = fy; 104 | tmp(1, 2) = -y / z * fy; 105 | 106 | _jacobianOplusXi = -1. / z * tmp * rot_c_w_.toRotationMatrix(); 107 | } 108 | 109 | // Vector2 EdgeProjectXYZOnlyStereo::cam_project(const Vector3 &trans_xyz) const 110 | // { 111 | // Vector2 proj = project2d(trans_xyz); 112 | 113 | // return Vector2(proj[0] * fx + cx, proj[1] * fy + cy); 114 | // } 115 | 116 | Vector3 EdgeProjectXYZOnlyStereo::cam_project(const Vector3 &trans_xyz) const { 117 | const number_t invz = 1.0f / trans_xyz[2]; 118 | Vector3 res; 119 | res[0] = trans_xyz[0] * invz * fx + cx; 120 | res[1] = trans_xyz[1] * invz * fy + cy; 121 | res[2] = res[0] - bf * invz; 122 | return res; 123 | } 124 | 125 | void EdgeProjectXYZOnlyStereo::computeError() { 126 | const VertexSBAPointXYZ *v1 = 127 | static_cast(_vertices[0]); 128 | 129 | Vector3 obs(_measurement); 130 | _error = obs - cam_project(rot_c_w_ * v1->estimate() + t_c_w_); 131 | } 132 | 133 | void EdgeProjectXYZOnlyStereo::linearizeOplus() { 134 | // VertexSE3Expmap *vj = static_cast(_vertices[1]); 135 | // SE3Quat T(vj->estimate()); 136 | VertexSBAPointXYZ *vi = static_cast(_vertices[0]); 137 | Vector3 xyz = vi->estimate(); 138 | 139 | Vector3 xyz_trans = rot_c_w_ * xyz + t_c_w_; 140 | 141 | double x = xyz_trans[0]; 142 | double y = xyz_trans[1]; 143 | double z = xyz_trans[2]; 144 | double z_2 = z * z; 145 | 146 | // Eigen::Matrix tmp; 147 | // tmp(0, 0) = fx; 148 | // tmp(0, 1) = 0; 149 | // tmp(0, 2) = -x / z * fx; 150 | 151 | // tmp(1, 0) = 0; 152 | // tmp(1, 1) = fy; 153 | // tmp(1, 2) = -y / z * fy; 154 | 155 | Eigen::Matrix3d R = rot_c_w_.toRotationMatrix(); 156 | 157 | _jacobianOplusXi(0, 0) = -fx * R(0, 0) / z + fx * x * R(2, 0) / z_2; 158 | _jacobianOplusXi(0, 1) = -fx * R(0, 1) / z + fx * x * R(2, 1) / z_2; 159 | _jacobianOplusXi(0, 2) = -fx * R(0, 2) / z + fx * x * R(2, 2) / z_2; 160 | 161 | _jacobianOplusXi(1, 0) = -fy * R(1, 0) / z + fy * y * R(2, 0) / z_2; 162 | _jacobianOplusXi(1, 1) = -fy * R(1, 1) / z + fy * y * R(2, 1) / z_2; 163 | _jacobianOplusXi(1, 2) = -fy * R(1, 2) / z + fy * y * R(2, 2) / z_2; 164 | 165 | _jacobianOplusXi(2, 0) = _jacobianOplusXi(0, 0) - bf * R(2, 0) / z_2; 166 | _jacobianOplusXi(2, 1) = _jacobianOplusXi(0, 1) - bf * R(2, 1) / z_2; 167 | _jacobianOplusXi(2, 2) = _jacobianOplusXi(0, 2) - bf * R(2, 2) / z_2; 168 | } 169 | 170 | } // namespace g2o -------------------------------------------------------------------------------- /gmmloc/src/gmm/gaussian.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/gmm/gaussian.h" 2 | 3 | namespace gmmloc { 4 | 5 | using namespace std; 6 | 7 | const double pi3_sqrt = sqrt((M_PI * 2.0) * (M_PI * 2.0) * (M_PI * 2.0)); 8 | 9 | const double pi3_sqrt_inv = 1.0 / pi3_sqrt; 10 | 11 | std::ostream &operator<<(std::ostream &os, const GaussianComponent &gc) { 12 | os << "mean: " << gc.mean_.transpose() << endl 13 | << "is degenerated: " << gc.is_degenerated << endl; 14 | return os; 15 | } 16 | 17 | void GaussianComponent2d::decompose() { 18 | // Eigen::Matrix3d aa; 19 | // cov_.determinant(); 20 | auto eigen_solver = Eigen::SelfAdjointEigenSolver(cov_); 21 | 22 | axis_ = eigen_solver.eigenvectors(); 23 | scale_ = eigen_solver.eigenvalues(); 24 | 25 | // Matrix2d rot_mat; 26 | // rot_mat.col(0) = axis_.col(0).normalized(); 27 | // rot_mat.col(1) = axis_.col(1).normalized(); 28 | // rot_mat.col(2) = axis_.col(0).cross(axis_.col(1)).normalized(); 29 | 30 | // rot_ = Quaterniond(rot_mat); 31 | theta_ = atan(axis_(1, 0) / axis_(0, 0)); 32 | } 33 | 34 | // template 35 | // void GaussianComponent::decompose() 36 | void GaussianComponent::decompose() { 37 | // Eigen::Matrix3d aa; 38 | // cov_.determinant(); 39 | auto eigen_solver = Eigen::SelfAdjointEigenSolver(cov_); 40 | 41 | axis_ = eigen_solver.eigenvectors(); 42 | scale_ = eigen_solver.eigenvalues(); 43 | // cout << scale_.transpose() << endl; 44 | if (scale_.x() < 1e-4) { 45 | is_degenerated = true; 46 | 47 | sqrt_info_ = cov_inv_.llt().matrixL(); 48 | } 49 | sqrt_info_ = cov_inv_.llt().matrixL(); 50 | 51 | const double scale_thresh = 0.2; 52 | if (scale_.y() > scale_thresh && scale_.z() > scale_thresh) { 53 | is_salient = true; 54 | // cout << scale_.transpose() << endl; 55 | } 56 | 57 | Matrix3d rot_mat; 58 | rot_mat.col(0) = axis_.col(0).normalized(); 59 | rot_mat.col(1) = axis_.col(1).normalized(); 60 | rot_mat.col(2) = axis_.col(0).cross(axis_.col(1)).normalized(); 61 | 62 | rot_ = Quaterniond(rot_mat); 63 | } 64 | 65 | double GaussianComponent::chi2(const Vec &feature) { 66 | // return feature-mean_ 67 | Vec delta = feature - mean_; 68 | 69 | return delta.transpose() * cov_inv_ * delta; 70 | } 71 | 72 | double GaussianComponent::predict(const Vec &feature) { 73 | Vec delta = feature - mean_; 74 | 75 | return pi3_sqrt_inv * det_sqrt_inv_ * 76 | exp(-0.5 * delta.transpose() * cov_inv_ * delta); 77 | } 78 | 79 | } // namespace gmmloc -------------------------------------------------------------------------------- /gmmloc/src/gmm/gmm_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/gmm/gmm_utils.h" 2 | 3 | #include "gmmloc/utils/protobuf_utils.h" 4 | 5 | #include "GMM.pb.h" 6 | 7 | namespace gmmloc { 8 | 9 | bool GMMUtility::loadGMMModel(const std::string &file_path, GMM::Ptr &model) { 10 | // CHECK_NOTNULL(model); 11 | 12 | GaussianComponents comps; 13 | 14 | CHECK(!file_path.empty()); 15 | std::fstream infile; 16 | infile.open(file_path, std::fstream::in); 17 | // std::ios_base::openmode file_flags = std::fstream::out | 18 | // std::fstream::binary; 19 | if (!infile.is_open()) { 20 | LOG(ERROR) << "Could not open protobuf file to load layer: " << file_path; 21 | return false; 22 | } 23 | 24 | uint32_t tmp_byte_offset = 0; 25 | 26 | uint32_t num_comps; 27 | if (!pb::readProtoMsgCountFromStream(&infile, &num_comps, &tmp_byte_offset)) { 28 | LOG(ERROR) << "failed read number of messages."; 29 | return false; 30 | } 31 | 32 | if (num_comps == 0u) { 33 | LOG(WARNING) << "protobuf file empty!"; 34 | return false; 35 | } 36 | 37 | for (uint32_t i = 0; i < num_comps; i++) { 38 | 39 | // FIXME: eof not working? 40 | // if (infile.eof()) { 41 | // LOG(ERROR) << "incomplete map file.."; 42 | // } 43 | 44 | ComponentProto comp_proto; 45 | if (!pb::readProtoMsgFromStream(&infile, &comp_proto, &tmp_byte_offset)) { 46 | LOG(ERROR) << "failed to read component message."; 47 | return false; 48 | } 49 | 50 | CHECK_EQ(comp_proto.mean_size(), 3); 51 | CHECK_EQ(comp_proto.covariance_size(), 9); 52 | Vector3d mean; 53 | Matrix3d cov; 54 | mean << comp_proto.mean(0), comp_proto.mean(1), comp_proto.mean(2); 55 | cov << comp_proto.covariance(0), comp_proto.covariance(1), 56 | comp_proto.covariance(2), comp_proto.covariance(3), 57 | comp_proto.covariance(4), comp_proto.covariance(5), 58 | comp_proto.covariance(6), comp_proto.covariance(7), 59 | comp_proto.covariance(8); 60 | 61 | comps.push_back(new GaussianComponent(mean, cov)); 62 | } 63 | 64 | model = GMM::Ptr(new GMM(comps)); 65 | 66 | return true; 67 | } 68 | 69 | bool GMMUtility::saveGMMModel(const std::string &file_path, GMM::Ptr &model, 70 | bool clear_file) { 71 | CHECK_NOTNULL(model); 72 | 73 | GMMProto proto_gmm; 74 | CHECK(!file_path.empty()); 75 | std::fstream outfile; 76 | std::ios_base::openmode file_flags = std::fstream::out | std::fstream::binary; 77 | 78 | outfile.open(file_path, file_flags); 79 | if (!outfile.is_open()) { 80 | LOG(ERROR) << "fail to save model, path: " << file_path; 81 | return false; 82 | } 83 | 84 | size_t num_to_write = model->countComponents(); 85 | const auto &comps = model->getComponents(); 86 | 87 | const uint32_t num_messages = num_to_write; 88 | if (!pb::writeProtoMsgCountToStream(num_messages, &outfile)) { 89 | LOG(ERROR) << "fail to write message number to file."; 90 | outfile.close(); 91 | return false; 92 | } 93 | 94 | for (auto &comp : comps) { 95 | ComponentProto proto_comp; 96 | 97 | proto_comp.set_is_degenerated(comp->is_degenerated); 98 | 99 | proto_comp.set_is_salient(comp->is_salient); 100 | 101 | const Vector3d &mean = comp->mean(); 102 | const Matrix3d &cov = comp->cov(); 103 | for (size_t i = 0; i < 3; i++) { 104 | proto_comp.add_mean(mean(i)); 105 | } 106 | for (size_t i = 0; i < 9; i++) { 107 | proto_comp.add_covariance(cov(i)); 108 | } 109 | 110 | if (!pb::writeProtoMsgToStream(proto_comp, &outfile)) { 111 | LOG(ERROR) << "failed to write component message."; 112 | outfile.close(); 113 | return false; 114 | } 115 | } 116 | 117 | outfile.close(); 118 | return true; 119 | } 120 | 121 | GaussianComponent2d::Ptr 122 | GMMUtility::projectGaussian(const GaussianComponent &g, PinholeCamera::Ptr cam, 123 | const Eigen::Quaterniond &rot_c_w, 124 | const Eigen::Vector3d &t_c_w) { 125 | auto &&cov3d = g.cov(); 126 | auto &&mu3d = g.mean(); 127 | 128 | Eigen::Vector3d mu3d_cam = rot_c_w * mu3d + t_c_w; 129 | 130 | Eigen::Vector2d mu2d; 131 | Eigen::Matrix jacob_proj; 132 | auto res_proj = cam->project3(mu3d_cam, &mu2d, &jacob_proj); 133 | 134 | if (res_proj.getDetailedStatus() != 135 | ProjectionResult::Status::KEYPOINT_VISIBLE) { 136 | // cout << "not visible" << endl; 137 | return nullptr; 138 | } 139 | 140 | auto rot = rot_c_w.toRotationMatrix(); 141 | 142 | auto cov2d = 143 | jacob_proj * rot * cov3d * rot.transpose() * jacob_proj.transpose(); 144 | 145 | return GaussianComponent2d::Ptr(new GaussianComponent2d(mu2d, cov2d)); 146 | } 147 | 148 | } // namespace gmmloc 149 | -------------------------------------------------------------------------------- /gmmloc/src/modules/tracking_opt.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/modules/tracking.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include "gmmloc/config.h" 16 | 17 | namespace gmmloc { 18 | 19 | using namespace std; 20 | 21 | int Tracking::optimizeCurrentPose() { 22 | 23 | g2o::SparseOptimizer optimizer; 24 | 25 | g2o::OptimizationAlgorithmLevenberg *solver; 26 | 27 | solver = new g2o::OptimizationAlgorithmLevenberg( 28 | g2o::make_unique( 29 | g2o::make_unique< 30 | g2o::LinearSolverDense>())); 31 | optimizer.setAlgorithm(solver); 32 | 33 | int num_init_correspondences = 0; 34 | 35 | // Set Frame vertex 36 | g2o::VertexSE3Expmap *vertex_se3 = new g2o::VertexSE3Expmap(); 37 | vertex_se3->setEstimate(curr_frame_->getTcw()); 38 | vertex_se3->setId(0); 39 | vertex_se3->setFixed(false); 40 | optimizer.addVertex(vertex_se3); 41 | 42 | // Set MapPoint vertices 43 | const int N = curr_frame_->num_feats_; 44 | 45 | // for Monocular 46 | vector edges_mono; 47 | vector indices_edge_mono; 48 | edges_mono.reserve(N); 49 | indices_edge_mono.reserve(N); 50 | 51 | // for Stereo 52 | vector edges_stereo; 53 | vector indices_edge_stereo; 54 | edges_stereo.reserve(N); 55 | indices_edge_stereo.reserve(N); 56 | 57 | const float delta_mono = sqrt(5.991); 58 | const float delta_stereo = sqrt(7.815); 59 | 60 | { 61 | unique_lock lock(MapPoint::global_mutex_); 62 | 63 | for (int i = 0; i < N; i++) { 64 | MapPoint *mappt = curr_frame_->mappoints_[i]; 65 | if (mappt) { 66 | // Monocular observation 67 | if (curr_frame_->features_[i].u_right < 0) { 68 | num_init_correspondences++; 69 | curr_frame_->is_outlier_[i] = false; 70 | 71 | const auto &kp = curr_frame_->features_[i]; 72 | Vector2d obs = kp.uv; 73 | // obs << kp.uv; 74 | 75 | g2o::EdgeSE3ProjectXYZOnlyPose *e = 76 | new g2o::EdgeSE3ProjectXYZOnlyPose(); 77 | 78 | e->setVertex(0, dynamic_cast( 79 | optimizer.vertex(0))); 80 | e->setMeasurement(obs); 81 | const float inv_sigma2 = frame::sigma2_inv[kp.octave]; 82 | e->setInformation(Eigen::Matrix2d::Identity() * inv_sigma2); 83 | 84 | g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; 85 | e->setRobustKernel(rk); 86 | rk->setDelta(delta_mono); 87 | 88 | e->fx = curr_frame_->camera_->fx(); 89 | e->fy = curr_frame_->camera_->fy(); 90 | e->cx = curr_frame_->camera_->cx(); 91 | e->cy = curr_frame_->camera_->cy(); 92 | 93 | e->Xw = mappt->getPosition(); 94 | 95 | optimizer.addEdge(e); 96 | 97 | edges_mono.push_back(e); 98 | indices_edge_mono.push_back(i); 99 | } else { 100 | num_init_correspondences++; 101 | curr_frame_->is_outlier_[i] = false; 102 | 103 | // SET EDGE 104 | Eigen::Matrix obs; 105 | const auto &kp = curr_frame_->features_[i]; 106 | const float &kp_ur = curr_frame_->features_[i].u_right; 107 | obs << kp.uv, kp_ur; 108 | 109 | g2o::EdgeStereoSE3ProjectXYZOnlyPose *e = 110 | new g2o::EdgeStereoSE3ProjectXYZOnlyPose(); 111 | 112 | e->setVertex(0, dynamic_cast( 113 | optimizer.vertex(0))); 114 | e->setMeasurement(obs); 115 | const float inv_sigma2 = frame::sigma2_inv[kp.octave]; 116 | Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * inv_sigma2; 117 | e->setInformation(Info); 118 | 119 | g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber; 120 | e->setRobustKernel(rk); 121 | rk->setDelta(delta_stereo); 122 | 123 | e->fx = curr_frame_->camera_->fx(); 124 | e->fy = curr_frame_->camera_->fy(); 125 | e->cx = curr_frame_->camera_->cx(); 126 | e->cy = curr_frame_->camera_->cy(); 127 | e->bf = curr_frame_->mbf; 128 | e->Xw = mappt->getPosition(); 129 | 130 | optimizer.addEdge(e); 131 | 132 | edges_stereo.push_back(e); 133 | indices_edge_stereo.push_back(i); 134 | } 135 | } 136 | } 137 | } 138 | 139 | if (num_init_correspondences < 3) 140 | return 0; 141 | 142 | // We perform 4 optimizations, after each optimization we classify observation 143 | // as inlier/outlier At the next optimization, outliers are not included, but 144 | // at the end they can be classified as inliers again. 145 | const float chi2Mono[4] = {5.991, 5.991, 5.991, 5.991}; 146 | const float chi2Stereo[4] = {7.815, 7.815, 7.815, 7.815}; 147 | const int its[4] = {10, 10, 10, 10}; 148 | 149 | int num_bad = 0; 150 | for (size_t it = 0; it < 4; it++) { 151 | 152 | vertex_se3->setEstimate(curr_frame_->getTcw()); 153 | optimizer.initializeOptimization(0); 154 | optimizer.optimize(its[it]); 155 | 156 | num_bad = 0; 157 | for (size_t i = 0, iend = edges_mono.size(); i < iend; i++) { 158 | g2o::EdgeSE3ProjectXYZOnlyPose *e = edges_mono[i]; 159 | 160 | const size_t idx = indices_edge_mono[i]; 161 | 162 | if (curr_frame_->is_outlier_[idx]) { 163 | e->computeError(); 164 | } 165 | 166 | const float chi2 = e->chi2(); 167 | 168 | if (chi2 > chi2Mono[it]) { 169 | curr_frame_->is_outlier_[idx] = true; 170 | e->setLevel(1); // outlier 171 | num_bad++; 172 | } else { 173 | curr_frame_->is_outlier_[idx] = false; 174 | e->setLevel(0); // inlier 175 | } 176 | 177 | if (it == 2) 178 | e->setRobustKernel(0); 179 | } 180 | 181 | for (size_t i = 0, iend = edges_stereo.size(); i < iend; i++) { 182 | g2o::EdgeStereoSE3ProjectXYZOnlyPose *e = edges_stereo[i]; 183 | 184 | const size_t idx = indices_edge_stereo[i]; 185 | 186 | if (curr_frame_->is_outlier_[idx]) { 187 | e->computeError(); 188 | } 189 | 190 | const float chi2 = e->chi2(); 191 | 192 | if (chi2 > chi2Stereo[it]) { 193 | curr_frame_->is_outlier_[idx] = true; 194 | e->setLevel(1); 195 | num_bad++; 196 | } else { 197 | e->setLevel(0); 198 | curr_frame_->is_outlier_[idx] = false; 199 | } 200 | 201 | if (it == 2) 202 | e->setRobustKernel(0); 203 | } 204 | 205 | if (optimizer.edges().size() < 10) 206 | break; 207 | } 208 | 209 | // Recover optimized pose and return number of inliers 210 | g2o::VertexSE3Expmap *vertex_se3_recov = 211 | static_cast(optimizer.vertex(0)); 212 | g2o::SE3Quat pose = vertex_se3_recov->estimate(); 213 | // cv::Mat pose = Converter::toCvMat(SE3quat_recov); 214 | curr_frame_->setTcw(pose); 215 | 216 | return num_init_correspondences - num_bad; 217 | } 218 | 219 | } // namespace gmmloc -------------------------------------------------------------------------------- /gmmloc/src/types/map.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/types/map.h" 2 | 3 | #include 4 | 5 | #include "gmmloc/common/common.h" 6 | 7 | namespace gmmloc { 8 | 9 | using namespace std; 10 | 11 | void Map::addKeyFrame(KeyFrame *kf_ptr) { 12 | unique_lock lock(mutex_map_); 13 | keyframes_.insert(kf_ptr); 14 | if (kf_ptr->idx_ > max_kf_idx_) 15 | max_kf_idx_ = kf_ptr->idx_; 16 | } 17 | 18 | void Map::addObservation(MapPoint *mappt) { 19 | unique_lock lock(mutex_map_); 20 | mappoints_.insert(mappt); 21 | } 22 | 23 | void Map::updateFrameInfo(Frame *curr_frame) { 24 | LOG(INFO) << "update frame info."; 25 | 26 | FrameInfo *info = new FrameInfo; 27 | info->Trc = SE3QuatPtr(new SE3Quat); 28 | 29 | (*info->Trc) = curr_frame->ref_keyframe_->getTcw() * curr_frame->getTwc(); 30 | info->timestamp = curr_frame->timestamp_; 31 | info->ref = curr_frame->ref_keyframe_; 32 | 33 | (*curr_frame->T_c_r_) = info->Trc->inverse(); 34 | 35 | unique_lock lock(mutex_info_); 36 | 37 | frame_info_.push_back(info); 38 | } 39 | 40 | void Map::removeMapPoint(MapPoint *mappt) { 41 | unordered_map observations; 42 | { 43 | unique_lock lock1(mappt->mutex_attr_); 44 | unique_lock lock2(mappt->mutex_pos_); 45 | mappt->not_valid_ = true; 46 | observations = mappt->observations_; 47 | mappt->observations_.clear(); 48 | } 49 | for (auto &&obs : observations) { 50 | KeyFrame *kf_ptr = obs.first; 51 | kf_ptr->removeObservation(obs.second); 52 | } 53 | 54 | { 55 | unique_lock lock(mutex_map_); 56 | mappoints_.erase(mappt); 57 | } 58 | } 59 | 60 | void Map::removeKeyFrame(KeyFrame *kf_ptr) { 61 | { 62 | unique_lock lock(kf_ptr->mutex_connections_); 63 | if (kf_ptr->idx_ == 0) 64 | return; 65 | } 66 | 67 | for (auto &&info : kf_ptr->map_frame_weights_) 68 | info.first->removeConnection(kf_ptr); 69 | 70 | for (auto &&mp : kf_ptr->mappoints_) { 71 | if (mp) { 72 | if (mp->removeObservation(kf_ptr)) { 73 | this->removeMapPoint(mp); 74 | } 75 | } 76 | } 77 | 78 | { 79 | unique_lock lock(kf_ptr->mutex_connections_); 80 | unique_lock lock1(kf_ptr->mutex_attr_); 81 | 82 | kf_ptr->best_cov_kf_ = kf_ptr->ordered_keyframes_[0]; 83 | kf_ptr->map_frame_weights_.clear(); 84 | kf_ptr->ordered_keyframes_.clear(); 85 | 86 | kf_ptr->not_valid_ = true; 87 | } 88 | 89 | // update map information 90 | { 91 | auto kf_tgt = kf_ptr->getBestCovisibilityKeyFrame(); 92 | CHECK(!kf_tgt->not_valid_); 93 | CHECK_NOTNULL(kf_tgt); 94 | 95 | SE3Quat Ttr = kf_tgt->getTcw() * kf_ptr->getTwc(); 96 | 97 | unique_lock lock(mutex_info_); 98 | for_each(frame_info_.begin(), frame_info_.end(), [&](FrameInfo *info) { 99 | if (info->ref == kf_ptr) { 100 | info->ref = kf_tgt; 101 | (*info->Trc) = Ttr * (*info->Trc); 102 | } 103 | }); 104 | } 105 | 106 | { 107 | unique_lock lock(mutex_map_); 108 | keyframes_.erase(kf_ptr); 109 | } 110 | } 111 | 112 | void Map::replaceMapPoint(MapPoint *src, MapPoint *tgt) { 113 | if (tgt->idx_ == src->idx_) 114 | return; 115 | 116 | int nvisible, nfound; 117 | unordered_map obs; 118 | { 119 | unique_lock lock1(src->mutex_attr_); 120 | unique_lock lock2(src->mutex_pos_); 121 | obs = src->observations_; 122 | src->observations_.clear(); 123 | src->not_valid_ = true; 124 | nvisible = src->num_visible_; 125 | nfound = src->num_found_; 126 | src->ptr_replaced_ = tgt; 127 | } 128 | 129 | for (unordered_map::iterator mit = obs.begin(), 130 | mend = obs.end(); 131 | mit != mend; mit++) { 132 | KeyFrame *kf_ptr = mit->first; 133 | 134 | if (!tgt->checkObservation(kf_ptr)) { 135 | kf_ptr->replaceObservation(mit->second, tgt); 136 | tgt->addObservation(kf_ptr, mit->second); 137 | } else { 138 | kf_ptr->removeObservation(mit->second); 139 | } 140 | } 141 | 142 | tgt->num_visible_ += nvisible; 143 | tgt->num_found_ += nfound; 144 | tgt->computeDistinctiveDescriptors(); 145 | 146 | { 147 | unique_lock lock(mutex_map_); 148 | mappoints_.erase(src); 149 | } 150 | } 151 | 152 | void Map::setTrajectory(const eigen_aligned_std_vector &rot, 153 | const eigen_aligned_std_vector &trans) { 154 | 155 | CHECK_EQ(rot.size(), trans.size()); 156 | 157 | for (size_t i = 0; i < rot.size(); i++) { 158 | gt_traj_.push_back(SE3QuatConstPtr(new SE3Quat(rot[i], trans[i]))); 159 | } 160 | } 161 | 162 | void Map::summarize() { 163 | std::string file_name = common::output_path + "/traj_est.txt"; 164 | ofstream fs(file_name); 165 | 166 | eigen_aligned_std_vector traj_; 167 | fs << std::fixed; 168 | for (auto &&info : frame_info_) { 169 | /* code */ 170 | CHECK_NOTNULL(info->ref); 171 | 172 | const auto ×tamp = info->timestamp; 173 | 174 | const SE3Quat Twc = info->ref->getTwc() * (*info->Trc); 175 | const auto &rot = Twc.rotation(); 176 | const auto &trans = Twc.translation(); 177 | 178 | traj_.push_back(Twc); 179 | 180 | fs << setprecision(6) << timestamp << " " << setprecision(9) << trans.x() 181 | << ' ' << trans.y() << ' ' << trans.z() << ' ' << rot.x() << ' ' 182 | << rot.y() << ' ' << rot.z() << ' ' << rot.w() << endl; 183 | } 184 | fs.close(); 185 | 186 | LOG(WARNING) << "trajectory saved to " << file_name; 187 | 188 | } 189 | 190 | vector Map::getAllKeyFrames() { 191 | unique_lock lock(mutex_map_); 192 | return vector(keyframes_.begin(), keyframes_.end()); 193 | } 194 | 195 | vector Map::getAllMapPoints() { 196 | unique_lock lock(mutex_map_); 197 | return vector(mappoints_.begin(), mappoints_.end()); 198 | } 199 | 200 | long unsigned int Map::countMapPoints() { 201 | unique_lock lock(mutex_map_); 202 | return mappoints_.size(); 203 | } 204 | 205 | long unsigned int Map::countKeyFrames() { 206 | unique_lock lock(mutex_map_); 207 | return keyframes_.size(); 208 | } 209 | 210 | void Map::clear() { 211 | for (set::iterator sit = mappoints_.begin(), 212 | send = mappoints_.end(); 213 | sit != send; sit++) 214 | delete *sit; 215 | 216 | for (set::iterator sit = keyframes_.begin(), 217 | send = keyframes_.end(); 218 | sit != send; sit++) 219 | delete *sit; 220 | 221 | mappoints_.clear(); 222 | keyframes_.clear(); 223 | max_kf_idx_ = 0; 224 | } 225 | 226 | } // namespace gmmloc 227 | -------------------------------------------------------------------------------- /gmmloc/src/types/mappoint.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/types/mappoint.h" 2 | 3 | #include 4 | 5 | #include "gmmloc/config.h" 6 | #include "gmmloc/global.h" 7 | 8 | #include "gmmloc/cv/orb_matcher.h" 9 | 10 | namespace gmmloc { 11 | 12 | using namespace std; 13 | 14 | long unsigned int MapPoint::next_idx_ = 0; 15 | mutex MapPoint::global_mutex_, MapPoint::global_mutex_idx; 16 | 17 | MapPoint::MapPoint(const Vector3d &Pos, KeyFrame *ref_kf) 18 | : ref_idx_(ref_kf->idx_), frame_idx_(ref_kf->frame_idx_), ref_kf_(ref_kf), 19 | not_valid_(false), num_visible_(1), num_found_(1) { 20 | type_ = FromTriMono; 21 | 22 | pos_ = Pos; 23 | normal_.setZero(); 24 | 25 | unique_lock lock(global_mutex_idx); 26 | idx_ = next_idx_++; 27 | } 28 | 29 | MapPoint::MapPoint(const Vector3d &Pos, Frame *frame, const int &idxF) 30 | : frame_idx_(frame->idx_), not_valid_(false), num_visible_(1), 31 | num_found_(1) { 32 | type_ = FromTriMono; 33 | 34 | pos_ = Pos; 35 | 36 | Vector3d Ow = frame->getTwc().translation(); 37 | normal_ = pos_ - Ow; 38 | normal_.normalize(); 39 | 40 | Vector3d PC = pos_ - Ow; 41 | const float dist = PC.norm(); 42 | const int level = frame->features_[idxF].octave; 43 | const float levelScaleFactor = frame::scale_factors[level]; 44 | const int nLevels = frame::num_levels; 45 | 46 | max_dist_ = dist * levelScaleFactor; 47 | min_dist_ = max_dist_ / frame::scale_factors[nLevels - 1]; 48 | 49 | frame->features_[idxF].desc.copyTo(desc_); 50 | 51 | unique_lock lock(global_mutex_idx); 52 | idx_ = next_idx_++; 53 | } 54 | 55 | void MapPoint::setPosition(const Vector3d &pos) { 56 | unique_lock lock2(global_mutex_); 57 | unique_lock lock(mutex_pos_); 58 | pos_ = pos; 59 | } 60 | 61 | Vector3d MapPoint::getPosition() { 62 | shared_lock lock(mutex_pos_); 63 | 64 | return pos_; 65 | } 66 | 67 | Vector3d MapPoint::getNormal() { 68 | shared_lock lock(mutex_pos_); 69 | return normal_; 70 | } 71 | 72 | void MapPoint::addObservation(KeyFrame *kf_ptr, size_t idx) { 73 | unique_lock lock(mutex_attr_); 74 | if (observations_.count(kf_ptr)) 75 | return; 76 | observations_[kf_ptr] = idx; 77 | 78 | if (kf_ptr->features_[idx].u_right >= 0) 79 | num_obs_ += 2; 80 | else 81 | num_obs_++; 82 | } 83 | 84 | unordered_map MapPoint::getObservations() { 85 | shared_lock lock(mutex_attr_); 86 | return observations_; 87 | } 88 | 89 | int MapPoint::countObservations() { 90 | shared_lock lock(mutex_attr_); 91 | return num_obs_; 92 | } 93 | 94 | bool MapPoint::removeObservation(KeyFrame *kf_ptr) { 95 | bool bBad = false; 96 | { 97 | unique_lock lock(mutex_attr_); 98 | 99 | if (observations_.count(kf_ptr)) { 100 | size_t idx = observations_[kf_ptr]; 101 | if (kf_ptr->features_[idx].u_right >= 0) 102 | num_obs_ -= 2; 103 | else 104 | num_obs_--; 105 | 106 | observations_.erase(kf_ptr); 107 | 108 | if (num_obs_ > 0 && ref_kf_ == kf_ptr) { 109 | ref_kf_ = observations_.begin()->first; 110 | } 111 | 112 | if (num_obs_ <= 2) 113 | bBad = true; 114 | } 115 | } 116 | 117 | return bBad; 118 | } 119 | 120 | MapPoint *MapPoint::getReplaced() { 121 | unique_lock lock1(mutex_attr_); 122 | unique_lock lock2(mutex_pos_); 123 | return ptr_replaced_; 124 | } 125 | 126 | void MapPoint::computeDistinctiveDescriptors() { 127 | vector vDescriptors; 128 | 129 | unordered_map observations; 130 | 131 | { 132 | unique_lock lock1(mutex_attr_); 133 | if (not_valid_) 134 | return; 135 | 136 | observations = observations_; 137 | } 138 | 139 | if (observations.empty()) 140 | return; 141 | 142 | vDescriptors.reserve(observations.size()); 143 | 144 | for (unordered_map::iterator mit = observations.begin(), 145 | mend = observations.end(); 146 | mit != mend; mit++) { 147 | KeyFrame *kf_ptr = mit->first; 148 | 149 | if (!kf_ptr->not_valid_) 150 | vDescriptors.push_back(kf_ptr->features_[mit->second].desc); 151 | } 152 | 153 | if (vDescriptors.empty()) 154 | return; 155 | 156 | // Compute distances between them 157 | const size_t N = vDescriptors.size(); 158 | 159 | std::vector> Distances; 160 | Distances.resize(N, vector(N, 0)); 161 | for (size_t i = 0; i < N; i++) { 162 | Distances[i][i] = 0; 163 | for (size_t j = i + 1; j < N; j++) { 164 | int distij = 165 | ORBmatcher::DescriptorDistance(vDescriptors[i], vDescriptors[j]); 166 | Distances[i][j] = distij; 167 | Distances[j][i] = distij; 168 | } 169 | } 170 | 171 | // Take the descriptor with least median distance to the rest 172 | int BestMedian = INT_MAX; 173 | int BestIdx = 0; 174 | for (size_t i = 0; i < N; i++) { 175 | vector vDists(Distances[i].begin(), Distances[i].end()); 176 | sort(vDists.begin(), vDists.end()); 177 | 178 | int median = vDists[0.5 * (N - 1)]; 179 | 180 | if (median < BestMedian) { 181 | BestMedian = median; 182 | BestIdx = i; 183 | } 184 | } 185 | 186 | { 187 | unique_lock lock(mutex_attr_); 188 | 189 | desc_ = vDescriptors[BestIdx].clone(); 190 | } 191 | } 192 | 193 | cv::Mat MapPoint::getDescriptor() { 194 | shared_lock lock(mutex_attr_); 195 | return desc_.clone(); 196 | } 197 | 198 | int MapPoint::getIndexInKeyFrame(KeyFrame *kf_ptr) { 199 | shared_lock lock(mutex_attr_); 200 | if (observations_.count(kf_ptr)) 201 | return observations_[kf_ptr]; 202 | else 203 | return -1; 204 | } 205 | 206 | bool MapPoint::checkObservation(KeyFrame *kf_ptr) { 207 | shared_lock lock(mutex_attr_); 208 | return (observations_.count(kf_ptr)); 209 | } 210 | 211 | void MapPoint::updateNormalAndDepth() { 212 | unordered_map observations; 213 | KeyFrame *pRefKF; 214 | Vector3d pos; 215 | { 216 | unique_lock lock1(mutex_attr_); 217 | shared_lock lock2(mutex_pos_); 218 | if (not_valid_) 219 | return; 220 | 221 | observations = observations_; 222 | pRefKF = ref_kf_; 223 | pos = pos_; 224 | } 225 | 226 | if (observations.empty()) 227 | return; 228 | 229 | Vector3d normal = Vector3d::Zero(); 230 | int n = 0; 231 | for (unordered_map::iterator mit = observations.begin(), 232 | mend = observations.end(); 233 | mit != mend; mit++) { 234 | KeyFrame *kf_ptr = mit->first; 235 | Vector3d Owi = kf_ptr->getTwc().translation(); 236 | Vector3d normali = pos - Owi; 237 | normal = normal + normali.normalized(); 238 | 239 | n++; 240 | } 241 | 242 | Vector3d PC = pos - pRefKF->getTwc().translation(); 243 | 244 | const float dist = PC.norm(); 245 | const int level = pRefKF->features_[observations[pRefKF]].octave; 246 | const float levelScaleFactor = frame::scale_factors[level]; 247 | const int nLevels = frame::num_levels; 248 | 249 | { 250 | shared_lock lock3(mutex_pos_); 251 | max_dist_ = dist * levelScaleFactor; 252 | min_dist_ = max_dist_ / frame::scale_factors[nLevels - 1]; 253 | normal_ = normal / n; 254 | } 255 | } 256 | 257 | bool MapPoint::checkScaleAndVisible(const Vector3d &t_w_c, ProjStat &stat) { 258 | float min_dist, max_dist; 259 | Vector3d vec_pt_c; 260 | { 261 | shared_lock lock(mutex_pos_); 262 | max_dist = 1.2f * max_dist_; 263 | min_dist = 0.8f * min_dist_; 264 | 265 | vec_pt_c = pos_ - t_w_c; 266 | } 267 | 268 | const float dist = vec_pt_c.norm(); 269 | 270 | if (dist < min_dist || dist > max_dist) 271 | return false; 272 | 273 | float view_cos; 274 | { 275 | shared_lock lock(mutex_pos_); 276 | view_cos = vec_pt_c.dot(normal_) / dist; 277 | } 278 | 279 | const float th_view_cos = 0.5; 280 | if (view_cos < th_view_cos) 281 | return false; 282 | 283 | float ratio; 284 | { 285 | shared_lock lock(mutex_pos_); 286 | ratio = max_dist_ / dist; 287 | } 288 | 289 | int lvl_scale = ceil(log(ratio) / frame::scale_factor_log); 290 | if (lvl_scale < 0) 291 | lvl_scale = 0; 292 | else if (lvl_scale >= frame::num_levels) 293 | lvl_scale = frame::num_levels - 1; 294 | 295 | stat.dist = dist; 296 | stat.view_cos = view_cos; 297 | stat.scale_pred = lvl_scale; 298 | return true; 299 | } 300 | 301 | } // namespace gmmloc 302 | -------------------------------------------------------------------------------- /gmmloc/src/utils/cv_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/utils/cv_utils.h" 2 | 3 | #include 4 | 5 | namespace gmmloc { 6 | 7 | using namespace std; 8 | 9 | Rectify::Rectify(const string &config) { 10 | cv::FileStorage fsSettings(config, cv::FileStorage::READ); 11 | if (!fsSettings.isOpened()) { 12 | cerr << "ERROR: Wrong path to settings" << endl; 13 | throw; 14 | } 15 | 16 | cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; 17 | fsSettings["LEFT.K"] >> K_l; 18 | fsSettings["RIGHT.K"] >> K_r; 19 | 20 | fsSettings["LEFT.P"] >> P_l; 21 | fsSettings["RIGHT.P"] >> P_r; 22 | 23 | fsSettings["LEFT.R"] >> R_l; 24 | fsSettings["RIGHT.R"] >> R_r; 25 | 26 | fsSettings["LEFT.D"] >> D_l; 27 | fsSettings["RIGHT.D"] >> D_r; 28 | 29 | int rows_l = fsSettings["LEFT.height"]; 30 | int cols_l = fsSettings["LEFT.width"]; 31 | int rows_r = fsSettings["RIGHT.height"]; 32 | int cols_r = fsSettings["RIGHT.width"]; 33 | 34 | if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || 35 | R_r.empty() || D_l.empty() || D_r.empty() || rows_l == 0 || rows_r == 0 || 36 | cols_l == 0 || cols_r == 0) { 37 | cerr << "ERROR: Calibration parameters to rectify stereo are missing!" 38 | << endl; 39 | throw; 40 | } 41 | 42 | cv::initUndistortRectifyMap(K_l, D_l, R_l, P_l.rowRange(0, 3).colRange(0, 3), 43 | cv::Size(cols_l, rows_l), CV_32F, M1l, M2l); 44 | cv::initUndistortRectifyMap(K_r, D_r, R_r, P_r.rowRange(0, 3).colRange(0, 3), 45 | cv::Size(cols_r, rows_r), CV_32F, M1r, M2r); 46 | } 47 | 48 | void Rectify::doRectifyL(const cv::Mat &img_src, cv::Mat &img_rec) { 49 | cv::remap(img_src, img_rec, M1l, M2l, cv::INTER_LINEAR); 50 | } 51 | 52 | void Rectify::doRectifyR(const cv::Mat &img_src, cv::Mat &img_rec) { 53 | cv::remap(img_src, img_rec, M1r, M2r, cv::INTER_LINEAR); 54 | } 55 | 56 | } // namespace gmmloc 57 | -------------------------------------------------------------------------------- /gmmloc/src/utils/dataloader.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/utils/dataloader.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | namespace gmmloc { 11 | 12 | using namespace std; 13 | 14 | bool isPathExist(const std::string &s) { 15 | struct stat buffer; 16 | return (stat(s.c_str(), &buffer) == 0); 17 | } 18 | 19 | Dataloader::Dataloader(const std::string &str, DataType cfg) 20 | : cfg_(cfg), base_path_(str) {} 21 | 22 | bool Dataloader::getTrajectory(eigen_aligned_std_vector &rot, 23 | eigen_aligned_std_vector &trans) { 24 | rot = rot_; 25 | trans = trans_; 26 | 27 | return true; 28 | } 29 | 30 | DataloaderEuRoC::DataloaderEuRoC(const std::string &str, 31 | const std::string &traj_file, DataType cfg) 32 | : Dataloader(str, cfg) { 33 | 34 | LOG(WARNING) << "loading data from: " << base_path_; 35 | if (!isPathExist(base_path_)) 36 | throw std::runtime_error("base path not exists: " + base_path_); 37 | 38 | if (cfg_ & DataType::GT) { 39 | if (!isPathExist(traj_file)) 40 | throw std::runtime_error("traj_file not exists: " + traj_file); 41 | 42 | loadTrajectory(traj_file); 43 | LOG(WARNING) << "load trajectory: " << traj_file; 44 | num_ = rot_.size(); 45 | } 46 | 47 | if (cfg_ & DataType::Mono) { 48 | loadImages(base_path_); 49 | num_ = mono_file_.size(); 50 | } 51 | } 52 | 53 | DataFrame::Ptr DataloaderEuRoC::getNextFrame() { 54 | if (idx_ >= num_) 55 | return nullptr; 56 | 57 | auto ptr = getFrameByIndex(idx_); 58 | 59 | idx_++; 60 | return ptr; 61 | } 62 | 63 | DataFrame::Ptr DataloaderEuRoC::getFrameByIndex(size_t idx) { 64 | if (idx >= num_) 65 | return nullptr; 66 | 67 | auto ptr = DataFrame::Ptr(new DataFrame()); 68 | 69 | if (cfg_ & DataType::GT) { 70 | ptr->rot_w_c = rot_[idx]; 71 | ptr->t_w_c = trans_[idx]; 72 | } 73 | 74 | if (cfg_ & DataType::Mono) { 75 | if (isPathExist(mono_file_[idx])) { 76 | ptr->mono = cv::imread(mono_file_[idx]); 77 | } else { 78 | ptr->mono = cv::Mat(); 79 | } 80 | 81 | ptr->timestamp = time_stamp_[idx]; 82 | } 83 | if (cfg_ & DataType::Depth) { 84 | if (isPathExist(depth_file_[idx])) { 85 | ptr->depth = cv::imread(depth_file_[idx]); 86 | } else { 87 | ptr->depth = cv::Mat(); 88 | } 89 | } 90 | 91 | ptr->idx = idx; 92 | return ptr; 93 | } 94 | 95 | void DataloaderEuRoC::loadImages(const string &base_path) { 96 | ifstream fTimes; 97 | string strPathTimeFile = base_path + "/cam0/data.csv"; 98 | string strPrefixLeft = base_path + "/cam0/data/"; 99 | string strPrefixRight = base_path + "/cam1/data/"; 100 | 101 | fTimes.open(strPathTimeFile.c_str()); 102 | string s; 103 | getline(fTimes, s); 104 | while (!fTimes.eof()) { 105 | string s; 106 | getline(fTimes, s); 107 | if (!s.empty()) { 108 | int index = s.find_first_of(","); 109 | string t = s.substr(0, index); 110 | 111 | time_stamp_.push_back(stod(t) / 10.0e8); 112 | mono_file_.push_back(strPrefixLeft + t + ".png"); 113 | depth_file_.push_back(strPrefixRight + t + ".png"); 114 | } 115 | } 116 | } 117 | 118 | void DataloaderEuRoC::loadTrajectory(const std::string &traj_file) { 119 | ifstream fs_mean(traj_file.c_str()); 120 | string str_line; 121 | while (getline(fs_mean, str_line) && !fs_mean.eof()) { 122 | stringstream ss(str_line); 123 | double time, x, y, z, qx, qy, qz, qw; 124 | ss >> time; 125 | ss >> x; 126 | ss >> y; 127 | ss >> z; 128 | ss >> qx; 129 | ss >> qy; 130 | ss >> qz; 131 | ss >> qw; 132 | 133 | Eigen::Vector3d trans_tmp(x, y, z); 134 | Eigen::Quaterniond q_tmp(qw, qx, qy, qz); 135 | trans_.push_back(trans_tmp); 136 | rot_.push_back(q_tmp); 137 | } 138 | } 139 | 140 | } // namespace gmmloc 141 | -------------------------------------------------------------------------------- /gmmloc/src/utils/math_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "gmmloc/utils/math_utils.h" 2 | 3 | namespace gmmloc { 4 | Matrix3d MathUtils::skew(const Vector3d &v) { 5 | Matrix3d m; 6 | m.fill(0.); 7 | m(0, 1) = -v(2); 8 | m(0, 2) = v(1); 9 | m(1, 2) = -v(0); 10 | m(1, 0) = v(2); 11 | m(2, 0) = -v(1); 12 | m(2, 1) = v(0); 13 | return m; 14 | } 15 | 16 | Matrix3d MathUtils::computeEssentialMatrix(const SE3Quat &Tcw1, 17 | const SE3Quat &Tcw2 18 | 19 | ) { 20 | 21 | Quaterniond rot_c1_w = Tcw1.rotation(); 22 | Vector3d t_c1_w = Tcw1.translation(); 23 | Quaterniond rot_c2_w = Tcw2.rotation(); 24 | Vector3d t_c2_w = Tcw2.translation(); 25 | 26 | Quaterniond rot_c1_c2 = rot_c1_w * rot_c2_w.conjugate(); 27 | Vector3d t_c1_c2 = -(rot_c1_c2 * t_c2_w) + t_c1_w; 28 | 29 | Matrix3d t12_skew = skew(t_c1_c2); 30 | 31 | return t12_skew * rot_c1_c2.toRotationMatrix(); 32 | } 33 | 34 | Matrix3d MathUtils::computeFundamentalMatrix(const SE3Quat &Tcw1, 35 | const Matrix3d &K1, 36 | const SE3Quat &Tcw2, 37 | const Matrix3d &K2 38 | 39 | ) { 40 | Matrix3d essential_mat = computeEssentialMatrix(Tcw1, Tcw2); 41 | 42 | return K1.transpose().inverse() * essential_mat * K2.inverse(); 43 | } 44 | 45 | } // namespace gmmloc 46 | -------------------------------------------------------------------------------- /gmmloc/src/utils/protobuf_utils.cpp: -------------------------------------------------------------------------------- 1 | // adapted from voxblox 2 | 3 | #include "gmmloc/utils/protobuf_utils.h" 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace gmmloc { 10 | 11 | namespace pb { 12 | bool readProtoMsgCountFromStream(std::fstream *stream_in, uint32_t *message_count, 13 | uint32_t *byte_offset) { 14 | CHECK_NOTNULL(stream_in); 15 | CHECK_NOTNULL(message_count); 16 | CHECK_NOTNULL(byte_offset); 17 | CHECK(stream_in->is_open()); 18 | stream_in->clear(); 19 | stream_in->seekg(*byte_offset, std::ios::beg); 20 | google::protobuf::io::IstreamInputStream raw_in(stream_in); 21 | google::protobuf::io::CodedInputStream coded_in(&raw_in); 22 | const int prev_position = coded_in.CurrentPosition(); 23 | if (!coded_in.ReadVarint32(message_count)) { 24 | LOG(ERROR) << "Could not read message size."; 25 | return false; 26 | } 27 | *byte_offset += coded_in.CurrentPosition() - prev_position; 28 | return true; 29 | } 30 | 31 | bool writeProtoMsgCountToStream(uint32_t message_count, 32 | std::fstream *stream_out) { 33 | CHECK_NOTNULL(stream_out); 34 | CHECK(stream_out->is_open()); 35 | stream_out->clear(); 36 | google::protobuf::io::OstreamOutputStream raw_out(stream_out); 37 | google::protobuf::io::CodedOutputStream coded_out(&raw_out); 38 | coded_out.WriteVarint32(message_count); 39 | return true; 40 | } 41 | 42 | bool readProtoMsgFromStream(std::fstream *stream_in, 43 | google::protobuf::Message *message, 44 | uint32_t *byte_offset) { 45 | CHECK_NOTNULL(stream_in); 46 | CHECK_NOTNULL(message); 47 | CHECK_NOTNULL(byte_offset); 48 | CHECK(stream_in->is_open()); 49 | 50 | stream_in->clear(); 51 | stream_in->seekg(*byte_offset, std::ios::beg); 52 | google::protobuf::io::IstreamInputStream raw_in(stream_in); 53 | google::protobuf::io::CodedInputStream coded_in(&raw_in); 54 | 55 | const int prev_position = coded_in.CurrentPosition(); 56 | 57 | uint32_t message_size; 58 | if (!coded_in.ReadVarint32(&message_size)) { 59 | LOG(ERROR) << "Could not read protobuf message size."; 60 | return false; 61 | } 62 | 63 | if (message_size == 0u) { 64 | LOG(ERROR) << "Empty protobuf message!"; 65 | return false; 66 | } 67 | google::protobuf::io::CodedInputStream::Limit limit = 68 | coded_in.PushLimit(message_size); 69 | if (!message->ParseFromCodedStream(&coded_in)) { 70 | LOG(ERROR) << "Could not parse stream."; 71 | return false; 72 | } 73 | if (!coded_in.ConsumedEntireMessage()) { 74 | LOG(ERROR) << "Could not consume protobuf message."; 75 | return false; 76 | } 77 | coded_in.PopLimit(limit); 78 | *byte_offset += coded_in.CurrentPosition() - prev_position; 79 | return true; 80 | } 81 | 82 | bool writeProtoMsgToStream(const google::protobuf::Message &message, 83 | std::fstream *stream_out) { 84 | CHECK_NOTNULL(stream_out); 85 | CHECK(stream_out->is_open()); 86 | google::protobuf::io::OstreamOutputStream raw_out(stream_out); 87 | google::protobuf::io::CodedOutputStream coded_out(&raw_out); 88 | const uint32_t size_bytes = message.ByteSize(); 89 | coded_out.WriteVarint32(size_bytes); 90 | uint8_t *buffer = coded_out.GetDirectBufferForNBytesAndAdvance(size_bytes); 91 | if (buffer != nullptr) { 92 | message.SerializeWithCachedSizesToArray(buffer); 93 | } else { 94 | message.SerializeWithCachedSizes(&coded_out); 95 | if (coded_out.HadError()) { 96 | return false; 97 | } 98 | } 99 | 100 | return true; 101 | } 102 | 103 | } // namespace pb 104 | 105 | } // namespace gmmloc 106 | -------------------------------------------------------------------------------- /gmmloc/src/utils/timing.cpp: -------------------------------------------------------------------------------- 1 | // adapted from voxblox 2 | 3 | #include "gmmloc/utils/timing.h" 4 | 5 | namespace gmmloc { 6 | 7 | namespace timing { 8 | const double kNumSecondsPerNanosecond = 1.e-9; 9 | 10 | Timing &Timing::Instance() { 11 | static Timing t; 12 | return t; 13 | } 14 | 15 | Timing::Timing() : maxTagLength_(0) {} 16 | 17 | Timing::~Timing() {} 18 | 19 | // Static functions to query the timers: 20 | size_t Timing::GetHandle(std::string const &tag) { 21 | std::lock_guard lock(Instance().mutex_); 22 | // Search for an existing tag. 23 | map_t::iterator i = Instance().tagMap_.find(tag); 24 | if (i == Instance().tagMap_.end()) { 25 | // If it is not there, create a tag. 26 | size_t handle = Instance().timers_.size(); 27 | Instance().tagMap_[tag] = handle; 28 | Instance().timers_.push_back(TimerMapValue()); 29 | // Track the maximum tag length to help printing a table of timing values 30 | // later. 31 | Instance().maxTagLength_ = std::max(Instance().maxTagLength_, tag.size()); 32 | return handle; 33 | } else { 34 | return i->second; 35 | } 36 | } 37 | 38 | std::string Timing::GetTag(size_t handle) { 39 | std::lock_guard lock(Instance().mutex_); 40 | std::string tag; 41 | 42 | // Perform a linear search for the tag. 43 | for (typename map_t::value_type current_tag : Instance().tagMap_) { 44 | if (current_tag.second == handle) { 45 | return current_tag.first; 46 | } 47 | } 48 | return tag; 49 | } 50 | 51 | // Class functions used for timing. 52 | Timer::Timer(size_t handle, bool constructStopped) 53 | : timing_(false), handle_(handle) { 54 | if (!constructStopped) 55 | Start(); 56 | } 57 | 58 | Timer::Timer(std::string const &tag, bool constructStopped) 59 | : timing_(false), handle_(Timing::GetHandle(tag)) { 60 | if (!constructStopped) 61 | Start(); 62 | } 63 | 64 | Timer::~Timer() { 65 | if (IsTiming()) 66 | Stop(); 67 | } 68 | 69 | void Timer::Start() { 70 | timing_ = true; 71 | time_ = std::chrono::system_clock::now(); 72 | } 73 | 74 | void Timer::Stop() { 75 | std::chrono::time_point now = 76 | std::chrono::system_clock::now(); 77 | double dt = 78 | static_cast( 79 | std::chrono::duration_cast(now - time_) 80 | .count()) * 81 | kNumSecondsPerNanosecond; 82 | 83 | Timing::Instance().AddTime(handle_, dt); 84 | timing_ = false; 85 | } 86 | 87 | bool Timer::IsTiming() const { return timing_; } 88 | 89 | void Timing::AddTime(size_t handle, double seconds) { 90 | std::lock_guard lock(Instance().mutex_); 91 | timers_[handle].acc_.Add(seconds); 92 | } 93 | 94 | double Timing::GetTotalSeconds(size_t handle) { 95 | std::lock_guard lock(Instance().mutex_); 96 | return Instance().timers_[handle].acc_.Sum(); 97 | } 98 | double Timing::GetTotalSeconds(std::string const &tag) { 99 | return GetTotalSeconds(GetHandle(tag)); 100 | } 101 | double Timing::GetMeanSeconds(size_t handle) { 102 | std::lock_guard lock(Instance().mutex_); 103 | return Instance().timers_[handle].acc_.Mean(); 104 | } 105 | double Timing::GetMeanSeconds(std::string const &tag) { 106 | return GetMeanSeconds(GetHandle(tag)); 107 | } 108 | size_t Timing::GetNumSamples(size_t handle) { 109 | return Instance().timers_[handle].acc_.TotalSamples(); 110 | } 111 | size_t Timing::GetNumSamples(std::string const &tag) { 112 | return GetNumSamples(GetHandle(tag)); 113 | } 114 | double Timing::GetVarianceSeconds(size_t handle) { 115 | std::lock_guard lock(Instance().mutex_); 116 | return Instance().timers_[handle].acc_.LazyVariance(); 117 | } 118 | double Timing::GetVarianceSeconds(std::string const &tag) { 119 | return GetVarianceSeconds(GetHandle(tag)); 120 | } 121 | double Timing::GetMinSeconds(size_t handle) { 122 | std::lock_guard lock(Instance().mutex_); 123 | return Instance().timers_[handle].acc_.Min(); 124 | } 125 | double Timing::GetMinSeconds(std::string const &tag) { 126 | return GetMinSeconds(GetHandle(tag)); 127 | } 128 | double Timing::GetMaxSeconds(size_t handle) { 129 | std::lock_guard lock(Instance().mutex_); 130 | return Instance().timers_[handle].acc_.Max(); 131 | } 132 | double Timing::GetMaxSeconds(std::string const &tag) { 133 | return GetMaxSeconds(GetHandle(tag)); 134 | } 135 | 136 | double Timing::GetHz(size_t handle) { 137 | std::lock_guard lock(Instance().mutex_); 138 | const double rolling_mean = Instance().timers_[handle].acc_.RollingMean(); 139 | CHECK_GT(rolling_mean, 0.0); 140 | return 1.0 / rolling_mean; 141 | } 142 | 143 | double Timing::GetHz(std::string const &tag) { return GetHz(GetHandle(tag)); } 144 | 145 | std::string Timing::SecondsToTimeString(double seconds) { 146 | char buffer[256]; 147 | snprintf(buffer, sizeof(buffer), "%09.6f", seconds); 148 | return buffer; 149 | } 150 | 151 | void Timing::Print(std::ostream &out) { 152 | map_t &tagMap = Instance().tagMap_; 153 | 154 | if (tagMap.empty()) { 155 | return; 156 | } 157 | 158 | out << "SM Timing\n"; 159 | out << "-----------\n"; 160 | for (typename map_t::value_type t : tagMap) { 161 | size_t i = t.second; 162 | out.width((std::streamsize)Instance().maxTagLength_); 163 | out.setf(std::ios::left, std::ios::adjustfield); 164 | out << t.first << "\t"; 165 | out.width(7); 166 | 167 | out.setf(std::ios::right, std::ios::adjustfield); 168 | out << GetNumSamples(i) << "\t"; 169 | if (GetNumSamples(i) > 0) { 170 | out << SecondsToTimeString(GetTotalSeconds(i)) << "\t"; 171 | double meansec = GetMeanSeconds(i); 172 | double stddev = sqrt(GetVarianceSeconds(i)); 173 | out << "(" << SecondsToTimeString(meansec) << " +- "; 174 | out << SecondsToTimeString(stddev) << ")\t"; 175 | 176 | double minsec = GetMinSeconds(i); 177 | double maxsec = GetMaxSeconds(i); 178 | 179 | // The min or max are out of bounds. 180 | out << "[" << SecondsToTimeString(minsec) << "," 181 | << SecondsToTimeString(maxsec) << "]"; 182 | } 183 | out << std::endl; 184 | } 185 | } 186 | std::string Timing::Print() { 187 | std::stringstream ss; 188 | Print(ss); 189 | return ss.str(); 190 | } 191 | 192 | void Timing::Reset() { 193 | std::lock_guard lock(Instance().mutex_); 194 | Instance().tagMap_.clear(); 195 | } 196 | } // namespace timing 197 | 198 | } // namespace gmmloc 199 | -------------------------------------------------------------------------------- /gmmloc/src/visualization/gmm_visualizer.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "gmmloc/visualization/gmm_visualizer.h" 3 | 4 | #include 5 | 6 | using namespace std; 7 | 8 | namespace gmmloc { 9 | 10 | GMMVisualizer::GMMVisualizer(GMM::ConstPtr model, const string topic_name, 11 | const string frame_id, const double cov_factor) 12 | : frame_id_(frame_id), msg_(nullptr), cov_factor_(cov_factor) { 13 | ros::NodeHandle nh("~"); 14 | 15 | pub_ = nh.advertise(topic_name, 1, true); 16 | 17 | republish(model); 18 | } 19 | 20 | void GMMVisualizer::republish(GMM::ConstPtr gmm) { 21 | 22 | const auto &model = gmm->getComponents(); 23 | 24 | if (msg_ == nullptr) { 25 | msg_ = 26 | visualization_msgs::MarkerArrayPtr(new visualization_msgs::MarkerArray); 27 | } 28 | 29 | // visualization_msgs::MarkerArray a; 30 | msg_->markers.clear(); 31 | auto stamp = ros::Time::now(); 32 | 33 | int count = 0; 34 | auto marker = visualization_msgs::Marker(); 35 | marker.header.frame_id = frame_id_; 36 | marker.header.seq = 0; 37 | marker.header.stamp = stamp; 38 | for (auto &g : model) { 39 | 40 | marker.id = count++; 41 | marker.type = visualization_msgs::Marker::SPHERE; 42 | marker.action = visualization_msgs::Marker::ADD; 43 | marker.scale.x = sqrt(g->scale_.x()) * cov_factor_; 44 | marker.scale.y = sqrt(g->scale_.y()) * cov_factor_; 45 | marker.scale.z = sqrt(g->scale_.z()) * cov_factor_; 46 | marker.color.a = 0.5; 47 | marker.color.r = 0.4; 48 | marker.color.g = 0.4; 49 | marker.color.b = 0.4; 50 | 51 | marker.pose.orientation.x = g->rot_.x(); 52 | marker.pose.orientation.y = g->rot_.y(); 53 | marker.pose.orientation.z = g->rot_.z(); 54 | marker.pose.orientation.w = g->rot_.w(); 55 | marker.pose.position.x = g->mean_.x(); 56 | marker.pose.position.y = g->mean_.y(); 57 | marker.pose.position.z = g->mean_.z(); 58 | 59 | msg_->markers.push_back(marker); 60 | } 61 | 62 | pub_.publish(msg_); 63 | } 64 | 65 | void GMMVisualizer::republish(GMM::ConstPtr gmm, 66 | const std::vector &indices) { 67 | std::unordered_set set; 68 | std::copy(indices.begin(), indices.end(), std::inserter(set, set.end())); 69 | 70 | const auto &model = gmm->getComponents(); 71 | 72 | if (msg_ == nullptr) { 73 | msg_ = 74 | visualization_msgs::MarkerArrayPtr(new visualization_msgs::MarkerArray); 75 | } 76 | 77 | // visualization_msgs::MarkerArray a; 78 | msg_->markers.clear(); 79 | auto stamp = ros::Time::now(); 80 | 81 | int count = 0; 82 | auto marker = visualization_msgs::Marker(); 83 | marker.header.frame_id = frame_id_; 84 | marker.header.seq = 0; 85 | marker.header.stamp = stamp; 86 | for (int i = 0; i < model.size(); i++) { 87 | auto &&g = model[i]; 88 | 89 | marker.id = count++; 90 | marker.type = visualization_msgs::Marker::SPHERE; 91 | marker.action = visualization_msgs::Marker::ADD; 92 | marker.scale.x = sqrt(g->scale_.x()) * cov_factor_; 93 | marker.scale.y = sqrt(g->scale_.y()) * cov_factor_; 94 | marker.scale.z = sqrt(g->scale_.z()) * cov_factor_; 95 | 96 | if (set.count(i)) { 97 | marker.color.r = 0.8; 98 | marker.color.g = 0.0; 99 | marker.color.b = 0.1; 100 | // marker.color.r = 0.7764705882352941; 101 | // marker.color.g = 0.15294117647058825; 102 | // marker.color.b = 0.1568627450980392; 103 | } else { 104 | marker.color.r = 0.4; 105 | marker.color.g = 0.4; 106 | marker.color.b = 0.4; 107 | } 108 | 109 | marker.color.a = 1.0; 110 | marker.pose.orientation.x = g->rot_.x(); 111 | marker.pose.orientation.y = g->rot_.y(); 112 | marker.pose.orientation.z = g->rot_.z(); 113 | marker.pose.orientation.w = g->rot_.w(); 114 | marker.pose.position.x = g->mean_.x(); 115 | marker.pose.position.y = g->mean_.y(); 116 | marker.pose.position.z = g->mean_.z(); 117 | 118 | msg_->markers.push_back(marker); 119 | } 120 | 121 | pub_.publish(msg_); 122 | } 123 | 124 | } // namespace gmmloc -------------------------------------------------------------------------------- /gmmloc_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | project(gmmloc_ros) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | find_package(catkin_simple REQUIRED) 7 | catkin_simple() 8 | 9 | cs_install() 10 | cs_export() 11 | -------------------------------------------------------------------------------- /gmmloc_ros/cfg/euroc_rect.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | LEFT.height: 480 4 | LEFT.width: 752 5 | LEFT.D: !!opencv-matrix 6 | rows: 1 7 | cols: 5 8 | dt: d 9 | data:[-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] 10 | LEFT.K: !!opencv-matrix 11 | rows: 3 12 | cols: 3 13 | dt: d 14 | data: [458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0] 15 | LEFT.R: !!opencv-matrix 16 | rows: 3 17 | cols: 3 18 | dt: d 19 | data: [0.999966347530033, -0.001422739138722922, 0.008079580483432283, 0.001365741834644127, 0.9999741760894847, 0.007055629199258132, -0.008089410156878961, -0.007044357138835809, 0.9999424675829176] 20 | LEFT.P: !!opencv-matrix 21 | rows: 3 22 | cols: 4 23 | dt: d 24 | data: [435.2046959714599, 0, 367.4517211914062, 0, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 25 | 26 | RIGHT.height: 480 27 | RIGHT.width: 752 28 | RIGHT.D: !!opencv-matrix 29 | rows: 1 30 | cols: 5 31 | dt: d 32 | data:[-0.28368365, 0.07451284, -0.00010473, -3.555907e-05, 0.0] 33 | RIGHT.K: !!opencv-matrix 34 | rows: 3 35 | cols: 3 36 | dt: d 37 | data: [457.587, 0.0, 379.999, 0.0, 456.134, 255.238, 0.0, 0.0, 1] 38 | RIGHT.R: !!opencv-matrix 39 | rows: 3 40 | cols: 3 41 | dt: d 42 | data: [0.9999633526194376, -0.003625811871560086, 0.007755443660172947, 0.003680398547259526, 0.9999684752771629, -0.007035845251224894, -0.007729688520722713, 0.007064130529506649, 0.999945173484644] 43 | RIGHT.P: !!opencv-matrix 44 | rows: 3 45 | cols: 4 46 | dt: d 47 | data: [435.2046959714599, 0, 367.4517211914062, -47.90639384423901, 0, 435.2046959714599, 252.2008514404297, 0, 0, 0, 1, 0] 48 | -------------------------------------------------------------------------------- /gmmloc_ros/cfg/v1.yaml: -------------------------------------------------------------------------------- 1 | # %YAML:1.0 2 | 3 | sensor_type: 1 4 | camera: 5 | fx: 435.2046959714599 6 | fy: 435.2046959714599 7 | cx: 367.4517211914062 8 | cy: 252.2008514404297 9 | 10 | # 0, 1, 2 11 | distortion_type: 0 12 | # distortion: [-0.158714574009141, 0.005655228335481,5.803507635175249e-04, -0.005116425300158, 0.018317072669110] 13 | fps: 20.0 14 | width: 752 15 | height: 480 16 | 17 | bf: 47.90639384423901 18 | 19 | do_equalization: true 20 | do_rectify: true 21 | 22 | frame: 23 | th_depth: 35.0 24 | num_features: 1200 25 | 26 | map: 27 | neighbor_dist_thresh : 2.5 28 | 29 | loc: 30 | tri_use_stereo: true 31 | tri_check_deg: true 32 | tri_lambda2: 400.0 33 | 34 | tri_check_str_chi2: true 35 | tri_str_thresh: 0.0064 36 | 37 | ba_lambda2: 400.0 38 | ba_verbose: false 39 | 40 | ba_first_as_prior: true 41 | 42 | -------------------------------------------------------------------------------- /gmmloc_ros/cfg/v2.yaml: -------------------------------------------------------------------------------- 1 | # %YAML:1.0 2 | 3 | sensor_type: 1 4 | camera: 5 | fx: 435.2046959714599 6 | fy: 435.2046959714599 7 | cx: 367.4517211914062 8 | cy: 252.2008514404297 9 | 10 | # 0, 1, 2 11 | distortion_type: 0 12 | # distortion: [-0.158714574009141, 0.005655228335481,5.803507635175249e-04, -0.005116425300158, 0.018317072669110] 13 | fps: 20.0 14 | width: 752 15 | height: 480 16 | 17 | bf: 47.90639384423901 18 | 19 | do_equalization: false 20 | do_rectify: true 21 | 22 | frame: 23 | th_depth: 35.0 24 | num_features: 1200 25 | 26 | map: 27 | neighbor_dist_thresh : 2.5 28 | 29 | loc: 30 | tri_use_stereo: true 31 | tri_check_deg: true 32 | tri_lambda2: 400.0 33 | 34 | tri_check_str_chi2: true 35 | tri_str_thresh: 0.0064 36 | 37 | ba_lambda2: 400.0 38 | ba_verbose: false 39 | 40 | ba_first_as_prior: true 41 | 42 | -------------------------------------------------------------------------------- /gmmloc_ros/cfg/viz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 363 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5826771855354309 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: mappoints 30 | - Class: rviz/Displays 31 | Help Height: 70 32 | Name: Displays 33 | Property Tree Widget: 34 | Expanded: 35 | - /Global Options1 36 | - /frames1 37 | - /frames1/Namespaces1 38 | - /mappoints1 39 | - /gmm_model1 40 | - /TF1 41 | - /TF1/Frames1 42 | - /TF1/Tree1 43 | - /traj_cam1 44 | - /traj_gt1 45 | Splitter Ratio: 0.5 46 | Tree Height: 728 47 | Preferences: 48 | PromptSaveOnExit: true 49 | Toolbars: 50 | toolButtonStyle: 2 51 | Visualization Manager: 52 | Class: "" 53 | Displays: 54 | - Alpha: 0.5 55 | Cell Size: 1 56 | Class: rviz/Grid 57 | Color: 160; 160; 164 58 | Enabled: false 59 | Line Style: 60 | Line Width: 0.029999999329447746 61 | Value: Lines 62 | Name: Grid 63 | Normal Cell Count: 0 64 | Offset: 65 | X: 0 66 | Y: 0 67 | Z: 0 68 | Plane: XY 69 | Plane Cell Count: 10 70 | Reference Frame: 71 | Value: false 72 | - Class: rviz/MarkerArray 73 | Enabled: true 74 | Marker Topic: /gmmloc/keyframes 75 | Name: frames 76 | Namespaces: 77 | edge: false 78 | keyframes: false 79 | Queue Size: 100 80 | Value: true 81 | - Alpha: 1 82 | Autocompute Intensity Bounds: true 83 | Autocompute Value Bounds: 84 | Max Value: 10 85 | Min Value: -10 86 | Value: true 87 | Axis: Z 88 | Channel Name: intensity 89 | Class: rviz/PointCloud2 90 | Color: 255; 255; 255 91 | Color Transformer: RGB8 92 | Decay Time: 0 93 | Enabled: true 94 | Invert Rainbow: false 95 | Max Color: 255; 255; 255 96 | Max Intensity: 4096 97 | Min Color: 0; 0; 0 98 | Min Intensity: 0 99 | Name: mappoints 100 | Position Transformer: XYZ 101 | Queue Size: 10 102 | Selectable: true 103 | Size (Pixels): 3 104 | Size (m): 0.03999999910593033 105 | Style: Flat Squares 106 | Topic: /gmmloc/mappoints 107 | Unreliable: false 108 | Use Fixed Frame: true 109 | Use rainbow: true 110 | Value: true 111 | - Class: rviz/MarkerArray 112 | Enabled: true 113 | Marker Topic: /gmmloc/gmm_model 114 | Name: gmm_model 115 | Namespaces: 116 | "": true 117 | Queue Size: 100 118 | Value: true 119 | - Class: rviz/TF 120 | Enabled: true 121 | Frame Timeout: 15 122 | Frames: 123 | All Enabled: false 124 | camera: 125 | Value: true 126 | map: 127 | Value: false 128 | world: 129 | Value: false 130 | Marker Scale: 4 131 | Name: TF 132 | Show Arrows: false 133 | Show Axes: true 134 | Show Names: false 135 | Tree: 136 | map: 137 | camera: 138 | {} 139 | world: 140 | {} 141 | Update Interval: 0 142 | Value: true 143 | - Alpha: 1 144 | Buffer Length: 1 145 | Class: rviz/Path 146 | Color: 31; 119; 180 147 | Enabled: true 148 | Head Diameter: 0.30000001192092896 149 | Head Length: 0.20000000298023224 150 | Length: 0.30000001192092896 151 | Line Style: Billboards 152 | Line Width: 0.05000000074505806 153 | Name: traj_cam 154 | Offset: 155 | X: 0 156 | Y: 0 157 | Z: 0 158 | Pose Color: 255; 85; 255 159 | Pose Style: None 160 | Radius: 0.029999999329447746 161 | Shaft Diameter: 0.10000000149011612 162 | Shaft Length: 0.10000000149011612 163 | Topic: /gmmloc/camera 164 | Unreliable: false 165 | Value: true 166 | - Alpha: 1 167 | Buffer Length: 1 168 | Class: rviz/Path 169 | Color: 44; 160; 44 170 | Enabled: true 171 | Head Diameter: 0.30000001192092896 172 | Head Length: 0.20000000298023224 173 | Length: 0.30000001192092896 174 | Line Style: Billboards 175 | Line Width: 0.05000000074505806 176 | Name: traj_gt 177 | Offset: 178 | X: 0 179 | Y: 0 180 | Z: 0 181 | Pose Color: 255; 85; 255 182 | Pose Style: None 183 | Radius: 0.029999999329447746 184 | Shaft Diameter: 0.10000000149011612 185 | Shaft Length: 0.10000000149011612 186 | Topic: /gmmloc/gt 187 | Unreliable: false 188 | Value: true 189 | Enabled: true 190 | Global Options: 191 | Background Color: 255; 255; 255 192 | Default Light: true 193 | Fixed Frame: map 194 | Frame Rate: 30 195 | Name: root 196 | Tools: 197 | - Class: rviz/Interact 198 | Hide Inactive Objects: true 199 | - Class: rviz/MoveCamera 200 | - Class: rviz/Select 201 | - Class: rviz/FocusCamera 202 | - Class: rviz/Measure 203 | - Class: rviz/SetInitialPose 204 | Theta std deviation: 0.2617993950843811 205 | Topic: /initialpose 206 | X std deviation: 0.5 207 | Y std deviation: 0.5 208 | - Class: rviz/SetGoal 209 | Topic: /move_base_simple/goal 210 | - Class: rviz/PublishPoint 211 | Single click: true 212 | Topic: /clicked_point 213 | Value: true 214 | Views: 215 | Current: 216 | Class: rviz/Orbit 217 | Distance: 15.851724624633789 218 | Enable Stereo Rendering: 219 | Stereo Eye Separation: 0.05999999865889549 220 | Stereo Focal Distance: 1 221 | Swap Stereo Eyes: false 222 | Value: false 223 | Focal Point: 224 | X: -0.6694507002830505 225 | Y: 0.9132702350616455 226 | Z: 0.3716384470462799 227 | Focal Shape Fixed Size: true 228 | Focal Shape Size: 0.05000000074505806 229 | Invert Z Axis: false 230 | Name: Current View 231 | Near Clip Distance: 0.009999999776482582 232 | Pitch: 0.8747971653938293 233 | Target Frame: tf2 234 | Value: Orbit (rviz) 235 | Yaw: 5.141366481781006 236 | Saved: ~ 237 | Window Geometry: 238 | Displays: 239 | collapsed: false 240 | Height: 1017 241 | Hide Left Dock: false 242 | Hide Right Dock: false 243 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035bfc0200000009fb0000001200530065006c0065006300740069006f006e000000003b0000035f0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b0000035f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006900730070006c006100790073010000003d0000035b000000c900ffffff00000001000001000000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000051e0000035b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 244 | Selection: 245 | collapsed: false 246 | Time: 247 | collapsed: false 248 | Tool Properties: 249 | collapsed: false 250 | Views: 251 | collapsed: false 252 | Width: 1920 253 | X: 1920 254 | Y: 27 255 | -------------------------------------------------------------------------------- /gmmloc_ros/data/map/v1.gmm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUSTGZ-IADC/gmmloc/08eb28140b100bd9f6c796b9d365f9a45fa560c2/gmmloc_ros/data/map/v1.gmm -------------------------------------------------------------------------------- /gmmloc_ros/data/map/v2.gmm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUSTGZ-IADC/gmmloc/08eb28140b100bd9f6c796b9d365f9a45fa560c2/gmmloc_ros/data/map/v2.gmm -------------------------------------------------------------------------------- /gmmloc_ros/launch/v1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /gmmloc_ros/launch/v2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /gmmloc_ros/launch/viz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /gmmloc_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gmmloc_ros 4 | 0.0.0 5 | gmmloc ros wrapper 6 | 7 | hyhuang 8 | 9 | GPL-v3 10 | 11 | catkin 12 | catkin_simple 13 | 14 | gmmloc 15 | 16 | -------------------------------------------------------------------------------- /gmmloc_ros/scripts/evaluate_euroc.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | # TODO: generic script for both zsh && bash 4 | NUM_TEST=5 5 | DATA_PATH=/PATH/TO/EUROC/DATASET 6 | V1_SEQUENCES=('V1_01_easy' 'V1_02_medium' 'V1_03_difficult') 7 | # V1_SEQUENCES=('V1_01_easy') 8 | V2_SEQUENCES=('V2_01_easy' 'V2_02_medium') 9 | # V2_SEQUENCES=('V2_02_medium') 10 | 11 | trap ctrl_c INT 12 | 13 | function ctrl_c() { 14 | kill %1 15 | exit 16 | } 17 | 18 | function srcc() { 19 | prefix='' 20 | if [ "$#" -eq 1 ]; then 21 | prefix="_$1" 22 | print $prefix 23 | fi 24 | 25 | cwd=`pwd` 26 | cdir=`pwd` 27 | while [ $cdir != $HOME ]; do 28 | dir_to_check="$cdir/devel$prefix" 29 | if [ -d $dir_to_check ]; then 30 | source "$dir_to_check/setup.bash"; 31 | echo "source $dir_to_check/setup.bash"; 32 | break; 33 | fi 34 | cd .. 35 | cdir=`pwd` 36 | done 37 | if [ $cdir = $HOME ]; then 38 | echo "failed to find a catkin... " 39 | fi 40 | 41 | cd $cwd 42 | } 43 | 44 | 45 | rm -rf $(rospack find gmmloc_ros)/expr/ 46 | mkdir -p $(rospack find gmmloc_ros)/expr/ 47 | 48 | srcc 49 | 50 | roscore & 51 | 52 | sleep 2 53 | 54 | rosparam set /gmmloc/vocabulary_path $(rospack find gmmloc_ros)/voc/ORBvoc.bin 55 | rosparam set /gmmloc/output_path $(rospack find gmmloc_ros)/expr/ 56 | 57 | rosparam set /gmmloc/gmm_path $(rospack find gmmloc_ros)/data/map/v1.gmm 58 | rosparam set /gmmloc/rect_config $(rospack find gmmloc_ros)/cfg/euroc_rect.yaml 59 | 60 | # rosparam set /gmmloc/online True 61 | 62 | rosparam load $(rospack find gmmloc_ros)/cfg/v1.yaml /gmmloc 63 | expr_path=$(rospack find gmmloc_ros)/expr/ 64 | 65 | for seq in ${V1_SEQUENCES[*]} 66 | do 67 | rosparam set /gmmloc/data_path "$DATA_PATH/$seq/mav0/" 68 | rosparam set /gmmloc/gt_path $(rospack find gmmloc_ros)/data/gt_sync/$seq.txt 69 | 70 | for (( i=1; i<=$NUM_TEST; i++ )) 71 | do 72 | echo 73 | echo '#####################################################################' 74 | echo "$seq, $i/$NUM_TEST" 75 | 76 | rosrun gmmloc gmmloc_node __name:=gmmloc -alsologtostderr -colorlogtostderr --minloglevel=2 77 | 78 | mv $expr_path/traj_est.txt $expr_path/$seq$i.txt 79 | done 80 | done 81 | 82 | rosparam set /gmmloc/gmm_path $(rospack find gmmloc_ros)/data/map/v2.gmm 83 | rosparam set /gmmloc/rect_config $(rospack find gmmloc_ros)/cfg/euroc_rect.yaml 84 | 85 | rosparam load $(rospack find gmmloc_ros)/cfg/v2.yaml /gmmloc 86 | expr_path=$(rospack find gmmloc_ros) 87 | 88 | for seq in ${V2_SEQUENCES[*]} 89 | do 90 | rosparam set /gmmloc/data_path "$DATA_PATH/$seq/mav0/" 91 | rosparam set /gmmloc/gt_path $(rospack find gmmloc_ros)/data/gt_sync/$seq.txt 92 | 93 | for (( i=1; i<=$NUM_TEST; i++ )) 94 | do 95 | echo 96 | echo '#####################################################################' 97 | echo "$seq, $i/$NUM_TEST" 98 | 99 | rosrun gmmloc gmmloc_node __name:=gmmloc -alsologtostderr -colorlogtostderr --minloglevel=2 100 | 101 | mv $expr_path/expr/traj_est.txt $expr_path/expr/$seq$i.txt 102 | done 103 | done 104 | 105 | python3 scripts/evo_euroc.py --pkg_path $(rospack find gmmloc_ros) 106 | 107 | kill %1 108 | 109 | -------------------------------------------------------------------------------- /gmmloc_ros/scripts/evo_euroc.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | 3 | import evo 4 | 5 | import argparse 6 | import glob 7 | 8 | from pathlib import Path 9 | 10 | from evo.tools import file_interface 11 | from evo.core import metrics, sync, trajectory 12 | 13 | import numpy as np 14 | 15 | parser = argparse.ArgumentParser() 16 | 17 | parser.add_argument("--pkg_path", type=str) 18 | args = parser.parse_args() 19 | 20 | gt_path = Path(args.pkg_path) / "data/gt" 21 | data_path = Path(args.pkg_path) / "expr" 22 | 23 | # a = 24 | # print(a) 25 | 26 | # exit() 27 | 28 | pose_relation = metrics.PoseRelation.translation_part 29 | 30 | seqs = ['V1_01_easy', 'V1_02_medium', 'V1_03_difficult', 31 | 'V2_01_easy', 'V2_02_medium', 'V2_03_difficult'] 32 | 33 | # for delta in deltas: 34 | # res_final = [] 35 | for seq in seqs: 36 | traj_files = glob.glob(str(data_path / f'{seq}?.txt')) 37 | gt_file = f'{gt_path}/{seq}.csv' 38 | 39 | if not len(traj_files) == 0: 40 | mean, rmse = [], [] 41 | for traj_file in traj_files: 42 | traj_gt = file_interface.read_euroc_csv_trajectory(gt_file) 43 | traj_est = file_interface.read_tum_trajectory_file(traj_file) 44 | 45 | traj_gt, traj_est = sync.associate_trajectories(traj_gt, traj_est) 46 | traj_est_aligned, rot, trans, scale = trajectory.align_trajectory( 47 | traj_est, traj_gt, correct_scale=True, return_parameters=True) 48 | 49 | data = (traj_gt, traj_est_aligned) 50 | ape_metric = metrics.APE(pose_relation=pose_relation) 51 | 52 | ape_metric.process_data(data) 53 | ape_stat = ape_metric.get_all_statistics() 54 | mean.append(ape_stat['mean']) 55 | rmse.append(ape_stat['rmse']) 56 | 57 | print(f'{seq}: mean: {np.mean(mean)}, rmse: {np.mean(rmse)}') 58 | else: 59 | print(f'{seq} empty') 60 | 61 | -------------------------------------------------------------------------------- /gmmloc_ros/voc/ORBvoc.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUSTGZ-IADC/gmmloc/08eb28140b100bd9f6c796b9d365f9a45fa560c2/gmmloc_ros/voc/ORBvoc.bin -------------------------------------------------------------------------------- /orb_dbow2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(orb_dbow2) 3 | 4 | # Set optimized building: 5 | if(MSVC_IDE) 6 | add_definitions(-D_CRT_SECURE_NO_WARNINGS) 7 | else(MSVC_IDE) 8 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") 10 | endif(MSVC_IDE) 11 | 12 | set(HDRS_DBOW2 13 | include/dbow2/BowVector.h 14 | include/dbow2/FORB.h 15 | include/dbow2/FClass.h 16 | include/dbow2/FeatureVector.h 17 | include/dbow2/ScoringObject.h 18 | include/dbow2/TemplatedVocabulary.h) 19 | set(SRCS_DBOW2 20 | src/dbow2/BowVector.cpp 21 | src/dbow2/FORB.cpp 22 | src/dbow2/FeatureVector.cpp 23 | src/dbow2/ScoringObject.cpp) 24 | 25 | set(HDRS_DUTILS 26 | include/dutils/Random.h 27 | include/dutils/Timestamp.h) 28 | set(SRCS_DUTILS 29 | src/dutils/Random.cpp 30 | src/dutils/Timestamp.cpp) 31 | 32 | find_package(catkin_simple REQUIRED) 33 | catkin_simple() 34 | 35 | find_package(OpenCV REQUIRED) 36 | 37 | # catkin_package( 38 | # INCLUDE_DIRS include 39 | # LIBRARIES ${PROJECT_NAME} 40 | # CATKIN_DEPENDS 41 | # DEPENDS OpenCV 42 | # ) 43 | 44 | # set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 45 | 46 | include_directories( 47 | include 48 | ${catkin_INCLUDE_DIRS} 49 | ${OpenCV_INCLUDE_DIRS} 50 | ) 51 | 52 | cs_add_library(${PROJECT_NAME} SHARED 53 | ${SRCS_DBOW2} 54 | ${SRCS_DUTILS} 55 | ) 56 | target_link_libraries(${PROJECT_NAME} 57 | ${OpenCV_LIBS} 58 | ${catkin_INCLUDE_DIRS} 59 | ) 60 | 61 | cs_install() 62 | cs_export() 63 | -------------------------------------------------------------------------------- /orb_dbow2/LICENSE.txt: -------------------------------------------------------------------------------- 1 | DBoW2: bag-of-words library for C++ with generic descriptors 2 | 3 | Copyright (c) 2015 Dorian Galvez-Lopez (Universidad de Zaragoza) 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | 3. Neither the name of copyright holders nor the names of its 15 | contributors may be used to endorse or promote products derived 16 | from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 20 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 22 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | If you use it in an academic work, please cite: 31 | 32 | @ARTICLE{GalvezTRO12, 33 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 34 | journal={IEEE Transactions on Robotics}, 35 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 36 | year={2012}, 37 | month={October}, 38 | volume={28}, 39 | number={5}, 40 | pages={1188--1197}, 41 | doi={10.1109/TRO.2012.2197158}, 42 | ISSN={1552-3098} 43 | } 44 | 45 | -------------------------------------------------------------------------------- /orb_dbow2/README.txt: -------------------------------------------------------------------------------- 1 | You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2). 2 | See the original DBoW2 library at: https://github.com/dorian3d/DBoW2 3 | All files included in this version are BSD, see LICENSE.txt 4 | 5 | We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils. 6 | See the original DLib library at: https://github.com/dorian3d/DLib 7 | All files included in this version are BSD, see LICENSE.txt 8 | -------------------------------------------------------------------------------- /orb_dbow2/include/orb_dbow2/dbow2/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "../dutils/config.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Id of words 22 | typedef unsigned int WordId; 23 | 24 | /// Value of a word 25 | typedef double WordValue; 26 | 27 | /// Id of nodes in the vocabulary treee 28 | typedef unsigned int NodeId; 29 | 30 | /// L-norms for normalization 31 | EXPORT typedef enum LNorm 32 | { 33 | L1, 34 | L2 35 | } LNorm; 36 | 37 | /// Weighting type 38 | EXPORT typedef enum WeightingType 39 | { 40 | TF_IDF, 41 | TF, 42 | IDF, 43 | BINARY 44 | } WeightingType; 45 | 46 | /// Scoring type 47 | EXPORT typedef enum ScoringType 48 | { 49 | L1_NORM, 50 | L2_NORM, 51 | CHI_SQUARE, 52 | KL, 53 | BHATTACHARYYA, 54 | DOT_PRODUCT, 55 | } ScoringType; 56 | 57 | /// Vector of words to represent images 58 | /// stl的map结构，key为wordId，value为tfidf中的tf 59 | class EXPORT BowVector: 60 | public std::map 61 | { 62 | public: 63 | 64 | /** 65 | * Constructor 66 | */ 67 | BowVector(void); 68 | 69 | /** 70 | * Destructor 71 | */ 72 | ~BowVector(void); 73 | 74 | /** 75 | * Adds a value to a word value existing in the vector, or creates a new 76 | * word with the given value 77 | * @param id word id to look for 78 | * @param v value to create the word with, or to add to existing word 79 | */ 80 | void addWeight(WordId id, WordValue v); 81 | 82 | /** 83 | * Adds a word with a value to the vector only if this does not exist yet 84 | * @param id word id to look for 85 | * @param v value to give to the word if this does not exist 86 | */ 87 | void addIfNotExist(WordId id, WordValue v); 88 | 89 | /** 90 | * L1-Normalizes the values in the vector 91 | * @param norm_type norm used 92 | */ 93 | void normalize(LNorm norm_type); 94 | 95 | /** 96 | * Prints the content of the bow vector 97 | * @param out stream 98 | * @param v 99 | */ 100 | friend std::ostream& operator<<(std::ostream &out, const BowVector &v); 101 | 102 | /** 103 | * Saves the bow vector as a vector in a matlab file 104 | * @param filename 105 | * @param W number of words in the vocabulary 106 | */ 107 | void saveM(const std::string &filename, size_t W) const; 108 | }; 109 | 110 | } // namespace DBoW2 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /orb_dbow2/include/orb_dbow2/dbow2/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "../dutils/config.h" 18 | 19 | namespace DBoW2 { 20 | 21 | /// Generic class to encapsulate functions to manage descriptors. 22 | /** 23 | * This class must be inherited. Derived classes can be used as the 24 | * parameter F when creating Templated structures 25 | * (TemplatedVocabulary, TemplatedDatabase, ...) 26 | */ 27 | class EXPORT FClass 28 | { 29 | class TDescriptor; 30 | typedef const TDescriptor *pDescriptor; 31 | 32 | /** 33 | * Calculates the mean value of a set of descriptors 34 | * @param descriptors 35 | * @param mean mean descriptor 36 | */ 37 | virtual void meanValue(const std::vector &descriptors, 38 | TDescriptor &mean) = 0; 39 | 40 | /** 41 | * Calculates the distance between two descriptors 42 | * @param a 43 | * @param b 44 | * @return distance 45 | */ 46 | static double distance(const TDescriptor &a, const TDescriptor &b); 47 | 48 | /** 49 | * Returns a string version of the descriptor 50 | * @param a descriptor 51 | * @return string version 52 | */ 53 | static std::string toString(const TDescriptor &a); 54 | 55 | /** 56 | * Returns a descriptor from a string 57 | * @param a descriptor 58 | * @param s string version 59 | */ 60 | static void fromString(TDescriptor &a, const std::string &s); 61 | 62 | /** 63 | * Returns a mat with the descriptors in float format 64 | * @param descriptors 65 | * @param mat (out) NxL 32F matrix 66 | */ 67 | static void toMat32F(const std::vector &descriptors, 68 | cv::Mat &mat); 69 | }; 70 | 71 | } // namespace DBoW2 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /orb_dbow2/include/orb_dbow2/dbow2/FORB.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.h 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_ORB__ 11 | #define __D_T_F_ORB__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "FClass.h" 18 | 19 | #include "../dutils/config.h" 20 | 21 | namespace DBoW2 { 22 | 23 | /// Functions to manipulate ORB descriptors 24 | class EXPORT FORB : protected FClass 25 | { 26 | public: 27 | 28 | /// Descriptor type 29 | typedef cv::Mat TDescriptor; // CV_8U 30 | /// Pointer to a single descriptor 31 | typedef const TDescriptor *pDescriptor; 32 | /// Descriptor length (in bytes) 33 | static const int L = 32; 34 | 35 | /** 36 | * Calculates the mean value of a set of descriptors 37 | * @param descriptors 38 | * @param mean mean descriptor 39 | */ 40 | static void meanValue(const std::vector &descriptors, 41 | TDescriptor &mean); 42 | 43 | /** 44 | * Calculates the distance between two descriptors 45 | * @param a 46 | * @param b 47 | * @return distance 48 | */ 49 | static int distance(const TDescriptor &a, const TDescriptor &b); 50 | 51 | /** 52 | * Returns a string version of the descriptor 53 | * @param a descriptor 54 | * @return string version 55 | */ 56 | static std::string toString(const TDescriptor &a); 57 | 58 | /** 59 | * Returns a descriptor from a string 60 | * @param a descriptor 61 | * @param s string version 62 | */ 63 | static void fromString(TDescriptor &a, const std::string &s); 64 | 65 | /** 66 | * Returns a mat with the descriptors in float format 67 | * @param descriptors 68 | * @param mat (out) NxL 32F matrix 69 | */ 70 | static void toMat32F(const std::vector &descriptors, 71 | cv::Mat &mat); 72 | 73 | static void toMat8U(const std::vector &descriptors, 74 | cv::Mat &mat); 75 | 76 | }; 77 | 78 | } // namespace DBoW2 79 | 80 | #endif 81 | 82 | -------------------------------------------------------------------------------- /orb_dbow2/include/orb_dbow2/dbow2/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include "BowVector.h" 14 | #include 15 | #include 16 | #include 17 | 18 | #include "../dutils/config.h" 19 | 20 | namespace DBoW2 { 21 | 22 | /// Vector of nodes with indexes of local features 23 | class EXPORT FeatureVector: 24 | public std::map > 25 | { 26 | public: 27 | 28 | /** 29 | * Constructor 30 | */ 31 | FeatureVector(void); 32 | 33 | /** 34 | * Destructor 35 | */ 36 | ~FeatureVector(void); 37 | 38 | /** 39 | * Adds a feature to an existing node, or adds a new node with an initial 40 | * feature 41 | * @param id node id to add or to modify 42 | * @param i_feature index of feature to add to the given node 43 | */ 44 | void addFeature(NodeId id, unsigned int i_feature); 45 | 46 | /** 47 | * Sends a string versions of the feature vector through the stream 48 | * @param out stream 49 | * @param v feature vector 50 | */ 51 | friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v); 52 | 53 | }; 54 | 55 | } // namespace DBoW2 56 | 57 | #endif 58 | 59 | -------------------------------------------------------------------------------- /orb_dbow2/include/orb_dbow2/dbow2/ScoringObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: ScoringObject.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions to compute bow scores 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_SCORING_OBJECT__ 11 | #define __D_T_SCORING_OBJECT__ 12 | 13 | #include "BowVector.h" 14 | 15 | #include "../dutils/config.h" 16 | 17 | namespace DBoW2 { 18 | 19 | /// Base class of scoring functions 20 | class EXPORT GeneralScoring 21 | { 22 | public: 23 | /** 24 | * Computes the score between two vectors. Vectors must be sorted and 25 | * normalized if necessary 26 | * @param v (in/out) 27 | * @param w (in/out) 28 | * @return score 29 | */ 30 | virtual double score(const BowVector &v, const BowVector &w) const = 0; 31 | 32 | /** 33 | * Returns whether a vector must be normalized before scoring according 34 | * to the scoring scheme 35 | * @param norm norm to use 36 | * @return true iff must normalize 37 | */ 38 | virtual bool mustNormalize(LNorm &norm) const = 0; 39 | 40 | /// Log of epsilon 41 | static const double LOG_EPS; 42 | // If you change the type of WordValue, make sure you change also the 43 | // epsilon value (this is needed by the KL method) 44 | 45 | virtual ~GeneralScoring() {} //!< Required for virtual base classes 46 | 47 | }; 48 | 49 | /** 50 | * Macro for defining Scoring classes 51 | * @param NAME name of class 52 | * @param MUSTNORMALIZE if vectors must be normalized to compute the score 53 | * @param NORM type of norm to use when MUSTNORMALIZE 54 | */ 55 | #define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \ 56 | NAME: public GeneralScoring \ 57 | { public: \ 58 | /** \ 59 | * Computes score between two vectors \ 60 | * @param v \ 61 | * @param w \ 62 | * @return score between v and w \ 63 | */ \ 64 | virtual double score(const BowVector &v, const BowVector &w) const; \ 65 | \ 66 | /** \ 67 | * Says if a vector must be normalized according to the scoring function \ 68 | * @param norm (out) if true, norm to use 69 | * @return true iff vectors must be normalized \ 70 | */ \ 71 | virtual inline bool mustNormalize(LNorm &norm) const \ 72 | { norm = NORM; return MUSTNORMALIZE; } \ 73 | } 74 | 75 | /// L1 Scoring object 76 | class EXPORT __SCORING_CLASS(L1Scoring, true, L1); 77 | 78 | /// L2 Scoring object 79 | class EXPORT __SCORING_CLASS(L2Scoring, true, L2); 80 | 81 | /// Chi square Scoring object 82 | class EXPORT __SCORING_CLASS(ChiSquareScoring, true, L1); 83 | 84 | /// KL divergence Scoring object 85 | class EXPORT __SCORING_CLASS(KLScoring, true, L1); 86 | 87 | /// Bhattacharyya Scoring object 88 | class EXPORT __SCORING_CLASS(BhattacharyyaScoring, true, L1); 89 | 90 | /// Dot product Scoring object 91 | class EXPORT __SCORING_CLASS(DotProductScoring, false, L1); 92 | 93 | #undef __SCORING_CLASS 94 | 95 | } // namespace DBoW2 96 | 97 | #endif 98 | 99 | -------------------------------------------------------------------------------- /orb_dbow2/include/orb_dbow2/dutils/Random.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010, November 2011 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | #ifndef __D_RANDOM__ 13 | #define __D_RANDOM__ 14 | 15 | #include 16 | #include 17 | 18 | #include "config.h" 19 | 20 | namespace DUtils { 21 | 22 | /// Functions to generate pseudo-random numbers 23 | class EXPORT Random 24 | { 25 | public: 26 | class UnrepeatedRandomizer; 27 | 28 | public: 29 | /** 30 | * Sets the random number seed to the current time 31 | */ 32 | static void SeedRand(); 33 | 34 | /** 35 | * Sets the random number seed to the current time only the first 36 | * time this function is called 37 | */ 38 | static void SeedRandOnce(); 39 | 40 | /** 41 | * Sets the given random number seed 42 | * @param seed 43 | */ 44 | static void SeedRand(int seed); 45 | 46 | /** 47 | * Sets the given random number seed only the first time this function 48 | * is called 49 | * @param seed 50 | */ 51 | static void SeedRandOnce(int seed); 52 | 53 | /** 54 | * Returns a random number in the range [0..1] 55 | * @return random T number in [0..1] 56 | */ 57 | template 58 | static T RandomValue(){ 59 | return (T)rand()/(T)RAND_MAX; 60 | } 61 | 62 | /** 63 | * Returns a random number in the range [min..max] 64 | * @param min 65 | * @param max 66 | * @return random T number in [min..max] 67 | */ 68 | template 69 | static T RandomValue(T min, T max){ 70 | return Random::RandomValue() * (max - min) + min; 71 | } 72 | 73 | /** 74 | * Returns a random int in the range [min..max] 75 | * @param min 76 | * @param max 77 | * @return random int in [min..max] 78 | */ 79 | static int RandomInt(int min, int max); 80 | 81 | /** 82 | * Returns a random number from a gaussian distribution 83 | * @param mean 84 | * @param sigma standard deviation 85 | */ 86 | template 87 | static T RandomGaussianValue(T mean, T sigma) 88 | { 89 | // Box-Muller transformation 90 | T x1, x2, w, y1; 91 | 92 | do { 93 | x1 = (T)2. * RandomValue() - (T)1.; 94 | x2 = (T)2. * RandomValue() - (T)1.; 95 | w = x1 * x1 + x2 * x2; 96 | } while ( w >= (T)1. || w == (T)0. ); 97 | 98 | w = sqrt( ((T)-2.0 * log( w ) ) / w ); 99 | y1 = x1 * w; 100 | 101 | return( mean + y1 * sigma ); 102 | } 103 | 104 | private: 105 | 106 | /// If SeedRandOnce() or SeedRandOnce(int) have already been called 107 | static bool m_already_seeded; 108 | 109 | }; 110 | 111 | // --------------------------------------------------------------------------- 112 | 113 | /// Provides pseudo-random numbers with no repetitions 114 | class Random::UnrepeatedRandomizer 115 | { 116 | public: 117 | 118 | /** 119 | * Creates a randomizer that returns numbers in the range [min, max] 120 | * @param min 121 | * @param max 122 | */ 123 | UnrepeatedRandomizer(int min, int max); 124 | ~UnrepeatedRandomizer(){} 125 | 126 | /** 127 | * Copies a randomizer 128 | * @param rnd 129 | */ 130 | UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd); 131 | 132 | /** 133 | * Copies a randomizer 134 | * @param rnd 135 | */ 136 | UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd); 137 | 138 | /** 139 | * Returns a random number not given before. If all the possible values 140 | * were already given, the process starts again 141 | * @return unrepeated random number 142 | */ 143 | int get(); 144 | 145 | /** 146 | * Returns whether all the possible values between min and max were 147 | * already given. If get() is called when empty() is true, the behaviour 148 | * is the same than after creating the randomizer 149 | * @return true iff all the values were returned 150 | */ 151 | inline bool empty() const { return m_values.empty(); } 152 | 153 | /** 154 | * Returns the number of values still to be returned 155 | * @return amount of values to return 156 | */ 157 | inline unsigned int left() const { return m_values.size(); } 158 | 159 | /** 160 | * Resets the randomizer as it were just created 161 | */ 162 | void reset(); 163 | 164 | protected: 165 | 166 | /** 167 | * Creates the vector with available values 168 | */ 169 | void createValues(); 170 | 171 | protected: 172 | 173 | /// Min of range of values 174 | int m_min; 175 | /// Max of range of values 176 | int m_max; 177 | 178 | /// Available values 179 | std::vector m_values; 180 | 181 | }; 182 | 183 | } 184 | 185 | #endif 186 | 187 | -------------------------------------------------------------------------------- /orb_dbow2/include/orb_dbow2/dutils/Timestamp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.h 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_TIMESTAMP__ 11 | #define __D_TIMESTAMP__ 12 | 13 | #include 14 | 15 | #include "config.h" 16 | 17 | using namespace std; 18 | 19 | namespace DUtils { 20 | 21 | /// Timestamp 22 | class EXPORT Timestamp 23 | { 24 | public: 25 | 26 | /// Options to initiate a timestamp 27 | enum tOptions 28 | { 29 | NONE = 0, 30 | CURRENT_TIME = 0x1, 31 | ZERO = 0x2 32 | }; 33 | 34 | public: 35 | 36 | /** 37 | * Creates a timestamp 38 | * @param option option to set the initial time stamp 39 | */ 40 | Timestamp(Timestamp::tOptions option = NONE); 41 | 42 | /** 43 | * Destructor 44 | */ 45 | virtual ~Timestamp(void); 46 | 47 | /** 48 | * Says if the timestamp is "empty": seconds and usecs are both 0, as 49 | * when initiated with the ZERO flag 50 | * @return true iif secs == usecs == 0 51 | */ 52 | bool empty() const; 53 | 54 | /** 55 | * Sets this instance to the current time 56 | */ 57 | void setToCurrentTime(); 58 | 59 | /** 60 | * Sets the timestamp from seconds and microseconds 61 | * @param secs: seconds 62 | * @param usecs: microseconds 63 | */ 64 | inline void setTime(unsigned long secs, unsigned long usecs){ 65 | m_secs = secs; 66 | m_usecs = usecs; 67 | } 68 | 69 | /** 70 | * Returns the timestamp in seconds and microseconds 71 | * @param secs seconds 72 | * @param usecs microseconds 73 | */ 74 | inline void getTime(unsigned long &secs, unsigned long &usecs) const 75 | { 76 | secs = m_secs; 77 | usecs = m_usecs; 78 | } 79 | 80 | /** 81 | * Sets the timestamp from a string with the time in seconds 82 | * @param stime: string such as "1235603336.036609" 83 | */ 84 | void setTime(const string &stime); 85 | 86 | /** 87 | * Sets the timestamp from a number of seconds from the epoch 88 | * @param s seconds from the epoch 89 | */ 90 | void setTime(double s); 91 | 92 | /** 93 | * Returns this timestamp as the number of seconds in (long) float format 94 | */ 95 | double getFloatTime() const; 96 | 97 | /** 98 | * Returns this timestamp as the number of seconds in fixed length string format 99 | */ 100 | string getStringTime() const; 101 | 102 | /** 103 | * Returns the difference in seconds between this timestamp (greater) and t (smaller) 104 | * If the order is swapped, a negative number is returned 105 | * @param t: timestamp to subtract from this timestamp 106 | * @return difference in seconds 107 | */ 108 | double operator- (const Timestamp &t) const; 109 | 110 | /** 111 | * Returns a copy of this timestamp + s seconds + us microseconds 112 | * @param s seconds 113 | * @param us microseconds 114 | */ 115 | Timestamp plus(unsigned long s, unsigned long us) const; 116 | 117 | /** 118 | * Returns a copy of this timestamp - s seconds - us microseconds 119 | * @param s seconds 120 | * @param us microseconds 121 | */ 122 | Timestamp minus(unsigned long s, unsigned long us) const; 123 | 124 | /** 125 | * Adds s seconds to this timestamp and returns a reference to itself 126 | * @param s seconds 127 | * @return reference to this timestamp 128 | */ 129 | Timestamp& operator+= (double s); 130 | 131 | /** 132 | * Substracts s seconds to this timestamp and returns a reference to itself 133 | * @param s seconds 134 | * @return reference to this timestamp 135 | */ 136 | Timestamp& operator-= (double s); 137 | 138 | /** 139 | * Returns a copy of this timestamp + s seconds 140 | * @param s: seconds 141 | */ 142 | Timestamp operator+ (double s) const; 143 | 144 | /** 145 | * Returns a copy of this timestamp - s seconds 146 | * @param s: seconds 147 | */ 148 | Timestamp operator- (double s) const; 149 | 150 | /** 151 | * Returns whether this timestamp is at the future of t 152 | * @param t 153 | */ 154 | bool operator> (const Timestamp &t) const; 155 | 156 | /** 157 | * Returns whether this timestamp is at the future of (or is the same as) t 158 | * @param t 159 | */ 160 | bool operator>= (const Timestamp &t) const; 161 | 162 | /** 163 | * Returns whether this timestamp and t represent the same instant 164 | * @param t 165 | */ 166 | bool operator== (const Timestamp &t) const; 167 | 168 | /** 169 | * Returns whether this timestamp is at the past of t 170 | * @param t 171 | */ 172 | bool operator< (const Timestamp &t) const; 173 | 174 | /** 175 | * Returns whether this timestamp is at the past of (or is the same as) t 176 | * @param t 177 | */ 178 | bool operator<= (const Timestamp &t) const; 179 | 180 | /** 181 | * Returns the timestamp in a human-readable string 182 | * @param machine_friendly if true, the returned string is formatted 183 | * to yyyymmdd_hhmmss, without weekday or spaces 184 | * @note This has not been tested under Windows 185 | * @note The timestamp is truncated to seconds 186 | */ 187 | string Format(bool machine_friendly = false) const; 188 | 189 | /** 190 | * Returns a string version of the elapsed time in seconds, with the format 191 | * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us 192 | * @param s: elapsed seconds (given by getFloatTime) to format 193 | */ 194 | static string Format(double s); 195 | 196 | 197 | protected: 198 | /// Seconds 199 | unsigned long m_secs; // seconds 200 | /// Microseconds 201 | unsigned long m_usecs; // microseconds 202 | }; 203 | 204 | } 205 | 206 | #endif 207 | 208 | -------------------------------------------------------------------------------- /orb_dbow2/include/orb_dbow2/dutils/config.h: -------------------------------------------------------------------------------- 1 | #ifndef CONFIG_H 2 | #define CONFIG_H 3 | 4 | #ifdef WIN32 5 | #ifndef DBoW2_EXPORTS 6 | #define DBoW2_EXPORTS 7 | #endif 8 | #ifdef DBoW2_EXPORTS 9 | #define EXPORT __declspec(dllexport) 10 | #else 11 | #define EXPORT __declspec(dllimport) 12 | #endif 13 | #else 14 | #define EXPORT 15 | #endif 16 | 17 | #endif // CONFIG_H 18 | -------------------------------------------------------------------------------- /orb_dbow2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | orb_dbow2 4 | 1.0.0 5 | catkinized dbow2 for orb_slam2 6 | 7 | hyhuang 8 | 9 | MIT 10 | 11 | catkin 12 | catkin_simple 13 | 14 | 15 | -------------------------------------------------------------------------------- /orb_dbow2/src/dbow2/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "orb_dbow2/dbow2/BowVector.h" 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) 23 | { 24 | } 25 | 26 | // -------------------------------------------------------------------------- 27 | 28 | BowVector::~BowVector(void) 29 | { 30 | } 31 | 32 | // -------------------------------------------------------------------------- 33 | 34 | void BowVector::addWeight(WordId id, WordValue v) 35 | { 36 | BowVector::iterator vit = this->lower_bound(id); 37 | 38 | if(vit != this->end() && !(this->key_comp()(id, vit->first))) 39 | { 40 | vit->second += v; 41 | } 42 | else 43 | { 44 | this->insert(vit, BowVector::value_type(id, v)); 45 | } 46 | } 47 | 48 | // -------------------------------------------------------------------------- 49 | 50 | void BowVector::addIfNotExist(WordId id, WordValue v) 51 | { 52 | BowVector::iterator vit = this->lower_bound(id); 53 | 54 | if(vit == this->end() || (this->key_comp()(id, vit->first))) 55 | { 56 | this->insert(vit, BowVector::value_type(id, v)); 57 | } 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void BowVector::normalize(LNorm norm_type) 63 | { 64 | double norm = 0.0; 65 | BowVector::iterator it; 66 | 67 | if(norm_type == DBoW2::L1) 68 | { 69 | for(it = begin(); it != end(); ++it) 70 | norm += fabs(it->second); 71 | } 72 | else 73 | { 74 | for(it = begin(); it != end(); ++it) 75 | norm += it->second * it->second; 76 | norm = sqrt(norm); 77 | } 78 | 79 | if(norm > 0.0) 80 | { 81 | for(it = begin(); it != end(); ++it) 82 | it->second /= norm; 83 | } 84 | } 85 | 86 | // -------------------------------------------------------------------------- 87 | 88 | std::ostream& operator<< (std::ostream &out, const BowVector &v) 89 | { 90 | BowVector::const_iterator vit; 91 | std::vector::const_iterator iit; 92 | unsigned int i = 0; 93 | const unsigned int N = v.size(); 94 | for(vit = v.begin(); vit != v.end(); ++vit, ++i) 95 | { 96 | out << "<" << vit->first << ", " << vit->second << ">"; 97 | 98 | if(i < N-1) out << ", "; 99 | } 100 | return out; 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | void BowVector::saveM(const std::string &filename, size_t W) const 106 | { 107 | std::fstream f(filename.c_str(), std::ios::out); 108 | 109 | WordId last = 0; 110 | BowVector::const_iterator bit; 111 | for(bit = this->begin(); bit != this->end(); ++bit) 112 | { 113 | for(; last < bit->first; ++last) 114 | { 115 | f << "0 "; 116 | } 117 | f << bit->second << " "; 118 | 119 | last = bit->first + 1; 120 | } 121 | for(; last < (WordId)W; ++last) 122 | f << "0 "; 123 | 124 | f.close(); 125 | } 126 | 127 | // -------------------------------------------------------------------------- 128 | 129 | } // namespace DBoW2 130 | 131 | -------------------------------------------------------------------------------- /orb_dbow2/src/dbow2/FORB.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FORB.cpp 3 | * Date: June 2012 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for ORB descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | * Distance function has been modified 9 | * 10 | */ 11 | 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "orb_dbow2/dbow2/FORB.h" 20 | 21 | using namespace std; 22 | 23 | namespace DBoW2 { 24 | 25 | // -------------------------------------------------------------------------- 26 | 27 | // const int FORB::L=32; 28 | 29 | void FORB::meanValue(const std::vector &descriptors, 30 | FORB::TDescriptor &mean) 31 | { 32 | if(descriptors.empty()) 33 | { 34 | mean.release(); 35 | return; 36 | } 37 | else if(descriptors.size() == 1) 38 | { 39 | mean = descriptors[0]->clone(); 40 | } 41 | else 42 | { 43 | vector sum(FORB::L * 8, 0); 44 | 45 | for(size_t i = 0; i < descriptors.size(); ++i) 46 | { 47 | const cv::Mat &d = *descriptors[i]; 48 | const unsigned char *p = d.ptr(); 49 | 50 | for(int j = 0; j < d.cols; ++j, ++p) 51 | { 52 | if(*p & (1 << 7)) ++sum[ j*8 ]; 53 | if(*p & (1 << 6)) ++sum[ j*8 + 1 ]; 54 | if(*p & (1 << 5)) ++sum[ j*8 + 2 ]; 55 | if(*p & (1 << 4)) ++sum[ j*8 + 3 ]; 56 | if(*p & (1 << 3)) ++sum[ j*8 + 4 ]; 57 | if(*p & (1 << 2)) ++sum[ j*8 + 5 ]; 58 | if(*p & (1 << 1)) ++sum[ j*8 + 6 ]; 59 | if(*p & (1)) ++sum[ j*8 + 7 ]; 60 | } 61 | } 62 | 63 | mean = cv::Mat::zeros(1, FORB::L, CV_8U); 64 | unsigned char *p = mean.ptr(); 65 | 66 | const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2; 67 | for(size_t i = 0; i < sum.size(); ++i) 68 | { 69 | if(sum[i] >= N2) 70 | { 71 | // set bit 72 | *p |= 1 << (7 - (i % 8)); 73 | } 74 | 75 | if(i % 8 == 7) ++p; 76 | } 77 | } 78 | } 79 | 80 | // -------------------------------------------------------------------------- 81 | 82 | int FORB::distance(const FORB::TDescriptor &a, 83 | const FORB::TDescriptor &b) 84 | { 85 | // Bit set count operation from 86 | // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel 87 | 88 | const int *pa = a.ptr(); 89 | const int *pb = b.ptr(); 90 | 91 | int dist=0; 92 | 93 | for(int i=0; i<8; i++, pa++, pb++) 94 | { 95 | unsigned int v = *pa ^ *pb; 96 | v = v - ((v >> 1) & 0x55555555); 97 | v = (v & 0x33333333) + ((v >> 2) & 0x33333333); 98 | dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; 99 | } 100 | 101 | return dist; 102 | } 103 | 104 | // -------------------------------------------------------------------------- 105 | 106 | std::string FORB::toString(const FORB::TDescriptor &a) 107 | { 108 | stringstream ss; 109 | const unsigned char *p = a.ptr(); 110 | 111 | for(int i = 0; i < a.cols; ++i, ++p) 112 | { 113 | ss << (int)*p << " "; 114 | } 115 | 116 | return ss.str(); 117 | } 118 | 119 | // -------------------------------------------------------------------------- 120 | 121 | void FORB::fromString(FORB::TDescriptor &a, const std::string &s) 122 | { 123 | a.create(1, FORB::L, CV_8U); 124 | unsigned char *p = a.ptr(); 125 | 126 | stringstream ss(s); 127 | for(int i = 0; i < FORB::L; ++i, ++p) 128 | { 129 | int n; 130 | ss >> n; 131 | 132 | if(!ss.fail()) 133 | *p = (unsigned char)n; 134 | } 135 | 136 | } 137 | 138 | // -------------------------------------------------------------------------- 139 | 140 | void FORB::toMat32F(const std::vector &descriptors, 141 | cv::Mat &mat) 142 | { 143 | if(descriptors.empty()) 144 | { 145 | mat.release(); 146 | return; 147 | } 148 | 149 | const size_t N = descriptors.size(); 150 | 151 | mat.create(N, FORB::L*8, CV_32F); 152 | float *p = mat.ptr(); 153 | 154 | for(size_t i = 0; i < N; ++i) 155 | { 156 | const int C = descriptors[i].cols; 157 | const unsigned char *desc = descriptors[i].ptr(); 158 | 159 | for(int j = 0; j < C; ++j, p += 8) 160 | { 161 | p[0] = (desc[j] & (1 << 7) ? 1 : 0); 162 | p[1] = (desc[j] & (1 << 6) ? 1 : 0); 163 | p[2] = (desc[j] & (1 << 5) ? 1 : 0); 164 | p[3] = (desc[j] & (1 << 4) ? 1 : 0); 165 | p[4] = (desc[j] & (1 << 3) ? 1 : 0); 166 | p[5] = (desc[j] & (1 << 2) ? 1 : 0); 167 | p[6] = (desc[j] & (1 << 1) ? 1 : 0); 168 | p[7] = desc[j] & (1); 169 | } 170 | } 171 | } 172 | 173 | // -------------------------------------------------------------------------- 174 | 175 | void FORB::toMat8U(const std::vector &descriptors, 176 | cv::Mat &mat) 177 | { 178 | mat.create(descriptors.size(), 32, CV_8U); 179 | 180 | unsigned char *p = mat.ptr(); 181 | 182 | for(size_t i = 0; i < descriptors.size(); ++i, p += 32) 183 | { 184 | const unsigned char *d = descriptors[i].ptr(); 185 | std::copy(d, d+32, p); 186 | } 187 | 188 | } 189 | 190 | // -------------------------------------------------------------------------- 191 | 192 | } // namespace DBoW2 193 | 194 | 195 | -------------------------------------------------------------------------------- /orb_dbow2/src/dbow2/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "orb_dbow2/dbow2/FeatureVector.h" 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace DBoW2 { 17 | 18 | // --------------------------------------------------------------------------- 19 | 20 | FeatureVector::FeatureVector(void) 21 | { 22 | } 23 | 24 | // --------------------------------------------------------------------------- 25 | 26 | FeatureVector::~FeatureVector(void) 27 | { 28 | } 29 | 30 | // --------------------------------------------------------------------------- 31 | 32 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) 33 | { 34 | FeatureVector::iterator vit = this->lower_bound(id); 35 | 36 | if(vit != this->end() && vit->first == id) 37 | { 38 | vit->second.push_back(i_feature); 39 | } 40 | else 41 | { 42 | vit = this->insert(vit, FeatureVector::value_type(id, 43 | std::vector() )); 44 | vit->second.push_back(i_feature); 45 | } 46 | } 47 | 48 | // --------------------------------------------------------------------------- 49 | 50 | std::ostream& operator<<(std::ostream &out, 51 | const FeatureVector &v) 52 | { 53 | if(!v.empty()) 54 | { 55 | FeatureVector::const_iterator vit = v.begin(); 56 | 57 | const std::vector* f = &vit->second; 58 | 59 | out << "<" << vit->first << ": ["; 60 | if(!f->empty()) out << (*f)[0]; 61 | for(unsigned int i = 1; i < f->size(); ++i) 62 | { 63 | out << ", " << (*f)[i]; 64 | } 65 | out << "]>"; 66 | 67 | for(++vit; vit != v.end(); ++vit) 68 | { 69 | f = &vit->second; 70 | 71 | out << ", <" << vit->first << ": ["; 72 | if(!f->empty()) out << (*f)[0]; 73 | for(unsigned int i = 1; i < f->size(); ++i) 74 | { 75 | out << ", " << (*f)[i]; 76 | } 77 | out << "]>"; 78 | } 79 | } 80 | 81 | return out; 82 | } 83 | 84 | // --------------------------------------------------------------------------- 85 | 86 | } // namespace DBoW2 87 | -------------------------------------------------------------------------------- /orb_dbow2/src/dutils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "orb_dbow2/dutils/Random.h" 12 | #include "orb_dbow2/dutils/Timestamp.h" 13 | 14 | #include 15 | using namespace std; 16 | 17 | bool DUtils::Random::m_already_seeded = false; 18 | 19 | void DUtils::Random::SeedRand(){ 20 | Timestamp time; 21 | time.setToCurrentTime(); 22 | srand((unsigned)time.getFloatTime()); 23 | } 24 | 25 | void DUtils::Random::SeedRandOnce() 26 | { 27 | if(!m_already_seeded) 28 | { 29 | DUtils::Random::SeedRand(); 30 | m_already_seeded = true; 31 | } 32 | } 33 | 34 | void DUtils::Random::SeedRand(int seed) 35 | { 36 | srand(seed); 37 | } 38 | 39 | void DUtils::Random::SeedRandOnce(int seed) 40 | { 41 | if(!m_already_seeded) 42 | { 43 | DUtils::Random::SeedRand(seed); 44 | m_already_seeded = true; 45 | } 46 | } 47 | 48 | int DUtils::Random::RandomInt(int min, int max){ 49 | int d = max - min + 1; 50 | return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min; 51 | } 52 | 53 | // --------------------------------------------------------------------------- 54 | // --------------------------------------------------------------------------- 55 | 56 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) 57 | { 58 | if(min <= max) 59 | { 60 | m_min = min; 61 | m_max = max; 62 | } 63 | else 64 | { 65 | m_min = max; 66 | m_max = min; 67 | } 68 | 69 | createValues(); 70 | } 71 | 72 | // --------------------------------------------------------------------------- 73 | 74 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer 75 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 76 | { 77 | *this = rnd; 78 | } 79 | 80 | // --------------------------------------------------------------------------- 81 | 82 | int DUtils::Random::UnrepeatedRandomizer::get() 83 | { 84 | if(empty()) createValues(); 85 | 86 | DUtils::Random::SeedRandOnce(); 87 | 88 | int k = DUtils::Random::RandomInt(0, m_values.size()-1); 89 | int ret = m_values[k]; 90 | m_values[k] = m_values.back(); 91 | m_values.pop_back(); 92 | 93 | return ret; 94 | } 95 | 96 | // --------------------------------------------------------------------------- 97 | 98 | void DUtils::Random::UnrepeatedRandomizer::createValues() 99 | { 100 | int n = m_max - m_min + 1; 101 | 102 | m_values.resize(n); 103 | for(int i = 0; i < n; ++i) m_values[i] = m_min + i; 104 | } 105 | 106 | // --------------------------------------------------------------------------- 107 | 108 | void DUtils::Random::UnrepeatedRandomizer::reset() 109 | { 110 | if((int)m_values.size() != m_max - m_min + 1) createValues(); 111 | } 112 | 113 | // --------------------------------------------------------------------------- 114 | 115 | DUtils::Random::UnrepeatedRandomizer& 116 | DUtils::Random::UnrepeatedRandomizer::operator= 117 | (const DUtils::Random::UnrepeatedRandomizer& rnd) 118 | { 119 | if(this != &rnd) 120 | { 121 | this->m_min = rnd.m_min; 122 | this->m_max = rnd.m_max; 123 | this->m_values = rnd.m_values; 124 | } 125 | return *this; 126 | } 127 | 128 | // --------------------------------------------------------------------------- 129 | 130 | 131 | -------------------------------------------------------------------------------- /orb_dbow2/src/dutils/Timestamp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Timestamp.cpp 3 | * Author: Dorian Galvez-Lopez 4 | * Date: March 2009 5 | * Description: timestamping functions 6 | * 7 | * Note: in windows, this class has a 1ms resolution 8 | * 9 | * License: see the LICENSE.txt file 10 | * 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #ifdef _WIN32 21 | #ifndef WIN32 22 | #define WIN32 23 | #endif 24 | #endif 25 | 26 | #ifdef WIN32 27 | #include 28 | #define sprintf sprintf_s 29 | #else 30 | #include 31 | #endif 32 | 33 | #include "orb_dbow2/dutils/Timestamp.h" 34 | 35 | using namespace std; 36 | 37 | using namespace DUtils; 38 | 39 | Timestamp::Timestamp(Timestamp::tOptions option) 40 | { 41 | if(option & CURRENT_TIME) 42 | setToCurrentTime(); 43 | else if(option & ZERO) 44 | setTime(0.); 45 | } 46 | 47 | Timestamp::~Timestamp(void) 48 | { 49 | } 50 | 51 | bool Timestamp::empty() const 52 | { 53 | return m_secs == 0 && m_usecs == 0; 54 | } 55 | 56 | void Timestamp::setToCurrentTime(){ 57 | 58 | #ifdef WIN32 59 | struct __timeb32 timebuffer; 60 | _ftime32_s( &timebuffer ); // C4996 61 | // Note: _ftime is deprecated; consider using _ftime_s instead 62 | m_secs = timebuffer.time; 63 | m_usecs = timebuffer.millitm * 1000; 64 | #else 65 | struct timeval now; 66 | gettimeofday(&now, NULL); 67 | m_secs = now.tv_sec; 68 | m_usecs = now.tv_usec; 69 | #endif 70 | 71 | } 72 | 73 | void Timestamp::setTime(const string &stime){ 74 | string::size_type p = stime.find('.'); 75 | if(p == string::npos){ 76 | m_secs = atol(stime.c_str()); 77 | m_usecs = 0; 78 | }else{ 79 | m_secs = atol(stime.substr(0, p).c_str()); 80 | 81 | string s_usecs = stime.substr(p+1, 6); 82 | m_usecs = atol(stime.substr(p+1).c_str()); 83 | m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length())); 84 | } 85 | } 86 | 87 | void Timestamp::setTime(double s) 88 | { 89 | m_secs = (unsigned long)s; 90 | m_usecs = (s - (double)m_secs) * 1e6; 91 | } 92 | 93 | double Timestamp::getFloatTime() const { 94 | return double(m_secs) + double(m_usecs)/1000000.0; 95 | } 96 | 97 | string Timestamp::getStringTime() const { 98 | char buf[32]; 99 | sprintf(buf, "%.6lf", this->getFloatTime()); 100 | return string(buf); 101 | } 102 | 103 | double Timestamp::operator- (const Timestamp &t) const { 104 | return this->getFloatTime() - t.getFloatTime(); 105 | } 106 | 107 | Timestamp& Timestamp::operator+= (double s) 108 | { 109 | *this = *this + s; 110 | return *this; 111 | } 112 | 113 | Timestamp& Timestamp::operator-= (double s) 114 | { 115 | *this = *this - s; 116 | return *this; 117 | } 118 | 119 | Timestamp Timestamp::operator+ (double s) const 120 | { 121 | unsigned long secs = (long)floor(s); 122 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 123 | 124 | return this->plus(secs, usecs); 125 | } 126 | 127 | Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const 128 | { 129 | Timestamp t; 130 | 131 | const unsigned long max = 1000000ul; 132 | 133 | if(m_usecs + usecs >= max) 134 | t.setTime(m_secs + secs + 1, m_usecs + usecs - max); 135 | else 136 | t.setTime(m_secs + secs, m_usecs + usecs); 137 | 138 | return t; 139 | } 140 | 141 | Timestamp Timestamp::operator- (double s) const 142 | { 143 | unsigned long secs = (long)floor(s); 144 | unsigned long usecs = (long)((s - (double)secs) * 1e6); 145 | 146 | return this->minus(secs, usecs); 147 | } 148 | 149 | Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const 150 | { 151 | Timestamp t; 152 | 153 | const unsigned long max = 1000000ul; 154 | 155 | if(m_usecs < usecs) 156 | t.setTime(m_secs - secs - 1, max - (usecs - m_usecs)); 157 | else 158 | t.setTime(m_secs - secs, m_usecs - usecs); 159 | 160 | return t; 161 | } 162 | 163 | bool Timestamp::operator> (const Timestamp &t) const 164 | { 165 | if(m_secs > t.m_secs) return true; 166 | else if(m_secs == t.m_secs) return m_usecs > t.m_usecs; 167 | else return false; 168 | } 169 | 170 | bool Timestamp::operator>= (const Timestamp &t) const 171 | { 172 | if(m_secs > t.m_secs) return true; 173 | else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs; 174 | else return false; 175 | } 176 | 177 | bool Timestamp::operator< (const Timestamp &t) const 178 | { 179 | if(m_secs < t.m_secs) return true; 180 | else if(m_secs == t.m_secs) return m_usecs < t.m_usecs; 181 | else return false; 182 | } 183 | 184 | bool Timestamp::operator<= (const Timestamp &t) const 185 | { 186 | if(m_secs < t.m_secs) return true; 187 | else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs; 188 | else return false; 189 | } 190 | 191 | bool Timestamp::operator== (const Timestamp &t) const 192 | { 193 | return(m_secs == t.m_secs && m_usecs == t.m_usecs); 194 | } 195 | 196 | 197 | string Timestamp::Format(bool machine_friendly) const 198 | { 199 | struct tm tm_time; 200 | 201 | time_t t = (time_t)getFloatTime(); 202 | 203 | #ifdef WIN32 204 | localtime_s(&tm_time, &t); 205 | #else 206 | localtime_r(&t, &tm_time); 207 | #endif 208 | 209 | char buffer[128]; 210 | 211 | if(machine_friendly) 212 | { 213 | strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time); 214 | } 215 | else 216 | { 217 | strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001 218 | } 219 | 220 | return string(buffer); 221 | } 222 | 223 | string Timestamp::Format(double s) { 224 | int days = int(s / (24. * 3600.0)); 225 | s -= days * (24. * 3600.0); 226 | int hours = int(s / 3600.0); 227 | s -= hours * 3600; 228 | int minutes = int(s / 60.0); 229 | s -= minutes * 60; 230 | int seconds = int(s); 231 | int ms = int((s - seconds)*1e6); 232 | 233 | stringstream ss; 234 | ss.fill('0'); 235 | bool b; 236 | if((b = (days > 0))) ss << days << "d "; 237 | if((b = (b || hours > 0))) ss << setw(2) << hours << ":"; 238 | if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":"; 239 | if(b) ss << setw(2); 240 | ss << seconds; 241 | if(!b) ss << "." << setw(6) << ms; 242 | 243 | return ss.str(); 244 | } 245 | 246 | 247 | --------------------------------------------------------------------------------