├── legend_vertical.png ├── .gitmodules ├── class_colour_scheme.data ├── src ├── map_interface │ ├── ElasticFusionCuda.h │ ├── ElasticFusionCuda.cu │ ├── ElasticFusionInterface.h │ └── ElasticFusionInterface.cpp ├── gui │ ├── GuiCuda.h │ ├── Gui.h │ ├── GuiCuda.cu │ └── Gui.cpp ├── utilities │ ├── Types.h │ ├── RawLogReader.h │ ├── LiveLogReader.h │ ├── PNGLogReader.h │ ├── LogReader.h │ ├── JPEGLoader.h │ ├── ThreadMutexObject.h │ ├── LiveLogReader.cpp │ ├── RawLogReader.cpp │ ├── Array.h │ ├── Array.cpp │ ├── PNGLogReader.cpp │ ├── Stopwatch.h │ ├── OpenNI2Interface.h │ └── OpenNI2Interface.cpp ├── cnn_interface │ ├── CaffeInterface.h │ └── CaffeInterface.cpp ├── semantic_fusion │ ├── CRF │ │ ├── util.h │ │ ├── util.cpp │ │ ├── fastmath.h │ │ ├── densecrf.h │ │ ├── densecrf.cpp │ │ └── permutohedral.h │ ├── SemanticFusionCuda.h │ ├── SemanticFusionInterface.h │ ├── SemanticFusionInterface.cpp │ └── SemanticFusionCuda.cu └── main.cpp ├── FindGlog.cmake ├── CMakeLists.txt ├── CudaComputeTargetFlags.cmake ├── FindOpenNI2.cmake ├── README.md └── LICENSE.txt /legend_vertical.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Ewenwan/semanticfusion/HEAD/legend_vertical.png -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "elasticfusionpublic"] 2 | path = elasticfusionpublic 3 | url = https://bitbucket.org/dysonroboticslab/elasticfusionpublic.git 4 | branch = semanticfusion 5 | [submodule "caffe_semanticfusion"] 6 | path = caffe_semanticfusion 7 | url = https://bitbucket.org/dysonroboticslab/caffe_semanticfusion.git 8 | -------------------------------------------------------------------------------- /class_colour_scheme.data: -------------------------------------------------------------------------------- 1 | Class Name Id R G B NOTES 2 | ========================================= 3 | Background 0 20 20 20 DARK GREY 4 | Bed 1 0 128 128 TEAL 5 | Bookshelf 2 250 50 50 DARK RED 6 | Ceiling 3 102 0 204 PURPLE 7 | Chair 4 50 50 250 BRIGHT BLUE 8 | Floor 5 220 220 220 LIGHT GREY 9 | Furniture 6 255 69 20 ORANGE 10 | Objects 7 255 20 127 PINK 11 | Picture 8 50 50 150 DARK BLUE 12 | Sofa 9 222 180 140 TAN 13 | Table 10 50 250 50 BRIGHT GREEN 14 | Tv 11 255 215 0 GOLD 15 | Wall 12 150 150 150 GREY 16 | Window 13 0 255 255 CYAN 17 | -------------------------------------------------------------------------------- /src/map_interface/ElasticFusionCuda.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | void updateSurfelClasses(const int n, float* map_surfels, const float* classes, const float* probs, const float* class_colours, const float threshold); 23 | -------------------------------------------------------------------------------- /src/gui/GuiCuda.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | void colouredArgMax(int n, float const * probabilities, const int num_classes, float const * color_lookup, float* colour, float const * map, const int map_size,cudaTextureObject_t ids, const float threshold); 23 | -------------------------------------------------------------------------------- /src/utilities/Types.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef TYPES_H_ 20 | #define TYPES_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | typedef unsigned char* ImagePtr; 28 | typedef unsigned short* DepthPtr; 29 | 30 | struct ClassColour { 31 | ClassColour() 32 | : name(""), r(0), g(0), b(0) {} 33 | ClassColour(std::string name_, int r_, int g_, int b_) 34 | : name(name_), r(r_), g(g_), b(b_) {} 35 | std::string name; 36 | int r, g, b; 37 | }; 38 | 39 | #endif /* TYPES_H_ */ 40 | -------------------------------------------------------------------------------- /FindGlog.cmake: -------------------------------------------------------------------------------- 1 | # The following variables are optionally searched for defaults 2 | # GLOG_ROOT_DIR: Base directory where all GLOG components are found 3 | # 4 | # The following are set after configuration is done: 5 | # GLOG_FOUND 6 | # GLOG_INCLUDE_DIRS 7 | # GLOG_LIBRARIES 8 | 9 | include(FindPackageHandleStandardArgs) 10 | 11 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 12 | 13 | if(WIN32) 14 | find_path(GLOG_INCLUDE_DIR glog/logging.h 15 | PATHS ${GLOG_ROOT_DIR}/src/windows) 16 | else() 17 | find_path(GLOG_INCLUDE_DIR glog/logging.h 18 | PATHS ${GLOG_ROOT_DIR}) 19 | endif() 20 | 21 | if(MSVC) 22 | find_library(GLOG_LIBRARY_RELEASE libglog_static 23 | PATHS ${GLOG_ROOT_DIR} 24 | PATH_SUFFIXES Release) 25 | 26 | find_library(GLOG_LIBRARY_DEBUG libglog_static 27 | PATHS ${GLOG_ROOT_DIR} 28 | PATH_SUFFIXES Debug) 29 | 30 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 31 | else() 32 | find_library(GLOG_LIBRARY glog 33 | PATHS ${GLOG_ROOT_DIR} 34 | PATH_SUFFIXES 35 | lib 36 | lib64) 37 | endif() 38 | 39 | find_package_handle_standard_args(GLOG DEFAULT_MSG 40 | GLOG_INCLUDE_DIR GLOG_LIBRARY) 41 | 42 | if(GLOG_FOUND) 43 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 44 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 45 | endif() 46 | -------------------------------------------------------------------------------- /src/cnn_interface/CaffeInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef CAFFE_INTERFACE_H_ 20 | #define CAFFE_INTERFACE_H_ 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | class CaffeInterface { 31 | public: 32 | CaffeInterface() : initialised_(false) {} 33 | virtual ~CaffeInterface() {} 34 | bool Init(const std::string& model_path, const std::string& weights); 35 | std::shared_ptr > ProcessFrame( 36 | const ImagePtr rgb, const DepthPtr depth, 37 | const int height, const int width); 38 | int num_output_classes(); 39 | private: 40 | bool initialised_; 41 | std::unique_ptr > network_; 42 | std::shared_ptr > output_probabilities_; 43 | }; 44 | 45 | #endif /* CAFFE_INTERFACE_H_ */ 46 | -------------------------------------------------------------------------------- /src/utilities/RawLogReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef RAWLOGREADER_H_ 20 | #define RAWLOGREADER_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "LogReader.h" 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | class RawLogReader : public LogReader 36 | { 37 | public: 38 | RawLogReader(std::string file, bool flipColors); 39 | 40 | virtual ~RawLogReader(); 41 | 42 | void getNext(); 43 | 44 | void getBack(); 45 | 46 | int getNumFrames(); 47 | 48 | bool hasMore(); 49 | 50 | bool rewound(); 51 | 52 | void fastForward(int frame); 53 | 54 | const std::string getFile(); 55 | 56 | void setAuto(bool value); 57 | 58 | std::stack filePointers; 59 | 60 | private: 61 | void getCore(); 62 | }; 63 | 64 | #endif /* RAWLOGREADER_H_ */ 65 | -------------------------------------------------------------------------------- /src/semantic_fusion/CRF/util.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Philipp Krähenbühl 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the Stanford University nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY Philipp Krähenbühl ''AS IS'' AND ANY 17 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL Philipp Krähenbühl BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include "densecrf.h" 31 | #include "permutohedral.h" 32 | 33 | // Memory handling switches between SSE and new 34 | float* allocate ( size_t N ) ; 35 | void deallocate ( float *& ptr ) ; 36 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | if(POLICY CMP0007) 3 | cmake_policy(SET CMP0007 NEW) 4 | endif() 5 | 6 | project(SemanticFusion) 7 | 8 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}") 9 | 10 | find_package(CUDA REQUIRED) 11 | find_package(Glog REQUIRED) 12 | find_package(OpenCV REQUIRED) 13 | find_package(OpenNI2 REQUIRED) 14 | 15 | add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/elasticfusionpublic/Core/src") 16 | add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/caffe_semanticfusion") 17 | 18 | file(GLOB_RECURSE srcs src/*.cpp) 19 | file(GLOB_RECURSE cuda src/*.cu) 20 | 21 | set(CUDA_ARCH_BIN "30 35 50 52 61" CACHE STRING "Specify 'real' GPU arch to build binaries for, BIN(PTX) format is supported. Example: 1.3 2.1(1.3) or 13 21(13)") 22 | include(CudaComputeTargetFlags.cmake) 23 | APPEND_TARGET_ARCH_FLAGS() 24 | 25 | set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "--ftz=true;--prec-div=false;--prec-sqrt=false") 26 | set(CUDA_HOST_COMPILER gcc-5) 27 | 28 | set(CMAKE_CXX_FLAGS "-O3 -msse2 -msse3 -Wall -std=c++11") 29 | 30 | cuda_add_library(semantic_fusion_cuda_library SHARED ${cuda} ) 31 | 32 | add_executable(${CMAKE_PROJECT_NAME} 33 | ${srcs} 34 | ) 35 | 36 | target_link_libraries(${CMAKE_PROJECT_NAME} 37 | ${CUDA_LIBRARIES} 38 | ${GLOG_LIBRARY} 39 | ${OpenCV_LIBS} 40 | ${OPENNI2_LIBRARY} 41 | semantic_fusion_cuda_library 42 | efusion 43 | caffe 44 | ) 45 | 46 | target_include_directories(${CMAKE_PROJECT_NAME} PUBLIC 47 | $ 48 | ${EIGEN_INCLUDE_DIRS} 49 | ${CUDA_INCLUDE_DIRS} 50 | ${GLOG_INCLUDE_DIR} 51 | ${OPENNI2_INCLUDE_DIR} 52 | ) 53 | -------------------------------------------------------------------------------- /src/utilities/LiveLogReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef LIVELOGREADER_H_ 20 | #define LIVELOGREADER_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "LogReader.h" 28 | #include "OpenNI2Interface.h" 29 | 30 | class LiveLogReader : public LogReader 31 | { 32 | public: 33 | LiveLogReader(std::string file, bool flipColors); 34 | 35 | virtual ~LiveLogReader(); 36 | 37 | void getNext(); 38 | 39 | int getNumFrames(); 40 | 41 | bool hasMore(); 42 | 43 | bool rewound() 44 | { 45 | return false; 46 | } 47 | 48 | bool is_valid() 49 | { 50 | return valid; 51 | } 52 | 53 | void getBack() 54 | { 55 | 56 | } 57 | 58 | void fastForward(int frame) 59 | { 60 | 61 | } 62 | 63 | void setAuto(bool value); 64 | 65 | const std::string getFile() { 66 | return file; 67 | }; 68 | 69 | OpenNI2Interface * asus; 70 | 71 | private: 72 | int64_t lastFrameTime; 73 | int lastGot; 74 | bool valid; 75 | }; 76 | 77 | #endif /* LIVELOGREADER_H_ */ 78 | -------------------------------------------------------------------------------- /src/semantic_fusion/SemanticFusionCuda.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include 20 | 21 | void fuseSemanticProbabilities(cudaTextureObject_t ids, const int ids_width, const int ids_height, 22 | const float* probabilities, const int prob_width, const int prob_height, 23 | const int prob_channels,float* map_table, float* map_max, 24 | const int map_size); 25 | 26 | void updateProbabilityTable(int* deleted_ids, const int num_deleted, const int current_table_size, 27 | float const* probability_table, const int prob_width, const int prob_height, 28 | const int new_prob_width, float* new_probability_table, 29 | float const* map_table, float* new_map_table); 30 | 31 | void renderProbabilityMap(cudaTextureObject_t ids, const int ids_width, const int ids_height, 32 | const float* probability_table, const int prob_width, const int prob_height, 33 | float* rendered_probabilities); 34 | 35 | 36 | void updateMaxClass(const int n, const float* probabilities, const int classes, 37 | float* map_max, const int map_size); 38 | -------------------------------------------------------------------------------- /CudaComputeTargetFlags.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Compute target flags macros by Anatoly Baksheev 3 | # 4 | # Usage in CmakeLists.txt: 5 | # include(CudaComputeTargetFlags.cmake) 6 | # APPEND_TARGET_ARCH_FLAGS() 7 | 8 | #compute flags macros 9 | MACRO(CUDA_COMPUTE_TARGET_FLAGS arch_bin arch_ptx cuda_nvcc_target_flags) 10 | string(REGEX REPLACE "\\." "" ARCH_BIN_WITHOUT_DOTS "${${arch_bin}}") 11 | string(REGEX REPLACE "\\." "" ARCH_PTX_WITHOUT_DOTS "${${arch_ptx}}") 12 | 13 | set(cuda_computer_target_flags_temp "") 14 | 15 | # Tell NVCC to add binaries for the specified GPUs 16 | string(REGEX MATCHALL "[0-9()]+" ARCH_LIST "${ARCH_BIN_WITHOUT_DOTS}") 17 | foreach(ARCH IN LISTS ARCH_LIST) 18 | if (ARCH MATCHES "([0-9]+)\\(([0-9]+)\\)") 19 | # User explicitly specified PTX for the concrete BIN 20 | set(cuda_computer_target_flags_temp ${cuda_computer_target_flags_temp} -gencode arch=compute_${CMAKE_MATCH_2},code=sm_${CMAKE_MATCH_1}) 21 | else() 22 | # User didn't explicitly specify PTX for the concrete BIN, we assume PTX=BIN 23 | set(cuda_computer_target_flags_temp ${cuda_computer_target_flags_temp} -gencode arch=compute_${ARCH},code=sm_${ARCH}) 24 | endif() 25 | endforeach() 26 | 27 | # Tell NVCC to add PTX intermediate code for the specified architectures 28 | string(REGEX MATCHALL "[0-9]+" ARCH_LIST "${ARCH_PTX_WITHOUT_DOTS}") 29 | foreach(ARCH IN LISTS ARCH_LIST) 30 | set(cuda_computer_target_flags_temp ${cuda_computer_target_flags_temp} -gencode arch=compute_${ARCH},code=compute_${ARCH}) 31 | endforeach() 32 | 33 | set(${cuda_nvcc_target_flags} ${cuda_computer_target_flags_temp}) 34 | ENDMACRO() 35 | 36 | MACRO(APPEND_TARGET_ARCH_FLAGS) 37 | set(cuda_nvcc_target_flags "") 38 | CUDA_COMPUTE_TARGET_FLAGS(CUDA_ARCH_BIN CUDA_ARCH_PTX cuda_nvcc_target_flags) 39 | if (cuda_nvcc_target_flags) 40 | message(STATUS "CUDA NVCC target flags: ${cuda_nvcc_target_flags}") 41 | list(APPEND CUDA_NVCC_FLAGS ${cuda_nvcc_target_flags}) 42 | endif() 43 | ENDMACRO() -------------------------------------------------------------------------------- /src/semantic_fusion/CRF/util.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Philipp Krähenbühl 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the Stanford University nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY Philipp Krähenbühl ''AS IS'' AND ANY 17 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL Philipp Krähenbühl BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include "util.h" 29 | #include 30 | 31 | float* allocate(size_t N) { 32 | float * r = NULL; 33 | if (N>0) 34 | #ifdef SSE_DENSE_CRF 35 | r = (float*)_mm_malloc( N*sizeof(float)+16, 16 ); 36 | #else 37 | r = new float[N]; 38 | #endif 39 | memset( r, 0, sizeof(float)*N); 40 | return r; 41 | } 42 | void deallocate(float*& ptr) { 43 | if (ptr) 44 | #ifdef SSE_DENSE_CRF 45 | _mm_free( ptr ); 46 | #else 47 | delete[] ptr; 48 | #endif 49 | ptr = NULL; 50 | } 51 | -------------------------------------------------------------------------------- /src/utilities/PNGLogReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef PNGLOGREADER_H_ 20 | #define PNGLOGREADER_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "LogReader.h" 28 | #include 29 | #include 30 | 31 | struct FrameInfo { 32 | int64_t timestamp; 33 | std::string depth_path; 34 | std::string rgb_path; 35 | std::string depth_id; 36 | std::string rgb_id; 37 | bool labeled_frame; 38 | std::string frame_id; 39 | }; 40 | 41 | class PNGLogReader : public LogReader 42 | { 43 | public: 44 | PNGLogReader(std::string file, std::string labels_file); 45 | 46 | virtual ~PNGLogReader(); 47 | 48 | void getNext(); 49 | 50 | int getNumFrames(); 51 | 52 | bool hasMore(); 53 | 54 | bool isLabeledFrame(); 55 | 56 | std::string getLabelFrameId(); 57 | 58 | bool rewound() { return false; } 59 | 60 | void getBack() { } 61 | 62 | void fastForward(int frame) {} 63 | 64 | void setAuto(bool value) {} 65 | 66 | const std::string getFile() { 67 | return file; 68 | }; 69 | 70 | bool hasDepthFilled() { return has_depth_filled; } 71 | 72 | private: 73 | int64_t lastFrameTime; 74 | int lastGot; 75 | std::vector frames_; 76 | Bytef * decompressionBufferDepthFilled; 77 | bool has_depth_filled; 78 | public: 79 | int num_labelled; 80 | }; 81 | 82 | #endif /* PNGLOGREADER_H_ */ 83 | -------------------------------------------------------------------------------- /FindOpenNI2.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Find OpenNI2 3 | # 4 | # This sets the following variables: 5 | # OPENNI2_FOUND - True if OPENNI was found. 6 | # OPENNI2_INCLUDE_DIRS - Directories containing the OPENNI include files. 7 | # OPENNI2_LIBRARIES - Libraries needed to use OPENNI. 8 | 9 | find_package(PkgConfig) 10 | if(${CMAKE_VERSION} VERSION_LESS 2.8.2) 11 | pkg_check_modules(PC_OPENNI openni2-dev) 12 | else() 13 | pkg_check_modules(PC_OPENNI QUIET openni2-dev) 14 | endif() 15 | 16 | set(OPENNI2_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER}) 17 | 18 | #add a hint so that it can find it without the pkg-config 19 | find_path(OPENNI2_INCLUDE_DIR OpenNI.h 20 | HINTS 21 | ${PC_OPENNI_INCLUDEDIR} 22 | ${PC_OPENNI_INCLUDE_DIRS} 23 | PATHS 24 | "${PROGRAM_FILES}/OpenNI2/Include" 25 | "${CMAKE_SOURCE_DIR}/../OpenNI2/Include" 26 | "${CMAKE_SOURCE_DIR}/../../OpenNI2/Include" 27 | "${CMAKE_SOURCE_DIR}/../../../OpenNI2/Include" 28 | "${CMAKE_SOURCE_DIR}/../../../code/OpenNI2/Include" 29 | /usr/include 30 | /user/include 31 | PATH_SUFFIXES openni2 ni2 32 | ) 33 | 34 | if(${CMAKE_CL_64}) 35 | set(OPENNI_PATH_SUFFIXES lib64) 36 | else() 37 | set(OPENNI_PATH_SUFFIXES lib) 38 | endif() 39 | 40 | #add a hint so that it can find it without the pkg-config 41 | find_library(OPENNI2_LIBRARY 42 | NAMES OpenNI2 43 | HINTS 44 | ${PC_OPENNI_LIBDIR} 45 | ${PC_OPENNI_LIBRARY_DIRS} 46 | PATHS 47 | "${PROGRAM_FILES}}/OpenNI2/Redist" 48 | "${PROGRAM_FILES}/OpenNI2" 49 | "${CMAKE_SOURCE_DIR}/../OpenNI2/Bin/x64-Release" 50 | "${CMAKE_SOURCE_DIR}/../../OpenNI2/Bin/x64-Release" 51 | "${CMAKE_SOURCE_DIR}/../../../OpenNI2/Bin/x64-Release" 52 | "${CMAKE_SOURCE_DIR}/../../../code/OpenNI2/Bin/x64-Release" 53 | /usr/lib 54 | /user/lib 55 | PATH_SUFFIXES ${OPENNI_PATH_SUFFIXES} 56 | ) 57 | 58 | set(OPENNI2_INCLUDE_DIRS ${OPENNI2_INCLUDE_DIR}) 59 | set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY}) 60 | 61 | include(FindPackageHandleStandardArgs) 62 | find_package_handle_standard_args(OpenNI2 DEFAULT_MSG 63 | OPENNI2_LIBRARY OPENNI2_INCLUDE_DIR) 64 | 65 | mark_as_advanced(OPENNI2_LIBRARY OPENNI2_INCLUDE_DIR) 66 | -------------------------------------------------------------------------------- /src/map_interface/ElasticFusionCuda.cu: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | #define gpuErrChk(ans) { gpuAssert((ans), __FILE__, __LINE__); } 25 | 26 | inline void gpuAssert(cudaError_t code, const char *file, int line, bool 27 | abort=true) { 28 | if (code != cudaSuccess) { 29 | fprintf(stderr,"GPUassert: %s %s %d\n", 30 | cudaGetErrorString(code), file, line); 31 | if (abort) exit(code); 32 | } 33 | } 34 | 35 | __global__ 36 | void updateSurfelClassesKernel(const int n, float* map_surfels, const float* classes, const float* probs, const float* class_colours, const float threshold) 37 | { 38 | const int surfel_size = 12; 39 | const int surfel_color_offset = 5; 40 | const int id = blockIdx.x * blockDim.x + threadIdx.x; 41 | if (id < n) { 42 | const int class_id = static_cast(classes[id]); 43 | const float prob = probs[id]; 44 | if (class_id >= 0 && prob > threshold) { 45 | map_surfels[id * surfel_size + surfel_color_offset] = class_colours[class_id]; 46 | } else { 47 | map_surfels[id * surfel_size + surfel_color_offset] = -1.0f; 48 | } 49 | } 50 | } 51 | 52 | __host__ 53 | void updateSurfelClasses(const int n, float* map_surfels, const float* classes, const float* probs, const float* class_colours, const float threshold) 54 | { 55 | const int threads = 512; 56 | const int blocks = (n + threads - 1) / threads; 57 | dim3 dimGrid(blocks); 58 | dim3 dimBlock(threads); 59 | updateSurfelClassesKernel<<>>(n,map_surfels,classes,probs,class_colours,threshold); 60 | gpuErrChk(cudaGetLastError()); 61 | gpuErrChk(cudaDeviceSynchronize()); 62 | } 63 | -------------------------------------------------------------------------------- /src/utilities/LogReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef LOGREADER_H_ 20 | #define LOGREADER_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "JPEGLoader.h" 29 | 30 | class LogReader 31 | { 32 | public: 33 | LogReader(std::string file, bool flipColors) 34 | : flipColors(flipColors), 35 | timestamp(0), 36 | depth(0), 37 | rgb(0), 38 | currentFrame(0), 39 | decompressionBufferDepth(0), 40 | decompressionBufferImage(0), 41 | file(file), 42 | width(Resolution::getInstance().width()), 43 | height(Resolution::getInstance().height()), 44 | numPixels(width * height) 45 | {} 46 | 47 | virtual ~LogReader() 48 | {} 49 | 50 | virtual void getNext() = 0; 51 | 52 | virtual int getNumFrames() = 0; 53 | 54 | virtual bool hasDepthFilled() { return false;} 55 | 56 | virtual bool hasMore() = 0; 57 | 58 | virtual bool rewound() = 0; 59 | 60 | virtual void getBack() = 0; 61 | 62 | virtual void fastForward(int frame) = 0; 63 | 64 | virtual const std::string getFile() = 0; 65 | 66 | virtual void setAuto(bool value) = 0; 67 | 68 | virtual bool isLabeledFrame() { return false; } 69 | virtual bool is_valid() { return false; } 70 | virtual std::string getLabelFrameId() { return ""; } 71 | 72 | bool flipColors; 73 | int64_t timestamp; 74 | 75 | unsigned short * depth; 76 | unsigned char * rgb; 77 | int currentFrame; 78 | 79 | unsigned short * depthfilled; 80 | 81 | protected: 82 | Bytef * decompressionBufferDepth; 83 | Bytef * decompressionBufferImage; 84 | unsigned char * depthReadBuffer; 85 | unsigned char * imageReadBuffer; 86 | int32_t depthSize; 87 | int32_t imageSize; 88 | 89 | const std::string file; 90 | FILE * fp; 91 | int32_t numFrames; 92 | int width; 93 | int height; 94 | int numPixels; 95 | 96 | JPEGLoader jpeg; 97 | }; 98 | 99 | #endif /* LOGREADER_H_ */ 100 | -------------------------------------------------------------------------------- /src/gui/Gui.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef GUI_H_ 20 | #define GUI_H_ 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | struct ClassIdInput; 35 | 36 | class Gui { 37 | public: 38 | //enum SelectProbabilityMap {Books,Chairs,Floor}; 39 | Gui(bool live_capture,std::vector class_colour_lookup, const int segmentation_width, const int segmentation_height); 40 | virtual ~Gui(); 41 | 42 | void preCall(); 43 | void renderMap(const std::unique_ptr& map); 44 | void postCall(); 45 | void displayArgMaxClassColouring(const std::string & id, float* device_ptr, int channels,const float* map, const int map_size, cudaTextureObject_t ids, const float threshold); 46 | void displayRawNetworkPredictions(const std::string & id, float* device_ptr); 47 | void displayImg(const std::string & id, GPUTexture * img); 48 | 49 | bool reset() const { return pangolin::Pushed(*reset_.get()); } 50 | bool paused() const { return *pause_.get(); } 51 | bool step() const { return pangolin::Pushed(*step_.get()); } 52 | bool tracking() const { return *tracking_.get(); } 53 | bool class_colours() const { return *class_view_.get(); } 54 | 55 | private: 56 | int width_; 57 | int height_; 58 | const int segmentation_width_; 59 | const int segmentation_height_; 60 | int panel_; 61 | std::vector class_colour_lookup_; 62 | float* class_colour_lookup_gpu_; 63 | float* segmentation_rendering_gpu_; 64 | 65 | std::unique_ptr> reset_; 66 | std::unique_ptr> pause_; 67 | std::unique_ptr> step_; 68 | std::unique_ptr> tracking_; 69 | std::unique_ptr> class_view_; 70 | std::unique_ptr> class_choice_; 71 | std::unique_ptr probability_texture_array_; 72 | std::unique_ptr rendered_segmentation_texture_array_; 73 | pangolin::GlRenderBuffer* render_buffer_; 74 | pangolin::GlFramebuffer* color_frame_buffer_; 75 | GPUTexture* color_texture_; 76 | pangolin::OpenGlRenderState s_cam_; 77 | }; 78 | 79 | 80 | #endif /* GUI_H_ */ 81 | -------------------------------------------------------------------------------- /src/utilities/JPEGLoader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef TOOLS_JPEGLOADER_H_ 20 | #define TOOLS_JPEGLOADER_H_ 21 | 22 | extern "C" 23 | { 24 | #include "jpeglib.h" 25 | } 26 | 27 | #include 28 | #include 29 | 30 | static void jpegFail(j_common_ptr cinfo) 31 | { 32 | assert(false && "JPEG decoding error!"); 33 | } 34 | 35 | static void doNothing(j_decompress_ptr) 36 | { 37 | 38 | } 39 | 40 | class JPEGLoader 41 | { 42 | public: 43 | JPEGLoader() 44 | {} 45 | 46 | void readData(unsigned char * src, const int numBytes, unsigned char * data) 47 | { 48 | jpeg_decompress_struct cinfo; // IJG JPEG codec structure 49 | 50 | jpeg_error_mgr errorMgr; 51 | 52 | errorMgr.error_exit = jpegFail; 53 | 54 | cinfo.err = jpeg_std_error(&errorMgr); 55 | 56 | jpeg_create_decompress(&cinfo); 57 | 58 | jpeg_source_mgr srcMgr; 59 | 60 | cinfo.src = &srcMgr; 61 | 62 | // Prepare for suspending reader 63 | srcMgr.init_source = doNothing; 64 | srcMgr.resync_to_restart = jpeg_resync_to_restart; 65 | srcMgr.term_source = doNothing; 66 | srcMgr.next_input_byte = src; 67 | srcMgr.bytes_in_buffer = numBytes; 68 | 69 | jpeg_read_header(&cinfo, TRUE); 70 | 71 | jpeg_calc_output_dimensions(&cinfo); 72 | 73 | jpeg_start_decompress(&cinfo); 74 | 75 | int width = cinfo.output_width; 76 | int height = cinfo.output_height; 77 | 78 | JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)((j_common_ptr)&cinfo, JPOOL_IMAGE, width * 4, 1); 79 | 80 | for(; height--; data += (width * 3)) 81 | { 82 | jpeg_read_scanlines(&cinfo, buffer, 1); 83 | 84 | unsigned char * bgr = (unsigned char *)buffer[0]; 85 | unsigned char * rgb = (unsigned char *)data; 86 | 87 | for(int i = 0; i < width; i++, bgr += 3, rgb += 3) 88 | { 89 | unsigned char t0 = bgr[0], t1 = bgr[1], t2 = bgr[2]; 90 | rgb[2] = t0; rgb[1] = t1; rgb[0] = t2; 91 | } 92 | } 93 | 94 | jpeg_finish_decompress(&cinfo); 95 | 96 | jpeg_destroy_decompress(&cinfo); 97 | } 98 | }; 99 | 100 | 101 | #endif /* TOOLS_JPEGLOADER_H_ */ 102 | -------------------------------------------------------------------------------- /src/semantic_fusion/CRF/fastmath.h: -------------------------------------------------------------------------------- 1 | // This code here comes from http://www.xnoiz.co.cc/fast-exp-x/ 2 | 3 | /* 4 | Copyright (c) 2011, Philipp Krähenbühl 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Stanford University nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY Philipp Krähenbühl ''AS IS'' AND ANY 19 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL Philipp Krähenbühl BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #pragma once 31 | 32 | inline float fast_log2 (float val) { 33 | int * const exp_ptr = reinterpret_cast (&val); 34 | int x = *exp_ptr; 35 | const int log_2 = ((x >> 23) & 255) - 128; 36 | x &= ~(255 << 23); 37 | x += 127 << 23; 38 | *exp_ptr = x; 39 | 40 | return (val + log_2); 41 | } 42 | inline float fast_log (const float &val) { 43 | return (fast_log2 (val) * 0.69314718f); 44 | } 45 | inline float very_fast_exp(float x) { 46 | // err <= 3e-3 47 | // return 1 48 | // -x*(0.9664 49 | // -x*(0.3536)); 50 | 51 | return 1 52 | -x*(0.9999999995f 53 | -x*(0.4999999206f 54 | 55 | -x*(0.1666653019f 56 | -x*(0.0416573475f 57 | -x*(0.0083013598f 58 | 59 | -x*(0.0013298820f 60 | -x*(0.0001413161f))))))); 61 | } 62 | inline float fast_exp(float x) { 63 | bool lessZero = true; 64 | if (x < 0) { 65 | 66 | lessZero = false; 67 | x = -x; 68 | 69 | } 70 | // This diry little trick only works because of the normalization and the fact that one element in the normalization is 1 71 | if (x > 20) 72 | return 0; 73 | int mult = 0; 74 | 75 | while (x > 0.69*2*2*2) { 76 | 77 | mult+=3; 78 | x /= 8.0f; 79 | } 80 | 81 | while (x > 0.69*2*2) { 82 | mult+=2; 83 | 84 | x /= 4.0f; 85 | } 86 | while (x > 0.69) { 87 | 88 | mult++; 89 | x /= 2.0f; 90 | } 91 | 92 | x = very_fast_exp(x); 93 | while (mult) { 94 | 95 | mult--; 96 | x = x*x; 97 | } 98 | 99 | if (lessZero) { 100 | return 1 / x; 101 | 102 | } else { 103 | return x; 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /src/map_interface/ElasticFusionInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef ELASTIC_FUSION_INTERFACE_H_ 20 | #define ELASTIC_FUSION_INTERFACE_H_ 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | class ElasticFusionInterface { 30 | public: 31 | // NOTE this must be performed in the header to globally initialise these 32 | // variables... 33 | ElasticFusionInterface() 34 | : initialised_(false) 35 | , height_(480) 36 | , width_(640) 37 | , tracking_only_(false) 38 | { 39 | Resolution::getInstance(width_, height_); 40 | // Primesense intrinsics 41 | Intrinsics::getInstance(528, 528, width_ / 2, height_ / 2); 42 | } 43 | virtual ~ElasticFusionInterface(); 44 | 45 | virtual bool Init(std::vector class_colour_lookup); 46 | virtual bool ProcessFrame(const ImagePtr rgb, const DepthPtr depth, const int64_t timestamp); 47 | int height() const { return height_; } 48 | int width() const { return width_; } 49 | 50 | const std::vector& GetSurfelIdsCpu(); 51 | cudaTextureObject_t GetSurfelIdsGpu(); 52 | void UpdateSurfelClass(const int surfel_id, const int class_id); 53 | void UpdateSurfelClassGpu(const int n, const float* surfelclasses, const float* surfelprobs, const float threshold); 54 | 55 | int* GetDeletedSurfelIdsGpu(); 56 | 57 | float* GetMapSurfelsGpu() { 58 | if (elastic_fusion_) { 59 | return elastic_fusion_->getGlobalModel().getMapSurfelsGpu(); 60 | } 61 | return nullptr; 62 | } 63 | 64 | int GetMapSurfelCount() { 65 | if (elastic_fusion_) { 66 | return elastic_fusion_->getGlobalModel().lastCount(); 67 | } 68 | return 0; 69 | } 70 | 71 | int GetMapSurfelDeletedCount() { 72 | if (elastic_fusion_ && !tracking_only_) { 73 | return elastic_fusion_->getGlobalModel().deletedCount(); 74 | } 75 | return 0; 76 | } 77 | 78 | void RenderMapToBoundGlBuffer(const pangolin::OpenGlRenderState& camera, const bool classes); 79 | GPUTexture* getRawImageTexture(); 80 | GPUTexture* getIdsTexture(); 81 | 82 | void setTrackingOnly(const bool tracking) { 83 | if (elastic_fusion_) { 84 | elastic_fusion_->setTrackingOnly(tracking); 85 | tracking_only_ = tracking; 86 | } 87 | } 88 | 89 | private: 90 | bool initialised_; 91 | int height_; 92 | int width_; 93 | std::unique_ptr elastic_fusion_; 94 | std::vector surfel_ids_; 95 | std::vector class_color_lookup_; 96 | float* class_color_lookup_gpu_; 97 | bool tracking_only_; 98 | }; 99 | 100 | #endif /* ELASTIC_FUSION_INTERFACE_H_ */ 101 | -------------------------------------------------------------------------------- /src/utilities/ThreadMutexObject.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef THREADMUTEXOBJECT_H_ 20 | #define THREADMUTEXOBJECT_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | template 28 | class ThreadMutexObject 29 | { 30 | public: 31 | ThreadMutexObject() 32 | {} 33 | 34 | ThreadMutexObject(T initialValue) 35 | : object(initialValue), 36 | lastCopy(initialValue) 37 | {} 38 | 39 | void assign(T newValue) 40 | { 41 | mutex.lock(); 42 | 43 | object = lastCopy = newValue; 44 | 45 | mutex.unlock(); 46 | } 47 | 48 | std::mutex & getMutex() 49 | { 50 | return mutex; 51 | } 52 | 53 | T & getReference() 54 | { 55 | return object; 56 | } 57 | 58 | void assignAndNotifyAll(T newValue) 59 | { 60 | mutex.lock(); 61 | 62 | object = newValue; 63 | 64 | signal.notify_all(); 65 | 66 | mutex.unlock(); 67 | } 68 | 69 | void notifyAll() 70 | { 71 | mutex.lock(); 72 | 73 | signal.notify_all(); 74 | 75 | mutex.unlock(); 76 | } 77 | 78 | T getValue() 79 | { 80 | mutex.lock(); 81 | 82 | lastCopy = object; 83 | 84 | mutex.unlock(); 85 | 86 | return lastCopy; 87 | } 88 | 89 | T waitForSignal() 90 | { 91 | mutex.lock(); 92 | 93 | signal.wait(mutex); 94 | 95 | lastCopy = object; 96 | 97 | mutex.unlock(); 98 | 99 | return lastCopy; 100 | } 101 | 102 | T getValueWait(int wait = 33000) 103 | { 104 | std::this_thread::sleep_for(std::chrono::microseconds(wait)); 105 | 106 | mutex.lock(); 107 | 108 | lastCopy = object; 109 | 110 | mutex.unlock(); 111 | 112 | return lastCopy; 113 | } 114 | 115 | T & getReferenceWait(int wait = 33000) 116 | { 117 | std::this_thread::sleep_for(std::chrono::microseconds(wait)); 118 | 119 | mutex.lock(); 120 | 121 | lastCopy = object; 122 | 123 | mutex.unlock(); 124 | 125 | return lastCopy; 126 | } 127 | 128 | void operator++(int) 129 | { 130 | mutex.lock(); 131 | 132 | object++; 133 | 134 | mutex.unlock(); 135 | } 136 | 137 | private: 138 | T object; 139 | T lastCopy; 140 | std::mutex mutex; 141 | std::condition_variable_any signal; 142 | }; 143 | 144 | #endif /* THREADMUTEXOBJECT_H_ */ 145 | -------------------------------------------------------------------------------- /src/gui/GuiCuda.cu: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | #define gpuErrChk(ans) { gpuAssert((ans), __FILE__, __LINE__); } 25 | 26 | inline void gpuAssert(cudaError_t code, const char *file, int line, bool 27 | abort=true) { 28 | if (code != cudaSuccess) { 29 | fprintf(stderr,"GPUassert: %s %s %d\n", 30 | cudaGetErrorString(code), file, line); 31 | if (abort) exit(code); 32 | } 33 | } 34 | 35 | __global__ 36 | void colouredArgMaxKernel(int n, float const* probabilities, const int num_classes, float const* color_lookup, float* colour, float const* map_max, const int map_size,cudaTextureObject_t ids, const float threshold) 37 | { 38 | const int id = blockIdx.x * blockDim.x + threadIdx.x; 39 | if (id < n) { 40 | const int y = id / 640; 41 | const int x = id - (y * 640); 42 | const int start_windowx = (x - 1) > 0 ? (x - 1) : 0; 43 | const int start_windowy = (y - 1) > 0 ? (y - 1) : 0; 44 | const int end_windowx = (x + 1) < 640 ? (x + 1) : 639; 45 | const int end_windowy = (y + 1) < 480 ? (y + 1) : 479; 46 | 47 | int max_class_id = -1; 48 | float max_class_prob = threshold; 49 | for (int sx = start_windowx; sx <= end_windowx; ++sx) { 50 | for (int sy = start_windowy; sy <= end_windowy; ++sy) { 51 | const int surfel_id = tex2D(ids,sx,sy); 52 | if (surfel_id > 0) { 53 | float const* id_probabilities = map_max + surfel_id; 54 | if (id_probabilities[map_size] > max_class_prob) { 55 | max_class_id = static_cast(id_probabilities[0]); 56 | max_class_prob = id_probabilities[map_size]; 57 | } 58 | } 59 | } 60 | } 61 | 62 | float* local_colour = colour + (id * 4); 63 | if (max_class_id >= 0) { 64 | local_colour[0] = color_lookup[max_class_id * 3 + 0]; 65 | local_colour[1] = color_lookup[max_class_id * 3 + 1]; 66 | local_colour[2] = color_lookup[max_class_id * 3 + 2]; 67 | local_colour[3] = 1.0f; 68 | } else { 69 | local_colour[0] = 0.0; 70 | local_colour[1] = 0.0; 71 | local_colour[2] = 0.0; 72 | local_colour[3] = 1.0f; 73 | } 74 | } 75 | } 76 | 77 | __host__ 78 | void colouredArgMax(int n, float const * probabilities, const int num_classes, float const* color_lookup, float* colour, float const * map, const int map_size,cudaTextureObject_t ids, const float threshold) 79 | { 80 | const int threads = 512; 81 | const int blocks = (n + threads - 1) / threads; 82 | dim3 dimGrid(blocks); 83 | dim3 dimBlock(threads); 84 | colouredArgMaxKernel<<>>(n,probabilities,num_classes,color_lookup,colour,map,map_size,ids,threshold); 85 | gpuErrChk(cudaGetLastError()); 86 | gpuErrChk(cudaDeviceSynchronize()); 87 | } 88 | -------------------------------------------------------------------------------- /src/utilities/LiveLogReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "LiveLogReader.h" 20 | 21 | LiveLogReader::LiveLogReader(std::string file, bool flipColors) 22 | : LogReader(file, flipColors), 23 | lastFrameTime(-1), 24 | lastGot(-1), 25 | valid(false) 26 | { 27 | std::cout << "Creating live capture... "; std::cout.flush(); 28 | 29 | asus = new OpenNI2Interface(Resolution::getInstance().width(), Resolution::getInstance().height()); 30 | 31 | decompressionBufferDepth = new Bytef[Resolution::getInstance().numPixels() * 2]; 32 | 33 | decompressionBufferImage = new Bytef[Resolution::getInstance().numPixels() * 3]; 34 | 35 | if(!asus->ok()) 36 | { 37 | std::cout << "failed!" << std::endl; 38 | std::cout << asus->error(); 39 | valid = false; 40 | } 41 | else 42 | { 43 | std::cout << "success!" << std::endl; 44 | 45 | std::cout << "Waiting for first frame"; std::cout.flush(); 46 | 47 | int lastDepth = asus->latestDepthIndex.getValue(); 48 | 49 | do 50 | { 51 | usleep(33333); 52 | std::cout << "."; std::cout.flush(); 53 | lastDepth = asus->latestDepthIndex.getValue(); 54 | } while(lastDepth == -1); 55 | 56 | std::cout << " got it!" << std::endl; 57 | valid = true; 58 | } 59 | } 60 | 61 | LiveLogReader::~LiveLogReader() 62 | { 63 | delete [] decompressionBufferDepth; 64 | 65 | delete [] decompressionBufferImage; 66 | 67 | delete asus; 68 | } 69 | 70 | void LiveLogReader::getNext() 71 | { 72 | int lastDepth = asus->latestDepthIndex.getValue(); 73 | 74 | assert(lastDepth != -1); 75 | 76 | int bufferIndex = lastDepth % OpenNI2Interface::numBuffers; 77 | 78 | if(bufferIndex == lastGot) 79 | { 80 | return; 81 | } 82 | 83 | if(lastFrameTime == asus->frameBuffers[bufferIndex].second) 84 | { 85 | return; 86 | } 87 | 88 | memcpy(&decompressionBufferDepth[0], asus->frameBuffers[bufferIndex].first.first, Resolution::getInstance().numPixels() * 2); 89 | memcpy(&decompressionBufferImage[0], asus->frameBuffers[bufferIndex].first.second, Resolution::getInstance().numPixels() * 3); 90 | 91 | lastFrameTime = asus->frameBuffers[bufferIndex].second; 92 | 93 | timestamp = lastFrameTime; 94 | 95 | rgb = (unsigned char *)&decompressionBufferImage[0]; 96 | depth = (unsigned short *)&decompressionBufferDepth[0]; 97 | 98 | imageReadBuffer = 0; 99 | depthReadBuffer = 0; 100 | 101 | imageSize = Resolution::getInstance().numPixels() * 3; 102 | depthSize = Resolution::getInstance().numPixels() * 2; 103 | 104 | if(flipColors) 105 | { 106 | for(int i = 0; i < Resolution::getInstance().numPixels() * 3; i += 3) 107 | { 108 | std::swap(rgb[i + 0], rgb[i + 2]); 109 | } 110 | } 111 | } 112 | 113 | int LiveLogReader::getNumFrames() 114 | { 115 | return std::numeric_limits::max(); 116 | } 117 | 118 | bool LiveLogReader::hasMore() 119 | { 120 | return true; 121 | } 122 | 123 | void LiveLogReader::setAuto(bool value) 124 | { 125 | asus->setAutoExposure(value); 126 | asus->setAutoWhiteBalance(value); 127 | } 128 | -------------------------------------------------------------------------------- /src/semantic_fusion/SemanticFusionInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef SEMANTIC_FUSION_INTERFACE_H_ 20 | #define SEMANTIC_FUSION_INTERFACE_H_ 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | // This is just to get Blobs for now 28 | #include 29 | #include 30 | #include 31 | 32 | #include "CRF/densecrf.h" 33 | 34 | class SemanticFusionInterface { 35 | public: 36 | SemanticFusionInterface(const int num_classes, const int prior_sample_size, 37 | const int max_components = 3000000, const float colour_threshold = 0.0) 38 | : current_table_size_(0) 39 | , num_classes_(num_classes) 40 | , prior_sample_size_(prior_sample_size) 41 | , max_components_(max_components) 42 | , colour_threshold_(colour_threshold) 43 | { 44 | // This table contains for each component (surfel) the probability of 45 | // it being associated with each class 46 | class_probabilities_gpu_.reset(new caffe::Blob(1,1,num_classes_,max_components_)); 47 | class_probabilities_gpu_buffer_.reset(new caffe::Blob(1,1,num_classes_,max_components_)); 48 | // This contains two rows - one is the max class (if none then negative) the 49 | // other is the probability 50 | class_max_gpu_.reset(new caffe::Blob(1,1,3,max_components_)); 51 | class_max_gpu_buffer_.reset(new caffe::Blob(1,1,3,max_components_)); 52 | rendered_class_probabilities_gpu_.reset(new caffe::Blob(1,num_classes_,640,480)); 53 | } 54 | virtual ~SemanticFusionInterface() {} 55 | 56 | int UpdateSurfelProbabilities(const int surfel_id, const std::vector& class_probs); 57 | void UpdateProbabilities(std::shared_ptr > probs,const std::unique_ptr& map); 58 | void UpdateProbabilityTable(const std::unique_ptr& map); 59 | void CalculateProjectedProbabilityMap(const std::unique_ptr& map); 60 | 61 | void CRFUpdate(const std::unique_ptr& map, const int iterations); 62 | 63 | void SaveArgMaxPredictions(std::string& filename,const std::unique_ptr& map); 64 | std::shared_ptr > get_rendered_probability(); 65 | std::shared_ptr > get_class_max_gpu(); 66 | int max_num_components() const; 67 | private: 68 | 69 | // Returns negative if the class is below the threshold - otherwise returns the class 70 | std::vector > class_probabilities_; 71 | std::shared_ptr > class_probabilities_gpu_; 72 | int current_table_size_; 73 | // This is used to store the table swap after updating 74 | std::shared_ptr > class_probabilities_gpu_buffer_; 75 | std::shared_ptr > class_max_gpu_; 76 | std::shared_ptr > class_max_gpu_buffer_; 77 | // This stores the rendered probabilities of surfels from the map 78 | std::shared_ptr > rendered_class_probabilities_gpu_; 79 | const int num_classes_; 80 | const int prior_sample_size_; 81 | const int max_components_; 82 | const float colour_threshold_; 83 | }; 84 | 85 | #endif /* SEMANTIC_FUSION_INTERFACE_H_ */ 86 | -------------------------------------------------------------------------------- /src/cnn_interface/CaffeInterface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "CaffeInterface.h" 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | bool CaffeInterface::Init(const std::string& model_path, const std::string& weights) { 27 | caffe::Caffe::SetDevice(0); 28 | caffe::Caffe::set_mode(caffe::Caffe::GPU); 29 | network_.reset(new caffe::Net(model_path, caffe::TEST)); 30 | network_->CopyTrainedLayersFrom(weights); 31 | initialised_ = true; 32 | return true; 33 | } 34 | 35 | int CaffeInterface::num_output_classes() { 36 | if (!initialised_) { 37 | return 0; 38 | } 39 | return network_->output_blobs()[0]->shape()[1]; 40 | } 41 | 42 | std::shared_ptr > CaffeInterface::ProcessFrame(const ImagePtr rgb, const DepthPtr depth, 43 | const int height, const int width) { 44 | if (!initialised_) { 45 | return std::shared_ptr >(); 46 | } 47 | cv::Mat input_image(height,width,CV_8UC3, rgb); 48 | cv::Mat input_depth(height,width,CV_16UC1, depth); 49 | std::vector* > inputs = network_->input_blobs(); 50 | CHECK_EQ(inputs.size(),1) << "Only single inputs supported"; 51 | const int network_width = inputs[0]->width(); 52 | const int network_height = inputs[0]->height(); 53 | cv::Mat resized_image(network_height,network_width,CV_8UC3); 54 | cv::resize(input_image,resized_image,resized_image.size(),0,0); 55 | cv::Mat resized_depth(network_height,network_width,CV_16UC1); 56 | cv::resize(input_depth,resized_depth,resized_depth.size(),0,0,cv::INTER_NEAREST); 57 | 58 | // This performs inpainting of the depth map on the fly, however for NYU 59 | // experiments we used the same matlab inpainting given with the toolkit, so 60 | // the input to the CNN is the same as with the normal NYU baseline results 61 | cv::Mat depthf(network_width,network_height, CV_8UC1); 62 | resized_depth.convertTo(depthf, CV_8UC1, 25.0/1000.0); 63 | const unsigned char noDepth = 0; 64 | cv::inpaint(depthf, (depthf == noDepth), depthf, 5.0, cv::INPAINT_TELEA); 65 | 66 | float* input_data = inputs[0]->mutable_cpu_data(); 67 | const float mean[] = {104.0,117.0,123.0}; 68 | for (int h = 0; h < network_height; ++h) { 69 | const uchar* image_ptr = resized_image.ptr(h); 70 | const uint16_t* depth_ptr = resized_depth.ptr(h); 71 | int image_index = 0; 72 | int depth_index = 0; 73 | for (int w = 0; w < network_width; ++w) { 74 | float r = static_cast(image_ptr[image_index++]); 75 | float g = static_cast(image_ptr[image_index++]); 76 | float b = static_cast(image_ptr[image_index++]); 77 | const int b_offset = ((0 * network_height) + h) * network_width + w; 78 | const int g_offset = ((1 * network_height) + h) * network_width + w; 79 | const int r_offset = ((2 * network_height) + h) * network_width + w; 80 | input_data[b_offset] = b - mean[0]; 81 | input_data[g_offset] = g - mean[1]; 82 | input_data[r_offset] = r - mean[2]; 83 | if (inputs[0]->channels() == 4) { 84 | //Convert to m from mm, and remove mean of 2.8m 85 | float d = static_cast(depth_ptr[depth_index++]) * (1.0f/1000.0f) ; 86 | const int d_offset = ((3 * network_height) + h) * network_width + w; 87 | input_data[d_offset] = d - 2.841; 88 | } 89 | } 90 | } 91 | float loss; 92 | const std::vector* > output = network_->Forward(inputs,&loss); 93 | if (!output_probabilities_) { 94 | output_probabilities_.reset(new caffe::Blob(output[0]->shape())); 95 | } 96 | output_probabilities_->CopyFrom(*output[0]); 97 | return output_probabilities_; 98 | } 99 | -------------------------------------------------------------------------------- /src/utilities/RawLogReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "RawLogReader.h" 20 | 21 | RawLogReader::RawLogReader(std::string file, bool flipColors) 22 | : LogReader(file, flipColors) 23 | { 24 | assert(pangolin::FileExists(file.c_str())); 25 | 26 | fp = fopen(file.c_str(), "rb"); 27 | 28 | currentFrame = 0; 29 | 30 | assert(fread(&numFrames, sizeof(int32_t), 1, fp)); 31 | 32 | depthReadBuffer = new unsigned char[numPixels * 2]; 33 | imageReadBuffer = new unsigned char[numPixels * 3]; 34 | decompressionBufferDepth = new Bytef[Resolution::getInstance().numPixels() * 2]; 35 | decompressionBufferImage = new Bytef[Resolution::getInstance().numPixels() * 3]; 36 | } 37 | 38 | RawLogReader::~RawLogReader() 39 | { 40 | delete [] depthReadBuffer; 41 | delete [] imageReadBuffer; 42 | delete [] decompressionBufferDepth; 43 | delete [] decompressionBufferImage; 44 | 45 | fclose(fp); 46 | } 47 | 48 | void RawLogReader::getBack() 49 | { 50 | assert(filePointers.size() > 0); 51 | 52 | fseek(fp, filePointers.top(), SEEK_SET); 53 | 54 | filePointers.pop(); 55 | 56 | getCore(); 57 | } 58 | 59 | void RawLogReader::getNext() 60 | { 61 | filePointers.push(ftell(fp)); 62 | 63 | getCore(); 64 | } 65 | 66 | void RawLogReader::getCore() 67 | { 68 | assert(fread(×tamp, sizeof(int64_t), 1, fp)); 69 | 70 | assert(fread(&depthSize, sizeof(int32_t), 1, fp)); 71 | assert(fread(&imageSize, sizeof(int32_t), 1, fp)); 72 | 73 | assert(fread(depthReadBuffer, depthSize, 1, fp)); 74 | 75 | if(imageSize > 0) 76 | { 77 | assert(fread(imageReadBuffer, imageSize, 1, fp)); 78 | } 79 | 80 | if(depthSize == numPixels * 2) 81 | { 82 | memcpy(&decompressionBufferDepth[0], depthReadBuffer, numPixels * 2); 83 | } 84 | else 85 | { 86 | unsigned long decompLength = numPixels * 2; 87 | uncompress(&decompressionBufferDepth[0], (unsigned long *)&decompLength, (const Bytef *)depthReadBuffer, depthSize); 88 | } 89 | 90 | if(imageSize == numPixels * 3) 91 | { 92 | memcpy(&decompressionBufferImage[0], imageReadBuffer, numPixels * 3); 93 | } 94 | else if(imageSize > 0) 95 | { 96 | jpeg.readData(imageReadBuffer, imageSize, (unsigned char *)&decompressionBufferImage[0]); 97 | } 98 | else 99 | { 100 | memset(&decompressionBufferImage[0], 0, numPixels * 3); 101 | } 102 | 103 | depth = (unsigned short *)decompressionBufferDepth; 104 | rgb = (unsigned char *)&decompressionBufferImage[0]; 105 | 106 | if(flipColors) 107 | { 108 | for(int i = 0; i < Resolution::getInstance().numPixels() * 3; i += 3) 109 | { 110 | std::swap(rgb[i + 0], rgb[i + 2]); 111 | } 112 | } 113 | 114 | currentFrame++; 115 | } 116 | 117 | void RawLogReader::fastForward(int frame) 118 | { 119 | while(currentFrame < frame && hasMore()) 120 | { 121 | filePointers.push(ftell(fp)); 122 | 123 | assert(fread(×tamp, sizeof(int64_t), 1, fp)); 124 | 125 | assert(fread(&depthSize, sizeof(int32_t), 1, fp)); 126 | assert(fread(&imageSize, sizeof(int32_t), 1, fp)); 127 | 128 | assert(fread(depthReadBuffer, depthSize, 1, fp)); 129 | 130 | if(imageSize > 0) 131 | { 132 | assert(fread(imageReadBuffer, imageSize, 1, fp)); 133 | } 134 | 135 | currentFrame++; 136 | } 137 | } 138 | 139 | int RawLogReader::getNumFrames() 140 | { 141 | return numFrames; 142 | } 143 | 144 | bool RawLogReader::hasMore() 145 | { 146 | return currentFrame + 1 < numFrames; 147 | } 148 | 149 | bool RawLogReader::rewound() 150 | { 151 | if(filePointers.size() == 0) 152 | { 153 | fclose(fp); 154 | fp = fopen(file.c_str(), "rb"); 155 | 156 | assert(fread(&numFrames, sizeof(int32_t), 1, fp)); 157 | 158 | currentFrame = 0; 159 | 160 | return true; 161 | } 162 | 163 | return false; 164 | } 165 | 166 | const std::string RawLogReader::getFile() 167 | { 168 | return file; 169 | } 170 | 171 | void RawLogReader::setAuto(bool value) 172 | { 173 | 174 | } 175 | -------------------------------------------------------------------------------- /src/utilities/Array.h: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT 3 | * 4 | * All contributions by the University of California: 5 | * Copyright (c) 2014, 2015, The Regents of the University of California (Regents) 6 | * All rights reserved. 7 | * 8 | * All other contributions: 9 | * Copyright (c) 2014, 2015, the respective contributors 10 | * All rights reserved. 11 | * 12 | * Caffe uses a shared copyright model: each contributor holds copyright over 13 | * their contributions to Caffe. The project versioning records all such 14 | * contribution and copyright details. If a contributor wants to further mark 15 | * their specific copyright on a particular contribution, they should indicate 16 | * their copyright solely in the commit message of the change when it is 17 | * committed. 18 | * 19 | * LICENSE 20 | * 21 | * Redistribution and use in source and binary forms, with or without 22 | * modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this 25 | * list of conditions and the following disclaimer. 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, 27 | * this list of conditions and the following disclaimer in the documentation 28 | * and/or other materials provided with the distribution. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 31 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 32 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 33 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 34 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 35 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 36 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 37 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 38 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 39 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | */ 41 | 42 | #ifndef ARRAY_H_ 43 | #define ARRAY_H_ 44 | 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include 55 | #include 56 | 57 | inline void MallocHost(void** ptr, size_t size, bool* use_cuda) { 58 | CUDA_CHECK(cudaMallocHost(ptr, size)); 59 | *use_cuda = true; 60 | return; 61 | } 62 | 63 | inline void FreeHost(void* ptr, bool use_cuda) { 64 | if (use_cuda) { 65 | CUDA_CHECK(cudaFreeHost(ptr)); 66 | return; 67 | } 68 | free(ptr); 69 | } 70 | 71 | 72 | class SyncedMemory { 73 | public: 74 | SyncedMemory() 75 | : cpu_ptr_(NULL), gpu_ptr_(NULL), size_(0), head_(UNINITIALIZED), 76 | own_cpu_data_(false), cpu_malloc_use_cuda_(false), own_gpu_data_(false), 77 | gpu_device_(-1) {} 78 | explicit SyncedMemory(size_t size) 79 | : cpu_ptr_(NULL), gpu_ptr_(NULL), size_(size), head_(UNINITIALIZED), 80 | own_cpu_data_(false), cpu_malloc_use_cuda_(false), own_gpu_data_(false), 81 | gpu_device_(-1) {} 82 | ~SyncedMemory(); 83 | const void* cpu_data(); 84 | void set_cpu_data(void* data); 85 | const void* gpu_data(); 86 | void set_gpu_data(void* data); 87 | void* mutable_cpu_data(); 88 | void* mutable_gpu_data(); 89 | enum SyncedHead { UNINITIALIZED, HEAD_AT_CPU, HEAD_AT_GPU, SYNCED }; 90 | SyncedHead head() { return head_; } 91 | size_t size() { return size_; } 92 | 93 | void async_gpu_push(const cudaStream_t& stream); 94 | 95 | private: 96 | void to_cpu(); 97 | void to_gpu(); 98 | void* cpu_ptr_; 99 | void* gpu_ptr_; 100 | size_t size_; 101 | SyncedHead head_; 102 | bool own_cpu_data_; 103 | bool cpu_malloc_use_cuda_; 104 | bool own_gpu_data_; 105 | int gpu_device_; 106 | }; 107 | 108 | class Array3D { 109 | public: 110 | explicit Array3D(const int channels, const int height, const int width) 111 | : data_() 112 | , capacity_(0) 113 | { 114 | Reshape(channels,height,width); 115 | } 116 | 117 | void Reshape(const int channels, const int height, const int width); 118 | 119 | inline int count() const { return height_ * width_ * channels_; } 120 | inline int channels() const { return channels_; } 121 | inline int height() const { return height_; } 122 | inline int width() const { return width_; } 123 | inline int offset(const int c, const int h, const int w) const { 124 | return ((c * height() + h) * width() + w); 125 | } 126 | 127 | const float* cpu_data() const { 128 | return (const float*) data_->cpu_data(); 129 | } 130 | float* mutable_cpu_data() { 131 | return static_cast(data_->mutable_cpu_data()); 132 | } 133 | const float* gpu_data() const { 134 | return (const float*)data_->gpu_data(); 135 | } 136 | float* mutable_gpu_data() { 137 | return static_cast(data_->mutable_gpu_data()); 138 | } 139 | 140 | protected: 141 | std::shared_ptr data_; 142 | int capacity_; 143 | int channels_; 144 | int height_; 145 | int width_; 146 | }; 147 | 148 | #endif /* ARRAY_H_ */ 149 | -------------------------------------------------------------------------------- /src/map_interface/ElasticFusionInterface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "ElasticFusionInterface.h" 20 | #include "ElasticFusionCuda.h" 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | float encode_colour(ClassColour rgb_colour) { 27 | int colour = rgb_colour.r; 28 | colour = (colour << 8) + rgb_colour.g; 29 | colour = (colour << 8) + rgb_colour.b; 30 | return float(colour); 31 | } 32 | 33 | bool ElasticFusionInterface::Init(std::vector class_colour_lookup) { 34 | GLXContext context = glXGetCurrentContext(); 35 | if (context == nullptr) { 36 | return false; 37 | } 38 | // These are the regular elastic fusion parameters. The depthCut has been 39 | // increase to 8m. Also to prevent the white border in the NYU the ICP weight 40 | // can be turned to 100, as in the commented line below. However, the code 41 | // in elasticfusion has also been modified to ignore a 16 pixel border of the 42 | // RGB residual, which allows some RGB tracking while also ignoring the border. 43 | elastic_fusion_.reset(new ElasticFusion(200, 35000, 5e-05, 1e-05, true, 44 | false,false,115,10,8,10)); 45 | //false,false,115,10,8,100)); 46 | const int surfel_render_size = Resolution::getInstance().width() * 47 | Resolution::getInstance().height(); 48 | surfel_ids_.resize(surfel_render_size); 49 | initialised_ = true; 50 | class_color_lookup_.clear(); 51 | for (unsigned int class_id = 0; class_id < class_colour_lookup.size(); ++class_id) { 52 | class_color_lookup_.push_back(encode_colour(class_colour_lookup[class_id])); 53 | } 54 | cudaMalloc((void **)&class_color_lookup_gpu_, class_color_lookup_.size() * sizeof(float)); 55 | cudaMemcpy(class_color_lookup_gpu_, class_color_lookup_.data(), class_color_lookup_.size() * sizeof(float), cudaMemcpyHostToDevice); 56 | return true; 57 | } 58 | 59 | ElasticFusionInterface::~ElasticFusionInterface() { 60 | cudaFree(class_color_lookup_gpu_); 61 | } 62 | 63 | int* ElasticFusionInterface::GetDeletedSurfelIdsGpu() { 64 | if (elastic_fusion_) { 65 | return elastic_fusion_->getGlobalModel().getDeletedSurfelsGpu(); 66 | } 67 | return nullptr; 68 | } 69 | 70 | bool ElasticFusionInterface::ProcessFrame(const ImagePtr rgb, const DepthPtr depth, const int64_t timestamp) { 71 | if (elastic_fusion_) { 72 | elastic_fusion_->processFrame(rgb,depth,timestamp); 73 | if (elastic_fusion_->getLost()) { 74 | return false; 75 | } 76 | return true; 77 | } 78 | return false; 79 | } 80 | 81 | GPUTexture* ElasticFusionInterface::getRawImageTexture() { 82 | return elastic_fusion_->getTextures()[GPUTexture::RGB]; 83 | } 84 | 85 | GPUTexture* ElasticFusionInterface::getIdsTexture() { 86 | return elastic_fusion_->getIndexMap().surfelIdTex(); 87 | } 88 | 89 | const std::vector& ElasticFusionInterface::GetSurfelIdsCpu() { 90 | GPUTexture* id_tex = elastic_fusion_->getIndexMap().surfelIdTex(); 91 | id_tex->texture->Download(surfel_ids_.data(),GL_LUMINANCE_INTEGER_EXT,GL_INT); 92 | return surfel_ids_; 93 | } 94 | 95 | cudaTextureObject_t ElasticFusionInterface::GetSurfelIdsGpu() { 96 | GPUTexture* id_tex = elastic_fusion_->getIndexMap().surfelIdTex(); 97 | return id_tex->texObj; 98 | } 99 | 100 | void ElasticFusionInterface::UpdateSurfelClass(const int surfel_id, const int class_id) { 101 | elastic_fusion_->getGlobalModel().updateSurfelClass(surfel_id,class_color_lookup_[class_id]); 102 | } 103 | 104 | void ElasticFusionInterface::UpdateSurfelClassGpu(const int n, const float* surfelclasses, const float* surfelprobs, const float threshold) { 105 | if (elastic_fusion_) { 106 | float* map_surfels = elastic_fusion_->getGlobalModel().getMapSurfelsGpu(); 107 | updateSurfelClasses(n, map_surfels, surfelclasses, surfelprobs, class_color_lookup_gpu_, threshold); 108 | } 109 | } 110 | 111 | void ElasticFusionInterface::RenderMapToBoundGlBuffer(const pangolin::OpenGlRenderState& camera,const bool classes) { 112 | elastic_fusion_->getGlobalModel().renderPointCloud(camera.GetProjectionModelViewMatrix(), 113 | elastic_fusion_->getConfidenceThreshold(), 114 | false, 115 | false, 116 | true, 117 | false, 118 | false, 119 | false, 120 | classes, 121 | elastic_fusion_->getTick(), 122 | elastic_fusion_->getTimeDelta()); 123 | } 124 | -------------------------------------------------------------------------------- /src/semantic_fusion/CRF/densecrf.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Philipp Krähenbühl 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the Stanford University nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY Philipp Krähenbühl ''AS IS'' AND ANY 17 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL Philipp Krähenbühl BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #pragma once 29 | 30 | #include 31 | #include 32 | 33 | class PairwisePotential{ 34 | public: 35 | virtual ~PairwisePotential(); 36 | virtual void apply( float * out_values, const float * in_values, float * tmp, int value_size ) const = 0; 37 | }; 38 | class SemiMetricFunction{ 39 | public: 40 | virtual ~SemiMetricFunction(); 41 | // For two probabilities apply the semi metric transform: v_i = sum_j mu_ij u_j 42 | virtual void apply( float * out_values, const float * in_values, int value_size ) const = 0; 43 | }; 44 | 45 | 46 | class DenseCRF { 47 | protected: 48 | 49 | // Number of variables and labels 50 | int N_, M_; 51 | float *unary_, *additional_unary_, *current_, *next_, *tmp_; 52 | 53 | // Store all pairwise potentials 54 | std::vector pairwise_; 55 | 56 | 57 | // Auxillary functions 58 | void expAndNormalize( float* out, const float* in, float scale = 1.0, float relax = 1.0 ); 59 | 60 | // Don't copy this object, bad stuff will happen 61 | DenseCRF( DenseCRF & o ){} 62 | public: 63 | // Create a dense CRF model of size N with M labels 64 | DenseCRF( int N, int M ); 65 | virtual ~DenseCRF(); 66 | // Add a pairwise potential defined over some feature space 67 | // The potential will have the form: w*exp(-0.5*|f_i - f_j|^2) 68 | // The kernel shape should be captured by transforming the 69 | // features before passing them into this function 70 | void addPairwiseEnergy( const float * features, int D, float w=1.0f, const SemiMetricFunction * function=NULL ); 71 | 72 | // Add your own favorite pairwise potential (ownwership will be transfered to this class) 73 | void addPairwiseEnergy( PairwisePotential* potential ); 74 | 75 | // Set the unary potential for all variables and labels (memory order is [x0l0 x0l1 x0l2 .. x1l0 x1l1 ...]) 76 | void setUnaryEnergy( const float * unary ); 77 | 78 | // Run inference and return the probabilities 79 | void inference( int n_iterations, float* result, float relax=1.0 ); 80 | 81 | // Run MAP inference and return the map for each pixel 82 | void map( int n_iterations, short int* result, float relax=1.0 ); 83 | 84 | // Step by step inference 85 | void startInference(); 86 | void stepInference( float relax = 1.0 ); 87 | 88 | // Run inference and return the pointer to the result 89 | float* runInference( int n_iterations, float relax); 90 | }; 91 | 92 | class DenseCRF2D:public DenseCRF{ 93 | protected: 94 | // Width, height of the 2d grid 95 | int W_, H_; 96 | public: 97 | // Create a 2d dense CRF model of size W x H with M labels 98 | DenseCRF2D( int W, int H, int M ); 99 | virtual ~DenseCRF2D(); 100 | // Add a Gaussian pairwise potential with standard deviation sx and sy 101 | void addPairwiseGaussian( float sx, float sy, float w, const SemiMetricFunction * function=NULL ); 102 | 103 | // Add a Bilateral pairwise potential with spacial standard deviations sx, sy and color standard deviations sr,sg,sb 104 | void addPairwiseBilateral( float sx, float sy, float sr, float sg, float sb, const unsigned char * im, float w, const SemiMetricFunction * function=NULL ); 105 | }; 106 | 107 | class DenseCRF3D:public DenseCRF{ 108 | protected: 109 | const float spatial_stddev_; 110 | const float colour_stddev_; 111 | const float normal_stddev_; 112 | public: 113 | // Create a 2d dense CRF model of size W x H with M labels 114 | DenseCRF3D(int N, int M, float spatial_stddev, float colour_stddev, float normal_stddev ); 115 | virtual ~DenseCRF3D(); 116 | // Add a Gaussian pairwise potential with standard deviation sx and sy 117 | void addPairwiseGaussian(const float* surfel_data, float w, const std::vector& valid); 118 | // Add a Bilateral pairwise potential with spacial standard deviations sx, sy and color standard deviations sr,sg,sb 119 | void addPairwiseBilateral(const float* surfel_data, float w, const std::vector& valid); 120 | // Add a Bilateral pairwise potential with spacial standard deviations sx, sy and color standard deviations sr,sg,sb 121 | void addPairwiseNormal(const float* surfel_data, float w); 122 | }; 123 | -------------------------------------------------------------------------------- /src/utilities/Array.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * COPYRIGHT 3 | * 4 | * All contributions by the University of California: 5 | * Copyright (c) 2014, 2015, The Regents of the University of California (Regents) 6 | * All rights reserved. 7 | * 8 | * All other contributions: 9 | * Copyright (c) 2014, 2015, the respective contributors 10 | * All rights reserved. 11 | * 12 | * Caffe uses a shared copyright model: each contributor holds copyright over 13 | * their contributions to Caffe. The project versioning records all such 14 | * contribution and copyright details. If a contributor wants to further mark 15 | * their specific copyright on a particular contribution, they should indicate 16 | * their copyright solely in the commit message of the change when it is 17 | * committed. 18 | * 19 | * LICENSE 20 | * 21 | * Redistribution and use in source and binary forms, with or without 22 | * modification, are permitted provided that the following conditions are met: 23 | * 24 | * 1. Redistributions of source code must retain the above copyright notice, this 25 | * list of conditions and the following disclaimer. 26 | * 2. Redistributions in binary form must reproduce the above copyright notice, 27 | * this list of conditions and the following disclaimer in the documentation 28 | * and/or other materials provided with the distribution. 29 | * 30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 31 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 32 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 33 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 34 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 35 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 36 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 37 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 38 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 39 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 40 | */ 41 | #include "Array.h" 42 | 43 | void Array3D::Reshape(const int channels, const int height, const int width) { 44 | channels_ = channels; 45 | height_ = height; 46 | width_ = width; 47 | if (count() > capacity_) { 48 | capacity_ = count(); 49 | data_.reset(new SyncedMemory(capacity_ * sizeof(float))); 50 | } 51 | } 52 | 53 | SyncedMemory::~SyncedMemory() { 54 | if (cpu_ptr_ && own_cpu_data_) { 55 | FreeHost(cpu_ptr_, cpu_malloc_use_cuda_); 56 | } 57 | if (gpu_ptr_ && own_gpu_data_) { 58 | int initial_device; 59 | cudaGetDevice(&initial_device); 60 | if (gpu_device_ != -1) { 61 | CUDA_CHECK(cudaSetDevice(gpu_device_)); 62 | } 63 | CUDA_CHECK(cudaFree(gpu_ptr_)); 64 | cudaSetDevice(initial_device); 65 | } 66 | } 67 | 68 | inline void SyncedMemory::to_cpu() { 69 | switch (head_) { 70 | case UNINITIALIZED: 71 | MallocHost(&cpu_ptr_, size_, &cpu_malloc_use_cuda_); 72 | memset(cpu_ptr_, 0, size_); 73 | head_ = HEAD_AT_CPU; 74 | own_cpu_data_ = true; 75 | break; 76 | case HEAD_AT_GPU: 77 | if (cpu_ptr_ == NULL) { 78 | MallocHost(&cpu_ptr_, size_, &cpu_malloc_use_cuda_); 79 | own_cpu_data_ = true; 80 | } 81 | cudaMemcpy(cpu_ptr_, gpu_ptr_, size_, cudaMemcpyDefault); 82 | head_ = SYNCED; 83 | break; 84 | case HEAD_AT_CPU: 85 | case SYNCED: 86 | break; 87 | } 88 | } 89 | 90 | inline void SyncedMemory::to_gpu() { 91 | switch (head_) { 92 | case UNINITIALIZED: 93 | CUDA_CHECK(cudaGetDevice(&gpu_device_)); 94 | CUDA_CHECK(cudaMalloc(&gpu_ptr_, size_)); 95 | cudaMemset(gpu_ptr_,0,size_); 96 | head_ = HEAD_AT_GPU; 97 | own_gpu_data_ = true; 98 | break; 99 | case HEAD_AT_CPU: 100 | if (gpu_ptr_ == NULL) { 101 | CUDA_CHECK(cudaGetDevice(&gpu_device_)); 102 | CUDA_CHECK(cudaMalloc(&gpu_ptr_, size_)); 103 | own_gpu_data_ = true; 104 | } 105 | cudaMemcpy(gpu_ptr_, cpu_ptr_, size_, cudaMemcpyDefault); 106 | head_ = SYNCED; 107 | break; 108 | case HEAD_AT_GPU: 109 | case SYNCED: 110 | break; 111 | } 112 | } 113 | 114 | const void* SyncedMemory::cpu_data() { 115 | to_cpu(); 116 | return (const void*)cpu_ptr_; 117 | } 118 | 119 | void SyncedMemory::set_cpu_data(void* data) { 120 | CHECK(data); 121 | if (own_cpu_data_) { 122 | FreeHost(cpu_ptr_, cpu_malloc_use_cuda_); 123 | } 124 | cpu_ptr_ = data; 125 | head_ = HEAD_AT_CPU; 126 | own_cpu_data_ = false; 127 | } 128 | 129 | const void* SyncedMemory::gpu_data() { 130 | to_gpu(); 131 | return (const void*)gpu_ptr_; 132 | } 133 | 134 | void SyncedMemory::set_gpu_data(void* data) { 135 | CHECK(data); 136 | if (own_gpu_data_) { 137 | int initial_device; 138 | cudaGetDevice(&initial_device); 139 | if (gpu_device_ != -1) { 140 | CUDA_CHECK(cudaSetDevice(gpu_device_)); 141 | } 142 | CUDA_CHECK(cudaFree(gpu_ptr_)); 143 | cudaSetDevice(initial_device); 144 | } 145 | gpu_ptr_ = data; 146 | head_ = HEAD_AT_GPU; 147 | own_gpu_data_ = false; 148 | } 149 | 150 | void* SyncedMemory::mutable_cpu_data() { 151 | to_cpu(); 152 | head_ = HEAD_AT_CPU; 153 | return cpu_ptr_; 154 | } 155 | 156 | void* SyncedMemory::mutable_gpu_data() { 157 | to_gpu(); 158 | head_ = HEAD_AT_GPU; 159 | return gpu_ptr_; 160 | } 161 | 162 | void SyncedMemory::async_gpu_push(const cudaStream_t& stream) { 163 | CHECK(head_ == HEAD_AT_CPU); 164 | if (gpu_ptr_ == NULL) { 165 | CUDA_CHECK(cudaGetDevice(&gpu_device_)); 166 | CUDA_CHECK(cudaMalloc(&gpu_ptr_, size_)); 167 | own_gpu_data_ = true; 168 | } 169 | const cudaMemcpyKind put = cudaMemcpyHostToDevice; 170 | CUDA_CHECK(cudaMemcpyAsync(gpu_ptr_, cpu_ptr_, size_, put, stream)); 171 | // Assume caller will synchronize on the stream before use 172 | head_ = SYNCED; 173 | } 174 | -------------------------------------------------------------------------------- /src/utilities/PNGLogReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "PNGLogReader.h" 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | PNGLogReader::PNGLogReader(std::string file, std::string labels_file) 29 | : LogReader(file, true) 30 | , lastFrameTime(-1) 31 | , lastGot(-1) 32 | , has_depth_filled(false) 33 | , num_labelled(0) 34 | { 35 | decompressionBufferDepth = new Bytef[Resolution::getInstance().numPixels() * 2]; 36 | decompressionBufferDepthFilled = new Bytef[Resolution::getInstance().numPixels() * 2]; 37 | decompressionBufferImage = new Bytef[Resolution::getInstance().numPixels() * 3]; 38 | std::ifstream infile(file.c_str()); 39 | std::string timestamp, depth_path, rgb_path, depth_id, rgb_id; 40 | std::map depth_id_lookup; 41 | std::string scene_id = file.substr(file.rfind("/") + 1); 42 | std::string base_path = file; 43 | base_path.erase(base_path.rfind('/')); 44 | std::cout<<"Looking for RGB/Depth images in folder:"<> timestamp >> depth_path >> rgb_path >> depth_id >> rgb_id) { 48 | FrameInfo frame_info; 49 | std::stringstream ss(timestamp.c_str()); 50 | ss >> frame_info.timestamp; 51 | frame_info.depth_path = base_path + "/" + depth_path; 52 | if (id == 0) { 53 | std::cout<<"E.g.:"<> depth_id >> rgb_id >> frame_id) { 68 | if (depth_id_lookup.find(depth_id) != depth_id_lookup.end()) { 69 | int found_id = depth_id_lookup[depth_id]; 70 | frames_[found_id].labeled_frame = true; 71 | frames_[found_id].frame_id = frame_id; 72 | std::cout<<"Found:"<(frames_.size())) { 92 | lastGot++; 93 | FrameInfo info = frames_[lastGot]; 94 | timestamp = info.timestamp; 95 | cv::Mat rgb_image = cv::imread(info.rgb_path,CV_LOAD_IMAGE_COLOR); 96 | if (flipColors) { 97 | cv::cvtColor(rgb_image, rgb_image, CV_BGR2RGB); 98 | } 99 | rgb = (unsigned char *)&decompressionBufferImage[0]; 100 | int index = 0; 101 | for (int i = 0; i < rgb_image.rows; ++i) { 102 | for (int j = 0; j < rgb_image.cols; ++j) { 103 | rgb[index++] = rgb_image.at(i,j)[0]; 104 | rgb[index++] = rgb_image.at(i,j)[1]; 105 | rgb[index++] = rgb_image.at(i,j)[2]; 106 | } 107 | } 108 | depth = (unsigned short *)&decompressionBufferDepth[0]; 109 | cv::Mat depth_image = cv::imread(info.depth_path,CV_LOAD_IMAGE_ANYDEPTH); 110 | index = 0; 111 | for (int i = 0; i < depth_image.rows; ++i) { 112 | for (int j = 0; j < depth_image.cols; ++j) { 113 | depth[index++] = depth_image.at(i,j); 114 | } 115 | } 116 | 117 | depthfilled = (unsigned short *)&decompressionBufferDepthFilled[0]; 118 | std::string depth_filled_str = info.depth_path; 119 | depth_filled_str.erase(depth_filled_str.end()-9,depth_filled_str.end()); 120 | depth_filled_str += "depthfilled.png"; 121 | cv::Mat depthfill_image = cv::imread(depth_filled_str,CV_LOAD_IMAGE_ANYDEPTH); 122 | if (depthfill_image.data) { 123 | index = 0; 124 | for (int i = 0; i < depthfill_image.rows; ++i) { 125 | for (int j = 0; j < depthfill_image.cols; ++j) { 126 | depthfilled[index++] = depthfill_image.at(i,j); 127 | } 128 | } 129 | has_depth_filled = true; 130 | } else { 131 | has_depth_filled = false; 132 | } 133 | 134 | imageSize = Resolution::getInstance().numPixels() * 3; 135 | depthSize = Resolution::getInstance().numPixels() * 2; 136 | } 137 | } 138 | 139 | bool PNGLogReader::isLabeledFrame() 140 | { 141 | return frames_[lastGot].labeled_frame; 142 | } 143 | 144 | std::string PNGLogReader::getLabelFrameId() { 145 | if (isLabeledFrame()) { 146 | return frames_[lastGot].frame_id; 147 | } 148 | return ""; 149 | } 150 | 151 | int PNGLogReader::getNumFrames() 152 | { 153 | return static_cast(frames_.size()); 154 | } 155 | 156 | bool PNGLogReader::hasMore() 157 | { 158 | return (lastGot + 1) < static_cast(frames_.size()); 159 | } 160 | -------------------------------------------------------------------------------- /src/utilities/Stopwatch.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef STOPWATCH_H_ 20 | #define STOPWATCH_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #define SEND_INTERVAL_MS 10000 37 | 38 | #ifndef DISABLE_STOPWATCH 39 | #define STOPWATCH(name, expression) \ 40 | do \ 41 | { \ 42 | const unsigned long long int startTime = Stopwatch::getInstance().getCurrentSystemTime(); \ 43 | expression \ 44 | const unsigned long long int endTime = Stopwatch::getInstance().getCurrentSystemTime(); \ 45 | Stopwatch::getInstance().addStopwatchTiming(name, endTime - startTime); \ 46 | } \ 47 | while(false) 48 | 49 | #define TICK(name) \ 50 | do \ 51 | { \ 52 | Stopwatch::getInstance().tick(name, Stopwatch::getInstance().getCurrentSystemTime()); \ 53 | } \ 54 | while(false) 55 | 56 | #define TOCK(name) \ 57 | do \ 58 | { \ 59 | Stopwatch::getInstance().tock(name, Stopwatch::getInstance().getCurrentSystemTime()); \ 60 | } \ 61 | while(false) 62 | #else 63 | #define STOPWATCH(name, expression) \ 64 | expression 65 | 66 | #define TOCK(name) ((void)0) 67 | 68 | #define TICK(name) ((void)0) 69 | 70 | #endif 71 | 72 | class Stopwatch 73 | { 74 | public: 75 | static Stopwatch & getInstance() 76 | { 77 | static Stopwatch instance; 78 | return instance; 79 | } 80 | 81 | void addStopwatchTiming(std::string name, unsigned long long int duration) 82 | { 83 | if(duration > 0) 84 | { 85 | timings[name] = (float)(duration) / 1000.0f; 86 | } 87 | } 88 | 89 | void setCustomSignature(unsigned long long int newSignature) 90 | { 91 | signature = newSignature; 92 | } 93 | 94 | const std::map & getTimings() 95 | { 96 | return timings; 97 | } 98 | 99 | void printAll() 100 | { 101 | for(std::map::const_iterator it = timings.begin(); it != timings.end(); it++) 102 | { 103 | std::cout << it->first << ": " << it->second << "ms" << std::endl; 104 | } 105 | 106 | std::cout << std::endl; 107 | } 108 | 109 | void pulse(std::string name) 110 | { 111 | timings[name] = 1; 112 | } 113 | 114 | void sendAll() 115 | { 116 | gettimeofday(&clock, 0); 117 | 118 | if((currentSend = (clock.tv_sec * 1000000 + clock.tv_usec)) - lastSend > SEND_INTERVAL_MS) 119 | { 120 | int size = 0; 121 | unsigned char * data = serialiseTimings(size); 122 | 123 | sendto(sockfd, data, size, 0, (struct sockaddr *) &servaddr, sizeof(servaddr)); 124 | 125 | free(data); 126 | 127 | lastSend = currentSend; 128 | } 129 | } 130 | 131 | static unsigned long long int getCurrentSystemTime() 132 | { 133 | timeval tv; 134 | gettimeofday(&tv, 0); 135 | unsigned long long int time = (unsigned long long int)(tv.tv_sec * 1000000 + tv.tv_usec); 136 | return time; 137 | } 138 | 139 | void tick(std::string name, unsigned long long int start) 140 | { 141 | tickTimings[name] = start; 142 | } 143 | 144 | void tock(std::string name, unsigned long long int end) 145 | { 146 | float duration = (float)(end - tickTimings[name]) / 1000.0f; 147 | 148 | if(duration > 0) 149 | { 150 | timings[name] = duration; 151 | } 152 | } 153 | 154 | private: 155 | Stopwatch() 156 | { 157 | memset(&servaddr, 0, sizeof(servaddr)); 158 | servaddr.sin_family = AF_INET; 159 | servaddr.sin_addr.s_addr = inet_addr("127.0.0.1"); 160 | servaddr.sin_port = htons(45454); 161 | sockfd = socket(AF_INET, SOCK_DGRAM, 0); 162 | 163 | gettimeofday(&clock, 0); 164 | 165 | signature = clock.tv_sec * 1000000 + clock.tv_usec; 166 | 167 | currentSend = lastSend = clock.tv_sec * 1000000 + clock.tv_usec; 168 | } 169 | 170 | virtual ~Stopwatch() 171 | { 172 | close(sockfd); 173 | } 174 | 175 | unsigned char * serialiseTimings(int & packetSize) 176 | { 177 | packetSize = sizeof(int) + sizeof(unsigned long long int); 178 | 179 | for(std::map::const_iterator it = timings.begin(); it != timings.end(); it++) 180 | { 181 | packetSize += it->first.length() + 1 + sizeof(float); 182 | } 183 | 184 | int * dataPacket = (int *)calloc(packetSize, sizeof(unsigned char)); 185 | 186 | dataPacket[0] = packetSize * sizeof(unsigned char); 187 | 188 | *((unsigned long long int *)&dataPacket[1]) = signature; 189 | 190 | float * valuePointer = (float *)&((unsigned long long int *)&dataPacket[1])[1]; 191 | 192 | for(std::map::const_iterator it = timings.begin(); it != timings.end(); it++) 193 | { 194 | valuePointer = (float *)mempcpy(valuePointer, it->first.c_str(), it->first.length() + 1); 195 | *valuePointer++ = it->second; 196 | } 197 | 198 | return (unsigned char *)dataPacket; 199 | } 200 | 201 | timeval clock; 202 | long long int currentSend, lastSend; 203 | unsigned long long int signature; 204 | int sockfd; 205 | struct sockaddr_in servaddr; 206 | std::map timings; 207 | std::map tickTimings; 208 | }; 209 | 210 | #endif /* STOPWATCH_H_ */ 211 | -------------------------------------------------------------------------------- /src/utilities/OpenNI2Interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef OPENNI2INTERFACE_H_ 20 | #define OPENNI2INTERFACE_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "ThreadMutexObject.h" 30 | 31 | class OpenNI2Interface 32 | { 33 | public: 34 | OpenNI2Interface(int inWidth = 640, int inHeight = 480, int fps = 30); 35 | virtual ~OpenNI2Interface(); 36 | 37 | const int width, height, fps; 38 | 39 | void printModes(); 40 | bool findMode(int x, int y, int fps); 41 | void setAutoExposure(bool value); 42 | void setAutoWhiteBalance(bool value); 43 | bool getAutoExposure(); 44 | bool getAutoWhiteBalance(); 45 | 46 | bool ok() 47 | { 48 | return initSuccessful; 49 | } 50 | 51 | std::string error() 52 | { 53 | errorText.erase(std::remove_if(errorText.begin(), errorText.end(), &OpenNI2Interface::isTab), errorText.end()); 54 | return errorText; 55 | } 56 | 57 | static const int numBuffers = 10; 58 | ThreadMutexObject latestDepthIndex; 59 | std::pair, int64_t> frameBuffers[numBuffers]; 60 | 61 | class RGBCallback : public openni::VideoStream::NewFrameListener 62 | { 63 | public: 64 | RGBCallback(int64_t & lastRgbTime, 65 | ThreadMutexObject & latestRgbIndex, 66 | std::pair * rgbBuffers) 67 | : lastRgbTime(lastRgbTime), 68 | latestRgbIndex(latestRgbIndex), 69 | rgbBuffers(rgbBuffers) 70 | {} 71 | 72 | virtual ~RGBCallback() {} 73 | 74 | void onNewFrame(openni::VideoStream& stream) 75 | { 76 | stream.readFrame(&frame); 77 | 78 | lastRgbTime = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1); 79 | 80 | int bufferIndex = (latestRgbIndex.getValue() + 1) % numBuffers; 81 | 82 | memcpy(rgbBuffers[bufferIndex].first, frame.getData(), frame.getWidth() * frame.getHeight() * 3); 83 | 84 | rgbBuffers[bufferIndex].second = lastRgbTime; 85 | 86 | latestRgbIndex++; 87 | } 88 | 89 | private: 90 | openni::VideoFrameRef frame; 91 | int64_t & lastRgbTime; 92 | ThreadMutexObject & latestRgbIndex; 93 | std::pair * rgbBuffers; 94 | }; 95 | 96 | class DepthCallback : public openni::VideoStream::NewFrameListener 97 | { 98 | public: 99 | DepthCallback(int64_t & lastDepthTime, 100 | ThreadMutexObject & latestDepthIndex, 101 | ThreadMutexObject & latestRgbIndex, 102 | std::pair * rgbBuffers, 103 | std::pair, int64_t> * frameBuffers) 104 | : lastDepthTime(lastDepthTime), 105 | latestDepthIndex(latestDepthIndex), 106 | latestRgbIndex(latestRgbIndex), 107 | rgbBuffers(rgbBuffers), 108 | frameBuffers(frameBuffers) 109 | {} 110 | 111 | virtual ~DepthCallback() {} 112 | 113 | void onNewFrame(openni::VideoStream& stream) 114 | { 115 | stream.readFrame(&frame); 116 | 117 | lastDepthTime = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1); 118 | 119 | int bufferIndex = (latestDepthIndex.getValue() + 1) % numBuffers; 120 | 121 | memcpy(frameBuffers[bufferIndex].first.first, frame.getData(), frame.getWidth() * frame.getHeight() * 2); 122 | 123 | frameBuffers[bufferIndex].second = lastDepthTime; 124 | 125 | int lastImageVal = latestRgbIndex.getValue(); 126 | 127 | if(lastImageVal == -1) 128 | { 129 | return; 130 | } 131 | 132 | lastImageVal %= numBuffers; 133 | 134 | memcpy(frameBuffers[bufferIndex].first.second, rgbBuffers[lastImageVal].first, frame.getWidth() * frame.getHeight() * 3); 135 | 136 | latestDepthIndex++; 137 | } 138 | 139 | private: 140 | openni::VideoFrameRef frame; 141 | int64_t & lastDepthTime; 142 | ThreadMutexObject & latestDepthIndex; 143 | ThreadMutexObject & latestRgbIndex; 144 | 145 | std::pair * rgbBuffers; 146 | std::pair, int64_t> * frameBuffers; 147 | }; 148 | 149 | private: 150 | openni::Device device; 151 | 152 | openni::VideoStream depthStream; 153 | openni::VideoStream rgbStream; 154 | 155 | //Map for formats from OpenNI2 156 | std::map formatMap; 157 | 158 | int64_t lastRgbTime; 159 | int64_t lastDepthTime; 160 | 161 | ThreadMutexObject latestRgbIndex; 162 | std::pair rgbBuffers[numBuffers]; 163 | 164 | RGBCallback * rgbCallback; 165 | DepthCallback * depthCallback; 166 | 167 | bool initSuccessful; 168 | std::string errorText; 169 | 170 | //For removing tabs from OpenNI's error messages 171 | static bool isTab(char c) 172 | { 173 | switch(c) 174 | { 175 | case '\t': 176 | return true; 177 | default: 178 | return false; 179 | } 180 | } 181 | }; 182 | 183 | #endif /* OPENNI2INTERFACE_H_ */ 184 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | std::vector load_colour_scheme(std::string filename, int num_classes) { 36 | std::vector colour_scheme(num_classes); 37 | std::ifstream file(filename); 38 | std::string str; 39 | int line_number = 1; 40 | while (std::getline(file, str)) 41 | { 42 | std::istringstream buffer(str); 43 | std::string textual_prefix; 44 | int id, r, g, b; 45 | if (line_number > 2) { 46 | buffer >> textual_prefix >> id >> r >> g >> b; 47 | ClassColour class_colour(textual_prefix,r,g,b); 48 | assert(id < num_classes); 49 | colour_scheme[id] = class_colour; 50 | } 51 | line_number++; 52 | } 53 | return colour_scheme; 54 | } 55 | 56 | int main(int argc, char *argv[]) 57 | { 58 | // CNN Skip params 59 | const int cnn_skip_frames = 10; 60 | // Option CPU-based CRF smoothing 61 | const bool use_crf = false; 62 | const int crf_skip_frames = 500; 63 | const int crf_iterations = 10; 64 | // Load the network model and parameters 65 | CaffeInterface caffe; 66 | // This is for the RGB-D network 67 | caffe.Init("../caffe_semanticfusion/models/nyu_rgbd/inference.prototxt","../caffe_semanticfusion/models/nyu_rgbd/inference.caffemodel"); 68 | // This is for the RGB network 69 | //caffe.Init("../caffe/models/nyu_rgb/inference.prototxt","../caffe/models/nyu_rgb/inference.caffemodel"); 70 | const int num_classes = caffe.num_output_classes(); 71 | std::cout<<"Network produces "< class_colour_lookup = load_colour_scheme("../class_colour_scheme.data",num_classes); 74 | std::unique_ptr semantic_fusion(new SemanticFusionInterface(num_classes,100)); 75 | // Initialise the Gui, Map, and Kinect Log Reader 76 | const int width = 640; 77 | const int height = 480; 78 | Resolution::getInstance(width, height); 79 | Intrinsics::getInstance(528, 528, 320, 240); 80 | std::unique_ptr gui(new Gui(true,class_colour_lookup,640,480)); 81 | std::unique_ptr map(new ElasticFusionInterface()); 82 | // Choose the input Reader, live for a running OpenNI device, PNG for textfile lists of PNG frames 83 | std::unique_ptr log_reader; 84 | if (argc > 2) { 85 | log_reader.reset(new PNGLogReader(argv[1],argv[2])); 86 | } else { 87 | log_reader.reset(new LiveLogReader("./live",false)); 88 | if (!log_reader->is_valid()) { 89 | std::cout<<"Error initialising live device..."<Init(class_colour_lookup)) { 94 | std::cout<<"ElasticFusionInterface init failure"< > segmented_prob; 99 | while(!pangolin::ShouldQuit() && log_reader->hasMore()) { 100 | gui->preCall(); 101 | // Read and perform an elasticFusion update 102 | if (!gui->paused() || gui->step()) { 103 | log_reader->getNext(); 104 | map->setTrackingOnly(gui->tracking()); 105 | if (!map->ProcessFrame(log_reader->rgb, log_reader->depth,log_reader->timestamp)) { 106 | std::cout<<"Elastic fusion lost!"<tracking()) { 114 | semantic_fusion->UpdateProbabilityTable(map); 115 | } 116 | // We do not need to perform a CNN update every frame, we perform it every 117 | // 'cnn_skip_frames' 118 | if (frame_num == 0 || (frame_num > 1 && ((frame_num + 1) % cnn_skip_frames == 0))) { 119 | if (log_reader->hasDepthFilled()) { 120 | segmented_prob = caffe.ProcessFrame(log_reader->rgb, log_reader->depthfilled, height, width); 121 | } else { 122 | segmented_prob = caffe.ProcessFrame(log_reader->rgb, log_reader->depth, height, width); 123 | } 124 | semantic_fusion->UpdateProbabilities(segmented_prob,map); 125 | } 126 | if (use_crf && frame_num % crf_skip_frames == 0) { 127 | std::cout<<"Performing CRF Update..."<CRFUpdate(map,crf_iterations); 129 | } 130 | } 131 | frame_num++; 132 | // This is for outputting the predicted frames 133 | if (log_reader->isLabeledFrame()) { 134 | // Change this to save the NYU raw label predictions to a folder. 135 | // Note these are raw, without the CNN fall-back predictions where there 136 | // is no surfel to give a prediction. 137 | std::string save_dir("./"); 138 | std::string label_dir(log_reader->getLabelFrameId()); 139 | std::string suffix("_label.png"); 140 | save_dir += label_dir; 141 | save_dir += suffix; 142 | std::cout<<"Saving labeled frame to "<SaveArgMaxPredictions(save_dir,map); 144 | } 145 | gui->renderMap(map); 146 | gui->displayRawNetworkPredictions("pred",segmented_prob->mutable_gpu_data()); 147 | // This is to display a predicted semantic segmentation from the fused map 148 | semantic_fusion->CalculateProjectedProbabilityMap(map); 149 | gui->displayArgMaxClassColouring("segmentation",semantic_fusion->get_rendered_probability()->mutable_gpu_data(), 150 | num_classes,semantic_fusion->get_class_max_gpu()->gpu_data(), 151 | semantic_fusion->max_num_components(),map->GetSurfelIdsGpu(),0.0); 152 | // This one requires the size of the segmentation display to be set in the Gui constructor to 224,224 153 | gui->displayImg("raw",map->getRawImageTexture()); 154 | gui->postCall(); 155 | if (gui->reset()) { 156 | map.reset(new ElasticFusionInterface()); 157 | if (!map->Init(class_colour_lookup)) { 158 | std::cout<<"ElasticFusionInterface init failure"< 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "Gui.h" 20 | #include "GuiCuda.h" 21 | #include 22 | 23 | #define gpuErrChk(ans) { gpuAssert((ans), __FILE__, __LINE__); } 24 | 25 | inline void gpuAssert(cudaError_t code, const char *file, int line, bool 26 | abort=true) { 27 | if (code != cudaSuccess) { 28 | fprintf(stderr,"GPUassert: %s %s %d\n", 29 | cudaGetErrorString(code), file, line); 30 | if (abort) exit(code); 31 | } 32 | } 33 | 34 | struct ClassIdInput 35 | { 36 | ClassIdInput() 37 | : class_id_(0) {} 38 | ClassIdInput(int class_id) 39 | : class_id_(class_id) {} 40 | int class_id_; 41 | }; 42 | 43 | std::ostream& operator<< (std::ostream& os, const ClassIdInput& o){ 44 | os << o.class_id_; 45 | return os; 46 | } 47 | 48 | std::istream& operator>> (std::istream& is, ClassIdInput& o){ 49 | is >> o.class_id_; 50 | return is; 51 | } 52 | 53 | 54 | Gui::Gui(bool live_capture, std::vector class_colour_lookup, const int segmentation_width, const int segmentation_height) 55 | : width_(1280) 56 | , height_(980) 57 | , segmentation_width_(segmentation_width) 58 | , segmentation_height_(segmentation_height) 59 | , panel_(205) 60 | , class_colour_lookup_(class_colour_lookup) 61 | { 62 | width_ += panel_; 63 | pangolin::Params window_params; 64 | window_params.Set("SAMPLE_BUFFERS", 0); 65 | window_params.Set("SAMPLES", 0); 66 | pangolin::CreateWindowAndBind("SemanticFusion", width_, height_, window_params); 67 | render_buffer_ = new pangolin::GlRenderBuffer(3840, 2160); 68 | color_texture_ = new GPUTexture(render_buffer_->width, render_buffer_->height, GL_RGBA32F, GL_LUMINANCE, GL_FLOAT, true); 69 | color_frame_buffer_ = new pangolin::GlFramebuffer; 70 | color_frame_buffer_->AttachColour(*color_texture_->texture); 71 | color_frame_buffer_->AttachDepth(*render_buffer_); 72 | s_cam_ = pangolin::OpenGlRenderState(pangolin::ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000), 73 | pangolin::ModelViewLookAt(0, 0, -1, 0, 0, 1, pangolin::AxisNegY)); 74 | pangolin::Display("cam").SetBounds(0, 1.0f, 0, 1.0f, -640 / 480.0) 75 | .SetHandler(new pangolin::Handler3D(s_cam_)); 76 | // Small views along the bottom 77 | pangolin::Display("raw").SetAspect(640.0f/480.0f); 78 | pangolin::Display("pred").SetAspect(640.0f/480.0f); 79 | pangolin::Display("segmentation").SetAspect(640.0f/480.0f); 80 | pangolin::Display("multi").SetBounds(pangolin::Attach::Pix(0),1/4.0f,pangolin::Attach::Pix(180),1.0).SetLayout(pangolin::LayoutEqualHorizontal) 81 | .AddDisplay(pangolin::Display("pred")) 82 | .AddDisplay(pangolin::Display("segmentation")) 83 | .AddDisplay(pangolin::Display("raw")); 84 | 85 | // Vertical view along the side 86 | pangolin::Display("legend").SetAspect(640.0f/480.0f); 87 | pangolin::Display("vert").SetBounds(pangolin::Attach::Pix(0),1/4.0f,pangolin::Attach::Pix(180),1.0).SetLayout(pangolin::LayoutEqualVertical) 88 | .AddDisplay(pangolin::Display("legend")); 89 | 90 | // The control panel 91 | pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(panel_)); 92 | pause_.reset(new pangolin::Var("ui.Pause", false, true)); 93 | step_.reset(new pangolin::Var("ui.Step", false, false)); 94 | reset_.reset(new pangolin::Var("ui.Reset", false, false)); 95 | tracking_.reset(new pangolin::Var("ui.Tracking Only", false, false)); 96 | class_view_.reset(new pangolin::Var("ui.Class Colours", false, false)); 97 | class_choice_.reset(new pangolin::Var("ui.Show class probs", ClassIdInput(0))); 98 | probability_texture_array_.reset(new pangolin::GlTextureCudaArray(224,224,GL_LUMINANCE32F_ARB)); 99 | rendered_segmentation_texture_array_.reset(new pangolin::GlTextureCudaArray(segmentation_width_,segmentation_height_,GL_RGBA32F)); 100 | 101 | // The gpu colour lookup 102 | std::vector class_colour_lookup_rgb; 103 | for (unsigned int class_id = 0; class_id < class_colour_lookup_.size(); ++class_id) { 104 | class_colour_lookup_rgb.push_back(static_cast(class_colour_lookup_[class_id].r)/255.0f); 105 | class_colour_lookup_rgb.push_back(static_cast(class_colour_lookup_[class_id].g)/255.0f); 106 | class_colour_lookup_rgb.push_back(static_cast(class_colour_lookup_[class_id].b)/255.0f); 107 | } 108 | cudaMalloc((void **)&class_colour_lookup_gpu_, class_colour_lookup_rgb.size() * sizeof(float)); 109 | cudaMemcpy(class_colour_lookup_gpu_, class_colour_lookup_rgb.data(), class_colour_lookup_rgb.size() * sizeof(float), cudaMemcpyHostToDevice); 110 | cudaMalloc((void **)&segmentation_rendering_gpu_, 4 * segmentation_width_ * segmentation_height_ * sizeof(float)); 111 | } 112 | 113 | Gui::~Gui() { 114 | cudaFree(class_colour_lookup_gpu_); 115 | cudaFree(segmentation_rendering_gpu_); 116 | } 117 | 118 | void Gui::preCall() 119 | { 120 | glEnable(GL_DEPTH_TEST); 121 | glDepthMask(GL_TRUE); 122 | glDepthFunc(GL_LESS); 123 | glClearColor(1.0,1.0,1.0, 0.0f); 124 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 125 | width_ = pangolin::DisplayBase().v.w; 126 | height_ = pangolin::DisplayBase().v.h; 127 | pangolin::Display("cam").Activate(s_cam_); 128 | } 129 | 130 | void Gui::renderMap(const std::unique_ptr& map) { 131 | map->RenderMapToBoundGlBuffer(s_cam_,class_colours()); 132 | } 133 | 134 | void Gui::postCall() { 135 | pangolin::FinishFrame(); 136 | glFinish(); 137 | } 138 | 139 | void Gui::displayArgMaxClassColouring(const std::string & id, float* device_ptr, int channels, const float* map_max, const int map_size,cudaTextureObject_t ids, const float threshold) { 140 | colouredArgMax(segmentation_width_*segmentation_height_,device_ptr,channels,class_colour_lookup_gpu_,segmentation_rendering_gpu_,map_max,map_size,ids,threshold); 141 | gpuErrChk(cudaGetLastError()); 142 | gpuErrChk(cudaGetLastError()); 143 | pangolin::CudaScopedMappedArray arr_tex(*rendered_segmentation_texture_array_.get()); 144 | cudaMemcpyToArray(*arr_tex, 0, 0, (void*)segmentation_rendering_gpu_, sizeof(float) * 4 * segmentation_width_ * segmentation_height_, cudaMemcpyDeviceToDevice); 145 | gpuErrChk(cudaGetLastError()); 146 | gpuErrChk(cudaGetLastError()); 147 | glDisable(GL_DEPTH_TEST); 148 | pangolin::Display(id).Activate(); 149 | rendered_segmentation_texture_array_->RenderToViewport(true); 150 | glEnable(GL_DEPTH_TEST); 151 | } 152 | 153 | void Gui::displayRawNetworkPredictions(const std::string & id, float* device_ptr) { 154 | pangolin::CudaScopedMappedArray arr_tex(*probability_texture_array_.get()); 155 | gpuErrChk(cudaGetLastError()); 156 | float* my_device_ptr = device_ptr + (224 * 224) * class_choice_.get()->Get().class_id_; 157 | cudaMemcpyToArray(*arr_tex, 0, 0, (void*)my_device_ptr, sizeof(float) * 224 * 224 , cudaMemcpyDeviceToDevice); 158 | gpuErrChk(cudaGetLastError()); 159 | glDisable(GL_DEPTH_TEST); 160 | pangolin::Display(id).Activate(); 161 | probability_texture_array_->RenderToViewport(true); 162 | glEnable(GL_DEPTH_TEST); 163 | } 164 | 165 | void Gui::displayImg(const std::string & id, GPUTexture * img) { 166 | glDisable(GL_DEPTH_TEST); 167 | pangolin::Display(id).Activate(); 168 | img->texture->RenderToViewport(true); 169 | glEnable(GL_DEPTH_TEST); 170 | } 171 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 偏重怎样结合CNN搭建一套稠密语义SLAM的系统。SemanticFusion架构上主要分为三部分： 2 | 3 | 1） 前面提到过的ElasticFusion这种稠密SLAM来计算位姿并建出稠密地图； 4 | 稠密SLAM重建目前也相对比较成熟， 5 | 从最开始的KinectFusion（TSDF数据结构 + ICP）， 6 | 到后来的InfiniTAM（用哈希表来索引很稀疏的voxel）, 7 | ElasticFusion（用surfel点表示模型并用非刚性的图结构）， 8 | DynamicFusion（引入了体翘曲场这样深度数据通过体翘曲场的变换后， 9 | 才能融入到TSDF数据结构中，完成有非刚性物体的动态场景重建）都做的比较成熟。 10 | 工业界实现非常好的是微软的HoloLens，在台积电的24核DSP上把mesh simplification这些操作都搞了上去。 11 | 12 | 2） CNN用RGB或RGBD图来生成一个概率图，每个像素都对应着识别出来的物体类别； 13 | 14 | 3）通过贝叶斯更新(CRF,条件随机场)来把识别的结果和SLAM生成的关联信息整合进统一的稠密语义地图中。 15 | 16 | 17 | 18 | # README # 19 | 20 | ### Related publications ### 21 | Please cite this work if you make use of our system in any of your own endeavors: 22 | 23 | * **[SemanticFusion: Dense 3D Semantic Mapping with Convolutional Neural Networks](http://wp.doc.ic.ac.uk/bjm113/wp-content/uploads/sites/113/2017/07/SemanticFusion_ICRA17_CameraReady.pdf)**, *J. McCormac, A. Handa, A. J. Davison, and S. Leutenegger*, ICRA '17 24 | * **[ElasticFusion: Real-Time Dense SLAM and Light Source Estimation](http://www.thomaswhelan.ie/Whelan16ijrr.pdf)**, *T. Whelan, R. F. Salas-Moreno, B. Glocker, A. J. Davison and S. Leutenegger*, IJRR '16 25 | 26 | # 1. How to build it? # 27 | The system has been developed for Linux. It has been tested on Arch Linux, with gcc-7 (gcc-5 for CUDA compilation), CUDA 8.0, OpenGL 4.5.0 (NVIDIA Driver 384.90), and CMake 3.8.2, with an Intel Core i7-5820K CPU @ 3.30GHz and Nvidia Geforce GTX 1080. Unfortunately I do not have the capacity to debug system specific issues, but will try to help time-permitting. 28 | Clone recursively with: 29 | 30 | ``` 31 | git clone --recursive https://bitbucket.org/dysonroboticslab/semanticfusion 32 | ``` 33 | 34 | Make sure you can have all the dependencies and can compile and run the two major software packages this framework uses: [elasticfusion](https://github.com/mp3guy/ElasticFusion) and [caffe](https://github.com/BVLC/caffe) (an old version with commit ```b23e9b1b2196a230d0```). They have both been slightly modified for this repo, so to be sure they are working build both of the projects that are cloned within the this repo. The changes however are minor, so the compilation and dependency instructions provided for those projects still apply. 35 | 36 | For ElasticFusion use the CMake build system, rather than the build.sh. Only the ```Core``` ElasticFusion folder needs to compile, however by building the GUI and checking ElasticFusion runs live is useful for debugging issues down the line, as SemanticFusion uses a similar GUI. 37 | 38 | For caffe it uses the OpenBLAS library (from the [AUR](https://aur.archlinux.org/packages/openblas-lapack/)) by default as specified in ```caffe/cmake/Dependencies.cmake``` line 84. Feel free to use another implementation - although note it has not been tested. Make sure to build caffe with CMake, not make, i.e. 39 | 40 | ``` 41 | mkdir caffe_semanticfusion/build 42 | cd caffe_semanticfusion/build 43 | cmake .. 44 | make 45 | ``` 46 | 47 | If both of the dependencies are working, make a build directory and compile - this should build both sub-projects and then SemanticFusion. 48 | 49 | ``` 50 | cd semanticfusion 51 | mkdir build 52 | cd build 53 | cmake .. 54 | make -j7 55 | ``` 56 | 57 | There are sometimes issues with finding OpenNI2. A quick fix for this can be to edit the ```FindOpenNI2.cmake``` file in the root directory to point towards the location of your OpenNI2 directory. These paths should be added in two places. I.e. 58 | 59 | ``` 60 | ... 61 | 28 "${CMAKE_SOURCE_DIR}/../../../code/OpenNI2/Include" 62 | xx /path/to/your/OpenNI2/Include 63 | 29 /usr/include 64 | ... 65 | 52 "${CMAKE_SOURCE_DIR}/../../../code/OpenNI2/Bin/x64-Release" 66 | xx /path/to/your/OpenNI2/Bin/x64-Release 67 | 53 /usr/lib 68 | ``` 69 | 70 | # 2. Download Models # 71 | 72 | The CNN models are available [here](https://www.doc.ic.ac.uk/~bjm113/semantic_fusion_data/) with the _model.tar.gz suffix. Download and extract them to the caffe subfolder of this project ```caffe_semanticfusion/models/nyu_rgbd``` and ```caffe_semanticfusion/models/nyu_rgb```. By default the ```nyu_rgbd``` CNN is used, but to edit this (for example to the RGB-only CNN, simply modify the CNN path in the ```src/main.cpp```, and recompile. I.e. 73 | 74 | ``` 75 | caffe.Init("../caffe_semanticfusion/models/nyu_rgb/inference.prototxt","../caffe_semanticfusion/models/nyu_rgb/inference.caffemodel"); 76 | ``` 77 | 78 | # 3. How to run it? # 79 | 80 | If you have a primesense camera and OpenNI2 working (i.e. you can run ElasticFusion live) then you can run SemanticFusion with the 13-NYU classes by simply running the program with no arguments in the build directory. You need to make sure OpenNI2 can detect and access the feed from the camera, and if you have multi-gpus ensure your desktop is running on the same GPU as SemanticFusion as an OpenGL context is required. 81 | 82 | ``` 83 | ./SemanticFusion 84 | ``` 85 | 86 | A sequence of the NYU preprocessed dataset used for validation has been made available [here](https://www.doc.ic.ac.uk/~bjm113/semantic_fusion_data/) in ```nyu_data_small.tar.gz```. Each sequence is quite large, so only a single 200MB sequence has been made available. Download and extract this to any local directory e.g. ```/path/to/your/nyu_data_small```. 87 | 88 | To run on this sequence provide two arguments to the SemanticFusion program to save the prediction pngs to the working directory (NOTE the second argument does not indicate where predictions are saved, it is a textfile denoting which predictions should be saved as they have a corresponding NYU ground truth annotation): 89 | 90 | ``` 91 | ./SemanticFusion /path/to/your/nyu_data_small/bathroom_0003.txt /path/to/your/nyu_data_small/output_predictions.txt 92 | ``` 93 | 94 | The folder contains a textfile with frame paths in order, and a labelled frame textfile which specified which frames to output as labelled predictions. These predictions are raw and so have 0 labels in areas without the reconstructed map. To produce the final output predictions used for results, these zeros are overwritten by the baseline single-frame CNN network predictions. 95 | 96 | The bathroom_0003.txt file looks like this: 97 | 98 | ``` 99 | 1295406963667 /mnt/disk/datasets/nyu_processed/bathroom_0003/1_depth.png /mnt/disk/datasets/nyu_processed/bathroom_0003/1_rgb.png d-1295406963.666549-2770369813.pgm r-1295406963.67524 9-2771355972.ppm 100 | 1295406963699 /mnt/disk/datasets/nyu_processed/bathroom_0003/2_depth.png /mnt/disk/datasets/nyu_processed/bathroom_0003/2_rgb.png d-1295406963.698573-2772371968.pgm r-1295406963.70540 3-2773356259.ppm 101 | 1295406963730 /mnt/disk/datasets/nyu_processed/bathroom_0003/3_depth.png /mnt/disk/datasets/nyu_processed/bathroom_0003/3_rgb.png d-1295406963.729955-2774374123.pgm r-1295406963.73539 8-2775356547.ppm 102 | ``` 103 | 104 | And the output_predictions text file has the NYU depth and rgb frame id to test label number for validation. 105 | 106 | ``` 107 | kitchen_0004/d-1294851097.542259-2322437371.pgm kitchen_0004/r-1294851097.528505-2321469380.ppm 1 108 | kitchen_0004/d-1294851106.887887-2881038616.pgm kitchen_0004/r-1294851106.894265-2883550167.ppm 2 109 | office_0005/d-1294851456.202533-2366762831.pgm office_0005/r-1294851456.187715-2365725825.ppm 9 110 | ``` 111 | 112 | # 4. License # 113 | SemanticFusion is freely available for non-commercial use only. Full terms and conditions which govern its use are detailed [here](http://www.imperial.ac.uk/dyson-robotics-lab/downloads/semantic-fusion/semantic-fusion-license/) and in the LICENSE.txt file. 114 | 115 | # 5. Notes # 116 | 117 | *** Performance vs. paper *** 118 | 119 | This system has been re-based to the open-source version of ElasticFusion from an internal version. As mentioned in its README: 120 | 121 | * 122 | A substantial amount of refactoring was carried out in order to open source this system, including rewriting a lot of functionality to avoid certain licenses and reduce dependencies. Although great care was taken during this process, it is possible that performance regressions were introduced and have not yet been discovered. 123 | * 124 | 125 | However, we re-validated the RGBD-SF on the NYU test set using this public implementation and the results where very similar (58.8% class average, 67.4% pixel avg.). 126 | 127 | *** Colour scheme *** 128 | 129 | The colour scheme used for semantic classes is given in the ```class_colour_scheme.data``` file. As an easy lookup, the legend is uploaded in this project. 130 | 131 | ![Legend](legend_vertical.png) 132 | -------------------------------------------------------------------------------- /src/semantic_fusion/SemanticFusionInterface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "SemanticFusionInterface.h" 20 | #include "SemanticFusionCuda.h" 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace std 35 | { 36 | template 37 | inline void my_hash_combine(std::size_t & seed, const T & v) 38 | { 39 | std::hash hasher; 40 | seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2); 41 | } 42 | 43 | template struct hash> 44 | { 45 | inline size_t operator()(const pair & v) const 46 | { 47 | size_t seed = 0; 48 | my_hash_combine(seed, v.first); 49 | my_hash_combine(seed, v.second); 50 | return seed; 51 | } 52 | }; 53 | } 54 | 55 | // Basic algorithm for removing old items from probability table 56 | template 57 | void remove_index(std::vector& vector, const std::vector& to_remove) { 58 | auto vector_base = vector.begin(); 59 | typename std::vector::size_type down_by = 0; 60 | for (auto iter = to_remove.cbegin(); 61 | iter < to_remove.cend(); 62 | iter++, down_by++) 63 | { 64 | typename std::vector::size_type next = (iter + 1 == to_remove.cend() 65 | ? vector.size() 66 | : *(iter + 1)); 67 | std::move(vector_base + *iter + 1, 68 | vector_base + next, 69 | vector_base + *iter - down_by); 70 | } 71 | vector.resize(vector.size() - to_remove.size()); 72 | } 73 | 74 | void SemanticFusionInterface::CalculateProjectedProbabilityMap(const std::unique_ptr& map) { 75 | const int id_width = map->width(); 76 | const int id_height = map->height(); 77 | const int table_height = class_probabilities_gpu_->height(); 78 | const int table_width = class_probabilities_gpu_->width(); 79 | renderProbabilityMap(map->GetSurfelIdsGpu(),id_width,id_height, 80 | class_probabilities_gpu_->mutable_gpu_data(), 81 | table_width,table_height, 82 | rendered_class_probabilities_gpu_->mutable_gpu_data()); 83 | } 84 | 85 | std::shared_ptr > SemanticFusionInterface::get_rendered_probability() { 86 | return rendered_class_probabilities_gpu_; 87 | } 88 | 89 | std::shared_ptr > SemanticFusionInterface::get_class_max_gpu() { 90 | return class_max_gpu_; 91 | } 92 | 93 | int SemanticFusionInterface::max_num_components() const { 94 | return max_components_; 95 | } 96 | 97 | void SemanticFusionInterface::UpdateProbabilityTable(const std::unique_ptr& map) 98 | { 99 | const int new_table_width = map->GetMapSurfelCount(); 100 | const int num_deleted = map->GetMapSurfelDeletedCount(); 101 | const int table_width = class_probabilities_gpu_->width(); 102 | const int table_height = class_probabilities_gpu_->height(); 103 | updateProbabilityTable(map->GetDeletedSurfelIdsGpu(),num_deleted,current_table_size_, 104 | class_probabilities_gpu_->gpu_data(), table_width, table_height, 105 | new_table_width, class_probabilities_gpu_buffer_->mutable_gpu_data(), 106 | class_max_gpu_->gpu_data(),class_max_gpu_buffer_->mutable_gpu_data()); 107 | // We then swap the pointers from the buffer to the other one 108 | class_probabilities_gpu_.swap(class_probabilities_gpu_buffer_); 109 | class_max_gpu_.swap(class_max_gpu_buffer_); 110 | current_table_size_ = new_table_width; 111 | } 112 | 113 | int SemanticFusionInterface::UpdateSurfelProbabilities(const int surfel_id, 114 | const std::vector& class_probs) 115 | { 116 | assert(static_cast(class_probabilities_.size()) > surfel_id); 117 | std::vector& surfel_probs = class_probabilities_[surfel_id]; 118 | assert(static_cast(class_probs.size()) == num_classes_); 119 | assert(static_cast(surfel_probs.size()) == num_classes_); 120 | float normalisation_denominator = 0.0; 121 | for (int class_id = 0; class_id < num_classes_; class_id++) { 122 | surfel_probs[class_id] *= class_probs[class_id]; 123 | normalisation_denominator += surfel_probs[class_id]; 124 | } 125 | float max_prob = 0.0; 126 | int max_class = -1; 127 | for (int class_id = 0; class_id < num_classes_; class_id++) { 128 | surfel_probs[class_id] /= normalisation_denominator; 129 | if (surfel_probs[class_id] >= max_prob) { 130 | max_prob = surfel_probs[class_id]; 131 | max_class = class_id; 132 | } 133 | } 134 | if (max_prob >= colour_threshold_) { 135 | return max_class; 136 | } 137 | return -1; 138 | } 139 | 140 | void SemanticFusionInterface::UpdateProbabilities(std::shared_ptr > probs, 141 | const std::unique_ptr& map) 142 | { 143 | CHECK_EQ(num_classes_,probs->channels()); 144 | const int id_width = map->width(); 145 | const int id_height = map->height(); 146 | const int prob_width = probs->width(); 147 | const int prob_height = probs->height(); 148 | const int prob_channels = probs->channels(); 149 | const int map_size = class_probabilities_gpu_->width(); 150 | fuseSemanticProbabilities(map->GetSurfelIdsGpu(),id_width,id_height,probs->gpu_data(), 151 | prob_width,prob_height,prob_channels, 152 | class_probabilities_gpu_->mutable_gpu_data(), 153 | class_max_gpu_->mutable_gpu_data(),map_size); 154 | map->UpdateSurfelClassGpu(map_size,class_max_gpu_->gpu_data(),class_max_gpu_->gpu_data() + map_size,colour_threshold_); 155 | } 156 | 157 | void SemanticFusionInterface::CRFUpdate(const std::unique_ptr& map, const int iterations) { 158 | float* surfel_map = map->GetMapSurfelsGpu(); 159 | // We very inefficiently allocate and clear a chunk of memory for every CRF update 160 | float * my_surfels = new float[current_table_size_ * 12]; 161 | cudaMemcpy(my_surfels,surfel_map, sizeof(float) * current_table_size_ * 12, cudaMemcpyDeviceToHost); 162 | // Get the semantic table on CPU and add as unary potentials 163 | float* prob_table = class_probabilities_gpu_->mutable_cpu_data(); 164 | 165 | std::vector valid_ids; 166 | for (int i = 0; i < current_table_size_; ++i) { 167 | valid_ids.push_back(i); 168 | } 169 | std::vector unary_potentials(valid_ids.size() * num_classes_); 170 | for(int i = 0; i < static_cast(valid_ids.size()); ++i) { 171 | int id = valid_ids[i]; 172 | for (int j = 0; j < num_classes_; ++j) { 173 | unary_potentials[i * num_classes_ + j] = -log(prob_table[j * max_components_ + id] + 1.0e-12); 174 | } 175 | } 176 | DenseCRF3D crf(valid_ids.size(),num_classes_,0.05,20,0.1); 177 | crf.setUnaryEnergy(unary_potentials.data()); 178 | // Add pairwise energies 179 | crf.addPairwiseGaussian(my_surfels,3,valid_ids); 180 | crf.addPairwiseBilateral(my_surfels,10,valid_ids); 181 | // Finally read the values back to the probability table 182 | float* resulting_probs = crf.runInference(iterations, 1.0); 183 | for (int i = 0; i < static_cast(valid_ids.size()); ++i) { 184 | for (int j = 0; j < num_classes_; ++j) { 185 | const int id = valid_ids[i]; 186 | // Sometimes it returns nan resulting probs... filter these out 187 | if (resulting_probs[i * num_classes_ + j] > 0.0 && resulting_probs[i * num_classes_ + j] < 1.0) { 188 | prob_table[j * max_components_ + id] = resulting_probs[i * num_classes_ + j]; 189 | } 190 | } 191 | } 192 | const float* gpu_prob_table = class_probabilities_gpu_->gpu_data(); 193 | float* gpu_max_map = class_max_gpu_->mutable_gpu_data(); 194 | updateMaxClass(current_table_size_,gpu_prob_table,num_classes_,gpu_max_map,max_components_); 195 | map->UpdateSurfelClassGpu(max_components_,class_max_gpu_->gpu_data(),class_max_gpu_->gpu_data() + max_components_,colour_threshold_); 196 | delete [] my_surfels; 197 | } 198 | 199 | void SemanticFusionInterface::SaveArgMaxPredictions(std::string& filename,const std::unique_ptr& map) { 200 | const float* max_prob = class_max_gpu_->cpu_data() + max_components_; 201 | const float* max_class = class_max_gpu_->cpu_data(); 202 | const std::vector& surfel_ids = map->GetSurfelIdsCpu(); 203 | cv::Mat argmax_image(240,320,CV_8UC3); 204 | for (int h = 0; h < 240; ++h) { 205 | for (int w = 0; w < 320; ++w) { 206 | float this_max_prob = 0.0; 207 | int this_max_class = 0; 208 | const int start = 0; 209 | const int end = 2; 210 | for (int x = start; x < end; ++x) { 211 | for (int y = start; y < end; ++y) { 212 | int id = surfel_ids[((h * 2) + y) * 640 + (w * 2 + x)]; 213 | if (id > 0 && id < current_table_size_) { 214 | if (max_prob[id] > this_max_prob) { 215 | this_max_prob = max_prob[id]; 216 | this_max_class = max_class[id]; 217 | } 218 | } 219 | } 220 | } 221 | argmax_image.at(h,w)[0] = static_cast(this_max_class); 222 | argmax_image.at(h,w)[1] = static_cast(this_max_class); 223 | argmax_image.at(h,w)[2] = static_cast(this_max_class); 224 | } 225 | } 226 | cv::imwrite(filename,argmax_image); 227 | } 228 | -------------------------------------------------------------------------------- /src/semantic_fusion/SemanticFusionCuda.cu: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of SemanticFusion. 3 | * 4 | * Copyright (C) 2017 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is SemanticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | #define gpuErrChk(ans) { gpuAssert((ans), __FILE__, __LINE__); } 25 | 26 | inline void gpuAssert(cudaError_t code, const char *file, int line, bool 27 | abort=true) { 28 | if (code != cudaSuccess) { 29 | fprintf(stderr,"GPUassert: %s %s %d\n", 30 | cudaGetErrorString(code), file, line); 31 | if (abort) exit(code); 32 | } 33 | } 34 | 35 | __global__ 36 | void semanticTableUpdate(cudaTextureObject_t ids, const int ids_width, const int ids_height, 37 | const float* probabilities, const int prob_width, const int prob_height, 38 | const int prob_channels,float* map_table,float* map_max, 39 | const int map_size) 40 | { 41 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 42 | const int y = blockIdx.y * blockDim.y + threadIdx.y; 43 | // New uniqueness code 44 | const int check_patch = 16; 45 | const int x_min = (x - check_patch) < 0 ? 0 : (x - check_patch); 46 | const int x_max = (x + check_patch) > 640 ? 640 : (x + check_patch); 47 | const int y_min = (y - check_patch) < 0 ? 0 : (y - check_patch); 48 | int surfel_id = tex2D(ids,x,y); 49 | int first_h, first_w; 50 | for (int h = y_min; h < 480; ++h) { 51 | for (int w = x_min; w < x_max; ++w) { 52 | int other_surfel_id = tex2D(ids,w,h); 53 | if (other_surfel_id == surfel_id) { 54 | first_h = h; 55 | first_w = w; 56 | break; 57 | } 58 | } 59 | } 60 | if (first_h != y || first_w != x) { 61 | surfel_id = 0; 62 | } 63 | if (surfel_id > 0) { 64 | const int prob_x = static_cast((float(x) / ids_width) * prob_width); 65 | const int prob_y = static_cast((float(y) / ids_height) * prob_height); 66 | const int channel_offset = prob_width * prob_height; 67 | const float* probability = probabilities + (prob_y * prob_width + prob_x); 68 | float* prior_probability = map_table + surfel_id; 69 | float total = 0.0; 70 | for (int class_id = 0; class_id < prob_channels; ++class_id) { 71 | prior_probability[0] *= probability[0]; 72 | total += prior_probability[0]; 73 | probability += channel_offset; 74 | prior_probability += map_size; 75 | } 76 | // Reset the pointers to the beginning again 77 | probability = probabilities + (prob_y * prob_width + prob_x); 78 | prior_probability = map_table + surfel_id; 79 | float max_probability = 0.0; 80 | int max_class = -1; 81 | float new_total = 0.0; 82 | for (int class_id = 0; class_id < prob_channels; ++class_id) { 83 | // Something has gone unexpectedly wrong - reinitialse 84 | if (total <= 1e-5) { 85 | prior_probability[0] = 1.0f / prob_channels; 86 | } else { 87 | prior_probability[0] /= total; 88 | if (class_id > 0 && prior_probability[0] > max_probability) { 89 | max_probability = prior_probability[0]; 90 | max_class = class_id; 91 | } 92 | } 93 | new_total += prior_probability[0]; 94 | probability += channel_offset; 95 | prior_probability += map_size; 96 | } 97 | map_max[surfel_id] = static_cast(max_class); 98 | map_max[surfel_id + map_size] = max_probability; 99 | map_max[surfel_id + map_size + map_size] += 1.0; 100 | } 101 | } 102 | 103 | __host__ 104 | void fuseSemanticProbabilities(cudaTextureObject_t ids, const int ids_width, const int ids_height, 105 | const float* probabilities, const int prob_width, const int prob_height, 106 | const int prob_channels,float* map_table, float* map_max, 107 | const int map_size) 108 | { 109 | // NOTE Res must be pow 2 and > 32 110 | const int blocks = 32; 111 | dim3 dimGrid(blocks,blocks); 112 | dim3 dimBlock(640/blocks,480/blocks); 113 | semanticTableUpdate<<>>(ids,ids_width,ids_height,probabilities,prob_width,prob_height,prob_channels,map_table,map_max,map_size); 114 | gpuErrChk(cudaGetLastError()); 115 | gpuErrChk(cudaDeviceSynchronize()); 116 | } 117 | 118 | __global__ 119 | void updateTable(int n, const int* deleted_ids, const int num_deleted, const int current_table_size, 120 | float const* probability_table, const int prob_width, const int prob_height, 121 | const int new_prob_width, float* new_probability_table, float const * map_table, float* new_map_table) 122 | { 123 | const int index = blockIdx.x * blockDim.x + threadIdx.x; 124 | if (index < n) { 125 | const int class_id = index / new_prob_width; 126 | const int component_id = index - (class_id * new_prob_width); 127 | const int new_id = (class_id * prob_width) + component_id; 128 | if (component_id >= num_deleted) { 129 | // Initialise to prior (prob height is the number of classes) 130 | new_probability_table[new_id] = 1.0f / prob_height; 131 | // Reset the max class surfel colouring lookup 132 | new_map_table[component_id] = -1.0; 133 | new_map_table[component_id + prob_width] = -1.0; 134 | new_map_table[component_id + prob_width + prob_width] = 0.0; 135 | } else { 136 | int offset = deleted_ids[component_id]; 137 | new_probability_table[new_id] = probability_table[(class_id * prob_width) + offset]; 138 | // Also must update our max class mapping 139 | new_map_table[component_id] = map_table[offset]; 140 | new_map_table[component_id + prob_width] = map_table[prob_width + offset]; 141 | new_map_table[component_id + prob_width + prob_width] = map_table[prob_width + prob_width + offset]; 142 | } 143 | } 144 | } 145 | 146 | __host__ 147 | void updateProbabilityTable(int* filtered_ids, const int num_filtered, const int current_table_size, 148 | float const* probability_table, const int prob_width, const int prob_height, 149 | const int new_prob_width, float* new_probability_table, 150 | float const* map_table, float* new_map_table) 151 | { 152 | const int threads = 512; 153 | const int num_to_update = new_prob_width * prob_height; 154 | const int blocks = (num_to_update + threads - 1) / threads; 155 | dim3 dimGrid(blocks); 156 | dim3 dimBlock(threads); 157 | updateTable<<>>(num_to_update,filtered_ids,num_filtered,current_table_size,probability_table,prob_width,prob_height,new_prob_width,new_probability_table, map_table, new_map_table); 158 | gpuErrChk(cudaGetLastError()); 159 | gpuErrChk(cudaDeviceSynchronize()); 160 | } 161 | 162 | 163 | __global__ 164 | void renderProbabilityMapKernel(cudaTextureObject_t ids, const int ids_width, const int ids_height, 165 | const float* probability_table, const int prob_width, const int prob_height, 166 | float* rendered_probabilities) 167 | { 168 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 169 | const int y = blockIdx.y * blockDim.y + threadIdx.y; 170 | const int surfel_id = tex2D(ids,x,y); 171 | int projected_probability_offset = y * ids_width + x; 172 | int probability_table_offset = surfel_id; 173 | for (int class_id = 0; class_id < prob_height; ++class_id) { 174 | if (surfel_id > 0) { 175 | rendered_probabilities[projected_probability_offset] = probability_table[probability_table_offset]; 176 | } else { 177 | rendered_probabilities[projected_probability_offset] = ((class_id == 0) ? 1.0 : 0.0); 178 | } 179 | projected_probability_offset += (ids_width * ids_height); 180 | probability_table_offset += prob_width; 181 | } 182 | } 183 | 184 | 185 | __host__ 186 | void renderProbabilityMap(cudaTextureObject_t ids, const int ids_width, const int ids_height, 187 | const float* probability_table, const int prob_width, const int prob_height, 188 | float* rendered_probabilities) 189 | { 190 | // NOTE Res must be pow 2 and > 32 191 | const int blocks = 32; 192 | dim3 dimGrid(blocks,blocks); 193 | dim3 dimBlock(ids_width/blocks,ids_height/blocks); 194 | renderProbabilityMapKernel<<>>(ids,ids_width,ids_height,probability_table,prob_width,prob_height,rendered_probabilities); 195 | gpuErrChk(cudaGetLastError()); 196 | gpuErrChk(cudaDeviceSynchronize()); 197 | } 198 | 199 | __global__ 200 | void updateMaxClassKernel(const int n, const float* probabilities, const int classes, 201 | float* map_max, const int map_size) 202 | { 203 | const int index = blockIdx.x * blockDim.x + threadIdx.x; 204 | if (index < n) { 205 | // Reset the pointers to the beginning again 206 | const float* probability = probabilities + index; 207 | probability += map_size; 208 | float max_probability = 0.0; 209 | int max_class = -1; 210 | for (int class_id = 1; class_id < classes; ++class_id) { 211 | if (probability[0] > max_probability) { 212 | max_probability = probability[0]; 213 | max_class = class_id; 214 | } 215 | probability += map_size; 216 | } 217 | map_max[index] = static_cast(max_class); 218 | map_max[index + map_size] = max_probability; 219 | } 220 | } 221 | 222 | __host__ 223 | void updateMaxClass(const int n, const float* probabilities, const int classes, 224 | float* map_max, const int map_size) 225 | { 226 | const int threads = 512; 227 | const int blocks = (n + threads - 1) / threads; 228 | dim3 dimGrid(blocks); 229 | dim3 dimBlock(threads); 230 | updateMaxClassKernel<<>>(n,probabilities,classes,map_max,map_size); 231 | gpuErrChk(cudaGetLastError()); 232 | gpuErrChk(cudaDeviceSynchronize()); 233 | } 234 | -------------------------------------------------------------------------------- /src/utilities/OpenNI2Interface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of ElasticFusion. 3 | * 4 | * Copyright (C) 2015 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that 7 | * make up the software that is ElasticFusion is permitted for 8 | * non-commercial purposes only. The full terms and conditions that 9 | * apply to the code within this file are detailed within the LICENSE.txt 10 | * file and at 11 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "OpenNI2Interface.h" 20 | 21 | OpenNI2Interface::OpenNI2Interface(int inWidth, int inHeight, int fps) 22 | : width(inWidth), 23 | height(inHeight), 24 | fps(fps), 25 | initSuccessful(true) 26 | { 27 | //Setup 28 | openni::Status rc = openni::STATUS_OK; 29 | 30 | const char * deviceURI = openni::ANY_DEVICE; 31 | 32 | rc = openni::OpenNI::initialize(); 33 | 34 | std::string errorString(openni::OpenNI::getExtendedError()); 35 | 36 | if(errorString.length() > 0) 37 | { 38 | errorText.append(errorString); 39 | initSuccessful = false; 40 | } 41 | else 42 | { 43 | rc = device.open(deviceURI); 44 | if (rc != openni::STATUS_OK) 45 | { 46 | errorText.append(openni::OpenNI::getExtendedError()); 47 | openni::OpenNI::shutdown(); 48 | initSuccessful = false; 49 | } 50 | else 51 | { 52 | openni::VideoMode depthMode; 53 | depthMode.setFps(fps); 54 | depthMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM); 55 | depthMode.setResolution(width, height); 56 | 57 | openni::VideoMode colorMode; 58 | colorMode.setFps(fps); 59 | colorMode.setPixelFormat(openni::PIXEL_FORMAT_RGB888); 60 | colorMode.setResolution(width, height); 61 | 62 | rc = depthStream.create(device, openni::SENSOR_DEPTH); 63 | if (rc == openni::STATUS_OK) 64 | { 65 | depthStream.setVideoMode(depthMode); 66 | rc = depthStream.start(); 67 | if (rc != openni::STATUS_OK) 68 | { 69 | errorText.append(openni::OpenNI::getExtendedError()); 70 | depthStream.destroy(); 71 | initSuccessful = false; 72 | } 73 | } 74 | else 75 | { 76 | errorText.append(openni::OpenNI::getExtendedError()); 77 | initSuccessful = false; 78 | } 79 | 80 | rc = rgbStream.create(device, openni::SENSOR_COLOR); 81 | if (rc == openni::STATUS_OK) 82 | { 83 | rgbStream.setVideoMode(colorMode); 84 | rc = rgbStream.start(); 85 | if (rc != openni::STATUS_OK) 86 | { 87 | errorText.append(openni::OpenNI::getExtendedError()); 88 | rgbStream.destroy(); 89 | initSuccessful = false; 90 | } 91 | } 92 | else 93 | { 94 | errorText.append(openni::OpenNI::getExtendedError()); 95 | initSuccessful = false; 96 | } 97 | 98 | if (!depthStream.isValid() || !rgbStream.isValid()) 99 | { 100 | errorText.append(openni::OpenNI::getExtendedError()); 101 | openni::OpenNI::shutdown(); 102 | initSuccessful = false; 103 | } 104 | 105 | if(initSuccessful) 106 | { 107 | //For printing out 108 | formatMap[openni::PIXEL_FORMAT_DEPTH_1_MM] = "1mm"; 109 | formatMap[openni::PIXEL_FORMAT_DEPTH_100_UM] = "100um"; 110 | formatMap[openni::PIXEL_FORMAT_SHIFT_9_2] = "Shift 9 2"; 111 | formatMap[openni::PIXEL_FORMAT_SHIFT_9_3] = "Shift 9 3"; 112 | 113 | formatMap[openni::PIXEL_FORMAT_RGB888] = "RGB888"; 114 | formatMap[openni::PIXEL_FORMAT_YUV422] = "YUV422"; 115 | formatMap[openni::PIXEL_FORMAT_GRAY8] = "GRAY8"; 116 | formatMap[openni::PIXEL_FORMAT_GRAY16] = "GRAY16"; 117 | formatMap[openni::PIXEL_FORMAT_JPEG] = "JPEG"; 118 | 119 | assert(findMode(width, height, fps) && "Sorry, mode not supported!"); 120 | 121 | latestDepthIndex.assign(-1); 122 | latestRgbIndex.assign(-1); 123 | 124 | for(int i = 0; i < numBuffers; i++) 125 | { 126 | uint8_t * newImage = (uint8_t *)calloc(width * height * 3, sizeof(uint8_t)); 127 | rgbBuffers[i] = std::pair(newImage, 0); 128 | } 129 | 130 | for(int i = 0; i < numBuffers; i++) 131 | { 132 | uint8_t * newDepth = (uint8_t *)calloc(width * height * 2, sizeof(uint8_t)); 133 | uint8_t * newImage = (uint8_t *)calloc(width * height * 3, sizeof(uint8_t)); 134 | frameBuffers[i] = std::pair, int64_t>(std::pair(newDepth, newImage), 0); 135 | } 136 | 137 | rgbCallback = new RGBCallback(lastRgbTime, 138 | latestRgbIndex, 139 | rgbBuffers); 140 | 141 | depthCallback = new DepthCallback(lastDepthTime, 142 | latestDepthIndex, 143 | latestRgbIndex, 144 | rgbBuffers, 145 | frameBuffers); 146 | 147 | depthStream.setMirroringEnabled(false); 148 | rgbStream.setMirroringEnabled(false); 149 | 150 | device.setDepthColorSyncEnabled(true); 151 | device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR); 152 | 153 | setAutoExposure(true); 154 | setAutoWhiteBalance(true); 155 | 156 | rgbStream.addNewFrameListener(rgbCallback); 157 | depthStream.addNewFrameListener(depthCallback); 158 | } 159 | } 160 | } 161 | } 162 | 163 | OpenNI2Interface::~OpenNI2Interface() 164 | { 165 | if(initSuccessful) 166 | { 167 | rgbStream.removeNewFrameListener(rgbCallback); 168 | depthStream.removeNewFrameListener(depthCallback); 169 | 170 | depthStream.stop(); 171 | rgbStream.stop(); 172 | depthStream.destroy(); 173 | rgbStream.destroy(); 174 | device.close(); 175 | openni::OpenNI::shutdown(); 176 | 177 | for(int i = 0; i < numBuffers; i++) 178 | { 179 | free(rgbBuffers[i].first); 180 | } 181 | 182 | for(int i = 0; i < numBuffers; i++) 183 | { 184 | free(frameBuffers[i].first.first); 185 | free(frameBuffers[i].first.second); 186 | } 187 | 188 | delete rgbCallback; 189 | delete depthCallback; 190 | } 191 | } 192 | 193 | bool OpenNI2Interface::findMode(int x, int y, int fps) 194 | { 195 | const openni::Array & depthModes = depthStream.getSensorInfo().getSupportedVideoModes(); 196 | 197 | bool found = false; 198 | 199 | for(int i = 0; i < depthModes.getSize(); i++) 200 | { 201 | if(depthModes[i].getResolutionX() == x && 202 | depthModes[i].getResolutionY() == y && 203 | depthModes[i].getFps() == fps) 204 | { 205 | found = true; 206 | break; 207 | } 208 | } 209 | 210 | if(!found) 211 | { 212 | return false; 213 | } 214 | 215 | found = false; 216 | 217 | const openni::Array & rgbModes = rgbStream.getSensorInfo().getSupportedVideoModes(); 218 | 219 | for(int i = 0; i < rgbModes.getSize(); i++) 220 | { 221 | if(rgbModes[i].getResolutionX() == x && 222 | rgbModes[i].getResolutionY() == y && 223 | rgbModes[i].getFps() == fps) 224 | { 225 | found = true; 226 | break; 227 | } 228 | } 229 | 230 | return found; 231 | } 232 | 233 | void OpenNI2Interface::printModes() 234 | { 235 | const openni::Array & depthModes = depthStream.getSensorInfo().getSupportedVideoModes(); 236 | 237 | openni::VideoMode currentDepth = depthStream.getVideoMode(); 238 | 239 | std::cout << "Depth Modes: (" << currentDepth.getResolutionX() << 240 | "x" << 241 | currentDepth.getResolutionY() << 242 | " @ " << 243 | currentDepth.getFps() << 244 | "fps " << 245 | formatMap[currentDepth.getPixelFormat()] << ")" << std::endl; 246 | 247 | for(int i = 0; i < depthModes.getSize(); i++) 248 | { 249 | std::cout << depthModes[i].getResolutionX() << 250 | "x" << 251 | depthModes[i].getResolutionY() << 252 | " @ " << 253 | depthModes[i].getFps() << 254 | "fps " << 255 | formatMap[depthModes[i].getPixelFormat()] << std::endl; 256 | } 257 | 258 | const openni::Array & rgbModes = rgbStream.getSensorInfo().getSupportedVideoModes(); 259 | 260 | openni::VideoMode currentRGB = depthStream.getVideoMode(); 261 | 262 | std::cout << "RGB Modes: (" << currentRGB.getResolutionX() << 263 | "x" << 264 | currentRGB.getResolutionY() << 265 | " @ " << 266 | currentRGB.getFps() << 267 | "fps " << 268 | formatMap[currentRGB.getPixelFormat()] << ")" << std::endl; 269 | 270 | for(int i = 0; i < rgbModes.getSize(); i++) 271 | { 272 | std::cout << rgbModes[i].getResolutionX() << 273 | "x" << 274 | rgbModes[i].getResolutionY() << 275 | " @ " << 276 | rgbModes[i].getFps() << 277 | "fps " << 278 | formatMap[rgbModes[i].getPixelFormat()] << std::endl; 279 | } 280 | } 281 | 282 | void OpenNI2Interface::setAutoExposure(bool value) 283 | { 284 | rgbStream.getCameraSettings()->setAutoExposureEnabled(value); 285 | } 286 | 287 | void OpenNI2Interface::setAutoWhiteBalance(bool value) 288 | { 289 | rgbStream.getCameraSettings()->setAutoWhiteBalanceEnabled(value); 290 | } 291 | 292 | bool OpenNI2Interface::getAutoExposure() 293 | { 294 | return rgbStream.getCameraSettings()->getAutoExposureEnabled(); 295 | } 296 | 297 | bool OpenNI2Interface::getAutoWhiteBalance() 298 | { 299 | return rgbStream.getCameraSettings()->getAutoWhiteBalanceEnabled(); 300 | } 301 | -------------------------------------------------------------------------------- /src/semantic_fusion/CRF/densecrf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Philipp Krähenbühl 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the Stanford University nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY Philipp Krähenbühl ''AS IS'' AND ANY 17 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL Philipp Krähenbühl BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include "densecrf.h" 29 | #include "fastmath.h" 30 | #include "permutohedral.h" 31 | #include "util.h" 32 | #include 33 | #include 34 | 35 | PairwisePotential::~PairwisePotential() { 36 | } 37 | 38 | SemiMetricFunction::~SemiMetricFunction() { 39 | } 40 | 41 | class PottsPotential: public PairwisePotential { 42 | protected: 43 | Permutohedral lattice_; 44 | PottsPotential( const PottsPotential&o ){} 45 | int N_; 46 | float w_; 47 | float *norm_; 48 | public: 49 | ~PottsPotential(){ 50 | deallocate( norm_ ); 51 | } 52 | PottsPotential(const float* features, int D, int N, float w, bool per_pixel_normalization=true) :N_(N), w_(w) { 53 | lattice_.init( features, D, N ); 54 | norm_ = allocate( N ); 55 | for ( int i=0; i& valid) { 161 | const int features = 6; 162 | const int surfel_size = 12; 163 | float * feature = new float [N_*features]; 164 | for (int i=0; i(&encoded_colour); 176 | feature[(i*features)+6] = static_cast(colour[0]) / colour_stddev_; 177 | feature[(i*features)+7] = static_cast(colour[1])/ colour_stddev_; 178 | feature[(i*features)+8] = static_cast(colour[2])/ colour_stddev_; 179 | */ 180 | } 181 | addPairwiseEnergy(feature, features, w, NULL); 182 | delete [] feature; 183 | } 184 | 185 | void DenseCRF3D::addPairwiseBilateral ( const float* surfel_data, float w, const std::vector& valid) { 186 | const int features = 6; 187 | float * feature = new float [N_*features]; 188 | const int surfel_size = 12; 189 | for (int i=0; i(encoded_colour); 204 | int r = static_cast(colour >> 16 & 0xFF); 205 | int g = static_cast(colour >> 8 & 0xFF); 206 | int b = static_cast(colour & 0xFF); 207 | feature[(i*features)+(idx++)] = static_cast(r) / colour_stddev_; 208 | feature[(i*features)+(idx++)] = static_cast(g) / colour_stddev_; 209 | feature[(i*features)+(idx++)] = static_cast(b) / colour_stddev_; 210 | } 211 | addPairwiseEnergy(feature, features, w, NULL); 212 | delete [] feature; 213 | } 214 | 215 | void DenseCRF3D::addPairwiseNormal ( const float* surfel_data, float w) { 216 | /* 217 | float * feature = new float [N_*3]; 218 | const int surfel_size = 12; 219 | for (int i=0; iapply( next_, current_, tmp_, M_ ); 313 | // Exponentiate and normalize 314 | expAndNormalize( current_, next_, 1.0, relax ); 315 | } 316 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | SEMANTICFUSION SOFTWARE 2 | 3 | LICENCE AGREEMENT 4 | 5 | WE (Imperial College of Science, Technology and Medicine, (“Imperial College London”)) 6 | ARE WILLING TO LICENSE THIS SOFTWARE TO YOU (a licensee “You”) ONLY ON THE 7 | CONDITION THAT YOU ACCEPT ALL OF THE TERMS CONTAINED IN THE 8 | FOLLOWING AGREEMENT. PLEASE READ THE AGREEMENT CAREFULLY BEFORE 9 | DOWNLOADING THE SOFTWARE. BY EXERCISING THE OPTION TO DOWNLOAD 10 | THE SOFTWARE YOU AGREE TO BE BOUND BY THE TERMS OF THE AGREEMENT. 11 | SOFTWARE LICENCE AGREEMENT (EXCLUDING BSD COMPONENTS) 12 | 13 | 1.This Agreement pertains to a worldwide, non-exclusive, temporary, fully paid-up, royalty 14 | free, non-transferable, non-sub- licensable licence (the “Licence”) to use the elastic fusion 15 | source code, including any modification, part or derivative (the “Software”). 16 | Ownership and Licence. Your rights to use and download the Software onto your computer, 17 | and all other copies that You are authorised to make, are specified in this Agreement. 18 | However, we (or our licensors) retain all rights, including but not limited to all copyright and 19 | other intellectual property rights anywhere in the world, in the Software not expressly 20 | granted to You in this Agreement. 21 | 22 | 2. Permitted use of the Licence: 23 | 24 | (a) You may download and install the Software onto one computer or server for use in 25 | accordance with Clause 2(b) of this Agreement provided that You ensure that the Software is 26 | not accessible by other users unless they have themselves accepted the terms of this licence 27 | agreement. 28 | 29 | (b) You may use the Software solely for non-commercial, internal or academic research 30 | purposes and only in accordance with the terms of this Agreement. You may not use the 31 | Software for commercial purposes, including but not limited to (1) integration of all or part of 32 | the source code or the Software into a product for sale or licence by or on behalf of You to 33 | third parties or (2) use of the Software or any derivative of it for research to develop software 34 | products for sale or licence to a third party or (3) use of the Software or any derivative of it 35 | for research to develop non-software products for sale or licence to a third party, or (4) use of 36 | the Software to provide any service to an external organisation for which payment is 37 | received. 38 | 39 | Should You wish to use the Software for commercial purposes, You shall 40 | email researchcontracts.engineering@imperial.ac.uk . 41 | 42 | (c) Right to Copy. You may copy the Software for back-up and archival purposes, provided 43 | that each copy is kept in your possession and provided You reproduce our copyright notice 44 | (set out in Schedule 1) on each copy. 45 | 46 | (d) Transfer and sub-licensing. You may not rent, lend, or lease the Software and You may 47 | not transmit, transfer or sub-license this licence to use the Software or any of your rights or 48 | obligations under this Agreement to another party. 49 | 50 | (e) Identity of Licensee. The licence granted herein is personal to You. You shall not permit 51 | any third party to access, modify or otherwise use the Software nor shall You access modify 52 | or otherwise use the Software on behalf of any third party. If You wish to obtain a licence for 53 | mutiple users or a site licence for the Software please contact us 54 | at researchcontracts.engineering@imperial.ac.uk . 55 | 56 | (f) Publications and presentations. You may make public, results or data obtained from, 57 | dependent on or arising from research carried out using the Software, provided that any such 58 | presentation or publication identifies the Software as the source of the results or the data, 59 | including the Copyright Notice given in each element of the Software, and stating that the 60 | Software has been made available for use by You under licence from Imperial College London 61 | and You provide a copy of any such publication to Imperial College London. 62 | 63 | 3. Prohibited Uses. You may not, without written permission from us 64 | at researchcontracts.engineering@imperial.ac.uk : 65 | 66 | (a) Use, copy, modify, merge, or transfer copies of the Software or any documentation 67 | provided by us which relates to the Software except as provided in this Agreement; 68 | 69 | (b) Use any back-up or archival copies of the Software (or allow anyone else to use such 70 | copies) for any purpose other than to replace the original copy in the event it is destroyed or 71 | becomes defective; or 72 | 73 | (c) Disassemble, decompile or "unlock", reverse translate, or in any manner decode the 74 | Software for any reason. 75 | 76 | 4. Warranty Disclaimer 77 | 78 | (a) Disclaimer. The Software has been developed for research purposes only. You 79 | acknowledge that we are providing the Software to You under this licence agreement free of 80 | charge and on condition that the disclaimer set out below shall apply. We do not represent or 81 | warrant that the Software as to: (i) the quality, accuracy or reliability of the Software; (ii) the 82 | suitability of the Software for any particular use or for use under any specific conditions; and 83 | (iii) whether use of the Software will infringe third-party rights. 84 | You acknowledge that You have reviewed and evaluated the Software to determine that it 85 | meets your needs and that You assume all responsibility and liability for determining the 86 | suitability of the Software as fit for your particular purposes and requirements. Subject to 87 | Clause 4(b), we exclude and expressly disclaim all express and implied representations, 88 | warranties, conditions and terms not stated herein (including the implied conditions or 89 | warranties of satisfactory quality, merchantable quality, merchantability and fitness for 90 | purpose). 91 | 92 | (b) Savings. Some jurisdictions may imply warranties, conditions or terms or impose 93 | obligations upon us which cannot, in whole or in part, be excluded, restricted or modified or 94 | otherwise do not allow the exclusion of implied warranties, conditions or terms, in which 95 | case the above warranty disclaimer and exclusion will only apply to You to the extent 96 | permitted in the relevant jurisdiction and does not in any event exclude any implied 97 | warranties, conditions or terms which may not under applicable law be excluded. 98 | 99 | (c) Imperial College London disclaims all responsibility for the use which is made of the 100 | Software and any liability for the outcomes arising from using the Software. 101 | 102 | 5. Limitation of Liability 103 | 104 | (a) You acknowledge that we are providing the Software to You under this licence agreement 105 | free of charge and on condition that the limitation of liability set out below shall apply. 106 | Accordingly, subject to Clause 5(b), we exclude all liability whether in contract, tort, 107 | negligence or otherwise, in respect of the Software and/or any related documentation 108 | provided to You by us including, but not limited to, liability for loss or corruption of data, 109 | loss of contracts, loss of income, loss of profits, loss of cover and any consequential or indirect 110 | loss or damage of any kind arising out of or in connection with this licence agreement, 111 | however caused. This exclusion shall apply even if we have been advised of the possibility of 112 | such loss or damage. 113 | 114 | (b) You agree to indemnify Imperial College London and hold it harmless from and against 115 | any and all claims, damages and liabilities asserted by third parties (including claims for 116 | negligence) which arise directly or indirectly from the use of the Software or any derivative 117 | of it or the sale of any products based on the Software. You undertake to make no liability 118 | claim against any employee, student, agent or appointee of Imperial College London, in 119 | connection with this Licence or the Software. 120 | 121 | (c) Nothing in this Agreement shall have the effect of excluding or limiting our statutory 122 | liability. 123 | 124 | (d) Some jurisdictions do not allow these limitations or exclusions either wholly or in part, 125 | and, to that extent, they may not apply to you. Nothing in this licence agreement will affect 126 | your statutory rights or other relevant statutory provisions which cannot be excluded, 127 | restricted or modified, and its terms and conditions must be read and construed subject to any 128 | such statutory rights and/or provisions. 129 | 130 | 6. Confidentiality. You agree not to disclose any confidential information provided to You by 131 | us pursuant to this Agreement to any third party without our prior written consent. The 132 | obligations in this Clause 6 shall survive the termination of this Agreement for any reason. 133 | 134 | 7. Termination. 135 | 136 | (a) We may terminate this licence agreement and your right to use the Software at any time 137 | with immediate effect upon written notice to You. 138 | 139 | (b) This licence agreement and your right to use the Software automatically terminate if You: 140 | (i) fail to comply with any provisions of this Agreement; or 141 | (ii) destroy the copies of the Software in your possession, or voluntarily return the Software 142 | to us. 143 | 144 | (c) Upon termination You will destroy all copies of the Software. 145 | 146 | (d) Otherwise, the restrictions on your rights to use the Software will expire 10 (ten) years 147 | after first use of the Software under this licence agreement. 148 | 149 | 8. Miscellaneous Provisions. 150 | 151 | (a) This Agreement will be governed by and construed in accordance with the substantive 152 | laws of England and Wales whose courts shall have exclusive jurisdiction over all disputes 153 | which may arise between us. 154 | 155 | (b) This is the entire agreement between us relating to the Software, and supersedes any prior 156 | purchase order, communications, advertising or representations concerning the Software. 157 | 158 | (c) No change or modification of this Agreement will be valid unless it is in writing, and is 159 | signed by us. 160 | 161 | (d) The unenforceability or invalidity of any part of this Agreement will not affect the 162 | enforceability or validity of the remaining parts. 163 | 164 | BSD Elements of the Software 165 | 166 | For BSD elements of the Software, the following terms shall apply: 167 | 168 | Copyright as indicated in the header of the individual element of the Software. 169 | 170 | All rights reserved. 171 | 172 | Redistribution and use in source and binary forms, with or without modification, are 173 | permitted provided that the following conditions are met: 174 | 175 | 1. Redistributions of source code must retain the above copyright notice, this list of 176 | conditions and the following disclaimer. 177 | 178 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of 179 | conditions and the following disclaimer in the documentation and/or other materials 180 | provided with the distribution. 181 | 182 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to 183 | endorse or promote products derived from this software without specific prior written 184 | permission. 185 | 186 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 187 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 188 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 189 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 190 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 191 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 192 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 193 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 194 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 195 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 196 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 197 | 198 | SCHEDULE 1 199 | 200 | The Software 201 | 202 | SemanticFusion is a real-time visual SLAM system capable of semantically annotating a dense 203 | 3D scene using Convolutional Neural Networks. It is based on the techniques described in the 204 | following publication: 205 | 206 | John McCormac, Ankur Handa, Andrew J Davison, Stefan Leutenegger. SemanticFusion: 207 | Dense 3D Semantic Mapping with Convolutional Neural Networks. IEEE International 208 | Conference on Robotics and Automation (ICRA), 2017 209 | 210 | Acknowledgments 211 | 212 | If you use the software, you should reference the following paper in any publication: 213 | 214 | John McCormac, Ankur Handa, Andrew J Davison, Stefan Leutenegger. SemanticFusion: 215 | Dense 3D Semantic Mapping with Convolutional Neural Networks. IEEE International 216 | Conference on Robotics and Automation (ICRA), 2017 217 | -------------------------------------------------------------------------------- /src/semantic_fusion/CRF/permutohedral.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2011, Philipp Krähenbühl 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the Stanford University nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY Philipp Krähenbühl ''AS IS'' AND ANY 17 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL Philipp Krähenbühl BE LIABLE FOR ANY 20 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #pragma once 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | /************************************************/ 37 | /*** Hash Table ***/ 38 | /************************************************/ 39 | 40 | class HashTable{ 41 | // Don't copy! 42 | HashTable( const HashTable & o ): key_size_ ( o.key_size_ ), filled_(0), capacity_(o.capacity_) { 43 | table_ = new int[ capacity_ ]; 44 | keys_ = new short[ (capacity_/2+10)*key_size_ ]; 45 | memset( table_, -1, capacity_*sizeof(int) ); 46 | } 47 | protected: 48 | size_t key_size_, filled_, capacity_; 49 | short * keys_; 50 | int * table_; 51 | void grow(){ 52 | // Swap out the old memory 53 | short * old_keys = keys_; 54 | int * old_table = table_; 55 | int old_capacity = capacity_; 56 | capacity_ *= 2; 57 | // Allocate the new memory 58 | keys_ = new short[ (old_capacity+10)*key_size_ ]; 59 | table_ = new int[ capacity_ ]; 60 | memset( table_, -1, capacity_*sizeof(int) ); 61 | memcpy( keys_, old_keys, filled_*key_size_*sizeof(short) ); 62 | 63 | // Reinsert each element 64 | for( int i=0; i= 0){ 66 | int e = old_table[i]; 67 | size_t h = hash( old_keys+(getKey(e)-keys_) ) % capacity_; 68 | for(; table_[h] >= 0; h = h= capacity_) grow(); 102 | // Get the hash value 103 | size_t h = hash( k ) % capacity_; 104 | // Find the element with he right key, using linear probing 105 | while(1){ 106 | int e = table_[h]; 107 | if (e==-1){ 108 | if (create){ 109 | // Insert a new key and return the new id 110 | for( size_t i=0; i0; j-- ){ 206 | float cf = f[j-1]*scale_factor[j-1]; 207 | elevated[j] = sm - j*cf; 208 | sm += cf; 209 | } 210 | elevated[0] = sm; 211 | 212 | // Find the closest 0-colored simplex through rounding 213 | float down_factor = 1.0f / (d_+1); 214 | float up_factor = (d_+1); 215 | int sum = 0; 216 | for( int i=0; i<=d_; i++ ){ 217 | int rd = (int)round( down_factor * elevated[i]); 218 | rem0[i] = rd*up_factor; 219 | sum += rd; 220 | } 221 | 222 | // Find the simplex we are in and store it in rank (where rank describes what position coorinate i has in the sorted order of the features values) 223 | for( int i=0; i<=d_; i++ ) 224 | rank[i] = 0; 225 | for( int i=0; i d_ ){ 242 | rank[i] -= d_+1; 243 | rem0[i] -= d_+1; 244 | } 245 | } 246 | 247 | // Compute the barycentric coordinates (p.10 in [Adams etal 2010]) 248 | for( int i=0; i<=d_+1; i++ ) 249 | barycentric[i] = 0; 250 | for( int i=0; i<=d_; i++ ){ 251 | float v = (elevated[i] - rem0[i])*down_factor; 252 | barycentric[d_-rank[i] ] += v; 253 | barycentric[d_-rank[i]+1] -= v; 254 | } 255 | // Wrap around 256 | barycentric[0] += 1.0f + barycentric[d_+1]; 257 | 258 | // Compute all vertices and their offset 259 | for( int remainder=0; remainder<=d_; remainder++ ){ 260 | for( int i=0; i 0 (used for blurring) 312 | float * values = new float[ (M_+2)*value_size ]; 313 | float * new_values = new float[ (M_+2)*value_size ]; 314 | 315 | for( int i=0; i<(M_+2)*value_size; i++ ) 316 | values[i] = new_values[i] = 0; 317 | 318 | // Splatting 319 | for( int i=0; i