├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── Objects └── Camera │ └── CameraParameters.h ├── README.md ├── Utils ├── EvaluationHelper.h ├── define.h ├── eigen_glm.h ├── parser.hpp └── util.h ├── app ├── CMakeLists.txt └── GraphSLAM │ ├── CMakeLists.txt │ └── main.cc ├── cmake ├── CUDACheckCompute.cmake ├── ConfigureTimeDependency.cmake ├── External_Eigen3.CMakeLists.txt.in ├── External_Json.CMakeLists.txt.in ├── External_OpenCV.CMakeLists.txt.in ├── External_tinyply.CMakeLists.txt.in ├── FindJson.cmake ├── FindOnnxRuntime.cmake ├── Findtinyply.cmake ├── Flags.cmake ├── LinkAssimp.cmake ├── LinkEigen3.cmake ├── LinkJson.cmake ├── LinkOnnxRuntime.cmake ├── LinkOpenCV.cmake ├── Linktinyply.cmake ├── UseAssimp.cmake ├── UseCUDA.cmake ├── UseEigen3.cmake ├── UseJson.cmake ├── UseOnnxRuntime.cmake ├── UseOpenCV.cmake ├── UseOpenMP.cmake ├── Usetinyply.cmake └── cuda_compute_capability.cpp ├── img └── teaser.png ├── lib ├── inseg_lib │ ├── camera.h │ ├── disjoint_set.h │ ├── image_pyramid.h │ ├── inseg.h │ ├── inseg_config.h │ ├── lib.h │ ├── main_config.h │ ├── map.h │ ├── map_config.h │ ├── map_segments.h │ ├── segmentation.h │ ├── segmentation_config.h │ └── surfel.h ├── libInSegLib_linux.a └── libInSegLib_osx.a ├── libDataLoader ├── CMakeLists.txt ├── ScanNetLoader │ ├── CMakeLists.txt │ ├── sensorData.cpp │ ├── sensorData.h │ └── sensorData1 │ │ ├── stb_image.h │ │ └── stb_image_write.h ├── include │ └── dataLoader │ │ ├── CameraParameters.h │ │ ├── Scan3R_json_loader.h │ │ ├── dataloader_3rscan.h │ │ ├── dataloader_scannet.h │ │ ├── dataset3RScan.h │ │ ├── datasetScanNet.h │ │ ├── dataset_base.h │ │ ├── dataset_loader.h │ │ ├── dataset_loader_facotry.h │ │ ├── dataset_types.h │ │ └── util.h └── src │ ├── dataloader_3rscan.cpp │ └── dataloader_scannet.cpp ├── libGraphSLAM ├── CMakeLists.txt ├── GraphSLAM.cpp ├── GraphSLAM.h ├── config.cpp ├── config.h ├── declaration.h ├── disjSet.h ├── edge.cpp ├── edge.h ├── graph.cpp ├── graph.h ├── graphPredictor │ ├── EatGCN.cpp │ ├── EatGCN.h │ ├── GraphPredictor.cpp │ ├── GraphPredictor.h │ ├── MemoryBlock.h │ ├── OnnxModelBase.h │ ├── ParamLoader.h │ ├── UtilDataProcessing.h │ ├── UtilInSegPlyLoader.h │ └── UtilMath.h ├── node.cpp └── node.h ├── libGraphSLAMGUI ├── CMakeLists.txt ├── CameraDrawer.cpp ├── CameraDrawer.h ├── GraphSLAMGUI.cpp ├── GraphSLAMGUI.h ├── ImageDrawer.cpp ├── ImageDrawer.h ├── Label_NYU40.h ├── SurfelDrawer.cpp ├── SurfelDrawer.h ├── TrajectoryDrawer.cpp ├── TrajectoryDrawer.h └── graphDrawer │ ├── edge_drawer.cpp │ ├── edge_drawer.h │ ├── graph_drawer.cpp │ ├── graph_drawer.h │ ├── node_drawer.cpp │ ├── node_drawer.h │ ├── text_drawer.cpp │ └── text_drawer.h ├── prepare_example_data.sh ├── renderer ├── Renderer.h ├── Renderer3RScan.h ├── RendererFactory.h ├── RendererScanNet.h ├── RendererType.h └── Scan3R_json_loader.h └── scripts ├── README.md └── RUN_GraphSLAM.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | cmake-build-* 3 | build 4 | bin 5 | external/ 6 | *.zip 7 | __pycache__ 8 | data/ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "ORUtils"] 2 | path = ORUtils 3 | url = git@github.com:ShunChengWu/ORUtils.git 4 | [submodule "libGUI3D"] 5 | path = libGUI3D 6 | url = git@github.com:ShunChengWu/libGUI3D.git 7 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.13) 2 | project(GraphSLAM) 3 | ########### 4 | # Options # 5 | ########### 6 | OPTION(BUILD_GRAPHPRED "compile with scene graph prediction" OFF) 7 | OPTION(BUILD_GUI "compile with GUI" OFF) 8 | OPTION(USE_RENDEREDVIEW "Enable generating rendered view for ScanNet and 3RScan dataset" ON) 9 | ################### 10 | # Project setting # 11 | ################### 12 | set(CMAKE_CXX_STANDARD 17) 13 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unused-function" ) 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake ${CMAKE_MODULE_PATH}) 17 | ################################# 18 | # Specify the CMake module path # 19 | ################################# 20 | SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake ${CMAKE_MODULE_PATH}) 21 | SET(CMAKE_PREFIX_PATH ${CMAKE_BINARY_DIR} ${CMAKE_PREFIX_PATH}) 22 | 23 | ################################# 24 | # Add additional compiler flags # 25 | ################################# 26 | IF(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") 27 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libstdc++ -Wno-deprecated-declarations -Wno-unused-function") 28 | ENDIF() 29 | IF(NOT MSVC_IDE) 30 | SET(CFLAGS_WARN "-Wall -Wextra -Wno-unused-parameter -Wno-strict-aliasing") 31 | SET(CMAKE_CXX_FLAGS "-fPIC ${CFLAGS_WARN} ${CMAKE_CXX_FLAGS}") 32 | ENDIF() 33 | ############ 34 | # External # 35 | ############ 36 | SET(WITH_OPENCV TRUE) 37 | SET(WITH_EIGEN TRUE) 38 | SET(WITH_TINYPLY TRUE) 39 | SET(WITH_JSON TRUE) 40 | INCLUDE(Usetinyply) 41 | INCLUDE(UseJson) 42 | INCLUDE(UseOpenMP) 43 | if(APPLE) 44 | set(INSEG_LIB ${PROJECT_SOURCE_DIR}/lib/libInSegLib_osx.a) 45 | else() 46 | set(INSEG_LIB ${PROJECT_SOURCE_DIR}/lib/libInSegLib_linux.a) 47 | endif() 48 | ################ 49 | # Include dirs # 50 | ################ 51 | INCLUDE_DIRECTORIES(ORUtils/include) 52 | ###################### 53 | # Add subdirectories # 54 | ###################### 55 | IF(BUILD_GUI) 56 | ADD_SUBDIRECTORY(libGUI3D) 57 | ADD_SUBDIRECTORY(libGraphSLAMGUI) 58 | ENDIF(BUILD_GUI) 59 | ADD_SUBDIRECTORY(libDataLoader) 60 | ADD_SUBDIRECTORY(libGraphSLAM) 61 | ADD_SUBDIRECTORY(app) 62 | 63 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2021, ShunChengWu 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /Objects/Camera/CameraParameters.h: -------------------------------------------------------------------------------- 1 | #ifndef _H_PSLAM_CAMERA_PARAMETERS 2 | #define _H_PSLAM_CAMERA_PARAMETERS 3 | namespace PSLAM { 4 | struct CameraParameters { 5 | float cx, cy; 6 | float fx, fy; 7 | unsigned short width, height; 8 | 9 | void Set(unsigned int width, unsigned int height, float fx, float fy, float cx, float cy) { 10 | this->width = width; 11 | this->height = height; 12 | this->fx = fx; 13 | this->fy = fy; 14 | this->cx = cx; 15 | this->cy = cy; 16 | } 17 | }; 18 | } 19 | #endif 20 | 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SceneGraphFusion 2 | ![teaser](img/teaser.png) 3 | **Authors**: [Shun-Cheng Wu][sc], [Johanna Wald][jojo], [Keisuke Tateno][keisu], [Nassir Navab][nassir] and [Federico Tombari][fede] 4 | 5 | [sc]:http://campar.in.tum.de/Main/ShunChengWu 6 | [keisu]:http://campar.in.tum.de/Main/KeisukeTateno 7 | [jojo]:http://campar.in.tum.de/Main/JohannaWald 8 | [nassir]:http://campar.in.tum.de/Main/NassirNavabCv 9 | [fede]:http://campar.in.tum.de/Main/FedericoTombari 10 | 11 | This is the SceneGraphFusion c++ framework. For the network part of the code, check [here](https://github.com/ShunChengWu/3DSSG). 12 | 13 | # Prerequisites 14 | ## git, cmake 15 | ``` 16 | apt update 17 | apt install git 18 | # cmake 19 | sudo apt install python3-pip 20 | pip3 install cmake 21 | # create ssh key and associate to your account in order to clone this project. 22 | # * Generate new ssh key [link](https://docs.github.com/en/github/authenticating-to-github/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent) 23 | # * Adding a new ssh key to your account [link](https://docs.github.com/en/github/authenticating-to-github/adding-a-new-ssh-key-to-your-github-account) 24 | ``` 25 | # OpenCV, Eigen 26 | ``` 27 | # Eigen3 3.3.90 28 | # OpenCV 4.0.0 29 | # This project will try to install them locally if they were not found. 30 | ``` 31 | # Assimp 32 | loading meshes for online rendered view generation. 33 | ``` 34 | apt install libassimp-dev 35 | ``` 36 | 37 | # Optional dependencies 38 | With GUI: 39 | ``` 40 | # OpenGL & Glfw3 41 | apt install freeglut3-dev libglfw3-dev 42 | 43 | ``` 44 | With graph prediction: 45 | ``` 46 | # Onnxruntime 47 | # See https://github.com/microsoft/onnxruntime for more information 48 | git clone --recursive -b v1.8.2 https://github.com/microsoft/onnxruntime 49 | cd onnxruntime 50 | # For Linux 51 | ./build.sh --config RelWithDebInfo --build_shared_lib --parallel 52 | cd build/Linux/RelWithDebInfo 53 | sudo make install 54 | ``` 55 | 56 | #### BUILD 57 | build the basic segmentation system 58 | ``` 59 | git clone {this_repo} 60 | cd SceneGraphFusion 61 | git submodule init 62 | git submodule update 63 | mkdir build 64 | cd build 65 | cmake .. 66 | make 67 | ``` 68 | build with GUI or graph prediction, pass these options in cmake: 69 | ``` 70 | cmake -DBUILD_GRAPHPRED=ON -DBUILD_GUI=ON .. 71 | ``` 72 | 73 | # Run 74 | ``` 75 | ./exe_GraphSLAM --pth_in path/to/3RScan/squence/ 76 | # or with GUI 77 | ./exe_GraphSLAM_GUI --pth_in path/to/3RScan/squence/ 78 | # to see usage and options 79 | ./exe_GraphSLAM --help 80 | # to run with graph prediction (need to build with graph predict) 81 | ./exe_GraphSLAM_GUI --pth_in path/to/3RScan/squence/ --pth_model /path/to/the/traced/model/folder/ 82 | ``` 83 | 84 | We provide data loader for 3RScan dataset and ScanNet. In default, we use rendered view for both dataset. The rendered view 85 | will be generated during on time. 86 | 87 | you can download the traced model of our pretrained network [here](https://drive.google.com/file/d/1_745ofaOUyP_iFK8A3cSW60L4V7TlWa7/view?usp=sharing). 88 | The model is trained with 20 NYUv2 object classes used in ScanNet benchmark, and with 8 support types of predicates. 89 | 90 | [comment]: <> (For 3RScan you will need to generate rendered depths and aligned poses. See [3RScan](https://github.com/WaldJohannaU/3RScan/tree/master/c%2B%2B)) 91 | ### Example: 92 | To run our system with a test sequence, run `bash prepare_example_data.sh`. It will download an example sequence provided from 93 | [3RScan](https://github.com/WaldJohannaU/3RScan) repository. Then run 94 | ``` 95 | cd bin 96 | ./exe_GraphSLAM --pth_in ../data/3RScan/4acaebcc-6c10-2a2a-858b-29c7e4fb410d/sequence/ 97 | ``` 98 | Or using the full sequence from either ScanNet or 3RScan 99 | ``` 100 | # For 3RScan: 101 | ./exe_GraphSLAM --pth_in [path/to/3RScan]/[sequence_id]/sequence/ 102 | # For ScanNet 103 | ./exe_GraphSLAM --pth_in [path/to/scannet]/[scan_id]/[scan_id].sens 104 | # Run with graph prediction 105 | ./exe_GraphSLAM --pth_in ./path/to/data --pth_model /path/to/model 106 | ``` 107 | 108 | ### Run with example sequence 109 | If you would like to try out the code without downloading the 3RScan & 3DSSG datset, you can clone the repository of [3RScan](https://github.com/WaldJohannaU/3RScan.git) which consist of two example sequences. 110 | ``` 111 | git clone git@github.com:WaldJohannaU/3RScan.git 112 | cd 3RSan 113 | bash setup.sh 114 | ``` 115 | Then cd to the `exe_GraphSLAM` executable directory and do 116 | ``` 117 | ./exe_GraphSLAM --pth_in /3RScan/data/3RScan/4acaebcc-6c10-2a2a-858b-29c7e4fb410d/sequence/ 118 | # with graph_pred 119 | ./exe_GraphSLAM --pth_in /3RScan/data/3RScan/4acaebcc-6c10-2a2a-858b-29c7e4fb410d/sequence/ --pth_model /CVPR21_traced/traced/ 120 | ``` 121 | 122 | 123 | # License 124 | [![License](https://img.shields.io/badge/License-BSD%202--Clause-orange.svg)](https://opensource.org/licenses/BSD-2-Clause) 125 | 126 | ### Paper 127 | If you find the code useful please consider citing our [paper](https://arxiv.org/pdf/2103.14898.pdf): 128 | 129 | ``` 130 | @inproceedings{Wu2021, 131 | title = {{SceneGraphFusion: Incremental 3D Scene Graph Prediction from RGB-D Sequences}}, 132 | author = {Shun-Cheng Wu and Johanna Wald and Keisuke Tateno and Nassir Navab and Federico Tombari}, 133 | booktitle = {Proceedings IEEE Conference on Computer Vision and Pattern Recognition (CVPR)}, 134 | year = {2021} 135 | } 136 | ``` 137 | 138 | # Acknowledgements 139 | This work is supported by the German Research Foundation (DFG, project number 407378162) and the Bavarian State Ministry of Education, Science and 140 | the Arts in the framework of the Centre Digitisation Bavaria (ZD.B). 141 | 142 | # Troubleshooting 143 | 144 | In some platforms the CMake config for Assimp is called `assimpConfig.cmake` and 145 | in others `AssimpConfig.cmake`. This mismatch can lead to CMake not finding the 146 | library despite being available in the system. If this is the case, edit 147 | `cmake/UseAssimp.cmake` to look like this: 148 | 149 | ``` 150 | find_package(assimp QUIET) 151 | OPTION(WITH_ASSIMP "Build with Assimp support?" ${assimp_FOUND}) 152 | 153 | IF(WITH_ASSIMP) 154 | MESSAGE(STATUS "WITH Assimp") 155 | find_package(assimp REQUIRED) 156 | INCLUDE_DIRECTORIES(${assimp_INCLUDE_DIRS}) 157 | ENDIF() 158 | ``` 159 | 160 | See https://github.com/assimp/assimp/pull/3455, 161 | https://github.com/microsoft/vcpkg/issues/14256 for more details 162 | -------------------------------------------------------------------------------- /Utils/EvaluationHelper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | //#include 10 | 11 | namespace SCSLAM { 12 | namespace EVALUATION { 13 | class Logging { 14 | public: 15 | Logging()=default; 16 | ~Logging()= default; 17 | // Suppress the default copy constructor and assignment operator 18 | Logging(const Logging&); 19 | Logging& operator=(const Logging&); 20 | 21 | 22 | static std::string GenerateOutputPathAndName(std::string pth_out) { 23 | return pth_out; 24 | } 25 | 26 | static void saveTimeInfo(std::map> ×_){ 27 | /// Save Time Information 28 | for (const std::pair &time : getWatch.getTimings()) { 29 | if(!getWatch.updated(time.first)) continue; 30 | getWatch.getUpdateStats()[time.first] = false; 31 | 32 | if(times_.find(time.first) == times_.end()) { 33 | times_[time.first].reserve(1e5); // assume will have this amount of images 34 | times_[time.first].push_back(time.second); 35 | } else { 36 | times_[time.first].push_back(time.second); 37 | } 38 | } 39 | } 40 | 41 | static bool printResultToFile(const std::string &pth_out, const std::string &filename = "times.csv") { 42 | std::map> times_; 43 | for (auto &pair : getWatch.getCTimings()) { 44 | const auto &name = pair.first; 45 | if(pair.second.second==0) continue; 46 | 47 | const auto &value= pair.second.first / float(pair.second.second); 48 | if(!getWatch.updated(name)) continue; 49 | getWatch.getUpdateStats()[name] = false; 50 | 51 | if(times_.find(name) == times_.end()) { 52 | times_[name].reserve(1e5); // assume will have this amount of images 53 | times_[name].push_back(value); 54 | } else { 55 | times_[name].push_back(value); 56 | } 57 | } 58 | return printResultToFile(times_,pth_out,filename); 59 | } 60 | 61 | static bool printResultToFile(const std::map> & times_, const std::string &pth_out, const std::string &filename = "times.csv") { 62 | // checkInputFolder(pth_out); 63 | const std::string outputFileName = pth_out + "/" + filename; 64 | std::fstream logFile_(outputFileName, std::ios::out); 65 | if(!logFile_.is_open()) { 66 | SCLOG(ERROR) <<"Unable to open file at " << outputFileName << "\n"; 67 | return false; 68 | } 69 | 70 | if(!logFile_.is_open()) return false; 71 | logFile_ << "Name;Mean;Variance;Standard Deviation;Number of Measurements" << ";\n"; 72 | double mean, var, stdvar; 73 | for (auto &time : times_) { 74 | mean = var = stdvar = 0; 75 | if (time.second.size() == 1) { 76 | mean = time.second[0]; 77 | var = stdvar = 0; 78 | } else { 79 | mean = std::accumulate(time.second.begin(), time.second.end(), 0.f) / time.second.size(); 80 | for(size_t i=0;iProcessFrame" || 94 | // time.first == "[SLAM][ProcessFrame]4.Mapping") { 95 | // logFile_ << time.first<<"\n"; 96 | // for(size_t i=0;i 6 | #define VecEigenf(x) std::vector,Eigen::aligned_allocator>> 7 | #define VecEigeni(x) std::vector,Eigen::aligned_allocator>> 8 | #define VecEigenuchar(x) std::vector,Eigen::aligned_allocator>> 9 | 10 | #endif 11 | -------------------------------------------------------------------------------- /Utils/eigen_glm.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/20/20. 3 | // 4 | 5 | #ifndef GT_GRAPH_VIEWER_EIGEN2GLM_H 6 | #define GT_GRAPH_VIEWER_EIGEN2GLM_H 7 | 8 | #ifdef COMPILE_WITH_EIGEN 9 | #include 10 | #include 11 | //#include 12 | 13 | template 14 | inline glm::mat E2GLM(const Eigen::Matrix& em) 15 | { 16 | glm::mat mat; 17 | for (int i = 0; i < m; ++i) 18 | { 19 | for (int j = 0; j < n; ++j) 20 | { 21 | mat[j][i] = em(i, j); 22 | } 23 | } 24 | return mat; 25 | } 26 | 27 | template 28 | inline Eigen::Matrix GLM2E(const glm::mat &mat) { 29 | Eigen::Matrix em; 30 | for (int i = 0; i < m; ++i) 31 | { 32 | for (int j = 0; j < n; ++j) 33 | { 34 | em(i, j) = mat[j][i]; 35 | } 36 | } 37 | return em; 38 | } 39 | 40 | template 41 | inline glm::vec E2GLM(const Eigen::Matrix& em) 42 | { 43 | glm::vec v; 44 | for (int i = 0; i < m; ++i) 45 | { 46 | v[i] = em(i); 47 | } 48 | return v; 49 | } 50 | 51 | #endif 52 | #endif //GT_GRAPH_VIEWER_EIGEN2GLM_H 53 | -------------------------------------------------------------------------------- /Utils/util.h: -------------------------------------------------------------------------------- 1 | #ifndef _H_INC_UTIL_ 2 | #define _H_INC_UTIL_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // Includes for GLTexture 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #ifdef _WIN32 16 | #include 17 | #endif 18 | 19 | static int create_folder(const char *path) 20 | { 21 | #if defined _MSC_VER 22 | _mkdir(folder.c_str()); 23 | #else 24 | mkdir(path, 0777); 25 | #endif 26 | } 27 | 28 | template 29 | Eigen::Matrix perspective(double fovy_x, double fovy_y, double zNear, double zFar) { 30 | assert(zFar > zNear); 31 | Eigen::Matrix res = Eigen::Matrix::Zero(); 32 | res(0,0) = 1.0 / (tan(fovy_x / 2.0)); 33 | res(1,1) = 1.0 / (tan(fovy_y / 2.0)); 34 | res(2,2) = -(zFar + zNear) / (zFar - zNear); 35 | res(3,2) = -1.0; 36 | res(2,3) = -(2.0 * zFar * zNear) / (zFar - zNear); 37 | return res; 38 | } 39 | 40 | template 41 | Eigen::Matrix lookAt(Eigen::Matrix const& eye, 42 | Eigen::Matrix const& center, 43 | Eigen::Matrix const & up) { 44 | const Eigen::Matrix f = (center - eye).normalized(); 45 | Eigen::Matrix u = up.normalized(); 46 | const Eigen::Matrix s = f.cross(u).normalized(); 47 | u = s.cross(f); 48 | 49 | Eigen::Matrix res; 50 | res << s.x(), s.y(), s.z(), -s.dot(eye), 51 | u.x(), u.y(), u.z(), -u.dot(eye), 52 | -f.x(), -f.y(), -f.z(), f.dot(eye), 53 | 0, 0, 0, 1; 54 | 55 | return res; 56 | } 57 | 58 | #endif // _H_INC_UTIL_ 59 | 60 | -------------------------------------------------------------------------------- /app/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ADD_SUBDIRECTORY(GraphSLAM) 2 | 3 | 4 | -------------------------------------------------------------------------------- /app/GraphSLAM/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE(UseOpenCV) 2 | INCLUDE(UseEigen3) 3 | IF(BUILD_GUI) 4 | SET(targetname exe_GraphSLAM_GUI) 5 | ADD_EXECUTABLE(${targetname} main.cc) 6 | TARGET_LINK_LIBRARIES(${targetname} 7 | PUBLIC libGraphSLAM 8 | PUBLIC libDataLoader 9 | ) 10 | INCLUDE(${PROJECT_SOURCE_DIR}/cmake/LinkOpenCV.cmake) 11 | TARGET_LINK_LIBRARIES(${targetname} 12 | PUBLIC libGraphSLAMGUI 13 | ) 14 | TARGET_COMPILE_DEFINITIONS(${targetname} PUBLIC COMPILE_WITH_PSLAM_GUI) 15 | ENDIF(BUILD_GUI) 16 | 17 | IF(USE_RENDEREDVIEW) 18 | INCLUDE(UseAssimp) 19 | ENDIF() 20 | 21 | SET(targetname exe_GraphSLAM) 22 | ADD_EXECUTABLE(${targetname} main.cc) 23 | TARGET_LINK_LIBRARIES(${targetname} PUBLIC libGraphSLAM libDataLoader) 24 | INCLUDE(LinkEigen3) 25 | INCLUDE(LinkOpenCV) 26 | 27 | IF(USE_RENDEREDVIEW) 28 | INCLUDE(LinkAssimp) 29 | TARGET_LINK_LIBRARIES(${targetname} PUBLIC GUI3D) 30 | ENDIF() -------------------------------------------------------------------------------- /cmake/CUDACheckCompute.cmake: -------------------------------------------------------------------------------- 1 | ############################# 2 | #Sourced from: 3 | #https://raw.githubusercontent.com/jwetzl/CudaLBFGS/master/CheckComputeCapability.cmake 4 | ############################# 5 | # Check for GPUs present and their compute capability 6 | # based on http://stackoverflow.com/questions/2285185/easiest-way-to-test-for-existence-of-cuda-capable-gpu-from-cmake/2297877#2297877 (Christopher Bruns) 7 | 8 | if(CUDA_FOUND) 9 | message(STATUS "${CMAKE_MODULE_PATH}/cuda_compute_capability.cpp") 10 | try_run(RUN_RESULT_VAR COMPILE_RESULT_VAR 11 | ${CMAKE_BINARY_DIR} 12 | ${CMAKE_MODULE_PATH}/cuda_compute_capability.cpp 13 | CMAKE_FLAGS 14 | -DINCLUDE_DIRECTORIES:STRING=${CUDA_TOOLKIT_INCLUDE} 15 | -DLINK_LIBRARIES:STRING=${CUDA_CUDART_LIBRARY} 16 | COMPILE_OUTPUT_VARIABLE COMPILE_OUTPUT_VAR 17 | RUN_OUTPUT_VARIABLE RUN_OUTPUT_VAR) 18 | message(STATUS "Compile: ${RUN_OUTPUT_VAR}") 19 | if (COMPILE_RESULT_VAR) 20 | message(STATUS "compiled -> " ${RUN_RESULT_VAR}) 21 | else() 22 | message(STATUS "didn't compile") 23 | endif() 24 | # COMPILE_RESULT_VAR is TRUE when compile succeeds 25 | # RUN_RESULT_VAR is zero when a GPU is found 26 | if(COMPILE_RESULT_VAR AND NOT RUN_RESULT_VAR) 27 | message(STATUS "worked") 28 | set(CUDA_HAVE_GPU TRUE CACHE BOOL "Whether a CUDA-capable GPU is present") 29 | set(CUDA_COMPUTE_CAPABILITY ${RUN_OUTPUT_VAR} CACHE STRING "Compute capabilities of CUDA-capable GPUs present (separated by semicolons)") 30 | mark_as_advanced(CUDA_COMPUTE_CAPABILITY) 31 | else() 32 | message(STATUS "didn't work") 33 | set(CUDA_HAVE_GPU FALSE CACHE BOOL "Whether a CUDA-capable GPU is present") 34 | endif() 35 | endif() 36 | -------------------------------------------------------------------------------- /cmake/ConfigureTimeDependency.cmake: -------------------------------------------------------------------------------- 1 | 2 | # Adds steps to download, configure and install dependencies at configure-time 3 | function(add_configure_time_dependency name) 4 | if (NOT TARGET update-${name}) 5 | MESSAGE("-build " ${name}) 6 | # SET(${name}_internal_cache 1 CACHE INTERNAL "${NAME} cache") 7 | configure_file(${CMAKE_SOURCE_DIR}/cmake/External_${name}.CMakeLists.txt.in ${CMAKE_BINARY_DIR}/external/${name}/CMakeLists.txt) 8 | execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . 9 | RESULT_VARIABLE result 10 | WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/external/${name} ) 11 | if(result) 12 | message(FATAL_ERROR "CMake step for ${name} failed: ${result}") 13 | endif() 14 | execute_process(COMMAND ${CMAKE_COMMAND} --build . 15 | RESULT_VARIABLE result 16 | WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/external/${name} ) 17 | if(result) 18 | message(FATAL_ERROR "Build step for ${name} failed: ${result}") 19 | endif() 20 | 21 | # Add update target to get new sources and recompile 22 | add_custom_target(update-${name} 23 | COMMAND ${CMAKE_COMMAND} --build . --target ${name}-update 24 | COMMAND ${CMAKE_COMMAND} --build . --target all 25 | WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/external/${name} 26 | ) 27 | ELSE() 28 | MESSAGE("-skip " ${name}) 29 | ENDIF() 30 | endfunction() -------------------------------------------------------------------------------- /cmake/External_Eigen3.CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | 3 | project(eigen3-download C CXX) 4 | include(ExternalProject) 5 | 6 | set(CMAKE_CXX_STANDARD 11) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | 9 | ExternalProject_Add( 10 | eigen_external 11 | GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git 12 | GIT_TAG 3.4 13 | PREFIX "${CMAKE_BINARY_DIR}/eigen3-external" 14 | INSTALL_DIR "${CMAKE_BINARY_DIR}/eigen3-external" 15 | UPDATE_COMMAND "" 16 | PATCH_COMMAND "" 17 | BUILD_COMMAND make -j8 18 | CMAKE_GENERATOR ${gen} 19 | CMAKE_ARGS 20 | -DCMAKE_CXX_STANDARD=11 21 | -DEIGEN_TEST_CXX11=ON 22 | -DCMAKE_INSTALL_PREFIX:PATH=${PROJECT_SOURCE_DIR}/external/ 23 | ) 24 | -------------------------------------------------------------------------------- /cmake/External_Json.CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.2) 2 | 3 | project(Json-download NONE) 4 | include(ExternalProject) 5 | 6 | ExternalProject_Add(Json_external 7 | GIT_REPOSITORY git@github.com:dropbox/json11.git 8 | GIT_TAG v1.0.0 9 | SOURCE_DIR Json 10 | BINARY_DIR Json-build 11 | UPDATE_COMMAND "" 12 | PATCH_COMMAND "" 13 | BUILD_COMMAND make -j8 14 | CMAKE_GENERATOR ${gen} 15 | CMAKE_ARGS 16 | -DCMAKE_BUILD_TYPE:STRING=Release 17 | -DCMAKE_INSTALL_PREFIX:PATH=${PROJECT_SOURCE_DIR}/external/ 18 | ) -------------------------------------------------------------------------------- /cmake/External_OpenCV.CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.2) 2 | 3 | project(opencv-download NONE) 4 | include(ExternalProject) 5 | 6 | ExternalProject_Add(opencv_external 7 | GIT_REPOSITORY https://github.com/opencv/opencv.git 8 | GIT_TAG 4.3.0 9 | SOURCE_DIR opencv 10 | BINARY_DIR opencv-build 11 | UPDATE_COMMAND "" 12 | PATCH_COMMAND "" 13 | BUILD_COMMAND make -j8 14 | CMAKE_GENERATOR ${gen} 15 | CMAKE_ARGS 16 | # ${ep_common_args} 17 | -DBUILD_DOCS:BOOL=OFF 18 | -DBUILD_EXAMPLES:BOOL=OFF 19 | -DBUILD_NEW_PYTHON_SUPPORT:BOOL=OFF 20 | -DBUILD_PACKAGE:BOOL=OFF 21 | -DBUILD_SHARED_LIBS:BOOL=ON 22 | -DBUILD_TESTS:BOOL=OFF 23 | -DCMAKE_BUILD_TYPE:STRING=Release 24 | -DWITH_FFMPEG:BOOL=OFF 25 | -DCMAKE_INSTALL_PREFIX:PATH=${CMAKE_BINARY_DIR}/ 26 | ) -------------------------------------------------------------------------------- /cmake/External_tinyply.CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.2) 2 | 3 | project(tinyply-download NONE) 4 | include(ExternalProject) 5 | 6 | ExternalProject_Add(tinyply_external 7 | GIT_REPOSITORY git@github.com:ddiakopoulos/tinyply.git 8 | GIT_TAG 2.3.2 9 | SOURCE_DIR tinyply 10 | BINARY_DIR tinyply-build 11 | UPDATE_COMMAND "" 12 | PATCH_COMMAND "" 13 | BUILD_COMMAND make -j8 14 | CMAKE_GENERATOR ${gen} 15 | CMAKE_ARGS 16 | -DCMAKE_BUILD_TYPE:STRING=Release 17 | -DCMAKE_INSTALL_PREFIX:PATH=${PROJECT_SOURCE_DIR}/external/ 18 | ) -------------------------------------------------------------------------------- /cmake/FindJson.cmake: -------------------------------------------------------------------------------- 1 | 2 | FIND_PATH(JSON_INCLUDE_DIR NAMES json11.hpp 3 | PATHS 4 | ${PROJECT_SOURCE_DIR}/external/include 5 | ${PROJECT_SOURCE_DIR}/external/include/${CMAKE_LIBRARY_ARCHITECTURE} 6 | ${CMAKE_BINARY_DIR}/include/ 7 | ${CMAKE_BINARY_DIR}/include/${CMAKE_LIBRARY_ARCHITECTURE} 8 | /usr/include/ 9 | /usr/local/include/json 10 | NO_DEFAULT_PATH 11 | ) 12 | FIND_LIBRARY(JSON_LIBRARY 13 | NAMES 14 | libjson11.a 15 | PATHS 16 | ${PROJECT_SOURCE_DIR}/external/lib/ 17 | ${PROJECT_SOURCE_DIR}/external/lib/${CMAKE_LIBRARY_ARCHITECTURE} 18 | ${CMAKE_BINARY_DIR}/lib/${CMAKE_LIBRARY_ARCHITECTURE} 19 | ) 20 | 21 | include(FindPackageHandleStandardArgs) 22 | find_package_handle_standard_args(Json DEFAULT_MSG 23 | JSON_LIBRARY JSON_INCLUDE_DIR 24 | ) 25 | 26 | IF (JSON_INCLUDE_DIR AND JSON_LIBRARY) 27 | SET( JSON_FOUND 1 CACHE STRING "Set to 1 if GLEW is found, 0 otherwise") 28 | ELSE () 29 | SET( JSON_FOUND 0 CACHE STRING "Set to 1 if GLEW is found, 0 otherwise") 30 | ENDIF () 31 | 32 | MARK_AS_ADVANCED( 33 | # JSON_FOUND 34 | JSON_LIBRARY 35 | JSON_INCLUDE_DIR 36 | ) 37 | -------------------------------------------------------------------------------- /cmake/FindOnnxRuntime.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Try to find onnxruntime library and include path. 3 | # Once done this will define 4 | # OnnxRuntime_FOUND 5 | # ONNXROOT_DIR 6 | # OnnxRuntime_INCLUDE_DIRS 7 | # OnnxRuntime_LIBRARY 8 | # 9 | # And will create an interface library 10 | # OnnxRuntime 11 | 12 | FIND_PATH(ONNXROOT_DIR NAMES ./include/onnxruntime/core/session/onnxruntime_cxx_api.h 13 | PATHS 14 | /usr/local/ 15 | NO_DEFAULT_PATH 16 | ) 17 | IF(ONNXROOT_DIR) 18 | SET(OnnxRuntime_INCLUDE_DIRS ${ONNXROOT_DIR}/include ${ONNXROOT_DIR}/include/onnxruntime/core/session) 19 | 20 | FIND_LIBRARY(OnnxRuntime_LIBRARY 21 | NAMES 22 | libonnxruntime.dylib 23 | libonnxruntime.so 24 | PATHS 25 | ${ONNXROOT_DIR}/lib 26 | ) 27 | ENDIF(ONNXROOT_DIR) 28 | 29 | IF (ONNXROOT_DIR AND OnnxRuntime_LIBRARY) 30 | SET( OnnxRuntime_FOUND 1 CACHE STRING "Set to 1 if GLEW is found, 0 otherwise") 31 | ELSE (ONNXROOT_DIR AND OnnxRuntime_LIBRARY) 32 | SET( OnnxRuntime_FOUND 0 CACHE STRING "Set to 1 if GLEW is found, 0 otherwise") 33 | ENDIF (ONNXROOT_DIR AND OnnxRuntime_LIBRARY) 34 | 35 | MARK_AS_ADVANCED( OnnxRuntime_FOUND ) 36 | 37 | include(FindPackageHandleStandardArgs) 38 | find_package_handle_standard_args(OnnxRuntime 39 | REQUIRED_VARS OnnxRuntime_LIBRARY OnnxRuntime_INCLUDE_DIRS) 40 | 41 | if(OnnxRuntime_FOUND) 42 | if(NOT TARGET OnnxRuntime) 43 | add_library(OnnxRuntime UNKNOWN IMPORTED) 44 | set_target_properties(OnnxRuntime PROPERTIES 45 | INTERFACE_INCLUDE_DIRECTORIES "${OnnxRuntime_INCLUDE_DIRS}") 46 | set_target_properties(OnnxRuntime PROPERTIES 47 | IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" 48 | IMPORTED_LOCATION "${OnnxRuntime_LIBRARY}") 49 | TARGET_COMPILE_DEFINITIONS(OnnxRuntime INTERFACE COMPILE_WITH_ONNX) 50 | endif() 51 | endif() -------------------------------------------------------------------------------- /cmake/Findtinyply.cmake: -------------------------------------------------------------------------------- 1 | 2 | FIND_PATH(TINYPLY_INCLUDE_DIR NAMES tinyply.h 3 | PATHS 4 | ${PROJECT_SOURCE_DIR}/external/include 5 | ${CMAKE_BINARY_DIR}/include/ 6 | ${CMAKE_BINARY_DIR}/include/${CMAKE_LIBRARY_ARCHITECTURE} 7 | /usr/include/ 8 | /usr/local/include/ 9 | NO_DEFAULT_PATH 10 | ) 11 | FIND_LIBRARY(TINYPLY_LIBRARY 12 | NAMES 13 | libtinyply.a 14 | PATHS 15 | ${PROJECT_SOURCE_DIR}/external/lib 16 | ${CMAKE_BINARY_DIR}/lib/ 17 | ${CMAKE_BINARY_DIR}/lib/${CMAKE_LIBRARY_ARCHITECTURE} 18 | ) 19 | 20 | include(FindPackageHandleStandardArgs) 21 | find_package_handle_standard_args(tinyply DEFAULT_MSG 22 | TINYPLY_LIBRARY TINYPLY_INCLUDE_DIR 23 | ) 24 | 25 | IF (TINYPLY_INCLUDE_DIR AND TINYPLY_LIBRARY) 26 | SET( TINYPLY_FOUND 1 CACHE STRING "Set to 1 if GLEW is found, 0 otherwise") 27 | ELSE () 28 | SET( TINYPLY_FOUND 0 CACHE STRING "Set to 1 if GLEW is found, 0 otherwise") 29 | ENDIF () 30 | 31 | MARK_AS_ADVANCED( 32 | # TINYPLY_FOUND 33 | TINYPLY_LIBRARY 34 | TINYPLY_INCLUDE_DIR 35 | ) -------------------------------------------------------------------------------- /cmake/Flags.cmake: -------------------------------------------------------------------------------- 1 | ############### 2 | # Flags.cmake # 3 | ############### 4 | 5 | # If on Mac OS X: 6 | IF(${CMAKE_SYSTEM} MATCHES "Darwin") 7 | # Make sure that C++11 warnings are disabled. 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-c++11-extensions") 9 | 10 | # Make sure that the template depth is sufficient. 11 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=512") 12 | 13 | IF(${CMAKE_SYSTEM} MATCHES "Darwin-13.") 14 | # If on Mac OS X 10.9 (Mavericks), use the libstdc++ implementation of the C++ Standard Library and prevent C++11 code from being compiled. 15 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libstdc++") 16 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -stdlib=libstdc++") 17 | ADD_DEFINITIONS(-DNO_CPP11) 18 | ELSE() 19 | # Otherwise, use the libc++ implementation of the C++ Standard Library, and enable C++11 support. 20 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++ -std=c++11") 21 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -stdlib=libc++ -std=c++11") 22 | ENDIF() 23 | ENDIF() 24 | 25 | # If on Linux, make sure that C++11 support is enabled. 26 | IF(${CMAKE_SYSTEM} MATCHES "Linux") 27 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -pthread") 28 | ENDIF() 29 | 30 | # If on Windows and using Visual Studio: 31 | IF(MSVC_IDE) 32 | # Disable the annoying warnings about using secure CRT functions (they're Microsoft-specific, so we can't use them portably). 33 | ADD_DEFINITIONS(-D_CRT_SECURE_NO_WARNINGS) 34 | 35 | # Prevent the definitions of min and max when including windows.h. 36 | ADD_DEFINITIONS(-DNOMINMAX) 37 | 38 | # Make sure that the maths constants are defined. 39 | ADD_DEFINITIONS(-D_USE_MATH_DEFINES) 40 | 41 | # Define a macro needed when using Boost.ASIO. 42 | ADD_DEFINITIONS(-D_WIN32_WINNT=0x0501) 43 | ENDIF() 44 | -------------------------------------------------------------------------------- /cmake/LinkAssimp.cmake: -------------------------------------------------------------------------------- 1 | IF(WITH_ASSIMP) 2 | target_link_libraries(${targetname} PUBLIC ${ASSIMP_LIBRARIES} -ldl) 3 | TARGET_COMPILE_DEFINITIONS(${targetname} PUBLIC COMPILE_WITH_ASSIMP) 4 | ENDIF() -------------------------------------------------------------------------------- /cmake/LinkEigen3.cmake: -------------------------------------------------------------------------------- 1 | #################### 2 | # LinkEigen3.cmake # 3 | #################### 4 | if(TARGET Eigen3::Eigen) 5 | target_link_libraries(${targetname} PUBLIC Eigen3::Eigen) 6 | elseif(EIGEN3_INCLUDE_DIR) 7 | target_include_directories(${targetname} PUBLIC ${EIGEN3_INCLUDE_DIR}) 8 | else() 9 | MESSAGE(FATAL_ERROR "cannot find eigen3") 10 | endif() -------------------------------------------------------------------------------- /cmake/LinkJson.cmake: -------------------------------------------------------------------------------- 1 | IF(NOT TARGET libJson) 2 | MESSAGE(FATAL_ERROR "Try to link Json but it was not found") 3 | ELSE() 4 | TARGET_LINK_LIBRARIES(${targetname} PUBLIC libJson) 5 | TARGET_COMPILE_DEFINITIONS(${targetname} PUBLIC COMPILE_WITH_JSON) 6 | ENDIF() -------------------------------------------------------------------------------- /cmake/LinkOnnxRuntime.cmake: -------------------------------------------------------------------------------- 1 | #################### 2 | # LinkEigen3.cmake # 3 | #################### 4 | IF(NOT TARGET OnnxRuntime) 5 | MESSAGE(WARNING "Trying to link onnxruntime but it was not found.") 6 | else() 7 | TARGET_LINK_LIBRARIES(${targetname} PUBLIC OnnxRuntime) 8 | endif() -------------------------------------------------------------------------------- /cmake/LinkOpenCV.cmake: -------------------------------------------------------------------------------- 1 | #################### 2 | # LinkOpenGL.cmake # 3 | #################### 4 | IF(WITH_OPENCV) 5 | TARGET_LINK_LIBRARIES(${targetname} PUBLIC ${OpenCV_LIBS}) 6 | TARGET_INCLUDE_DIRECTORIES(${targetname} PUBLIC ${OpenCV_INCLUDE_DIRS}) 7 | TARGET_LINK_DIRECTORIES(${targetname} PUBLIC ${OpenCV_LIB_DIRS}) 8 | TARGET_COMPILE_DEFINITIONS(${targetname} PUBLIC COMPILE_WITH_OPENCV) 9 | 10 | MESSAGE("OpenCV_INCLUDE_DIRS: " ${OpenCV_INCLUDE_DIRS}) 11 | ENDIF() -------------------------------------------------------------------------------- /cmake/Linktinyply.cmake: -------------------------------------------------------------------------------- 1 | IF(NOT TARGET libtinyply) 2 | MESSAGE(FATAL_ERROR "Try to link libtinyply but it was not found") 3 | ELSE() 4 | TARGET_LINK_LIBRARIES(${targetname} PUBLIC libtinyply) 5 | TARGET_COMPILE_DEFINITIONS(${targetname} PUBLIC COMPILE_WITH_TINYPLY) 6 | ENDIF() -------------------------------------------------------------------------------- /cmake/UseAssimp.cmake: -------------------------------------------------------------------------------- 1 | find_package(Assimp QUIET) 2 | OPTION(WITH_ASSIMP "Build with Assimp support?" ${Assimp_FOUND}) 3 | 4 | IF(WITH_ASSIMP) 5 | MESSAGE(STATUS "WITH Assimp") 6 | find_package(Assimp REQUIRED) 7 | INCLUDE_DIRECTORIES(${Assimp_INCLUDE_DIRS}) 8 | ENDIF() -------------------------------------------------------------------------------- /cmake/UseCUDA.cmake: -------------------------------------------------------------------------------- 1 | ################# 2 | # UseCUDA.cmake # 3 | ################# 4 | 5 | FIND_PACKAGE(CUDA QUIET) 6 | 7 | OPTION(WITH_CUDA "Build with CUDA support?" ${CUDA_FOUND}) 8 | 9 | IF(WITH_CUDA) 10 | set(CMAKE_CUDA_COMPILER "/usr/local/cuda/bin/nvcc") 11 | # SET(CMAKE_CUDA_COMPILER:PATH=/usr/local/cuda/bin/nvcc) 12 | enable_language(CUDA) 13 | INCLUDE_DIRECTORIES(${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES}) 14 | # Auto-detect the CUDA compute capability. 15 | SET(CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}") 16 | IF(NOT DEFINED CUDA_COMPUTE_CAPABILITY) 17 | INCLUDE("${CMAKE_MODULE_PATH}/CUDACheckCompute.cmake") 18 | ENDIF() 19 | 20 | message("CUDA_COMPUTE_CAPABILITY: " ${CUDA_COMPUTE_CAPABILITY}) 21 | 22 | # Set the compute capability flags. 23 | FOREACH(compute_capability ${CUDA_COMPUTE_CAPABILITY}) 24 | LIST(APPEND CUDA_NVCC_FLAGS --generate-code arch=compute_${compute_capability},code=sm_${compute_capability}) 25 | ENDFOREACH() 26 | 27 | # Enable fast math. 28 | SET(CUDA_NVCC_FLAGS --use_fast_math ; ${CUDA_NVCC_FLAGS}) 29 | 30 | # If on Windows, make it possible to enable GPU debug information. 31 | IF(MSVC_IDE) 32 | OPTION(ENABLE_CUDA_DEBUGGING "Enable CUDA debugging?" OFF) 33 | IF(ENABLE_CUDA_DEBUGGING) 34 | SET(CUDA_NVCC_FLAGS -G; ${CUDA_NVCC_FLAGS}) 35 | ENDIF() 36 | ENDIF() 37 | 38 | # If on Mac OS X 10.9 (Mavericks), make sure everything compiles and links using the correct C++ Standard Library. 39 | IF(${CMAKE_SYSTEM} MATCHES "Darwin-13.") 40 | SET(CUDA_HOST_COMPILER /usr/bin/clang) 41 | SET(CUDA_NVCC_FLAGS -Xcompiler -stdlib=libstdc++; -Xlinker -stdlib=libstdc++; ${CUDA_NVCC_FLAGS}) 42 | ENDIF() 43 | 44 | # If on Linux: 45 | IF(${CMAKE_SYSTEM} MATCHES "Linux") 46 | # Make sure that C++11 support is enabled when compiling with nvcc. From CMake 3.5 onwards, 47 | # the host flag -std=c++11 is automatically propagated to nvcc. Manually setting it prevents 48 | # the project from building. 49 | IF(${CMAKE_VERSION} VERSION_LESS 3.5) 50 | SET(CUDA_NVCC_FLAGS -std=c++11; ${CUDA_NVCC_FLAGS}) 51 | ENDIF() 52 | 53 | # Work around an Ubuntu 16.04 compilation error. 54 | IF(${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU" AND ${CMAKE_CXX_COMPILER_VERSION} VERSION_GREATER 5.0) 55 | ADD_DEFINITIONS(-D_FORCE_INLINES) 56 | ENDIF() 57 | ENDIF() 58 | 59 | # If not on Windows, disable some annoying nvcc warnings. 60 | IF(NOT MSVC_IDE) 61 | SET(CUDA_NVCC_FLAGS -Xcudafe "--diag_suppress=cc_clobber_ignored" ; -Xcudafe "--diag_suppress=set_but_not_used" ; ${CUDA_NVCC_FLAGS}) 62 | ENDIF() 63 | 64 | MESSAGE("COMPILE_WITH_CUDA") 65 | ADD_DEFINITIONS(-DCOMPILE_WITH_CUDA) 66 | ELSE() 67 | MESSAGE("COMPILE_WITHOUT_CUDA") 68 | ADD_DEFINITIONS(-DCOMPILE_WITHOUT_CUDA) 69 | ENDIF() 70 | -------------------------------------------------------------------------------- /cmake/UseEigen3.cmake: -------------------------------------------------------------------------------- 1 | ################### 2 | # UseEigen3.cmake # 3 | ################### 4 | find_package(Eigen3 3.4 QUIET) 5 | OPTION(WITH_EIGEN "Build with libEIGEN support?" OFF) 6 | #MESSAGE("EIGEN!" ${WITH_EIGEN}) 7 | IF(WITH_EIGEN) 8 | IF(NOT Eigen3_FOUND) 9 | INCLUDE(ConfigureTimeDependency) 10 | add_configure_time_dependency(Eigen3) 11 | find_package(Eigen3 REQUIRED) 12 | ENDIF() 13 | 14 | IF(TARGET Eigen3::Eigen) 15 | GET_TARGET_PROPERTY(EIGEN3_INCLUDE_DIR Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES) 16 | ENDIF() 17 | include_directories(${EIGEN3_INCLUDE_DIR}) 18 | MESSAGE("EIGEN3_INCLUDE_DIR: " ${EIGEN3_INCLUDE_DIR}) 19 | ADD_DEFINITIONS(-DCOMPILE_WITH_EIGEN) 20 | ENDIF() 21 | 22 | 23 | -------------------------------------------------------------------------------- /cmake/UseJson.cmake: -------------------------------------------------------------------------------- 1 | ################# 2 | # UseJson.cmake # 3 | ################# 4 | FIND_PACKAGE(Json QUIET) 5 | 6 | OPTION(WITH_JSON "Build with Json" ${JSON_FOUND}) 7 | 8 | IF(WITH_JSON) 9 | if(NOT JSON_FOUND) 10 | INCLUDE(ConfigureTimeDependency) 11 | add_configure_time_dependency(Json) 12 | FIND_PACKAGE(Json REQUIRED) 13 | ENDIF() 14 | 15 | ADD_DEFINITIONS(-DCOMPILE_WITH_JSON) 16 | MESSAGE("JSON_INCLUDE_DIR: " ${JSON_INCLUDE_DIR}) 17 | INCLUDE_DIRECTORIES(${JSON_INCLUDE_DIR}) 18 | # Create a target for easy linking library 19 | IF(NOT TARGET libJson) 20 | ADD_LIBRARY(libJson 21 | INTERFACE 22 | ) 23 | TARGET_LINK_LIBRARIES(libJson 24 | INTERFACE ${JSON_LIBRARY} 25 | ) 26 | TARGET_INCLUDE_DIRECTORIES(libJson 27 | INTERFACE ${JSON_INCLUDE_DIR} 28 | ) 29 | TARGET_COMPILE_DEFINITIONS(libJson INTERFACE COMPILE_WITH_JSON) 30 | ENDIF() 31 | 32 | ENDIF() -------------------------------------------------------------------------------- /cmake/UseOnnxRuntime.cmake: -------------------------------------------------------------------------------- 1 | ################### 2 | # UseEigen3.cmake # 3 | ################### 4 | 5 | FIND_PACKAGE(OnnxRuntime) 6 | 7 | OPTION(WITH_ONNX "Build with libEIGEN support?" ${OnnxRuntime_FOUND}) 8 | 9 | IF(WITH_ONNX) 10 | include_directories(${OnnxRuntime_INCLUDE_DIRS}) 11 | ADD_DEFINITIONS(-DCOMPILE_WITH_ONNX) 12 | ENDIF() 13 | 14 | 15 | -------------------------------------------------------------------------------- /cmake/UseOpenCV.cmake: -------------------------------------------------------------------------------- 1 | ################### 2 | # UseOpenCV.cmake # 3 | ################### 4 | find_package(OpenCV 4.0.0 QUIET) 5 | OPTION(WITH_OPENCV "Build with OpenCV support?" ${OpenCV_FOUND}) 6 | 7 | IF(WITH_OPENCV) 8 | MESSAGE("WITH_OPENCV: " ${WITH_OPENCV}) 9 | IF(NOT OpenCV_FOUND) 10 | INCLUDE(ConfigureTimeDependency) 11 | add_configure_time_dependency(OpenCV) 12 | find_package(OpenCV REQUIRED) 13 | ENDIF() 14 | INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS}) 15 | link_directories(${OpenCV_LIB_DIRS}) 16 | ADD_DEFINITIONS(-DCOMPILE_WITH_OPENCV) 17 | ELSE() 18 | ADD_DEFINITIONS(-DCOMPILE_WITHOUT_OPENCV) 19 | ENDIF() 20 | -------------------------------------------------------------------------------- /cmake/UseOpenMP.cmake: -------------------------------------------------------------------------------- 1 | ################### 2 | # UseOpenMP.cmake # 3 | ################### 4 | FIND_PACKAGE(OpenMP QUIET) 5 | OPTION(WITH_OPENMP "Enable OpenMP support?" ${OPENMP_FOUND}) 6 | 7 | IF(WITH_OPENMP) 8 | FIND_PACKAGE(OpenMP REQUIRED) 9 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 10 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 11 | set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 12 | ADD_DEFINITIONS(-DWITH_OPENMP) 13 | ENDIF() 14 | -------------------------------------------------------------------------------- /cmake/Usetinyply.cmake: -------------------------------------------------------------------------------- 1 | ### 2 | FIND_PACKAGE(tinyply QUIET) 3 | 4 | OPTION(WITH_TINYPLY "Build with tinyply" ${TINYPLY_FOUND}) 5 | 6 | IF(WITH_TINYPLY) 7 | IF(NOT tinyply_FOUND) 8 | INCLUDE(ConfigureTimeDependency) 9 | add_configure_time_dependency(tinyply) 10 | FIND_PACKAGE(tinyply REQUIRED) 11 | ENDIF() 12 | ADD_DEFINITIONS(-DCOMPILE_WITH_TINYPLY) 13 | 14 | IF(NOT TARGET libtinyply) 15 | ADD_LIBRARY(libtinyply 16 | INTERFACE 17 | ) 18 | TARGET_LINK_LIBRARIES(libtinyply 19 | INTERFACE ${TINYPLY_LIBRARY} 20 | ) 21 | TARGET_INCLUDE_DIRECTORIES(libtinyply 22 | INTERFACE ${TINYPLY_INCLUDE_DIR} 23 | ) 24 | TARGET_COMPILE_DEFINITIONS(libtinyply INTERFACE COMPILE_WITH_TINYPLY) 25 | ENDIF() 26 | ENDIF() -------------------------------------------------------------------------------- /cmake/cuda_compute_capability.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2011 Florian Rathgeber, florian.rathgeber@gmail.com 3 | * 4 | * This code is licensed under the MIT License. See the FindCUDA.cmake script 5 | * for the text of the license. 6 | * 7 | * Based on code by Christopher Bruns published on Stack Overflow (CC-BY): 8 | * http://stackoverflow.com/questions/2285185 9 | */ 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | int main() { 19 | int deviceCount; 20 | std::set computeCapabilities; 21 | 22 | if (cudaGetDeviceCount(&deviceCount) != cudaSuccess) 23 | { 24 | printf("Couldn't get device count: %s\n", cudaGetErrorString(cudaGetLastError())); 25 | return 1; 26 | } 27 | /* machines with no GPUs can still report one emulation device */ 28 | for (int device = 0; device < deviceCount; ++device) 29 | { 30 | cudaDeviceProp currentProperties; 31 | cudaGetDeviceProperties(¤tProperties, device); 32 | if (currentProperties.major != 9999) {/* 9999 means emulation only */ 33 | std::stringstream ss; 34 | ss << currentProperties.major; 35 | if(currentProperties.major == 2 && currentProperties.minor == 1) 36 | { 37 | ss << 0; // There is no compute_21 architecture. 38 | } 39 | else 40 | { 41 | ss << currentProperties.minor; 42 | } 43 | 44 | computeCapabilities.insert(ss.str()); 45 | } 46 | } 47 | 48 | /* don't just return the number of gpus, because other runtime cuda 49 | errors can also yield non-zero return values */ 50 | for(std::set::const_iterator it = computeCapabilities.begin(); it != computeCapabilities.end(); ++it) 51 | { 52 | // Add a semicolon if we have already printed some output. 53 | if(it != computeCapabilities.begin()) std::cout << ';'; 54 | std::cout << *it; 55 | } 56 | 57 | return computeCapabilities.size() == 0; /* 0 devices -> failure */ 58 | } 59 | -------------------------------------------------------------------------------- /img/teaser.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunChengWu/SceneGraphFusion/5bf9017c00949aedca1430854240667b3fa06565/img/teaser.png -------------------------------------------------------------------------------- /lib/inseg_lib/camera.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_CAMERA_H_ 2 | #define INSEG_LIB_CAMERA_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace inseg_lib { 9 | 10 | // Camera parameter class to represent camera intrinsics. Focal length and 11 | // principle point, image width and height. 12 | class CamParams { 13 | public: 14 | CamParams() = default; 15 | // Get a scaled version of the camera parameters. 16 | CamParams CreateScaledCamParams(const float scale) const; 17 | 18 | // Denormalize and normalize 2D point according to intrinsics. 19 | Eigen::Vector2f Denormalize(const Eigen::Vector2f& xy) const; 20 | Eigen::Vector2f Normalize(const Eigen::Vector2f& uv) const; 21 | 22 | // Focal Length. 23 | float fx = 0; 24 | float fy = 0; 25 | // Principal Point. 26 | float cx = 0; 27 | float cy = 0; 28 | // Image Size. 29 | int img_width = 0; 30 | int img_height = 0; 31 | }; 32 | 33 | class InSegCamera { 34 | public: 35 | // Set camera paramter list (for image pyramid). maximum levels is the height 36 | // of the pyramid, minimum level is the level that is used for the processing 37 | // such as labeling, segmentation and tracking. cparam is the base of pyramid. 38 | void Init(const CamParams& cparam, 39 | const int max_levels, 40 | const int min_levels); 41 | 42 | // Return camera paramters for a specific level of the image pyramid. 43 | const CamParams& GetCamParams(const int level) const; 44 | // Returns the camera paramater of the index image whereas the index map is 4 45 | // times as big as the main image. 46 | const CamParams& GetIndexCamParams() const; 47 | // Returns height of the image pyramid which is also the max level parameter. 48 | int GetMaxLevel() const; 49 | private: 50 | // Camera paramaters of index image. 51 | CamParams camera_param_index_; 52 | // Vector of camera parameters that represents the image pyramid. 53 | std::vector camera_params_; 54 | }; 55 | } // namespace inseg_lib 56 | 57 | #endif // INSEG_LIB_CAMERA_H_ 58 | -------------------------------------------------------------------------------- /lib/inseg_lib/disjoint_set.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_DISJOINT_SET_H_ 2 | #define INSEG_LIB_DISJOINT_SET_H_ 3 | 4 | #include 5 | 6 | namespace inseg_lib { 7 | 8 | // Simple implementation of disjoint-set data structure. 9 | class DisjointSet { 10 | public: 11 | DisjointSet() = default; 12 | 13 | explicit DisjointSet(const int size); 14 | // Resizes the set and initializes memory. 15 | void Resize(const int size); 16 | 17 | // Finds an item. 18 | int Find(const int x); 19 | // Finds a set. 20 | bool FindSet(const int x, const int y); 21 | // Combines two sets. 22 | bool UnionSet(const int x, const int y); 23 | int GetSize(const int x); 24 | // Get size of the disjoint set which is the number of root elements. 25 | int GetTotalSize(); 26 | private: 27 | std::vector parents_; 28 | // This tells us how many elements are in the disjoint set which is the total 29 | // size. It is set to the size of the root elements in the beginning and 30 | // decreased whenever a set it combined. 31 | int num_elements_ = 0; 32 | }; 33 | } // namespace inseg_lib 34 | 35 | #endif // INSEG_LIB_DISJOINT_SET_H_ 36 | -------------------------------------------------------------------------------- /lib/inseg_lib/image_pyramid.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_IMAGE_PYRAMID_H_ 2 | #define INSEG_LIB_IMAGE_PYRAMID_H_ 3 | 4 | #include 5 | 6 | #include "camera.h" 7 | #include "map_config.h" 8 | 9 | namespace inseg_lib { 10 | 11 | // Class represents frame pyramid with depth map, point cloud, normal map. 12 | // Model frame is frame from the model view. 13 | class ImagePyramid { 14 | public: 15 | explicit ImagePyramid(const MapConfig& config, 16 | const int max_level, const int min_level); 17 | 18 | void CreateDepthPyramid(const cv::Mat& depth_image, 19 | const cv::Mat& color_image, 20 | const bool normalize_depth, 21 | const InSegCamera& camera, 22 | const float uncertainty_coefficient_depth); 23 | 24 | // Returns point cloud (vertex map) at the level. 25 | cv::Mat& GetFramePointCloud(); 26 | const cv::Mat& GetFramePointCloud(int level) const; 27 | 28 | // Returns normal map at the level. 29 | const cv::Mat& GetFrameNormalMap(int level = 0) const; 30 | 31 | const cv::Mat& GetRGBImage() const; 32 | const cv::Mat& GetRGBImageHD() const; 33 | 34 | // Returns normal map. 35 | cv::Mat& GetFrameLabelMapNotConst(); 36 | cv::Mat& GetModelLabelMapNotConst(); 37 | 38 | // Returns the segmented depth map. 39 | const cv::Mat& GetFrameLabelMap() const; 40 | 41 | void PyrModelDown(const int max_level); 42 | void PyrFrameDown(const int max_level); 43 | void PyrDownMap(const cv::Mat& upper, cv::Mat* cur); 44 | 45 | // Returns point cloud map 46 | cv::Mat& GetModelPointCloud(); 47 | const cv::Mat& GetModelPointCloud(int level) const; 48 | 49 | // Returns normal map at the level. 50 | cv::Mat& GetModelNormalMapNotConst(int level = 0); 51 | cv::Mat& GetFrameNormalMapNotConst(int level = 0); 52 | 53 | const cv::Mat& GetModelNormalMap(int level = 0) const; 54 | 55 | // Sets label map from model view. 56 | void SetModelLabelMap(cv::Mat* label); 57 | void SetFrameLabelMap(cv::Mat* label); 58 | 59 | // Propagation of segmentation (rendered point). 60 | const cv::Mat& GetModelLabelMap() const; 61 | private: 62 | const int min_level_; 63 | const MapConfig& config_; 64 | 65 | // Image pyramids of depth, point cloud and normal map. 66 | std::vector frame_pyr_depth_img_; 67 | std::vector frame_pyr_point_cloud_; 68 | // Image pyramids of depth map. These pyramid levels are needed for tracking. 69 | std::vector model_pyr_point_cloud_; 70 | std::vector frame_pyr_rgb_img_; 71 | cv::Mat frame_rgb_img_; 72 | 73 | // Image pyramids of normal map. 74 | std::vector model_pyr_normal_map_; 75 | std::vector frame_pyr_normal_map_; 76 | 77 | // Label map. 78 | cv::Mat frame_label_map_; 79 | // Image pyramids of label map. 80 | cv::Mat model_label_map_; 81 | 82 | // Image pyramids of smoothed depth image and point cloud. 83 | std::vector frame_pyr_smooth_depth_img_; 84 | std::vector frame_pyr_smooth_point_cloud_; 85 | }; 86 | 87 | // Calculate normal Map from depth map. 88 | void FilterBilateral3x3(const cv::Mat& source, 89 | const float uncertainty_coef_depth, 90 | cv::Mat* destination); 91 | 92 | float AddBilateralFilterElem(const float depth, 93 | const int16_t coefficient, 94 | const float reference_depth, 95 | const float depth_variance); 96 | 97 | void CalculateNormalMap(const cv::Mat& point_cloud, cv::Mat* normal_image); 98 | void CalculatePointCloud(const cv::Mat& depth_image, 99 | const CamParams& cam_params, 100 | const bool normalize_depth, 101 | cv::Mat* point_cloud); 102 | 103 | } // namespace inseg_lib 104 | 105 | #endif // INSEG_LIB_IMAGE_PYRAMID_H_ 106 | -------------------------------------------------------------------------------- /lib/inseg_lib/inseg.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_INSEG_H_ 2 | #define INSEG_LIB_INSEG_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "image_pyramid.h" 13 | #include "inseg_config.h" 14 | #include "main_config.h" 15 | #include "map.h" 16 | #include "map_config.h" 17 | #include "segmentation.h" 18 | #include "segmentation_config.h" 19 | 20 | namespace inseg_lib { 21 | 22 | // Depth based inseg main class. 23 | class InSegReconstruction { 24 | public: 25 | InSegReconstruction(std::shared_ptr segments, 26 | InSegConfig& config, 27 | MainConfig& main_config, 28 | MapConfig& map_config, 29 | const SegmentationConfig& segmentation_config); 30 | 31 | // Prepares image pyramid and also applies segmentation and tracking. 32 | // The current pose is used to propage the labels from the global model. 33 | // The confidence of correctly predicted segments is increased and added 34 | // to the global segmentation model. 35 | // If two segments overlap in consecutive frames they are merged. 36 | void ProcessFrame(const cv::Mat& depth_map, const cv::Mat& color_image); 37 | 38 | // Get current camera pose. 39 | const Eigen::Matrix4f& pose() const { return pose_; } 40 | 41 | // Set the pose e.g. tangos visual odomertry. 42 | void set_pose(const Eigen::Matrix4f& pose); 43 | 44 | // Returns the map (used for point cloud visualization). 45 | Map& map() { return map_; } 46 | 47 | // Returns image pyramid. 48 | ImagePyramid& image_pyramid() { 49 | return image_pyramid_; 50 | } 51 | 52 | // Initializes the global (segmentation) map. Once the map is initialized the 53 | // incremental segmentation starts. 54 | void InitializeMap(const Eigen::Matrix4f& pose); 55 | // Initializes the camera parameters for the image pyramid. 56 | void InitializeCamera(const CamParams& cparam); 57 | void Reset(); 58 | 59 | const cv::Mat& GetFrameLabelMap(); 60 | 61 | void SetLabelMap(); 62 | void SetPropagatedLabelMap(); 63 | 64 | int GetLabelSize() const; 65 | 66 | MapConfig& map_config; 67 | InSegConfig& config; 68 | MainConfig& main_config; 69 | private: 70 | // Segments the image and labels it by using point cloud and normal map. 71 | void Label(); 72 | 73 | // Make index map for correspondences between input frame and surfels on 74 | // world map. 75 | void MakeIndexMap(); 76 | 77 | // Updates the Global Model (surfels and segments). Labels are updated, at 78 | // each new frame with segmentation information associated with the current 79 | // depth map. Segment are potentially merged. 80 | void UpdateMap(); 81 | 82 | // In the label propagation function correspondences between the visible 83 | // segments of the GSM, and those on the current depth map are estimated 84 | // by checking whether the underlying surface of both segments are the same. 85 | void PropagateLabels(); 86 | 87 | // Computes the number of labels currently in the label_map. 88 | void CalculateLabelSizeList(const cv::Mat* label_map, 89 | const int label_num, 90 | std::vector* label_size_list); 91 | 92 | InSegCamera camera_; 93 | std::unique_ptr segmentation_; 94 | ImagePyramid image_pyramid_; 95 | 96 | // Point cloud map in the world coordinate system. 97 | Map map_; 98 | 99 | // Camera pose in the world coordinate system. 100 | Eigen::Matrix4f pose_; 101 | 102 | // Look up data structure for labels to be merged. 103 | DisjointSet world_label_match_list_; 104 | 105 | // Index map. 106 | // TODO(WaldJohannaU): Change this to Eigen Matrix. 107 | cv::Mat index_map_; 108 | 109 | int label_num_ = 0; 110 | int label_num_frame_ = 0; 111 | 112 | cv::Mat label_map_frame_; 113 | cv::Mat label_map_propagated_frame_; 114 | 115 | // Number of labels in the global segmentation map. 116 | int world_label_num_ = 0; 117 | std::vector label_size_list_; 118 | std::set instance_labels; 119 | 120 | // Label map to represent the world map. 121 | cv::Mat world_label_map_; 122 | // The label_connect_confidence_list_[i][j] is the merging confidence to merge 123 | // the segment with label i with another segment with label j. To do so, 124 | // label_connect_confidence_list_[i] stores a hash-map to save the confidence 125 | // values for the merging candidates for label j (key of inner hashmap). In 126 | // most cases the inner hash map has only one or no entries. The confidence 127 | // value of the map is increased every time the segments overlap. 128 | std::map> label_connect_confidence_list_; 129 | }; 130 | 131 | } // namespace inseg_lib 132 | 133 | #endif // INSEG_LIB_INSEG_H_ 134 | -------------------------------------------------------------------------------- /lib/inseg_lib/inseg_config.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_INSEG_CONFIG_H_ 2 | #define INSEG_LIB_INSEG_CONFIG_H_ 3 | 4 | namespace inseg_lib { 5 | 6 | // Settings for the InSegReconstruction class. 7 | struct InSegConfig { 8 | // Defines if dynamic points should be removed. 9 | // This is useful if the scene is not static. 10 | // Might remove noisy depth input on object borders. 11 | bool remove_dynamic_point = false; 12 | 13 | // Threshold when a surfel is set to invalid (in frames). 14 | int surfel_invalid_frame_threshold = 30; 15 | 16 | // Frame threshold when a point is considered to be stable. 17 | int surfel_stable_frame_threshold = 2; 18 | 19 | // Angle threshold of normal vectors in the map update stage. 20 | // Point are considered invalid if angle (degrees) is higher than threshold. 21 | float update_point_angle_threshold = 75; 22 | 23 | // Threshold for the dot product of the normal of the surfels. 24 | // If dot product is less than the threshold we have a valid normal. 25 | float update_point_dot_product_threshold = 30; 26 | 27 | // Confidence value frame threshold when two label are merged. 28 | // This happens when they are when overlapping. 29 | int label_merge_confidence_threshold = 3; 30 | 31 | // Overlap threshold to identify correct segments in propagation step. 32 | // If two labels overlap by this ratio they are merged. 33 | float label_merge_overlap_ratio = 0.3; // 0.2 34 | 35 | // Smalles size of a segment to be labled. 36 | // This threshold helps to remove noise. 37 | int label_min_size = 10; 38 | 39 | // This angle threshold is used to determine segment correspondences in the 40 | // label propagation step. If vertices are close (along viewing ray) and the 41 | // angle between the corresponding normals are lower than this threshold 42 | // the segments are considered to overlap. 43 | float label_point_dot_product_threshold = 10; 44 | 45 | // Threshold for the dot product of edges in segmentation step. 46 | // This defines how the surfaces are segmented. 47 | // float depth_edge_threshold = 0.98f; // segments_0 0.992f 48 | //TODO: add a IO for this 49 | float depth_edge_threshold = 0.98f;// for 3RScan 50 | // float depth_edge_threshold = 0.90f;// for ScanNet 51 | 52 | // Label size threshold used for the merging process. 53 | int label_size_threshold = 15; 54 | 55 | // Label region ratio for the merging process. 56 | // If two labels overlap by this ratio they will be merged. 57 | float label_region_ratio = 0.3; 58 | }; 59 | } // namespace inseg_lib 60 | 61 | #endif // INSEG_LIB_INSEG_CONFIG_H_ 62 | -------------------------------------------------------------------------------- /lib/inseg_lib/lib.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_LIB_H_ 2 | #define INSEG_LIB_LIB_H_ 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include "image_pyramid.h" 9 | #include "inseg.h" 10 | #include "inseg_config.h" 11 | #include "main_config.h" 12 | #include "map.h" 13 | #include "map_config.h" 14 | #include "segmentation_config.h" 15 | 16 | namespace inseg_lib { 17 | 18 | // Interface class for InSegLib. 19 | class InSegLibInterface { 20 | public: 21 | bool initialized = false; 22 | 23 | // Process frame function for the main thread. 24 | virtual void ProcessFrame(const cv::Mat& depth_map, 25 | const cv::Mat& color_map) = 0; 26 | // Initialize camera intrinsics for image pyramid. 27 | virtual void InitializeCamera(const Eigen::Matrix3d& intrinsics, 28 | const int image_width, 29 | const int image_height) = 0; 30 | // Returns the current camera pose in world coordinates. 31 | virtual const Eigen::Matrix4f& pose() const = 0; 32 | // Set pose from outside (e.g. tango). 33 | virtual void set_pose(const Eigen::Matrix4f& pose) = 0; 34 | virtual void InitializeMap(const Eigen::Matrix4f& pose) = 0; 35 | virtual void Reset() = 0; 36 | // Get the current reconstructed and segmented map. 37 | virtual Map& map() = 0; 38 | // Returns image pyramid. 39 | virtual ImagePyramid& image_pyramid() = 0; 40 | // Returns the segmentation of the depth image. 41 | virtual const cv::Mat& GetSegmentedLabelMap() = 0; 42 | virtual const cv::Mat& GetFrameLabelMap() = 0; 43 | 44 | virtual void SetLabelMap() = 0; 45 | virtual void SetPropagatedLabelMap() = 0; 46 | virtual int GetLabelSize() = 0; 47 | }; 48 | 49 | // class InSegReconstruction; 50 | class InSegLib : public InSegLibInterface { 51 | public: 52 | InSegLib(std::shared_ptr segments, 53 | InSegConfig& config, 54 | MainConfig& main_config, 55 | MapConfig& map_config, 56 | const SegmentationConfig& segmentation_config); 57 | // Main process. 58 | void ProcessFrame(const cv::Mat& depth_map, 59 | const cv::Mat& color_map) override; 60 | 61 | // Initialize map data and camera. 62 | void InitializeMap(const Eigen::Matrix4f& pose) override; 63 | void InitializeCamera(const Eigen::Matrix3d& intrinsics, 64 | const int image_width, 65 | const int image_height) override; 66 | void Reset() override; 67 | // Returns the current camera pose in world coordinates. 68 | const Eigen::Matrix4f& pose() const override; 69 | void SetLabelMap() override; 70 | void SetPropagatedLabelMap() override; 71 | 72 | // Set pose from outside (e.g. tango). 73 | void set_pose(const Eigen::Matrix4f& pose) override; 74 | // Get the current reconstructed and segmented map. 75 | Map& map() override; 76 | // Returns image pyramid. 77 | ImagePyramid& image_pyramid() override; 78 | // Returns the segmentation of the depth image. 79 | const cv::Mat& GetSegmentedLabelMap() override; 80 | const cv::Mat& GetFrameLabelMap() override; 81 | 82 | int GetLabelSize() override; 83 | private: 84 | InSegReconstruction inseg_; 85 | // segmented map from depth image. 86 | cv::Mat segmented_labels_; 87 | }; 88 | } // namespace inseg_lib 89 | 90 | #endif // INSEG_LIB_LIB_H_ 91 | -------------------------------------------------------------------------------- /lib/inseg_lib/main_config.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_MAIN_CONFIG_H_ 2 | #define INSEG_LIB_MAIN_CONFIG_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace inseg_lib { 8 | 9 | // Represents an invalid label. 10 | constexpr int16_t kLabelUnknown = 0; 11 | // Represents an edge in the edge image. 12 | constexpr int16_t kEdge = 0; 13 | // Represents no edge in the edge image. 14 | constexpr int16_t kNoEdge = 255; 15 | 16 | struct MainConfig { 17 | // Depth uncertainty coefficient. 18 | double uncertainty_coefficient_depth = 0.0000285f; 19 | 20 | // Max pyramid level for main the process. 21 | int max_pyr_level = 6; 22 | 23 | // Main pyramid level for main process. 24 | int min_pyr_level = 3; 25 | }; 26 | 27 | } // namespace inseg_lib 28 | 29 | #endif // INSEG_LIB_MAIN_CONFIG_H_ 30 | -------------------------------------------------------------------------------- /lib/inseg_lib/map.h: -------------------------------------------------------------------------------- 1 | #ifndef _H_INC_INSEG_MAP_DATA_ 2 | #define _H_INC_INSEG_MAP_DATA_ 3 | 4 | #include 5 | #include 6 | // #include 7 | 8 | #include "camera.h" 9 | #include "map_config.h" 10 | #include "map_segments.h" 11 | #include "surfel.h" 12 | 13 | // Surfel, Map (holds list of surfel), Frame Pyramid 14 | namespace inseg_lib { 15 | 16 | class Map { // represents world map with list of surfels (Global Model in the paper) 17 | public: 18 | Map(std::shared_ptr segments, 19 | MapConfig& config); 20 | ~Map(); 21 | 22 | bool SaveSegments(const std::string& file, const std::string& file_list, const int color_type) const; 23 | // save model as .ply 24 | bool SaveModel(const std::string& filename, const int color_type = 2, 25 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity()) const; 26 | 27 | int GetSize() const; 28 | 29 | const std::shared_ptr GetSurfel(const int index) const; 30 | 31 | void SetColor(const int index, const cv::Vec3b& col); 32 | void SetLabelConfidence(const int index, const float value); 33 | void SetStable(const int index, bool value); 34 | void SetInvalid(const int index); 35 | void Clear(); 36 | void Add(const std::shared_ptr surfel); 37 | void UpdateSurfel(const int index, const Eigen::Vector3f& pos, const Eigen::Vector3f& normal, 38 | const float confidence, const float radius); 39 | void Merge(const int index, const int index2); 40 | // Index is the index of the surfel in the global model. 41 | void SetLabel(const int index, const int label); 42 | std::vector> surfels; 43 | std::shared_ptr segments_; 44 | private: 45 | void WritePlyHeader(std::ofstream& fout, const int vertex) const; 46 | bool SaveSurfels(const std::string& filename, 47 | const int color_type, 48 | const std::vector>& surfel_list, 49 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity()) const; 50 | }; 51 | 52 | const cv::Vec3b& CalculateLabelColor(const int16_t label); 53 | 54 | } // namespace inseg_lib 55 | 56 | #endif // INSEG_LIB_MAP_H_ 57 | -------------------------------------------------------------------------------- /lib/inseg_lib/map_config.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_MAP_CONFIG_H_ 2 | #define INSEG_LIB_MAP_CONFIG_H_ 3 | 4 | namespace inseg_lib { 5 | 6 | struct MapConfig { 7 | // Near threshold for depth values in mm. All other values are dropped. 8 | double near_depth_threshold = 10; 9 | 10 | // Far threshold for depth values in mm. All other values are dropped. 11 | double far_depth_threshold = 10000; 12 | 13 | // Number of bilateral filter iterations that should be applied 14 | // (0=disabled). 15 | int num_iterations_bilateral_filtering = 0; 16 | 17 | // Normalize depth data (for synthetic renderings). 18 | bool normalize_depth = false; 19 | }; 20 | } // namespace inseg_lib 21 | 22 | #endif // INSEG_LIB_MAP_CONFIG_H_ 23 | -------------------------------------------------------------------------------- /lib/inseg_lib/map_segments.h: -------------------------------------------------------------------------------- 1 | #ifndef _H_INC_INSEG_MAP_SEGMENTS_ 2 | #define _H_INC_INSEG_MAP_SEGMENTS_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "surfel.h" 9 | #include 10 | #include 11 | 12 | namespace inseg_lib { 13 | 14 | class SegmentsInterface { 15 | public: 16 | virtual ~SegmentsInterface() { } 17 | virtual void Merge(const int segment_index1, const int segment_index2) = 0; 18 | virtual int Add(const std::shared_ptr &surfel) = 0; 19 | virtual void Update(const std::shared_ptr surfel, 20 | const Eigen::Vector3f& pos, const Eigen::Vector3f& normal) = 0; 21 | virtual void Clear() = 0; 22 | virtual int UpdateLabel(const std::shared_ptr surfel, const int label) = 0; 23 | }; 24 | 25 | } // namespace inseg_lib 26 | 27 | #endif // _H_INC_INSEG_MAP_SEGMENTS_ 28 | -------------------------------------------------------------------------------- /lib/inseg_lib/segmentation.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_SEGMENTATION_H_ 2 | #define INSEG_LIB_SEGMENTATION_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "disjoint_set.h" 10 | 11 | namespace inseg_lib { 12 | 13 | // Interface class for the segmentation. 14 | class SegmentationInterface { 15 | public: 16 | virtual int Segment(const cv::Mat& point_cloud, 17 | const cv::Mat& normal_map, 18 | cv::Mat* label_map) = 0; 19 | }; 20 | 21 | // Segmentation by Normal Edge. 22 | class InSegSegmentation : public SegmentationInterface { 23 | public: 24 | InSegSegmentation(const float depth_edge_threshold, 25 | const float uncertainty_coefficient_depth, 26 | const int min_region_size) 27 | : depth_edge_threshold_(depth_edge_threshold), 28 | uncertainty_coefficient_depth_(uncertainty_coefficient_depth), 29 | min_region_size_(min_region_size) {} 30 | int Segment(const cv::Mat& point_cloud, 31 | const cv::Mat& normal_map, 32 | cv::Mat* label_map) override; 33 | 34 | private: 35 | void CalculateGeometricalEdge(const cv::Mat& point_cloud, 36 | const cv::Mat& normal_map); 37 | void Segment(cv::Mat* label_map); 38 | void RegionGrowing(const cv::Mat& point_cloud, 39 | const cv::Mat& normal_map, cv::Mat* label_map); 40 | int Relabel(cv::Mat* label_map); 41 | void CreateEdges(const cv::Mat& normal_edge, const cv::Mat& point_cloud, 42 | const cv::Mat& normal_map); 43 | float CalculateEdgeWeight(const Eigen::Vector3f& point, 44 | const Eigen::Vector3f& normal, 45 | const Eigen::Vector3f& other_vert, 46 | const Eigen::Vector3f& other_normal); 47 | float CalculateAngleMin(const Eigen::Vector3f& point, 48 | const Eigen::Vector3f& normal, 49 | const Eigen::Vector3f& other_vert, 50 | const Eigen::Vector3f& other_normal, float min_dot); 51 | 52 | const float depth_edge_threshold_; 53 | const float uncertainty_coefficient_depth_; 54 | const int min_region_size_; 55 | cv::Mat normal_edge_; 56 | DisjointSet labels_; 57 | }; 58 | } // namespace inseg_lib 59 | 60 | #endif // INSEG_LIB_SEGMENTATION_H_ 61 | -------------------------------------------------------------------------------- /lib/inseg_lib/segmentation_config.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_SEGMENTATION_CONFIG_H_ 2 | #define INSEG_LIB_SEGMENTATION_CONFIG_H_ 3 | 4 | namespace inseg_lib { 5 | 6 | struct SegmentationConfig { 7 | // Minimum region size in pixel of a label to be propageded. 8 | int min_region_size = 10; 9 | }; 10 | } // namespace inseg_lib 11 | 12 | #endif // INSEG_LIB_SEGMENTATION_CONFIG_H_ 13 | -------------------------------------------------------------------------------- /lib/inseg_lib/surfel.h: -------------------------------------------------------------------------------- 1 | #ifndef INSEG_LIB_SURFEL_H_ 2 | #define INSEG_LIB_SURFEL_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace inseg_lib { 11 | 12 | class Surfel { 13 | public: 14 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 15 | Surfel() = default; 16 | Surfel(const Eigen::Vector3f& pos, const Eigen::Vector3f& normal, 17 | const float radius, const float confidence, const int label); 18 | ~Surfel() = default; 19 | bool operator< (const Surfel &ref) const; 20 | 21 | void SetLabel(const int label); 22 | int GetLabel() const; 23 | 24 | int index_in_graph = -1; 25 | 26 | // This is to determinate if surfel is stable 27 | float confidence = 0; 28 | int time_invalid = 0; 29 | 30 | // For the update process, only checked if < 0 at some point 31 | int label_confidence = 0; 32 | 33 | // set true either because of m_timeInvalid or error threshold 34 | bool is_valid = false; 35 | bool is_stable = false; 36 | 37 | // corresponding color used for drawing 38 | std::array color = {{255, 255, 255}}; 39 | 40 | Eigen::Vector3f pos; 41 | Eigen::Vector3f normal; 42 | float radius = 0; 43 | int label = 0; 44 | }; 45 | } 46 | typedef std::shared_ptr SurfelPtr; 47 | 48 | #endif // INSEG_LIB_SURFEL_H_ 49 | -------------------------------------------------------------------------------- /lib/libInSegLib_linux.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunChengWu/SceneGraphFusion/5bf9017c00949aedca1430854240667b3fa06565/lib/libInSegLib_linux.a -------------------------------------------------------------------------------- /lib/libInSegLib_osx.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ShunChengWu/SceneGraphFusion/5bf9017c00949aedca1430854240667b3fa06565/lib/libInSegLib_osx.a -------------------------------------------------------------------------------- /libDataLoader/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(targetname libDataLoader) 2 | 3 | INCLUDE(UseOpenCV) 4 | INCLUDE(UseEigen3) 5 | 6 | SET(sources 7 | src/dataloader_scannet.cpp 8 | src/dataloader_3rscan.cpp 9 | ScanNetLoader/sensorData.cpp 10 | ) 11 | SET(headers 12 | include/dataLoader/dataset_loader.h 13 | include/dataLoader/dataset_loader_facotry.h 14 | ) 15 | 16 | ADD_LIBRARY(${targetname} ${sources} ${headers}) 17 | TARGET_INCLUDE_DIRECTORIES(${targetname} PUBLIC ./include) -------------------------------------------------------------------------------- /libDataLoader/ScanNetLoader/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 2 | 3 | UNSET(COMPILE_WITH_CUDA) 4 | ############################################################################### 5 | ### -- 3rd party 6 | #find_package(PkgConfig REQUIRED) 7 | #pkg_check_modules(JSONCPP jsoncpp) # For Standford2D3DS 8 | 9 | #find_package(OpenCV REQUIRED) 10 | #INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS}) 11 | #link_directories(${OpenCV_LIB_DIRS}) 12 | 13 | ########################################################################################### 14 | SET(targetname ScanNetLoader) 15 | file(GLOB_RECURSE srcs *.cpp) 16 | file(GLOB_RECURSE headers *.h *.hpp) 17 | file(GLOB_RECURSE protos *.proto) 18 | 19 | SET(sources 20 | ${srcs} 21 | ${protos} 22 | ) 23 | 24 | SET(headers 25 | ${headers} 26 | ) 27 | 28 | ADD_LIBRARY(${targetname} ${sources} ${headers}) 29 | #target_link_libraries(${targetname} 30 | # PUBLIC ${JSONCPP_LIBRARIES} 31 | # PUBLIC ${OpenCV_LIBS} 32 | #) 33 | 34 | #target_include_directories(${targetname} PUBLIC ${PROJECT_SOURCE_DIR}/ThirdParty/stb) 35 | #target_include_directories(${targetname} PUBLIC ${CMAKE_CURRENT_BINARY_DIR}) 36 | #target_compile_definitions(${targetname} PUBLIC -DWITH_OPENCV PUBLIC -DWITH_REALSENSE2) 37 | #set_target_properties(${targetname} PROPERTIES LINKER_LANGUAGE CXX) 38 | set_target_properties(${targetname} PROPERTIES CXX_STANDARD 11) 39 | #target_include_directories(${targetname} PUBLIC ./include) 40 | #target_include_directories(${targetname} PUBLIC ./Loader) 41 | 42 | -------------------------------------------------------------------------------- /libDataLoader/ScanNetLoader/sensorData.cpp: -------------------------------------------------------------------------------- 1 | #include "sensorData.h" 2 | namespace ml { 3 | 4 | #define STB_IMAGE_STATIC 5 | #define STB_IMAGE_IMPLEMENTATION 6 | #define STB_IMAGE_IMPLEMENTATION_SCANNET 7 | #include "sensorData1/stb_image.h" 8 | #define STB_IMAGE_WRITE_IMPLEMENTATION 9 | #define STB_IMAGE_WRITE_IMPLEMENTATION_SCANNET 10 | #include "sensorData1//stb_image_write.h" 11 | //#include 12 | 13 | void SensorData::RGBDFrame::compressDepth(const unsigned short *depth, unsigned int width, unsigned int height, 14 | COMPRESSION_TYPE_DEPTH type) { 15 | freeDepth(); 16 | 17 | if (type == TYPE_RAW_USHORT) { 18 | if (m_depthSizeBytes != width * height) { 19 | freeDepth(); 20 | m_depthSizeBytes = width * height * sizeof(unsigned short); 21 | m_depthCompressed = (unsigned char *) std::malloc(m_depthSizeBytes); 22 | } 23 | std::memcpy(m_depthCompressed, depth, m_depthSizeBytes); 24 | } else if (type == TYPE_ZLIB_USHORT) { 25 | freeDepth(); 26 | 27 | int out_len = 0; 28 | int quality = 8; 29 | int n = 2; 30 | unsigned char *tmpBuff = (unsigned char *) std::malloc((width * n + 1) * height); 31 | std::memcpy(tmpBuff, depth, width * height * sizeof(unsigned short)); 32 | m_depthCompressed = stbi_zlib_compress(tmpBuff, width * height * sizeof(unsigned short), &out_len, 33 | quality); 34 | std::free(tmpBuff); 35 | m_depthSizeBytes = out_len; 36 | } else if (type == TYPE_OCCI_USHORT) { 37 | freeDepth(); 38 | #ifdef _USE_UPLINK_COMPRESSION 39 | //TODO fix the shift here 40 | int out_len = 0; 41 | int n = 2; 42 | unsigned int tmpBuffSize = (width*n + 1) * height; 43 | unsigned char* tmpBuff = (unsigned char *)std::malloc(tmpBuffSize); 44 | out_len = uplinksimple::encode(depth, width*height, tmpBuff, tmpBuffSize); 45 | m_depthSizeBytes = out_len; 46 | m_depthCompressed = (unsigned char*)std::malloc(out_len); 47 | std::memcpy(m_depthCompressed, tmpBuff, out_len); 48 | std::free(tmpBuff); 49 | #else 50 | throw MLIB_EXCEPTION("need UPLINK_COMPRESSION"); 51 | #endif 52 | } else { 53 | throw MLIB_EXCEPTION("unknown compression type"); 54 | } 55 | } 56 | 57 | vec3uc* SensorData::RGBDFrame::decompressColorAlloc_stb(COMPRESSION_TYPE_COLOR type) const { //can handle PNG, JPEG etc. 58 | if (type != TYPE_JPEG && type != TYPE_PNG) throw MLIB_EXCEPTION("invliad type"); 59 | if (m_colorCompressed == NULL || m_colorSizeBytes == 0) throw MLIB_EXCEPTION("decompression error"); 60 | int channels = 3; 61 | int width, height; 62 | unsigned char* raw = ScanNetstb::stbi_load_from_memory(m_colorCompressed, (int)m_colorSizeBytes, &width, &height, NULL, channels); 63 | 64 | return (vec3uc*)raw; 65 | } 66 | 67 | unsigned short* SensorData::RGBDFrame::decompressDepthAlloc_stb(COMPRESSION_TYPE_DEPTH type) const { 68 | if (type != TYPE_ZLIB_USHORT) throw MLIB_EXCEPTION("invliad type"); 69 | unsigned short* res; 70 | int len; 71 | res = (unsigned short*)ScanNetstb::stbi_zlib_decode_malloc((const char*)m_depthCompressed, (int)m_depthSizeBytes, &len); 72 | return res; 73 | } 74 | } -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/CameraParameters.h: -------------------------------------------------------------------------------- 1 | #ifndef _H_PSLAM_CAMERA_PARAMETERS 2 | #define _H_PSLAM_CAMERA_PARAMETERS 3 | namespace PSLAM { 4 | struct CameraParameters { 5 | float cx, cy; 6 | float fx, fy; 7 | unsigned short width, height; 8 | float scale = 1.f; 9 | void Set(unsigned int width, unsigned int height, float fx, float fy, float cx, float cy, float scale = 1.f) { 10 | this->width = width; 11 | this->height = height; 12 | this->fx = fx; 13 | this->fy = fy; 14 | this->cx = cx; 15 | this->cy = cy; 16 | this->scale = scale; 17 | } 18 | }; 19 | } 20 | #endif 21 | -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/Scan3R_json_loader.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 2/5/21. 3 | // 4 | 5 | #ifndef RELOCALIZATION3D_SCAN3R_JSON_LOADER_H 6 | #define RELOCALIZATION3D_SCAN3R_JSON_LOADER_H 7 | 8 | #include 9 | namespace PSLAM::io { 10 | class Scan3RLoader { 11 | public: 12 | struct MovedObject { 13 | explicit MovedObject(int id) : id(id) {} 14 | 15 | int id; 16 | int symmetry = 0; 17 | Eigen::Matrix4f transformation; 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | }; 20 | 21 | typedef std::shared_ptr MovedObjectPtr; 22 | 23 | static inline MovedObjectPtr MakeMovedObjectPtr(int id) { 24 | return std::make_shared(id); 25 | } 26 | 27 | struct Ambiguity { 28 | int instance_source, instance_target; 29 | /// align source to target 30 | Eigen::Matrix4f transformation; 31 | 32 | Ambiguity(int source, int target) : instance_source(source), instance_target(target) {} 33 | }; 34 | 35 | typedef std::shared_ptr AmbiguityPtr; 36 | 37 | static inline AmbiguityPtr MakeAmbiguityPtr(int source, int target) { 38 | return std::make_shared(source, target); 39 | } 40 | 41 | struct ScanInfo; 42 | typedef std::shared_ptr ScanInfoPtr; 43 | 44 | struct ScanInfo { 45 | // general 46 | std::string scan_id; 47 | std::string type; 48 | 49 | // as a reference scan 50 | // SC::DisJointSet ambiguious_sets; 51 | std::unordered_map> ambiguities; 52 | std::unordered_map rescans; 53 | 54 | // as a rescan 55 | /// align rescan its reference scan 56 | Eigen::Matrix4f transformation{Eigen::Matrix4f::Identity()}; 57 | std::unordered_map moved_rigid_objects; 58 | 59 | explicit ScanInfo(std::string id) : scan_id(std::move(id)) {} 60 | 61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 62 | }; 63 | 64 | static inline ScanInfoPtr MakeScanInfo(const std::string &scan_id) { 65 | return std::make_shared(scan_id); 66 | } 67 | 68 | explicit Scan3RLoader(const std::string &path) { 69 | Load(path); 70 | } 71 | 72 | bool IsRescan(const std::string &scan_id) { 73 | return reference2rescans.find(scan_id) == reference2rescans.end(); 74 | } 75 | 76 | bool IsReference(const std::string &scan_id) { 77 | return reference2rescans.find(scan_id) != reference2rescans.end(); 78 | } 79 | 80 | std::map rescanToReference; 81 | std::map > reference2rescans; 82 | std::map scaninfos; 83 | 84 | private: 85 | static void ReadMatrix(Eigen::Matrix4f &matrix, const json11::Json &json_matrix) { 86 | matrix = Eigen::Matrix4f::Identity(); 87 | int i = 0; 88 | for (auto &elem: json_matrix.array_items()) 89 | matrix(i++) = elem.number_value(); 90 | } 91 | 92 | void Load(const std::string &path) { 93 | auto scan3r = ORUtils::JsonUtils::LoadJson(path); 94 | for (const auto &s : scan3r.array_items()) { 95 | 96 | const std::string &reference_id = s["reference"].string_value(); 97 | const std::string &type = s["type"].string_value(); 98 | 99 | auto &refScanInfo = scaninfos[reference_id]; 100 | refScanInfo = MakeScanInfo(reference_id); 101 | refScanInfo->type = type; 102 | 103 | // Add ambiguity information 104 | const auto &ambiguities = s["ambiguity"].array_items(); 105 | for (const auto &ambiguity : ambiguities) { 106 | const auto &instance_source = ambiguity["instance_source"].int_value(); 107 | const auto &instance_target = ambiguity["instance_target"].int_value(); 108 | auto am = MakeAmbiguityPtr(instance_source, instance_target); 109 | refScanInfo->ambiguities[instance_source].push_back(am); 110 | ReadMatrix(am->transformation, ambiguity["transform"]); 111 | 112 | // auto &set = refScanInfo->ambiguious_sets; 113 | // if (!set.exist(instance_source)) set.add(instance_source); 114 | // if (!set.exist(instance_target)) set.add(instance_target); 115 | // set.unionSet(instance_source, instance_target); 116 | } 117 | 118 | 119 | const auto &scans = s["scans"].array_items(); 120 | // Add rescan information 121 | for (auto &scan: scans) { 122 | const std::string &scan_id = scan["reference"].string_value(); 123 | rescanToReference[scan_id] = reference_id; 124 | reference2rescans[reference_id].push_back(scan_id); 125 | 126 | auto scanInfo = MakeScanInfo(scan_id); 127 | refScanInfo->rescans[scan_id] = scanInfo; 128 | ReadMatrix(scanInfo->transformation, scan["transform"]); 129 | 130 | for (const auto &rigid : scan["rigid"].array_items()) { 131 | const int instance_id = rigid["instance_reference"].int_value(); 132 | MovedObjectPtr &movedObject = scanInfo->moved_rigid_objects[instance_id] = MakeMovedObjectPtr( 133 | instance_id); 134 | Eigen::Matrix4f &rigid_transform = movedObject->transformation; 135 | ReadMatrix(rigid_transform, rigid["transform"]); 136 | // The transformation in the json is actually from reference to rescan 137 | rigid_transform = rigid_transform.inverse().eval(); 138 | } 139 | } 140 | } 141 | } 142 | }; 143 | } 144 | 145 | #endif //RELOCALIZATION3D_SCAN3R_JSON_LOADER_H 146 | -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/dataloader_3rscan.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 1/13/21. 3 | // 4 | 5 | #ifndef LIBSURFELRECONSTRUCTION_DATALOADER_3RSCAN_H 6 | #define LIBSURFELRECONSTRUCTION_DATALOADER_3RSCAN_H 7 | #include "dataset_loader.h" 8 | namespace PSLAM { 9 | class DatasetLoader_3RScan : public DatasetLoader_base { 10 | public: 11 | DatasetLoader_3RScan(std::shared_ptr dataset); 12 | void Reset() override; 13 | bool Retrieve() override; 14 | 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | private: 17 | std::string pose_file_name_ = ""; 18 | 19 | Eigen::Matrix rotation_matrix_Z(const float rot = M_PI); 20 | 21 | const std::string GetFileName(const std::string &folder, 22 | const std::string &subfolder, 23 | const std::string &prefix, 24 | const std::string &suffix, 25 | int number_length) const; 26 | bool IsV2() const; 27 | 28 | Eigen::Matrix4f m_poseTransform; 29 | }; 30 | } 31 | 32 | #endif //LIBSURFELRECONSTRUCTION_DATALOADER_3RSCAN_H 33 | -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/dataloader_scannet.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 2/27/21. 3 | // 4 | 5 | #ifndef GRAPHSLAM_DATALOADER_SCANNET_H 6 | #define GRAPHSLAM_DATALOADER_SCANNET_H 7 | 8 | #include "dataset_loader.h" 9 | #include 10 | #include 11 | #include 12 | #include "../../ScanNetLoader/sensorData.h" 13 | //#include "ScanNetLoader/sensorData.h" 14 | //#include "CameraParameters.h" 15 | //#include "../Objects/Camera/CameraParameters.h" 16 | 17 | namespace PSLAM { 18 | class DatasetLoader_ScanNet : public DatasetLoader_base { 19 | public: 20 | explicit DatasetLoader_ScanNet(std::shared_ptr dataset); 21 | void Reset() override; 22 | bool Retrieve() override; 23 | private: 24 | std::unique_ptr<::ml::SensorData> m_loader; 25 | bool GetDepth(); 26 | bool GetRGB(); 27 | bool GetPose(); 28 | 29 | const std::string GetFileName(const std::string& folder, 30 | const std::string& subfolder, 31 | const std::string& prefix, 32 | const std::string& suffix, 33 | int number_length) const; 34 | }; 35 | } 36 | 37 | #endif //GRAPHSLAM_DATALOADER_SCANNET_H 38 | -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/dataset3RScan.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 1/13/21. 3 | // 4 | 5 | #ifndef LIBSURFELRECONSTRUCTION_DATASET3RSCAN_H 6 | #define LIBSURFELRECONSTRUCTION_DATASET3RSCAN_H 7 | 8 | #include "dataset_base.h" 9 | 10 | namespace PSLAM { 11 | class Scan3RDataset : public DatasetDefinitionBase { 12 | public: 13 | Scan3RDataset(INPUTE_TYPE type, const std::string &path) { 14 | datasetType = type; 15 | folder = path; 16 | frame_index_counter = 1; 17 | number_length = 1; 18 | prefix_pose = "/frame-"; 19 | prefix_depth = "/frame-"; 20 | prefix_rgb = "/frame-"; 21 | 22 | suffix_depth = ".depth.pgm"; 23 | suffix_rgb = ".color.jpg"; 24 | suffix_pose = ".pose.txt"; 25 | if (rotate_pose_img) { 26 | // suffix_depth = ".rendered.depth.png"; 27 | // suffix_pose = ".align.pose.txt"; 28 | suffix_depth = ".depth.pgm"; 29 | suffix_pose = ".pose.txt"; 30 | // suffix_pose = use_aligned_pose? ".align.pose.txt" : ".pose.txt"; 31 | } 32 | 33 | min_pyr_level = 3; 34 | number_pose = 6; 35 | number_length = 6; 36 | } 37 | bool use_aligned_pose = true; 38 | }; 39 | } 40 | 41 | #endif //LIBSURFELRECONSTRUCTION_DATASET3RSCAN_H 42 | -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/datasetScanNet.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 1/13/21. 3 | // 4 | 5 | #ifndef LIBSURFELRECONSTRUCTION_DATASETSCANNET_H 6 | #define LIBSURFELRECONSTRUCTION_DATASETSCANNET_H 7 | 8 | #include "dataset_base.h" 9 | 10 | namespace PSLAM { 11 | class ScanNetDataset : public DatasetDefinitionBase { 12 | public: 13 | ScanNetDataset(INPUTE_TYPE type, const std::string &path) { 14 | datasetType = type; 15 | folder = path; 16 | frame_index_counter = 2; //TODO: make a IO for this 17 | number_length = 1; 18 | 19 | prefix_pose = "/frame-"; 20 | prefix_depth = "/frame-"; 21 | prefix_rgb = "/frame-"; 22 | 23 | suffix_depth = ".depth.pgm"; 24 | suffix_rgb = ".color.png"; 25 | suffix_pose = ".pose.txt"; 26 | if (rotate_pose_img) { 27 | suffix_depth = ".rendered.depth.png"; 28 | suffix_pose = ".pose.txt"; 29 | } 30 | 31 | min_pyr_level = 3; 32 | number_pose = 6; 33 | number_length = 6; 34 | } 35 | }; 36 | } 37 | 38 | #endif //LIBSURFELRECONSTRUCTION_DATASETSCANNET_H 39 | -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/dataset_base.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 1/13/21. 3 | // 4 | 5 | #ifndef LIBSURFELRECONSTRUCTION_DATASET_BASE_H 6 | #define LIBSURFELRECONSTRUCTION_DATASET_BASE_H 7 | #include "dataset_types.h" 8 | #include 9 | namespace PSLAM { 10 | class DatasetDefinitionBase { 11 | public: 12 | DatasetDefinitionBase() = default; 13 | virtual ~DatasetDefinitionBase() = default; 14 | 15 | INPUTE_TYPE datasetType; 16 | std::string folder = ""; 17 | 18 | // TODO change to true if V2 19 | bool rotate_pose_img = true; 20 | 21 | int frame_index_counter = 1; 22 | int number_length = 0; 23 | int number_pose = 0; 24 | 25 | int max_depth = 5000; // mm 26 | 27 | std::string prefix_depth = ""; 28 | std::string prefix_rgb = ""; 29 | std::string prefix_pose = ""; 30 | 31 | std::string suffix_depth = ""; 32 | std::string suffix_rgb = ""; 33 | std::string suffix_pose = ""; 34 | 35 | std::string folder_depth = ""; 36 | std::string folder_rgb = ""; 37 | std::string folder_pose = ""; 38 | 39 | int min_pyr_level = 1; 40 | }; 41 | } 42 | 43 | #endif //LIBSURFELRECONSTRUCTION_DATASET_BASE_H 44 | -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/dataset_loader.h: -------------------------------------------------------------------------------- 1 | #ifndef _H_INC_DATASET_LOADER_ 2 | #define _H_INC_DATASET_LOADER_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "dataset_base.h" 14 | #include "CameraParameters.h" 15 | 16 | namespace PSLAM { 17 | class DatasetLoader_base { 18 | public: 19 | explicit DatasetLoader_base(std::shared_ptr dataset): 20 | m_dataset(std::move(dataset)){ 21 | } 22 | virtual ~DatasetLoader_base() = default; 23 | 24 | virtual void Reset() = 0; 25 | virtual bool Retrieve() = 0; 26 | 27 | [[nodiscard]] const cv::Mat& GetRGBImage() const { return m_rgb; } 28 | [[nodiscard]] const cv::Mat& GetDepthImage() const { return m_d; } 29 | 30 | [[nodiscard]] const Eigen::Matrix4f& GetPose() const { return m_pose; } 31 | [[nodiscard]] const CameraParameters &GetCamParamDepth() const { return m_cam_param_d; } 32 | [[nodiscard]] const CameraParameters &GetCamParamRGB() const { return m_cam_param_rgb; } 33 | [[nodiscard]] CameraParameters &GetCamParamDepth() { return m_cam_param_d; } 34 | [[nodiscard]] CameraParameters &GetCamParamRGB() { return m_cam_param_rgb; } 35 | const DatasetDefinitionBase * GetDataBase(){return m_dataset.get();} 36 | 37 | void SetFrameIndex(int idx) {frame_index = idx;} 38 | 39 | const int& GetFrameIndex() const { return frame_index; } 40 | 41 | const int& GetFinalIndex() const { return frame_index_max; } 42 | 43 | const DatasetDefinitionBase* GetBase() const {return m_dataset.get(); } 44 | 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | protected: 47 | std::shared_ptr m_dataset; 48 | int frame_index = 0, frame_index_max = 0; 49 | cv::Mat m_rgb, m_d; 50 | Eigen::Matrix4f m_pose; 51 | CameraParameters m_cam_param_rgb, m_cam_param_d; 52 | }; 53 | 54 | 55 | 56 | 57 | 58 | } 59 | #endif 60 | -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/dataset_loader_facotry.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "dataset3RScan.h" 3 | #include "datasetScanNet.h" 4 | #include "dataloader_3rscan.h" 5 | #include "dataloader_scannet.h" 6 | 7 | namespace PSLAM { 8 | struct DataLoaderFactory { 9 | static DatasetLoader_base *Make( 10 | const std::string &pth, INPUTE_TYPE inputeType = DATASET_DETECT) { 11 | DatasetLoader_base *output = nullptr; 12 | // detect datatype by checking file name 13 | 14 | if(inputeType == DATASET_DETECT){ 15 | std::cerr << "detect data type: "; 16 | if(pth.find(".sens") != std::string::npos || pth.find("scene") != std::string::npos) { 17 | inputeType = DATASET_SCANNET; 18 | std::cerr << "ScanNet"; 19 | } else { 20 | inputeType = DATASET_3RSCAN; 21 | std::cerr << "3RScan"; 22 | } 23 | } 24 | 25 | // Create dataloader 26 | switch (inputeType) { 27 | case DATASET_3RSCAN: { 28 | auto path = pth.back() == '/'? pth : pth+"/"; 29 | auto database = std::make_shared(inputeType, path); 30 | output = new DatasetLoader_3RScan(database); 31 | break; 32 | } 33 | case DATASET_SCANNET: { 34 | auto database = std::make_shared(inputeType, pth); 35 | output = new DatasetLoader_ScanNet(database); 36 | break; 37 | } 38 | case DATASET_DETECT: 39 | break; 40 | 41 | } 42 | return output; 43 | } 44 | }; 45 | } -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/dataset_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 1/13/21. 3 | // 4 | 5 | #ifndef LIBSURFELRECONSTRUCTION_DATASET_TYPES_H 6 | #define LIBSURFELRECONSTRUCTION_DATASET_TYPES_H 7 | namespace PSLAM { 8 | enum INPUTE_TYPE { 9 | DATASET_DETECT, DATASET_3RSCAN, DATASET_SCANNET 10 | }; 11 | } 12 | #endif //LIBSURFELRECONSTRUCTION_DATASET_TYPES_H 13 | -------------------------------------------------------------------------------- /libDataLoader/include/dataLoader/util.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 1/13/21. 3 | // 4 | 5 | #ifndef LIBSURFELRECONSTRUCTION_UTIL_H 6 | #define LIBSURFELRECONSTRUCTION_UTIL_H 7 | #include 8 | #include 9 | #include 10 | namespace PSLAM { 11 | static inline bool isFileExist(const std::string &filename) { 12 | std::ifstream file_tmp(filename); 13 | if (!file_tmp.is_open()) { 14 | return false; 15 | } 16 | file_tmp.close(); 17 | return true; 18 | } 19 | 20 | static inline void LoadPose(Eigen::Matrix4f &pose, const std::string &path, bool rotate) { 21 | pose.setIdentity(); 22 | std::ifstream file(path); 23 | assert(file.is_open()); 24 | if (file.is_open()) { 25 | for (int i = 0; i < 4; i++) 26 | for (int j = 0; j < 4; j++) 27 | file >> pose(i, j); 28 | pose.block<3, 1>(0, 3) *= 1000.0f; 29 | file.close(); 30 | } 31 | // std::cout << "pose\n"<< pose << "\n"; 32 | } 33 | } 34 | 35 | 36 | 37 | #endif //LIBSURFELRECONSTRUCTION_UTIL_H 38 | -------------------------------------------------------------------------------- /libDataLoader/src/dataloader_3rscan.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 1/13/21. 3 | // 4 | #include "../include/dataLoader/dataloader_3rscan.h" 5 | #include "../include/dataLoader/util.h" 6 | #include "../include/dataLoader/Scan3R_json_loader.h" 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace PSLAM; 14 | 15 | static const std::vector split(const std::string s, const std::string delim) { 16 | std::vector list; 17 | auto start = 0U; 18 | auto end = s.find(delim); 19 | while (true) { 20 | list.push_back(s.substr(start, end - start)); 21 | if (end == std::string::npos) 22 | break; 23 | start = end + delim.length(); 24 | end = s.find(delim, start); 25 | } 26 | return list; 27 | } 28 | 29 | static bool LoadInfoIntrinsics(const std::string& filename, 30 | const bool depth_intrinsics, 31 | CameraParameters& intrinsics) { 32 | const std::string search_tag = depth_intrinsics ? "m_calibrationDepthIntrinsic" : "m_calibrationColorIntrinsic"; 33 | const std::string search_tag_w = depth_intrinsics? "m_depthWidth":"m_colorWidth"; 34 | const std::string search_tag_h = depth_intrinsics? "m_depthHeight":"m_colorHeight"; 35 | std::string line{""}; 36 | std::ifstream file(filename); 37 | int width,height; 38 | float fx,fy,cx,cy; 39 | if (file.is_open()) { 40 | while (std::getline(file,line)) { 41 | if (line.rfind(search_tag_w, 0) == 0) 42 | width = std::stoi(line.substr(line.find("= ")+2, std::string::npos)); 43 | else if (line.rfind(search_tag_h, 0) == 0) 44 | height = std::stoi(line.substr(line.find("= ")+2, std::string::npos)); 45 | else if (line.rfind(search_tag, 0) == 0) { 46 | const std::string model = line.substr(line.find("= ")+2, std::string::npos); 47 | const auto parts = split(model, " "); 48 | fx = std::stof(parts[0]); 49 | fy = std::stof(parts[5]); 50 | cx = std::stof(parts[2]); 51 | cy = std::stof(parts[6]); 52 | } 53 | } 54 | file.close(); 55 | intrinsics.Set(width,height,fx,fy,cx,cy,1.f); 56 | return true; 57 | } 58 | 59 | return false; 60 | } 61 | 62 | DatasetLoader_3RScan::DatasetLoader_3RScan(std::shared_ptr dataset): 63 | DatasetLoader_base(std::move(dataset)) { 64 | if(!LoadInfoIntrinsics(m_dataset->folder+"/_info.txt",true,m_cam_param_d)) 65 | throw std::runtime_error("unable to open _info file"); 66 | if(!LoadInfoIntrinsics(m_dataset->folder+"/_info.txt",false,m_cam_param_rgb)) 67 | throw std::runtime_error("unable to open _info file"); 68 | m_poseTransform.setIdentity(); 69 | if(reinterpret_cast(m_dataset.get())->use_aligned_pose) { 70 | auto seq_folder = tools::PathTool::find_parent_folder(m_dataset->folder,1); 71 | auto seq_name = tools::PathTool::getFileName(seq_folder); 72 | auto data_folder = tools::PathTool::find_parent_folder(seq_folder,1); 73 | auto scan3rLoader = PSLAM::io::Scan3RLoader(data_folder+"/3RScan.json"); 74 | if(scan3rLoader.IsRescan(seq_name)) { 75 | // find ref scan ID 76 | auto ref_id = scan3rLoader.rescanToReference.at(seq_name); 77 | m_poseTransform = scan3rLoader.scaninfos.at(ref_id)->rescans.at(seq_name)->transformation; 78 | m_poseTransform.topRightCorner<3,1>()*=1e3; 79 | } 80 | } 81 | } 82 | 83 | const std::string DatasetLoader_3RScan::GetFileName(const std::string& folder, 84 | const std::string& subfolder, 85 | const std::string& prefix, 86 | const std::string& suffix, 87 | int number_length = -1) const { 88 | std::stringstream filename; 89 | const std::string path = (folder == "/" ? "" : folder) + 90 | (subfolder == "/" ? "" : subfolder) + 91 | (prefix == "/" ? "" : prefix); 92 | if (number_length < 0) 93 | filename << path << (suffix == "/" ? "" : suffix); 94 | else 95 | filename << path << std::setfill('0') << std::setw(number_length) << frame_index << (suffix == "/" ? "" : suffix); 96 | std::string s(filename.str()); 97 | return s; 98 | } 99 | 100 | bool DatasetLoader_3RScan::IsV2() const { 101 | return m_dataset->rotate_pose_img; 102 | } 103 | 104 | bool DatasetLoader_3RScan::Retrieve() { 105 | std::string depthFilename = GetFileName(m_dataset->folder, 106 | m_dataset->folder_depth, 107 | m_dataset->prefix_depth, 108 | m_dataset->suffix_depth, 109 | m_dataset->number_length); 110 | std::string colorFilename = GetFileName(m_dataset->folder, 111 | m_dataset->folder_rgb, 112 | m_dataset->prefix_rgb, 113 | m_dataset->suffix_rgb, 114 | m_dataset->number_length); 115 | pose_file_name_ = GetFileName(m_dataset->folder, 116 | m_dataset->folder_pose, 117 | m_dataset->prefix_pose, 118 | m_dataset->suffix_pose, 119 | m_dataset->number_length); 120 | bool isExist = isFileExist(depthFilename); 121 | if (!isExist) { 122 | frame_index = 0; 123 | SCLOG(VERBOSE) << "Cannot find path:\n" << depthFilename << "\n" << colorFilename; 124 | 125 | return false; 126 | } 127 | m_d = cv::imread(depthFilename, -1); 128 | // mask depth 129 | for(size_t c=0;c<(size_t)m_d.cols;++c){ 130 | for(size_t r=0;r<(size_t)m_d.rows;++r){ 131 | if(m_d.at(r,c)>=m_dataset->max_depth) 132 | m_d.at(r,c) = 0; 133 | } 134 | } 135 | 136 | if (isFileExist(colorFilename.c_str())) { 137 | m_rgb = cv::imread(colorFilename, -1); 138 | } 139 | if (m_dataset->rotate_pose_img) { 140 | cv::rotate(m_d, m_d, cv::ROTATE_90_COUNTERCLOCKWISE); 141 | } 142 | LoadPose(m_pose, pose_file_name_,m_dataset->rotate_pose_img); 143 | m_pose = m_poseTransform * m_pose; 144 | frame_index += m_dataset->frame_index_counter; 145 | return true; 146 | } 147 | 148 | 149 | 150 | void DatasetLoader_3RScan::Reset() { 151 | frame_index = 0; 152 | } 153 | 154 | Eigen::Matrix DatasetLoader_3RScan::rotation_matrix_Z(const float rot) { 155 | Eigen::Matrix res = Eigen::Matrix::Identity(); 156 | res << cos(rot), -sin(rot), 0, 0, 157 | sin(rot), cos(rot), 0, 0, 158 | 0, 0, 1, 0, 159 | 0, 0, 0, 1; 160 | return res; 161 | } -------------------------------------------------------------------------------- /libGraphSLAM/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | SET(targetname libGraphSLAM) 2 | 3 | INCLUDE(UseEigen3) 4 | INCLUDE(UseOpenCV) 5 | 6 | if(APPLE) 7 | set(INSEG_LIB ${PROJECT_SOURCE_DIR}/lib/libInSegLib_osx.a) 8 | else() 9 | set(INSEG_LIB ${PROJECT_SOURCE_DIR}/lib/libInSegLib_linux.a) 10 | endif() 11 | 12 | FILE(GLOB sources "*.cpp") 13 | FILE(GLOB headers "*.h") 14 | IF(BUILD_GRAPHPRED) 15 | INCLUDE(UseOnnxRuntime) 16 | FILE(GLOB src_gp "graphPredictor/*.cpp") 17 | FILE(GLOB h_gp "graphPredictor/*.h") 18 | SET(sources ${sources} ${src_gp}) 19 | SET(headers ${headers} ${h_gp}) 20 | ENDIF(BUILD_GRAPHPRED) 21 | 22 | 23 | 24 | ADD_LIBRARY(${targetname} ${sources} ${headers}) 25 | TARGET_LINK_LIBRARIES(${targetname} 26 | PUBLIC ${INSEG_LIB} 27 | ) 28 | TARGET_INCLUDE_DIRECTORIES(${targetname} 29 | PUBLIC ../lib) 30 | IF(BUILD_GRAPHPRED) 31 | INCLUDE(LinkOnnxRuntime) 32 | target_compile_definitions(${targetname} PUBLIC COMPILE_WITH_GRAPHPRED) 33 | ENDIF(BUILD_GRAPHPRED) 34 | INCLUDE(Linktinyply) 35 | INCLUDE(LinkJson) 36 | -------------------------------------------------------------------------------- /libGraphSLAM/GraphSLAM.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/21/20. 3 | // 4 | 5 | #ifndef GRAPHSLAM_GRAPHSLAM_H 6 | #define GRAPHSLAM_GRAPHSLAM_H 7 | #include "config.h" 8 | #include "disjSet.h" 9 | #include "graph.h" 10 | #include "../Objects/Camera/CameraParameters.h" 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #ifdef COMPILE_WITH_GRAPHPRED 18 | #include "graphPredictor/GraphPredictor.h" 19 | #else 20 | #include 21 | #endif 22 | namespace PSLAM { 23 | class GraphSLAM { 24 | public: 25 | explicit GraphSLAM(ConfigPSLAM *config, const CameraParameters &camParamD); 26 | ~GraphSLAM(); 27 | 28 | ////////// 29 | /// Frameprocessing 30 | ////////// 31 | bool Initialize(const CameraParameters &camParamD); 32 | 33 | void ProcessFrame(int idx, const cv::Mat &colorImage, const cv::Mat &depthImage, const Eigen::Matrix4f *pose); 34 | 35 | bool &UseThread(){return mConfig->use_thread;} 36 | 37 | ////////// 38 | /// Access 39 | ////////// 40 | inseg_lib::InSegLib *GetInSeg() { return inseg_.get(); } 41 | 42 | Graph *GetGraph() { return mGraph.get(); } 43 | 44 | ConfigPSLAM *GetConfig(){return mConfig;} 45 | 46 | 47 | #ifdef COMPILE_WITH_GRAPHPRED 48 | GraphPredictor *GetGraphPred() { return mpGraphPredictor.get(); } 49 | #endif 50 | 51 | #ifdef COMPILE_WITH_JSON 52 | json11::Json GetSceneGraph(bool full); 53 | #endif 54 | 55 | ////////// 56 | /// IO 57 | ////////// 58 | void SaveModel(const std::string &output_folder) const; 59 | 60 | void SaveGraph(const std::string &output_folder, bool fullProb); 61 | 62 | void SaveSurfelsToPLY(int segment_filter, const std::string &output_folder, const std::string &output_name, bool binary); 63 | 64 | enum SAVECOLORMODE { 65 | SAVECOLORMODE_RGB, SAVECOLORMODE_SEGMENT, SAVECOLORMODE_INSTANCE, SAVECOLORMODE_SEMANTIC, SAVECOLORMODEL_PANOPTIC 66 | }; 67 | 68 | void SaveNodesToPLY(int segment_filter, const std::string &output_folder, SAVECOLORMODE saveColorMode, bool binary=false); 69 | 70 | void SaveSurfelsToPLY(const std::string &output_folder, const std::string &output_name, 71 | const std::vector> &surfels, bool binary); 72 | 73 | ////////// 74 | /// Graph 75 | ////////// 76 | void RunFullPrediction(); 77 | /// Start backend 78 | void Start(); 79 | /// Stop backend 80 | void Stop(); 81 | 82 | void AddSelectedNodeToUpdate(int idx); 83 | 84 | bool LoadPredictModel(); 85 | 86 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 87 | std::set mLastUpdatedSegments{}; 88 | private: 89 | // system 90 | size_t mTimeStamp=0; 91 | bool mbInitMap = false; 92 | 93 | Eigen::Matrix4f pose_; 94 | std::vector> trajectory_{}; 95 | 96 | // Inseg Standard configuration. 97 | std::shared_ptr inseg_; 98 | 99 | // Graph 100 | ConfigPSLAM *mConfig; 101 | std::shared_ptr mGraph; 102 | #ifdef COMPILE_WITH_GRAPHPRED 103 | GraphPredictorPtr mpGraphPredictor; 104 | #endif 105 | 106 | std::vector> GetUpdatedSurfels(); 107 | 108 | std::vector> FilterSegment( 109 | int segment_filter, const std::vector> &surfels); 110 | 111 | 112 | 113 | }; 114 | } 115 | 116 | #endif //GRAPHSLAM_GRAPHSLAM_H 117 | -------------------------------------------------------------------------------- /libGraphSLAM/config.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 11/4/20. 3 | // 4 | #include "config.h" 5 | using namespace PSLAM; 6 | ConfigPSLAM::ConfigPSLAM(const std::string &path){ 7 | if(!path.empty()) pth=path; 8 | std::fstream f(pth,std::ios::in); 9 | if(!f.is_open())return; 10 | } 11 | 12 | ConfigPSLAM::~ConfigPSLAM(){} 13 | 14 | ConfigPSLAM::ConfigPSLAM() { 15 | use_thread = true; 16 | 17 | use_fusion = true; 18 | 19 | /// Use graph predict or not 20 | graph_predict = true; 21 | 22 | /// model path 23 | pth_model = "/home/sc/research/PersistentSLAM/python/3DSSG/experiments/exp10_2_1/traced/"; 24 | pth_model = "/home/sc/research/PersistentSLAM/python/3DSSG/experiments/exp10_4/traced/"; 25 | pth_model = "/home/sc/research/PersistentSLAM/python/3DSSG/experiments/exp10_5/traced/"; 26 | 27 | filter_num_node = 512; 28 | 29 | n_pts = 512; 30 | 31 | neighbor_margin=500; // mm 32 | 33 | /// Only update the information of a node if the node has size change to N percent 34 | update_thres_node_size = 0.2; 35 | /// Update a node if its time stamp is older than n 36 | update_thres_time_stamp = 50; 37 | 38 | 39 | pth = "./config_graph.txt"; 40 | } -------------------------------------------------------------------------------- /libGraphSLAM/config.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 10/9/20. 3 | // 4 | 5 | #ifndef GRAPHSLAM_CONFIG_H 6 | #define GRAPHSLAM_CONFIG_H 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | //#include 14 | namespace PSLAM { 15 | struct ConfigPSLAM { 16 | ConfigPSLAM(); 17 | explicit ConfigPSLAM(const std::string &path); 18 | ~ConfigPSLAM(); 19 | bool use_thread; 20 | 21 | bool use_fusion; 22 | 23 | /// Use graph predict or not 24 | bool graph_predict; 25 | 26 | /// model path 27 | std::string pth_model; 28 | 29 | int filter_num_node; 30 | 31 | // int predict_when_have_at_least_n_node; 32 | size_t n_pts; 33 | 34 | float neighbor_margin; // mm 35 | 36 | /// Only update the information of a node if the node has size change to N percent 37 | float update_thres_node_size; 38 | /// Update a node if its time stamp is older than n 39 | int update_thres_time_stamp; 40 | 41 | std::string pth; 42 | 43 | 44 | inseg_lib::InSegConfig inseg_config; 45 | inseg_lib::MainConfig main_config; 46 | inseg_lib::MapConfig map_config; 47 | inseg_lib::SegmentationConfig segmentation_config; 48 | }; 49 | 50 | 51 | } 52 | 53 | #endif //GRAPHSLAM_CONFIG_H 54 | -------------------------------------------------------------------------------- /libGraphSLAM/declaration.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace PSLAM { 4 | class Node; 5 | } 6 | namespace PSLAM { 7 | class Edge; 8 | } 9 | 10 | #include "node.h" 11 | #include "edge.h" -------------------------------------------------------------------------------- /libGraphSLAM/edge.cpp: -------------------------------------------------------------------------------- 1 | #include "edge.h" 2 | using namespace PSLAM; 3 | 4 | Edge::Edge(Edge &edge) { 5 | this->nodeFrom = edge.nodeFrom; 6 | this->nodeTo = edge.nodeTo; 7 | this->labelProp = edge.labelProp; 8 | this->mClsWeight = edge.mClsWeight; 9 | #ifdef COMPILE_WITH_GRAPHPRED 10 | this->mFeatures = edge.mFeatures; 11 | #endif 12 | } 13 | 14 | std::string Edge::GetLabel() const { 15 | std::unique_lock lock(mMutLabel); 16 | return label; 17 | } 18 | 19 | void Edge::UpdatePrediction(const std::map &prop, bool fusion){ 20 | std::unique_lock lock(mMutex); 21 | if(prop.empty()) return; 22 | 23 | for (const auto &pair:prop) { 24 | const auto &name = pair.first; 25 | auto new_v = pair.second; 26 | if (labelProp.find(name) == labelProp.end()) { 27 | labelProp.insert({name, new_v}); 28 | mClsWeight.insert({name, 1}); 29 | } else { 30 | if(fusion) { 31 | const static float max_w = 100; 32 | const static float new_w = 1; 33 | const auto old_v = labelProp.at(name); 34 | const auto old_w = mClsWeight.at(name); 35 | labelProp.at(name) = (old_v * old_w + new_v * new_w) / float(old_w + new_w); 36 | mClsWeight.at(name) = (old_w + new_w); 37 | mClsWeight.at(name) = std::min(max_w, mClsWeight.at(name)); 38 | } else { 39 | labelProp.at(name) = new_v; 40 | } 41 | } 42 | } 43 | 44 | if (labelProp.empty())return; 45 | float max_prop = labelProp.begin()->second; 46 | std::string ll; 47 | for(const auto &pair:labelProp){ 48 | if(pair.second>=max_prop){ 49 | max_prop = pair.second; 50 | ll = pair.first; 51 | } 52 | } 53 | std::unique_lock lock_label(mMutLabel); 54 | label = ll; 55 | } 56 | -------------------------------------------------------------------------------- /libGraphSLAM/edge.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifdef COMPILE_WITH_GRAPHPRED 3 | #include "graphPredictor/MemoryBlock.h" 4 | #endif 5 | #include 6 | #include 7 | #include 8 | namespace PSLAM { 9 | class Edge { 10 | public: 11 | inline static std::string None() {return "none";} 12 | inline static std::string Same() {return "same part";} 13 | Edge() = default; 14 | Edge(Edge &edge); 15 | 16 | int nodeFrom{}, nodeTo{}; 17 | /// Shared 18 | std::map labelProp; 19 | std::map mClsWeight; 20 | /// Thread_SG 21 | #ifdef COMPILE_WITH_GRAPHPRED 22 | std::map> mFeatures; 23 | #endif 24 | mutable std::mutex mMutex, mMutLabel; 25 | 26 | std::string GetLabel() const; 27 | void UpdatePrediction(const std::map &prop, bool fusion); 28 | 29 | std::string label = None(); 30 | }; 31 | typedef std::shared_ptr EdgePtr; 32 | static inline EdgePtr MakeEdgePtr(){return std::make_shared();} 33 | } 34 | -------------------------------------------------------------------------------- /libGraphSLAM/graph.h: -------------------------------------------------------------------------------- 1 | #ifndef _H_INC_INSEG_MAP_SEGMENTS_IMPL_ 2 | #define _H_INC_INSEG_MAP_SEGMENTS_IMPL_ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "../libGraphSLAM/config.h" 8 | #include 9 | #include 10 | #include "node.h" 11 | #include "edge.h" 12 | #include 13 | #include 14 | 15 | namespace PSLAM { 16 | 17 | class Graph: public inseg_lib::SegmentsInterface { 18 | public: 19 | ~Graph() {} 20 | Graph(const ConfigPSLAM *configPslam, bool useThread); 21 | 22 | int Add(const SurfelPtr &surfel) override; 23 | /// Merge segment idx_from to idx_to 24 | void Merge(const int seg_idx_to, const int seg_idx_from) override; 25 | void Clear() override; 26 | void Update(const SurfelPtr surfel, 27 | const Eigen::Vector3f& pos, 28 | const Eigen::Vector3f& normal) override; 29 | void CheckConnectivity(float margin); 30 | /** 31 | * Update the label of a surfel 32 | * Remove surfel from node[surfel->label_old], chagne surfel->label to label and add it to node[label] 33 | * @param surfel target surfel 34 | * @param label new label 35 | * @return the index of the surfel in the graph 36 | */ 37 | int UpdateLabel(const SurfelPtr surfel, const int label) override; 38 | int AddEdge(int from, int to, const std::shared_ptr &edge); 39 | int AddEdge(const std::shared_ptr &edge); 40 | 41 | void 42 | UpdateSelectedNodes(const std::unordered_set &filtered_selected_nodes, const size_t time, const bool force); 43 | 44 | void Wait(); 45 | 46 | // === 47 | // thread 48 | // === 49 | std::map nodes; 50 | std::map, EdgePtr > edges; 51 | std::set nodes_to_update; 52 | std::mutex mMutNode; // when access nodes 53 | std::mutex mMutEdge; // when access edges 54 | std::mutex mMutThis; 55 | private: 56 | const ConfigPSLAM *mConfigPslam; 57 | void RemoveSurfelFromNode(const SurfelPtr &surfel); 58 | void RemoveNode(int idx); 59 | void RemoveEdge(const std::pair &pair); 60 | bool mbThread; 61 | 62 | std::unique_ptr mPools; 63 | }; 64 | 65 | } // namespace inseg_lib 66 | 67 | #endif // _H_INC_INSEG_MAP_SEGMENTS_IMPL_ 68 | -------------------------------------------------------------------------------- /libGraphSLAM/graphPredictor/EatGCN.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 10/15/20. 3 | // 4 | 5 | #ifndef GRAPHSLAM_EATGCN_H 6 | #define GRAPHSLAM_EATGCN_H 7 | 8 | #include "OnnxModelBase.h" 9 | #include "ParamLoader.h" 10 | #include "MemoryBlock.h" 11 | #include "UtilDataProcessing.h" 12 | #include 13 | 14 | namespace PSLAM { 15 | class EatGCN : private PSLAM::ONNX::OnnxModelBase { 16 | public: 17 | using PSLAM::ParamLoader::mModelParams; 18 | using PSLAM::ParamLoader::mParams; 19 | using PSLAM::ParamLoader::mLabels; 20 | using PSLAM::ParamLoader::mRelationships; 21 | 22 | enum OP_NAME { 23 | OP_ENC_OBJ, OP_ENC_REL, OP_CLS_OBJ, OP_CLS_REL 24 | }; 25 | enum GCN_OP_NAME { 26 | OP_GCN_ATTEN, OP_GCN_PROP 27 | }; 28 | 29 | std::map mOp2Name; 30 | std::map mGOp2Name; 31 | 32 | EatGCN(std::string path, Ort::MemoryInfo *memoryInfo, bool verbose=false); 33 | 34 | std::vector ComputeGCN(GCN_OP_NAME name, size_t level, const std::vector &data, 35 | const std::vector> &dims); 36 | 37 | std::vector ComputeGCN(GCN_OP_NAME name, size_t level, const std::vector &inputs); 38 | 39 | std::vector 40 | Compute(OP_NAME name, const std::vector &data, const std::vector> &dims); 41 | 42 | std::vector Compute(OP_NAME name, std::vector &inputs); 43 | 44 | std::tuple Run( 45 | const MemoryBlock3D &input_nodes, 46 | const MemoryBlock2D &descriptor, 47 | const MemoryBlock2D &edge_index); 48 | 49 | protected: 50 | using PSLAM::ONNX::OnnxModelBase::Run; 51 | private: 52 | bool bVerbose=false; 53 | static MemoryBlock2D ConcatObjFeature(size_t n_node, size_t dim_obj_feature, Ort::Value &obj_feature, 54 | const MemoryBlock2D &descriptor); 55 | 56 | void InitModel() override; 57 | }; 58 | } 59 | 60 | #endif //GRAPHSLAM_EATGCN_H 61 | -------------------------------------------------------------------------------- /libGraphSLAM/graphPredictor/GraphPredictor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/24/20. 3 | // 4 | 5 | #pragma once 6 | //#include "PointNetGCN.h" 7 | #include 8 | #include 9 | #include 10 | #include "../node.h" 11 | #include "../edge.h" 12 | //#include "../nodeProperties.h" 13 | #include "../graph.h" 14 | #include "../config.h" 15 | #include 16 | #include "EatGCN.h" 17 | #include 18 | 19 | namespace PSLAM { 20 | #ifdef COMPILE_WITH_ONNX 21 | typedef std::tuple,std::map> > GCNINPUTDATA; 22 | class GraphPredictor { 23 | public: 24 | typedef std::tuple< std::map>, std::vector,std::map> > Prediction; 25 | 26 | explicit GraphPredictor( const ConfigPSLAM *configPslam); 27 | ~GraphPredictor(); 28 | // std::tuple,std::vector> Predict(std::vector> &surfels); 29 | void RUN(Graph *graph); 30 | 31 | void Stop(); 32 | 33 | void SetThread(bool option); 34 | 35 | /// Check if thread is dead 36 | bool Pin(); 37 | 38 | bool Updated(); 39 | 40 | void SetUpdate(bool option); 41 | 42 | std::shared_ptr Predict( 43 | const std::shared_ptr& data); 44 | 45 | const std::map& GetParams(); 46 | 47 | std::map>> GetTimes(); 48 | 49 | bool mbVerbose=false; 50 | std::map mLabels, mRelationships; 51 | std::map mLabelsName2Idx,mRelationshipsName2Idx; 52 | private: 53 | const ConfigPSLAM *mConfigPslam; 54 | std::unique_ptr mMemoryInfo; 55 | std::unique_ptr mEatGCN; 56 | Graph *mpGraph = nullptr; 57 | std::future mThread; 58 | bool mbShouldStop = false; 59 | bool mbThread = true; 60 | bool source_to_target; 61 | bool mDebug = false; 62 | std::atomic_bool mUpdated; 63 | void Process_IMPL(); 64 | void UpdateNodeFeature(const std::map &selected_nodes); 65 | void UpdateEdgeFeature(const std::map &selected_nodes); 66 | void UpdateGCNFeature(const std::map &selected_nodes, size_t level); 67 | void UpdatePrediction(const std::map &selected_nodes, 68 | const std::map> &sizeAndEdge); 69 | void UpdateShapeFeature(const std::vector &selected_nodes); 70 | 71 | std::map getNeighbor(const std::map &vNodes, size_t min_pts); 72 | 73 | std::map>> mTimes; 74 | }; 75 | #else 76 | class GraphPredictor { 77 | public: 78 | public: 79 | explicit GraphPredictor(const std::string &path_onnx){}; 80 | 81 | std::tuple,std::vector> Predict(std::vector> &surfels){}; 82 | 83 | bool mbVerbose=true; 84 | std::map mLabels, mRelationships; 85 | private: 86 | }; 87 | #endif 88 | typedef std::shared_ptr GraphPredictorPtr; 89 | static inline GraphPredictorPtr MakeGraphPredictor(const ConfigPSLAM *configPslam) {return std::make_shared(configPslam);} 90 | } -------------------------------------------------------------------------------- /libGraphSLAM/graphPredictor/OnnxModelBase.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 10/9/20. 3 | // 4 | 5 | #ifndef GRAPHSLAM_ONNXMODELBASE_H 6 | #define GRAPHSLAM_ONNXMODELBASE_H 7 | #include "ParamLoader.h" 8 | #include 9 | namespace PSLAM { 10 | namespace ONNX { 11 | template 12 | static size_t GetTotalSizeFromVector(const std::vector &vec){ 13 | size_t size = 1; 14 | for (auto &v:vec) 15 | size*=v; 16 | return size; 17 | } 18 | 19 | template 20 | static inline Ort::Value CreateTensor(const Ort::MemoryInfo *memoryInfo, T* data, const std::vector &dims ) { 21 | return Ort::Value::CreateTensor(*memoryInfo, 22 | data, GetTotalSizeFromVector(dims), dims.data(), dims.size()); 23 | } 24 | static inline void PrintTensorShape(const std::string &name, Ort::Value &t){ 25 | { 26 | std::stringstream ss; 27 | for(auto& v : t.GetTensorTypeAndShapeInfo().GetShape()) 28 | ss << v << ", "; 29 | printf("%s shape: %s\n", name.c_str(),ss.str().c_str()); 30 | } 31 | }; 32 | 33 | template 34 | static void PrintVector(const std::string &name, const T * data, const std::vector &dims){ 35 | assert(dims.size()==2); 36 | std::stringstream ss; 37 | ss.precision(3); 38 | ss << "==" << name << "==\n"; 39 | size_t elem_to_show_per_row = 3; 40 | for(unsigned int i=0;i 10) { 42 | if(i==3) { 43 | ss << "...\n"; 44 | continue; 45 | } 46 | if(i>3 && i < dims.at(0)-3) continue; 47 | } 48 | if(dims.at(1)>7){ 49 | for(size_t j=0;j pOrtEnv; 75 | std::map> mSessions; 76 | 77 | /** 78 | * Create Ort model by loop over the mModelParams. 79 | */ 80 | virtual void InitModel(){ 81 | pOrtEnv = std::make_unique(ORT_LOGGING_LEVEL_WARNING, "test"); 82 | Ort::SessionOptions sessionOptions; 83 | sessionOptions.SetIntraOpNumThreads(1); 84 | sessionOptions.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL); 85 | sessionOptions.SetExecutionMode(ExecutionMode::ORT_SEQUENTIAL); 86 | for(auto &pair:mModelParams) { 87 | mSessions[pair.first] = std::make_unique< Ort::Session>(*pOrtEnv, (pth_base + pair.second->model_path).c_str(), sessionOptions); 88 | } 89 | } 90 | 91 | template 92 | size_t GetTotalSizeFromVector(const std::vector &vec){ 93 | size_t size = 1; 94 | for (auto &v:vec) 95 | size*=v; 96 | return size; 97 | } 98 | 99 | inline std::vector Run(const std::string &name, const std::vector &input_tensor){ 100 | return mSessions.at(name)->Run(Ort::RunOptions{nullptr}, 101 | mModelParams.at(name)->inputName.data(), input_tensor.data(), mModelParams.at(name)->inputName.size(), 102 | mModelParams.at(name)->outputName.data(), mModelParams.at(name)->outputName.size()); 103 | } 104 | 105 | template 106 | inline Ort::Value CreateTensor(T* data, const std::vector &dims ) { 107 | return Ort::Value::CreateTensor(*mMemoryInfo, 108 | data, GetTotalSizeFromVector(dims), dims.data(), dims.size()); 109 | } 110 | 111 | static void PrintTensorShape(Ort::Value &t, const std::string &name){ 112 | { 113 | std::stringstream ss; 114 | for(auto& v : t.GetTensorTypeAndShapeInfo().GetShape()) 115 | ss << v << ", "; 116 | std::cerr << name << " shape: " << ss.str(); 117 | } 118 | }; 119 | template 120 | static void PrintVector(const std::string &name, const T * data, const std::vector &dims){ 121 | assert(dims.size()==2); 122 | std::cerr << "=="< 10) { 126 | if(i==3) { 127 | std::cerr << "...\n"; 128 | continue; 129 | } 130 | if(i>3 && i < (size_t)dims.at(0)-3) continue; 131 | } 132 | if(dims.at(1)>7){ 133 | for(size_t j=0;j 8 | #include 9 | #include 10 | #include 11 | namespace PSLAM { 12 | struct ONNX_MODEL_PARAMS { 13 | std::string model_path; 14 | std::vector inputName_;// this is to store the memory location. the onnx needs const char* format. a direct store of const char * in a vector is problematic. 15 | std::vector outputName_; 16 | std::vector inputName; 17 | std::vector outputName; 18 | ONNX_MODEL_PARAMS(){} 19 | ONNX_MODEL_PARAMS(std::string name, std::vector in, std::vector out){ 20 | Set(std::move(name),std::move(in),std::move(out)); 21 | } 22 | void Set(std::string name, std::vector in, std::vector out){ 23 | model_path = std::move(name); 24 | inputName_ = std::move(in); 25 | outputName_ = std::move(out); 26 | for(const auto& s : inputName_) { 27 | inputName.push_back(&s[0]); 28 | } 29 | 30 | for(const auto& s : outputName_) { 31 | char *buff = new char[s.size() + 1]; // allocate memory 32 | std::strncpy(buff, s.c_str(), s.size() + 1); // copy data to memory 33 | outputName.push_back(buff); 34 | } 35 | } 36 | ~ONNX_MODEL_PARAMS(){} 37 | }; 38 | 39 | class ParamLoader { 40 | public: 41 | std::string pth_base; 42 | ParamLoader(std::string path, std::string name_args = "args.json"):pth_base(path){ 43 | const std::string pth_onnx = pth_base + name_args; 44 | auto json = ORUtils::JsonUtils::LoadJson(pth_onnx); 45 | LoadArgs(mParams, mModelParams, json); 46 | 47 | /// Read labels 48 | { 49 | std::fstream file(path+"/"+"classes.txt", std::ios::in); 50 | if(!file.is_open())SCLOG(ERROR) << "Cannot open class file at path " << path+"/"+"classes.txt"; 51 | std::string line; 52 | std::vector labels; 53 | while (std::getline(file, line)) labels.push_back(line); 54 | 55 | SCLOG(VERBOSE) << "Labels"; 56 | for(size_t i=0;i labels; 66 | while (std::getline(file, line)) labels.push_back(line); 67 | SCLOG(VERBOSE) << "Relationships"; 68 | for(size_t i=0;i> mModelParams; 75 | std::map mParams; 76 | std::map mLabels, mRelationships; 77 | private: 78 | static void LoadArgs(std::map &otherParams, 79 | std::map> &modelParams, const json11::Json &json, bool verbose=false) { 80 | for (auto &object:json.object_items()){ 81 | const auto &name = object.first; 82 | if (name.find("model_") != std::string::npos && object.second.is_object()) { 83 | auto t_name = name.substr(6, name.length()); 84 | if (verbose) printf("name: %s, items: %zu\n", t_name.c_str(), object.second.object_items().size()); 85 | auto path = object.second.object_items().at("path").string_value(); 86 | std::vector input_names, output_names; 87 | for (const auto &name_ : object.second.object_items().at("input").array_items()) 88 | input_names.push_back(name_.string_value()); 89 | for (const auto &name_ : object.second.object_items().at("output").array_items()) 90 | output_names.push_back(name_.string_value()); 91 | modelParams[t_name].reset(new ONNX_MODEL_PARAMS(path, input_names, output_names)); 92 | 93 | if (verbose) 94 | for (const auto &m_obj : object.second.object_items()) { 95 | std::cout << m_obj.first << ": "; 96 | if (m_obj.second.is_string()) 97 | std::cout << m_obj.second.string_value() << " "; 98 | if (m_obj.second.is_array()) { 99 | for (auto &v : m_obj.second.array_items()) { 100 | std::cout << v.string_value() << " "; 101 | } 102 | } 103 | std::cout << "\n"; 104 | } 105 | } else if (object.second.is_bool() || object.second.is_string() || object.second.is_number()) { 106 | otherParams[name] = object.second; 107 | } else if (object.second.is_object()) 108 | LoadArgs(otherParams, modelParams, object.second); 109 | } 110 | } 111 | }; 112 | } 113 | 114 | #endif //GRAPHSLAM_PARAMLOADER_H 115 | -------------------------------------------------------------------------------- /libGraphSLAM/graphPredictor/UtilInSegPlyLoader.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 10/9/20. 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "MemoryBlock.h" 11 | 12 | #ifndef GRAPHSLAM_UTILINSEGPLYLOADER_H 13 | #define GRAPHSLAM_UTILINSEGPLYLOADER_H 14 | namespace Util { 15 | void print_headers(const tinyply::PlyFile &file){ 16 | for (const auto &e : file.get_elements()) { 17 | std::cout << "\t[ply_header] element: " << e.name << " (" << e.size << ")" << std::endl; 18 | for (const auto &p : e.properties) { 19 | std::cout << "\t[ply_header] \tproperty: " << p.name << " (type=" 20 | << tinyply::PropertyTable[p.propertyType].str << ")"; 21 | if (p.isList) std::cout << " (list_type=" << tinyply::PropertyTable[p.listType].str << ")"; 22 | std::cout << std::endl; 23 | } 24 | } 25 | } 26 | } 27 | #endif //GRAPHSLAM_UTILINSEGPLYLOADER_H 28 | -------------------------------------------------------------------------------- /libGraphSLAM/graphPredictor/UtilMath.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 10/9/20. 3 | // 4 | 5 | #ifndef GRAPHSLAM_UTILMATH_H 6 | #define GRAPHSLAM_UTILMATH_H 7 | #include 8 | #include "MemoryBlock.h" 9 | 10 | namespace Math { 11 | static ORUtils::Vector3 sum(const ORUtils::Vector3 *data, size_t size) { 12 | ORUtils::Vector3 sum(0.f); 13 | for (size_t i = 0; i < size; ++i) 14 | sum += data[i]; 15 | return sum; 16 | } 17 | 18 | static MemoryBlock sum(const MemoryBlock &data, size_t size, size_t stride, size_t dim) { 19 | MemoryBlock sum(data.type(), dim); 20 | sum.Clear(); 21 | for (size_t i = 0; i < size; ++i) { 22 | for (size_t j = 0; j < dim; ++j) { 23 | sum.at(j) += data.Get()[i * stride + j]; 24 | } 25 | } 26 | return sum; 27 | } 28 | 29 | static ORUtils::Vector3 mean(const ORUtils::Vector3 *data, size_t size) { 30 | return sum(data, size) / size; 31 | } 32 | 33 | static MemoryBlock mean(const MemoryBlock &data, size_t size, size_t stride, size_t dim) { 34 | auto result = sum(data, size, stride, dim); 35 | for (size_t i = 0; i < dim; ++i) result.at(i) /= size; 36 | return result; 37 | } 38 | 39 | static MemoryBlock mean(const MemoryBlock2D &data, size_t dim = 3) { 40 | auto result = sum(data, data.mDims.x, data.mDims.y, dim); 41 | for (size_t i = 0; i < dim; ++i) result.at(i) /= data.mDims.x; 42 | return result; 43 | } 44 | 45 | static ORUtils::Vector3 stddev(const ORUtils::Vector3 *data, size_t size) { 46 | auto m = mean(data, size); 47 | ORUtils::Vector3 std(0.f); 48 | for (size_t i = 0; i < size; ++i) 49 | for (size_t j = 0; j < 3; ++j) 50 | std[j] += pow(data[i][j] - m[j], 2); 51 | for (size_t j = 0; j < 3; ++j) 52 | std[j] = sqrt(std[j] / size); 53 | return std; 54 | } 55 | 56 | static MemoryBlock stddev(const MemoryBlock &data, size_t size, size_t stride, size_t dim) { 57 | auto m = mean(data, size, stride, dim); 58 | MemoryBlock result(data.type(), dim); 59 | result.Clear(); 60 | for (size_t i = 0; i < size; ++i) 61 | for (size_t j = 0; j < dim; ++j) 62 | result.at(j) += pow(data.at(i * stride + j) - m.at(j), 2); 63 | for (size_t j = 0; j < dim; ++j) 64 | result.at(j) = sqrt(result.at(j) / size); 65 | return result; 66 | } 67 | 68 | static void normalize(ORUtils::Vector3 *data, size_t size) { 69 | auto centroid = mean(data, size); 70 | float max_dist = 0; 71 | for (size_t i = 0; i < size; ++i) { 72 | data[i] -= centroid; 73 | float dist = 0; 74 | for (size_t j = 0; j < 3; ++j) 75 | dist += pow(data[i][j], 2); 76 | dist = sqrt(dist); 77 | if (dist > max_dist)max_dist = dist; 78 | } 79 | for (size_t i = 0; i < size; ++i) data[i] /= max_dist; 80 | } 81 | 82 | static void normalize(MemoryBlock2D &data, size_t dim) { 83 | auto centroid = mean(data, dim); 84 | float max_dist = 0; 85 | for (size_t i = 0; i < data.mDims.x; ++i) { 86 | for (size_t j = 0; j < dim; ++j) { 87 | data.at(i, j) -= centroid.at(j); 88 | } 89 | float dist = 0; 90 | for (size_t j = 0; j < dim; ++j) 91 | dist += pow(data.at(i, j), 2); 92 | if (dist > max_dist)max_dist = dist; 93 | } 94 | max_dist = sqrt(max_dist); 95 | for (size_t i = 0; i < data.mDims.x; ++i) 96 | for (size_t j = 0; j < dim; ++j) data.at(i, j) /= max_dist; 97 | } 98 | } 99 | 100 | #endif //GRAPHSLAM_UTILMATH_H 101 | -------------------------------------------------------------------------------- /libGraphSLAM/node.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "declaration.h" 3 | #ifdef COMPILE_WITH_GRAPHPRED 4 | #include "graphPredictor/MemoryBlock.h" 5 | #endif 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace PSLAM { 18 | class Node { 19 | typedef std::unique_lock Lock; 20 | public: 21 | inline static std::string Unknown() { return "unknown"; }; 22 | 23 | Node(int label); 24 | 25 | /// Thread 26 | int Add(const SurfelPtr &surfel); 27 | 28 | void Remove(const int index_in_graph_old); 29 | 30 | void Update(const int label, 31 | const int index, 32 | const Eigen::Vector3f &pos, 33 | const Eigen::Vector3f &normal, 34 | const std::array &color); 35 | 36 | void RemoveEdge(const EdgePtr &edge); 37 | 38 | void UpdatePrediction(const std::map &pd, 39 | const std::map> &sizeAndEdge, bool fusion); 40 | 41 | bool CheckConnectivity(Node *nodeP, float margin, bool modify); 42 | 43 | const std::string GetLabel() const; 44 | 45 | void UpdateSelectedNode(const size_t time, const size_t filter_size, const size_t num_pts, bool force); 46 | 47 | int GetPointSize(); 48 | public: 49 | std::unordered_map surfels; 50 | std::unordered_set edges; 51 | /// The idx of this node (label) 52 | int idx; 53 | std::atomic_int instance_idx; 54 | bool mDebug; 55 | /// The last predicted semantic label of this node 56 | size_t time_stamp; 57 | 58 | // threads 59 | std::vector selected_surfels; 60 | Eigen::Vector3f mCentroid, mStd, mBBox_min, mBBox_max; 61 | size_t lastUpdatePropertySize; 62 | std::atomic_bool mbNeedUpdateNodeFeature; 63 | 64 | Eigen::Vector3f centroid, pos_sum; 65 | Eigen::Vector3f bbox_max, bbox_min; 66 | 67 | /// Shared between 68 | std::unordered_set neighbors; 69 | 70 | /// Only access by thread_gcc 71 | #ifdef COMPILE_WITH_GRAPHPRED 72 | std::map> mFeatures; 73 | #endif 74 | bool bNeedCheckNeighbor = false; 75 | std::mutex mMutNode; 76 | mutable std::mutex mMutSurfel, mMutEdge, mMutNN, mMutSelected; 77 | 78 | /// Prediction related members 79 | // stores the class and its probability 80 | mutable std::mutex mMutPred; 81 | std::map mClsProb; 82 | std::map mClsWeight; 83 | std::map> mSizeAndEdge; 84 | 85 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 86 | private: 87 | /// This contains indices to be reused. 88 | std::queue mqFreeIndices; 89 | /// The unique index for each input surfels. 90 | size_t mIdxCounter; 91 | 92 | /// Prediction related members 93 | mutable std::mutex mMutPDLabel; 94 | std::string last_class_predicted = Unknown(); 95 | }; 96 | typedef std::shared_ptr NodePtr; 97 | static inline NodePtr MakeNodePtr(int label) { return std::make_shared(label); } 98 | } // namespace inseg_lib -------------------------------------------------------------------------------- /libGraphSLAMGUI/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | IF(USE_RENDEREDVIEW) 2 | INCLUDE(UseAssimp) 3 | ENDIF() 4 | 5 | SET(targetname libGraphSLAMGUI) 6 | SET(sources 7 | GraphSLAMGUI.cpp 8 | SurfelDrawer.cpp 9 | ImageDrawer.cpp 10 | CameraDrawer.cpp 11 | TrajectoryDrawer.cpp 12 | ) 13 | SET(headers 14 | GraphSLAMGUI.h 15 | SurfelDrawer.h 16 | ImageDrawer.h 17 | CameraDrawer.h 18 | TrajectoryDrawer.h 19 | ) 20 | FILE(GLOB src_graphDrawer "./graphDrawer/*.cpp") 21 | FILE(GLOB head_graphDrawer "./graphDrawer/*.h") 22 | 23 | INCLUDE(UseOpenCV) 24 | INCLUDE(UseEigen3) 25 | 26 | ADD_LIBRARY(${targetname} 27 | ${sources} ${headers} 28 | ${src_graphDrawer} ${head_graphDrawer} 29 | ) 30 | TARGET_LINK_LIBRARIES(${targetname} 31 | PUBLIC libDataLoader 32 | PUBLIC libGraphSLAM 33 | PUBLIC GUI3D # need to put this the last. otherwise some weird bugs appear. (not able to show text and lines, etc.) 34 | ) 35 | 36 | IF(USE_RENDEREDVIEW) 37 | INCLUDE(LinkAssimp) 38 | ENDIF() -------------------------------------------------------------------------------- /libGraphSLAMGUI/CameraDrawer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/23/20. 3 | // 4 | 5 | #include "CameraDrawer.h" 6 | 7 | using namespace PSLAM; 8 | 9 | CameraDrawer::~CameraDrawer(){ 10 | if(bInited) { 11 | glDeleteVertexArrays(1, &VAO); 12 | glDeleteBuffers(1, &VBO); 13 | glDeleteBuffers(1, &EBO); 14 | } 15 | } 16 | 17 | void CameraDrawer::Init() { 18 | const std::string vsShader = "#version 330\n" 19 | "layout(location = 0) in vec3 position;\n" 20 | "uniform mat4 model;\n" 21 | "uniform mat4 view;\n" 22 | "uniform mat4 projection;\n" 23 | "void main() {\n" 24 | " gl_Position = projection * view * model * vec4(position, 1.0);\n" 25 | "}"; 26 | 27 | const std::string fsShader = "#version 330 core\n" 28 | "out vec4 FragColor;\n" 29 | "uniform vec4 color;\n" 30 | "void main()\n" 31 | "{\n" 32 | " FragColor = color; \n" 33 | "}"; 34 | 35 | unsigned int indices[] = {0, 1, 0, 2, 0, 3, 0, 4, 36 | 1, 2, 2, 3, 3, 4, 4, 1}; 37 | const float size = 0.1f; 38 | float vertices[] = { 0, 0, 0, 39 | -0.5f*size, -0.5f*size, 1*size, 40 | 0.5f*size, -0.5f*size, 1*size, 41 | 0.5f*size, 0.5f*size, 1*size, 42 | -0.5f*size, 0.5f*size, 1*size}; 43 | 44 | // screen quad VAO 45 | unsigned int &quadVAO = VAO; 46 | unsigned int &quadVBO = VBO; 47 | glGenVertexArrays(1, &quadVAO); 48 | glGenBuffers(1, &quadVBO); 49 | glBindVertexArray(quadVAO); 50 | glBindBuffer(GL_ARRAY_BUFFER, quadVBO); 51 | glBufferData(GL_ARRAY_BUFFER, sizeof(indices), &indices, GL_STATIC_DRAW); 52 | glEnableVertexAttribArray(0); 53 | glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void *) 0); 54 | glEnableVertexAttribArray(1); 55 | glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void *) (2 * sizeof(float))); 56 | 57 | 58 | glGenBuffers(1, &VBO); 59 | glGenVertexArrays(1, &VAO); 60 | glGenBuffers(1, &EBO); 61 | glBindVertexArray(VAO); 62 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 63 | glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), &vertices, GL_STATIC_DRAW); 64 | 65 | // Lines 66 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, EBO); 67 | glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(indices), indices, GL_STATIC_DRAW); 68 | 69 | glEnableVertexAttribArray(0); 70 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(float), nullptr); 71 | glBindVertexArray(0); 72 | 73 | 74 | // Init shader 75 | mShader = std::make_unique(vsShader,fsShader); 76 | mShader->use(); 77 | mShader->set("color", color); 78 | mShader->set("model", glm::mat4(1.f)); 79 | bInited=true; 80 | } 81 | 82 | void CameraDrawer::Draw(Eigen::Matrix4f model, 83 | const Eigen::Matrix4f &projection, 84 | const Eigen::Matrix4f &viewMatrix) { 85 | assert(bInited); 86 | model = model * mScale;// glm::scale(model,glm::vec3(scale, scale, scale)); // it's a bit too big for our scene, so scale it down 87 | 88 | mShader->use(); 89 | mShader->set("color", color); 90 | mShader->set("model",model); 91 | mShader->set("view",viewMatrix); 92 | mShader->set("projection",projection); 93 | 94 | glBindVertexArray(VAO); 95 | glLineWidth(3); 96 | glDrawElements(GL_LINES, 16, GL_UNSIGNED_INT, 0); 97 | glLineWidth(1); 98 | } -------------------------------------------------------------------------------- /libGraphSLAMGUI/CameraDrawer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/23/20. 3 | // 4 | 5 | #ifndef GRAPHSLAM_CAMERADRAWER_H 6 | #define GRAPHSLAM_CAMERADRAWER_H 7 | 8 | #include "../libGUI3D/libGUI3D/glShader.hpp" 9 | #include 10 | 11 | namespace PSLAM { 12 | class CameraDrawer{ 13 | public: 14 | CameraDrawer()=default; 15 | ~CameraDrawer(); 16 | void Init(); 17 | void Draw(Eigen::Matrix4f model, const Eigen::Matrix4f& projection, const Eigen::Matrix4f& viewMatrix); 18 | 19 | void SetColor(const Eigen::Vector4f &c) { 20 | this->color = c; 21 | } 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | private: 24 | unsigned int VAO, VBO, EBO; 25 | float mScale = 0.2f; 26 | std::unique_ptr mShader; 27 | bool bInited=false; 28 | Eigen::Vector4f color {0,1,1,1}; 29 | }; 30 | } 31 | 32 | #endif //GRAPHSLAM_CAMERADRAWER_H 33 | -------------------------------------------------------------------------------- /libGraphSLAMGUI/GraphSLAMGUI.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/21/20. 3 | // 4 | 5 | #ifndef GRAPHSLAM_SURFELGUI_H 6 | #define GRAPHSLAM_GRAPHSLAMGUI_H 7 | 8 | #include "../libGUI3D/libGUI3D/GUI3D.h" 9 | #include 10 | #include "graphDrawer/graph_drawer.h" 11 | #include "../libGraphSLAM/GraphSLAM.h" 12 | #include "../Utils/eigen_glm.h" 13 | #include "SurfelDrawer.h" 14 | #include "ImageDrawer.h" 15 | #include "CameraDrawer.h" 16 | #include "TrajectoryDrawer.h" 17 | #include 18 | #include "Label_NYU40.h" 19 | 20 | #ifdef COMPILE_WITH_ASSIMP 21 | #include "../renderer/Renderer.h" 22 | #include "../renderer/RendererFactory.h" 23 | #endif 24 | 25 | namespace PSLAM { 26 | 27 | class GraphSLAMGUI : public SC::GUI3D { 28 | public: 29 | enum DRAWGRAPHTYPE { 30 | DRAWGRAPHTYPE_SEGMENTS, DRAWGRAPHTYPE_INSTANCE 31 | }; 32 | enum COLORTYPE { 33 | COLOR_LABEL,COLOR_PHONG,COLOR_NORMAL,COLOR_COLOR,COLOR_UPDATED,COLOR_SEMANTIC,COLOR_INSTANCE,COLOR_PANOPTIC 34 | }; 35 | enum LABELTYPE { 36 | LABEL_SEGMENT, LABEL_INSTANCE, LABEL_NAME 37 | }; 38 | enum PROCESS { 39 | PROCESS_STOP, PROCESS_ONCE, PROCESS_CONTINUE 40 | }; 41 | enum DRAWEDGETYPE{ 42 | DRAWEDGETYPE_RELATION, DRAWEDGETYPE_NEIGHBOR 43 | }; 44 | 45 | GraphSLAMGUI(GraphSLAM *graphSlam, DatasetLoader_base *dataloader); 46 | 47 | ~GraphSLAMGUI(); 48 | 49 | void drawUI() override; 50 | 51 | void drawGL() override; 52 | 53 | void SetRender(int width, int height, const std::string &path, bool align); 54 | 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | private: 57 | bool bFaceCulling=true; 58 | GraphSLAM *mpGraphSLAM; 59 | DatasetLoader_base *mpDataLoader; 60 | int mProcessMode=PROCESS_STOP; 61 | CameraDrawer mCameraDrawer; 62 | TrajectoryDrawer mTrajectoryDrawer; 63 | int mColorType = COLOR_LABEL; 64 | int mLabelType = LABEL_NAME; 65 | int mEdgeType = DRAWEDGETYPE_RELATION; 66 | int mDrawGraphType = DRAWGRAPHTYPE_SEGMENTS; 67 | int mSelectedNodeIdx = 0; 68 | std::map mEdgeUISwitch; 69 | cv::Mat mRGB, mDepth; 70 | 71 | #ifdef COMPILE_WITH_ASSIMP 72 | std::unique_ptr mMeshRender; 73 | #endif 74 | 75 | GraphDrawer mGraphDrawer; 76 | SurfelDrawer mSurfelDrawer; 77 | 78 | ImageDrawer mImageDrawer[3]; 79 | bool bProcess = true; 80 | bool bNeedUpdate = false; 81 | bool bRenderSurfel=true; 82 | bool bNeedUpdateTexture=false; 83 | bool bDrawTraj=true; 84 | bool bDrawCam=true; 85 | bool bRenderNodeLabel=true; 86 | bool bRenderEdgeLabel=true; 87 | bool bRecordImg=false; 88 | int mNodeFilterSize=512; 89 | 90 | void key_callback_impl(GLFWwindow* window, int key, int scancode, int action, int mods)override {} 91 | 92 | void MainUI(); 93 | void Process(); 94 | bool ProcessSLAM(); 95 | void UpdateGraphRenderer(); 96 | 97 | void UI_Class_Relationships(); 98 | 99 | void UI_Graph_Info(); 100 | 101 | void UI_Graph_Config(); 102 | 103 | void UI_Graph_EdgeInfo(size_t node_id); 104 | 105 | void GetSurfelColor(Eigen::Vector3f& surfel_color, const inseg_lib::Surfel *surfel); 106 | 107 | std::string GetNodeLabel(const Node *node) const; 108 | 109 | std::string GetEdgeLabel(const Edge *edge); 110 | 111 | void GetEdgeColor(Eigen::Vector3f& edge_color, const PSLAM::Edge *edge); 112 | 113 | void RecordImg(); 114 | }; 115 | } 116 | 117 | #endif //GRAPHSLAM_SURFELGUI_H 118 | -------------------------------------------------------------------------------- /libGraphSLAMGUI/ImageDrawer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/23/20. 3 | // 4 | #include "ImageDrawer.h" 5 | 6 | using namespace PSLAM; 7 | #include 8 | 9 | 10 | ImageDrawer::~ImageDrawer(){ 11 | if(bInited){ 12 | glDeleteVertexArrays(1, &VAO); 13 | glDeleteBuffers(1, &VBO); 14 | glDeleteBuffers(1, &textID); 15 | } 16 | } 17 | void ImageDrawer::Init(unsigned int textureWidth, unsigned int textureHeight, GLenum format) { 18 | mFormat = format; 19 | const std::string vsShader = "#version 330\n" 20 | "layout(location = 0) in vec3 position;\n" 21 | "layout(location = 1) in vec2 vertexUV;\n" 22 | "out vec2 UV;\n" 23 | "void main() {\n" 24 | " gl_Position = vec4(position, 1.0);\n" 25 | " UV = vertexUV;\n" 26 | "}"; 27 | const std::string fsShader = "#version 330\n" 28 | "in vec2 UV;\n" 29 | "uniform sampler2D myTextureSampler;\n" 30 | "out vec4 color;\n" 31 | "void main() {\n" 32 | " color = vec4(texture(myTextureSampler, UV).rgb, 1.0);\n" 33 | "}"; 34 | 35 | float quadVertices[] = { // vertex attributes for a quad that fills the entire screen in Normalized Device Coordinates. 36 | // positions // texCoords 37 | -1.0f, 1.0f, 0.0f, 1.0f, 38 | -1.0f, -1.0f, 0.0f, 0.0f, 39 | 1.0f, -1.0f, 1.0f, 0.0f, 40 | 41 | -1.0f, 1.0f, 0.0f, 1.0f, 42 | 1.0f, -1.0f, 1.0f, 0.0f, 43 | 1.0f, 1.0f, 1.0f, 1.0f 44 | }; 45 | 46 | // Init shader 47 | mShader = std::make_unique(vsShader,fsShader); 48 | mShader->use(); 49 | 50 | // screen quad VAO 51 | unsigned int &quadVAO = VAO; 52 | unsigned int &quadVBO = VBO; 53 | glGenVertexArrays(1, &quadVAO); 54 | glGenBuffers(1, &quadVBO); 55 | glBindVertexArray(quadVAO); 56 | glBindBuffer(GL_ARRAY_BUFFER, quadVBO); 57 | glBufferData(GL_ARRAY_BUFFER, sizeof(quadVertices), &quadVertices, GL_STATIC_DRAW); 58 | glEnableVertexAttribArray(0); 59 | glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void *) 0); 60 | glEnableVertexAttribArray(1); 61 | glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 4 * sizeof(float), (void *) (2 * sizeof(float))); 62 | 63 | glGenTextures(1, &textID); 64 | glBindTexture(GL_TEXTURE_2D, textID); 65 | glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, textureWidth, textureHeight, 0, GL_RGB, 66 | GL_UNSIGNED_BYTE, nullptr); 67 | 68 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); 69 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); 70 | // For Alpha texture 71 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, mFormat == GL_RGBA ? GL_CLAMP_TO_EDGE 72 | : GL_REPEAT); // for this tutorial: use GL_CLAMP_TO_EDGE to prevent semi-transparent borders. Due to interpolation it takes texels from next repeat 73 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, mFormat == GL_RGBA ? GL_CLAMP_TO_EDGE : GL_REPEAT); 74 | } 75 | 76 | void ImageDrawer::Update(const unsigned char *data, int width, int height) { 77 | glBindTexture(GL_TEXTURE_2D, textID); 78 | glTexImage2D(GL_TEXTURE_2D, 0, mFormat, width, height, 0, mFormat, GL_UNSIGNED_BYTE, data); 79 | } 80 | 81 | void ImageDrawer::Draw(const Eigen::Matrix4f& projection, const Eigen::Matrix4f& viewMatrix){ 82 | mShader->use(); 83 | /// Draw Image 84 | glActiveTexture(GL_TEXTURE0); 85 | glBindVertexArray(VAO); 86 | glBindTexture(GL_TEXTURE_2D, textID); 87 | glDrawArrays(GL_TRIANGLES, 0, 6); 88 | } -------------------------------------------------------------------------------- /libGraphSLAMGUI/ImageDrawer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/23/20. 3 | // 4 | 5 | #ifndef GRAPHSLAM_IMAGEDRAWER_H 6 | #define GRAPHSLAM_IMAGEDRAWER_H 7 | 8 | #include "../libGUI3D/libGUI3D/glShader.hpp" 9 | #include 10 | namespace PSLAM { 11 | class ImageDrawer { 12 | public: 13 | ~ImageDrawer(); 14 | void Init(unsigned int textureWidth, unsigned int textureHeight, GLenum format = GL_RGBA); 15 | void Update(const unsigned char *data, int width, int height); 16 | void Draw(const Eigen::Matrix4f& projection, const Eigen::Matrix4f& viewMatrix); 17 | private: 18 | GLenum mFormat; 19 | unsigned int VAO, VBO, textID; 20 | std::unique_ptr mShader; 21 | bool bInited=false; 22 | }; 23 | } 24 | 25 | #endif //GRAPHSLAM_IMAGEDRAWER_H 26 | -------------------------------------------------------------------------------- /libGraphSLAMGUI/SurfelDrawer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/21/20. 3 | // 4 | 5 | #include 6 | #include "SurfelDrawer.h" 7 | #include "../Utils/define.h" 8 | #include 9 | 10 | using namespace PSLAM; 11 | SurfelDrawer::~SurfelDrawer(){ 12 | if(bInited) { 13 | glDeleteVertexArrays(1, &VAO); 14 | glDeleteBuffers(1, &VBO); 15 | } 16 | } 17 | 18 | void SurfelDrawer::Init(std::function getcolor){ 19 | pGetColor = getcolor; 20 | std::string shader_folder =std::string( GUI_FOLDER_PATH) + "/Shaders/"; 21 | const std::string vsShader = shader_folder+"/surfel.vs"; 22 | const std::string fsShader = shader_folder+"/surfel.fs"; 23 | const std::string gsShader = shader_folder+"/surfel.gs"; 24 | 25 | mShader = std::make_unique(vsShader,fsShader,gsShader); 26 | mShader->use(); 27 | mShader->set("model", glm::mat4(1.f)); 28 | mShader->set("diffuse_strength",mDiffuseStrength); 29 | UpdateBuffer(mBufferSize,true); 30 | } 31 | 32 | void SurfelDrawer::Update(const std::vector>& surfels){ 33 | VecEigenf(3) points,colors,normals; 34 | VecEigenf(1) radius; 35 | points.reserve(surfels.size() ); 36 | colors.reserve(surfels.size() ); 37 | radius.reserve(surfels.size()); 38 | normals.reserve(surfels.size()); 39 | 40 | Eigen::Vector3f final_color = {0.f,0.f,0.f}; 41 | 42 | size_t counter=0; 43 | for(const auto & i : surfels){ 44 | auto * surfel = i.get(); 45 | if ((!surfel->is_valid || !surfel->is_stable) || (surfel->label == 0)) { 46 | continue; 47 | } 48 | const Eigen::Vector3f& point = surfel->pos; 49 | const Eigen::Vector3f& normal = surfel->normal; 50 | pGetColor(final_color,surfel); 51 | points.emplace_back(point*1e-3); 52 | normals.emplace_back(normal); 53 | colors.emplace_back(final_color); 54 | radius.emplace_back(surfel->radius*1e-3); 55 | counter++; 56 | } 57 | 58 | mPointSize = counter; 59 | UpdateBuffer(mPointSize); 60 | 61 | 62 | glBindVertexArray(VAO); 63 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 64 | // position 65 | glBufferSubData(GL_ARRAY_BUFFER, 0, 66 | mPointSize * 3 * sizeof(GLfloat), points.data()); 67 | // color 68 | glBufferSubData(GL_ARRAY_BUFFER, mBufferSize * (3) * sizeof(GLfloat), 69 | mPointSize * 3 * sizeof(GLfloat), colors.data()); 70 | // normal 71 | glBufferSubData(GL_ARRAY_BUFFER, mBufferSize * (3+3) * sizeof(GLfloat), 72 | mPointSize * 3 * sizeof(GLfloat), normals.data()); 73 | 74 | glBufferSubData(GL_ARRAY_BUFFER, mBufferSize * (3+3+3) * sizeof(GLfloat), 75 | mPointSize * 1 * sizeof(GLfloat), radius.data()); 76 | 77 | glBindBuffer(GL_ARRAY_BUFFER, 0); 78 | } 79 | 80 | void SurfelDrawer::Draw(const Eigen::Matrix4f& projection, const Eigen::Matrix4f& viewMatrix) { 81 | if(!bInited) SCLOG(ERROR) << "NodeDrawer: Draw was called before Init!"; 82 | mShader->use(); 83 | mShader->set("diffuse_strength",mDiffuseStrength); 84 | mShader->set("projection", projection); 85 | mShader->set("view", viewMatrix); 86 | glBindVertexArray(VAO); 87 | glDrawArrays(GL_POINTS, 0, mPointSize); 88 | glBindBuffer(GL_ARRAY_BUFFER, 0); 89 | } 90 | 91 | void SurfelDrawer::UpdateBuffer(size_t newSize, bool force){ 92 | if(bInited) { 93 | if (!force) { 94 | if (mBufferSize > newSize) return; 95 | } else { 96 | glDeleteVertexArrays(1, &VAO); 97 | glDeleteBuffers(1, &VBO); 98 | } 99 | } 100 | mBufferSize = (std::floor(newSize / 1e6) + 1) * 1e6; 101 | // create buffer 102 | uint numStride = 3+3+3+1; 103 | size_t totalSize = mBufferSize * numStride; 104 | glGenVertexArrays(1, &VAO); 105 | glGenBuffers(1, &VBO); 106 | glBindVertexArray(VAO); 107 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 108 | glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * totalSize, nullptr, GL_DYNAMIC_DRAW); 109 | 110 | // vec3 Position 111 | glEnableVertexAttribArray(0); 112 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), nullptr); 113 | // vec3 Color 114 | glEnableVertexAttribArray(1); 115 | glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), 116 | (void *) (mBufferSize * (3) * sizeof(GLfloat))); 117 | // vec3 normal 118 | glEnableVertexAttribArray(2); 119 | glVertexAttribPointer(2, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), 120 | (void *) (mBufferSize * (3+3) * sizeof(GLfloat))); 121 | 122 | glEnableVertexAttribArray(3); 123 | glVertexAttribPointer(3, 3, GL_FLOAT, GL_FALSE, 1 * sizeof(GLfloat), 124 | (void *) (mBufferSize * (3+3+3) * sizeof(GLfloat))); 125 | 126 | glBindBuffer(GL_ARRAY_BUFFER, 0); 127 | glBindVertexArray(0); 128 | 129 | bInited=true; 130 | } -------------------------------------------------------------------------------- /libGraphSLAMGUI/SurfelDrawer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/21/20. 3 | // 4 | 5 | #ifndef GRAPHSLAM_SURFELDRAWER_H 6 | #define GRAPHSLAM_SURFELDRAWER_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include "../libGUI3D/libGUI3D/glShader.hpp" 12 | #include "../lib/inseg_lib/surfel.h" 13 | 14 | namespace PSLAM { 15 | class SurfelDrawer { 16 | public: 17 | SurfelDrawer()=default; 18 | ~SurfelDrawer(); 19 | 20 | void Init(std::function getcolor); 21 | 22 | void Update(const std::vector>& surfels); 23 | void Draw(const Eigen::Matrix4f& projection, const Eigen::Matrix4f& viewMatrix); 24 | 25 | float mDiffuseStrength=1.f; 26 | private: 27 | bool bInited=false; 28 | std::function pGetColor = nullptr; 29 | size_t mPointSize=0, mBufferSize = 1e3; 30 | unsigned int VAO{},VBO{}; 31 | std::unique_ptr mShader; 32 | 33 | void UpdateBuffer(size_t newSize, bool force = false); 34 | }; 35 | } 36 | 37 | #endif //GRAPHSLAM_SURFELDRAWER_H 38 | -------------------------------------------------------------------------------- /libGraphSLAMGUI/TrajectoryDrawer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/24/20. 3 | // 4 | 5 | #include "TrajectoryDrawer.h" 6 | 7 | using namespace PSLAM; 8 | 9 | TrajectoryDrawer::~TrajectoryDrawer() { 10 | if(bInited) { 11 | glDeleteVertexArrays(1, &VAO); 12 | glDeleteBuffers(1, &VBO); 13 | } 14 | } 15 | 16 | void TrajectoryDrawer::Init() { 17 | const std::string shaderPath = std::string(GUI_FOLDER_PATH) + "Shaders/"; 18 | mShader = std::make_unique(shaderPath+"camera_shader.vs",shaderPath+"camera_shader.fs"); 19 | mShader->use(); 20 | mShader->set("color", Eigen::Vector4f{0, 1, 0, 1.f}); 21 | mShader->set("model", glm::mat4(1.f)); 22 | UpdateBuffer(2<<8); 23 | } 24 | 25 | void TrajectoryDrawer::Add(glm::vec3 point, float interval){ 26 | if(std::isnan(point.x) || std::isinf(point.x)) return; 27 | const std::string name = "Trajectory"; 28 | auto new_size = mvTrajectories.size()+1; 29 | UpdateBuffer(new_size); 30 | 31 | if (mvTrajectories.empty()) { 32 | mvTrajectories.emplace_back(point); 33 | } else { 34 | const auto &prev = mvTrajectories.back(); 35 | if (glm::distance(point, prev) > interval) { 36 | mvTrajectories.push_back(point); 37 | glBindVertexArray(VAO); 38 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 39 | glBufferSubData(GL_ARRAY_BUFFER, 0, 40 | mvTrajectories.size() * 3 * sizeof(GLfloat), mvTrajectories.data()); 41 | glBindBuffer(GL_ARRAY_BUFFER, 0); 42 | } 43 | } 44 | } 45 | 46 | void TrajectoryDrawer::Draw(const Eigen::Matrix4f &projection, const Eigen::Matrix4f &viewMatrix) { 47 | mShader->use(); 48 | mShader->set("projection", projection); 49 | mShader->set("view", viewMatrix); 50 | glBindVertexArray(VAO); 51 | glDrawArrays(GL_LINE_STRIP, 0, mvTrajectories.size()); 52 | glBindBuffer(GL_ARRAY_BUFFER, 0); 53 | } 54 | 55 | 56 | void TrajectoryDrawer::UpdateBuffer(size_t newSize, bool force) { 57 | if(bInited) { 58 | if (!force) { 59 | if (mBufferSize > newSize) return; 60 | } else { 61 | glDeleteVertexArrays(1, &VAO); 62 | glDeleteBuffers(1, &VBO); 63 | } 64 | } 65 | mBufferSize = (std::floor(newSize / 1e6) + 1) * 1e6; 66 | { 67 | auto tmp = mvTrajectories; 68 | mvTrajectories.clear(); 69 | mvTrajectories.reserve(mBufferSize); 70 | mvTrajectories.resize(tmp.size()); 71 | std::copy(tmp.begin(),tmp.end(),mvTrajectories.begin()); 72 | } 73 | // SCLOG(DEBUG) << "new surfel buffer size: " << mBufferSize; 74 | 75 | glGenVertexArrays(1, &VAO); 76 | glGenBuffers(1, &VBO); 77 | glBindVertexArray(VAO); 78 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 79 | glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * mBufferSize * 3, nullptr, GL_DYNAMIC_DRAW); 80 | glEnableVertexAttribArray(0); 81 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), nullptr); 82 | glBindBuffer(GL_ARRAY_BUFFER, 0); 83 | glBindVertexArray(0); 84 | bInited=true; 85 | } -------------------------------------------------------------------------------- /libGraphSLAMGUI/TrajectoryDrawer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 8/24/20. 3 | // 4 | 5 | #ifndef GRAPHSLAM_TRAJECTORYDRAWER_H 6 | #define GRAPHSLAM_TRAJECTORYDRAWER_H 7 | 8 | #include "../libGUI3D/libGUI3D/glShader.hpp" 9 | #include 10 | #include 11 | namespace PSLAM { 12 | class TrajectoryDrawer{ 13 | public: 14 | ~TrajectoryDrawer(); 15 | void Init(); 16 | void Add(glm::vec3 point, float interval=0.002); 17 | void Draw(const Eigen::Matrix4f& projection, const Eigen::Matrix4f& viewMatrix); 18 | private: 19 | std::vector mvTrajectories; 20 | unsigned int VAO, VBO, EBO; 21 | float mScale = 0.2f; 22 | std::unique_ptr mShader; 23 | bool bInited=false; 24 | size_t mBufferSize = 1e3; 25 | 26 | void UpdateBuffer(size_t newSize, bool force = false); 27 | }; 28 | } 29 | 30 | #endif //GRAPHSLAM_TRAJECTORYDRAWER_H 31 | -------------------------------------------------------------------------------- /libGraphSLAMGUI/graphDrawer/edge_drawer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 6/5/20. 3 | // 4 | #include "edge_drawer.h" 5 | 6 | #include 7 | using namespace PSLAM; 8 | 9 | EdgeDrawer::EdgeDrawer(): 10 | bInited(false), mTotalSize(0),mIndiceSize(0),mVertexBufferSize(1e6), mIndiceBufferSize(1e6), 11 | mLineWith(3){} 12 | 13 | void EdgeDrawer::Init(){ 14 | // Shader 15 | mShader = std::make_unique(vsShader,fsShader); 16 | // mShader.Init("EdgeDrawer", vsShader,fsShader); 17 | mShader->use(); 18 | 19 | //TOOD: make color depends on the edge label 20 | mShader->set("color",Eigen::Vector4f(5/255.f,205/255.f,212/255.f,1)); 21 | Eigen::Matrix4f modelmat = Eigen::Matrix4f::Identity(); 22 | mShader->set("model", modelmat); 23 | 24 | // Buffer 25 | size_t totalSize = mVertexBufferSize * 3; 26 | glGenVertexArrays(1, &VAO); 27 | glGenBuffers(1, &VBO); 28 | glGenBuffers(1, &EBO); 29 | 30 | glBindVertexArray(VAO); 31 | 32 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 33 | glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * totalSize * 3, NULL, GL_DYNAMIC_DRAW); 34 | 35 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, EBO); 36 | glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(GLuint) * mIndiceBufferSize, NULL, GL_DYNAMIC_DRAW); 37 | 38 | glEnableVertexAttribArray(0); 39 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), nullptr); 40 | glBindBuffer(GL_ARRAY_BUFFER, 0); 41 | glBindVertexArray(0); 42 | } 43 | 44 | void EdgeDrawer::Draw(const Eigen::Matrix4f& projection, const Eigen::Matrix4f& viewMatrix){ 45 | if(mTotalSize==0 || mIndiceSize==0) return; 46 | mShader->use(); 47 | mShader->set("projection",projection); 48 | mShader->set("view",viewMatrix); 49 | glBindVertexArray(VAO); 50 | glLineWidth(mLineWith); 51 | glDrawElements(GL_LINES, mIndiceSize, GL_UNSIGNED_INT, nullptr); 52 | glBindBuffer(GL_ARRAY_BUFFER, 0); 53 | } 54 | 55 | void EdgeDrawer::Update(const VecEigenf(3) *points, const std::vector *indices){ 56 | mTotalSize = points->size(); 57 | mIndiceSize = indices->size(); 58 | if(mTotalSize==0 || mIndiceSize==0) return; 59 | updateVertexBuffer(mTotalSize); 60 | updateIndiceBuffer(mIndiceSize); 61 | 62 | glBindVertexArray(VAO); 63 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 64 | 65 | glBufferSubData(GL_ARRAY_BUFFER, 0, 66 | mTotalSize * 3 * sizeof(GLfloat), points->data()); 67 | 68 | glBindBuffer(GL_ARRAY_BUFFER, EBO); 69 | glBufferSubData(GL_ELEMENT_ARRAY_BUFFER, 0, 70 | mIndiceSize * sizeof(unsigned int), indices->data()); 71 | 72 | glLineWidth(mLineWith); 73 | // glDrawElements(GL_LINES, mIndiceSize, GL_UNSIGNED_INT, nullptr); 74 | glBindBuffer(GL_ARRAY_BUFFER, 0); 75 | } 76 | 77 | void EdgeDrawer::draw_impl(const VecEigenf(3) *points, const std::vector *indices) { 78 | uint totalPoints = points->size(); 79 | updateVertexBuffer(totalPoints); 80 | updateIndiceBuffer(indices->size()); 81 | 82 | glBindVertexArray(VAO); 83 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 84 | 85 | glBufferSubData(GL_ARRAY_BUFFER, 0, 86 | totalPoints * 3 * sizeof(GLfloat), points->data()); 87 | 88 | glBindBuffer(GL_ARRAY_BUFFER, EBO); 89 | glBufferSubData(GL_ELEMENT_ARRAY_BUFFER, 0, 90 | indices->size() * sizeof(unsigned int), indices->data()); 91 | 92 | glLineWidth(mLineWith); 93 | glDrawElements(GL_LINES, indices->size(), GL_UNSIGNED_INT, nullptr); 94 | glBindBuffer(GL_ARRAY_BUFFER, 0); 95 | } 96 | 97 | void EdgeDrawer::updateVertexBuffer(size_t newSize, bool force) { 98 | if (!force) 99 | if (mVertexBufferSize > newSize) return; 100 | mVertexBufferSize = (std::floor(newSize / 1e6) + 1) * 1e6; 101 | 102 | glDeleteVertexArrays(1, &VAO); 103 | glDeleteBuffers(1, &VBO); 104 | 105 | uint numStride = 8; 106 | size_t totalSize = mVertexBufferSize * numStride; 107 | glGenVertexArrays(1, &VAO); 108 | glGenBuffers(1, &VBO); 109 | glBindVertexArray(VAO); 110 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 111 | glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * totalSize, nullptr, GL_DYNAMIC_DRAW); 112 | glEnableVertexAttribArray(0); 113 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), 0); 114 | glBindBuffer(GL_ARRAY_BUFFER, 0); 115 | glBindVertexArray(0); 116 | } 117 | 118 | void EdgeDrawer::updateIndiceBuffer(size_t newSize, bool force) { 119 | if (!force) 120 | if (mIndiceBufferSize > newSize) return; 121 | mIndiceBufferSize = (std::floor(newSize / 1e6) + 1) * 1e6; 122 | 123 | glDeleteBuffers(1,&EBO); 124 | glBindVertexArray(VAO); 125 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, EBO); 126 | glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(GLuint) * mIndiceBufferSize, NULL, GL_DYNAMIC_DRAW); 127 | 128 | glEnableVertexAttribArray(0); 129 | glBindBuffer(GL_ARRAY_BUFFER, 0); 130 | glBindVertexArray(0); 131 | } -------------------------------------------------------------------------------- /libGraphSLAMGUI/graphDrawer/edge_drawer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "../../Utils/define.h" 3 | #include 4 | #include "../../libGUI3D/libGUI3D/glShader.hpp" 5 | #include 6 | 7 | namespace PSLAM { 8 | class EdgeDrawer { 9 | public: 10 | explicit EdgeDrawer(); 11 | 12 | void Init(); 13 | 14 | void Update(const VecEigenf(3) *points, const std::vector *indices); 15 | 16 | void Draw(const Eigen::Matrix4f& projection, const Eigen::Matrix4f& viewMatrix); 17 | 18 | private: 19 | bool bInited; 20 | size_t mTotalSize, mIndiceSize, mVertexBufferSize, mIndiceBufferSize; 21 | unsigned int mLineWith; 22 | unsigned int VAO{},VBO{}, EBO{}; 23 | std::unique_ptr mShader; 24 | // Shader mShader; 25 | 26 | void draw_impl(const VecEigenf(3) *points, const std::vector *indices); 27 | 28 | void updateVertexBuffer(size_t newSize, bool force = false); 29 | 30 | void updateIndiceBuffer(size_t newSize, bool force = false); 31 | 32 | const std::string vsShader = 33 | "#version 330 core\n" 34 | "layout (location = 0) in vec3 aPos;\n" 35 | "\n" 36 | "uniform mat4 model;\n" 37 | "uniform mat4 view;\n" 38 | "uniform mat4 projection;\n" 39 | "\n" 40 | "void main()\n" 41 | "{\n" 42 | " \tgl_Position = projection * view * model * vec4(aPos,1);\n" 43 | "}"; 44 | const std::string fsShader = 45 | "#version 330 core\n" 46 | "out vec4 FragColor;\n" 47 | "\n" 48 | "uniform vec4 color;\n" 49 | "\n" 50 | "void main()\n" 51 | "{\n" 52 | " FragColor = color; \n" 53 | "}"; 54 | 55 | }; 56 | } -------------------------------------------------------------------------------- /libGraphSLAMGUI/graphDrawer/graph_drawer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 6/5/20. 3 | // 4 | #include "graph_drawer.h" 5 | #include "../../Utils/define.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | //TODO: add label display on edges 11 | 12 | using namespace PSLAM; 13 | 14 | GraphDrawer::GraphDrawer():mbDrawNode(true), mbDrawEdge(true), mbDrawLabel(true),mbInited(false){ 15 | 16 | } 17 | 18 | void GraphDrawer::Init(){ 19 | mNodeDrawer.Init(); 20 | mNodeDrawer.SetPointSize(0.1); 21 | mEdgeDrawer.Init(); 22 | mTextDrawer.Init(); 23 | } 24 | 25 | void GraphDrawer::Update(//float width, float height, 26 | const VecEigenf(3) &labelPositions, 27 | const VecEigenf(3) &labelColors, 28 | const std::vector &texts, 29 | const std::vector &edgeIndices 30 | ){ 31 | mNodeDrawer.Update(&labelPositions,&labelColors); 32 | mEdgeDrawer.Update(&labelPositions, &edgeIndices); 33 | UpdateTextBuffer(labelPositions,labelColors,texts); 34 | } 35 | 36 | void GraphDrawer::Draw(float width, float height, 37 | const Eigen::Matrix4f &view_matrix, 38 | const Eigen::Matrix4f &projection){ 39 | if(mbDrawNode) 40 | mNodeDrawer.Draw(projection, view_matrix); 41 | if(mbDrawLabel) 42 | DrawText(width,height,view_matrix,projection); 43 | if(mbDrawEdge) 44 | mEdgeDrawer.Draw(projection, view_matrix); 45 | } 46 | 47 | void GraphDrawer::UpdateTextBuffer(const VecEigenf(3) &labelPositions, 48 | const VecEigenf(3) &labelColors, 49 | const std::vector &texts){ 50 | // std::vector, Eigen::aligned_allocator>> labelPositionsOnImage(labelPositions.size()); 51 | mTextBuffer.clear(); 52 | mTextBuffer.reserve(labelPositions.size()); 53 | for(size_t i=0;i 4 | #include 5 | #include 6 | #include "text_drawer.h" 7 | #include "node_drawer.h" 8 | #include "edge_drawer.h" 9 | 10 | namespace PSLAM { 11 | struct TripletForSotring { 12 | Eigen::Vector3f position3D; 13 | Eigen::Vector3f position; 14 | Eigen::Vector3f color; 15 | std::string text; 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | }; 18 | 19 | class GraphDrawer { 20 | public: 21 | GraphDrawer(); 22 | void Init(); 23 | void Draw(float width, float height, 24 | const Eigen::Matrix4f &view_matrix, 25 | const Eigen::Matrix4f &projection); 26 | void Update(//float width, float height, 27 | const VecEigenf(3) &labelPositions, 28 | const VecEigenf(3) &labelColors, 29 | const std::vector &texts, 30 | const std::vector &edgeIndices 31 | ); 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | NodeDrawer mNodeDrawer; 34 | TextDrawer mTextDrawer; 35 | EdgeDrawer mEdgeDrawer; 36 | std::vector mTextBuffer; 37 | 38 | bool mbDrawNode, mbDrawEdge, mbDrawLabel; 39 | private: 40 | bool mbInited; 41 | 42 | 43 | void UpdateAndDrawLabel(float windowWidth, float windowHeight, 44 | const VecEigenf(3) &labelPositions, 45 | const VecEigenf(3) &labelColors, 46 | const std::vector &labelIndices, 47 | const Eigen::Matrix4f &view_matrix, 48 | const Eigen::Matrix4f &projection); 49 | 50 | void UpdateTextBuffer(const VecEigenf(3) &labelPositions, 51 | const VecEigenf(3) &labelColors, 52 | const std::vector &texts); 53 | 54 | void DrawText(float windowWidth, float windowHeight, 55 | const Eigen::Matrix4f &view_matrix, 56 | const Eigen::Matrix4f &projection); 57 | }; 58 | } -------------------------------------------------------------------------------- /libGraphSLAMGUI/graphDrawer/node_drawer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 6/5/20. 3 | // 4 | 5 | #include "node_drawer.h" 6 | 7 | #include 8 | using namespace PSLAM; 9 | 10 | NodeDrawer::NodeDrawer(): 11 | mfPointSize(1), bInited(false), mTotalPoints(0), mPointBufferSize(1e3) 12 | {} 13 | 14 | const std::string vsShader = 15 | "#version 330\n" 16 | "layout (location = 0) in vec3 aPos;\n" 17 | "layout (location = 1) in vec3 aColor;\n" 18 | "out vec3 FragPos;\n" 19 | "uniform mat4 model;\n" 20 | "uniform mat4 view;\n" 21 | "uniform mat4 projection;\n" 22 | "out VS_OUT {\n" 23 | " \tvec4 color;\n" 24 | "} vs_out;\n" 25 | "void main()\n" 26 | "{\n" 27 | " gl_Position = projection * view * model * vec4(aPos,1);\n" 28 | " FragPos = vec3(view * model * vec4(aPos,1));\n" 29 | " vs_out.color = vec4(aColor,1);\n" 30 | "}"; 31 | const std::string fsShader = 32 | "#version 330\n" 33 | "out vec4 FragColor;\n" 34 | "in vec3 FragPos;\n" 35 | "in vec4 fColor;\n" 36 | "in vec2 v_coord;\n" 37 | "uniform sampler2D texture1;\n" 38 | "void main()\n" 39 | "{\n" 40 | " float sq_norm = dot(v_coord, v_coord);\n" 41 | " if (sq_norm > 1.f) discard;\n" 42 | " FragColor = (fColor);\n" 43 | "} "; 44 | const std::string gsShader = 45 | "#version 330\n" 46 | "layout (points) in;\n" 47 | "layout (triangle_strip, max_vertices = 8) out;\n" 48 | "in VS_OUT {\n" 49 | " \tvec4 color;\n" 50 | "} gs_in[];\n" 51 | "out vec4 fColor;\n" 52 | "out vec2 v_coord;\n" 53 | "uniform mat4 modelPose;\n" 54 | "uniform float radius;\n" 55 | "\n" 56 | "//const float radius = 0.01;\n" 57 | "void build_house(vec4 position)\n" 58 | "{ \n" 59 | " fColor = gs_in[0].color; // gs_in[0] since there's only one input vertex\n" 60 | " fColor.w = 0.8;" 61 | " gl_Position = position + vec4(-radius, -radius, 0.0, 0.0); // 1:bottom-left \n" 62 | " v_coord = vec2(-1.0,-1.0);\n" 63 | " EmitVertex(); \n" 64 | " gl_Position = position + vec4( radius, -radius, 0.0, 0.0); // 2:bottom-right\n" 65 | " v_coord = vec2(1.0,-1.0);\n" 66 | " EmitVertex();\n" 67 | " gl_Position = position + vec4(-radius, radius, 0.0, 0.0); // 3:top-left\n" 68 | " v_coord = vec2(-1.0,1.0);\n" 69 | " EmitVertex();\n" 70 | " gl_Position = position + vec4( radius, radius, 0.0, 0.0); // 4:top-right\n" 71 | " v_coord = vec2(1.0,1.0);\n" 72 | " EmitVertex();\n" 73 | " EndPrimitive();\n" 74 | "}\n" 75 | "void main() { \n" 76 | " build_house(gl_in[0].gl_Position);\n" 77 | "}"; 78 | 79 | void NodeDrawer::Init(){ 80 | // load shader 81 | mShader = std::make_unique(vsShader,fsShader,gsShader); 82 | 83 | // create buffer 84 | updateVertexBuffer(mPointBufferSize); 85 | // uint numStride = 6; 86 | // size_t totalSize = mPointBufferSize * numStride; 87 | // glGenVertexArrays(1, &VAO); 88 | // glGenBuffers(1, &VBO); 89 | // glBindVertexArray(VAO); 90 | // glBindBuffer(GL_ARRAY_BUFFER, VBO); 91 | // glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * totalSize, nullptr, GL_DYNAMIC_DRAW); 92 | // glEnableVertexAttribArray(0); 93 | // glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), nullptr); 94 | // glEnableVertexAttribArray(1); 95 | // glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), 96 | // (void *) (mPointBufferSize * 3 * sizeof(GLfloat))); 97 | // glBindBuffer(GL_ARRAY_BUFFER, 0); 98 | // glBindVertexArray(0); 99 | // 100 | // bInited = true; 101 | 102 | mShader->use(); 103 | Eigen::Matrix4f modelmat = Eigen::Matrix4f::Identity(); 104 | mShader->set("model", modelmat); 105 | SetPointSize(mfPointSize); 106 | } 107 | 108 | void NodeDrawer::Update(const VecEigenf(3) *points, const VecEigenf(3) *colors){ 109 | mTotalPoints = points->size(); 110 | updateVertexBuffer(mTotalPoints); 111 | 112 | glBindVertexArray(VAO); 113 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 114 | glBufferSubData(GL_ARRAY_BUFFER, 0, 115 | mTotalPoints * 3 * sizeof(GLfloat), points->data()); 116 | glBufferSubData(GL_ARRAY_BUFFER, mPointBufferSize * 3 * sizeof(GLfloat), 117 | mTotalPoints * 3 * sizeof(GLfloat), colors->data()); 118 | glBindBuffer(GL_ARRAY_BUFFER, 0); 119 | } 120 | 121 | void NodeDrawer::SetPointSize(float size){ 122 | mfPointSize = size; 123 | mShader->use(); 124 | mShader->set("radius", mfPointSize); 125 | } 126 | 127 | void NodeDrawer::Draw(const Eigen::Matrix4f& projection, const Eigen::Matrix4f& viewMatrix) { 128 | if(!bInited) throw std::runtime_error("NodeDrawer: Draw was called before Init!.\n"); 129 | mShader->use(); 130 | mShader->set("projection",projection); 131 | mShader->set("view",viewMatrix); 132 | glBindVertexArray(VAO); 133 | glDrawArrays(GL_POINTS, 0, mTotalPoints); 134 | glBindBuffer(GL_ARRAY_BUFFER, 0); 135 | } 136 | 137 | void NodeDrawer::updateVertexBuffer(size_t newSize, bool force) { 138 | if(bInited) { 139 | if (!force) { 140 | if (mPointBufferSize > newSize) return; 141 | } else { 142 | glDeleteVertexArrays(1, &VAO); 143 | glDeleteBuffers(1, &VBO); 144 | } 145 | } 146 | mPointBufferSize = (std::floor(newSize / 1e6) + 1) * 1e6; 147 | 148 | uint numStride = 8; 149 | size_t totalSize = mPointBufferSize * numStride; 150 | glGenVertexArrays(1, &VAO); 151 | glGenBuffers(1, &VBO); 152 | glBindVertexArray(VAO); 153 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 154 | glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * totalSize, nullptr, GL_DYNAMIC_DRAW); 155 | glEnableVertexAttribArray(0); 156 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), 0); 157 | glEnableVertexAttribArray(1); 158 | glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 3 * sizeof(GLfloat), 159 | (void *) (mPointBufferSize * 3 * sizeof(GLfloat))); 160 | glBindBuffer(GL_ARRAY_BUFFER, 0); 161 | glBindVertexArray(0); 162 | bInited=true; 163 | } -------------------------------------------------------------------------------- /libGraphSLAMGUI/graphDrawer/node_drawer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "../../Utils/define.h" 3 | #include "../../libGUI3D/libGUI3D/glShader.hpp" 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace PSLAM { 10 | class NodeDrawer { 11 | public: 12 | NodeDrawer(); 13 | 14 | void Init(); 15 | 16 | void Update(const VecEigenf(3) *points, const VecEigenf(3) *colors); 17 | 18 | void Draw(const Eigen::Matrix4f& projection, const Eigen::Matrix4f& viewMatrix); 19 | 20 | void SetPointSize(float size); 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | float mfPointSize; 24 | private: 25 | bool bInited; 26 | size_t mTotalPoints; 27 | std::unique_ptr mShader; 28 | size_t mPointBufferSize; 29 | unsigned int VAO{}, VBO{}; 30 | 31 | void updateVertexBuffer(size_t newSize, bool force = false); 32 | }; 33 | } -------------------------------------------------------------------------------- /libGraphSLAMGUI/graphDrawer/text_drawer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "../../Utils/define.h" 4 | #include "../../libGUI3D/libGUI3D/glShader.hpp" 5 | #include 6 | #include 7 | #include 8 | 9 | #ifdef COMPILE_WITH_FREETYPE 10 | #include 11 | #include FT_FREETYPE_H 12 | #endif 13 | 14 | namespace PSLAM { 15 | class TextDrawer { 16 | struct Character { 17 | GLuint TextureID; // ID handle of the glyph texture 18 | Eigen::Vector2i Size; // Size of glyph 19 | Eigen::Vector2i Bearing; // Offset from baseline to left/top of glyph 20 | GLuint Advance; // Horizontal offset to advance to next glyph 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | }; 23 | 24 | public: 25 | 26 | TextDrawer(); 27 | 28 | void Init(); 29 | 30 | void Draw(const std::string& text, GLfloat x, GLfloat y, float width, float height, 31 | GLfloat scale, const Eigen::Vector3f& color); 32 | 33 | float text_size = 1; 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | private: 36 | bool bInited; 37 | std::unique_ptr mShader; 38 | unsigned int VAO,VBO; 39 | std::map Characters; 40 | 41 | void buildText(); 42 | 43 | void RenderText(std::string text, GLfloat x, GLfloat y, float width, float height, 44 | GLfloat scale, const Eigen::Vector3f& color); 45 | 46 | void buildFreeType(); 47 | 48 | const std::string vsShader = 49 | "#version 330 core\n" 50 | "layout (location = 0) in vec4 vertex; // \n" 51 | "out vec2 TexCoords;\n" 52 | "void main()\n" 53 | "{\n" 54 | " gl_Position = vec4(vertex.xy, 0.0, 1.0);\n" 55 | " TexCoords = vertex.zw;\n" 56 | "}"; 57 | 58 | const std::string fsShader = 59 | "#version 330 core\n" 60 | "in vec2 TexCoords;\n" 61 | "out vec4 color;\n" 62 | "uniform sampler2D text;\n" 63 | "uniform vec3 textColor;\n" 64 | "void main()\n" 65 | "{\n" 66 | " vec4 sampled = vec4(1.0, 1.0, 1.0, texture(text, TexCoords).r);\n" 67 | " color = vec4(textColor, 1.0) * sampled;\n" 68 | "}"; 69 | }; 70 | } -------------------------------------------------------------------------------- /prepare_example_data.sh: -------------------------------------------------------------------------------- 1 | # This script is from https://github.com/WaldJohannaU/3RScan with MIT License (Copyright (c) 2020 Johanna Wald) 2 | # 26 July 2022, modified by Shun-Cheng Wu 3 | 4 | if [[ ! -d "data" ]]; then 5 | mkdir data 6 | fi 7 | 8 | # download example data 9 | if [[ ! -d "data/3RScan" ]]; then 10 | if [[ ! -f "data/3RScan.v2.zip" ]]; then 11 | wget "http://www.campar.in.tum.de/public_datasets/3RScan/3RScan.v2.zip" -P data 12 | fi 13 | unzip "data/3RScan.v2.zip" -d ./data/3RScan 14 | fi 15 | 16 | if [[ ! -f "data/3RScan/3RScan.json" ]]; then 17 | wget "http://www.campar.in.tum.de/public_datasets/3RScan/3RScan.json" -P data/3RScan 18 | fi 19 | if [[ ! -f "data/3RScan/objects.json" ]]; then 20 | wget "http://www.campar.in.tum.de/public_datasets/3DSSG/3DSSG/objects.json" -P data/3RScan 21 | fi 22 | -------------------------------------------------------------------------------- /renderer/Renderer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 5/13/21. 3 | // 4 | 5 | #ifndef GRAPHSLAM_RENDERER_H 6 | #define GRAPHSLAM_RENDERER_H 7 | 8 | #include 9 | #include 10 | namespace PSLAM { 11 | class MeshRendererInterface { 12 | public: 13 | MeshRendererInterface(int width, int height, const std::string &folder, const std::string &scan_id) 14 | :m_width(width), m_height(height), m_folder(folder), m_scanId(scan_id){ 15 | InitGL(); 16 | } 17 | 18 | virtual ~MeshRendererInterface(){ 19 | glDeleteFramebuffers(1,&framebuffer); 20 | glDeleteRenderbuffers(1, &rbo); 21 | glDeleteTextures(1,&textureColorbuffer); 22 | } 23 | 24 | virtual void Render(const Eigen::Matrix4f &projection, const Eigen::Matrix4f &view, float near, float far)=0; 25 | 26 | cv::Mat GetRGB() const {return m_rgb;} 27 | cv::Mat GetDepth() const {return m_depth;} 28 | protected: 29 | int m_width, m_height; 30 | std::string m_folder, m_scanId; 31 | cv::Mat m_rgb, m_depth; 32 | GLuint framebuffer,textureColorbuffer,rbo; 33 | void InitGL(){ 34 | // Init GL buffers 35 | glGenFramebuffers(1, &framebuffer); 36 | glBindFramebuffer(GL_FRAMEBUFFER, framebuffer); 37 | 38 | glGenTextures(1, &textureColorbuffer); 39 | glBindTexture(GL_TEXTURE_2D, textureColorbuffer); 40 | glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, m_width, m_height, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL); 41 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); 42 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); 43 | glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, textureColorbuffer, 0); 44 | 45 | // create a renderbuffer object for depth and stencil attachment (we won't be sampling these) 46 | glGenRenderbuffers(1, &rbo); 47 | glBindRenderbuffer(GL_RENDERBUFFER, rbo); 48 | glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT32F, m_width, m_height); // use a single renderbuffer object for both a depth AND stencil buffer. 49 | glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rbo); // now actually attach it 50 | 51 | if(glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE) 52 | std::cout << "ERROR::FRAMEBUFFER:: Framebuffer is not complete!" << std::endl; 53 | glBindFramebuffer(GL_FRAMEBUFFER, 0); 54 | } 55 | }; 56 | } 57 | 58 | #endif //GRAPHSLAM_RENDERER_H 59 | -------------------------------------------------------------------------------- /renderer/RendererFactory.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 5/13/21. 3 | // 4 | 5 | #ifndef GRAPHSLAM_RENDERERFACTORY_H 6 | #define GRAPHSLAM_RENDERERFACTORY_H 7 | #include "Renderer.h" 8 | #include "RendererType.h" 9 | #include "Renderer3RScan.h" 10 | #include "RendererScanNet.h" 11 | namespace PSLAM { 12 | 13 | static MeshRendererInterface* MakeMeshRenderer(int width, int height, const std::string &folder, const std::string &scan_id, 14 | MeshRenderType meshRenderType = MeshRenderType_Detect, bool align = true) { 15 | if (meshRenderType == MeshRenderType_Detect) { 16 | auto folder_lower = folder; 17 | std::for_each(folder_lower.begin(),folder_lower.end(), [](char &c) { 18 | c = ::tolower(c); 19 | }); 20 | 21 | // find ScanNet 22 | if (scan_id.find("scene") != std::string::npos || 23 | folder_lower.find("scannet") != std::string::npos) { 24 | meshRenderType = MeshRenderType_ScanNet; 25 | } else if (folder_lower.find("3rscan") != std::string ::npos) { 26 | meshRenderType = MeshRenderType_3RScan; 27 | } else { 28 | throw std::runtime_error("unable to detect type."); 29 | } 30 | } 31 | 32 | 33 | MeshRendererInterface *renderer = nullptr; 34 | switch (meshRenderType) { 35 | case MeshRenderType_ScanNet:{ 36 | renderer = new MeshRendererScanNet(width, height, folder, scan_id, align); 37 | } 38 | break; 39 | case MeshRenderType_3RScan:{ 40 | renderer = new MeshRenderer3RScan(width, height, folder, scan_id, align); 41 | } 42 | break; 43 | case MeshRenderType_Detect: 44 | throw std::runtime_error("unable to detect renderer type"); 45 | break; 46 | } 47 | return renderer; 48 | } 49 | } 50 | 51 | #endif //GRAPHSLAM_RENDERERFACTORY_H 52 | -------------------------------------------------------------------------------- /renderer/RendererScanNet.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 5/13/21. 3 | // 4 | 5 | #ifndef GRAPHSLAM_RENDERERSCANNET_H 6 | #define GRAPHSLAM_RENDERERSCANNET_H 7 | #include "Renderer.h" 8 | #include 9 | 10 | namespace PSLAM { 11 | class MeshRendererScanNet : public MeshRendererInterface { 12 | public: 13 | MeshRendererScanNet(int width, int height, const std::string &dataPath, const std::string &sequence, bool toReference): 14 | MeshRendererInterface(width,height,dataPath,sequence) { 15 | Init(); 16 | } 17 | private: 18 | std::unique_ptr mShader; 19 | std::unique_ptr mModel; 20 | void Init(){ 21 | mShader = std::make_unique(vs, fs); 22 | mModel = std::make_unique( m_folder+"/"+m_scanId+"/"+m_scanId+"_vh_clean_2.ply" ); 23 | mModel->setShader(mShader.get()); 24 | } 25 | 26 | void Render(const Eigen::Matrix4f &projection, const Eigen::Matrix4f &view, float near, float far) override{ 27 | glEnable(GL_DEPTH_TEST); 28 | 29 | glBindFramebuffer(GL_FRAMEBUFFER, framebuffer); 30 | glClearColor(0.00f, 0.00f, 0.00f, 1.0f); 31 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 32 | 33 | glViewport(0,0,m_width,m_height); 34 | 35 | mShader->use(); 36 | mShader->set("view", view); 37 | mShader->set("projection", projection); 38 | mShader->set("model", glm::mat4(1.f)); 39 | mModel->Draw(); 40 | 41 | RenderRGB(m_rgb); 42 | cv::flip(m_rgb, m_rgb, 0); 43 | cv::cvtColor(m_rgb, m_rgb, cv::COLOR_RGB2BGR); 44 | 45 | RenderDepth(m_depth,near,far); 46 | cv::flip(m_depth, m_depth, 0); 47 | m_depth.convertTo(m_depth, CV_16U); 48 | 49 | glBindFramebuffer(GL_FRAMEBUFFER, 0); 50 | } 51 | 52 | void RenderRGB(cv::Mat &img) { 53 | img = cv::Mat(m_height, m_width, CV_8UC3); 54 | glReadPixels(0, 0, m_width,m_height, GL_RGB, GL_UNSIGNED_BYTE, 55 | img.data); 56 | } 57 | 58 | void RenderDepth(cv::Mat &img, float near, float far) { 59 | img.create(m_height, m_width, CV_32F); 60 | glReadPixels(0, 0,m_width,m_height, GL_DEPTH_COMPONENT, GL_FLOAT, img.data); 61 | for (size_t i = 0; i < (size_t) img.rows * img.cols; ++i) { 62 | if (img.at(i) == 1 || img.at(i) == 0) img.at(i) = -1; 63 | else { 64 | const float zn = (img.at(i) * 2 - 1); 65 | const float ze = 2.0f * near * far / (far + near - zn * (far - near)); 66 | img.at(i) = ze * 1000;; 67 | } 68 | } 69 | } 70 | 71 | std::string vs = 72 | "#version 330 core\n" 73 | "layout (location = 0) in vec3 aPos;\n" 74 | "layout (location = 1) in vec3 aColor;\n" 75 | "out vec3 color;\n" 76 | "uniform mat4 model;\n" 77 | "uniform mat4 view;\n" 78 | "uniform mat4 projection;\n" 79 | "\n" 80 | "void main()\n" 81 | "{\n" 82 | " gl_Position = projection * view * model * vec4(aPos, 1.0);\n" 83 | " color = aColor;\n" 84 | "}"; 85 | 86 | std::string fs = 87 | "#version 330 core\n" 88 | "out vec4 FragColor;\n" 89 | "\n" 90 | "in vec3 color;\n" 91 | "\n" 92 | "void main()\n" 93 | "{\n" 94 | " FragColor = vec4(color,1);\n" 95 | "}"; 96 | }; 97 | } 98 | 99 | 100 | #endif //GRAPHSLAM_RENDERERSCANNET_H 101 | -------------------------------------------------------------------------------- /renderer/RendererType.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 5/13/21. 3 | // 4 | 5 | #ifndef GRAPHSLAM_RENDERERTYPE_H 6 | #define GRAPHSLAM_RENDERERTYPE_H 7 | 8 | namespace PSLAM { 9 | enum MeshRenderType { 10 | MeshRenderType_ScanNet, MeshRenderType_3RScan, MeshRenderType_Detect 11 | }; 12 | } 13 | 14 | #endif //GRAPHSLAM_RENDERERTYPE_H 15 | -------------------------------------------------------------------------------- /renderer/Scan3R_json_loader.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by sc on 2/5/21. 3 | // 4 | 5 | #ifndef RELOCALIZATION3D_SCAN3R_JSON_LOADER_H 6 | #define RELOCALIZATION3D_SCAN3R_JSON_LOADER_H 7 | 8 | #include 9 | namespace PSLAM::io { 10 | class Scan3RLoader { 11 | public: 12 | struct MovedObject { 13 | explicit MovedObject(int id) : id(id) {} 14 | 15 | int id; 16 | int symmetry = 0; 17 | Eigen::Matrix4f transformation; 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | }; 20 | 21 | typedef std::shared_ptr MovedObjectPtr; 22 | 23 | static inline MovedObjectPtr MakeMovedObjectPtr(int id) { 24 | return std::make_shared(id); 25 | } 26 | 27 | struct Ambiguity { 28 | int instance_source, instance_target; 29 | /// align source to target 30 | Eigen::Matrix4f transformation; 31 | 32 | Ambiguity(int source, int target) : instance_source(source), instance_target(target) {} 33 | }; 34 | 35 | typedef std::shared_ptr AmbiguityPtr; 36 | 37 | static inline AmbiguityPtr MakeAmbiguityPtr(int source, int target) { 38 | return std::make_shared(source, target); 39 | } 40 | 41 | struct ScanInfo; 42 | typedef std::shared_ptr ScanInfoPtr; 43 | 44 | struct ScanInfo { 45 | // general 46 | std::string scan_id; 47 | std::string type; 48 | 49 | // as a reference scan 50 | // SC::DisJointSet ambiguious_sets; 51 | std::unordered_map> ambiguities; 52 | std::unordered_map rescans; 53 | 54 | // as a rescan 55 | /// align rescan its reference scan 56 | Eigen::Matrix4f transformation{Eigen::Matrix4f::Identity()}; 57 | std::unordered_map moved_rigid_objects; 58 | 59 | explicit ScanInfo(std::string id) : scan_id(std::move(id)) {} 60 | 61 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 62 | }; 63 | 64 | static inline ScanInfoPtr MakeScanInfo(const std::string &scan_id) { 65 | return std::make_shared(scan_id); 66 | } 67 | 68 | explicit Scan3RLoader(const std::string &path) { 69 | Load(path); 70 | } 71 | 72 | bool IsRescan(const std::string &scan_id) { 73 | return reference2rescans.find(scan_id) == reference2rescans.end(); 74 | } 75 | 76 | bool IsReference(const std::string &scan_id) { 77 | return reference2rescans.find(scan_id) != reference2rescans.end(); 78 | } 79 | 80 | std::map rescanToReference; 81 | std::map > reference2rescans; 82 | std::map scaninfos; 83 | 84 | private: 85 | static void ReadMatrix(Eigen::Matrix4f &matrix, const json11::Json &json_matrix) { 86 | matrix = Eigen::Matrix4f::Identity(); 87 | int i = 0; 88 | for (auto &elem: json_matrix.array_items()) 89 | matrix(i++) = elem.number_value(); 90 | } 91 | 92 | void Load(const std::string &path) { 93 | auto scan3r = ORUtils::JsonUtils::LoadJson(path); 94 | for (const auto &s : scan3r.array_items()) { 95 | 96 | const std::string &reference_id = s["reference"].string_value(); 97 | const std::string &type = s["type"].string_value(); 98 | 99 | auto &refScanInfo = scaninfos[reference_id]; 100 | refScanInfo = MakeScanInfo(reference_id); 101 | refScanInfo->type = type; 102 | 103 | // Add ambiguity information 104 | const auto &ambiguities = s["ambiguity"].array_items(); 105 | for (const auto &ambiguity : ambiguities) { 106 | const auto &instance_source = ambiguity["instance_source"].int_value(); 107 | const auto &instance_target = ambiguity["instance_target"].int_value(); 108 | auto am = MakeAmbiguityPtr(instance_source, instance_target); 109 | refScanInfo->ambiguities[instance_source].push_back(am); 110 | ReadMatrix(am->transformation, ambiguity["transform"]); 111 | 112 | // auto &set = refScanInfo->ambiguious_sets; 113 | // if (!set.exist(instance_source)) set.add(instance_source); 114 | // if (!set.exist(instance_target)) set.add(instance_target); 115 | // set.unionSet(instance_source, instance_target); 116 | } 117 | 118 | 119 | const auto &scans = s["scans"].array_items(); 120 | // Add rescan information 121 | for (auto &scan: scans) { 122 | const std::string &scan_id = scan["reference"].string_value(); 123 | rescanToReference[scan_id] = reference_id; 124 | reference2rescans[reference_id].push_back(scan_id); 125 | 126 | auto scanInfo = MakeScanInfo(scan_id); 127 | refScanInfo->rescans[scan_id] = scanInfo; 128 | ReadMatrix(scanInfo->transformation, scan["transform"]); 129 | 130 | for (const auto &rigid : scan["rigid"].array_items()) { 131 | const int instance_id = rigid["instance_reference"].int_value(); 132 | MovedObjectPtr &movedObject = scanInfo->moved_rigid_objects[instance_id] = MakeMovedObjectPtr( 133 | instance_id); 134 | Eigen::Matrix4f &rigid_transform = movedObject->transformation; 135 | ReadMatrix(rigid_transform, rigid["transform"]); 136 | // The transformation in the json is actually from reference to rescan 137 | rigid_transform = rigid_transform.inverse().eval(); 138 | } 139 | } 140 | } 141 | } 142 | }; 143 | } 144 | 145 | #endif //RELOCALIZATION3D_SCAN3R_JSON_LOADER_H 146 | -------------------------------------------------------------------------------- /scripts/README.md: -------------------------------------------------------------------------------- 1 | # Run GraphSLAM on datasets 2 | 3 | ``` 4 | python RUN_GraphSLAM.py --help 5 | ``` 6 | 7 | -------------------------------------------------------------------------------- /scripts/RUN_GraphSLAM.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Sat Sept 28 16:19:52 2020 5 | 6 | @author: sc 7 | 8 | """ 9 | 10 | if __name__ == '__main__' and __package__ is None: 11 | from os import sys 12 | sys.path.append('../') 13 | # sys.path.append('../python/GCN') 14 | import multiprocessing as mp 15 | import subprocess, os, sys, time 16 | from pathlib import Path 17 | from tqdm import tqdm 18 | import shlex 19 | import argparse 20 | 21 | # from utils import util_parser 22 | # par_model = util_parser.Parse3RScanModel() 23 | # par_label = util_parser.ParseLabelMapping() 24 | parser = argparse.ArgumentParser(description='This script runs GraphSLAM on the target sequences provided with the --txt. ',formatter_class=argparse.ArgumentDefaultsHelpFormatter) 25 | parser.add_argument('--scans', type=str, 26 | default='/media/sc/SSD1TB/dataset/3RScan/data/3RScan/', help='to the directory contains scans',required=False) 27 | parser.add_argument('--pth_out', type=str, default='./result_reconstruction/', help='where to save the prediction',required=False) 28 | parser.add_argument('--pth_model', type=str, default='', help='where to save the prediction',required=True) 29 | parser.add_argument('--txt', type=str, 30 | default='/home/sc/research/PersistentSLAM/python/3DSSG/data/ScanNet20_InSeg_Full/validation_scans.txt', 31 | help='a txt file contains a set of scene id.') 32 | parser.add_argument('--pred_script', type=str, default='../bin/exe_GraphSLAM', help='prediction script') 33 | parser.add_argument('--fusion', type=int, default=1, help='Fusion or not.') 34 | parser.add_argument('--thread', type=int, default=1, help='The number of threads to be used.') 35 | parser.add_argument('--thread_slam', type=int, default=1, help='The number of threads to be used.') 36 | parser.add_argument('--debug', type=int, default=0, help='') 37 | parser.add_argument('--on_failed', type=int, default=0, help='run but skip successed.') 38 | args = parser.parse_args() 39 | 40 | 41 | 42 | def process(pth_in, pth_out, rendered): 43 | startTime = time.time() 44 | try: 45 | print('Processing file', pth_in) 46 | process = subprocess.Popen( 47 | [args.pred_script, 48 | '--pth_in',pth_in, 49 | '--pth_out',pth_out, 50 | '--pth_model',args.pth_model, 51 | '--fusion',str(args.fusion), 52 | '--save',str(1), 53 | '--binary',str(1), 54 | '--thread',str(args.thread_slam), 55 | '--rendered',str(rendered), 56 | '--verbose',str(0) 57 | ], stdout=subprocess.PIPE) 58 | while True: 59 | output = process.stdout.readline() 60 | if process.poll() is not None: 61 | break 62 | # if output == '' and process.poll() is not None: 63 | # break 64 | if output: 65 | print(output.strip()) 66 | output = process.poll() 67 | # return rc 68 | # output = subprocess.check_output([args.pred_script, 69 | # '--pth_in',pth_in, 70 | # '--pth_out',pth_out, 71 | # '--pth_model',args.pth_model, 72 | # '--fusion',str(args.fusion), 73 | # '--save',str(1), 74 | # '--binary',str(1), 75 | # '--thread',str(0), 76 | # '--rendered',str(rendered) 77 | # ], 78 | # stderr=subprocess.STDOUT) 79 | # sys.stdout.write(output.decode('utf-8')) 80 | good = True 81 | except subprocess.CalledProcessError as e: 82 | print('[Catched Error]', "command '{}' return with error (code {}): {}".format(e.cmd, e.returncode, e.output.decode('utf-8'))) # omit errors 83 | good = False 84 | endTime = time.time() 85 | print('Processing file', os.path.splitext(os.path.basename(pth_in))[0], 'finished') 86 | return [pth_in, endTime-startTime, good ] 87 | 88 | def get_file_in_folder(path): 89 | files =[] 90 | for (dirpath, dirnames, filenames) in os.walk(path): 91 | for filename in filenames: 92 | if filename == args.plyname: 93 | files.append( os.path.abspath( os.path.join(dirpath, filename)) ) 94 | # break 95 | return files 96 | def read_lines_from_file(filepath): 97 | with open(filepath) as f: 98 | content = f.readlines() 99 | # you may also want to remove whitespace characters like `\n` at the end of each line 100 | return [x.strip() for x in content] 101 | 102 | if __name__ == '__main__': 103 | if args.thread > 1: 104 | pool = mp.Pool(args.thread) 105 | pool.daemon = True 106 | 107 | ''' Read split ''' 108 | print('read split file..') 109 | ids = open(args.txt).read().splitlines() 110 | print('there are',len(ids),'sequences') 111 | 112 | # check output folder exist 113 | Path(args.pth_out).mkdir(parents=True, exist_ok=True) 114 | 115 | results=[] 116 | counter=0 117 | # should_skip=True 118 | 119 | scene_to_process = [] 120 | # scene_to_process = ['4fbad314-465b-2a5d-8445-9d021f278c1e', '20c993af-698f-29c5-84b2-972451f94cfb'] 121 | only_on_failed = args.on_failed>0 122 | for scan_id in tqdm(sorted(ids)): 123 | counter+=1 124 | 125 | # if counter < 236: continue 126 | if len(scene_to_process) > 0: 127 | if scan_id not in scene_to_process: 128 | continue 129 | 130 | if only_on_failed: 131 | if len(os.listdir(args.pth_out+'/'+scan_id))==3: 132 | continue 133 | # print('false now') 134 | # should_skip =False 135 | # if should_skip: continue 136 | 137 | 138 | # if counter < 35: continue 139 | if scan_id.find('scene') >=0: 140 | # ScanNet 141 | pth_in = os.path.join(args.scans, scan_id, scan_id+'.sens') 142 | pth_out = os.path.join(args.pth_out, scan_id) 143 | rendered = 1 144 | else: 145 | # 3RScan 146 | pth_in = os.path.join(args.scans,scan_id,'sequence') 147 | pth_out = os.path.join(args.pth_out, scan_id) 148 | rendered = 0 149 | 150 | 151 | if args.thread > 1: 152 | results.append( 153 | pool.apply_async(process,(pth_in,pth_out,rendered))) 154 | else: 155 | results.append(process(pth_in,pth_out,rendered)) 156 | 157 | if args.debug>0: 158 | break 159 | 160 | 161 | if args.thread > 1: 162 | pool.close() 163 | pool.join() 164 | if args.thread > 1: 165 | results = [r.get() for r in results] 166 | 167 | 168 | for r in results: 169 | if r[2] is False: 170 | print('the scans that failed:') 171 | print(r[0]) 172 | --------------------------------------------------------------------------------