├── .clang-format ├── .github └── workflows │ └── build.yml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── FindG2O.cmake └── FindGLM.cmake ├── data └── shader │ ├── rainbow.frag │ ├── rainbow.vert │ ├── texture.frag │ └── texture.vert ├── docker ├── kinetic │ └── Dockerfile ├── kinetic_llvm │ └── Dockerfile ├── melodic │ └── Dockerfile ├── melodic_llvm │ └── Dockerfile ├── noetic │ └── Dockerfile ├── noetic_llvm │ └── Dockerfile └── run_with_x.sh ├── include ├── glk │ ├── colormap.hpp │ ├── drawble.hpp │ ├── frame_buffer.hpp │ ├── glsl_shader.hpp │ ├── lines.hpp │ ├── loaders │ │ └── ply_loader.hpp │ ├── mesh.hpp │ ├── mesh_utils.hpp │ ├── pointcloud_buffer.hpp │ ├── primitives │ │ ├── cone.hpp │ │ ├── coordinate_system.hpp │ │ ├── cube.hpp │ │ ├── grid.hpp │ │ ├── icosahedron.hpp │ │ └── primitives.hpp │ ├── texture.hpp │ └── texture_renderer.hpp ├── guik │ ├── camera_control.hpp │ ├── gl_canvas.hpp │ ├── imgui_application.hpp │ ├── model_control.hpp │ ├── progress_interface.hpp │ ├── progress_modal.hpp │ └── projection_control.hpp └── hdl_graph_slam │ ├── automatic_loop_close_window.hpp │ ├── edge_refinement_window.hpp │ ├── graph_edit_window.hpp │ ├── graph_merge_modal.hpp │ ├── interactive_graph.hpp │ ├── interactive_keyframe.hpp │ ├── manual_loop_close_model.hpp │ ├── parameter_server.hpp │ ├── plane_alignment_modal.hpp │ ├── plane_detection_window.hpp │ ├── registration_methods.hpp │ ├── robust_kernels.hpp │ ├── version.h.in │ ├── version_modal.hpp │ └── view │ ├── drawable_object.hpp │ ├── edge_plane_view.hpp │ ├── edge_se3_plane_view.hpp │ ├── edge_se3_view.hpp │ ├── edge_view.hpp │ ├── interactive_graph_view.hpp │ ├── keyframe_view.hpp │ ├── line_buffer.hpp │ ├── vertex_plane_cache.hpp │ ├── vertex_plane_view.hpp │ └── vertex_view.hpp ├── package.xml ├── src ├── glk │ ├── colormap.cpp │ ├── frame_buffer.cpp │ ├── glsl_shader.cpp │ ├── lines.cpp │ ├── loaders │ │ └── ply_loader.cpp │ ├── mesh.cpp │ ├── pointcloud_buffer.cpp │ └── primitives │ │ └── primitives.cpp ├── guik │ ├── camera_control.cpp │ ├── gl_canvas.cpp │ ├── imgui_application.cpp │ ├── model_control.cpp │ └── projection_control.cpp ├── hdl_graph_slam │ ├── automatic_loop_close_window.cpp │ ├── edge_refinement_window.cpp │ ├── graph_edit_window.cpp │ ├── interactive_graph.cpp │ ├── interactive_keyframe.cpp │ ├── manual_loop_close_modal.cpp │ ├── plane_alignment_modal.cpp │ ├── plane_detection_window.cpp │ ├── registration_methods.cpp │ ├── version_modal.cpp │ └── view │ │ ├── edge_view.cpp │ │ └── vertex_view.cpp ├── interactive_slam.cpp └── odometry2graph.cpp └── thirdparty └── gl3w ├── GL ├── gl3w.h └── glcorearb.h ├── KHR └── khrplatform.h └── gl3w.c /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlines: Left 9 | AlignOperands: true 10 | AlignTrailingComments: true 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: Empty 15 | AllowShortIfStatementsOnASingleLine: true 16 | AllowShortLoopsOnASingleLine: true 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: true 20 | AlwaysBreakTemplateDeclarations: true 21 | BinPackArguments: true 22 | BinPackParameters: false 23 | BraceWrapping: 24 | AfterClass: false 25 | AfterControlStatement: false 26 | AfterEnum: false 27 | AfterFunction: false 28 | AfterNamespace: false 29 | AfterObjCDeclaration: false 30 | AfterStruct: false 31 | AfterUnion: false 32 | AfterExternBlock: false 33 | BeforeCatch: false 34 | BeforeElse: false 35 | IndentBraces: false 36 | SplitEmptyFunction: true 37 | SplitEmptyRecord: true 38 | SplitEmptyNamespace: true 39 | BreakBeforeBinaryOperators: None 40 | BreakBeforeBraces: Attach 41 | BreakBeforeInheritanceComma: false 42 | BreakBeforeTernaryOperators: true 43 | BreakConstructorInitializersBeforeComma: false 44 | BreakConstructorInitializers: BeforeColon 45 | BreakAfterJavaFieldAnnotations: false 46 | BreakStringLiterals: false 47 | ColumnLimit: 256 48 | CommentPragmas: '^ IWYU pragma:' 49 | CompactNamespaces: false 50 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 51 | ConstructorInitializerIndentWidth: 4 52 | ContinuationIndentWidth: 4 53 | Cpp11BracedListStyle: true 54 | DerivePointerAlignment: true 55 | DisableFormat: false 56 | ExperimentalAutoDetectBinPacking: false 57 | FixNamespaceComments: true 58 | ForEachMacros: 59 | - foreach 60 | - Q_FOREACH 61 | - BOOST_FOREACH 62 | IncludeBlocks: Preserve 63 | IncludeCategories: 64 | - Regex: '^' 65 | Priority: 2 66 | - Regex: '^<.*\.h>' 67 | Priority: 1 68 | - Regex: '^<.*' 69 | Priority: 2 70 | - Regex: '.*' 71 | Priority: 3 72 | IncludeIsMainRegex: '([-_](test|unittest))?$' 73 | IndentCaseLabels: true 74 | IndentPPDirectives: None 75 | IndentWidth: 2 76 | IndentWrappedFunctionNames: false 77 | JavaScriptQuotes: Leave 78 | JavaScriptWrapImports: true 79 | KeepEmptyLinesAtTheStartOfBlocks: false 80 | MacroBlockBegin: '' 81 | MacroBlockEnd: '' 82 | MaxEmptyLinesToKeep: 1 83 | NamespaceIndentation: None 84 | ObjCBlockIndentWidth: 2 85 | ObjCSpaceAfterProperty: false 86 | ObjCSpaceBeforeProtocolList: false 87 | PenaltyBreakAssignment: 2 88 | PenaltyBreakBeforeFirstCallParameter: 1 89 | PenaltyBreakComment: 300 90 | PenaltyBreakFirstLessLess: 120 91 | PenaltyBreakString: 1000 92 | PenaltyExcessCharacter: 0 93 | PenaltyReturnTypeOnItsOwnLine: 200 94 | PointerAlignment: Left 95 | RawStringFormats: 96 | - Delimiter: pb 97 | Language: TextProto 98 | BasedOnStyle: google 99 | ReflowComments: true 100 | SortIncludes: false 101 | SortUsingDeclarations: false 102 | SpaceAfterCStyleCast: false 103 | SpaceAfterTemplateKeyword: false 104 | SpaceBeforeAssignmentOperators: true 105 | SpaceBeforeParens: Never 106 | SpaceInEmptyParentheses: false 107 | SpacesBeforeTrailingComments: 2 108 | SpacesInAngles: false 109 | SpacesInContainerLiterals: true 110 | SpacesInCStyleCastParentheses: false 111 | SpacesInParentheses: false 112 | SpacesInSquareBrackets: false 113 | Standard: Auto 114 | TabWidth: 8 115 | UseTab: Never 116 | ... 117 | 118 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | # Allows you to run this workflow manually from the Actions tab 10 | workflow_dispatch: 11 | 12 | jobs: 13 | build: 14 | runs-on: ubuntu-latest 15 | strategy: 16 | matrix: 17 | ROS_DISTRO: [melodic, melodic_llvm, noetic, noetic_llvm] 18 | 19 | steps: 20 | - uses: actions/checkout@v2 21 | 22 | - name: Docker login 23 | continue-on-error: true 24 | uses: docker/login-action@v1 25 | with: 26 | username: ${{ secrets.DOCKER_USERNAME }} 27 | password: ${{ secrets.DOCKER_TOKEN }} 28 | 29 | - name: Docker build 30 | uses: docker/build-push-action@v2 31 | with: 32 | file: ${{github.workspace}}/docker/${{ matrix.ROS_DISTRO }}/Dockerfile 33 | context: . 34 | push: false 35 | 36 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | include/hdl_graph_slam/version.h 2 | imgui.ini 3 | build -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "thirdparty/portable-file-dialogs"] 2 | path = thirdparty/portable-file-dialogs 3 | url = https://github.com/samhocevar/portable-file-dialogs.git 4 | [submodule "thirdparty/imgui"] 5 | path = thirdparty/imgui 6 | url = https://github.com/ocornut/imgui.git 7 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(interactive_slam) 3 | 4 | set (IS_VERSION_MAJOR 1) 5 | set (IS_VERSION_MINOR 0) 6 | set (IS_VERSION_REVISION 5) 7 | 8 | execute_process (COMMAND git rev-parse --short HEAD WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR} OUTPUT_VARIABLE GIT_LAST_COMMIT_HASH OUTPUT_STRIP_TRAILING_WHITESPACE) 9 | execute_process (COMMAND git log -1 --format=%cd WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR} OUTPUT_VARIABLE GIT_LAST_COMMIT_DATE OUTPUT_STRIP_TRAILING_WHITESPACE) 10 | 11 | set (LAST_COMMIT_HASH \"${GIT_LAST_COMMIT_HASH}\") 12 | set (LAST_COMMIT_DATE \"${GIT_LAST_COMMIT_DATE}\") 13 | 14 | configure_file ( 15 | "${PROJECT_SOURCE_DIR}/include/hdl_graph_slam/version.h.in" 16 | "${PROJECT_SOURCE_DIR}/include/hdl_graph_slam/version.h" 17 | ) 18 | 19 | if("$ENV{ROS_DISTRO}" STRGREATER "melodic") 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") 21 | else() 22 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 23 | endif() 24 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") 25 | 26 | find_package(catkin REQUIRED COMPONENTS fast_gicp roscpp ndt_omp hdl_graph_slam roslib) 27 | 28 | find_package(GLM REQUIRED) 29 | find_package(OpenGL REQUIRED) 30 | find_package(PCL REQUIRED) 31 | 32 | find_package(Eigen3 REQUIRED) 33 | find_package(Boost REQUIRED filesystem program_options) 34 | 35 | find_package(G2O REQUIRED) 36 | include_directories(SYSTEM ${G2O_INCLUDE_DIR} ${G2O_INCLUDE_DIRS}) 37 | link_directories(${G2O_LIBRARY_DIRS}) 38 | 39 | ################## 40 | # catking config # 41 | ################## 42 | catkin_package( 43 | # INCLUDE_DIRS include 44 | # LIBRARIES interactive_map_correction 45 | # CATKIN_DEPENDS other_catkin_pkg 46 | # DEPENDS system_lib 47 | ) 48 | 49 | ########### 50 | ## Build ## 51 | ########### 52 | 53 | include_directories( 54 | include 55 | thirdparty/gl3w 56 | thirdparty/imgui 57 | thirdparty/imgui/examples 58 | thirdparty/portable-file-dialogs 59 | ${GLM_INCLUDE_DIRS} 60 | ${Boost_INCLUDE_DIRS} 61 | ${EIGEN3_INCLUDE_DIRS} 62 | ${PCL_INCLUDE_DIRS} 63 | ${catkin_INCLUDE_DIRS} 64 | ) 65 | 66 | # imgui 67 | add_definitions(-DIMGUI_IMPL_OPENGL_LOADER_GL3W) 68 | 69 | ########### 70 | ## Build ## 71 | ########### 72 | 73 | add_library(imgui STATIC 74 | thirdparty/gl3w/gl3w.c 75 | thirdparty/imgui/imgui.cpp 76 | thirdparty/imgui/imgui_demo.cpp 77 | thirdparty/imgui/imgui_draw.cpp 78 | thirdparty/imgui/imgui_widgets.cpp 79 | thirdparty/imgui/examples/imgui_impl_glfw.cpp 80 | thirdparty/imgui/examples/imgui_impl_opengl3.cpp 81 | ) 82 | target_link_libraries(imgui 83 | ${OPENGL_LIBRARIES} 84 | glfw 85 | dl 86 | ) 87 | 88 | add_library(guik STATIC 89 | src/glk/mesh.cpp 90 | src/glk/lines.cpp 91 | src/glk/colormap.cpp 92 | src/glk/glsl_shader.cpp 93 | src/glk/frame_buffer.cpp 94 | src/glk/pointcloud_buffer.cpp 95 | src/glk/primitives/primitives.cpp 96 | src/glk/loaders/ply_loader.cpp 97 | src/guik/gl_canvas.cpp 98 | src/guik/model_control.cpp 99 | src/guik/camera_control.cpp 100 | src/guik/projection_control.cpp 101 | src/guik/imgui_application.cpp 102 | ) 103 | 104 | add_executable(odometry2graph 105 | src/odometry2graph.cpp 106 | src/hdl_graph_slam/version_modal.cpp 107 | ) 108 | target_link_libraries(odometry2graph 109 | ${PCL_LIBRARIES} 110 | ${G2O_TYPES_DATA} 111 | ${G2O_CORE_LIBRARY} 112 | ${G2O_TYPES_SLAM3D} 113 | ${OPENGL_LIBRARIES} 114 | ${catkin_LIBRARIES} 115 | glfw 116 | guik 117 | imgui 118 | ) 119 | 120 | add_executable(interactive_slam 121 | src/interactive_slam.cpp 122 | src/hdl_graph_slam/registration_methods.cpp 123 | src/hdl_graph_slam/interactive_graph.cpp 124 | src/hdl_graph_slam/interactive_keyframe.cpp 125 | src/hdl_graph_slam/version_modal.cpp 126 | src/hdl_graph_slam/graph_edit_window.cpp 127 | src/hdl_graph_slam/edge_refinement_window.cpp 128 | src/hdl_graph_slam/plane_detection_window.cpp 129 | src/hdl_graph_slam/plane_alignment_modal.cpp 130 | src/hdl_graph_slam/manual_loop_close_modal.cpp 131 | src/hdl_graph_slam/automatic_loop_close_window.cpp 132 | src/hdl_graph_slam/view/edge_view.cpp 133 | src/hdl_graph_slam/view/vertex_view.cpp 134 | ) 135 | target_link_libraries(interactive_slam 136 | ${PCL_LIBRARIES} 137 | ${G2O_TYPES_DATA} 138 | ${G2O_CORE_LIBRARY} 139 | ${G2O_STUFF_LIBRARY} 140 | ${G2O_SOLVER_PCG} 141 | ${G2O_SOLVER_CSPARSE} 142 | ${G2O_SOLVER_CHOLMOD} 143 | ${G2O_TYPES_SLAM3D} 144 | ${G2O_TYPES_SLAM3D_ADDONS} 145 | ${OPENGL_LIBRARIES} 146 | ${catkin_LIBRARIES} 147 | glfw 148 | guik 149 | imgui 150 | ) 151 | 152 | file(COPY data DESTINATION .) 153 | 154 | install(TARGETS 155 | interactive_slam 156 | odometry2graph 157 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 158 | ) 159 | 160 | install(DIRECTORY data 161 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 162 | ) 163 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # New SLAM Package Released 2 | 3 | We released [a new SLAM package GLIM](https://github.com/koide3/glim) that has [interactive map correction features](https://koide3.github.io/glim/quickstart.html#mapping-result). 4 | 5 | # interactive_slam 6 | ***interactive_slam*** is an open source 3D LIDAR-based mapping framework. In contrast to existing automatic SLAM packages, we aim to develop a semi-automatic framework which allows the user to interactively and intuitively correct mapping failures (e.g., corrupted odometry, wrong loop detection, distorted map, etc) with minimal human effort. This framework provides several map correction features: 7 | - [Manual & Automatic] Loop closing 8 | - [Manual] Plane-based map correction 9 | - [Manual] Multiple map merging 10 | - [Automatic] Pose edge refinement 11 | 12 | 13 | ![Screenshot_20191016_182424](https://user-images.githubusercontent.com/31344317/66906208-3f2e0880-f042-11e9-8366-c178f9c00b65.png) 14 | [[video]](https://youtu.be/vAqo6YkbKpU) 15 | 16 | 17 | This package is built on top of the ROS ecosystem. You can start building a map with a pose graph constructed by [hdl_graph_slam](https://github.com/koide3/hdl_graph_slam) or a customized [LeGO-LOAM](https://github.com/koide3/LeGO-LOAM-BOR), or odometry data generated by any ROS package. 18 | 19 | This package has been tested on Ubuntu 18.04 & ROS melodic or later. 20 | 21 | [![Build](https://github.com/SMRT-AIST/interactive_slam/actions/workflows/build.yml/badge.svg)](https://github.com/SMRT-AIST/interactive_slam/actions/workflows/build.yml) 22 | 23 | ## Installation 24 | ***interactive_slam*** depends on the following libraries: 25 | 26 | - [GL3W](https://github.com/skaslev/gl3w) 27 | - [GLFW](https://www.glfw.org/) 28 | - [Dear ImGui](https://github.com/ocornut/imgui) 29 | - [portable-file-dialog](portable-file-dialog) 30 | - [OpenMP](https://www.openmp.org/) 31 | - [PCL](https://pointclouds.org/) 32 | - [g2o](https://github.com/RainerKuemmerle/g2o) 33 | 34 | 35 | ```bash 36 | # for ROS noetic 37 | sudo apt-get install libglm-dev libglfw3-dev 38 | sudo apt-get install libsuitesparse-dev libeigen3-dev 39 | sudo apt-get install ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs 40 | ``` 41 | 42 | ```bash 43 | # Install g2o from original source code 44 | git clone https://github.com/RainerKuemmerle/g2o.git 45 | cd g2o 46 | mkdir build 47 | cd build 48 | cmake ../ 49 | make 50 | sudo make install 51 | ``` 52 | 53 | ```bash 54 | # Install Ceres Library 55 | git clone https://ceres-solver.googlesource.com/ceres-solver 56 | # Install all dependencies 57 | # CMake 58 | sudo apt-get install cmake 59 | # google-glog + gflags 60 | sudo apt-get install libgoogle-glog-dev libgflags-dev 61 | # Use ATLAS for BLAS & LAPACK 62 | sudo apt-get install libatlas-base-dev 63 | # Eigen3 64 | sudo apt-get install libeigen3-dev 65 | # SuiteSparse (optional) 66 | sudo apt-get install libsuitesparse-dev 67 | cd ceres-solver 68 | mkdir build 69 | cd build 70 | cmake ../ 71 | make 72 | sudo make install 73 | ``` 74 | 75 | ```bash 76 | cd ~/catkin_ws/src 77 | git clone https://github.com/koide3/ndt_omp 78 | # on melodic 79 | # git clone https://github.com/koide3/ndt_omp -b melodic 80 | git clone https://github.com/koide3/hdl_graph_slam 81 | git clone https://github.com/koide3/odometry_saver 82 | git clone https://github.com/SMRT-AIST/fast_gicp --recursive 83 | git clone https://github.com/SMRT-AIST/interactive_slam --recursive 84 | 85 | cd ~/catkin_ws 86 | catkin_make -DCMAKE_BUILD_TYPE=Release 87 | ``` 88 | 89 | ***ROS Kinetic users*** 90 | This package cannot be built using gcc and ld on Ubuntu 16. If you are on Ubuntu 16 and ROS kinetic, try the LLVM toolchain. Note: we recommend to use this package on melodic because we do only build-test but not run-test on kinetic. 91 | 92 | ```bash 93 | sudo apt install clang-6.0 lld-6.0 94 | sudo update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld-6.0 50 95 | sudo update-alternatives --install /usr/bin/clang clang /usr/bin/clang-6.0 50 96 | sudo update-alternatives --install /usr/bin/clang++ clang++ /usr/bin/clang++-6.0 50 97 | cd ~/catkin_ws && rm -rf build devel # be aware of that this command removes build and devel directories 98 | CC=clang CXX=clang++ catkin_make -DCMAKE_BUILD_TYPE=Release 99 | ``` 100 | 101 | ## Examples 102 | 103 | ### Example1 - Basic usage with hdl_graph_slam 104 | 105 | In this example, we edit a map (pose graph) constructed by [hdl_graph_slam](https://github.com/koide3/hdl_graph_slam). [See more](https://github.com/koide3/interactive_slam/wiki/Example1). 106 | ![Screenshot_20191016_175924 png](https://user-images.githubusercontent.com/31344317/66904272-c11c3280-f03e-11e9-9420-078d75c5c0e9.jpg) 107 | 108 | ### Example2 - Generating odometry with external ROS package 109 | 110 | In this example, we create a map with odometry data generated from a rosbag file with LeGO-LOAM. [See more](https://github.com/koide3/interactive_slam/wiki/Example2). 111 | 112 | ### Example3 - Plane-based map correction & Map merging 113 | 114 | In this example, we correct a largely bent map with plane constraints and merge it with another map. [See more](https://github.com/koide3/interactive_slam/wiki/Example3). 115 | 116 | ![Screenshot_20191016_182955 png](https://user-images.githubusercontent.com/31344317/66906642-fe82bf00-f042-11e9-9373-f810337f4d97.jpg) 117 | 118 | ## Graph/Odometry file format 119 | 120 | You can feed graph/odometry files generated by your program to ***interactive_slam***. [See more](https://github.com/koide3/interactive_slam/wiki/Format) 121 | 122 | ## FAQ 123 | 124 | [FAQ](https://github.com/koide3/interactive_slam/wiki/FAQ) 125 | 126 | ## License 127 | ***interactive_slam*** is released under GPLv3 license. 128 | 129 | ## Related packages 130 | 131 | - [hdl_graph_slam](https://github.com/koide3/hdl_graph_slam) 132 | - [hdl_localization](https://github.com/koide3/hdl_localization) 133 | - [hdl_people_tracking](https://github.com/koide3/hdl_people_tracking) 134 | 135 | ## Papers 136 | Kenji Koide, Jun Miura, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno, Interactive 3D Graph SLAM for Map Correction, IEEE Robotics and Automation Letters (RA-L), 2020 [DOI](https://doi.org/10.1109/LRA.2020.3028828) 137 | 138 | ## Contact 139 | [発展版機能について](https://github.com/SMRT-AIST/interactive_slam/wiki/%E7%99%BA%E5%B1%95%E7%89%88%E6%A9%9F%E8%83%BD%E3%81%AB%E3%81%A4%E3%81%84%E3%81%A6) 140 | 141 | Kenji Koide, k.koide@aist.go.jp, https://staff.aist.go.jp/k.koide 142 | 143 | Mobile Robotics Research Team 144 | National Institute of Advanced Industrial Science and Technology (AIST), Japan [\[URL\]](https://unit.aist.go.jp/hcmrc/mr-rt/index.html) 145 | -------------------------------------------------------------------------------- /cmake/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | /opt/ros/$ENV{ROS_DISTRO}/include 13 | NO_DEFAULT_PATH 14 | ) 15 | 16 | # Macro to unify finding both the debug and release versions of the 17 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 18 | # macro. 19 | 20 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 21 | 22 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 23 | NAMES "g2o_${MYLIBRARYNAME}_d" 24 | PATHS 25 | ${G2O_ROOT}/lib/Debug 26 | ${G2O_ROOT}/lib 27 | $ENV{G2O_ROOT}/lib/Debug 28 | $ENV{G2O_ROOT}/lib 29 | /opt/ros/$ENV{ROS_DISTRO}/lib 30 | NO_DEFAULT_PATH 31 | ) 32 | 33 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 34 | NAMES "g2o_${MYLIBRARYNAME}_d" 35 | PATHS 36 | ~/Library/Frameworks 37 | /Library/Frameworks 38 | /usr/local/lib 39 | /usr/local/lib64 40 | /usr/lib 41 | /usr/lib64 42 | /opt/local/lib 43 | /sw/local/lib 44 | /sw/lib 45 | ) 46 | 47 | FIND_LIBRARY(${MYLIBRARY} 48 | NAMES "g2o_${MYLIBRARYNAME}" 49 | PATHS 50 | ${G2O_ROOT}/lib/Release 51 | ${G2O_ROOT}/lib 52 | $ENV{G2O_ROOT}/lib/Release 53 | $ENV{G2O_ROOT}/lib 54 | /opt/ros/$ENV{ROS_DISTRO}/lib 55 | NO_DEFAULT_PATH 56 | ) 57 | 58 | FIND_LIBRARY(${MYLIBRARY} 59 | NAMES "g2o_${MYLIBRARYNAME}" 60 | PATHS 61 | ~/Library/Frameworks 62 | /Library/Frameworks 63 | /usr/local/lib 64 | /usr/local/lib64 65 | /usr/lib 66 | /usr/lib64 67 | /opt/local/lib 68 | /sw/local/lib 69 | /sw/lib 70 | ) 71 | 72 | IF(NOT ${MYLIBRARY}_DEBUG) 73 | IF(MYLIBRARY) 74 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 75 | ENDIF(MYLIBRARY) 76 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 77 | 78 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 79 | 80 | # Find the core elements 81 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 82 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 83 | 84 | # Find the CLI library 85 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 86 | 87 | # Find the pluggable solvers 88 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 93 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 94 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 95 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 96 | 97 | # Find the predefined types 98 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 99 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 102 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 103 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 104 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 105 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D_ADDONS types_slam3d_addons) 106 | 107 | # G2O solvers declared found if we found at least one solver 108 | SET(G2O_SOLVERS_FOUND "NO") 109 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 110 | SET(G2O_SOLVERS_FOUND "YES") 111 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 112 | 113 | # G2O itself declared found if we found the core libraries and at least one solver 114 | SET(G2O_FOUND "NO") 115 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 116 | SET(G2O_FOUND "YES") 117 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 118 | -------------------------------------------------------------------------------- /cmake/FindGLM.cmake: -------------------------------------------------------------------------------- 1 | # FindGLM - attempts to locate the glm matrix/vector library. 2 | # 3 | # This module defines the following variables (on success): 4 | # GLM_INCLUDE_DIRS - where to find glm/glm.hpp 5 | # GLM_FOUND - if the library was successfully located 6 | # 7 | # It is trying a few standard installation locations, but can be customized 8 | # with the following variables: 9 | # GLM_ROOT_DIR - root directory of a glm installation 10 | # Headers are expected to be found in either: 11 | # /glm/glm.hpp OR 12 | # /include/glm/glm.hpp 13 | # This variable can either be a cmake or environment 14 | # variable. Note however that changing the value 15 | # of the environment varible will NOT result in 16 | # re-running the header search and therefore NOT 17 | # adjust the variables set by this module. 18 | #============================================================================= 19 | # Copyright 2012 Carsten Neumann 20 | # 21 | # Distributed under the OSI-approved BSD License (the "License"); 22 | # see accompanying file Copyright.txt for details. 23 | # 24 | # This software is distributed WITHOUT ANY WARRANTY; without even the 25 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 26 | # See the License for more information. 27 | #============================================================================= 28 | # (To distribute this file outside of CMake, substitute the full 29 | # License text for the above reference.) 30 | # default search dirs 31 | 32 | SET(_glm_HEADER_SEARCH_DIRS 33 | "/usr/include" 34 | "/usr/local/include" 35 | "${CMAKE_SOURCE_DIR}/includes" 36 | "C:/Program Files (x86)/glm" ) 37 | # check environment variable 38 | SET(_glm_ENV_ROOT_DIR "$ENV{GLM_ROOT_DIR}") 39 | IF(NOT GLM_ROOT_DIR AND _glm_ENV_ROOT_DIR) 40 | SET(GLM_ROOT_DIR "${_glm_ENV_ROOT_DIR}") 41 | ENDIF(NOT GLM_ROOT_DIR AND _glm_ENV_ROOT_DIR) 42 | # put user specified location at beginning of search 43 | IF(GLM_ROOT_DIR) 44 | SET(_glm_HEADER_SEARCH_DIRS "${GLM_ROOT_DIR}" 45 | "${GLM_ROOT_DIR}/include" 46 | ${_glm_HEADER_SEARCH_DIRS}) 47 | ENDIF(GLM_ROOT_DIR) 48 | # locate header 49 | FIND_PATH(GLM_INCLUDE_DIR "glm/glm.hpp" 50 | PATHS ${_glm_HEADER_SEARCH_DIRS}) 51 | INCLUDE(FindPackageHandleStandardArgs) 52 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(GLM DEFAULT_MSG 53 | GLM_INCLUDE_DIR) 54 | IF(GLM_FOUND) 55 | SET(GLM_INCLUDE_DIRS "${GLM_INCLUDE_DIR}") 56 | MESSAGE(STATUS "GLM_INCLUDE_DIR = ${GLM_INCLUDE_DIR}") 57 | ENDIF(GLM_FOUND) 58 | -------------------------------------------------------------------------------- /data/shader/rainbow.frag: -------------------------------------------------------------------------------- 1 | #version 330 2 | uniform vec2 z_range; 3 | uniform int z_clipping; 4 | uniform int color_mode; 5 | 6 | in vec4 frag_color; 7 | flat in ivec4 frag_info; 8 | in vec3 frag_world_position; 9 | 10 | layout (location=0) out vec4 color; 11 | layout (location=1) out ivec4 info; 12 | 13 | void main() { 14 | if(z_clipping != 0 && color_mode == 0 && (frag_world_position.z < z_range[0] || frag_world_position.z > z_range[1])) { 15 | discard; 16 | } 17 | 18 | color = frag_color; 19 | info = frag_info; 20 | } -------------------------------------------------------------------------------- /data/shader/rainbow.vert: -------------------------------------------------------------------------------- 1 | #version 330 2 | uniform float point_size; 3 | uniform float point_scale; 4 | uniform bool apply_keyframe_scale; 5 | uniform float keyframe_scale; 6 | uniform mat4 model_matrix; 7 | uniform mat4 view_matrix; 8 | uniform mat4 projection_matrix; 9 | 10 | uniform int color_mode; 11 | uniform vec4 material_color; 12 | 13 | uniform vec2 z_range; 14 | uniform ivec4 info_values; 15 | 16 | in vec3 vert_position; 17 | in vec3 vert_direction; // line direction 18 | in vec4 vert_color; 19 | in ivec4 vert_info; 20 | 21 | out vec4 frag_color; 22 | flat out ivec4 frag_info; 23 | out vec3 frag_world_position; 24 | 25 | vec3 turbo(in float x) { 26 | const vec4 kRedVec4 = vec4(0.13572138, 4.61539260, -42.66032258, 132.13108234); 27 | const vec4 kGreenVec4 = vec4(0.09140261, 2.19418839, 4.84296658, -14.18503333); 28 | const vec4 kBlueVec4 = vec4(0.10667330, 12.64194608, -60.58204836, 110.36276771); 29 | const vec2 kRedVec2 = vec2(-152.94239396, 59.28637943); 30 | const vec2 kGreenVec2 = vec2(4.27729857, 2.82956604); 31 | const vec2 kBlueVec2 = vec2(-89.90310912, 27.34824973); 32 | 33 | x = clamp(x, 0.0, 1.0); 34 | vec4 v4 = vec4(1.0, x, x * x, x * x * x); 35 | vec2 v2 = v4.zw * v4.z; 36 | return vec3( 37 | dot(v4, kRedVec4) + dot(v2, kRedVec2), 38 | dot(v4, kGreenVec4) + dot(v2, kGreenVec2), 39 | dot(v4, kBlueVec4) + dot(v2, kBlueVec2) 40 | ); 41 | } 42 | 43 | vec4 rainbow(vec3 position) { 44 | float p = (position.z - z_range[0]) / (z_range[1] - z_range[0]); 45 | return vec4(turbo(p), 1.0); 46 | } 47 | 48 | void main() { 49 | vec4 world_position; 50 | if(apply_keyframe_scale){ 51 | world_position = model_matrix * vec4(keyframe_scale * vert_position, 1.0); 52 | } 53 | else{ 54 | world_position = model_matrix * vec4(vert_position, 1.0); 55 | } 56 | frag_world_position = world_position.xyz; 57 | gl_Position = projection_matrix * view_matrix * world_position; 58 | 59 | frag_info = info_values; 60 | if(color_mode == 0) { 61 | frag_color = rainbow(frag_world_position); 62 | } else if(color_mode == 1) { 63 | frag_color = material_color; 64 | } else if(color_mode == 2) { 65 | frag_color = vert_color; 66 | frag_info = vert_info; 67 | } 68 | 69 | vec3 ndc = gl_Position.xyz / gl_Position.w; 70 | float z_dist = 1.0 - ndc.z; 71 | gl_PointSize = point_scale * point_size * z_dist; 72 | } -------------------------------------------------------------------------------- /data/shader/texture.frag: -------------------------------------------------------------------------------- 1 | #version 330 2 | uniform sampler2D texture_sampler; 3 | 4 | in vec2 texcoord; 5 | out vec4 color; 6 | 7 | void main() { 8 | // color = vec4(texcoord.xy, 0.0, 1.0); 9 | color = texture(texture_sampler, texcoord); 10 | } -------------------------------------------------------------------------------- /data/shader/texture.vert: -------------------------------------------------------------------------------- 1 | #version 330 2 | in vec3 vert_position; 3 | out vec2 texcoord; 4 | 5 | void main() { 6 | texcoord = 0.5 * vert_position.xy + vec2(0.5, 0.5); 7 | gl_Position = vec4(vert_position, 1.0); 8 | } -------------------------------------------------------------------------------- /docker/kinetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | ros-kinetic-geodesy ros-kinetic-pcl-ros ros-kinetic-nmea-msgs ros-kinetic-rviz \ 6 | ros-kinetic-tf-conversions ros-kinetic-libg2o libglm-dev libglfw3-dev \ 7 | && apt-get clean \ 8 | && rm -rf /var/lib/apt/lists/* 9 | 10 | RUN mkdir -p /root/catkin_ws/src 11 | WORKDIR /root/catkin_ws/src 12 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_init_workspace' 13 | RUN git clone https://github.com/koide3/ndt_omp.git 14 | RUN git clone https://github.com/koide3/hdl_graph_slam.git 15 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 16 | 17 | # RUN git clone https://github.com/koide3/interactive_slam.git --recursive 18 | COPY . /root/catkin_ws/src/interactive_slam/ 19 | WORKDIR /root/catkin_ws/src/interactive_slam 20 | RUN git submodule init 21 | RUN git submodule update 22 | 23 | WORKDIR /root/catkin_ws 24 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_make' 25 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 26 | 27 | RUN apt-get clean && rm -rf /var/lib/apt/lists/* 28 | 29 | WORKDIR / 30 | 31 | ENTRYPOINT ["/ros_entrypoint.sh"] 32 | CMD ["bash"] 33 | -------------------------------------------------------------------------------- /docker/kinetic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | clang-6.0 lld-6.0 libomp-dev \ 6 | ros-kinetic-geodesy ros-kinetic-pcl-ros ros-kinetic-nmea-msgs ros-kinetic-rviz \ 7 | ros-kinetic-tf-conversions ros-kinetic-libg2o libglm-dev libglfw3-dev \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN mkdir -p /root/catkin_ws/src 12 | WORKDIR /root/catkin_ws/src 13 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_init_workspace' 14 | RUN git clone https://github.com/koide3/ndt_omp.git 15 | RUN git clone https://github.com/koide3/hdl_graph_slam.git 16 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 17 | 18 | # RUN git clone https://github.com/koide3/interactive_slam.git --recursive 19 | COPY . /root/catkin_ws/src/interactive_slam/ 20 | WORKDIR /root/catkin_ws/src/interactive_slam 21 | RUN git submodule init 22 | RUN git submodule update 23 | 24 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld-6.0 50 25 | RUN update-alternatives --install /usr/bin/clang clang /usr/bin/clang-6.0 50 26 | RUN update-alternatives --install /usr/bin/clang++ clang++ /usr/bin/clang++-6.0 50 27 | 28 | WORKDIR /root/catkin_ws 29 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; CC=clang CXX=clang++ catkin_make' 30 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 31 | 32 | RUN apt-get clean && rm -rf /var/lib/apt/lists/* 33 | 34 | WORKDIR / 35 | 36 | ENTRYPOINT ["/ros_entrypoint.sh"] 37 | CMD ["bash"] 38 | -------------------------------------------------------------------------------- /docker/melodic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends && apt-get install -y --no-install-recommends wget nano build-essential ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs ros-melodic-rviz ros-melodic-tf-conversions ros-melodic-libg2o libglm-dev libglfw3-dev \ 4 | && apt-get clean \ 5 | && rm -rf /var/lib/apt/lists/* 6 | 7 | RUN mkdir -p /root/catkin_ws/src 8 | WORKDIR /root/catkin_ws/src 9 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' 10 | RUN git clone https://github.com/koide3/ndt_omp.git -b melodic 11 | RUN git clone https://github.com/koide3/hdl_graph_slam.git 12 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 13 | 14 | # RUN git clone https://github.com/koide3/interactive_slam.git --recursive 15 | COPY . /root/catkin_ws/src/interactive_slam/ 16 | WORKDIR /root/catkin_ws/src/interactive_slam 17 | RUN git submodule init 18 | RUN git submodule update 19 | 20 | 21 | WORKDIR /root/catkin_ws 22 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_make' 23 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 24 | 25 | WORKDIR / 26 | 27 | ENTRYPOINT ["/ros_entrypoint.sh"] 28 | CMD ["bash"] 29 | -------------------------------------------------------------------------------- /docker/melodic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | && apt-get install -y --no-install-recommends wget nano build-essential \ 5 | clang lld libomp-dev \ 6 | ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs ros-melodic-rviz \ 7 | ros-melodic-tf-conversions ros-melodic-libg2o libglm-dev libglfw3-dev \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | 12 | RUN mkdir -p /root/catkin_ws/src 13 | WORKDIR /root/catkin_ws/src 14 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' 15 | RUN git clone https://github.com/koide3/ndt_omp.git -b melodic 16 | RUN git clone https://github.com/koide3/hdl_graph_slam.git 17 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 18 | 19 | # RUN git clone https://github.com/koide3/interactive_slam.git --recursive 20 | COPY . /root/catkin_ws/src/interactive_slam/ 21 | WORKDIR /root/catkin_ws/src/interactive_slam 22 | RUN git submodule init 23 | RUN git submodule update 24 | 25 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld-6.0 50 26 | 27 | WORKDIR /root/catkin_ws 28 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; CC=clang CXX=clang++ catkin_make' 29 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 30 | 31 | WORKDIR / 32 | 33 | ENTRYPOINT ["/ros_entrypoint.sh"] 34 | CMD ["bash"] 35 | -------------------------------------------------------------------------------- /docker/noetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | && apt-get install -y --no-install-recommends wget nano build-essential \ 5 | git libomp-dev \ 6 | ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs ros-noetic-rviz \ 7 | ros-noetic-tf-conversions ros-noetic-libg2o libglm-dev libglfw3-dev \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN mkdir -p /root/catkin_ws/src 12 | WORKDIR /root/catkin_ws/src 13 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace' 14 | RUN git clone https://github.com/koide3/ndt_omp.git 15 | RUN git clone https://github.com/koide3/hdl_graph_slam.git 16 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 17 | 18 | # RUN git clone https://github.com/koide3/interactive_slam.git --recursive 19 | COPY . /root/catkin_ws/src/interactive_slam/ 20 | WORKDIR /root/catkin_ws/src/interactive_slam 21 | RUN git submodule init 22 | RUN git submodule update 23 | 24 | 25 | WORKDIR /root/catkin_ws 26 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make' 27 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 28 | 29 | WORKDIR / 30 | 31 | ENTRYPOINT ["/ros_entrypoint.sh"] 32 | CMD ["bash"] 33 | -------------------------------------------------------------------------------- /docker/noetic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | && apt-get install -y --no-install-recommends wget nano build-essential \ 5 | git clang lld libomp-dev \ 6 | ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs ros-noetic-rviz \ 7 | ros-noetic-tf-conversions ros-noetic-libg2o libglm-dev libglfw3-dev \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | 12 | RUN mkdir -p /root/catkin_ws/src 13 | WORKDIR /root/catkin_ws/src 14 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace' 15 | RUN git clone https://github.com/koide3/ndt_omp.git 16 | RUN git clone https://github.com/koide3/hdl_graph_slam.git 17 | RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive 18 | 19 | # RUN git clone https://github.com/koide3/interactive_slam.git --recursive 20 | COPY . /root/catkin_ws/src/interactive_slam/ 21 | WORKDIR /root/catkin_ws/src/interactive_slam 22 | RUN git submodule init 23 | RUN git submodule update 24 | 25 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld-10 50 26 | 27 | WORKDIR /root/catkin_ws 28 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; CC=clang CXX=clang++ catkin_make' 29 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 30 | 31 | WORKDIR / 32 | 33 | ENTRYPOINT ["/ros_entrypoint.sh"] 34 | CMD ["bash"] 35 | -------------------------------------------------------------------------------- /docker/run_with_x.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | sudo docker run -it --net=host --env=DISPLAY --env=QT_X11_NO_MITSHM=1 --volume=/tmp/.X11-unix:/tmp/.X11-unix:rw --rm -p 52698:52698 hdl_graph_slam bash 4 | -------------------------------------------------------------------------------- /include/glk/colormap.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_COLORMAP_HPP 2 | #define GLK_COLORMAP_HPP 3 | 4 | #include 5 | 6 | namespace glk { 7 | 8 | enum COLORMAP_TYPE { TURBO = 0, NUM_COLORMAPS }; 9 | 10 | Eigen::Vector4i colormap(COLORMAP_TYPE type, int x); 11 | Eigen::Vector4f colormapf(COLORMAP_TYPE type, float x); 12 | Eigen::Vector4i colormap_categorical(COLORMAP_TYPE type, int x, int num_categories); 13 | Eigen::Vector4f colormap_categoricalf(COLORMAP_TYPE type, int x, int num_categories); 14 | } // namespace glk 15 | 16 | #endif -------------------------------------------------------------------------------- /include/glk/drawble.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_DRAWABLE_HPP 2 | #define GLK_DRAWABLE_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace glk { 11 | 12 | /** 13 | * @brief Drawable object interface 14 | * 15 | */ 16 | class Drawable { 17 | public: 18 | virtual ~Drawable() {} 19 | 20 | virtual void draw(glk::GLSLShader& shader) const = 0; 21 | }; 22 | 23 | } // namespace glk 24 | 25 | #endif -------------------------------------------------------------------------------- /include/glk/frame_buffer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_FRAME_BUFFER_HPP 2 | #define GLK_FRAME_BUFFER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace glk { 11 | 12 | /** 13 | * @brief OpenGL FrameBuffer wrapper class 14 | * 15 | */ 16 | class FrameBuffer { 17 | public: 18 | FrameBuffer(const Eigen::Vector2i& size); 19 | 20 | ~FrameBuffer(); 21 | 22 | void bind(); 23 | void unbind() const; 24 | 25 | void add_color_buffer(GLuint internal_format = GL_RGBA, GLuint format = GL_RGBA, GLuint type = GL_UNSIGNED_BYTE); 26 | 27 | const Texture& color() { return *color_buffers[0]; } 28 | const Texture& color(int i) { return *color_buffers[i]; } 29 | const Texture& depth() { return *depth_buffer; } 30 | 31 | private: 32 | int width; 33 | int height; 34 | 35 | GLint viewport[4]; 36 | 37 | std::vector> color_buffers; 38 | std::shared_ptr depth_buffer; 39 | 40 | GLuint frame_buffer; 41 | }; 42 | 43 | } // namespace glk 44 | 45 | #endif -------------------------------------------------------------------------------- /include/glk/glsl_shader.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_GLSL_SHADER_HPP 2 | #define GLK_GLSL_SHADER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace glk { 12 | 13 | /** 14 | * @brief GLSL shader class 15 | * 16 | */ 17 | class GLSLShader { 18 | public: 19 | GLSLShader() {} 20 | 21 | /** 22 | * @brief load GLSL shader from files 23 | * 24 | * @param shader_path "shader_path".vert and "shader_path".frag will be loaded 25 | * @return if the shader is successfully loaded 26 | */ 27 | bool init(const std::string& shader_path); 28 | 29 | /** 30 | * @brief bind the shader 31 | */ 32 | void use() const { glUseProgram(shader_program); } 33 | 34 | /** @brief find attribute variable location **/ 35 | GLint attrib(const std::string& name); 36 | /** @brief find uniform variable location **/ 37 | GLint uniform(const std::string& name); 38 | 39 | 40 | /*** getter and setter for uniforms ***/ 41 | Eigen::Vector4f get_uniform4f(const std::string& name) { 42 | Eigen::Vector4f vec; 43 | glGetUniformfv(shader_program, uniform(name), vec.data()); 44 | return vec; 45 | } 46 | 47 | Eigen::Matrix4f get_uniform_matrix4f(const std::string& name) { 48 | Eigen::Matrix4f mat; 49 | glGetUniformfv(shader_program, uniform(name), mat.data()); 50 | return mat; 51 | } 52 | 53 | float get_uniform1f(const std::string& name){ 54 | float res; 55 | glGetUniformfv(shader_program, uniform(name), &res); 56 | return res; 57 | } 58 | 59 | void set_uniform(const std::string& name, int value) { glUniform1i(uniform(name), value); } 60 | void set_uniform(const std::string& name, float value) { glUniform1f(uniform(name), value); } 61 | void set_uniform(const std::string& name, const Eigen::Vector2f& vector) { glUniform2fv(uniform(name), 1, vector.data()); } 62 | void set_uniform(const std::string& name, const Eigen::Vector3f& vector) { glUniform3fv(uniform(name), 1, vector.data()); } 63 | void set_uniform(const std::string& name, const Eigen::Vector4f& vector) { glUniform4fv(uniform(name), 1, vector.data()); } 64 | void set_uniform(const std::string& name, const Eigen::Vector4i& vector) { glUniform4iv(uniform(name), 1, vector.data()); } 65 | void set_uniform(const std::string& name, const Eigen::Matrix4f& matrix) { glUniformMatrix4fv(uniform(name), 1, GL_FALSE, matrix.data()); } 66 | 67 | private: 68 | GLuint read_shader_from_file(const std::string& filename, GLuint shader_type); 69 | 70 | private: 71 | GLuint shader_program; 72 | std::unordered_map attrib_cache; 73 | std::unordered_map uniform_cache; 74 | }; 75 | 76 | } // namespace glk 77 | 78 | #endif -------------------------------------------------------------------------------- /include/glk/lines.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_LINES_HPP 2 | #define GLK_LINES_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace glk { 13 | 14 | /** 15 | * @brief A class to draw a set of lines 16 | * 17 | */ 18 | class Lines : public Drawable { 19 | public: 20 | Lines(float line_width, const std::vector>& vertices, 21 | const std::vector>& colors = std::vector>(), 22 | const std::vector>& infos = std::vector>() 23 | ); 24 | virtual ~Lines() override; 25 | 26 | virtual void draw(glk::GLSLShader& shader) const override; 27 | 28 | private: 29 | Lines(const Lines&); 30 | Lines& operator=(const Lines&); 31 | 32 | private: 33 | int num_vertices; 34 | int num_indices; 35 | 36 | GLuint vao; // vertex array object 37 | GLuint vbo; // vertices 38 | GLuint cbo; // colors 39 | GLuint ibo; // infos 40 | GLuint ebo; // elements 41 | }; 42 | } 43 | 44 | #endif -------------------------------------------------------------------------------- /include/glk/loaders/ply_loader.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PLY_LOADER_HPP 2 | #define GLK_PLY_LOADER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace glk { 8 | 9 | /** 10 | * @brief Loader class for PLY file format 11 | * @note if it fails to read a file, the members become empty 12 | */ 13 | class PLYLoader { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | 17 | PLYLoader(const std::string& filename); 18 | 19 | public: 20 | std::vector> vertices; 21 | std::vector> normals; 22 | std::vector indices; 23 | }; 24 | 25 | } // namespace glk 26 | 27 | #endif -------------------------------------------------------------------------------- /include/glk/mesh.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_MESH_HPP 2 | #define GLK_MESH_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace glk { 12 | 13 | /** 14 | * @brief Mesh class 15 | * 16 | */ 17 | class Mesh : public Drawable { 18 | public: 19 | Mesh(const std::vector>& vertices, const std::vector>& normals, const std::vector& indices); 20 | virtual ~Mesh(); 21 | 22 | virtual void draw(glk::GLSLShader& shader) const override; 23 | 24 | private: 25 | Mesh(const Mesh&); 26 | Mesh& operator=(const Mesh&); 27 | 28 | private: 29 | int num_vertices; 30 | int num_indices; 31 | 32 | GLuint vao; 33 | GLuint vbo; 34 | GLuint nbo; 35 | GLuint ebo; 36 | }; 37 | 38 | } // namespace glk 39 | 40 | #endif -------------------------------------------------------------------------------- /include/glk/mesh_utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_MESH_UTILS_HPP 2 | #define GLK_PRIMITIVES_MESH_UTILS_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace glk { 9 | 10 | /** 11 | * @brief A class to make a mesh to be flat (so-called flat shading) 12 | * 13 | */ 14 | class Flatize { 15 | public: 16 | Flatize(const std::vector>& vertices_, const std::vector& indices_) { 17 | vertices.resize(indices_.size()); 18 | normals.resize(indices_.size()); 19 | indices.resize(indices_.size()); 20 | for (int i = 0; i < indices_.size(); i += 3) { 21 | Eigen::Vector3f v1 = vertices_[indices_[i]]; 22 | Eigen::Vector3f v2 = vertices_[indices_[i + 1]]; 23 | Eigen::Vector3f v3 = vertices_[indices_[i + 2]]; 24 | 25 | Eigen::Vector3f n = (v2 - v1).cross(v3 - v2); 26 | 27 | for (int j = 0; j < 3; j++) { 28 | vertices[i + j] = vertices_[indices_[i + j]]; 29 | normals[i + j] = n; 30 | indices[i + j] = i + j; 31 | } 32 | } 33 | } 34 | 35 | public: 36 | std::vector> vertices; 37 | std::vector> normals; 38 | std::vector indices; 39 | }; 40 | 41 | /** 42 | * @brief A class to estimate normals of vertices 43 | * 44 | */ 45 | class NormalEstimater { 46 | public: 47 | NormalEstimater(const std::vector>& vertices, const std::vector& indices) { 48 | normals.resize(vertices.size(), Eigen::Vector3f::Zero()); 49 | for (int i = 0; i < indices.size(); i += 3) { 50 | Eigen::Vector3f v1 = vertices[indices[i]]; 51 | Eigen::Vector3f v2 = vertices[indices[i + 1]]; 52 | Eigen::Vector3f v3 = vertices[indices[i + 2]]; 53 | 54 | Eigen::Vector3f n = (v2 - v1).cross(v3 - v2); 55 | 56 | normals[indices[i]] += n; 57 | normals[indices[i + 1]] += n; 58 | normals[indices[i + 2]] += n; 59 | } 60 | 61 | for (auto& normal : normals) { 62 | normal.normalize(); 63 | } 64 | } 65 | 66 | public: 67 | std::vector> normals; 68 | }; 69 | 70 | } // namespace glk 71 | 72 | #endif -------------------------------------------------------------------------------- /include/glk/pointcloud_buffer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_POINTCLOUD_BUFFER_HPP 2 | #define GLK_POINTCLOUD_BUFFER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace glk { 12 | 13 | class PointCloudBuffer { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | using Ptr = std::shared_ptr; 17 | 18 | PointCloudBuffer(const std::string& cloud_filename); 19 | PointCloudBuffer(const pcl::PointCloud::ConstPtr& cloud); 20 | ~PointCloudBuffer(); 21 | 22 | void draw(glk::GLSLShader& shader); 23 | 24 | private: 25 | GLuint vao; 26 | GLuint vbo; 27 | int stride; 28 | int num_points; 29 | }; 30 | 31 | } 32 | 33 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/cone.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_CONE_HPP 2 | #define GLK_PRIMITIVES_CONE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace glk { 9 | 10 | class Cone { 11 | public: 12 | Cone(int div=10) { 13 | vertices.push_back(Eigen::Vector3f::Zero()); 14 | vertices.push_back(Eigen::Vector3f::UnitZ()); 15 | 16 | double step = 2.0 * M_PI / div; 17 | for(int i = 0; i < div; i++) { 18 | double rad = step * i; 19 | vertices.push_back(Eigen::Vector3f(std::cos(rad), std::sin(rad), 0.0f)); 20 | 21 | int current_index = i + 2; 22 | int next_index = ((i + 1) % div) + 2; 23 | 24 | indices.push_back(0); 25 | indices.push_back(current_index); 26 | indices.push_back(next_index); 27 | 28 | indices.push_back(current_index); 29 | indices.push_back(1); 30 | indices.push_back(next_index); 31 | } 32 | } 33 | 34 | public: 35 | std::vector> vertices; 36 | std::vector indices; 37 | }; 38 | 39 | } // namespace glk 40 | 41 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/coordinate_system.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_COORDINATE_SYSTEM_HPP 2 | #define GLK_PRIMITIVES_COORDINATE_SYSTEM_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace glk { 12 | 13 | class CoordinateSystem { 14 | public: 15 | CoordinateSystem() { 16 | vertices.push_back(Eigen::Vector3f::Zero()); 17 | vertices.push_back(Eigen::Vector3f::UnitX()); 18 | vertices.push_back(Eigen::Vector3f::Zero()); 19 | vertices.push_back(Eigen::Vector3f::UnitY()); 20 | vertices.push_back(Eigen::Vector3f::Zero()); 21 | vertices.push_back(Eigen::Vector3f::UnitZ()); 22 | 23 | colors.push_back(Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f)); 24 | colors.push_back(Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f)); 25 | colors.push_back(Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f)); 26 | colors.push_back(Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f)); 27 | colors.push_back(Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f)); 28 | colors.push_back(Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f)); 29 | } 30 | 31 | public: 32 | std::vector> vertices; 33 | std::vector> colors; 34 | }; 35 | } // namespace glk 36 | 37 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/cube.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_CUBE_HPP 2 | #define GLK_PRIMITIVES_CUBE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace glk { 9 | 10 | class Cube { 11 | public: 12 | Cube() { 13 | // 6 ------ 7 X 14 | // /| /| | 15 | // 4 ------ 5 | 0 --- Z 16 | // | | | | / 17 | // | 2 -----| 3 Y 18 | // |/ |/ 19 | // 0 ------ 1 20 | std::vector> vertices = { 21 | Eigen::Vector3f(-0.5f, -0.5f, -0.5f), // 0 22 | Eigen::Vector3f(-0.5f, -0.5f, 0.5f), // 1 23 | Eigen::Vector3f(-0.5f, 0.5f, -0.5f), // 2 24 | Eigen::Vector3f(-0.5f, 0.5f, 0.5f), // 3 25 | Eigen::Vector3f(0.5f, -0.5f, -0.5f), // 4 26 | Eigen::Vector3f(0.5f, -0.5f, 0.5f), // 5 27 | Eigen::Vector3f(0.5f, 0.5f, -0.5f), // 6 28 | Eigen::Vector3f(0.5f, 0.5f, 0.5f), // 7 29 | }; 30 | 31 | std::vector> normals(vertices.size()); 32 | for (int i = 0; i < vertices.size(); i++) { 33 | normals[i] = vertices[i].normalized(); 34 | } 35 | 36 | std::vector indices = {0, 1, 2, 1, 3, 2, 1, 5, 3, 3, 5, 7, 0, 4, 1, 1, 4, 5, 3, 6, 2, 3, 7, 6, 0, 2, 4, 2, 6, 4, 4, 6, 5, 5, 6, 7}; 37 | 38 | this->vertices.swap(vertices); 39 | this->indices.swap(indices); 40 | } 41 | 42 | public: 43 | std::vector> vertices; 44 | std::vector> normals; 45 | std::vector indices; 46 | }; 47 | 48 | } // namespace glk 49 | 50 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/grid.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_GRID_HPP 2 | #define GLK_PRIMITIVES_GRID_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace glk { 8 | 9 | class Grid { 10 | public: 11 | Grid(double half_extent = 5.0, double step = 1.0) { 12 | for(double x = -half_extent; x <= half_extent + 1e-9; x+=step) { 13 | vertices.push_back(Eigen::Vector3f(x, -half_extent, 0.0f)); 14 | vertices.push_back(Eigen::Vector3f(x, half_extent, 0.0f)); 15 | vertices.push_back(Eigen::Vector3f(-half_extent, x, 0.0f)); 16 | vertices.push_back(Eigen::Vector3f(half_extent, x, 0.0f)); 17 | } 18 | } 19 | 20 | public: 21 | std::vector> vertices; 22 | }; 23 | } 24 | 25 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/icosahedron.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_ICOSAHEDRON_HPP 2 | #define GLK_PRIMITIVES_ICOSAHEDRON_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace glk { 9 | 10 | class Icosahedron { 11 | public: 12 | Icosahedron() { 13 | double t = (1.0 + std::sqrt(5.0)) / 2.0; 14 | 15 | std::vector> vertices = { 16 | Eigen::Vector3f(-1, t, 0), Eigen::Vector3f(1, t, 0), Eigen::Vector3f(-1, -t, 0), Eigen::Vector3f(1, -t, 0), 17 | Eigen::Vector3f(0, -1, t), Eigen::Vector3f(0, 1, t), Eigen::Vector3f(0, -1, -t), Eigen::Vector3f(0, 1, -t), 18 | Eigen::Vector3f(t, 0, -1), Eigen::Vector3f(t, 0, 1), Eigen::Vector3f(-t, 0, -1), Eigen::Vector3f(-t, 0, 1)}; 19 | 20 | std::vector> normals(vertices.size()); 21 | for(int i = 0; i < vertices.size(); i++) { 22 | normals[i] = vertices[i].normalized(); 23 | } 24 | 25 | std::vector indices = {0, 11, 5, 0, 5, 1, 0, 1, 7, 0, 7, 10, 0, 10, 11, 1, 5, 9, 5, 11, 26 | 4, 11, 10, 2, 10, 7, 6, 7, 1, 8, 3, 9, 4, 3, 4, 2, 3, 2, 6, 3, 27 | 6, 8, 3, 8, 9, 4, 9, 5, 2, 4, 11, 6, 2, 10, 8, 6, 7, 9, 8, 1}; 28 | 29 | this->vertices.swap(vertices); 30 | this->normals.swap(normals); 31 | this->indices.swap(indices); 32 | } 33 | 34 | void subdivide() { 35 | std::vector new_indices; 36 | for(int i = 0; i < indices.size(); i += 3) { 37 | int a = insert_middle_point(indices[i], indices[i + 1]); 38 | int b = insert_middle_point(indices[i + 1], indices[i + 2]); 39 | int c = insert_middle_point(indices[i + 2], indices[i]); 40 | 41 | std::vector tessellated = {indices[i], a, c, indices[i + 1], b, a, indices[i + 2], c, b, a, b, c}; 42 | 43 | new_indices.insert(new_indices.end(), tessellated.begin(), tessellated.end()); 44 | } 45 | 46 | indices.swap(new_indices); 47 | 48 | normals.resize(vertices.size()); 49 | for(int i = 0; i < vertices.size(); i++) { 50 | normals[i] = vertices[i].normalized(); 51 | } 52 | } 53 | 54 | void spherize() { 55 | for(auto& vertex : vertices) { 56 | vertex.normalize(); 57 | } 58 | 59 | normals.resize(vertices.size()); 60 | for(int i = 0; i < vertices.size(); i++) { 61 | normals[i] = vertices[i].normalized(); 62 | } 63 | } 64 | 65 | private: 66 | int insert_middle_point(int v1, int v2) { 67 | int smaller = std::min(v1, v2); 68 | int greater = std::max(v1, v2); 69 | int key = (smaller << 16) + greater; 70 | 71 | auto found = middle_points_cache.find(key); 72 | if(found != middle_points_cache.end()) { 73 | return found->second; 74 | } 75 | 76 | Eigen::Vector3f new_v = (vertices[v1] + vertices[v2]) / 2.0f; 77 | vertices.push_back(new_v); 78 | 79 | middle_points_cache.insert(found, std::make_pair(key, vertices.size() - 1)); 80 | return vertices.size() - 1; 81 | } 82 | 83 | public: 84 | std::vector> vertices; 85 | std::vector> normals; 86 | std::vector indices; 87 | 88 | std::map middle_points_cache; 89 | }; 90 | 91 | } // namespace glk 92 | 93 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/primitives.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_HPP 2 | #define GLK_PRIMITIVES_HPP 3 | 4 | #include 5 | 6 | namespace glk { 7 | 8 | class Primitives { 9 | private: 10 | Primitives() { meshes.resize(NUM_PRIMITIVES, nullptr); } 11 | 12 | public: 13 | enum PrimitiveType { ICOSAHEDRON = 0, SPHERE, CUBE, CONE, GRID, COORDINATE_SYSTEM, BUNNY, NUM_PRIMITIVES }; 14 | 15 | static Primitives *instance() { 16 | if (instance_ == nullptr) { 17 | instance_ = new Primitives(); 18 | } 19 | return instance_; 20 | } 21 | 22 | const glk::Drawable &primitive(PrimitiveType type); 23 | 24 | private: 25 | static Primitives *instance_; 26 | 27 | std::vector> meshes; 28 | }; 29 | } // namespace glk 30 | 31 | #endif -------------------------------------------------------------------------------- /include/glk/texture.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_TEXTURE_HPP 2 | #define GLK_TEXTURE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace glk { 9 | 10 | /** 11 | * @brief OpenGL texture wrapper 12 | * 13 | */ 14 | class Texture { 15 | public: 16 | Texture(const Eigen::Vector2i& size, GLuint internal_format, GLuint format, GLuint type) : width(size[0]), height(size[1]) { 17 | glGenTextures(1, &texture); 18 | glBindTexture(GL_TEXTURE_2D, texture); 19 | glTexImage2D(GL_TEXTURE_2D, 0, internal_format, size[0], size[1], 0, format, type, 0); 20 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); 21 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); 22 | glBindTexture(GL_TEXTURE_2D, 0); 23 | } 24 | 25 | ~Texture() { glDeleteRenderbuffers(1, &texture); } 26 | 27 | GLuint id() const { return texture; } 28 | Eigen::Vector2i size() const { return Eigen::Vector2i(width, height); } 29 | 30 | template 31 | std::vector read_pixels(GLuint format = GL_RGBA, GLuint type = GL_UNSIGNED_BYTE) const { 32 | std::vector pixels(width * height * 4); 33 | glBindTexture(GL_TEXTURE_2D, texture); 34 | glGetTexImage(GL_TEXTURE_2D, 0, format, type, pixels.data()); 35 | return pixels; 36 | } 37 | 38 | private: 39 | int width; 40 | int height; 41 | GLuint texture; 42 | }; 43 | } // namespace glk 44 | 45 | #endif -------------------------------------------------------------------------------- /include/glk/texture_renderer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_TEXTURE_RENDERER_HPP 2 | #define GLK_TEXTURE_RENDERER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace glk { 8 | 9 | /** 10 | * @brief TextureRenderer 11 | * 12 | */ 13 | class TextureRenderer { 14 | public: 15 | TextureRenderer(const std::string& data_directory) { 16 | std::vector> vertices = {Eigen::Vector3f(-1.0f, -1.0f, 0.0f), Eigen::Vector3f(1.0f, -1.0f, 0.0f), Eigen::Vector3f(-1.0f, 1.0f, 0.0f), Eigen::Vector3f(1.0f, 1.0f, 0.0f)}; 17 | 18 | glGenVertexArrays(1, &vao); 19 | glBindVertexArray(vao); 20 | 21 | glGenBuffers(1, &vbo); 22 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 23 | glBufferData(GL_ARRAY_BUFFER, sizeof(GL_FLOAT) * 3 * vertices.size(), vertices.data(), GL_STATIC_DRAW); 24 | 25 | if (!shader.init(data_directory + "/shader/texture")) { 26 | return; 27 | } 28 | 29 | shader.use(); 30 | GLint position_loc = shader.attrib("vert_position"); 31 | 32 | glBindVertexArray(vao); 33 | glEnableVertexAttribArray(position_loc); 34 | 35 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 36 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0); 37 | 38 | glBindBuffer(GL_ARRAY_BUFFER, 0); 39 | // glDisableVertexAttribArray(0); 40 | } 41 | 42 | ~TextureRenderer() { 43 | glDeleteVertexArrays(1, &vao); 44 | glDeleteBuffers(1, &vbo); 45 | } 46 | 47 | void draw(GLuint texture) { 48 | shader.use(); 49 | 50 | glEnable(GL_TEXTURE_2D); 51 | glBindTexture(GL_TEXTURE_2D, texture); 52 | 53 | glBindVertexArray(vao); 54 | glDrawArrays(GL_TRIANGLE_STRIP, 0, 4); 55 | glBindBuffer(GL_ARRAY_BUFFER, 0); 56 | 57 | glDisable(GL_TEXTURE_2D); 58 | } 59 | 60 | private: 61 | GLuint vao; 62 | GLuint vbo; 63 | 64 | glk::GLSLShader shader; 65 | }; 66 | 67 | } // namespace glk 68 | 69 | #endif -------------------------------------------------------------------------------- /include/guik/camera_control.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_CAMERA_CONTROL_HPP 2 | #define GLK_CAMERA_CONTROL_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace guik { 10 | 11 | /** 12 | * @brief A class to contol camera position with mouse 13 | * 14 | */ 15 | class CameraControl { 16 | public: 17 | virtual ~CameraControl() {} 18 | 19 | /** @brief mouse button callback */ 20 | virtual void mouse(const Eigen::Vector2i& p, int button, bool down) = 0; 21 | 22 | /** @brief mouse dragging callback */ 23 | virtual void drag(const Eigen::Vector2i& p, int button) = 0; 24 | 25 | /** @brief mouse scroll callback */ 26 | virtual void scroll(const Eigen::Vector2f& rel) = 0; 27 | 28 | /** @brief camera view matrix */ 29 | virtual Eigen::Matrix4f view_matrix() const = 0; 30 | }; 31 | 32 | /** 33 | * @brief A simple arctic camera control implementation 34 | * 35 | */ 36 | class ArcCameraControl : public CameraControl { 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | 40 | ArcCameraControl(); 41 | 42 | virtual ~ArcCameraControl() override; 43 | 44 | void mouse(const Eigen::Vector2i& p, int button, bool down) override; 45 | void drag(const Eigen::Vector2i& p, int button) override; 46 | void scroll(const Eigen::Vector2f& rel) override; 47 | 48 | Eigen::Quaternionf rotation() const; 49 | Eigen::Matrix4f view_matrix() const override; 50 | 51 | private: 52 | Eigen::Vector3f center; 53 | double distance; 54 | 55 | Eigen::Vector2i drag_last_pos; 56 | 57 | bool left_button_down; 58 | double theta; 59 | double phi; 60 | 61 | bool middle_button_down; 62 | }; 63 | 64 | } // namespace guik 65 | 66 | #endif -------------------------------------------------------------------------------- /include/guik/gl_canvas.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_GL_CANVAS_CANVAS_HPP 2 | #define GLK_GL_CANVAS_CANVAS_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | namespace guik { 14 | 15 | /** 16 | * @brief OpenGL canvas for imgui 17 | * 18 | */ 19 | class GLCanvas { 20 | public: 21 | GLCanvas(const std::string& data_directory, const Eigen::Vector2i& size); 22 | 23 | bool ready() const; 24 | 25 | void reset_camera(); 26 | void set_size(const Eigen::Vector2i& size); 27 | void mouse_control(); 28 | 29 | void bind(bool clear_buffers = true); 30 | void unbind(); 31 | 32 | void render_to_screen(int color_buffer_id = 0); 33 | 34 | Eigen::Vector4i pick_info(const Eigen::Vector2i& p, int window = 2) const; 35 | float pick_depth(const Eigen::Vector2i& p, int window = 2) const; 36 | Eigen::Vector3f unproject(const Eigen::Vector2i& p, float depth) const; 37 | 38 | void draw_ui(); 39 | void show_projection_setting(); 40 | 41 | public: 42 | Eigen::Vector2i size; 43 | std::unique_ptr shader; 44 | std::unique_ptr frame_buffer; 45 | std::unique_ptr texture_renderer; 46 | 47 | std::unique_ptr camera_control; 48 | std::unique_ptr projection_control; 49 | 50 | private: 51 | float point_size; 52 | float keyframe_scale; 53 | float min_z; 54 | float max_z; 55 | bool z_clipping; 56 | }; 57 | 58 | } // namespace guik 59 | 60 | #endif -------------------------------------------------------------------------------- /include/guik/imgui_application.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GUIK_IMGUI_APPLICATION_HPP 2 | #define GUIK_IMGUI_APPLICATION_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | class GLFWwindow; 11 | 12 | namespace guik { 13 | class Application { 14 | public: 15 | Application(); 16 | virtual ~Application(); 17 | 18 | virtual bool init(const char* window_name, const Eigen::Vector2i& size, const char* glsl_version = "#version 330"); 19 | 20 | void run(); 21 | 22 | void close(); 23 | 24 | Eigen::Vector2i framebuffer_size(); 25 | virtual void framebuffer_size_callback(const Eigen::Vector2i& size); 26 | 27 | inline void set_max_frame_rate(double frame_rate) { max_frame_rate = frame_rate; } 28 | 29 | virtual void draw_ui(); 30 | 31 | virtual void draw_gl(); 32 | 33 | protected: 34 | GLFWwindow* window; 35 | double max_frame_rate; 36 | }; 37 | 38 | } // namespace guik 39 | 40 | #endif -------------------------------------------------------------------------------- /include/guik/model_control.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GUIK_MODEL_CONTROL_HPP 2 | #define GUIK_MODEL_CONTROL_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace guik { 11 | 12 | class ModelControl { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | 16 | ModelControl(const std::string& name); 17 | 18 | void draw_ui(); 19 | Eigen::Matrix4f model_matrix() const; 20 | 21 | private: 22 | std::string name; 23 | Eigen::Affine3f pose; 24 | }; 25 | 26 | } // namespace guik 27 | 28 | #endif -------------------------------------------------------------------------------- /include/guik/progress_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GUIK_PROGRESS_INTERFACE_HPP 2 | #define GUIK_PROGRESS_INTERFACE_HPP 3 | 4 | #include 5 | 6 | namespace guik { 7 | 8 | struct ProgressInterface { 9 | ProgressInterface() {} 10 | virtual ~ProgressInterface() {} 11 | 12 | virtual void set_title(const std::string& title) {} 13 | virtual void set_text(const std::string& text) {} 14 | virtual void set_maximum(int max) {} 15 | virtual void set_current(int current) {} 16 | virtual void increment() {} 17 | }; 18 | 19 | } // namespace guik 20 | 21 | #endif -------------------------------------------------------------------------------- /include/guik/progress_modal.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GUIK_PROGRESS_MODAL_HPP 2 | #define GUIK_PROGRESS_MODAL_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | namespace guik { 16 | 17 | class ProgressModal : public ProgressInterface { 18 | public: 19 | ProgressModal(const std::string& modal_name) : modal_name(modal_name), running(false), max(0), current(0) {} 20 | virtual ~ProgressModal() override { 21 | if (thread.joinable()) { 22 | thread.join(); 23 | } 24 | } 25 | 26 | virtual void set_title(const std::string& title) override { 27 | std::lock_guard lock(mutex); 28 | this->title = title; 29 | } 30 | 31 | virtual void set_text(const std::string& text) override { 32 | std::lock_guard lock(mutex); 33 | this->text = text; 34 | } 35 | 36 | virtual void set_maximum(int max) override { this->max = max; } 37 | virtual void set_current(int current) override { this->current = current; } 38 | virtual void increment() override { current++; } 39 | 40 | template 41 | void open(const std::string& task_name, const std::function& task) { 42 | this->task_name = task_name; 43 | this->title.clear(); 44 | this->text.clear(); 45 | 46 | ImGui::OpenPopup(modal_name.c_str()); 47 | result_.clear(); 48 | running = true; 49 | current = 0; 50 | 51 | thread = std::thread([this, task]() { 52 | result_ = task(*this); 53 | running = false; 54 | }); 55 | } 56 | 57 | template 58 | T result() { 59 | T ret = boost::any_cast(result_); 60 | result_.clear(); 61 | return ret; 62 | } 63 | 64 | bool run(const std::string& task_name) { 65 | if(task_name != this->task_name) { 66 | return false; 67 | } 68 | 69 | bool terminated = false; 70 | if (ImGui::BeginPopupModal(modal_name.c_str(), nullptr, ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoTitleBar)) { 71 | { 72 | std::lock_guard lock(mutex); 73 | if (!title.empty()) { 74 | ImGui::Text(title.c_str()); 75 | } 76 | 77 | ImGui::Text("%c %s", "|/-\\"[(int)(ImGui::GetTime() / 0.05f) & 3], text.c_str()); 78 | } 79 | 80 | float fraction = current / static_cast(max); 81 | ImGui::ProgressBar(fraction, ImVec2(128, 16)); 82 | 83 | if (!running) { 84 | thread.join(); 85 | ImGui::CloseCurrentPopup(); 86 | terminated = true; 87 | } 88 | ImGui::EndPopup(); 89 | } 90 | 91 | return terminated; 92 | } 93 | 94 | private: 95 | std::mutex mutex; 96 | std::string modal_name; 97 | std::string title; 98 | std::string text; 99 | 100 | std::atomic_bool running; 101 | std::atomic_int max; 102 | std::atomic_int current; 103 | 104 | std::string task_name; 105 | 106 | std::thread thread; 107 | boost::any result_; 108 | }; 109 | 110 | } // namespace guik 111 | 112 | #endif -------------------------------------------------------------------------------- /include/guik/projection_control.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PROJECTION_CONTROL_HPP 2 | #define PROJECTION_CONTROL_HPP 3 | 4 | #include 5 | 6 | namespace guik { 7 | 8 | class ProjectionControl { 9 | public: 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | ProjectionControl(const Eigen::Vector2i& size); 13 | ~ProjectionControl(); 14 | 15 | void set_size(const Eigen::Vector2i& size) { 16 | this->size = size; 17 | } 18 | 19 | Eigen::Matrix4f projection_matrix() const; 20 | 21 | void draw_ui(); 22 | 23 | void show(); 24 | 25 | private: 26 | bool show_window; 27 | Eigen::Vector2i size; 28 | 29 | int projection_mode; 30 | 31 | float fovy; 32 | float width; 33 | float near; 34 | float far; 35 | }; 36 | 37 | } 38 | 39 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/automatic_loop_close_window.hpp: -------------------------------------------------------------------------------- 1 | #ifndef AUTOMATIC_LOOP_CLOSE_WINDOW_HPP 2 | #define AUTOMATIC_LOOP_CLOSE_WINDOW_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace hdl_graph_slam { 17 | 18 | class AutomaticLoopCloseWindow { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | 22 | AutomaticLoopCloseWindow(std::shared_ptr& graph); 23 | ~AutomaticLoopCloseWindow(); 24 | 25 | void draw_ui(); 26 | 27 | void draw_gl(glk::GLSLShader& shader); 28 | 29 | void show(); 30 | 31 | void start(); 32 | 33 | void close(); 34 | 35 | private: 36 | void loop_detection(); 37 | std::vector find_loop_candidates(const KeyFrameView::Ptr& keyframe); 38 | 39 | private: 40 | bool show_window; 41 | std::shared_ptr& graph; 42 | 43 | std::mutex loop_detection_mutex; 44 | std::thread loop_detection_thread; 45 | 46 | std::atomic_bool running; 47 | int loop_detection_source; 48 | 49 | KeyFrameView::Ptr loop_source; 50 | std::vector loop_candidates; 51 | 52 | float fitness_score_thresh; 53 | float fitness_score_max_range; 54 | 55 | int search_method; 56 | float distance_thresh; 57 | float accum_distance_thresh; 58 | 59 | RegistrationMethods registration_method; 60 | RobustKernels robust_kernel; 61 | 62 | bool optimize; 63 | }; 64 | } 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/edge_refinement_window.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EDGE_REFINEMENT_WINDOW_HPP 2 | #define EDGE_REFINEMENT_WINDOW_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace g2o { 18 | class EdgeSE3; 19 | } 20 | 21 | namespace hdl_graph_slam { 22 | 23 | struct EdgeInfo { 24 | public: 25 | EdgeInfo(g2o::EdgeSE3* edge); 26 | 27 | bool operator<(const EdgeInfo& rhs) const; 28 | 29 | bool operator>(const EdgeInfo& rhs) const; 30 | 31 | double error() const; 32 | 33 | double score() const; 34 | 35 | void update(); 36 | 37 | public: 38 | g2o::EdgeSE3* edge; 39 | 40 | int begin; 41 | int end; 42 | int num_evaluations; 43 | }; 44 | 45 | class EdgeRefinementWindow { 46 | public: 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | EdgeRefinementWindow(std::shared_ptr& graph); 50 | ~EdgeRefinementWindow(); 51 | 52 | void draw_ui(); 53 | 54 | void draw_gl(glk::GLSLShader& shader); 55 | 56 | void show(); 57 | 58 | void start(); 59 | 60 | void close(); 61 | 62 | private: 63 | void apply_robust_kernel(); 64 | 65 | void refinement(); 66 | void refinement_task(); 67 | 68 | private: 69 | bool show_window; 70 | std::shared_ptr& graph; 71 | 72 | std::mt19937 mt; 73 | std::atomic_bool running; 74 | std::thread refinement_thread; 75 | 76 | std::mutex edges_mutex; 77 | std::vector edges; 78 | g2o::EdgeSE3* inspected_edge; 79 | 80 | RegistrationMethods registration_method; 81 | RobustKernels robust_kernel; 82 | 83 | int optimization_cycle; 84 | int optimization_count; 85 | }; 86 | 87 | } // namespace hdl_graph_slam 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/graph_edit_window.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_GRAPH_EDIT_WINDOW_HPP 2 | #define HDL_GRAPH_SLAM_GRAPH_EDIT_WINDOW_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | namespace hdl_graph_slam { 12 | 13 | class GraphEditWindow { 14 | public: 15 | GraphEditWindow(std::shared_ptr& graph); 16 | ~GraphEditWindow(); 17 | 18 | void show() { show_window = true; } 19 | void draw_ui(); 20 | 21 | private: 22 | bool show_window; 23 | int selected_vertex; 24 | std::shared_ptr& graph; 25 | }; 26 | 27 | } // namespace hdl_graph_slam 28 | 29 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/graph_merge_modal.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_GRAPH_MERGE_MODAL_HPP 2 | #define HDL_GRAPH_SLAM_GRAPH_MERGE_MODAL_HPP 3 | 4 | 5 | 6 | class GraphMergeModal { 7 | public: 8 | 9 | 10 | }; 11 | 12 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/interactive_graph.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_INTERACTIVE_GRAPH_HPP 2 | #define HDL_GRAPH_SLAM_INTERACTIVE_GRAPH_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace g2o { 19 | class VertexPlane; 20 | class VertexSE3; 21 | class VertexSE3Edge; 22 | } // namespace g2o 23 | 24 | namespace hdl_graph_slam { 25 | 26 | class InformationMatrixCalculator; 27 | 28 | /** 29 | * @brief core class to interactively manipulate a pose graph 30 | * 31 | */ 32 | class InteractiveGraph : protected GraphSLAM { 33 | public: 34 | InteractiveGraph(); 35 | virtual ~InteractiveGraph(); 36 | 37 | bool load_map_data(const std::string& directory, guik::ProgressInterface& progress); 38 | bool merge_map_data(InteractiveGraph& graph_, const InteractiveKeyFrame::Ptr& key1, const InteractiveKeyFrame::Ptr& key2, const Eigen::Isometry3d& relative_pose); 39 | 40 | long anchor_node_id() const; 41 | 42 | g2o::EdgeSE3* add_edge(const KeyFrame::Ptr& key1, const KeyFrame::Ptr& key2, const Eigen::Isometry3d& relative_pose, const std::string& robust_kernel = "NONE", double robust_kernel_delta = 0.1); 43 | 44 | g2o::VertexPlane* add_plane(const Eigen::Vector4d& coeffs); 45 | g2o::EdgeSE3Plane* add_edge(const KeyFrame::Ptr& v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& coeffs, const Eigen::MatrixXd& information, const std::string& robust_kernel = "NONE", double robust_kernel_delta = 0.1); 46 | 47 | void apply_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& robust_kernel, double robust_kernel_delta); 48 | 49 | void add_edge_identity(g2o::VertexPlane* v1, g2o::VertexPlane* v2, double information_scale, const std::string& robust_kernel = "NONE", double robust_kernel_delta = 0.1); 50 | void add_edge_parallel(g2o::VertexPlane* v1, g2o::VertexPlane* v2, double information_scale, const std::string& robust_kernel = "NONE", double robust_kernel_delta = 0.1); 51 | void add_edge_perpendicular(g2o::VertexPlane* v1, g2o::VertexPlane* v2, double information_scale, const std::string& robust_kernel = "NONE", double robust_kernel_delta = 0.1); 52 | bool add_edge_prior_normal(long plane_vertex_id, const Eigen::Vector3d& normal, double information_scale, const std::string& robust_kernel = "NONE", double robust_kernel_delta = 0.1); 53 | bool add_edge_prior_distance(long plane_vertex_id, double distance, double information_scale, const std::string& robust_kernel = "NONE", double robust_kernel_delta = 0.1); 54 | 55 | void optimize(int num_iterations = -1); 56 | void optimize_background(int num_iterations = -1); 57 | 58 | std::string graph_statistics(bool update=false); 59 | std::string optimization_messages() const; 60 | 61 | void dump(const std::string& directory, guik::ProgressInterface& progress); 62 | bool save_pointcloud(const std::string& filename, guik::ProgressInterface& progress); 63 | 64 | using GraphSLAM::graph; 65 | using GraphSLAM::num_edges; 66 | using GraphSLAM::num_vertices; 67 | using GraphSLAM::set_solver; 68 | 69 | private: 70 | bool load_special_nodes(const std::string& directory, guik::ProgressInterface& progress); 71 | bool load_keyframes(const std::string& directory, guik::ProgressInterface& progress); 72 | 73 | private: 74 | g2o::VertexSE3* anchor_node; 75 | g2o::EdgeSE3* anchor_edge; 76 | g2o::VertexPlane* floor_node; 77 | 78 | long edge_id_gen; 79 | std::thread optimization_thread; 80 | 81 | public: 82 | mutable std::mutex optimization_mutex; 83 | 84 | std::string graph_stats; 85 | std::stringstream optimization_stream; 86 | 87 | ParameterServer params; 88 | 89 | // optimization statistics (just for visualization) 90 | int iterations; 91 | double chi2_before; 92 | double chi2_after; 93 | double elapsed_time_msec; 94 | 95 | std::unordered_map keyframes; 96 | std::unique_ptr inf_calclator; 97 | }; 98 | 99 | } // namespace hdl_graph_slam 100 | 101 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/interactive_keyframe.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INTERACTIVE_KEYFRAME_HPP 2 | #define INTERACTIVE_KEYFRAME_HPP 3 | 4 | #include 5 | #include 6 | 7 | 8 | namespace hdl_graph_slam { 9 | 10 | struct InteractiveKeyFrame : public KeyFrame { 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | using Ptr = std::shared_ptr; 15 | 16 | InteractiveKeyFrame(const std::string& directory, g2o::HyperGraph* graph); 17 | virtual ~InteractiveKeyFrame() override; 18 | 19 | std::vector neighbors(const Eigen::Vector3f& pt, double radius); 20 | 21 | pcl::PointCloud::Ptr normals(); 22 | 23 | public: 24 | Eigen::Vector3f min_pt; 25 | Eigen::Vector3f max_pt; 26 | 27 | boost::any kdtree_; 28 | pcl::PointCloud::Ptr normals_; 29 | }; 30 | 31 | } 32 | 33 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/manual_loop_close_model.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MANUAL_LOOP_CLOSE_MODAL_HPP 2 | #define MANUAL_LOOP_CLOSE_MODAL_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace hdl_graph_slam { 16 | 17 | class ManualLoopCloseModal { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | ManualLoopCloseModal(std::shared_ptr& graph, const std::string& data_directory); 22 | ~ManualLoopCloseModal(); 23 | 24 | bool set_begin_keyframe(int keyframe_id); 25 | 26 | bool set_end_keyframe(int keyframe_id); 27 | 28 | bool has_begin_keyframe(); 29 | 30 | bool run(); 31 | 32 | void close(); 33 | 34 | void draw_gl(glk::GLSLShader& shader); 35 | 36 | void draw_canvas(); 37 | 38 | private: 39 | void update_fitness_score(); 40 | 41 | void auto_align(); 42 | 43 | void scan_matching(); 44 | 45 | private: 46 | std::shared_ptr& graph; 47 | 48 | double fitness_score; 49 | KeyFrameView::Ptr begin_keyframe; 50 | KeyFrameView::Ptr end_keyframe; 51 | 52 | Eigen::Isometry3d begin_keyframe_pose; 53 | Eigen::Isometry3d end_keyframe_pose_init; 54 | Eigen::Isometry3d end_keyframe_pose; 55 | 56 | std::unique_ptr canvas; 57 | 58 | std::atomic_int auto_alignment_progress; 59 | std::future> auto_alignment_result; 60 | 61 | int global_registration_method; 62 | 63 | float fpfh_normal_estimation_radius; 64 | float fpfh_search_radius; 65 | int fpfh_max_iterations; 66 | int fpfh_num_samples; 67 | int fpfh_correspondence_randomness; 68 | float fpfh_similarity_threshold; 69 | float fpfh_max_correspondence_distance; 70 | float fpfh_inlier_fraction; 71 | 72 | RegistrationMethods registration_method; 73 | RobustKernels robust_kernel; 74 | }; 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/parameter_server.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PARAMETER_SERVER_HPP 2 | #define PARAMETER_SERVER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace hdl_graph_slam { 9 | 10 | struct ParameterServer { 11 | public: 12 | template 13 | T param(const std::string& name, const T& defalut_value) { 14 | auto found = params.find(name); 15 | if(found == params.end()) { 16 | params.insert(std::make_pair(name, defalut_value)); 17 | found = params.find(name); 18 | 19 | } 20 | 21 | if(found->second.type() == typeid(T)) { 22 | return boost::any_cast(found->second); 23 | } else if(found->second.type() == typeid(std::string)) { 24 | auto str = boost::any_cast(found->second); 25 | T p = boost::lexical_cast(str); 26 | found->second = p; 27 | return p; 28 | } 29 | 30 | std::cerr << "warning: param " << name << "'s type does not match!!" << std::endl; 31 | return defalut_value; 32 | } 33 | 34 | private: 35 | std::unordered_map params; 36 | }; 37 | 38 | } 39 | 40 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/plane_alignment_modal.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_PLANE_ALIGNMENT_MODAL_HPP 2 | #define HDL_GRAPH_SLAM_PLANE_ALIGNMENT_MODAL_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace g2o { 13 | class VertexPlane; 14 | } 15 | 16 | namespace hdl_graph_slam { 17 | 18 | class PlaneAlignmentModal { 19 | public: 20 | PlaneAlignmentModal(std::shared_ptr& graph); 21 | ~PlaneAlignmentModal(); 22 | 23 | bool set_begin_plane(int plane_id); 24 | bool set_end_plane(int plane_id); 25 | 26 | bool has_begin_plane(); 27 | 28 | bool run(); 29 | 30 | void close(); 31 | 32 | void draw_gl(glk::GLSLShader& shader); 33 | 34 | private: 35 | const std::shared_ptr& graph; 36 | 37 | std::unique_ptr plane_begin; 38 | std::unique_ptr plane_end; 39 | 40 | int plane_mode; 41 | float information_scale; 42 | 43 | RobustKernels robust_kernel; 44 | }; 45 | 46 | } // namespace hdl_graph_slam 47 | 48 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/plane_detection_window.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PLANE_DETECTION_WINDOW_HPP 2 | #define PLANE_DETECTION_WINDOW_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace hdl_graph_slam { 14 | 15 | struct RegionGrowingResult { 16 | public: 17 | using Ptr = std::shared_ptr; 18 | 19 | std::vector candidates; 20 | 21 | pcl::PointCloud::Ptr cloud; 22 | pcl::PointCloud::Ptr normals; 23 | glk::PointCloudBuffer::Ptr cloud_buffer; 24 | }; 25 | 26 | struct PlaneDetectionResult { 27 | public: 28 | using Ptr = std::shared_ptr; 29 | 30 | std::vector candidates; 31 | std::vector::Ptr> candidate_inliers; 32 | std::vector candidate_inlier_buffers; 33 | std::vector> candidate_local_coeffs; 34 | 35 | Eigen::VectorXf coeffs; 36 | }; 37 | 38 | class PlaneDetectionWindow { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | 42 | PlaneDetectionWindow(std::shared_ptr& graph); 43 | ~PlaneDetectionWindow(); 44 | 45 | void show(); 46 | void close(); 47 | void set_center_point(const Eigen::Vector3f& point); 48 | 49 | RegionGrowingResult::Ptr region_growing(guik::ProgressInterface& progress); 50 | PlaneDetectionResult::Ptr detect_plane(const RegionGrowingResult::Ptr& region_growing); 51 | pcl::PointCloud::Ptr detect_plane_with_coeffs(const pcl::PointCloud::ConstPtr& cloud, Eigen::Vector4f& coeffs); 52 | 53 | void draw_ui(); 54 | void draw_gl(glk::GLSLShader& shader); 55 | 56 | private: 57 | bool show_window; 58 | std::shared_ptr& graph; 59 | 60 | Eigen::Vector3f center_point; 61 | 62 | float initial_neighbor_search_radius; 63 | float normal_estimation_radius; 64 | int min_cluster_size; 65 | int max_cluster_size; 66 | int num_neighbors; 67 | float smoothness_threshold; 68 | float curvature_threshold; 69 | 70 | bool enable_normal_filtering; 71 | float normal_threshold; 72 | 73 | float ransac_distance_thresh; 74 | int min_plane_supports; 75 | 76 | int robust_kernel; 77 | float robust_kernel_delta; 78 | 79 | guik::ProgressModal region_growing_progress_modal; 80 | RegionGrowingResult::Ptr region_growing_result; 81 | PlaneDetectionResult::Ptr plane_detection_result; 82 | }; 83 | 84 | } // namespace hdl_graph_slam 85 | 86 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/registration_methods.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_REGISTRATION_METHODS_HPP 2 | #define HDL_GRAPH_SLAM_REGISTRATION_METHODS_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace hdl_graph_slam { 9 | 10 | class RegistrationMethods { 11 | public: 12 | RegistrationMethods(); 13 | ~RegistrationMethods(); 14 | 15 | void draw_ui(); 16 | 17 | pcl::Registration::Ptr method() const; 18 | 19 | private: 20 | int registration_method; 21 | float registration_resolution; 22 | 23 | float transformation_epsilon; 24 | int max_iterations; 25 | 26 | std::vector registration_methods; 27 | }; 28 | } // namespace hdl_graph_slam 29 | 30 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/robust_kernels.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_ROBUST_KERNELS_HPP 2 | #define HDL_GRAPH_SLAM_ROBUST_KERNELS_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace hdl_graph_slam { 9 | 10 | class RobustKernels { 11 | public: 12 | RobustKernels(int type, float delta) 13 | : kernel_type(type), 14 | kernel_delta(delta), 15 | kernels({"NONE", "Huber", "Cauchy", "DCS", "Fair", "GemanMcClure", "PseudoHuber", "Staturated", "Tukey", "Welsch"}) 16 | {} 17 | 18 | RobustKernels() 19 | : kernel_type(0), 20 | kernel_delta(0.01), 21 | kernels({"NONE", "Huber", "Cauchy", "DCS", "Fair", "GemanMcClure", "PseudoHuber", "Staturated", "Tukey", "Welsch"}) 22 | {} 23 | ~RobustKernels() {} 24 | 25 | void draw_ui() { 26 | ImGui::Text("Robust kernel"); 27 | ImGui::Combo("Kernel type", &kernel_type, kernels.data(), kernels.size()); 28 | ImGui::DragFloat("Kernel delta", &kernel_delta, 1e-4f, 1e-4f, 10.0f, "%.4f", 1.0f); 29 | } 30 | 31 | std::string type() const { 32 | return kernels[kernel_type]; 33 | } 34 | 35 | float delta() const { 36 | return kernel_delta; 37 | } 38 | 39 | private: 40 | int kernel_type; 41 | float kernel_delta; 42 | std::vector kernels; 43 | }; 44 | } 45 | 46 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/version.h.in: -------------------------------------------------------------------------------- 1 | #define IS_VERSION_MAJOR @IS_VERSION_MAJOR@ 2 | #define IS_VERSION_MINOR @IS_VERSION_MINOR@ 3 | #define IS_VERSION_REVISION @IS_VERSION_REVISION@ 4 | 5 | #define LAST_COMMIT_HASH @LAST_COMMIT_HASH@ 6 | #define LAST_COMMIT_DATE @LAST_COMMIT_DATE@ -------------------------------------------------------------------------------- /include/hdl_graph_slam/version_modal.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_VERSION_MODAL_HPP 2 | #define HDL_GRAPH_SLAM_VERSION_MODAL_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace hdl_graph_slam { 9 | 10 | class VersionModal { 11 | public: 12 | VersionModal(); 13 | ~VersionModal(); 14 | 15 | void open(); 16 | bool run(); 17 | }; 18 | 19 | } // namespace hdl_graph_slam 20 | 21 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/drawable_object.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_DRAWABLE_HPP 2 | #define HDL_GRAPH_SLAM_DRAWABLE_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace hdl_graph_slam { 8 | 9 | struct DrawFlags { 10 | public: 11 | DrawFlags() { 12 | draw_verticies = true; 13 | draw_edges = true; 14 | 15 | draw_keyframe_vertices = true; 16 | draw_plane_vertices = true; 17 | 18 | draw_se3_edges = true; 19 | draw_se3_plane_edges = true; 20 | draw_floor_edges = false; 21 | draw_plane_edges = true; 22 | 23 | z_clipping = true; 24 | } 25 | 26 | bool draw_verticies; 27 | bool draw_edges; 28 | 29 | bool draw_keyframe_vertices; 30 | bool draw_plane_vertices; 31 | 32 | bool draw_se3_edges; 33 | bool draw_se3_plane_edges; 34 | bool draw_floor_edges; 35 | bool draw_plane_edges; 36 | 37 | bool z_clipping; 38 | }; 39 | 40 | class DrawableObject { 41 | public: 42 | enum OBJECT_TYPE { POINTS = 1, VERTEX = (1 << 1), EDGE = (1 << 2), KEYFRAME = (1 << 3), PLANE = (1 << 4) }; 43 | 44 | using Ptr = std::shared_ptr; 45 | 46 | DrawableObject() {} 47 | virtual ~DrawableObject() {} 48 | 49 | virtual bool available() const { return true; } 50 | 51 | virtual void draw(const DrawFlags& flags, glk::GLSLShader& shader) {} 52 | 53 | virtual void draw(const DrawFlags& flags, glk::GLSLShader& shader, const Eigen::Vector4f& color, const Eigen::Matrix4f& model_matrix) {} 54 | }; 55 | 56 | } // namespace hdl_graph_slam 57 | 58 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/edge_plane_view.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EDGE_PLANE_VIEW_HPP 2 | #define EDGE_PLANE_VIEW_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace hdl_graph_slam { 12 | 13 | class EdgePlaneView : public EdgeView { 14 | public: 15 | EdgePlaneView(g2o::HyperGraph::Edge* edge, LineBuffer& line_buffer) : EdgeView(edge, line_buffer) { 16 | std::cout << "plane edge view created" << std::endl; 17 | v1 = dynamic_cast(edge->vertices()[0]); 18 | v2 = dynamic_cast(edge->vertices()[1]); 19 | assert(v1 != nullptr && v2 != nullptr); 20 | } 21 | 22 | virtual ~EdgePlaneView() override {} 23 | 24 | virtual bool available() const override { return true; } 25 | 26 | virtual Eigen::Vector3f representative_point() const override { 27 | VertexPlaneCache* cache1 = dynamic_cast(v1->userData()); 28 | VertexPlaneCache* cache2 = dynamic_cast(v2->userData()); 29 | if(cache1 == nullptr || cache2 == nullptr) { 30 | std::cerr << "warning: vertex plane cache has not been created!!" << std::endl; 31 | return Eigen::Vector3f::Zero(); 32 | } 33 | 34 | Eigen::Vector3f p1 = cache1->pose().translation().cast(); 35 | Eigen::Vector3f p2 = cache2->pose().translation().cast(); 36 | 37 | return (p1 + p2) * 0.5f; 38 | } 39 | 40 | virtual void draw(const DrawFlags& flags, glk::GLSLShader& shader) override { 41 | if(!flags.draw_edges || !flags.draw_plane_edges) { 42 | return; 43 | } 44 | 45 | VertexPlaneCache* cache1 = dynamic_cast(v1->userData()); 46 | VertexPlaneCache* cache2 = dynamic_cast(v2->userData()); 47 | if(cache1 == nullptr || cache2 == nullptr) { 48 | std::cerr << "warning: vertex plane cache has not been created!!" << std::endl; 49 | return; 50 | } 51 | 52 | Eigen::Vector3f p1 = cache1->pose().translation().cast(); 53 | Eigen::Vector3f p2 = cache2->pose().translation().cast(); 54 | 55 | Eigen::Vector4f c1(0.0f, 0.0f, 1.0f, 1.0f); 56 | Eigen::Vector4f c2(0.0f, 0.0f, 1.0f, 1.0f); 57 | Eigen::Vector4i info(EDGE, edge->id(), 0, 0); 58 | 59 | line_buffer.add_line(p1, p2, c1, c2, info); 60 | } 61 | 62 | static bool is_plane_edge(g2o::HyperGraph::Edge* edge) { 63 | return dynamic_cast(edge) || dynamic_cast(edge) || dynamic_cast(edge) || dynamic_cast(edge); 64 | } 65 | 66 | private: 67 | g2o::VertexPlane* v1; 68 | g2o::VertexPlane* v2; 69 | }; 70 | 71 | }; // namespace hdl_graph_slam 72 | 73 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/edge_se3_plane_view.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EDGE_SE3_PLANE_VIEW_HPP 2 | #define EDGE_SE3_PLANE_VIEW_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace hdl_graph_slam { 10 | 11 | class EdgeSE3PlaneView : public EdgeView { 12 | public: 13 | EdgeSE3PlaneView(g2o::HyperGraph::Edge* edge, LineBuffer& line_buffer) : EdgeView(edge, line_buffer) { 14 | v_se3 = dynamic_cast(edge->vertices()[0]); 15 | v_plane = dynamic_cast(edge->vertices()[1]); 16 | 17 | assert(v_se3 != nullptr && v_plane != nullptr); 18 | } 19 | 20 | virtual ~EdgeSE3PlaneView() override {} 21 | 22 | virtual bool available() const override { return true; } 23 | 24 | virtual Eigen::Vector3f representative_point() const override { 25 | VertexPlaneCache* cache = dynamic_cast(v_plane->userData()); 26 | if(cache == nullptr) { 27 | return Eigen::Vector3f::Zero(); 28 | } 29 | 30 | Eigen::Vector3f p1 = v_se3->estimate().translation().cast(); 31 | Eigen::Vector3f p2 = cache->pose().translation().cast(); 32 | 33 | return (p1 + p2) * 0.5f; 34 | } 35 | 36 | virtual void draw(const DrawFlags& flags, glk::GLSLShader& shader) override { 37 | if (!flags.draw_edges || !flags.draw_se3_plane_edges) { 38 | return; 39 | } 40 | 41 | if (!flags.draw_floor_edges) { 42 | Eigen::Vector4d coeffs = v_plane->estimate().coeffs(); 43 | if((coeffs - Eigen::Vector4d::UnitZ()).norm() < 1e-6) { 44 | return; 45 | } 46 | } 47 | 48 | VertexPlaneCache* cache = dynamic_cast(v_plane->userData()); 49 | if (cache == nullptr) { 50 | // std::cerr << "warning: vertex plane cache has not been created!!" << std::endl; 51 | return; 52 | } 53 | 54 | Eigen::Vector3f p1 = v_se3->estimate().translation().cast(); 55 | Eigen::Vector3f p2 = cache->pose().translation().cast(); 56 | 57 | Eigen::Vector4f c1(1.0f, 0.0f, 0.0f, 1.0f); 58 | Eigen::Vector4f c2(0.0f, 1.0f, 0.0f, 1.0f); 59 | Eigen::Vector4i info(EDGE, edge->id(), 0, 0); 60 | 61 | line_buffer.add_line(p1, p2, c1, c2, info); 62 | } 63 | 64 | private: 65 | EdgeSE3PlaneView(); 66 | 67 | private: 68 | g2o::VertexSE3* v_se3; 69 | g2o::VertexPlane* v_plane; 70 | }; 71 | 72 | }; // namespace hdl_graph_slam 73 | 74 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/edge_se3_view.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INTERACTIVE_EDGE_SE3_VIEW_HPP 2 | #define INTERACTIVE_EDGE_SE3_VIEW_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace hdl_graph_slam { 9 | 10 | class EdgeSE3View : public EdgeView { 11 | public: 12 | EdgeSE3View(g2o::HyperGraph::Edge* edge, LineBuffer& line_buffer) : EdgeView(edge, line_buffer) { 13 | v1 = dynamic_cast(edge->vertices()[0]); 14 | v2 = dynamic_cast(edge->vertices()[1]); 15 | 16 | assert(v1 != nullptr && v2 != nullptr); 17 | } 18 | 19 | virtual ~EdgeSE3View() override {} 20 | 21 | virtual bool available() const override { return true; } 22 | 23 | virtual Eigen::Vector3f representative_point() const override { 24 | Eigen::Vector3f p1 = v1->estimate().translation().cast(); 25 | Eigen::Vector3f p2 = v2->estimate().translation().cast(); 26 | 27 | return (p1 + p2) * 0.5f; 28 | } 29 | 30 | virtual void draw(const DrawFlags& flags, glk::GLSLShader& shader) override { 31 | if (!flags.draw_edges || !flags.draw_se3_edges) { 32 | return; 33 | } 34 | 35 | Eigen::Vector3f p1 = v1->estimate().translation().cast(); 36 | Eigen::Vector3f p2 = v2->estimate().translation().cast(); 37 | 38 | Eigen::Vector4f color(0.95f, 0.0f, 0.0f, 1.0f); 39 | Eigen::Vector4i info(EDGE, edge->id(), 0, 0); 40 | 41 | line_buffer.add_line(p1, p2, color, color, info); 42 | } 43 | 44 | private: 45 | EdgeSE3View(); 46 | 47 | private: 48 | g2o::VertexSE3* v1; 49 | g2o::VertexSE3* v2; 50 | }; 51 | } // namespace hdl_graph_slam 52 | 53 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/edge_view.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_EDGE_VIEW_HPP 2 | #define HDL_GRAPH_SLAM_EDGE_VIEW_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace hdl_graph_slam { 12 | 13 | class EdgeView : public DrawableObject { 14 | public: 15 | using Ptr = std::shared_ptr; 16 | 17 | static EdgeView::Ptr create(g2o::HyperGraph::Edge* edge, LineBuffer& line_buffer); 18 | 19 | EdgeView(g2o::HyperGraph::Edge* edge, LineBuffer& line_buffer); 20 | virtual ~EdgeView(); 21 | 22 | long id() const; 23 | virtual Eigen::Vector3f representative_point() const = 0; 24 | 25 | virtual void context_menu(bool& do_delete); 26 | 27 | private: 28 | EdgeView(); 29 | 30 | protected: 31 | LineBuffer& line_buffer; 32 | 33 | public: 34 | g2o::HyperGraph::Edge* edge; 35 | }; 36 | 37 | } // namespace hdl_graph_slam 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/interactive_graph_view.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_INTERACTIVE_GRAPH_VIEW_HPP 2 | #define HDL_GRAPH_SLAM_INTERACTIVE_GRAPH_VIEW_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace hdl_graph_slam { 17 | 18 | class InteractiveGraphView : public InteractiveGraph { 19 | public: 20 | InteractiveGraphView() { } 21 | virtual ~InteractiveGraphView() override {} 22 | 23 | void init_gl() { line_buffer.reset(new LineBuffer()); } 24 | 25 | void update_view() { 26 | bool keyframe_inserted = false; 27 | for (const auto& key_item : keyframes) { 28 | auto& keyframe = key_item.second; 29 | auto found = keyframes_view_map.find(keyframe); 30 | if (found == keyframes_view_map.end()) { 31 | keyframe_inserted = true; 32 | keyframes_view.push_back(std::make_shared(keyframe)); 33 | keyframes_view_map[keyframe] = keyframes_view.back(); 34 | 35 | vertices_view.push_back(keyframes_view.back()); 36 | vertices_view_map[keyframe->id()] = keyframes_view.back(); 37 | 38 | drawables.push_back(keyframes_view.back()); 39 | } 40 | } 41 | 42 | if (keyframe_inserted) { 43 | std::sort(keyframes_view.begin(), keyframes_view.end(), [=](const KeyFrameView::Ptr& lhs, const KeyFrameView::Ptr& rhs) { return lhs->lock()->id() < rhs->lock()->id(); }); 44 | } 45 | 46 | for (const auto& vertex : graph->vertices()) { 47 | auto found = vertices_view_map.find(vertex.second->id()); 48 | if (found != vertices_view_map.end()) { 49 | continue; 50 | } 51 | 52 | auto vertex_view = VertexView::create(vertex.second); 53 | if (vertex_view) { 54 | vertices_view.push_back(vertex_view); 55 | vertices_view_map[vertex.second->id()] = vertex_view; 56 | 57 | drawables.push_back(vertex_view); 58 | } 59 | } 60 | 61 | for (const auto& edge : graph->edges()) { 62 | auto found = edges_view_map.find(edge); 63 | if (found != edges_view_map.end()) { 64 | continue; 65 | } 66 | 67 | auto edge_view = EdgeView::create(edge, *line_buffer); 68 | if (edge_view) { 69 | edges_view.push_back(edge_view); 70 | edges_view_map[edge] = edge_view; 71 | 72 | drawables.push_back(edge_view); 73 | } 74 | } 75 | } 76 | 77 | void draw(const DrawFlags& flags, glk::GLSLShader& shader) { 78 | update_view(); 79 | line_buffer->clear(); 80 | 81 | for (auto& drawable : drawables) { 82 | if (drawable->available()) { 83 | drawable->draw(flags, shader); 84 | } 85 | } 86 | 87 | line_buffer->draw(shader); 88 | } 89 | 90 | void delete_edge(EdgeView::Ptr edge) { 91 | std::cout << "delete edge " << edge->id() << std::endl; 92 | 93 | auto found = std::find(edges_view.begin(), edges_view.end(), edge); 94 | while(found != edges_view.end()) { 95 | edges_view.erase(found); 96 | found = std::find(edges_view.begin(), edges_view.end(), edge); 97 | } 98 | 99 | auto found2 = std::find(drawables.begin(), drawables.end(), edge); 100 | while(found2 != drawables.end()) { 101 | drawables.erase(found2); 102 | found2 = std::find(drawables.begin(), drawables.end(), edge); 103 | } 104 | 105 | for(auto itr = edges_view_map.begin(); itr != edges_view_map.end(); itr++) { 106 | if(itr->second == edge) { 107 | edges_view_map.erase(itr); 108 | break; 109 | } 110 | } 111 | 112 | graph->removeEdge(edge->edge); 113 | } 114 | 115 | public: 116 | std::unique_ptr line_buffer; 117 | 118 | std::vector keyframes_view; 119 | std::unordered_map keyframes_view_map; 120 | 121 | std::vector vertices_view; 122 | std::unordered_map vertices_view_map; 123 | 124 | std::vector edges_view; 125 | std::unordered_map edges_view_map; 126 | 127 | std::vector drawables; 128 | }; 129 | 130 | } // namespace hdl_graph_slam 131 | 132 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/keyframe_view.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_KEYFRAME_VIEW_HPP 2 | #define HDL_GRAPH_SLAM_KEYFRAME_VIEW_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace hdl_graph_slam { 13 | 14 | class KeyFrameView : public VertexView { 15 | public: 16 | using Ptr = std::shared_ptr; 17 | 18 | KeyFrameView(const InteractiveKeyFrame::Ptr& kf) 19 | : VertexView(kf->node) 20 | { 21 | keyframe = kf; 22 | 23 | std::cout << kf->id() << " : "; 24 | pointcloud_buffer.reset(new glk::PointCloudBuffer(kf->cloud)); 25 | } 26 | 27 | InteractiveKeyFrame::Ptr lock() const { return keyframe.lock(); } 28 | 29 | virtual bool available() const override { return !keyframe.expired(); } 30 | 31 | virtual void draw(const DrawFlags& flags, glk::GLSLShader& shader) override { 32 | InteractiveKeyFrame::Ptr kf = keyframe.lock(); 33 | Eigen::Matrix4f model_matrix = kf->estimate().matrix().cast(); 34 | shader.set_uniform("color_mode", 0); 35 | shader.set_uniform("model_matrix", model_matrix); 36 | shader.set_uniform("info_values", Eigen::Vector4i(POINTS, 0, 0, 0)); 37 | 38 | pointcloud_buffer->draw(shader); 39 | 40 | if (!flags.draw_verticies || !flags.draw_keyframe_vertices) { 41 | return; 42 | } 43 | 44 | shader.set_uniform("color_mode", 1); 45 | shader.set_uniform("material_color", Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f)); 46 | shader.set_uniform("info_values", Eigen::Vector4i(VERTEX | KEYFRAME, kf->id(), 0, 0)); 47 | 48 | shader.set_uniform("apply_keyframe_scale", true); 49 | model_matrix.block<3, 3>(0, 0) *= 0.35; 50 | shader.set_uniform("model_matrix", model_matrix); 51 | const auto& sphere = glk::Primitives::instance()->primitive(glk::Primitives::SPHERE); 52 | sphere.draw(shader); 53 | shader.set_uniform("apply_keyframe_scale", false); 54 | } 55 | 56 | virtual void draw(const DrawFlags& flags, glk::GLSLShader& shader, const Eigen::Vector4f& color, const Eigen::Matrix4f& model_matrix) override { 57 | if (!available()) { 58 | return; 59 | } 60 | 61 | InteractiveKeyFrame::Ptr kf = keyframe.lock(); 62 | 63 | shader.set_uniform("color_mode", 1); 64 | shader.set_uniform("material_color", color); 65 | 66 | shader.set_uniform("model_matrix", model_matrix); 67 | shader.set_uniform("info_values", Eigen::Vector4i(POINTS, 0, 0, 0)); 68 | 69 | pointcloud_buffer->draw(shader); 70 | 71 | shader.set_uniform("color_mode", 1); 72 | shader.set_uniform("info_values", Eigen::Vector4i(VERTEX | KEYFRAME, kf->id(), 0, 0)); 73 | shader.set_uniform("apply_keyframe_scale", true); 74 | const auto& sphere = glk::Primitives::instance()->primitive(glk::Primitives::SPHERE); 75 | sphere.draw(shader); 76 | shader.set_uniform("apply_keyframe_scale", false); 77 | } 78 | 79 | private: 80 | std::weak_ptr keyframe; 81 | std::unique_ptr pointcloud_buffer; 82 | }; 83 | 84 | } // namespace hdl_graph_slam 85 | 86 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/line_buffer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_VIEW_LINE_BUFFER_HPP 2 | #define HDL_GRAPH_SLAM_VIEW_LINE_BUFFER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace hdl_graph_slam { 10 | 11 | class LineBuffer { 12 | public: 13 | LineBuffer() {} 14 | 15 | void clear() { 16 | vertices.clear(); 17 | colors.clear(); 18 | infos.clear(); 19 | } 20 | 21 | void add_line(const Eigen::Vector3f& v0, const Eigen::Vector3f& v1, const Eigen::Vector4f& c0, Eigen::Vector4f& c1, const Eigen::Vector4i& info) { 22 | vertices.push_back(v0); 23 | vertices.push_back(v1); 24 | colors.push_back(c0); 25 | colors.push_back(c1); 26 | infos.push_back(info); 27 | infos.push_back(info); 28 | } 29 | 30 | void draw(glk::GLSLShader& shader) const { 31 | shader.set_uniform("color_mode", 2); 32 | shader.set_uniform("model_matrix", Eigen::Matrix4f::Identity().eval()); 33 | 34 | float line_width = 0.1f * shader.get_uniform1f("keyframe_scale"); 35 | glk::Lines lines(line_width, vertices, colors, infos); 36 | lines.draw(shader); 37 | } 38 | 39 | private: 40 | std::vector> vertices; 41 | std::vector> colors; 42 | std::vector> infos; 43 | }; 44 | } 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/vertex_plane_cache.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_VIEW_PLANE_VERTEX_CACHE_HPP 2 | #define HDL_GRAPH_SLAM_VIEW_PLANE_VERTEX_CACHE_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace hdl_graph_slam { 12 | 13 | /** 14 | * @brief g2o cache class to calculate and store vertex plane position 15 | */ 16 | class VertexPlaneCache : public g2o::HyperGraph::Data { 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | 20 | VertexPlaneCache(g2o::VertexPlane* vertex) : vertex(vertex) { update(); } 21 | 22 | virtual bool read(std::istream& is) override { return true; } 23 | virtual bool write(std::ostream& os) const override { return true; } 24 | 25 | void update() { 26 | int num_edges = 0; 27 | Eigen::Vector3d sum_pos(0.0, 0.0, 0.0); 28 | for (const auto& edge_ : vertex->edges()) { 29 | g2o::EdgeSE3Plane* edge = dynamic_cast(edge_); 30 | if (edge == nullptr) { 31 | continue; 32 | } 33 | 34 | g2o::VertexSE3* v_se3 = dynamic_cast(edge->vertices()[0]); 35 | num_edges++; 36 | sum_pos += v_se3->estimate().translation(); 37 | } 38 | 39 | Eigen::Vector4d coeffs = vertex->estimate().coeffs(); 40 | Eigen::Vector3d se3_center = sum_pos / num_edges; 41 | 42 | double t = -(coeffs[3] + (coeffs.head<3>().array() * se3_center.array()).sum()) / coeffs.head<3>().squaredNorm(); 43 | 44 | Eigen::Vector3d position = se3_center + t * coeffs.head<3>(); 45 | 46 | Eigen::Vector3d axis = std::abs(Eigen::Vector3d::UnitX().dot(coeffs.head<3>())) < 0.9 ? Eigen::Vector3d::UnitX() : Eigen::Vector3d::UnitZ(); 47 | Eigen::Matrix3d rotation; 48 | rotation.block<3, 1>(0, 2) = coeffs.head<3>().normalized(); 49 | rotation.block<3, 1>(0, 0) = rotation.block<3, 1>(0, 2).cross(axis); 50 | rotation.block<3, 1>(0, 1) = rotation.block<3, 1>(0, 2).cross(rotation.block<3, 1>(0, 0)); 51 | 52 | pose_.setIdentity(); 53 | pose_.translation() = position; 54 | pose_.linear() = rotation; 55 | } 56 | 57 | const Eigen::Isometry3d& pose() { return pose_; } 58 | 59 | private: 60 | g2o::VertexPlane* vertex; 61 | Eigen::Isometry3d pose_; 62 | }; 63 | 64 | } // namespace hdl_graph_slam 65 | 66 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/vertex_plane_view.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_VERTEX_PLANE_VIEW_HPP 2 | #define HDL_GRAPH_SLAM_VERTEX_PLANE_VIEW_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace g2o { 19 | class VertexPlane; 20 | } 21 | 22 | namespace hdl_graph_slam { 23 | 24 | class VertexPlaneView : public VertexView { 25 | public: 26 | using Ptr = std::shared_ptr; 27 | 28 | VertexPlaneView(g2o::VertexPlane* vertex_plane) : VertexView(vertex_plane), vertex_plane(vertex_plane) { 29 | if (vertex_plane->userData() == nullptr) { 30 | vertex_plane->addUserData(new VertexPlaneCache(vertex_plane)); 31 | } 32 | } 33 | 34 | virtual ~VertexPlaneView() override {} 35 | 36 | virtual bool available() const override { return true; } 37 | 38 | virtual void draw(const DrawFlags& flags, glk::GLSLShader& shader) override { 39 | VertexPlaneCache* cache = dynamic_cast(vertex_plane->userData()); 40 | cache->update(); 41 | 42 | if (!flags.draw_verticies || !flags.draw_plane_vertices) { 43 | return; 44 | } 45 | 46 | Eigen::Matrix4f model_matrix = (cache->pose() * Eigen::Scaling(5.0, 5.0, 0.1)).matrix().cast(); 47 | 48 | shader.set_uniform("color_mode", 1); 49 | shader.set_uniform("model_matrix", model_matrix); 50 | shader.set_uniform("material_color", Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f)); 51 | shader.set_uniform("info_values", Eigen::Vector4i(VERTEX | PLANE, vertex->id(), 0, 0)); 52 | 53 | auto& cube = glk::Primitives::instance()->primitive(glk::Primitives::CUBE); 54 | cube.draw(shader); 55 | } 56 | 57 | virtual void draw(const DrawFlags& flags, glk::GLSLShader& shader, const Eigen::Vector4f& color, const Eigen::Matrix4f& model_matrix_) override { 58 | VertexPlaneCache* cache = dynamic_cast(vertex_plane->userData()); 59 | cache->update(); 60 | 61 | if (!flags.draw_verticies || !flags.draw_plane_vertices) { 62 | return; 63 | } 64 | 65 | Eigen::Matrix4f model_matrix = cache->pose().matrix().cast() * model_matrix_; 66 | 67 | shader.set_uniform("color_mode", 1); 68 | shader.set_uniform("model_matrix", model_matrix); 69 | shader.set_uniform("material_color", color); 70 | shader.set_uniform("info_values", Eigen::Vector4i(VERTEX | PLANE, vertex->id(), 0, 0)); 71 | 72 | auto& cube = glk::Primitives::instance()->primitive(glk::Primitives::CUBE); 73 | cube.draw(shader); 74 | } 75 | 76 | public: 77 | g2o::VertexPlane* vertex_plane; 78 | }; 79 | 80 | } // namespace hdl_graph_slam 81 | 82 | #endif -------------------------------------------------------------------------------- /include/hdl_graph_slam/view/vertex_view.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_GRAPH_SLAM_VIEW_VERTEX_VIEW_HPP 2 | #define HDL_GRAPH_SLAM_VIEW_VERTEX_VIEW_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace hdl_graph_slam { 11 | 12 | class VertexView : public DrawableObject { 13 | public: 14 | using Ptr = std::shared_ptr; 15 | 16 | VertexView(g2o::HyperGraph::Vertex* vertex); 17 | virtual ~VertexView(); 18 | 19 | static VertexView::Ptr create(g2o::HyperGraph::Vertex* vertex); 20 | 21 | long id() const { return vertex->id(); } 22 | 23 | virtual void context_menu(); 24 | 25 | private: 26 | VertexView(); 27 | 28 | protected: 29 | g2o::HyperGraph::Vertex* vertex; 30 | }; 31 | 32 | } // namespace hdl_graph_slam 33 | 34 | #endif -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | interactive_slam 4 | 1.0.0 5 | The interactive_slam package 6 | 7 | koide 8 | 9 | GPLv3 10 | 11 | https://github.com/koide3/interactive_slam 12 | 13 | roscpp 14 | ndt_omp 15 | hdl_graph_slam 16 | libglfw3-dev 17 | libglm-dev 18 | libg2o 19 | roslib 20 | catkin 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/glk/colormap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace glk{ 6 | 7 | float turbo_srgb_floats[256][3] = { 8 | {0.18995, 0.07176, 0.23217}, {0.19483, 0.08339, 0.26149}, {0.19956, 0.09498, 0.29024}, {0.20415, 0.10652, 0.31844}, {0.20860, 0.11802, 0.34607}, {0.21291, 0.12947, 0.37314}, {0.21708, 0.14087, 0.39964}, {0.22111, 0.15223, 0.42558}, 9 | {0.22500, 0.16354, 0.45096}, {0.22875, 0.17481, 0.47578}, {0.23236, 0.18603, 0.50004}, {0.23582, 0.19720, 0.52373}, {0.23915, 0.20833, 0.54686}, {0.24234, 0.21941, 0.56942}, {0.24539, 0.23044, 0.59142}, {0.24830, 0.24143, 0.61286}, 10 | {0.25107, 0.25237, 0.63374}, {0.25369, 0.26327, 0.65406}, {0.25618, 0.27412, 0.67381}, {0.25853, 0.28492, 0.69300}, {0.26074, 0.29568, 0.71162}, {0.26280, 0.30639, 0.72968}, {0.26473, 0.31706, 0.74718}, {0.26652, 0.32768, 0.76412}, 11 | {0.26816, 0.33825, 0.78050}, {0.26967, 0.34878, 0.79631}, {0.27103, 0.35926, 0.81156}, {0.27226, 0.36970, 0.82624}, {0.27334, 0.38008, 0.84037}, {0.27429, 0.39043, 0.85393}, {0.27509, 0.40072, 0.86692}, {0.27576, 0.41097, 0.87936}, 12 | {0.27628, 0.42118, 0.89123}, {0.27667, 0.43134, 0.90254}, {0.27691, 0.44145, 0.91328}, {0.27701, 0.45152, 0.92347}, {0.27698, 0.46153, 0.93309}, {0.27680, 0.47151, 0.94214}, {0.27648, 0.48144, 0.95064}, {0.27603, 0.49132, 0.95857}, 13 | {0.27543, 0.50115, 0.96594}, {0.27469, 0.51094, 0.97275}, {0.27381, 0.52069, 0.97899}, {0.27273, 0.53040, 0.98461}, {0.27106, 0.54015, 0.98930}, {0.26878, 0.54995, 0.99303}, {0.26592, 0.55979, 0.99583}, {0.26252, 0.56967, 0.99773}, 14 | {0.25862, 0.57958, 0.99876}, {0.25425, 0.58950, 0.99896}, {0.24946, 0.59943, 0.99835}, {0.24427, 0.60937, 0.99697}, {0.23874, 0.61931, 0.99485}, {0.23288, 0.62923, 0.99202}, {0.22676, 0.63913, 0.98851}, {0.22039, 0.64901, 0.98436}, 15 | {0.21382, 0.65886, 0.97959}, {0.20708, 0.66866, 0.97423}, {0.20021, 0.67842, 0.96833}, {0.19326, 0.68812, 0.96190}, {0.18625, 0.69775, 0.95498}, {0.17923, 0.70732, 0.94761}, {0.17223, 0.71680, 0.93981}, {0.16529, 0.72620, 0.93161}, 16 | {0.15844, 0.73551, 0.92305}, {0.15173, 0.74472, 0.91416}, {0.14519, 0.75381, 0.90496}, {0.13886, 0.76279, 0.89550}, {0.13278, 0.77165, 0.88580}, {0.12698, 0.78037, 0.87590}, {0.12151, 0.78896, 0.86581}, {0.11639, 0.79740, 0.85559}, 17 | {0.11167, 0.80569, 0.84525}, {0.10738, 0.81381, 0.83484}, {0.10357, 0.82177, 0.82437}, {0.10026, 0.82955, 0.81389}, {0.09750, 0.83714, 0.80342}, {0.09532, 0.84455, 0.79299}, {0.09377, 0.85175, 0.78264}, {0.09287, 0.85875, 0.77240}, 18 | {0.09267, 0.86554, 0.76230}, {0.09320, 0.87211, 0.75237}, {0.09451, 0.87844, 0.74265}, {0.09662, 0.88454, 0.73316}, {0.09958, 0.89040, 0.72393}, {0.10342, 0.89600, 0.71500}, {0.10815, 0.90142, 0.70599}, {0.11374, 0.90673, 0.69651}, 19 | {0.12014, 0.91193, 0.68660}, {0.12733, 0.91701, 0.67627}, {0.13526, 0.92197, 0.66556}, {0.14391, 0.92680, 0.65448}, {0.15323, 0.93151, 0.64308}, {0.16319, 0.93609, 0.63137}, {0.17377, 0.94053, 0.61938}, {0.18491, 0.94484, 0.60713}, 20 | {0.19659, 0.94901, 0.59466}, {0.20877, 0.95304, 0.58199}, {0.22142, 0.95692, 0.56914}, {0.23449, 0.96065, 0.55614}, {0.24797, 0.96423, 0.54303}, {0.26180, 0.96765, 0.52981}, {0.27597, 0.97092, 0.51653}, {0.29042, 0.97403, 0.50321}, 21 | {0.30513, 0.97697, 0.48987}, {0.32006, 0.97974, 0.47654}, {0.33517, 0.98234, 0.46325}, {0.35043, 0.98477, 0.45002}, {0.36581, 0.98702, 0.43688}, {0.38127, 0.98909, 0.42386}, {0.39678, 0.99098, 0.41098}, {0.41229, 0.99268, 0.39826}, 22 | {0.42778, 0.99419, 0.38575}, {0.44321, 0.99551, 0.37345}, {0.45854, 0.99663, 0.36140}, {0.47375, 0.99755, 0.34963}, {0.48879, 0.99828, 0.33816}, {0.50362, 0.99879, 0.32701}, {0.51822, 0.99910, 0.31622}, {0.53255, 0.99919, 0.30581}, 23 | {0.54658, 0.99907, 0.29581}, {0.56026, 0.99873, 0.28623}, {0.57357, 0.99817, 0.27712}, {0.58646, 0.99739, 0.26849}, {0.59891, 0.99638, 0.26038}, {0.61088, 0.99514, 0.25280}, {0.62233, 0.99366, 0.24579}, {0.63323, 0.99195, 0.23937}, 24 | {0.64362, 0.98999, 0.23356}, {0.65394, 0.98775, 0.22835}, {0.66428, 0.98524, 0.22370}, {0.67462, 0.98246, 0.21960}, {0.68494, 0.97941, 0.21602}, {0.69525, 0.97610, 0.21294}, {0.70553, 0.97255, 0.21032}, {0.71577, 0.96875, 0.20815}, 25 | {0.72596, 0.96470, 0.20640}, {0.73610, 0.96043, 0.20504}, {0.74617, 0.95593, 0.20406}, {0.75617, 0.95121, 0.20343}, {0.76608, 0.94627, 0.20311}, {0.77591, 0.94113, 0.20310}, {0.78563, 0.93579, 0.20336}, {0.79524, 0.93025, 0.20386}, 26 | {0.80473, 0.92452, 0.20459}, {0.81410, 0.91861, 0.20552}, {0.82333, 0.91253, 0.20663}, {0.83241, 0.90627, 0.20788}, {0.84133, 0.89986, 0.20926}, {0.85010, 0.89328, 0.21074}, {0.85868, 0.88655, 0.21230}, {0.86709, 0.87968, 0.21391}, 27 | {0.87530, 0.87267, 0.21555}, {0.88331, 0.86553, 0.21719}, {0.89112, 0.85826, 0.21880}, {0.89870, 0.85087, 0.22038}, {0.90605, 0.84337, 0.22188}, {0.91317, 0.83576, 0.22328}, {0.92004, 0.82806, 0.22456}, {0.92666, 0.82025, 0.22570}, 28 | {0.93301, 0.81236, 0.22667}, {0.93909, 0.80439, 0.22744}, {0.94489, 0.79634, 0.22800}, {0.95039, 0.78823, 0.22831}, {0.95560, 0.78005, 0.22836}, {0.96049, 0.77181, 0.22811}, {0.96507, 0.76352, 0.22754}, {0.96931, 0.75519, 0.22663}, 29 | {0.97323, 0.74682, 0.22536}, {0.97679, 0.73842, 0.22369}, {0.98000, 0.73000, 0.22161}, {0.98289, 0.72140, 0.21918}, {0.98549, 0.71250, 0.21650}, {0.98781, 0.70330, 0.21358}, {0.98986, 0.69382, 0.21043}, {0.99163, 0.68408, 0.20706}, 30 | {0.99314, 0.67408, 0.20348}, {0.99438, 0.66386, 0.19971}, {0.99535, 0.65341, 0.19577}, {0.99607, 0.64277, 0.19165}, {0.99654, 0.63193, 0.18738}, {0.99675, 0.62093, 0.18297}, {0.99672, 0.60977, 0.17842}, {0.99644, 0.59846, 0.17376}, 31 | {0.99593, 0.58703, 0.16899}, {0.99517, 0.57549, 0.16412}, {0.99419, 0.56386, 0.15918}, {0.99297, 0.55214, 0.15417}, {0.99153, 0.54036, 0.14910}, {0.98987, 0.52854, 0.14398}, {0.98799, 0.51667, 0.13883}, {0.98590, 0.50479, 0.13367}, 32 | {0.98360, 0.49291, 0.12849}, {0.98108, 0.48104, 0.12332}, {0.97837, 0.46920, 0.11817}, {0.97545, 0.45740, 0.11305}, {0.97234, 0.44565, 0.10797}, {0.96904, 0.43399, 0.10294}, {0.96555, 0.42241, 0.09798}, {0.96187, 0.41093, 0.09310}, 33 | {0.95801, 0.39958, 0.08831}, {0.95398, 0.38836, 0.08362}, {0.94977, 0.37729, 0.07905}, {0.94538, 0.36638, 0.07461}, {0.94084, 0.35566, 0.07031}, {0.93612, 0.34513, 0.06616}, {0.93125, 0.33482, 0.06218}, {0.92623, 0.32473, 0.05837}, 34 | {0.92105, 0.31489, 0.05475}, {0.91572, 0.30530, 0.05134}, {0.91024, 0.29599, 0.04814}, {0.90463, 0.28696, 0.04516}, {0.89888, 0.27824, 0.04243}, {0.89298, 0.26981, 0.03993}, {0.88691, 0.26152, 0.03753}, {0.88066, 0.25334, 0.03521}, 35 | {0.87422, 0.24526, 0.03297}, {0.86760, 0.23730, 0.03082}, {0.86079, 0.22945, 0.02875}, {0.85380, 0.22170, 0.02677}, {0.84662, 0.21407, 0.02487}, {0.83926, 0.20654, 0.02305}, {0.83172, 0.19912, 0.02131}, {0.82399, 0.19182, 0.01966}, 36 | {0.81608, 0.18462, 0.01809}, {0.80799, 0.17753, 0.01660}, {0.79971, 0.17055, 0.01520}, {0.79125, 0.16368, 0.01387}, {0.78260, 0.15693, 0.01264}, {0.77377, 0.15028, 0.01148}, {0.76476, 0.14374, 0.01041}, {0.75556, 0.13731, 0.00942}, 37 | {0.74617, 0.13098, 0.00851}, {0.73661, 0.12477, 0.00769}, {0.72686, 0.11867, 0.00695}, {0.71692, 0.11268, 0.00629}, {0.70680, 0.10680, 0.00571}, {0.69650, 0.10102, 0.00522}, {0.68602, 0.09536, 0.00481}, {0.67535, 0.08980, 0.00449}, 38 | {0.66449, 0.08436, 0.00424}, {0.65345, 0.07902, 0.00408}, {0.64223, 0.07380, 0.00401}, {0.63082, 0.06868, 0.00401}, {0.61923, 0.06367, 0.00410}, {0.60746, 0.05878, 0.00427}, {0.59550, 0.05399, 0.00453}, {0.58336, 0.04931, 0.00486}, 39 | {0.57103, 0.04474, 0.00529}, {0.55852, 0.04028, 0.00579}, {0.54583, 0.03593, 0.00638}, {0.53295, 0.03169, 0.00705}, {0.51989, 0.02756, 0.00780}, {0.50664, 0.02354, 0.00863}, {0.49321, 0.01963, 0.00955}, {0.47960, 0.01583, 0.01055}}; 40 | 41 | unsigned char turbo_srgb_bytes[256][3] = { 42 | {48, 18, 59}, {50, 21, 67}, {51, 24, 74}, {52, 27, 81}, {53, 30, 88}, {54, 33, 95}, {55, 36, 102}, {56, 39, 109}, {57, 42, 115}, {58, 45, 121}, {59, 47, 128}, {60, 50, 134}, {61, 53, 139}, {62, 56, 145}, {63, 59, 151}, 43 | {63, 62, 156}, {64, 64, 162}, {65, 67, 167}, {65, 70, 172}, {66, 73, 177}, {66, 75, 181}, {67, 78, 186}, {68, 81, 191}, {68, 84, 195}, {68, 86, 199}, {69, 89, 203}, {69, 92, 207}, {69, 94, 211}, {70, 97, 214}, {70, 100, 218}, 44 | {70, 102, 221}, {70, 105, 224}, {70, 107, 227}, {71, 110, 230}, {71, 113, 233}, {71, 115, 235}, {71, 118, 238}, {71, 120, 240}, {71, 123, 242}, {70, 125, 244}, {70, 128, 246}, {70, 130, 248}, {70, 133, 250}, {70, 135, 251}, {69, 138, 252}, 45 | {69, 140, 253}, {68, 143, 254}, {67, 145, 254}, {66, 148, 255}, {65, 150, 255}, {64, 153, 255}, {62, 155, 254}, {61, 158, 254}, {59, 160, 253}, {58, 163, 252}, {56, 165, 251}, {55, 168, 250}, {53, 171, 248}, {51, 173, 247}, {49, 175, 245}, 46 | {47, 178, 244}, {46, 180, 242}, {44, 183, 240}, {42, 185, 238}, {40, 188, 235}, {39, 190, 233}, {37, 192, 231}, {35, 195, 228}, {34, 197, 226}, {32, 199, 223}, {31, 201, 221}, {30, 203, 218}, {28, 205, 216}, {27, 208, 213}, {26, 210, 210}, 47 | {26, 212, 208}, {25, 213, 205}, {24, 215, 202}, {24, 217, 200}, {24, 219, 197}, {24, 221, 194}, {24, 222, 192}, {24, 224, 189}, {25, 226, 187}, {25, 227, 185}, {26, 228, 182}, {28, 230, 180}, {29, 231, 178}, {31, 233, 175}, {32, 234, 172}, 48 | {34, 235, 170}, {37, 236, 167}, {39, 238, 164}, {42, 239, 161}, {44, 240, 158}, {47, 241, 155}, {50, 242, 152}, {53, 243, 148}, {56, 244, 145}, {60, 245, 142}, {63, 246, 138}, {67, 247, 135}, {70, 248, 132}, {74, 248, 128}, {78, 249, 125}, 49 | {82, 250, 122}, {85, 250, 118}, {89, 251, 115}, {93, 252, 111}, {97, 252, 108}, {101, 253, 105}, {105, 253, 102}, {109, 254, 98}, {113, 254, 95}, {117, 254, 92}, {121, 254, 89}, {125, 255, 86}, {128, 255, 83}, {132, 255, 81}, {136, 255, 78}, 50 | {139, 255, 75}, {143, 255, 73}, {146, 255, 71}, {150, 254, 68}, {153, 254, 66}, {156, 254, 64}, {159, 253, 63}, {161, 253, 61}, {164, 252, 60}, {167, 252, 58}, {169, 251, 57}, {172, 251, 56}, {175, 250, 55}, {177, 249, 54}, {180, 248, 54}, 51 | {183, 247, 53}, {185, 246, 53}, {188, 245, 52}, {190, 244, 52}, {193, 243, 52}, {195, 241, 52}, {198, 240, 52}, {200, 239, 52}, {203, 237, 52}, {205, 236, 52}, {208, 234, 52}, {210, 233, 53}, {212, 231, 53}, {215, 229, 53}, {217, 228, 54}, 52 | {219, 226, 54}, {221, 224, 55}, {223, 223, 55}, {225, 221, 55}, {227, 219, 56}, {229, 217, 56}, {231, 215, 57}, {233, 213, 57}, {235, 211, 57}, {236, 209, 58}, {238, 207, 58}, {239, 205, 58}, {241, 203, 58}, {242, 201, 58}, {244, 199, 58}, 53 | {245, 197, 58}, {246, 195, 58}, {247, 193, 58}, {248, 190, 57}, {249, 188, 57}, {250, 186, 57}, {251, 184, 56}, {251, 182, 55}, {252, 179, 54}, {252, 177, 54}, {253, 174, 53}, {253, 172, 52}, {254, 169, 51}, {254, 167, 50}, {254, 164, 49}, 54 | {254, 161, 48}, {254, 158, 47}, {254, 155, 45}, {254, 153, 44}, {254, 150, 43}, {254, 147, 42}, {254, 144, 41}, {253, 141, 39}, {253, 138, 38}, {252, 135, 37}, {252, 132, 35}, {251, 129, 34}, {251, 126, 33}, {250, 123, 31}, {249, 120, 30}, 55 | {249, 117, 29}, {248, 114, 28}, {247, 111, 26}, {246, 108, 25}, {245, 105, 24}, {244, 102, 23}, {243, 99, 21}, {242, 96, 20}, {241, 93, 19}, {240, 91, 18}, {239, 88, 17}, {237, 85, 16}, {236, 83, 15}, {235, 80, 14}, {234, 78, 13}, 56 | {232, 75, 12}, {231, 73, 12}, {229, 71, 11}, {228, 69, 10}, {226, 67, 10}, {225, 65, 9}, {223, 63, 8}, {221, 61, 8}, {220, 59, 7}, {218, 57, 7}, {216, 55, 6}, {214, 53, 6}, {212, 51, 5}, {210, 49, 5}, {208, 47, 5}, 57 | {206, 45, 4}, {204, 43, 4}, {202, 42, 4}, {200, 40, 3}, {197, 38, 3}, {195, 37, 3}, {193, 35, 2}, {190, 33, 2}, {188, 32, 2}, {185, 30, 2}, {183, 29, 2}, {180, 27, 1}, {178, 26, 1}, {175, 24, 1}, {172, 23, 1}, 58 | {169, 22, 1}, {167, 20, 1}, {164, 19, 1}, {161, 18, 1}, {158, 16, 1}, {155, 15, 1}, {152, 14, 1}, {149, 13, 1}, {146, 11, 1}, {142, 10, 1}, {139, 9, 2}, {136, 8, 2}, {133, 7, 2}, {129, 6, 2}, {126, 5, 2}, 59 | {122, 4, 3}}; 60 | 61 | Eigen::Vector4i colormap(COLORMAP_TYPE type, int x) { 62 | x = std::max(0, std::min(255, x)); 63 | switch(type) { 64 | default: 65 | case TURBO: 66 | return Eigen::Vector4i(turbo_srgb_bytes[x][0], turbo_srgb_bytes[x][1], turbo_srgb_bytes[x][2], 255); 67 | break; 68 | } 69 | 70 | return Eigen::Vector4i::Zero(); 71 | } 72 | 73 | Eigen::Vector4f colormapf(COLORMAP_TYPE type, float x_) { 74 | int x = std::max(0, std::min(255, x_ * 255)); 75 | 76 | switch (type) { 77 | default: 78 | case TURBO: 79 | return Eigen::Vector4f(turbo_srgb_floats[x][0], turbo_srgb_floats[x][1], turbo_srgb_floats[x][2], 1.0f); 80 | break; 81 | } 82 | 83 | return Eigen::Vector4f::Zero(); 84 | } 85 | 86 | Eigen::Vector4i colormap_categorical(COLORMAP_TYPE type, int x_, int num_categories) { 87 | x_ = x_ % num_categories; 88 | int step = 256 / num_categories; 89 | int x = x_ * step + step / 2; 90 | 91 | return colormap(type, x); 92 | } 93 | 94 | Eigen::Vector4f colormap_categoricalf(COLORMAP_TYPE type, int x_, int num_categories) { 95 | x_ = x_ % num_categories; 96 | int step = 256 / num_categories; 97 | int x = x_ * step + step / 2; 98 | return colormapf(type, x / 255.0f); 99 | } 100 | 101 | } // namespace glk -------------------------------------------------------------------------------- /src/glk/frame_buffer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace glk { 9 | 10 | FrameBuffer::FrameBuffer(const Eigen::Vector2i& size) : width(size[0]), height(size[1]) { 11 | color_buffers.push_back(std::make_shared(size, GL_RGBA, GL_RGBA, GL_UNSIGNED_BYTE)); 12 | // color_buffers.push_back(std::make_shared(size, GL_RGBA32I, GL_RGBA_INTEGER, GL_INT)); 13 | depth_buffer = std::make_shared(size, GL_DEPTH_COMPONENT32F, GL_DEPTH_COMPONENT, GL_FLOAT); 14 | 15 | glGenFramebuffers(1, &frame_buffer); 16 | glBindFramebuffer(GL_FRAMEBUFFER, frame_buffer); 17 | 18 | glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, color_buffers[0]->id(), 0); 19 | // glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT1, GL_TEXTURE_2D, color_buffers[1]->id(), 0); 20 | glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depth_buffer->id(), 0); 21 | 22 | GLenum color_attachments[] = {GL_COLOR_ATTACHMENT0, GL_COLOR_ATTACHMENT1, GL_COLOR_ATTACHMENT2}; 23 | glDrawBuffers(color_buffers.size(), color_attachments); 24 | 25 | glBindFramebuffer(GL_FRAMEBUFFER, 0); 26 | } 27 | 28 | FrameBuffer::~FrameBuffer() { glDeleteFramebuffers(1, &frame_buffer); } 29 | 30 | void FrameBuffer::bind() { 31 | glGetIntegerv(GL_VIEWPORT, viewport); 32 | glViewport(0, 0, width, height); 33 | glBindFramebuffer(GL_FRAMEBUFFER, frame_buffer); 34 | } 35 | 36 | void FrameBuffer::unbind() const { 37 | glBindFramebuffer(GL_FRAMEBUFFER, 0); 38 | glViewport(viewport[0], viewport[1], viewport[2], viewport[3]); 39 | } 40 | 41 | void FrameBuffer::add_color_buffer(GLuint internal_format, GLuint format, GLuint type) { 42 | glBindFramebuffer(GL_FRAMEBUFFER, frame_buffer); 43 | 44 | color_buffers.push_back(std::make_shared(color_buffers.front()->size(), internal_format, format, type)); 45 | glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0 + color_buffers.size() - 1, GL_TEXTURE_2D, color_buffers.back()->id(), 0); 46 | 47 | std::vector color_attachments(color_buffers.size()); 48 | for(int i=0; i 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace glk { 15 | 16 | bool GLSLShader::init(const std::string& shader_path) { 17 | GLuint vertex_shader = read_shader_from_file(shader_path + ".vert", GL_VERTEX_SHADER); 18 | GLuint fragment_shader = read_shader_from_file(shader_path + ".frag", GL_FRAGMENT_SHADER); 19 | 20 | shader_program = glCreateProgram(); 21 | glAttachShader(shader_program, vertex_shader); 22 | glAttachShader(shader_program, fragment_shader); 23 | glLinkProgram(shader_program); 24 | 25 | glDeleteShader(vertex_shader); 26 | glDeleteShader(fragment_shader); 27 | 28 | GLint result = GL_FALSE; 29 | int info_log_length; 30 | 31 | glGetProgramiv(shader_program, GL_LINK_STATUS, &result); 32 | glGetProgramiv(shader_program, GL_INFO_LOG_LENGTH, &info_log_length); 33 | std::vector error_message(info_log_length); 34 | glGetProgramInfoLog(shader_program, info_log_length, nullptr, error_message.data()); 35 | 36 | if (result != GL_TRUE) { 37 | std::cerr << "error : failed to link program" << std::endl; 38 | std::cerr << std::string(error_message.begin(), error_message.end()) << std::endl; 39 | return false; 40 | } 41 | 42 | return true; 43 | } 44 | 45 | GLint GLSLShader::attrib(const std::string& name) { 46 | auto found = attrib_cache.find(name); 47 | if (found != attrib_cache.end()) { 48 | return found->second; 49 | } 50 | 51 | GLint id = glGetAttribLocation(shader_program, name.c_str()); 52 | if (id == -1) { 53 | std::cerr << "warning : attrib " << name << " not found" << std::endl; 54 | } 55 | 56 | attrib_cache[name] = id; 57 | return id; 58 | } 59 | 60 | GLint GLSLShader::uniform(const std::string& name) { 61 | auto found = uniform_cache.find(name); 62 | if (found != uniform_cache.end()) { 63 | return found->second; 64 | } 65 | 66 | GLint id = glGetUniformLocation(shader_program, name.c_str()); 67 | if (id == -1) { 68 | std::cerr << "warning : uniform " << name << " not found" << std::endl; 69 | } 70 | 71 | uniform_cache[name] = id; 72 | return id; 73 | } 74 | 75 | GLuint GLSLShader::read_shader_from_file(const std::string& filename, GLuint shader_type) { 76 | GLuint shader_id = glCreateShader(shader_type); 77 | std::ifstream ifs(filename); 78 | if (!ifs) { 79 | std::cerr << "error: failed to open " << filename << std::endl; 80 | return GL_FALSE; 81 | } 82 | 83 | std::stringstream sst; 84 | sst << ifs.rdbuf(); 85 | std::string source = sst.str(); 86 | 87 | GLint result = GL_FALSE; 88 | int info_log_length = 0; 89 | 90 | char const* source_ptr = source.c_str(); 91 | glShaderSource(shader_id, 1, &source_ptr, nullptr); 92 | glCompileShader(shader_id); 93 | 94 | glGetShaderiv(shader_id, GL_COMPILE_STATUS, &result); 95 | glGetShaderiv(shader_id, GL_INFO_LOG_LENGTH, &info_log_length); 96 | 97 | std::vector error_message(info_log_length); 98 | glGetShaderInfoLog(shader_id, info_log_length, nullptr, error_message.data()); 99 | 100 | if (result != GL_TRUE) { 101 | std::cerr << "error : failed to compile shader " << filename << std::endl; 102 | std::cerr << std::string(error_message.begin(), error_message.end()) << std::endl; 103 | } 104 | 105 | return shader_id; 106 | } 107 | 108 | } // namespace glk 109 | -------------------------------------------------------------------------------- /src/glk/lines.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace glk { 7 | 8 | Lines::Lines(float line_width, 9 | const std::vector>& vertices, 10 | const std::vector>& colors, 11 | const std::vector>& infos) 12 | : num_vertices(vertices.size()) { 13 | vao = vbo = cbo = ibo = 0; 14 | 15 | glGenVertexArrays(1, &vao); 16 | glBindVertexArray(vao); 17 | 18 | std::vector> vertices_ext(vertices.size() * 4); 19 | for(int i=0; i> colors_ext(colors.size() * 4); 43 | for(int i = 0; i < colors.size(); i+=2) { 44 | for(int j = 0; j < 4; j++) { 45 | colors_ext[i * 4 + j * 2] = colors[i]; 46 | colors_ext[i * 4 + j * 2 + 1] = colors[i + 1]; 47 | } 48 | } 49 | glGenBuffers(1, &cbo); 50 | glBindBuffer(GL_ARRAY_BUFFER, cbo); 51 | glBufferData(GL_ARRAY_BUFFER, sizeof(float) * colors_ext.size() * 4, colors_ext.data(), GL_STATIC_DRAW); 52 | } 53 | 54 | if(!infos.empty()) { 55 | std::vector> infos_ext(infos.size() * 4); 56 | for(int i = 0; i < infos.size(); i += 2) { 57 | for(int j = 0; j < 4; j++) { 58 | infos_ext[i * 4 + j * 2] = infos[i]; 59 | infos_ext[i * 4 + j * 2 + 1] = infos[i + 1]; 60 | } 61 | } 62 | glGenBuffers(1, &ibo); 63 | glBindBuffer(GL_ARRAY_BUFFER, ibo); 64 | glBufferData(GL_ARRAY_BUFFER, sizeof(int) * infos_ext.size() * 4, infos_ext.data(), GL_STATIC_DRAW); 65 | } 66 | 67 | /* 68 | std::vector indices; 69 | for(int i = 0; i < vertices_ext.size(); i += 4) { 70 | indices.push_back(i); 71 | indices.push_back(i + 1); 72 | indices.push_back(i + 2); 73 | indices.push_back(i + 1); 74 | indices.push_back(i + 3); 75 | indices.push_back(i + 2); 76 | } 77 | */ 78 | 79 | std::vector sub_indices = { 80 | 0, 1, 4, 81 | 1, 5, 4, 82 | 4, 5, 2, 83 | 5, 3, 2, 84 | 2, 3, 6, 85 | 3, 6, 7, 86 | 6, 7, 0, 87 | 7, 1, 0 88 | }; 89 | 90 | std::vector indices; 91 | for(int i = 0; i < vertices_ext.size(); i += 8) { 92 | for(int j=0; j 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace glk { 11 | 12 | PLYLoader::PLYLoader(const std::string &filename) { 13 | std::ifstream ifs(filename); 14 | if (!ifs) { 15 | std::cerr << "error: failed to open " << filename << std::endl; 16 | return; 17 | } 18 | 19 | int num_vertices = 0; 20 | int num_faces = 0; 21 | std::vector properties; 22 | while (!ifs.eof()) { 23 | std::string line; 24 | std::getline(ifs, line); 25 | 26 | if (line.empty()) { 27 | continue; 28 | } 29 | 30 | std::stringstream sst(line); 31 | std::string token; 32 | 33 | if (line.find("element vertex") != std::string::npos) { 34 | sst >> token >> token >> num_vertices; 35 | } 36 | if (line.find("element face") != std::string::npos) { 37 | sst >> token >> token >> num_faces; 38 | } 39 | 40 | if (line.find("property float32") != std::string::npos) { 41 | std::string property; 42 | sst >> token >> token >> property; 43 | properties.push_back(property); 44 | } 45 | 46 | if (line.find("end") != std::string::npos) { 47 | break; 48 | } 49 | } 50 | 51 | vertices.resize(num_vertices); 52 | for (int i = 0; i < num_vertices; i++) { 53 | std::string line; 54 | std::getline(ifs, line); 55 | 56 | std::stringstream sst(line); 57 | sst >> vertices[i][0] >> vertices[i][1] >> vertices[i][2]; 58 | } 59 | 60 | indices.resize(num_faces * 3); 61 | for (int i = 0; i < num_faces; i++) { 62 | std::string line; 63 | std::getline(ifs, line); 64 | 65 | int faces = 0; 66 | std::stringstream sst(line); 67 | sst >> faces >> indices[i * 3 + 2] >> indices[i * 3 + 1] >> indices[i * 3]; 68 | 69 | if (faces != 3) { 70 | std::cerr << "error : only faces with three vertices are supported!!" << std::endl; 71 | } 72 | } 73 | 74 | NormalEstimater nest(vertices, indices); 75 | normals = nest.normals; 76 | } 77 | 78 | } // namespace glk 79 | -------------------------------------------------------------------------------- /src/glk/mesh.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace glk { 11 | 12 | Mesh::Mesh(const std::vector>& vertices, const std::vector>& normals, const std::vector& indices) 13 | : num_vertices(vertices.size()), num_indices(indices.size()) { 14 | glGenVertexArrays(1, &vao); 15 | glBindVertexArray(vao); 16 | 17 | glGenBuffers(1, &vbo); 18 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 19 | glBufferData(GL_ARRAY_BUFFER, sizeof(float) * vertices.size() * 3, vertices.data(), GL_STATIC_DRAW); 20 | 21 | glGenBuffers(1, &nbo); 22 | glBindBuffer(GL_ARRAY_BUFFER, nbo); 23 | glBufferData(GL_ARRAY_BUFFER, sizeof(float) * normals.size() * 3, normals.data(), GL_STATIC_DRAW); 24 | 25 | glGenBuffers(1, &ebo); 26 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ebo); 27 | glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(int) * indices.size(), indices.data(), GL_STATIC_DRAW); 28 | 29 | glBindVertexArray(0); 30 | glBindBuffer(GL_ARRAY_BUFFER, 0); 31 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); 32 | } 33 | 34 | Mesh ::~Mesh() { 35 | glDeleteBuffers(1, &vbo); 36 | glDeleteBuffers(1, &ebo); 37 | glDeleteVertexArrays(1, &vao); 38 | } 39 | 40 | void Mesh::draw(glk::GLSLShader& shader) const { 41 | GLint position_loc = shader.attrib("vert_position"); 42 | GLint normal_loc = shader.attrib("vert_normal"); 43 | 44 | glBindVertexArray(vao); 45 | 46 | glEnableVertexAttribArray(position_loc); 47 | glEnableVertexAttribArray(normal_loc); 48 | 49 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 50 | glVertexAttribPointer(position_loc, 3, GL_FLOAT, GL_FALSE, 0, 0); 51 | 52 | glBindBuffer(GL_ARRAY_BUFFER, nbo); 53 | glVertexAttribPointer(normal_loc, 3, GL_FLOAT, GL_FALSE, 0, 0); 54 | 55 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ebo); 56 | glDrawElements(GL_TRIANGLES, num_indices, GL_UNSIGNED_INT, 0); 57 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); 58 | 59 | glDisableVertexAttribArray(position_loc); 60 | glDisableVertexAttribArray(normal_loc); 61 | 62 | glBindBuffer(GL_ARRAY_BUFFER, 0); 63 | glBindVertexArray(0); 64 | } 65 | 66 | } // namespace glk -------------------------------------------------------------------------------- /src/glk/pointcloud_buffer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace glk { 8 | 9 | PointCloudBuffer::PointCloudBuffer(const std::string& cloud_filename) { 10 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 11 | if(pcl::io::loadPCDFile(cloud_filename, *cloud)) { 12 | std::cerr << "error: failed to load " << cloud_filename << std::endl; 13 | num_points = 0; 14 | return; 15 | } 16 | 17 | stride = sizeof(pcl::PointXYZI); 18 | num_points = cloud->size(); 19 | std::cout << "num_points " << num_points << std::endl; 20 | 21 | glGenVertexArrays(1, &vao); 22 | glBindVertexArray(vao); 23 | 24 | glGenBuffers(1, &vbo); 25 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 26 | glBufferData(GL_ARRAY_BUFFER, cloud->size() * sizeof(pcl::PointXYZI), cloud->points.data(), GL_STATIC_DRAW); 27 | } 28 | 29 | PointCloudBuffer::PointCloudBuffer(const pcl::PointCloud::ConstPtr& cloud) { 30 | stride = sizeof(pcl::PointXYZI); 31 | num_points = cloud->size(); 32 | std::cout << "num_points " << num_points << ", stride " << stride << std::endl; 33 | 34 | glGenVertexArrays(1, &vao); 35 | glBindVertexArray(vao); 36 | 37 | glGenBuffers(1, &vbo); 38 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 39 | glBufferData(GL_ARRAY_BUFFER, cloud->size() * sizeof(pcl::PointXYZI), cloud->points.data(), GL_STATIC_DRAW); 40 | } 41 | 42 | PointCloudBuffer::~PointCloudBuffer() { 43 | glDeleteVertexArrays(1, &vao); 44 | glDeleteBuffers(1, &vbo); 45 | } 46 | 47 | void PointCloudBuffer::draw(glk::GLSLShader& shader) { 48 | if(num_points == 0) { 49 | return; 50 | } 51 | 52 | GLint position_loc = shader.attrib("vert_position"); 53 | 54 | glBindVertexArray(vao); 55 | glEnableVertexAttribArray(position_loc); 56 | 57 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 58 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, stride, 0); 59 | 60 | glDrawArrays(GL_POINTS, 0, num_points); 61 | 62 | glBindBuffer(GL_ARRAY_BUFFER, 0); 63 | glDisableVertexAttribArray(0); 64 | } 65 | 66 | } // namespace glk 67 | -------------------------------------------------------------------------------- /src/glk/primitives/primitives.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace glk { 16 | 17 | Primitives* Primitives::instance_ = nullptr; 18 | 19 | const glk::Drawable& Primitives::primitive(PrimitiveType type) { 20 | if(meshes[type] == nullptr) { 21 | switch(type) { 22 | default: 23 | std::cerr << "error : unknown primitive type " << type << std::endl; 24 | break; 25 | case ICOSAHEDRON: { 26 | glk::Icosahedron icosahedron; 27 | glk::Flatize flat(icosahedron.vertices, icosahedron.indices); 28 | meshes[type].reset(new glk::Mesh(flat.vertices, flat.normals, flat.indices)); 29 | } break; 30 | case SPHERE: { 31 | glk::Icosahedron icosahedron; 32 | icosahedron.subdivide(); 33 | icosahedron.subdivide(); 34 | icosahedron.spherize(); 35 | meshes[type].reset(new glk::Mesh(icosahedron.vertices, icosahedron.normals, icosahedron.indices)); 36 | } break; 37 | case CUBE: { 38 | glk::Cube cube; 39 | glk::Flatize flat(cube.vertices, cube.indices); 40 | meshes[type].reset(new glk::Mesh(flat.vertices, flat.normals, flat.indices)); 41 | } break; 42 | case CONE: { 43 | glk::Cone cone; 44 | glk::Flatize flat(cone.vertices, cone.indices); 45 | meshes[type].reset(new glk::Mesh(flat.vertices, flat.normals, flat.indices)); 46 | } break; 47 | case GRID: { 48 | glk::Grid grid; 49 | meshes[type].reset(new glk::Lines(0.01f, grid.vertices)); 50 | } break; 51 | case BUNNY: { 52 | glk::PLYLoader ply("data/models/bunny.ply"); 53 | meshes[type].reset(new glk::Mesh(ply.vertices, ply.normals, ply.indices)); 54 | } break; 55 | case COORDINATE_SYSTEM: { 56 | glk::CoordinateSystem coord; 57 | meshes[type].reset(new glk::Lines(0.01f, coord.vertices, coord.colors)); 58 | } break; 59 | } 60 | } 61 | 62 | return *meshes[type]; 63 | } 64 | } // namespace glk -------------------------------------------------------------------------------- /src/guik/camera_control.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace guik { 11 | 12 | ArcCameraControl::ArcCameraControl() : center(0.0f, 0.0f, 0.0f), distance(10.0f), left_button_down(false), theta(0.0f), phi(-60.0f * M_PI / 180.0f) { 13 | left_button_down = false; 14 | middle_button_down = false; 15 | } 16 | 17 | ArcCameraControl::~ArcCameraControl() {} 18 | 19 | void ArcCameraControl::mouse(const Eigen::Vector2i& p, int button, bool down) { 20 | if (button == 0) { 21 | left_button_down = down; 22 | } 23 | if (button == 2) { 24 | middle_button_down = down; 25 | } 26 | drag_last_pos = p; 27 | } 28 | 29 | void ArcCameraControl::drag(const Eigen::Vector2i& p, int button) { 30 | Eigen::Vector2i rel = p - drag_last_pos; 31 | 32 | if (left_button_down) { 33 | theta -= rel[0] * 0.01f; 34 | phi -= rel[1] * 0.01f; 35 | 36 | phi = std::min(M_PI_2 - 0.01, std::max(-M_PI_2 + 0.01, phi)); 37 | } 38 | 39 | if (middle_button_down) { 40 | center += Eigen::AngleAxisf(theta + M_PI_2, Eigen::Vector3f::UnitZ()) * Eigen::Vector3f(-rel[0], rel[1], 0.0f) * distance * 0.001f; 41 | } 42 | 43 | drag_last_pos = p; 44 | } 45 | 46 | void ArcCameraControl::scroll(const Eigen::Vector2f& rel) { 47 | if (rel[0] > 0) { 48 | distance = distance * 0.9f; 49 | } else if (rel[0] < 0) { 50 | distance = distance * 1.1f; 51 | } 52 | 53 | distance = std::max(0.1, distance); 54 | } 55 | 56 | Eigen::Quaternionf ArcCameraControl::rotation() const { return Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(phi, Eigen::Vector3f::UnitY()); } 57 | 58 | Eigen::Matrix4f ArcCameraControl::view_matrix() const { 59 | Eigen::Vector3f offset = rotation() * Eigen::Vector3f(distance, 0.0f, 0.0f); 60 | Eigen::Vector3f eye = center + offset; 61 | 62 | glm::mat4 mat = glm::lookAt(glm::vec3(eye[0], eye[1], eye[2]), glm::vec3(center[0], center[1], center[2]), glm::vec3(0.0f, 0.0f, 1.0f)); 63 | return Eigen::Map(glm::value_ptr(mat)); 64 | } 65 | 66 | } // namespace guik 67 | -------------------------------------------------------------------------------- /src/guik/gl_canvas.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | namespace guik { 21 | 22 | /** 23 | * @brief Construct a new GLCanvas object 24 | * 25 | * @param data_directory 26 | * @param size 27 | */ 28 | GLCanvas::GLCanvas(const std::string& data_directory, const Eigen::Vector2i& size) : size(size), point_size(50.0f), keyframe_scale(1.0f), min_z(-1.5f), max_z(5.0f), z_clipping(true) { 29 | frame_buffer.reset(new glk::FrameBuffer(size)); 30 | frame_buffer->add_color_buffer(GL_RGBA32I, GL_RGBA_INTEGER, GL_INT); 31 | 32 | shader.reset(new glk::GLSLShader()); 33 | if (!shader->init(data_directory + "/shader/rainbow")) { 34 | shader.reset(); 35 | return; 36 | } 37 | 38 | shader->use(); 39 | 40 | camera_control.reset(new guik::ArcCameraControl()); 41 | projection_control.reset(new guik::ProjectionControl(size)); 42 | texture_renderer.reset(new glk::TextureRenderer(data_directory)); 43 | } 44 | 45 | /** 46 | * @brief 47 | * 48 | * @return true 49 | * @return false 50 | */ 51 | bool GLCanvas::ready() const { return frame_buffer && shader && camera_control && texture_renderer; } 52 | 53 | void GLCanvas::reset_camera() { 54 | camera_control.reset(new guik::ArcCameraControl()); 55 | projection_control.reset(new guik::ProjectionControl(size)); 56 | } 57 | 58 | /** 59 | * @brief Set the Size object 60 | * 61 | * @param size 62 | */ 63 | void GLCanvas::set_size(const Eigen::Vector2i& size) { 64 | this->size = size; 65 | projection_control->set_size(size); 66 | frame_buffer.reset(new glk::FrameBuffer(size)); 67 | frame_buffer->add_color_buffer(GL_RGBA32I, GL_RGBA_INTEGER, GL_INT); 68 | } 69 | 70 | /** 71 | * @brief 72 | * 73 | */ 74 | void GLCanvas::bind(bool clear_buffers) { 75 | frame_buffer->bind(); 76 | glDisable(GL_SCISSOR_TEST); 77 | if(clear_buffers) { 78 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 79 | } 80 | 81 | GLint clear_color[] = {0, -1, 0, 0}; 82 | glClearTexImage(frame_buffer->color(1).id(), 0, GL_RGBA_INTEGER, GL_INT, clear_color); 83 | 84 | shader->use(); 85 | 86 | Eigen::Matrix4f view_matrix = camera_control->view_matrix(); 87 | Eigen::Matrix4f projection_matrix = projection_control->projection_matrix(); 88 | 89 | shader->set_uniform("view_matrix", view_matrix); 90 | shader->set_uniform("projection_matrix", projection_matrix); 91 | shader->set_uniform("z_clipping", z_clipping ? 1 : 0); 92 | shader->set_uniform("z_range", Eigen::Vector2f(min_z, max_z)); 93 | 94 | shader->set_uniform("color_mode", 0); 95 | shader->set_uniform("point_scale", 1.0f); 96 | shader->set_uniform("point_size", point_size); 97 | shader->set_uniform("keyframe_scale", keyframe_scale); 98 | 99 | glEnable(GL_DEPTH_TEST); 100 | } 101 | 102 | /** 103 | * @brief 104 | * 105 | */ 106 | void GLCanvas::unbind() { 107 | glDisable(GL_DEPTH_TEST); 108 | glFlush(); 109 | 110 | frame_buffer->unbind(); 111 | } 112 | 113 | /** 114 | * @brief 115 | * 116 | */ 117 | void GLCanvas::render_to_screen(int color_buffer_id) { texture_renderer->draw(frame_buffer->color(color_buffer_id).id()); } 118 | 119 | /** 120 | * @brief 121 | * 122 | */ 123 | void GLCanvas::mouse_control() { 124 | ImGuiIO& io = ImGui::GetIO(); 125 | auto mouse_pos = ImGui::GetMousePos(); 126 | auto drag_delta = ImGui::GetMouseDragDelta(); 127 | 128 | Eigen::Vector2i p(mouse_pos.x, mouse_pos.y); 129 | 130 | for (int i = 0; i < 3; i++) { 131 | if (ImGui::IsMouseClicked(i)) { 132 | camera_control->mouse(p, i, true); 133 | } 134 | if (ImGui::IsMouseReleased(i)) { 135 | camera_control->mouse(p, i, false); 136 | } 137 | if (ImGui::IsMouseDragging(i)) { 138 | camera_control->drag(p, i); 139 | } 140 | 141 | camera_control->scroll(Eigen::Vector2f(io.MouseWheel, io.MouseWheelH)); 142 | } 143 | } 144 | 145 | /** 146 | * @brief 147 | * 148 | * @param p 149 | * @param window 150 | * @return Eigen::Vector4i 151 | */ 152 | Eigen::Vector4i GLCanvas::pick_info(const Eigen::Vector2i& p, int window) const { 153 | if (p[0] < 5 || p[1] < 5 || p[0] > size[0] - 5 || p[1] > size[1] - 5) { 154 | return Eigen::Vector4i(-1, -1, -1, -1); 155 | } 156 | 157 | std::vector pixels = frame_buffer->color(1).read_pixels(GL_RGBA_INTEGER, GL_INT); 158 | 159 | std::vector> ps; 160 | 161 | for (int i = -window; i <= window; i++) { 162 | for (int j = -window; j <= window; j++) { 163 | ps.push_back(Eigen::Vector2i(i, j)); 164 | } 165 | } 166 | 167 | std::sort(ps.begin(), ps.end(), [=](const Eigen::Vector2i& lhs, const Eigen::Vector2i& rhs) { return lhs.norm() < rhs.norm(); }); 168 | for (int i = 0; i < ps.size(); i++) { 169 | Eigen::Vector2i p_ = p + ps[i]; 170 | int index = ((size[1] - p[1]) * size[0] + p_[0]) * 4; 171 | Eigen::Vector4i info = Eigen::Map(&pixels[index]); 172 | 173 | if (info[3] >= 0) { 174 | return info; 175 | } 176 | } 177 | 178 | return Eigen::Vector4i(-1, -1, -1, -1); 179 | } 180 | 181 | /** 182 | * @brief 183 | * 184 | * @param p 185 | * @param window 186 | * @return float 187 | */ 188 | float GLCanvas::pick_depth(const Eigen::Vector2i& p, int window) const { 189 | if (p[0] < 5 || p[1] < 5 || p[0] > size[0] - 5 || p[1] > size[1] - 5) { 190 | return -1.0f; 191 | } 192 | 193 | std::vector pixels = frame_buffer->depth().read_pixels(GL_DEPTH_COMPONENT, GL_FLOAT); 194 | 195 | std::vector> ps; 196 | 197 | for (int i = -window; i <= window; i++) { 198 | for (int j = -window; j <= window; j++) { 199 | ps.push_back(Eigen::Vector2i(i, j)); 200 | } 201 | } 202 | 203 | std::sort(ps.begin(), ps.end(), [=](const Eigen::Vector2i& lhs, const Eigen::Vector2i& rhs) { return lhs.norm() < rhs.norm(); }); 204 | for (int i = 0; i < ps.size(); i++) { 205 | Eigen::Vector2i p_ = p + ps[i]; 206 | int index = ((size[1] - p[1]) * size[0] + p_[0]); 207 | float depth = pixels[index]; 208 | 209 | if (depth < 1.0f) { 210 | return depth; 211 | } 212 | } 213 | 214 | return 1.0f; 215 | } 216 | 217 | /** 218 | * @brief 219 | * 220 | * @param p 221 | * @param depth 222 | * @return Eigen::Vector3f 223 | */ 224 | Eigen::Vector3f GLCanvas::unproject(const Eigen::Vector2i& p, float depth) const { 225 | Eigen::Matrix4f view_matrix = camera_control->view_matrix(); 226 | glm::mat4 view = glm::make_mat4(view_matrix.data()); 227 | 228 | Eigen::Matrix4f projection_matrix = projection_control->projection_matrix(); 229 | glm::mat4 projection = glm::make_mat4(projection_matrix.data()); 230 | 231 | glm::vec4 viewport = glm::vec4(0, 0, size[0], size[1]); 232 | glm::vec3 wincoord = glm::vec3(p[0], size[1] - p[1], depth); 233 | glm::vec3 objcoord = glm::unProject(wincoord, view, projection, viewport); 234 | 235 | return Eigen::Vector3f(objcoord.x, objcoord.y, objcoord.z); 236 | } 237 | 238 | void GLCanvas::draw_ui() { 239 | ImGui::Begin("shader setting", nullptr, ImGuiWindowFlags_AlwaysAutoResize); 240 | ImGui::DragFloat("point_size", &point_size, 10.0f); 241 | ImGui::DragFloat("keyframe_scale", &keyframe_scale, 0.1f); 242 | ImGui::DragFloat("min_z", &min_z, 0.1f); 243 | ImGui::DragFloat("max_z", &max_z, 0.1f); 244 | ImGui::Checkbox("z_clipping", &z_clipping); 245 | ImGui::End(); 246 | 247 | projection_control->draw_ui(); 248 | } 249 | 250 | void GLCanvas::show_projection_setting() { projection_control->show(); } 251 | 252 | } // namespace guik 253 | -------------------------------------------------------------------------------- /src/guik/imgui_application.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace guik { 16 | 17 | Application::Application() : window(nullptr), max_frame_rate(60.0) {} 18 | 19 | Application ::~Application() { 20 | if(!window) { 21 | return; 22 | } 23 | 24 | ImGui_ImplOpenGL3_Shutdown(); 25 | ImGui_ImplGlfw_Shutdown(); 26 | ImGui::DestroyContext(); 27 | 28 | glfwDestroyWindow(window); 29 | glfwTerminate(); 30 | } 31 | 32 | // dirty implementation 33 | std::unordered_map appmap; 34 | void fb_size_callback(GLFWwindow* window, int width, int height) { 35 | appmap[window]->framebuffer_size_callback(Eigen::Vector2i(width, height)); 36 | } 37 | 38 | bool Application::init(const char* window_name, const Eigen::Vector2i& size, const char* glsl_version) { 39 | glfwSetErrorCallback([](int err, const char* desc) { std::cerr << "glfw error " << err << ": " << desc << std::endl; }); 40 | if(!glfwInit()) { 41 | std::cerr << "failed to initialize GLFW" << std::endl; 42 | return false; 43 | } 44 | 45 | glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); 46 | glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0); 47 | 48 | window = glfwCreateWindow(size[0], size[1], window_name, nullptr, nullptr); 49 | if(window == nullptr) { 50 | return false; 51 | } 52 | appmap[window] = this; 53 | 54 | glfwSetFramebufferSizeCallback(window, fb_size_callback); 55 | 56 | glfwMakeContextCurrent(window); 57 | glfwSwapInterval(1); 58 | 59 | if(gl3wInit()) { 60 | std::cerr << "failed to initialize GL3W" << std::endl; 61 | return false; 62 | } 63 | 64 | // setup imgui 65 | IMGUI_CHECKVERSION(); 66 | ImGui::CreateContext(); 67 | 68 | ImGui::StyleColorsDark(); 69 | 70 | ImGui_ImplGlfw_InitForOpenGL(window, true); 71 | ImGui_ImplOpenGL3_Init(glsl_version); 72 | 73 | return true; 74 | } 75 | 76 | void Application::run() { 77 | while(!glfwWindowShouldClose(window)) { 78 | std::chrono::system_clock::time_point draw_start_time = std::chrono::system_clock::now(); 79 | glfwPollEvents(); 80 | ImGui_ImplOpenGL3_NewFrame(); 81 | ImGui_ImplGlfw_NewFrame(); 82 | ImGui::NewFrame(); 83 | 84 | draw_ui(); 85 | 86 | ImGui::Render(); 87 | 88 | int display_w, display_h; 89 | glfwGetFramebufferSize(window, &display_w, &display_h); 90 | glViewport(0, 0, display_w, display_h); 91 | glClearColor(0.6f, 0.6f, 0.6f, 1.0f); 92 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 93 | 94 | draw_gl(); 95 | 96 | ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData()); 97 | glfwSwapBuffers(window); 98 | 99 | std::chrono::duration draw_time_ms = std::chrono::system_clock::now() - draw_start_time; 100 | if (draw_time_ms.count() < 1000/max_frame_rate) { 101 | std::chrono::duration delta_ms(1000/max_frame_rate - draw_time_ms.count()); 102 | auto delta_ms_duration = std::chrono::duration_cast(delta_ms); 103 | std::this_thread::sleep_for(std::chrono::milliseconds(delta_ms_duration.count())); 104 | } 105 | } 106 | } 107 | 108 | void Application::close() { 109 | glfwSetWindowShouldClose(window, 1); 110 | } 111 | 112 | Eigen::Vector2i Application::framebuffer_size() { 113 | int width, height; 114 | glfwGetFramebufferSize(window, &width, &height); 115 | return Eigen::Vector2i(width, height); 116 | } 117 | 118 | void Application::framebuffer_size_callback(const Eigen::Vector2i& size) { 119 | std::cout << "FB:" << size.transpose() << std::endl; 120 | } 121 | 122 | void Application::draw_ui() { 123 | ImGui::ShowDemoWindow(); 124 | } 125 | 126 | void Application::draw_gl() {} 127 | 128 | } // namespace guik 129 | -------------------------------------------------------------------------------- /src/guik/model_control.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace guik { 9 | 10 | ModelControl::ModelControl(const std::string& name) : name(name), pose(Eigen::Isometry3f::Identity()) {} 11 | 12 | void ModelControl::draw_ui() { 13 | ImGui::Begin(name.c_str(), nullptr, ImGuiWindowFlags_AlwaysAutoResize); 14 | 15 | float value = 0.0f; 16 | ImGui::SetNextItemWidth(50); 17 | if (ImGui::DragFloat("##PX", &value, 0.01f, 0.0f, 0.0f, "PX")) { 18 | pose = pose * Eigen::Translation3f(Eigen::Vector3f::UnitX() * value); 19 | } 20 | 21 | value = 0.0f; 22 | ImGui::SameLine(); 23 | ImGui::SetNextItemWidth(50); 24 | if (ImGui::DragFloat("##PY", &value, 0.01f, 0.0f, 0.0f, "PY")) { 25 | pose = pose * Eigen::Translation3f(Eigen::Vector3f::UnitY() * value); 26 | } 27 | 28 | value = 0.0f; 29 | ImGui::SameLine(); 30 | ImGui::SetNextItemWidth(50); 31 | if (ImGui::DragFloat("##PZ", &value, 0.01f, 0.0f, 0.0f, "PZ")) { 32 | pose = pose * Eigen::Translation3f(Eigen::Vector3f::UnitZ() * value); 33 | } 34 | 35 | value = 0.0f; 36 | ImGui::SetNextItemWidth(50); 37 | if (ImGui::DragFloat("##RX", &value, 0.01f, 0.0f, 0.0f, "RX")) { 38 | pose = pose * Eigen::AngleAxisf(value, Eigen::Vector3f::UnitX()); 39 | } 40 | 41 | value = 0.0f; 42 | ImGui::SameLine(); 43 | ImGui::SetNextItemWidth(50); 44 | if (ImGui::DragFloat("##RY", &value, 0.01f, 0.0f, 0.0f, "RY")) { 45 | pose = pose * Eigen::AngleAxisf(value, Eigen::Vector3f::UnitY()); 46 | } 47 | 48 | value = 0.0f; 49 | ImGui::SameLine(); 50 | ImGui::SetNextItemWidth(50); 51 | if (ImGui::DragFloat("##RZ", &value, 0.01f, 0.0f, 0.0f, "RZ")) { 52 | pose = pose * Eigen::AngleAxisf(value, Eigen::Vector3f::UnitZ()); 53 | } 54 | 55 | value = 0.0f; 56 | ImGui::SetNextItemWidth(100); 57 | if (ImGui::DragFloat("##SCALE", &value, 0.01f, 0.0f, 0.0f, "SCALE")) { 58 | pose = pose * Eigen::UniformScaling(1.0f + value); 59 | } 60 | 61 | std::stringstream sst; 62 | sst << "trans:" << pose.translation().transpose() << "\n" 63 | << "quat :" << Eigen::Quaternionf(pose.rotation()).coeffs().transpose() << "\n" 64 | << "scale:" << pose.linear().colwise().norm() << std::endl; 65 | ImGui::Text(sst.str().c_str()); 66 | 67 | ImGui::End(); 68 | } 69 | 70 | Eigen::Matrix4f ModelControl::model_matrix() const { return pose.matrix(); } 71 | 72 | } // namespace guik 73 | -------------------------------------------------------------------------------- /src/guik/projection_control.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace guik { 10 | 11 | ProjectionControl::ProjectionControl(const Eigen::Vector2i& size) 12 | : show_window(false), 13 | size(size), 14 | projection_mode(0), 15 | fovy(30.0f), 16 | width(10.0f), 17 | near(1.0f), 18 | far(1000.0f) 19 | {} 20 | 21 | ProjectionControl::~ProjectionControl() {} 22 | 23 | Eigen::Matrix4f ProjectionControl::projection_matrix() const { 24 | double aspect_ratio = size[0] / static_cast(size[1]); 25 | 26 | glm::mat4 proj; 27 | if(projection_mode == 0) { 28 | proj = glm::perspective(fovy * M_PI / 180.0, aspect_ratio, near, far); 29 | } else { 30 | proj = glm::ortho(-width / 2.0f, width / 2.0f, -width / 2.0f / aspect_ratio, width / 2.0 / aspect_ratio, near, far); 31 | } 32 | 33 | return Eigen::Map(glm::value_ptr(proj)); 34 | } 35 | 36 | void ProjectionControl::show() { 37 | show_window = true; 38 | } 39 | 40 | void ProjectionControl::draw_ui() { 41 | if(!show_window) { 42 | return; 43 | } 44 | 45 | ImGui::Begin("projection control", &show_window, ImGuiWindowFlags_AlwaysAutoResize); 46 | const char* modes[] = { "PERSPECTIVE", "ORTHOGONAL" }; 47 | ImGui::Combo("Mode", &projection_mode, modes, IM_ARRAYSIZE(modes)); 48 | if(projection_mode == 0){ 49 | ImGui::DragFloat("FOV", &fovy, 0.1f, 1.0f, 180.0f); 50 | } else { 51 | ImGui::DragFloat("Width", &width, 0.1f, 1.0f, 1000.0f); 52 | } 53 | 54 | ImGui::DragFloat("Near", &near, 1.0f, 0.1f, far); 55 | ImGui::DragFloat("Far", &far, 1.0f, near, 10000.0f); 56 | 57 | ImGui::End(); 58 | } 59 | } -------------------------------------------------------------------------------- /src/hdl_graph_slam/automatic_loop_close_window.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace hdl_graph_slam { 11 | 12 | AutomaticLoopCloseWindow::AutomaticLoopCloseWindow(std::shared_ptr& graph) 13 | : show_window(false), graph(graph), running(false), loop_detection_source(0), fitness_score_thresh(0.3f), fitness_score_max_range(2.0f), search_method(1), distance_thresh(10.0f), accum_distance_thresh(15.0f), optimize(true) {} 14 | 15 | AutomaticLoopCloseWindow::~AutomaticLoopCloseWindow() { 16 | if (running) { 17 | running = false; 18 | } 19 | 20 | if (loop_detection_thread.joinable()) { 21 | loop_detection_thread.join(); 22 | } 23 | } 24 | 25 | void AutomaticLoopCloseWindow::draw_ui() { 26 | if (!show_window) { 27 | running = false; 28 | return; 29 | } 30 | 31 | ImGui::Begin("automatic loop close", &show_window, ImGuiWindowFlags_AlwaysAutoResize); 32 | 33 | registration_method.draw_ui(); 34 | ImGui::DragFloat("Fitness score thresh", &fitness_score_thresh, 0.01f, 0.01f, 10.0f); 35 | 36 | ImGui::Text("Loop detection"); 37 | const char* search_methods[] = {"RANDOM", "SEQUENTIAL"}; 38 | ImGui::Combo("Search method", &search_method, search_methods, IM_ARRAYSIZE(search_methods)); 39 | ImGui::DragFloat("Distance thresh", &distance_thresh, 0.5f, 0.5f, 100.0f); 40 | ImGui::DragFloat("Accum distance thresh", &accum_distance_thresh, 0.5f, 0.5f, 100.0f); 41 | 42 | robust_kernel.draw_ui(); 43 | 44 | ImGui::Checkbox("Optimization", &optimize); 45 | 46 | if (ImGui::Button("Start")) { 47 | if (!running) { 48 | running = true; 49 | loop_detection_thread = std::thread([&]() { loop_detection(); }); 50 | } 51 | } 52 | 53 | ImGui::SameLine(); 54 | if (ImGui::Button("Stop")) { 55 | if (running) { 56 | running = false; 57 | loop_detection_thread.join(); 58 | } 59 | } 60 | 61 | if (running) { 62 | ImGui::SameLine(); 63 | ImGui::Text("%c Running", "|/-\\"[(int)(ImGui::GetTime() / 0.05f) & 3]); 64 | } 65 | 66 | ImGui::End(); 67 | } 68 | 69 | void AutomaticLoopCloseWindow::loop_detection() { 70 | pcl::Registration::Ptr registration = registration_method.method(); 71 | 72 | while (running) { 73 | KeyFrameView::Ptr source = graph->keyframes_view[loop_detection_source]; 74 | Eigen::Isometry3d source_pose = source->lock()->node->estimate(); 75 | auto candidates = find_loop_candidates(source); 76 | 77 | { 78 | std::lock_guard lock(loop_detection_mutex); 79 | loop_source = source; 80 | loop_candidates = candidates; 81 | } 82 | 83 | bool edge_inserted = false; 84 | registration->setInputTarget(source->lock()->cloud); 85 | for (int i = 0; i < candidates.size(); i++) { 86 | registration->setInputSource(candidates[i]->lock()->cloud); 87 | 88 | pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 89 | Eigen::Isometry3d relative = source_pose.inverse() * candidates[i]->lock()->node->estimate(); 90 | registration->align(*aligned, relative.matrix().cast()); 91 | 92 | relative.matrix() = registration->getFinalTransformation().cast(); 93 | double fitness_score = InformationMatrixCalculator::calc_fitness_score(source->lock()->cloud, candidates[i]->lock()->cloud, relative, fitness_score_max_range); 94 | 95 | if (fitness_score < fitness_score_thresh) { 96 | edge_inserted = true; 97 | auto edge = graph->add_edge(source->lock(), candidates[i]->lock(), relative, robust_kernel.type(), robust_kernel.delta()); 98 | } 99 | } 100 | 101 | if (edge_inserted && optimize) { 102 | std::lock_guard lock(graph->optimization_mutex); 103 | graph->optimize(); 104 | } 105 | 106 | loop_detection_source++; 107 | if (search_method == 0) { 108 | loop_detection_source = rand() % graph->keyframes_view.size(); 109 | } 110 | 111 | loop_detection_source = loop_detection_source % graph->keyframes_view.size(); 112 | } 113 | } 114 | 115 | std::vector AutomaticLoopCloseWindow::find_loop_candidates(const KeyFrameView::Ptr& keyframe) { 116 | std::unordered_map accum_distances; 117 | accum_distances[keyframe->lock()->id()] = 0.0f; 118 | 119 | std::deque search_queue = {keyframe}; 120 | 121 | while (!search_queue.empty()) { 122 | auto target = search_queue.front()->lock(); 123 | float target_accum_distance = accum_distances[target->id()]; 124 | Eigen::Vector3d target_pos = target->estimate().translation(); 125 | search_queue.pop_front(); 126 | 127 | for (auto& edge_ : target->node->edges()) { 128 | g2o::EdgeSE3* edge = dynamic_cast(edge_); 129 | if (edge == nullptr) { 130 | continue; 131 | } 132 | 133 | g2o::VertexSE3* v1 = dynamic_cast(edge->vertices()[0]); 134 | g2o::VertexSE3* v2 = dynamic_cast(edge->vertices()[1]); 135 | 136 | g2o::VertexSE3* next = (v1->id() == target->node->id()) ? v2 : v1; 137 | if (graph->keyframes.find(next->id()) == graph->keyframes.end()) { 138 | continue; 139 | } 140 | 141 | float delta = (next->estimate().translation() - target_pos).norm(); 142 | float accum_distance = target_accum_distance + delta; 143 | 144 | auto found = accum_distances.find(next->id()); 145 | if (found == accum_distances.end() || found->second > accum_distance) { 146 | accum_distances[next->id()] = accum_distance; 147 | search_queue.push_back(graph->keyframes_view_map[graph->keyframes[next->id()]]); 148 | } 149 | } 150 | } 151 | 152 | std::unordered_set excluded_edges; 153 | for (auto& edge_ : keyframe->lock()->node->edges()) { 154 | g2o::EdgeSE3* edge = dynamic_cast(edge_); 155 | if (edge == nullptr) { 156 | continue; 157 | } 158 | 159 | excluded_edges.insert(edge->vertices()[0]->id()); 160 | excluded_edges.insert(edge->vertices()[1]->id()); 161 | } 162 | 163 | Eigen::Vector3d keyframe_pos = keyframe->lock()->node->estimate().translation(); 164 | std::vector loop_candidates; 165 | for (const auto& candidate : graph->keyframes_view) { 166 | if (excluded_edges.find(candidate->lock()->id()) != excluded_edges.end()) { 167 | continue; 168 | } 169 | 170 | double dist = (candidate->lock()->node->estimate().translation() - keyframe_pos).norm(); 171 | 172 | auto found = accum_distances.find(candidate->lock()->id()); 173 | if (found == accum_distances.end()) { 174 | if(dist < distance_thresh) { 175 | loop_candidates.push_back(candidate); 176 | } 177 | continue; 178 | } 179 | 180 | double accum_dist = found->second; 181 | if (accum_dist > accum_distance_thresh && dist < distance_thresh) { 182 | loop_candidates.push_back(candidate); 183 | } 184 | } 185 | 186 | return loop_candidates; 187 | } 188 | 189 | void AutomaticLoopCloseWindow::draw_gl(glk::GLSLShader& shader) { 190 | if (!running) { 191 | return; 192 | } 193 | 194 | std::lock_guard lock(loop_detection_mutex); 195 | if (loop_source == nullptr) { 196 | return; 197 | } 198 | 199 | shader.set_uniform("color_mode", 1); 200 | shader.set_uniform("point_scale", 2.0f); 201 | 202 | DrawFlags draw_flags; 203 | loop_source->draw(draw_flags, shader, Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f), loop_source->lock()->node->estimate().matrix().cast()); 204 | 205 | for (const auto& candidate : loop_candidates) { 206 | candidate->draw(draw_flags, shader, Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f), candidate->lock()->node->estimate().matrix().cast()); 207 | } 208 | } 209 | 210 | void AutomaticLoopCloseWindow::show() { show_window = true; } 211 | 212 | void AutomaticLoopCloseWindow::close() { 213 | loop_source = nullptr; 214 | loop_candidates.clear(); 215 | 216 | if(running) { 217 | running = false; 218 | loop_detection_thread.join(); 219 | } 220 | show_window = false; 221 | } 222 | 223 | } // namespace hdl_graph_slam -------------------------------------------------------------------------------- /src/hdl_graph_slam/edge_refinement_window.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace hdl_graph_slam { 10 | 11 | EdgeInfo::EdgeInfo(g2o::EdgeSE3* edge) : edge(edge), begin(edge->vertices()[0]->id()), end(edge->vertices()[1]->id()), num_evaluations(0) {} 12 | 13 | bool EdgeInfo::operator<(const EdgeInfo& rhs) const { 14 | return error() < rhs.error(); 15 | } 16 | 17 | bool EdgeInfo::operator>(const EdgeInfo& rhs) const { 18 | return error() > rhs.error(); 19 | } 20 | 21 | double EdgeInfo::error() const { 22 | return (edge->information() * edge->error()).array().abs().sum(); 23 | } 24 | 25 | EdgeRefinementWindow::EdgeRefinementWindow(std::shared_ptr& graph) : show_window(false), graph(graph), running(false), inspected_edge(nullptr), optimization_cycle(10), optimization_count(0) {} 26 | 27 | EdgeRefinementWindow::~EdgeRefinementWindow() { 28 | running = false; 29 | if(refinement_thread.joinable()) { 30 | refinement_thread.join(); 31 | } 32 | } 33 | 34 | void EdgeRefinementWindow::draw_ui() { 35 | if(!show_window) { 36 | running = false; 37 | return; 38 | } 39 | 40 | ImGui::Begin("edge refinement", &show_window, ImGuiWindowFlags_AlwaysAutoResize); 41 | 42 | registration_method.draw_ui(); 43 | robust_kernel.draw_ui(); 44 | 45 | if(ImGui::Button("Apply to all SE3 edges")) { 46 | apply_robust_kernel(); 47 | } 48 | 49 | std::stringstream format; 50 | format << (optimization_count % optimization_cycle) << "/%d"; 51 | ImGui::DragInt("Optimization cycle", &optimization_cycle, 1, 1, 1024, format.str().c_str()); 52 | 53 | if(ImGui::Button("Start")) { 54 | if(!running) { 55 | running = true; 56 | refinement_thread = std::thread([this]() { refinement_task(); }); 57 | } 58 | } 59 | 60 | ImGui::SameLine(); 61 | if(ImGui::Button("Stop")) { 62 | if(running) { 63 | running = false; 64 | refinement_thread.join(); 65 | } 66 | } 67 | 68 | if(running) { 69 | ImGui::SameLine(); 70 | ImGui::Text("%c Running", "|/-\\"[(int)(ImGui::GetTime() / 0.05f) & 3]); 71 | } 72 | 73 | ImGui::End(); 74 | 75 | ImGui::Begin("edges", &show_window, ImGuiWindowFlags_AlwaysAutoResize); 76 | 77 | ImGui::BeginChild("edge pane", ImVec2(256, 256)); 78 | ImGui::Columns(4, "edge_columns"); 79 | ImGui::Separator(); 80 | ImGui::Text("Begin"); 81 | ImGui::NextColumn(); 82 | ImGui::Text("End"); 83 | ImGui::NextColumn(); 84 | ImGui::Text("Error"); 85 | ImGui::NextColumn(); 86 | ImGui::Text("Evaluated"); 87 | ImGui::NextColumn(); 88 | ImGui::Separator(); 89 | 90 | for(const auto& edge : edges) { 91 | ImGui::Text("%d", edge.begin); 92 | ImGui::NextColumn(); 93 | ImGui::Text("%d", edge.end); 94 | ImGui::NextColumn(); 95 | ImGui::Text("%.5f", edge.error()); 96 | ImGui::NextColumn(); 97 | ImGui::Text("%d", edge.num_evaluations); 98 | ImGui::NextColumn(); 99 | } 100 | 101 | ImGui::EndChild(); 102 | ImGui::End(); 103 | } 104 | 105 | void EdgeRefinementWindow::apply_robust_kernel() { 106 | if(running) { 107 | running = false; 108 | refinement_thread.join(); 109 | } 110 | 111 | for(const auto& edge : edges) { 112 | graph->apply_robust_kernel(edge.edge, robust_kernel.type(), robust_kernel.delta()); 113 | } 114 | graph->optimize(); 115 | } 116 | 117 | void EdgeRefinementWindow::refinement() { 118 | std::vector accumulated_error(edges.size()); 119 | accumulated_error[0] = edges[0].error(); 120 | 121 | for(int i = 1; i < edges.size(); i++) { 122 | accumulated_error[i] = accumulated_error[i - 1] + edges[i].error(); 123 | } 124 | 125 | std::uniform_real_distribution<> udist(0.0, accumulated_error.back()); 126 | double roulette = udist(mt); 127 | 128 | auto loc = std::upper_bound(accumulated_error.begin(), accumulated_error.end(), roulette); 129 | size_t index = std::distance(accumulated_error.begin(), loc); 130 | 131 | auto& edge = edges[index]; 132 | 133 | auto v1 = graph->keyframes.find(edge.begin); 134 | auto v2 = graph->keyframes.find(edge.end); 135 | 136 | if(v1 == graph->keyframes.end() || v2 == graph->keyframes.end()) { 137 | return; 138 | } 139 | 140 | double fitness_score_before = InformationMatrixCalculator::calc_fitness_score(v1->second->cloud, v2->second->cloud, edge.edge->measurement(), 2.0); 141 | 142 | pcl::Registration::Ptr registration = registration_method.method(); 143 | 144 | Eigen::Isometry3d relative = v1->second->estimate().inverse() * v2->second->estimate(); 145 | 146 | registration->setInputTarget(v1->second->cloud); 147 | registration->setInputSource(v2->second->cloud); 148 | 149 | pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 150 | registration->align(*aligned, relative.matrix().cast()); 151 | 152 | relative.matrix() = registration->getFinalTransformation().cast(); 153 | double fitness_score_after = InformationMatrixCalculator::calc_fitness_score(v1->second->cloud, v2->second->cloud, relative, 2.0); 154 | 155 | if(fitness_score_after < fitness_score_before) { 156 | std::lock_guard lock(graph->optimization_mutex); 157 | edge.edge->setMeasurement(relative); 158 | graph->apply_robust_kernel(edge.edge, robust_kernel.type(), robust_kernel.delta()); 159 | 160 | if(((++optimization_count) % optimization_cycle) == 0) { 161 | graph->optimize(); 162 | } 163 | } 164 | 165 | { 166 | std::lock_guard lock(edges_mutex); 167 | std::sort(edges.begin(), edges.end(), std::greater()); 168 | edge.num_evaluations++; 169 | inspected_edge = edge.edge; 170 | } 171 | } 172 | 173 | void EdgeRefinementWindow::refinement_task() { 174 | while(running) { 175 | refinement(); 176 | } 177 | } 178 | 179 | void EdgeRefinementWindow::draw_gl(glk::GLSLShader& shader) { 180 | if(!running) { 181 | return; 182 | } 183 | 184 | g2o::HyperGraph::Edge* edge = nullptr; 185 | { 186 | std::lock_guard lock(edges_mutex); 187 | edge = inspected_edge; 188 | } 189 | 190 | auto found = graph->edges_view_map.find(edge); 191 | if(found == graph->edges_view_map.end()) { 192 | return; 193 | } 194 | 195 | const Eigen::Vector3f pt = found->second->representative_point(); 196 | const auto& cone = glk::Primitives::instance()->primitive(glk::Primitives::CONE); 197 | 198 | shader.set_uniform("color_mode", 1); 199 | shader.set_uniform("material_color", Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f)); 200 | 201 | auto trans = Eigen::Scaling(1.0f, 1.0f, 1.5f) * Eigen::Translation3f(Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX()); 202 | shader.set_uniform("model_matrix", (Eigen::Translation3f(pt + Eigen::Vector3f::UnitZ() * 0.1f) * trans).matrix()); 203 | cone.draw(shader); 204 | } 205 | 206 | void EdgeRefinementWindow::show() { 207 | show_window = true; 208 | 209 | graph->optimize(); 210 | 211 | edges.clear(); 212 | for(const auto& edge : graph->graph->edges()) { 213 | g2o::EdgeSE3* e = dynamic_cast(edge); 214 | if(e == nullptr) { 215 | continue; 216 | } 217 | 218 | edges.push_back(EdgeInfo(e)); 219 | } 220 | 221 | std::sort(edges.begin(), edges.end(), std::greater()); 222 | } 223 | 224 | void EdgeRefinementWindow::close() { 225 | if(running) { 226 | running = false; 227 | } 228 | show_window = false; 229 | edges.clear(); 230 | } 231 | 232 | } // namespace hdl_graph_slam -------------------------------------------------------------------------------- /src/hdl_graph_slam/graph_edit_window.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace hdl_graph_slam { 7 | 8 | GraphEditWindow::GraphEditWindow(std::shared_ptr& graph) : show_window(false), selected_vertex(0), graph(graph) {} 9 | GraphEditWindow::~GraphEditWindow() {} 10 | 11 | void GraphEditWindow::draw_ui() { 12 | if(!show_window) { 13 | return; 14 | } 15 | 16 | ImGui::Begin("graph edit", &show_window, ImGuiWindowFlags_AlwaysAutoResize); 17 | 18 | ImGuiTabBarFlags flags = ImGuiTabBarFlags_None; 19 | if(ImGui::BeginTabBar("tabbar", flags)) { 20 | if(ImGui::BeginTabItem("Fixed vertices")) { 21 | g2o::VertexSE3* anchor_node = dynamic_cast(graph->graph->vertex(graph->anchor_node_id())); 22 | if(anchor_node) { 23 | bool fixed = anchor_node->fixed(); 24 | if(ImGui::Checkbox("Anchor Fixed", &fixed)) { 25 | anchor_node->setFixed(fixed); 26 | } 27 | } else { 28 | ImGui::Text("warning: No anchor node created!!"); 29 | } 30 | 31 | bool fixed_vertex_exists = false; 32 | for(const auto& vertex : graph->graph->vertices()) { 33 | auto v = dynamic_cast(vertex.second); 34 | assert(v != nullptr); 35 | 36 | if(!v->fixed() || v->id() == graph->anchor_node_id()) { 37 | continue; 38 | } 39 | 40 | std::stringstream sst; 41 | sst << "Vertex " << v->id(); 42 | 43 | if(ImGui::Button(("Unfix##" + std::to_string(v->id())).c_str())) { 44 | v->setFixed(false); 45 | } 46 | ImGui::SameLine(); 47 | ImGui::Text("Vertex %d", v->id()); 48 | fixed_vertex_exists = true; 49 | } 50 | 51 | if(!fixed_vertex_exists) { 52 | ImGui::Text("No fixed vertices!!"); 53 | } 54 | 55 | ImGui::EndTabItem(); 56 | } 57 | 58 | if(ImGui::BeginTabItem("Some fancy function")) { 59 | for(const auto& edge : graph->graph->edges()) { 60 | auto e = dynamic_cast(edge); 61 | assert(e != nullptr); 62 | } 63 | 64 | ImGui::Text("hello"); 65 | ImGui::EndTabItem(); 66 | } 67 | 68 | ImGui::EndTabBar(); 69 | } 70 | 71 | ImGui::End(); 72 | } 73 | 74 | } // namespace hdl_graph_slam -------------------------------------------------------------------------------- /src/hdl_graph_slam/interactive_keyframe.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace hdl_graph_slam { 11 | 12 | InteractiveKeyFrame::InteractiveKeyFrame(const std::string& directory, g2o::HyperGraph* graph) 13 | : KeyFrame(directory, graph) 14 | { 15 | pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree()); 16 | kdtree->setInputCloud(cloud); 17 | kdtree_ = kdtree; 18 | } 19 | 20 | InteractiveKeyFrame::~InteractiveKeyFrame() { 21 | 22 | } 23 | 24 | std::vector InteractiveKeyFrame::neighbors(const Eigen::Vector3f& pt, double radius) { 25 | pcl::search::KdTree::Ptr kdtree = boost::any_cast::Ptr>(kdtree_); 26 | 27 | std::vector indices; 28 | std::vector squared_distances; 29 | 30 | pcl::PointXYZI p; 31 | p.getVector4fMap() = node->estimate().inverse().cast() * Eigen::Vector4f(pt[0], pt[1], pt[2], 1.0f); 32 | kdtree->radiusSearch(p, radius, indices, squared_distances); 33 | 34 | return indices; 35 | } 36 | 37 | pcl::PointCloud::Ptr InteractiveKeyFrame::normals() { 38 | if (!normals_) { 39 | normals_.reset(new pcl::PointCloud()); 40 | 41 | pcl::NormalEstimation ne; 42 | 43 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 44 | ne.setInputCloud(cloud); 45 | ne.setSearchMethod(tree); 46 | ne.setRadiusSearch(0.25f); 47 | 48 | ne.compute(*normals_); 49 | } 50 | 51 | return normals_; 52 | } 53 | 54 | } -------------------------------------------------------------------------------- /src/hdl_graph_slam/plane_alignment_modal.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace hdl_graph_slam { 6 | 7 | PlaneAlignmentModal::PlaneAlignmentModal(std::shared_ptr& graph) : graph(graph), plane_mode(1), information_scale(100.0f), robust_kernel(0, 0.01) {} 8 | PlaneAlignmentModal::~PlaneAlignmentModal() {} 9 | 10 | bool PlaneAlignmentModal::set_begin_plane(int plane_id) { 11 | auto found = graph->graph->vertices().find(plane_id); 12 | if (found == graph->graph->vertices().end()) { 13 | std::cerr << "vertex id " << plane_id << " not found" << std::endl; 14 | return false; 15 | } 16 | 17 | g2o::VertexPlane* vertex = dynamic_cast(found->second); 18 | if (vertex == nullptr) { 19 | std::cerr << "vertex id " << plane_id << " is not plane" << std::endl; 20 | return false; 21 | } 22 | 23 | plane_begin.reset(new VertexPlaneView(vertex)); 24 | return true; 25 | } 26 | 27 | bool PlaneAlignmentModal::set_end_plane(int plane_id) { 28 | auto found = graph->graph->vertices().find(plane_id); 29 | if (found == graph->graph->vertices().end()) { 30 | std::cerr << "vertex id " << plane_id << " not found" << std::endl; 31 | return false; 32 | } 33 | 34 | g2o::VertexPlane* vertex = dynamic_cast(found->second); 35 | if (vertex == nullptr) { 36 | std::cerr << "vertex id " << plane_id << " is not plane" << std::endl; 37 | return false; 38 | } 39 | 40 | plane_end.reset(new VertexPlaneView(vertex)); 41 | return true; 42 | } 43 | 44 | bool PlaneAlignmentModal::has_begin_plane() { 45 | return plane_begin != nullptr; 46 | } 47 | 48 | void PlaneAlignmentModal::close() { 49 | plane_begin = nullptr; 50 | plane_end = nullptr; 51 | } 52 | 53 | bool PlaneAlignmentModal::run() { 54 | bool close_window = false; 55 | if (ImGui::BeginPopupModal("plane alignment", nullptr, ImGuiWindowFlags_AlwaysAutoResize)) { 56 | ImGui::Text("Plane"); 57 | const char* plane_modes[] = {"IDENTITY", "PARALLEL", "PERPENDICULAR"}; 58 | ImGui::Combo("Plane mode", &plane_mode, plane_modes, IM_ARRAYSIZE(plane_modes)); 59 | ImGui::DragFloat("Information scale", &information_scale, 1.0f, 0.01f, 10000.0f, "%.3f", 1.0f); 60 | 61 | robust_kernel.draw_ui(); 62 | 63 | if (ImGui::Button("Add Edge")) { 64 | switch (plane_mode) { 65 | case 0: 66 | graph->add_edge_identity(plane_begin->vertex_plane, plane_end->vertex_plane, information_scale, robust_kernel.type(), robust_kernel.delta()); 67 | break; 68 | case 1: 69 | graph->add_edge_parallel(plane_begin->vertex_plane, plane_end->vertex_plane, information_scale, robust_kernel.type(), robust_kernel.delta()); 70 | break; 71 | case 2: 72 | graph->add_edge_perpendicular(plane_begin->vertex_plane, plane_end->vertex_plane, information_scale, robust_kernel.type(), robust_kernel.delta()); 73 | break; 74 | } 75 | graph->optimize(); 76 | close_window = true; 77 | } 78 | 79 | if (ImGui::Button("Cancel")) { 80 | close_window = true; 81 | } 82 | 83 | if (close_window) { 84 | ImGui::CloseCurrentPopup(); 85 | plane_begin.reset(); 86 | plane_end.reset(); 87 | } 88 | 89 | ImGui::EndPopup(); 90 | } 91 | return close_window; 92 | } 93 | 94 | void PlaneAlignmentModal::draw_gl(glk::GLSLShader& shader) { 95 | DrawFlags flags; 96 | 97 | Eigen::Matrix4f scale = (Eigen::Scaling(7.5f, 7.5f, 0.15f) * Eigen::Isometry3f::Identity()).matrix(); 98 | if (plane_begin) { 99 | plane_begin->draw(flags, shader, Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f), scale); 100 | } 101 | if (plane_end) { 102 | plane_end->draw(flags, shader, Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f), scale); 103 | } 104 | } 105 | } // namespace hdl_graph_slam -------------------------------------------------------------------------------- /src/hdl_graph_slam/plane_detection_window.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace hdl_graph_slam { 21 | 22 | PlaneDetectionWindow::PlaneDetectionWindow(std::shared_ptr& graph) 23 | : show_window(false), 24 | graph(graph), 25 | center_point(0.0f, 0.0f, 0.0f), 26 | initial_neighbor_search_radius(1.0f), 27 | normal_estimation_radius(1.0f), 28 | min_cluster_size(50), 29 | max_cluster_size(10000), 30 | num_neighbors(10), 31 | smoothness_threshold(10.0f), 32 | curvature_threshold(0.5f), 33 | enable_normal_filtering(true), 34 | normal_threshold(45.0f), 35 | ransac_distance_thresh(0.15f), 36 | min_plane_supports(100), 37 | robust_kernel(1), 38 | robust_kernel_delta(0.1f), 39 | region_growing_progress_modal("region growing progress") {} 40 | 41 | PlaneDetectionWindow::~PlaneDetectionWindow() {} 42 | 43 | void PlaneDetectionWindow::set_center_point(const Eigen::Vector3f& point) { 44 | region_growing_result = nullptr; 45 | plane_detection_result = nullptr; 46 | center_point = point; 47 | } 48 | 49 | void PlaneDetectionWindow::show() { show_window = true; } 50 | 51 | void PlaneDetectionWindow::close() { 52 | show_window = false; 53 | region_growing_result = nullptr; 54 | plane_detection_result = nullptr; 55 | } 56 | 57 | RegionGrowingResult::Ptr PlaneDetectionWindow::region_growing(guik::ProgressInterface& progress) { 58 | RegionGrowingResult::Ptr result(new RegionGrowingResult()); 59 | pcl::PointCloud::Ptr accumulated_points(new pcl::PointCloud()); 60 | 61 | progress.set_maximum(5); 62 | progress.set_text("accumulating points"); 63 | progress.increment(); 64 | for (const auto& keyframe : graph->keyframes) { 65 | std::vector neighbor_indices = keyframe.second->neighbors(center_point, initial_neighbor_search_radius); 66 | if (neighbor_indices.size() < 10) { 67 | continue; 68 | } 69 | 70 | result->candidates.push_back(keyframe.second); 71 | 72 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 73 | pcl::transformPointCloud(*keyframe.second->cloud, *cloud, keyframe.second->node->estimate().cast()); 74 | 75 | if (accumulated_points->empty()) { 76 | pcl::PointNormal pt; 77 | accumulated_points->push_back(cloud->at(neighbor_indices[0])); 78 | } 79 | 80 | for (int i = 0; i < cloud->size(); i++) { 81 | accumulated_points->push_back(cloud->at(i)); 82 | } 83 | } 84 | 85 | if(accumulated_points->size() < 50) { 86 | std::cerr << "too few points for region growing" << std::endl; 87 | return nullptr; 88 | } 89 | 90 | progress.set_text("normal estimation"); 91 | progress.increment(); 92 | pcl::PointCloud::Ptr accumulated_normals(new pcl::PointCloud()); 93 | pcl::NormalEstimationOMP ne; 94 | 95 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); 96 | ne.setInputCloud(accumulated_points); 97 | ne.setSearchMethod(tree); 98 | ne.setRadiusSearch(normal_estimation_radius); 99 | 100 | ne.compute(*accumulated_normals); 101 | 102 | progress.set_text("region growing"); 103 | progress.increment(); 104 | pcl::RegionGrowing reg; 105 | reg.setMinClusterSize(min_cluster_size); 106 | reg.setMaxClusterSize(max_cluster_size); 107 | 108 | pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree()); 109 | kdtree->setInputCloud(accumulated_points); 110 | reg.setSearchMethod(kdtree); 111 | 112 | reg.setNumberOfNeighbours(num_neighbors); 113 | reg.setInputCloud(accumulated_points); 114 | reg.setInputNormals(accumulated_normals); 115 | reg.setSmoothnessThreshold(smoothness_threshold / 180.0f * M_PI); 116 | reg.setCurvatureThreshold(curvature_threshold); 117 | 118 | pcl::PointIndices::Ptr cluster(new pcl::PointIndices()); 119 | reg.getSegmentFromPoint(0, *cluster); 120 | 121 | pcl::PointCloud::Ptr extracted_cloud(new pcl::PointCloud()); 122 | pcl::PointCloud::Ptr extracted_normals(new pcl::PointCloud()); 123 | 124 | progress.set_text("extract indices"); 125 | progress.increment(); 126 | pcl::ExtractIndices extract; 127 | extract.setInputCloud(accumulated_points); 128 | extract.setIndices(cluster); 129 | extract.setNegative(false); 130 | extract.filter(*extracted_cloud); 131 | 132 | pcl::ExtractIndices extract_normals; 133 | extract_normals.setInputCloud(accumulated_normals); 134 | extract_normals.setIndices(cluster); 135 | extract_normals.setNegative(false); 136 | extract_normals.filter(*extracted_normals); 137 | 138 | if(enable_normal_filtering) { 139 | progress.set_text("normal filtering"); 140 | result->cloud.reset(new pcl::PointCloud()); 141 | result->normals.reset(new pcl::PointCloud()); 142 | 143 | for(int i = 0; i < extracted_cloud->size(); i++) { 144 | double angle = std::acos(extracted_normals->at(i).getNormalVector3fMap().dot(extracted_normals->at(0).getNormalVector3fMap())); 145 | if(angle > normal_threshold * M_PI / 180.0) { 146 | continue; 147 | } 148 | 149 | result->cloud->push_back(extracted_cloud->at(i)); 150 | result->normals->push_back(extracted_normals->at(i)); 151 | } 152 | 153 | result->cloud->width = result->cloud->size(); 154 | result->cloud->height = 1; 155 | result->normals->width = result->normals->size(); 156 | result->normals->height = 1; 157 | } else { 158 | result->cloud = extracted_cloud; 159 | result->normals = extracted_normals; 160 | } 161 | 162 | progress.set_text("done"); 163 | progress.increment(); 164 | 165 | // result->plane_cloud = accumulated_points; 166 | return result; 167 | } 168 | 169 | PlaneDetectionResult::Ptr PlaneDetectionWindow::detect_plane(const RegionGrowingResult::Ptr& rg_result) { 170 | pcl::SampleConsensusModelPlane::Ptr model_p(new pcl::SampleConsensusModelPlane(rg_result->cloud)); 171 | pcl::RandomSampleConsensus ransac(model_p); 172 | ransac.setDistanceThreshold(ransac_distance_thresh); 173 | ransac.computeModel(); 174 | 175 | pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); 176 | ransac.getInliers(inliers->indices); 177 | 178 | if(std::find(inliers->indices.begin(), inliers->indices.end(), 0) == inliers->indices.end()) { 179 | std::cerr << "center_point not found in inliers!!" << std::endl; 180 | } 181 | 182 | PlaneDetectionResult::Ptr result(new PlaneDetectionResult()); 183 | ransac.getModelCoefficients(result->coeffs); 184 | 185 | for (const auto& candidate : rg_result->candidates) { 186 | Eigen::Vector4f coeffs = result->coeffs; 187 | Eigen::Vector4f local_coeffs; 188 | 189 | Eigen::Isometry3f trans = candidate->node->estimate().inverse().cast(); 190 | local_coeffs.head<3>() = trans.linear() * coeffs.head<3>(); 191 | local_coeffs[3] = coeffs[3] - trans.translation().dot(local_coeffs.head<3>()); 192 | 193 | auto inliers = detect_plane_with_coeffs(candidate->cloud, local_coeffs); 194 | if (inliers == nullptr || inliers->size() < min_plane_supports) { 195 | continue; 196 | } 197 | 198 | result->candidates.push_back(candidate); 199 | result->candidate_inliers.push_back(inliers); 200 | result->candidate_inlier_buffers.push_back(std::make_shared(inliers)); 201 | result->candidate_local_coeffs.push_back(local_coeffs); 202 | } 203 | 204 | return result; 205 | } 206 | 207 | pcl::PointCloud::Ptr PlaneDetectionWindow::detect_plane_with_coeffs(const pcl::PointCloud::ConstPtr& cloud, Eigen::Vector4f& coeffs) { 208 | pcl::SampleConsensusModelPlane::Ptr model_p(new pcl::SampleConsensusModelPlane(cloud)); 209 | pcl::PointIndices::Ptr init_indices(new pcl::PointIndices); 210 | model_p->selectWithinDistance(coeffs, ransac_distance_thresh, init_indices->indices); 211 | 212 | if (init_indices->indices.size() < 10) { 213 | return nullptr; 214 | } 215 | 216 | pcl::PointCloud::Ptr init_inliers(new pcl::PointCloud()); 217 | pcl::ExtractIndices extract; 218 | extract.setInputCloud(cloud); 219 | extract.setIndices(init_indices); 220 | extract.setNegative(false); 221 | extract.filter(*init_inliers); 222 | 223 | model_p.reset(new pcl::SampleConsensusModelPlane(init_inliers)); 224 | pcl::RandomSampleConsensus ransac(model_p); 225 | ransac.setDistanceThreshold(ransac_distance_thresh); 226 | ransac.computeModel(); 227 | 228 | Eigen::VectorXf coeffs_; 229 | ransac.getModelCoefficients(coeffs_); 230 | 231 | if (coeffs_.head<3>().dot(coeffs.head<3>()) < 0.0f) { 232 | coeffs_ *= -1.0f; 233 | } 234 | coeffs = coeffs_; 235 | 236 | pcl::PointIndices::Ptr indices(new pcl::PointIndices()); 237 | ransac.getInliers(indices->indices); 238 | 239 | pcl::PointCloud::Ptr inliers(new pcl::PointCloud()); 240 | extract.setInputCloud(init_inliers); 241 | extract.setIndices(indices); 242 | extract.filter(*inliers); 243 | 244 | return inliers; 245 | } 246 | 247 | void PlaneDetectionWindow::draw_ui() { 248 | if (!show_window) { 249 | region_growing_result.reset(); 250 | return; 251 | } 252 | 253 | ImGui::Begin("plane detection", &show_window, ImGuiWindowFlags_AlwaysAutoResize); 254 | 255 | ImGui::Text("Region growing"); 256 | ImGui::DragFloat("Initial neighbor radius", &initial_neighbor_search_radius, 0.1f, 0.1f, 30.0f); 257 | ImGui::DragFloat("Normal estimation radius", &normal_estimation_radius, 0.1f, 0.1f, 5.0f); 258 | ImGui::DragInt("Min cluster size", &min_cluster_size, 1, 10, 10000); 259 | ImGui::DragInt("Max cluster size", &max_cluster_size, 100, 100, 1000000); 260 | ImGui::DragInt("Num neighbors", &num_neighbors, 1, 1, 1000); 261 | ImGui::DragFloat("Smoothness threshold[deg]", &smoothness_threshold, 0.1f, 0.1f, 100.0f); 262 | ImGui::DragFloat("Curvature threshold", &curvature_threshold, 0.1f, 0.1f, 100.0f); 263 | 264 | ImGui::Checkbox("Normal filtering", &enable_normal_filtering); 265 | ImGui::DragFloat("Normal threshold[deg]", &normal_threshold, 1.0f, 1.0f, 180.0f); 266 | 267 | if (ImGui::Button("Perform")) { 268 | region_growing_result = nullptr; 269 | plane_detection_result = nullptr; 270 | region_growing_progress_modal.open("region growing", [this](guik::ProgressInterface& progress) { return region_growing(progress); }); 271 | } 272 | if (region_growing_progress_modal.run("region growing")) { 273 | region_growing_result = region_growing_progress_modal.result(); 274 | if(region_growing_result) { 275 | region_growing_result->cloud_buffer = std::make_shared(region_growing_result->cloud); 276 | } 277 | } 278 | 279 | if (region_growing_result) { 280 | ImGui::SameLine(); 281 | ImGui::Text("points:%d", region_growing_result->cloud->size()); 282 | } 283 | 284 | ImGui::Text("RANSAC plane"); 285 | ImGui::DragFloat("Distance thresh", &ransac_distance_thresh, 0.01f, 0.01f, 10.0f); 286 | ImGui::DragInt("Min plane supports", &min_plane_supports, 10, 10, 10000); 287 | 288 | if (ImGui::Button("Detect")) { 289 | if (region_growing_result) { 290 | plane_detection_result = nullptr; 291 | plane_detection_result = detect_plane(region_growing_result); 292 | } 293 | } 294 | 295 | if (plane_detection_result) { 296 | ImGui::SameLine(); 297 | Eigen::VectorXf coeffs = plane_detection_result->coeffs; 298 | ImGui::Text("coeffs:%.3f %.3f %.3f %.3f", coeffs[0], coeffs[1], coeffs[2], coeffs[3]); 299 | } 300 | 301 | ImGui::Text("Robust kernel"); 302 | const char* kernels[] = {"NONE", "Huber"}; 303 | ImGui::Combo("Kernel type", &robust_kernel, kernels, IM_ARRAYSIZE(kernels)); 304 | ImGui::DragFloat("Kernel delta", &robust_kernel_delta, 0.01f, 0.01f, 10.0f); 305 | 306 | if (ImGui::Button("Add edge")) { 307 | if (plane_detection_result) { 308 | auto plane_vertex = graph->add_plane(plane_detection_result->coeffs.cast()); 309 | 310 | for (int i = 0; i < plane_detection_result->candidates.size(); i++) { 311 | const auto& candidate = plane_detection_result->candidates[i]; 312 | const Eigen::Vector4f& coeffs = plane_detection_result->candidate_local_coeffs[i]; 313 | 314 | Eigen::Matrix3d information = Eigen::Matrix3d::Identity(); 315 | graph->add_edge(candidate, plane_vertex, coeffs.cast(), information, kernels[robust_kernel], robust_kernel_delta); 316 | } 317 | 318 | graph->optimize(); 319 | show_window = false; 320 | } 321 | } 322 | 323 | ImGui::End(); 324 | } 325 | 326 | void PlaneDetectionWindow::draw_gl(glk::GLSLShader& shader) { 327 | if (!show_window) { 328 | region_growing_result = nullptr; 329 | plane_detection_result = nullptr; 330 | return; 331 | } 332 | 333 | shader.set_uniform("color_mode", 1); 334 | shader.set_uniform("point_scale", 4.0f); 335 | 336 | if (plane_detection_result) { 337 | shader.set_uniform("material_color", Eigen::Vector4f(0.0f, 0.5f, 1.0f, 1.0f)); 338 | shader.set_uniform("model_matrix", Eigen::Matrix4f::Identity().eval()); 339 | // plane_detection_result->cloud_buffer->draw(shader); 340 | 341 | const auto& sphere = glk::Primitives::instance()->primitive(glk::Primitives::SPHERE); 342 | for (int i = 0; i < plane_detection_result->candidates.size(); i++) { 343 | const auto& candidate = plane_detection_result->candidates[i]; 344 | Eigen::Matrix4f model_matrix = candidate->node->estimate().cast().matrix(); 345 | model_matrix.block<3, 3>(0, 0) *= 0.5f; 346 | 347 | shader.set_uniform("material_color", Eigen::Vector4f(glk::colormap_categoricalf(glk::TURBO, i, 16))); 348 | shader.set_uniform("model_matrix", model_matrix); 349 | sphere.draw(shader); 350 | 351 | shader.set_uniform("model_matrix", candidate->node->estimate().cast().matrix()); 352 | plane_detection_result->candidate_inlier_buffers[i]->draw(shader); 353 | } 354 | return; 355 | } 356 | 357 | if (region_growing_result) { 358 | shader.set_uniform("material_color", Eigen::Vector4f(1.0f, 0.5f, 0.0f, 1.0f)); 359 | shader.set_uniform("model_matrix", Eigen::Matrix4f::Identity().eval()); 360 | region_growing_result->cloud_buffer->draw(shader); 361 | 362 | const auto& sphere = glk::Primitives::instance()->primitive(glk::Primitives::SPHERE); 363 | for (const auto& candidate : region_growing_result->candidates) { 364 | Eigen::Matrix4f model_matrix = candidate->node->estimate().cast().matrix(); 365 | model_matrix.block<3, 3>(0, 0) *= 0.5f; 366 | shader.set_uniform("model_matrix", model_matrix); 367 | sphere.draw(shader); 368 | } 369 | } 370 | } 371 | 372 | } // namespace hdl_graph_slam 373 | -------------------------------------------------------------------------------- /src/hdl_graph_slam/registration_methods.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace hdl_graph_slam { 14 | 15 | RegistrationMethods::RegistrationMethods() : registration_method(1), registration_resolution(2.0f), transformation_epsilon(1e-4), max_iterations(64), registration_methods({"ICP", "GICP", "NDT", "GICP_OMP", "NDT_OMP", "VGICP"}) {} 16 | 17 | RegistrationMethods::~RegistrationMethods() {} 18 | 19 | void RegistrationMethods::draw_ui() { 20 | ImGui::Text("Scan matching"); 21 | ImGui::Combo("Method", ®istration_method, registration_methods.data(), registration_methods.size()); 22 | if(std::string(registration_methods[registration_method]).find("NDT") != std::string::npos) { 23 | ImGui::DragFloat("Resolution", ®istration_resolution, 0.1f, 0.1f, 20.0f); 24 | } 25 | ImGui::DragFloat("Transformation epsilon", &transformation_epsilon, 1e-5f, 1e-5f, 1e-2f, "%.6f"); 26 | ImGui::DragInt("Max iterations", &max_iterations, 1, 1, 256); 27 | } 28 | 29 | pcl::Registration::Ptr RegistrationMethods::method() const { 30 | pcl::Registration::Ptr registration; 31 | 32 | switch(registration_method) { 33 | case 0: { 34 | auto icp = pcl::IterativeClosestPoint::Ptr(new pcl::IterativeClosestPoint); 35 | registration = icp; 36 | } break; 37 | 38 | default: 39 | std::cerr << "warning: unknown registration method!!" << std::endl; 40 | std::cerr << " : use GICP" << std::endl; 41 | case 1: { 42 | auto gicp = pcl::GeneralizedIterativeClosestPoint::Ptr(new pcl::GeneralizedIterativeClosestPoint); 43 | registration = gicp; 44 | } break; 45 | 46 | case 2: { 47 | auto ndt = pcl::NormalDistributionsTransform::Ptr(new pcl::NormalDistributionsTransform); 48 | ndt->setResolution(registration_resolution); 49 | registration = ndt; 50 | } break; 51 | 52 | case 3: { 53 | auto gicp = pclomp::GeneralizedIterativeClosestPoint::Ptr(new pclomp::GeneralizedIterativeClosestPoint); 54 | registration = gicp; 55 | } break; 56 | 57 | case 4: { 58 | auto ndt = pclomp::NormalDistributionsTransform::Ptr(new pclomp::NormalDistributionsTransform); 59 | ndt->setResolution(registration_resolution); 60 | registration = ndt; 61 | } break; 62 | 63 | case 5: { 64 | fast_gicp::FastVGICP::Ptr vgicp(new fast_gicp::FastVGICP()); 65 | vgicp->setNumThreads(0); 66 | vgicp->setResolution(1.0); 67 | vgicp->setCorrespondenceRandomness(20); 68 | registration = vgicp; 69 | } break; 70 | } 71 | 72 | registration->setTransformationEpsilon(transformation_epsilon); 73 | registration->setMaximumIterations(max_iterations); 74 | 75 | return registration; 76 | } 77 | } // namespace hdl_graph_slam 78 | -------------------------------------------------------------------------------- /src/hdl_graph_slam/version_modal.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace hdl_graph_slam { 5 | 6 | VersionModal::VersionModal() {} 7 | 8 | VersionModal ::~VersionModal() {} 9 | 10 | void VersionModal::open() { 11 | ImGui::OpenPopup("Interactive SLAM"); 12 | } 13 | 14 | bool VersionModal::run() { 15 | bool close_modal = false; 16 | ImGuiWindowFlags flags = ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoTitleBar; 17 | if(ImGui::BeginPopupModal("Interactive SLAM", nullptr, flags)) { 18 | ImGui::Text("Interactive SLAM"); 19 | ImGui::Text("Version: %d.%d.%d", IS_VERSION_MAJOR, IS_VERSION_MINOR, IS_VERSION_REVISION); 20 | ImGui::Text("Commit hash: %s", LAST_COMMIT_HASH); 21 | ImGui::Text("Commit date: %s", LAST_COMMIT_DATE); 22 | ImGui::Text("\nK.Koide (k.koide@aist.go.jp)"); 23 | ImGui::Text("Smart Mobility Research Team"); 24 | ImGui::Text("AIST, Japan"); 25 | 26 | if(ImGui::Button("OK")) { 27 | ImGui::CloseCurrentPopup(); 28 | close_modal = true; 29 | } 30 | ImGui::EndPopup(); 31 | } 32 | 33 | return close_modal; 34 | } 35 | } -------------------------------------------------------------------------------- /src/hdl_graph_slam/view/edge_view.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace hdl_graph_slam { 15 | 16 | EdgeView::Ptr EdgeView::create(g2o::HyperGraph::Edge* edge, LineBuffer& line_buffer) { 17 | if(dynamic_cast(edge)) { 18 | return std::make_shared(edge, line_buffer); 19 | } 20 | 21 | if(dynamic_cast(edge)) { 22 | return std::make_shared(edge, line_buffer); 23 | } 24 | 25 | if(EdgePlaneView::is_plane_edge(edge)) { 26 | return std::make_shared(edge, line_buffer); 27 | } 28 | 29 | return nullptr; 30 | } 31 | 32 | EdgeView::EdgeView(g2o::HyperGraph::Edge* edge, LineBuffer& line_buffer) : edge(edge), line_buffer(line_buffer) {} 33 | 34 | EdgeView::~EdgeView() {} 35 | 36 | long EdgeView::id() const { return edge->id(); } 37 | 38 | void EdgeView::context_menu(bool& do_delete) { 39 | g2o::Factory* factory = g2o::Factory::instance(); 40 | 41 | g2o::OptimizableGraph::Edge* e = dynamic_cast(edge); 42 | 43 | ImGui::Text("Type:%s", factory->tag(e).c_str()); 44 | 45 | std::stringstream sst; 46 | sst << "Vertices:"; 47 | for(int i=0; ivertices().size(); i++) { 48 | sst << e->vertices()[i]->id() << " "; 49 | } 50 | ImGui::Text(sst.str().c_str()); 51 | 52 | g2o::RobustKernel* robust_kernel = e->robustKernel(); 53 | if(robust_kernel == nullptr) { 54 | ImGui::Text("Robust kernel:NONE"); 55 | } else { 56 | std::string robust_kernel_type = g2o::kernel_type(robust_kernel); 57 | if(robust_kernel_type.empty()) { 58 | robust_kernel_type = "UNKNOWN"; 59 | } 60 | 61 | ImGui::Text("\nRobust kernel"); 62 | ImGui::Text("Type:%s", robust_kernel_type.c_str()); 63 | 64 | ImGui::SameLine(); 65 | float delta = robust_kernel->delta(); 66 | ImGui::PushItemWidth(128); 67 | if(ImGui::DragFloat("##Delta", &delta, 0.001f, 0.001f, 1000.0f, "Delta:%.3f")) { 68 | robust_kernel->setDelta(delta); 69 | } 70 | } 71 | 72 | do_delete = ImGui::Button("Delete Edge"); 73 | } 74 | 75 | 76 | } // namespace hdl_graph_slam -------------------------------------------------------------------------------- /src/hdl_graph_slam/view/vertex_view.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace hdl_graph_slam { 9 | 10 | VertexView::VertexView(g2o::HyperGraph::Vertex* vertex) : vertex(vertex) {} 11 | 12 | VertexView::~VertexView() {} 13 | 14 | VertexView::Ptr VertexView::create(g2o::HyperGraph::Vertex* vertex) { 15 | g2o::VertexPlane* vertex_plane = dynamic_cast(vertex); 16 | if (vertex_plane) { 17 | return std::make_shared(vertex_plane); 18 | } 19 | 20 | return nullptr; 21 | } 22 | 23 | void VertexView::context_menu() { 24 | g2o::OptimizableGraph::Vertex* v = dynamic_cast(vertex); 25 | 26 | g2o::Factory* factory = g2o::Factory::instance(); 27 | ImGui::Text("Type: %s", factory->tag(v).c_str()); 28 | ImGui::Text("Connected edges %d", v->edges().size()); 29 | 30 | bool fixed = v->fixed(); 31 | if(ImGui::Checkbox("Fixed", &fixed)) { 32 | v->setFixed(fixed); 33 | } 34 | } 35 | 36 | } // namespace hdl_graph_slam -------------------------------------------------------------------------------- /thirdparty/gl3w/KHR/khrplatform.h: -------------------------------------------------------------------------------- 1 | #ifndef __khrplatform_h_ 2 | #define __khrplatform_h_ 3 | 4 | /* 5 | ** Copyright (c) 2008-2018 The Khronos Group Inc. 6 | ** 7 | ** Permission is hereby granted, free of charge, to any person obtaining a 8 | ** copy of this software and/or associated documentation files (the 9 | ** "Materials"), to deal in the Materials without restriction, including 10 | ** without limitation the rights to use, copy, modify, merge, publish, 11 | ** distribute, sublicense, and/or sell copies of the Materials, and to 12 | ** permit persons to whom the Materials are furnished to do so, subject to 13 | ** the following conditions: 14 | ** 15 | ** The above copyright notice and this permission notice shall be included 16 | ** in all copies or substantial portions of the Materials. 17 | ** 18 | ** THE MATERIALS ARE PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 19 | ** EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 20 | ** MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 21 | ** IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 22 | ** CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, 23 | ** TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 24 | ** MATERIALS OR THE USE OR OTHER DEALINGS IN THE MATERIALS. 25 | */ 26 | 27 | /* Khronos platform-specific types and definitions. 28 | * 29 | * The master copy of khrplatform.h is maintained in the Khronos EGL 30 | * Registry repository at https://github.com/KhronosGroup/EGL-Registry 31 | * The last semantic modification to khrplatform.h was at commit ID: 32 | * 67a3e0864c2d75ea5287b9f3d2eb74a745936692 33 | * 34 | * Adopters may modify this file to suit their platform. Adopters are 35 | * encouraged to submit platform specific modifications to the Khronos 36 | * group so that they can be included in future versions of this file. 37 | * Please submit changes by filing pull requests or issues on 38 | * the EGL Registry repository linked above. 39 | * 40 | * 41 | * See the Implementer's Guidelines for information about where this file 42 | * should be located on your system and for more details of its use: 43 | * http://www.khronos.org/registry/implementers_guide.pdf 44 | * 45 | * This file should be included as 46 | * #include 47 | * by Khronos client API header files that use its types and defines. 48 | * 49 | * The types in khrplatform.h should only be used to define API-specific types. 50 | * 51 | * Types defined in khrplatform.h: 52 | * khronos_int8_t signed 8 bit 53 | * khronos_uint8_t unsigned 8 bit 54 | * khronos_int16_t signed 16 bit 55 | * khronos_uint16_t unsigned 16 bit 56 | * khronos_int32_t signed 32 bit 57 | * khronos_uint32_t unsigned 32 bit 58 | * khronos_int64_t signed 64 bit 59 | * khronos_uint64_t unsigned 64 bit 60 | * khronos_intptr_t signed same number of bits as a pointer 61 | * khronos_uintptr_t unsigned same number of bits as a pointer 62 | * khronos_ssize_t signed size 63 | * khronos_usize_t unsigned size 64 | * khronos_float_t signed 32 bit floating point 65 | * khronos_time_ns_t unsigned 64 bit time in nanoseconds 66 | * khronos_utime_nanoseconds_t unsigned time interval or absolute time in 67 | * nanoseconds 68 | * khronos_stime_nanoseconds_t signed time interval in nanoseconds 69 | * khronos_boolean_enum_t enumerated boolean type. This should 70 | * only be used as a base type when a client API's boolean type is 71 | * an enum. Client APIs which use an integer or other type for 72 | * booleans cannot use this as the base type for their boolean. 73 | * 74 | * Tokens defined in khrplatform.h: 75 | * 76 | * KHRONOS_FALSE, KHRONOS_TRUE Enumerated boolean false/true values. 77 | * 78 | * KHRONOS_SUPPORT_INT64 is 1 if 64 bit integers are supported; otherwise 0. 79 | * KHRONOS_SUPPORT_FLOAT is 1 if floats are supported; otherwise 0. 80 | * 81 | * Calling convention macros defined in this file: 82 | * KHRONOS_APICALL 83 | * KHRONOS_APIENTRY 84 | * KHRONOS_APIATTRIBUTES 85 | * 86 | * These may be used in function prototypes as: 87 | * 88 | * KHRONOS_APICALL void KHRONOS_APIENTRY funcname( 89 | * int arg1, 90 | * int arg2) KHRONOS_APIATTRIBUTES; 91 | */ 92 | 93 | #if defined(__SCITECH_SNAP__) && !defined(KHRONOS_STATIC) 94 | # define KHRONOS_STATIC 1 95 | #endif 96 | 97 | /*------------------------------------------------------------------------- 98 | * Definition of KHRONOS_APICALL 99 | *------------------------------------------------------------------------- 100 | * This precedes the return type of the function in the function prototype. 101 | */ 102 | #if defined(KHRONOS_STATIC) 103 | /* If the preprocessor constant KHRONOS_STATIC is defined, make the 104 | * header compatible with static linking. */ 105 | # define KHRONOS_APICALL 106 | #elif defined(_WIN32) 107 | # define KHRONOS_APICALL __declspec(dllimport) 108 | #elif defined (__SYMBIAN32__) 109 | # define KHRONOS_APICALL IMPORT_C 110 | #elif defined(__ANDROID__) 111 | # define KHRONOS_APICALL __attribute__((visibility("default"))) 112 | #else 113 | # define KHRONOS_APICALL 114 | #endif 115 | 116 | /*------------------------------------------------------------------------- 117 | * Definition of KHRONOS_APIENTRY 118 | *------------------------------------------------------------------------- 119 | * This follows the return type of the function and precedes the function 120 | * name in the function prototype. 121 | */ 122 | #if defined(_WIN32) && !defined(_WIN32_WCE) && !defined(KHRONOS_STATIC) 123 | /* Win32 but not WinCE */ 124 | # define KHRONOS_APIENTRY __stdcall 125 | #else 126 | # define KHRONOS_APIENTRY 127 | #endif 128 | 129 | /*------------------------------------------------------------------------- 130 | * Definition of KHRONOS_APIATTRIBUTES 131 | *------------------------------------------------------------------------- 132 | * This follows the closing parenthesis of the function prototype arguments. 133 | */ 134 | #if defined (__ARMCC_2__) 135 | #define KHRONOS_APIATTRIBUTES __softfp 136 | #else 137 | #define KHRONOS_APIATTRIBUTES 138 | #endif 139 | 140 | /*------------------------------------------------------------------------- 141 | * basic type definitions 142 | *-----------------------------------------------------------------------*/ 143 | #if (defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L) || defined(__GNUC__) || defined(__SCO__) || defined(__USLC__) 144 | 145 | 146 | /* 147 | * Using 148 | */ 149 | #include 150 | typedef int32_t khronos_int32_t; 151 | typedef uint32_t khronos_uint32_t; 152 | typedef int64_t khronos_int64_t; 153 | typedef uint64_t khronos_uint64_t; 154 | #define KHRONOS_SUPPORT_INT64 1 155 | #define KHRONOS_SUPPORT_FLOAT 1 156 | 157 | #elif defined(__VMS ) || defined(__sgi) 158 | 159 | /* 160 | * Using 161 | */ 162 | #include 163 | typedef int32_t khronos_int32_t; 164 | typedef uint32_t khronos_uint32_t; 165 | typedef int64_t khronos_int64_t; 166 | typedef uint64_t khronos_uint64_t; 167 | #define KHRONOS_SUPPORT_INT64 1 168 | #define KHRONOS_SUPPORT_FLOAT 1 169 | 170 | #elif defined(_WIN32) && !defined(__SCITECH_SNAP__) 171 | 172 | /* 173 | * Win32 174 | */ 175 | typedef __int32 khronos_int32_t; 176 | typedef unsigned __int32 khronos_uint32_t; 177 | typedef __int64 khronos_int64_t; 178 | typedef unsigned __int64 khronos_uint64_t; 179 | #define KHRONOS_SUPPORT_INT64 1 180 | #define KHRONOS_SUPPORT_FLOAT 1 181 | 182 | #elif defined(__sun__) || defined(__digital__) 183 | 184 | /* 185 | * Sun or Digital 186 | */ 187 | typedef int khronos_int32_t; 188 | typedef unsigned int khronos_uint32_t; 189 | #if defined(__arch64__) || defined(_LP64) 190 | typedef long int khronos_int64_t; 191 | typedef unsigned long int khronos_uint64_t; 192 | #else 193 | typedef long long int khronos_int64_t; 194 | typedef unsigned long long int khronos_uint64_t; 195 | #endif /* __arch64__ */ 196 | #define KHRONOS_SUPPORT_INT64 1 197 | #define KHRONOS_SUPPORT_FLOAT 1 198 | 199 | #elif 0 200 | 201 | /* 202 | * Hypothetical platform with no float or int64 support 203 | */ 204 | typedef int khronos_int32_t; 205 | typedef unsigned int khronos_uint32_t; 206 | #define KHRONOS_SUPPORT_INT64 0 207 | #define KHRONOS_SUPPORT_FLOAT 0 208 | 209 | #else 210 | 211 | /* 212 | * Generic fallback 213 | */ 214 | #include 215 | typedef int32_t khronos_int32_t; 216 | typedef uint32_t khronos_uint32_t; 217 | typedef int64_t khronos_int64_t; 218 | typedef uint64_t khronos_uint64_t; 219 | #define KHRONOS_SUPPORT_INT64 1 220 | #define KHRONOS_SUPPORT_FLOAT 1 221 | 222 | #endif 223 | 224 | 225 | /* 226 | * Types that are (so far) the same on all platforms 227 | */ 228 | typedef signed char khronos_int8_t; 229 | typedef unsigned char khronos_uint8_t; 230 | typedef signed short int khronos_int16_t; 231 | typedef unsigned short int khronos_uint16_t; 232 | 233 | /* 234 | * Types that differ between LLP64 and LP64 architectures - in LLP64, 235 | * pointers are 64 bits, but 'long' is still 32 bits. Win64 appears 236 | * to be the only LLP64 architecture in current use. 237 | */ 238 | #ifdef _WIN64 239 | typedef signed long long int khronos_intptr_t; 240 | typedef unsigned long long int khronos_uintptr_t; 241 | typedef signed long long int khronos_ssize_t; 242 | typedef unsigned long long int khronos_usize_t; 243 | #else 244 | typedef signed long int khronos_intptr_t; 245 | typedef unsigned long int khronos_uintptr_t; 246 | typedef signed long int khronos_ssize_t; 247 | typedef unsigned long int khronos_usize_t; 248 | #endif 249 | 250 | #if KHRONOS_SUPPORT_FLOAT 251 | /* 252 | * Float type 253 | */ 254 | typedef float khronos_float_t; 255 | #endif 256 | 257 | #if KHRONOS_SUPPORT_INT64 258 | /* Time types 259 | * 260 | * These types can be used to represent a time interval in nanoseconds or 261 | * an absolute Unadjusted System Time. Unadjusted System Time is the number 262 | * of nanoseconds since some arbitrary system event (e.g. since the last 263 | * time the system booted). The Unadjusted System Time is an unsigned 264 | * 64 bit value that wraps back to 0 every 584 years. Time intervals 265 | * may be either signed or unsigned. 266 | */ 267 | typedef khronos_uint64_t khronos_utime_nanoseconds_t; 268 | typedef khronos_int64_t khronos_stime_nanoseconds_t; 269 | #endif 270 | 271 | /* 272 | * Dummy value used to pad enum types to 32 bits. 273 | */ 274 | #ifndef KHRONOS_MAX_ENUM 275 | #define KHRONOS_MAX_ENUM 0x7FFFFFFF 276 | #endif 277 | 278 | /* 279 | * Enumerated boolean type 280 | * 281 | * Values other than zero should be considered to be true. Therefore 282 | * comparisons should not be made against KHRONOS_TRUE. 283 | */ 284 | typedef enum { 285 | KHRONOS_FALSE = 0, 286 | KHRONOS_TRUE = 1, 287 | KHRONOS_BOOLEAN_ENUM_FORCE_SIZE = KHRONOS_MAX_ENUM 288 | } khronos_boolean_enum_t; 289 | 290 | #endif /* __khrplatform_h_ */ 291 | --------------------------------------------------------------------------------