├── .clang-format ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── CMakeModules ├── CudaComputeTargetFlags.cmake ├── CudaDetect.cmake ├── FindBLAS.cmake ├── FindLAPACK.cmake ├── FindOpenNI2.cmake ├── FindRealSense.cmake └── FindSuiteSparse.cmake ├── Core ├── CMakeLists.txt ├── Cuda │ ├── containers │ │ ├── device_array.hpp │ │ ├── device_array_impl.hpp │ │ ├── device_memory.cpp │ │ ├── device_memory.hpp │ │ ├── device_memory_impl.hpp │ │ └── kernel_containers.hpp │ ├── convenience.cuh │ ├── cudafuncs.cu │ ├── cudafuncs.cuh │ ├── operators.cuh │ ├── reduce.cu │ └── types.cuh ├── Deformation.cpp ├── Deformation.h ├── ElasticFusion.cpp ├── ElasticFusion.h ├── Ferns.cpp ├── Ferns.h ├── GPUTexture.cpp ├── GPUTexture.h ├── GlobalModel.cpp ├── GlobalModel.h ├── IndexMap.cpp ├── IndexMap.h ├── PoseMatch.h ├── Shaders │ ├── ComputePack.cpp │ ├── ComputePack.h │ ├── FeedbackBuffer.cpp │ ├── FeedbackBuffer.h │ ├── FillIn.cpp │ ├── FillIn.h │ ├── Resize.cpp │ ├── Resize.h │ ├── Shaders.h │ ├── Uniform.h │ ├── Vertex.cpp │ ├── Vertex.h │ ├── color.glsl │ ├── combo_splat.frag │ ├── copy_unstable.geom │ ├── copy_unstable.vert │ ├── data.frag │ ├── data.geom │ ├── data.vert │ ├── depth_bilateral.frag │ ├── depth_metric.frag │ ├── depth_norm.frag │ ├── depth_splat.frag │ ├── draw_feedback.frag │ ├── draw_feedback.vert │ ├── draw_global_surface.frag │ ├── draw_global_surface.geom │ ├── draw_global_surface.vert │ ├── draw_global_surface_phong.frag │ ├── empty.vert │ ├── fill_normal.frag │ ├── fill_rgb.frag │ ├── fill_vertex.frag │ ├── fxaa.frag │ ├── geometry.glsl │ ├── index_map.frag │ ├── index_map.vert │ ├── init_unstable.vert │ ├── quad.geom │ ├── resize.frag │ ├── sample.geom │ ├── sample.vert │ ├── splat.vert │ ├── surfels.glsl │ ├── update.vert │ ├── vertex_feedback.geom │ ├── vertex_feedback.vert │ └── visualise_textures.frag └── Utils │ ├── CholeskyDecomp.cpp │ ├── CholeskyDecomp.h │ ├── DeformationGraph.cpp │ ├── DeformationGraph.h │ ├── GraphNode.h │ ├── Img.h │ ├── Intrinsics.cpp │ ├── Intrinsics.h │ ├── Jacobian.h │ ├── OdometryProvider.h │ ├── OrderedJacobianRow.h │ ├── Parse.cpp │ ├── Parse.h │ ├── RGBDOdometry.cpp │ ├── RGBDOdometry.h │ ├── Resolution.cpp │ ├── Resolution.h │ └── Stopwatch.h ├── LICENSE.txt ├── Main.cpp ├── MainController.cpp ├── MainController.h ├── README.md └── Tools ├── CameraInterface.h ├── GUI.h ├── GroundTruthOdometry.cpp ├── GroundTruthOdometry.h ├── JPEGLoader.h ├── LiveLogReader.cpp ├── LiveLogReader.h ├── LogReader.h ├── OpenNI2Interface.cpp ├── OpenNI2Interface.h ├── RawLogReader.cpp ├── RawLogReader.h ├── RealSenseInterface.cpp ├── RealSenseInterface.h └── ThreadMutexObject.h /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | AccessModifierOffset: -1 3 | AlignAfterOpenBracket: AlwaysBreak 4 | AlignConsecutiveAssignments: false 5 | AlignConsecutiveDeclarations: false 6 | AlignEscapedNewlinesLeft: true 7 | AlignOperands: false 8 | AlignTrailingComments: false 9 | AllowAllParametersOfDeclarationOnNextLine: false 10 | AllowShortBlocksOnASingleLine: false 11 | AllowShortCaseLabelsOnASingleLine: false 12 | AllowShortFunctionsOnASingleLine: Empty 13 | AllowShortIfStatementsOnASingleLine: false 14 | AllowShortLoopsOnASingleLine: false 15 | AlwaysBreakAfterReturnType: None 16 | AlwaysBreakBeforeMultilineStrings: true 17 | AlwaysBreakTemplateDeclarations: true 18 | BinPackArguments: false 19 | BinPackParameters: false 20 | BraceWrapping: 21 | AfterClass: false 22 | AfterControlStatement: false 23 | AfterEnum: false 24 | AfterFunction: false 25 | AfterNamespace: false 26 | AfterObjCDeclaration: false 27 | AfterStruct: false 28 | AfterUnion: false 29 | BeforeCatch: false 30 | BeforeElse: false 31 | IndentBraces: false 32 | BreakBeforeBinaryOperators: None 33 | BreakBeforeBraces: Attach 34 | BreakBeforeTernaryOperators: true 35 | BreakConstructorInitializersBeforeComma: false 36 | BreakAfterJavaFieldAnnotations: false 37 | BreakStringLiterals: false 38 | ColumnLimit: 100 39 | CommentPragmas: '^ IWYU pragma:' 40 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 41 | ConstructorInitializerIndentWidth: 4 42 | ContinuationIndentWidth: 4 43 | Cpp11BracedListStyle: true 44 | DerivePointerAlignment: false 45 | DisableFormat: false 46 | ForEachMacros: [ FOR_EACH_RANGE, FOR_EACH, ] 47 | IncludeCategories: 48 | - Regex: '^<.*\.h(pp)?>' 49 | Priority: 1 50 | - Regex: '^<.*' 51 | Priority: 2 52 | - Regex: '.*' 53 | Priority: 3 54 | IndentCaseLabels: true 55 | IndentWidth: 2 56 | IndentWrappedFunctionNames: false 57 | KeepEmptyLinesAtTheStartOfBlocks: false 58 | MacroBlockBegin: '' 59 | MacroBlockEnd: '' 60 | MaxEmptyLinesToKeep: 1 61 | NamespaceIndentation: None 62 | ObjCBlockIndentWidth: 2 63 | ObjCSpaceAfterProperty: false 64 | ObjCSpaceBeforeProtocolList: false 65 | PenaltyBreakBeforeFirstCallParameter: 1 66 | PenaltyBreakComment: 300 67 | PenaltyBreakFirstLessLess: 120 68 | PenaltyBreakString: 1000 69 | PenaltyExcessCharacter: 1000000 70 | PenaltyReturnTypeOnItsOwnLine: 200 71 | PointerAlignment: Left 72 | ReflowComments: true 73 | SortIncludes: true 74 | SpaceAfterCStyleCast: false 75 | SpaceBeforeAssignmentOperators: true 76 | SpaceBeforeParens: ControlStatements 77 | SpaceInEmptyParentheses: false 78 | SpacesBeforeTrailingComments: 1 79 | SpacesInAngles: false 80 | SpacesInContainerLiterals: true 81 | SpacesInCStyleCastParentheses: false 82 | SpacesInParentheses: false 83 | SpacesInSquareBrackets: false 84 | Standard: Cpp11 85 | TabWidth: 4 86 | UseTab: Never 87 | ... 88 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | deps/ 3 | .project 4 | .cproject 5 | *.user 6 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "third-party/Pangolin"] 2 | path = third-party/Pangolin 3 | url = https://github.com/stevenlovegrove/Pangolin.git 4 | [submodule "third-party/OpenNI2"] 5 | path = third-party/OpenNI2 6 | url = https://github.com/occipital/OpenNI2.git 7 | [submodule "third-party/Eigen"] 8 | path = third-party/Eigen 9 | url = https://github.com/eigenteam/eigen-git-mirror.git 10 | [submodule "third-party/Sophus"] 11 | path = third-party/Sophus 12 | url = https://github.com/strasdat/Sophus.git 13 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | 3 | project(ElasticFusion) 4 | 5 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") 6 | set(CMAKE_CXX_STANDARD 17) 7 | 8 | set(Pangolin_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third-party/Pangolin/build/src" CACHE PATH "Pangolin build directory") 9 | set(OPENNI2_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third-party/OpenNI2/Include" CACHE PATH "OpenNI2 Include directory") 10 | set(OPENNI2_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/third-party/OpenNI2/Bin/x64-Release/libOpenNI2.so" CACHE PATH "OpenNI2 library") 11 | set(Sophus_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third-party/Sophus" CACHE PATH "Sophus Include directory") 12 | set(efusion_SHADER_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Core/Shaders" CACHE PATH "Where the shaders live") 13 | 14 | find_package(LAPACK REQUIRED) 15 | find_package(BLAS REQUIRED) 16 | find_package(ZLIB REQUIRED) 17 | find_package(JPEG REQUIRED) 18 | find_package(Pangolin 0.1 REQUIRED) 19 | find_package(CUDA REQUIRED) 20 | find_package(OpenNI2 REQUIRED) 21 | find_package(SuiteSparse REQUIRED) 22 | 23 | include_directories(${JPEG_INCLUDE_DIR}) 24 | include_directories(${ZLIB_INCLUDE_DIR}) 25 | include_directories(${EIGEN_INCLUDE_DIRS}) 26 | include_directories(${Pangolin_INCLUDE_DIRS}) 27 | include_directories(${CUDA_INCLUDE_DIRS}) 28 | include_directories(${OPENNI2_INCLUDE_DIR}) 29 | include_directories(${SUITESPARSE_INCLUDE_DIRS}) 30 | include_directories(${Sophus_INCLUDE_DIR}) 31 | 32 | if(WITH_REALSENSE) 33 | include_directories(${REALSENSE_INCLUDE_DIR}) 34 | add_definitions(-DWITH_REALSENSE) 35 | set(EXTRA_LIBS ${EXTRA_LIBS} ${REALSENSE_LIBRARY}) 36 | endif() 37 | 38 | file(GLOB srcs *.cpp *.h *.cu *.cuh) 39 | file(GLOB tools_srcs Tools/*.cpp Tools/*.h Tools/*.cu Tools/*.cuh) 40 | 41 | add_definitions(-Dlinux=1) 42 | add_definitions(-DEIGEN_MAX_ALIGN_BYTES=0) 43 | add_definitions(-DEIGEN_MAX_STATIC_ALIGN_BYTES=0) 44 | 45 | set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} "-O3 -msse2 -msse3 -Wall -DSHADER_DIR=${efusion_SHADER_DIR}") 46 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -Wall -DSHADER_DIR=${efusion_SHADER_DIR}") 47 | 48 | add_subdirectory(Core) 49 | 50 | add_executable(ElasticFusion 51 | ${srcs} 52 | ${tools_srcs} 53 | ) 54 | 55 | target_link_libraries(ElasticFusion 56 | efusion 57 | ${ZLIB_LIBRARY} 58 | ${JPEG_LIBRARY} 59 | ${Pangolin_LIBRARIES} 60 | ${CUDA_LIBRARIES} 61 | ${EXTRA_LIBS} 62 | ${OPENNI2_LIBRARY} 63 | ${SUITESPARSE_LIBRARIES} 64 | ${BLAS_LIBRARIES} 65 | ${LAPACK_LIBRARIES} 66 | ) 67 | -------------------------------------------------------------------------------- /CMakeModules/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() -------------------------------------------------------------------------------- /CMakeModules/CudaDetect.cmake: -------------------------------------------------------------------------------- 1 | # Taken from https://github.com/BVLC/caffe/blob/master/cmake/Cuda.cmake 2 | ################################################################################################ 3 | # A function for automatic detection of GPUs installed (if autodetection is enabled) 4 | # Usage: 5 | # detect_installed_gpus(out_variable) 6 | function(detect_installed_gpus out_variable) 7 | if(NOT CUDA_gpu_detect_output) 8 | set(__cufile ${PROJECT_BINARY_DIR}/detect_cuda_archs.cu) 9 | 10 | file(WRITE ${__cufile} "" 11 | "#include \n" 12 | "int main()\n" 13 | "{\n" 14 | " int count = 0;\n" 15 | " if (cudaSuccess != cudaGetDeviceCount(&count)) return -1;\n" 16 | " if (count == 0) return -1;\n" 17 | " for (int device = 0; device < count; ++device)\n" 18 | " {\n" 19 | " cudaDeviceProp prop;\n" 20 | " if (cudaSuccess == cudaGetDeviceProperties(&prop, device))\n" 21 | " std::printf(\"%d.%d;\", prop.major, prop.minor);\n" 22 | " }\n" 23 | " return 0;\n" 24 | "}\n") 25 | 26 | execute_process(COMMAND "${CUDA_NVCC_EXECUTABLE}" "-Wno-deprecated-gpu-targets" "--run" "${__cufile}" 27 | WORKING_DIRECTORY "${PROJECT_BINARY_DIR}/CMakeFiles/" 28 | RESULT_VARIABLE __nvcc_res OUTPUT_VARIABLE __nvcc_out 29 | ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE) 30 | 31 | if(__nvcc_res EQUAL 0) 32 | string(REGEX REPLACE "\\." "" __nvcc_out "${__nvcc_out}") 33 | string(REGEX MATCHALL "[0-9;]+" __nvcc_out "${__nvcc_out}") 34 | list(REMOVE_DUPLICATES __nvcc_out) 35 | set(CUDA_gpu_detect_output ${__nvcc_out} CACHE INTERNAL "Returned GPU architectures from detect_gpus tool" FORCE) 36 | endif() 37 | endif() 38 | 39 | if(NOT CUDA_gpu_detect_output) 40 | message(STATUS "Automatic GPU detection failed. Is CUDA properly installed? .") 41 | else() 42 | set(${out_variable} ${CUDA_gpu_detect_output} PARENT_SCOPE) 43 | endif() 44 | endfunction() 45 | -------------------------------------------------------------------------------- /CMakeModules/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}/../third-party/OpenNI2/Include" 26 | "${CMAKE_SOURCE_DIR}/../../third-party/OpenNI2/Include" 27 | "${CMAKE_SOURCE_DIR}/../OpenNI2/Include" 28 | "${CMAKE_SOURCE_DIR}/../../OpenNI2/Include" 29 | "${CMAKE_SOURCE_DIR}/../../../OpenNI2/Include" 30 | "${CMAKE_SOURCE_DIR}/../../../../OpenNI2/Include" 31 | "${CMAKE_SOURCE_DIR}/../../../code/OpenNI2/Include" 32 | "${CMAKE_SOURCE_DIR}/../../../../code/OpenNI2/Include" 33 | "${CMAKE_SOURCE_DIR}/../deps/OpenNI2/Include" 34 | "${CMAKE_SOURCE_DIR}/../../deps/OpenNI2/Include" 35 | "${CMAKE_SOURCE_DIR}/../../../deps/OpenNI2/Include" 36 | "${CMAKE_SOURCE_DIR}/../../../../deps/OpenNI2/Include" 37 | /usr/include 38 | /user/include 39 | PATH_SUFFIXES openni2 ni2 40 | ) 41 | 42 | if(${CMAKE_CL_64}) 43 | set(OPENNI_PATH_SUFFIXES lib64) 44 | else() 45 | set(OPENNI_PATH_SUFFIXES lib) 46 | endif() 47 | 48 | #add a hint so that it can find it without the pkg-config 49 | find_library(OPENNI2_LIBRARY 50 | NAMES OpenNI2 51 | HINTS 52 | ${PC_OPENNI_LIBDIR} 53 | ${PC_OPENNI_LIBRARY_DIRS} 54 | PATHS 55 | "${PROGRAM_FILES}}/OpenNI2/Redist" 56 | "${PROGRAM_FILES}/OpenNI2" 57 | "${CMAKE_SOURCE_DIR}/../OpenNI2/Bin/x64-Release" 58 | "${CMAKE_SOURCE_DIR}/../../OpenNI2/Bin/x64-Release" 59 | "${CMAKE_SOURCE_DIR}/../../../OpenNI2/Bin/x64-Release" 60 | "${CMAKE_SOURCE_DIR}/../../../../OpenNI2/Bin/x64-Release" 61 | "${CMAKE_SOURCE_DIR}/../../../code/OpenNI2/Bin/x64-Release" 62 | "${CMAKE_SOURCE_DIR}/../../../../code/OpenNI2/Bin/x64-Release" 63 | "${CMAKE_SOURCE_DIR}/../deps/OpenNI2/Bin/x64-Release" 64 | "${CMAKE_SOURCE_DIR}/../../deps/OpenNI2/Bin/x64-Release" 65 | "${CMAKE_SOURCE_DIR}/../../../deps/OpenNI2/Bin/x64-Release" 66 | "${CMAKE_SOURCE_DIR}/../../../../deps/OpenNI2/Bin/x64-Release" 67 | /usr/lib 68 | /user/lib 69 | PATH_SUFFIXES ${OPENNI_PATH_SUFFIXES} 70 | ) 71 | 72 | set(OPENNI2_INCLUDE_DIRS ${OPENNI2_INCLUDE_DIR}) 73 | set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY}) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(OpenNI2 DEFAULT_MSG 77 | OPENNI2_LIBRARY OPENNI2_INCLUDE_DIR) 78 | 79 | mark_as_advanced(OPENNI2_LIBRARY OPENNI2_INCLUDE_DIR) 80 | -------------------------------------------------------------------------------- /CMakeModules/FindRealSense.cmake: -------------------------------------------------------------------------------- 1 | set(REALSENSE_ROOT "/usr/local" CACHE PATH "Root directory of libREALSENSE") 2 | 3 | FIND_PATH(REALSENSE_INCLUDE_DIR libREALSENSE HINTS "${REALSENSE_ROOT}/include") 4 | FIND_LIBRARY(REALSENSE_LIBRARY REALSENSE HINTS "${REALSENSE_ROOT}/bin/x64" "${REALSENSE_ROOT}/lib") 5 | 6 | find_package_handle_standard_args(REALSENSE DEFAULT_MSG REALSENSE_LIBRARY REALSENSE_INCLUDE_DIR) 7 | 8 | mark_as_advanced(REALSENSE_LIBRARY REALSENSE_INCLUDE_DIR) 9 | 10 | -------------------------------------------------------------------------------- /CMakeModules/FindSuiteSparse.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find SUITESPARSE 2 | # Once done this will define 3 | # 4 | # SUITESPARSE_FOUND - system has SUITESPARSE 5 | # SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory 6 | # SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE 7 | # SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package) 8 | # SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package) 9 | # SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs 10 | # SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs 11 | # SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly 12 | 13 | IF (SUITESPARSE_INCLUDE_DIRS) 14 | # Already in cache, be silent 15 | SET(SUITESPARSE_FIND_QUIETLY TRUE) 16 | ENDIF (SUITESPARSE_INCLUDE_DIRS) 17 | 18 | if (WIN32) 19 | # the libraries may have lib prefix 20 | set(ORIGINAL_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") 21 | set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") 22 | endif () 23 | 24 | FIND_PATH( SUITESPARSE_INCLUDE_DIR cholmod.h 25 | PATHS /usr/local/include 26 | /usr/include 27 | /usr/include/suitesparse/ 28 | ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod 29 | PATH_SUFFIXES cholmod/ CHOLMOD/ ) 30 | 31 | FIND_PATH( SUITESPARSE_LIBRARY_DIR 32 | NAMES libcholmod.so libcholmod.a 33 | PATHS /usr/lib 34 | /usr/lib64 35 | /usr/lib/x86_64-linux-gnu 36 | /usr/lib/i386-linux-gnu 37 | /usr/local/lib ) 38 | 39 | # Add cholmod include directory to collection include directories 40 | IF ( SUITESPARSE_INCLUDE_DIR ) 41 | list ( APPEND SUITESPARSE_INCLUDE_DIRS ${SUITESPARSE_INCLUDE_DIR} ) 42 | ENDIF( SUITESPARSE_INCLUDE_DIR ) 43 | 44 | # if we found the library, add it to the defined libraries 45 | IF ( SUITESPARSE_LIBRARY_DIR ) 46 | FIND_LIBRARY( SUITESPARSE_AMD_LIBRARY 47 | NAMES amd 48 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 49 | FIND_LIBRARY( SUITESPARSE_CAMD_LIBRARY 50 | NAMES camd 51 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 52 | FIND_LIBRARY( SUITESPARSE_CCOLAMD_LIBRARY 53 | NAMES ccolamd 54 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 55 | FIND_LIBRARY( SUITESPARSE_CHOLMOD_LIBRARY 56 | NAMES cholmod 57 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 58 | FIND_LIBRARY( SUITESPARSE_COLAMD_LIBRARY 59 | NAMES colamd 60 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 61 | FIND_LIBRARY( SUITESPARSE_CXSPARSE_LIBRARY 62 | NAMES cxsparse 63 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 64 | IF ( WIN32 ) 65 | FIND_LIBRARY( SUITESPARSE_SUITESPARSECONFIG_LIBRARY 66 | NAMES suitesparseconfig 67 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 68 | FIND_LIBRARY( SUITESPARSE_BLAS_LIBRARY 69 | NAMES blas 70 | PATHS ${SUITESPARSE_LIBRARY_DIR}/lapack_blas_windows ) 71 | FIND_LIBRARY( SUITESPARSE_LAPACK_LIBRARY 72 | NAMES lapack 73 | PATHS ${SUITESPARSE_LIBRARY_DIR}/lapack_blas_windows ) 74 | ENDIF () 75 | 76 | list ( APPEND SUITESPARSE_LIBRARIES ${SUITESPARSE_AMD_LIBRARY} 77 | ${SUITESPARSE_CAMD_LIBRARY} 78 | ${SUITESPARSE_CCOLAMD_LIBRARY} 79 | ${SUITESPARSE_CHOLMOD_LIBRARY} 80 | ${SUITESPARSE_COLAMD_LIBRARY} 81 | ${SUITESPARSE_CXSPARSE_LIBRARY} 82 | ${SUITESPARSE_SUITESPARSECONFIG_LIBRARY} 83 | ${SUITESPARSE_BLAS_LIBRARY} 84 | ${SUITESPARSE_LAPACK_LIBRARY}) 85 | 86 | # Metis and spqr are optional 87 | FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY 88 | NAMES metis 89 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 90 | IF (SUITESPARSE_METIS_LIBRARY) 91 | list ( APPEND SUITESPARSE_LIBRARIES ${SUITESPARSE_METIS_LIBRARY}) 92 | ENDIF(SUITESPARSE_METIS_LIBRARY) 93 | 94 | if(EXISTS "${SUITESPARSE_INCLUDE_DIR}/SuiteSparseQR.hpp") 95 | SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid") 96 | else() 97 | SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid") 98 | endif() 99 | 100 | if(SUITESPARSE_SPQR_VALID) 101 | FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY 102 | NAMES spqr 103 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 104 | IF (SUITESPARSE_SPQR_LIBRARY) 105 | list ( APPEND SUITESPARSE_LIBRARIES ${SUITESPARSE_SPQR_LIBRARY}) 106 | ENDIF (SUITESPARSE_SPQR_LIBRARY) 107 | endif() 108 | ENDIF( SUITESPARSE_LIBRARY_DIR ) 109 | 110 | IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 111 | SET(SUITESPARSE_FOUND TRUE) 112 | MESSAGE(STATUS "Found SuiteSparse") 113 | ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 114 | SET( SUITESPARSE_FOUND FALSE ) 115 | MESSAGE(FATAL_ERROR "Unable to find SuiteSparse") 116 | ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) 117 | 118 | if (WIN32) 119 | set(CMAKE_FIND_LIBRARY_PREFIXES "${ORIGINAL_CMAKE_FIND_LIBRARY_PREFIXES}") 120 | endif () 121 | -------------------------------------------------------------------------------- /Core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | 3 | project(libefusion) 4 | 5 | include_directories(${Pangolin_INCLUDE_DIRS}) 6 | include_directories(${CUDA_INCLUDE_DIRS}) 7 | include_directories(${EIGEN_INCLUDE_DIRS}) 8 | include_directories(${SUITESPARSE_INCLUDE_DIRS}) 9 | 10 | file(GLOB srcs *.cpp *.h *.cu *.cuh) 11 | file(GLOB utils_srcs Utils/*.cpp Utils/*.h Utils/*.cu Utils/*.cuh) 12 | file(GLOB shader_srcs Shaders/*.cpp Shaders/*.h Shaders/*.cu Shaders/*.cuh) 13 | file(GLOB cuda Cuda/*.cpp Cuda/*.h Cuda/*.cu Cuda/*.cuh) 14 | file(GLOB containers Cuda/containers/*.cpp Cuda/containers/*.h Cuda/containers/*.cu Cuda/containers/*.cuh) 15 | 16 | set(CUDA_ARCH_BIN "" 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)") 17 | set(CUDA_ARCH_PTX "" CACHE STRING "Specify 'virtual' PTX arch to build PTX intermediate code for. Example: 1.0 1.2 or 10 12") 18 | 19 | include("${CMAKE_MODULE_PATH}/CudaDetect.cmake") 20 | detect_installed_gpus(CUDA_NVCC_ARCHS) 21 | foreach(NVCC_ARCH IN LISTS CUDA_NVCC_ARCHS) 22 | list(APPEND CUDA_ARCH_BIN "${NVCC_ARCH} ") 23 | endforeach(NVCC_ARCH) 24 | 25 | include("${CMAKE_MODULE_PATH}/CudaComputeTargetFlags.cmake") 26 | APPEND_TARGET_ARCH_FLAGS() 27 | 28 | set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-Xcompiler;-fPIC;--expt-relaxed-constexpr;--disable-warnings") 29 | 30 | CUDA_COMPILE(cuda_objs ${cuda}) 31 | 32 | add_library(efusion SHARED 33 | ${srcs} 34 | ${utils_srcs} 35 | ${shader_srcs} 36 | ${cuda} 37 | ${cuda_objs} 38 | ${containers} 39 | ) 40 | 41 | target_link_libraries(efusion 42 | ${Eigen_LIBRARIES} 43 | ${Pangolin_LIBRARIES} 44 | ${CUDA_LIBRARIES} 45 | ${SUITESPARSE_LIBRARIES} 46 | ) 47 | -------------------------------------------------------------------------------- /Core/Cuda/containers/device_memory_impl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 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 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com) 35 | */ 36 | 37 | #ifndef DEVICE_MEMORY_IMPL_HPP_ 38 | #define DEVICE_MEMORY_IMPL_HPP_ 39 | 40 | ///////////////////// Inline implementations of DeviceMemory //////////////////////////////////////////// 41 | 42 | template inline T* DeviceMemory::ptr() { return ( T*)data_; } 43 | template inline const T* DeviceMemory::ptr() const { return (const T*)data_; } 44 | 45 | template inline DeviceMemory::operator PtrSz() const 46 | { 47 | PtrSz result; 48 | result.data = (U*)ptr(); 49 | result.size = sizeBytes_/sizeof(U); 50 | return result; 51 | } 52 | 53 | ///////////////////// Inline implementations of DeviceMemory2D //////////////////////////////////////////// 54 | 55 | template T* DeviceMemory2D::ptr(int y_arg) { return ( T*)(( char*)data_ + y_arg * step_); } 56 | template const T* DeviceMemory2D::ptr(int y_arg) const { return (const T*)((const char*)data_ + y_arg * step_); } 57 | 58 | template DeviceMemory2D::operator PtrStep() const 59 | { 60 | PtrStep result; 61 | result.data = (U*)ptr(); 62 | result.step = step_; 63 | return result; 64 | } 65 | 66 | template DeviceMemory2D::operator PtrStepSz() const 67 | { 68 | PtrStepSz result; 69 | result.data = (U*)ptr(); 70 | result.step = step_; 71 | result.cols = colsBytes_/sizeof(U); 72 | result.rows = rows_; 73 | return result; 74 | } 75 | 76 | #endif /* DEVICE_MEMORY_IMPL_HPP_ */ 77 | 78 | -------------------------------------------------------------------------------- /Core/Cuda/containers/kernel_containers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011, Willow Garage, Inc. 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 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com) 35 | */ 36 | 37 | 38 | #ifndef KERNEL_CONTAINERS_HPP_ 39 | #define KERNEL_CONTAINERS_HPP_ 40 | 41 | #include 42 | 43 | #if defined(__CUDACC__) 44 | #define GPU_HOST_DEVICE__ __host__ __device__ __forceinline__ 45 | #else 46 | #define GPU_HOST_DEVICE__ 47 | #endif 48 | 49 | template struct DevPtr 50 | { 51 | typedef T elem_type; 52 | const static size_t elem_size = sizeof(elem_type); 53 | 54 | T* data; 55 | 56 | GPU_HOST_DEVICE__ DevPtr() : data(0) {} 57 | GPU_HOST_DEVICE__ DevPtr(T* data_arg) : data(data_arg) {} 58 | 59 | GPU_HOST_DEVICE__ size_t elemSize() const { return elem_size; } 60 | GPU_HOST_DEVICE__ operator T*() { return data; } 61 | GPU_HOST_DEVICE__ operator const T*() const { return data; } 62 | }; 63 | 64 | template struct PtrSz : public DevPtr 65 | { 66 | GPU_HOST_DEVICE__ PtrSz() : size(0) {} 67 | GPU_HOST_DEVICE__ PtrSz(T* data_arg, size_t size_arg) : DevPtr(data_arg), size(size_arg) {} 68 | 69 | size_t size; 70 | }; 71 | 72 | template struct PtrStep : public DevPtr 73 | { 74 | GPU_HOST_DEVICE__ PtrStep() : step(0) {} 75 | GPU_HOST_DEVICE__ PtrStep(T* data_arg, size_t step_arg) : DevPtr(data_arg), step(step_arg) {} 76 | 77 | /** \brief stride between two consecutive rows in bytes. Step is stored always and everywhere in bytes!!! */ 78 | size_t step; 79 | 80 | GPU_HOST_DEVICE__ T* ptr(int y = 0) { return ( T*)( ( char*)DevPtr::data + y * step); } 81 | GPU_HOST_DEVICE__ const T* ptr(int y = 0) const { return (const T*)( (const char*)DevPtr::data + y * step); } 82 | }; 83 | 84 | template struct PtrStepSz : public PtrStep 85 | { 86 | GPU_HOST_DEVICE__ PtrStepSz() : cols(0), rows(0) {} 87 | GPU_HOST_DEVICE__ PtrStepSz(int rows_arg, int cols_arg, T* data_arg, size_t step_arg) 88 | : PtrStep(data_arg, step_arg), cols(cols_arg), rows(rows_arg) {} 89 | 90 | int cols; 91 | int rows; 92 | }; 93 | 94 | #endif /* KERNEL_CONTAINERS_HPP_ */ 95 | 96 | -------------------------------------------------------------------------------- /Core/Cuda/convenience.cuh: -------------------------------------------------------------------------------- 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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | * Software License Agreement (BSD License) 19 | * 20 | * Copyright (c) 2011, Willow Garage, Inc. 21 | * All rights reserved. 22 | * 23 | * Redistribution and use in source and binary forms, with or without 24 | * modification, are permitted provided that the following conditions 25 | * are met: 26 | * 27 | * * Redistributions of source code must retain the above copyright 28 | * notice, this list of conditions and the following disclaimer. 29 | * * Redistributions in binary form must reproduce the above 30 | * copyright notice, this list of conditions and the following 31 | * disclaimer in the documentation and/or other materials provided 32 | * with the distribution. 33 | * * Neither the name of Willow Garage, Inc. nor the names of its 34 | * contributors may be used to endorse or promote products derived 35 | * from this software without specific prior written permission. 36 | * 37 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 38 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 39 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 40 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 41 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 42 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 43 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 44 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 45 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 46 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 47 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 48 | * POSSIBILITY OF SUCH DAMAGE. 49 | * 50 | * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com) 51 | */ 52 | 53 | #ifndef CUDA_CONVENIENCE_CUH_ 54 | #define CUDA_CONVENIENCE_CUH_ 55 | 56 | #include 57 | #include 58 | #include 59 | 60 | static inline int getGridDim(int x, int y) { 61 | return (x + y - 1) / y; 62 | } 63 | 64 | static inline void cudaSafeCall(cudaError_t err) { 65 | if (cudaSuccess != err) { 66 | std::cout << "Error: " << cudaGetErrorString(err) << ": " << __FILE__ << ":" << __LINE__ 67 | << std::endl; 68 | exit(0); 69 | } 70 | } 71 | 72 | #endif /* CUDA_CONVENIENCE_CUH_ */ 73 | -------------------------------------------------------------------------------- /Core/Cuda/operators.cuh: -------------------------------------------------------------------------------- 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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | * Software License Agreement (BSD License) 19 | * 20 | * Copyright (c) 2011, Willow Garage, Inc. 21 | * All rights reserved. 22 | * 23 | * Redistribution and use in source and binary forms, with or without 24 | * modification, are permitted provided that the following conditions 25 | * are met: 26 | * 27 | * * Redistributions of source code must retain the above copyright 28 | * notice, this list of conditions and the following disclaimer. 29 | * * Redistributions in binary form must reproduce the above 30 | * copyright notice, this list of conditions and the following 31 | * disclaimer in the documentation and/or other materials provided 32 | * with the distribution. 33 | * * Neither the name of Willow Garage, Inc. nor the names of its 34 | * contributors may be used to endorse or promote products derived 35 | * from this software without specific prior written permission. 36 | * 37 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 38 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 39 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 40 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 41 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 42 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 43 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 44 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 45 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 46 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 47 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 48 | * POSSIBILITY OF SUCH DAMAGE. 49 | * 50 | * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com) 51 | */ 52 | 53 | #include 54 | 55 | #ifndef CUDA_OPERATORS_CUH_ 56 | #define CUDA_OPERATORS_CUH_ 57 | 58 | __device__ __host__ __forceinline__ float3 operator-(const float3& a, const float3& b) { 59 | return make_float3(a.x - b.x, a.y - b.y, a.z - b.z); 60 | } 61 | 62 | __device__ __host__ __forceinline__ float3 operator+(const float3& a, const float3& b) { 63 | return make_float3(a.x + b.x, a.y + b.y, a.z + b.z); 64 | } 65 | 66 | __device__ __host__ __forceinline__ float3 cross(const float3& a, const float3& b) { 67 | return make_float3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); 68 | } 69 | 70 | __device__ __host__ __forceinline__ float dot(const float3& a, const float3& b) { 71 | return a.x * b.x + a.y * b.y + a.z * b.z; 72 | } 73 | 74 | __device__ __host__ __forceinline__ float norm(const float3& a) { 75 | return sqrtf(dot(a, a)); 76 | } 77 | 78 | __device__ __host__ __forceinline__ float3 normalized(const float3& a) { 79 | const float rn = rsqrtf(dot(a, a)); 80 | return make_float3(a.x * rn, a.y * rn, a.z * rn); 81 | } 82 | 83 | __device__ __forceinline__ float3 operator*(const mat33& m, const float3& a) { 84 | return make_float3(dot(m.data[0], a), dot(m.data[1], a), dot(m.data[2], a)); 85 | } 86 | 87 | #endif /* CUDA_OPERATORS_CUH_ */ 88 | -------------------------------------------------------------------------------- /Core/Cuda/types.cuh: -------------------------------------------------------------------------------- 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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | * Software License Agreement (BSD License) 19 | * 20 | * Copyright (c) 2011, Willow Garage, Inc. 21 | * All rights reserved. 22 | * 23 | * Redistribution and use in source and binary forms, with or without 24 | * modification, are permitted provided that the following conditions 25 | * are met: 26 | * 27 | * * Redistributions of source code must retain the above copyright 28 | * notice, this list of conditions and the following disclaimer. 29 | * * Redistributions in binary form must reproduce the above 30 | * copyright notice, this list of conditions and the following 31 | * disclaimer in the documentation and/or other materials provided 32 | * with the distribution. 33 | * * Neither the name of Willow Garage, Inc. nor the names of its 34 | * contributors may be used to endorse or promote products derived 35 | * from this software without specific prior written permission. 36 | * 37 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 38 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 39 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 40 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 41 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 42 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 43 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 44 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 45 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 46 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 47 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 48 | * POSSIBILITY OF SUCH DAMAGE. 49 | * 50 | * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com) 51 | */ 52 | 53 | #ifndef CUDA_TYPES_CUH_ 54 | #define CUDA_TYPES_CUH_ 55 | 56 | #include 57 | 58 | #if !defined(__CUDACC__) 59 | #include 60 | #endif 61 | 62 | #define MAX_THREADS 1024 63 | 64 | constexpr int reduceThreads = 256; 65 | constexpr int reduceBlocks = 64; 66 | 67 | struct mat33 { 68 | mat33() {} 69 | 70 | #if !defined(__CUDACC__) 71 | mat33(Eigen::Matrix& e) { 72 | memcpy(data, e.data(), sizeof(mat33)); 73 | } 74 | #endif 75 | 76 | float3 data[3]; 77 | }; 78 | 79 | struct DataTerm { 80 | short2 zero; 81 | short2 one; 82 | float diff; 83 | bool valid; 84 | }; 85 | 86 | struct CameraModel { 87 | float fx, fy, cx, cy; 88 | CameraModel() : fx(0), fy(0), cx(0), cy(0) {} 89 | 90 | CameraModel(float fx_, float fy_, float cx_, float cy_) : fx(fx_), fy(fy_), cx(cx_), cy(cy_) {} 91 | 92 | CameraModel operator()(int level) const { 93 | int div = 1 << level; 94 | return (CameraModel(fx / div, fy / div, cx / div, cy / div)); 95 | } 96 | }; 97 | 98 | struct JtJJtrSE3 { 99 | // 27 floats for each product (27) 100 | float aa, ab, ac, ad, ae, af, ag, bb, bc, bd, be, bf, bg, cc, cd, ce, cf, cg, dd, de, df, dg, ee, 101 | ef, eg, ff, fg; 102 | 103 | // Extra data needed (29) 104 | float residual, inliers; 105 | 106 | __device__ inline void add(const JtJJtrSE3& a) { 107 | aa += a.aa; 108 | ab += a.ab; 109 | ac += a.ac; 110 | ad += a.ad; 111 | ae += a.ae; 112 | af += a.af; 113 | ag += a.ag; 114 | 115 | bb += a.bb; 116 | bc += a.bc; 117 | bd += a.bd; 118 | be += a.be; 119 | bf += a.bf; 120 | bg += a.bg; 121 | 122 | cc += a.cc; 123 | cd += a.cd; 124 | ce += a.ce; 125 | cf += a.cf; 126 | cg += a.cg; 127 | 128 | dd += a.dd; 129 | de += a.de; 130 | df += a.df; 131 | dg += a.dg; 132 | 133 | ee += a.ee; 134 | ef += a.ef; 135 | eg += a.eg; 136 | 137 | ff += a.ff; 138 | fg += a.fg; 139 | 140 | residual += a.residual; 141 | inliers += a.inliers; 142 | } 143 | }; 144 | 145 | struct JtJJtrSO3 { 146 | // 9 floats for each product (9) 147 | float aa, ab, ac, ad, bb, bc, bd, cc, cd; 148 | 149 | // Extra data needed (11) 150 | float residual, inliers; 151 | 152 | __device__ inline void add(const JtJJtrSO3& a) { 153 | aa += a.aa; 154 | ab += a.ab; 155 | ac += a.ac; 156 | ad += a.ad; 157 | 158 | bb += a.bb; 159 | bc += a.bc; 160 | bd += a.bd; 161 | 162 | cc += a.cc; 163 | cd += a.cd; 164 | 165 | residual += a.residual; 166 | inliers += a.inliers; 167 | } 168 | }; 169 | 170 | #endif /* CUDA_TYPES_CUH_ */ 171 | -------------------------------------------------------------------------------- /Core/Deformation.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef DEFORMATION_H_ 21 | #define DEFORMATION_H_ 22 | 23 | #include "Ferns.h" 24 | #include "GPUTexture.h" 25 | #include "Shaders/Shaders.h" 26 | #include "Shaders/Uniform.h" 27 | #include "Shaders/Vertex.h" 28 | #include "Utils/DeformationGraph.h" 29 | #include "Utils/Intrinsics.h" 30 | #include "Utils/Resolution.h" 31 | 32 | #include 33 | 34 | class Deformation { 35 | public: 36 | Deformation(); 37 | virtual ~Deformation(); 38 | 39 | std::vector& getGraph(); 40 | 41 | void getRawGraph(std::vector& graph); 42 | 43 | void sampleGraphModel(const std::pair& model); 44 | 45 | void sampleGraphFrom(Deformation& other); 46 | 47 | class Constraint { 48 | public: 49 | Constraint( 50 | const Eigen::Vector3d& src, 51 | const Eigen::Vector3d& target, 52 | const uint64_t& srcTime, 53 | const uint64_t& targetTime, 54 | const bool relative, 55 | const bool pin = false) 56 | : src(src), 57 | target(target), 58 | srcTime(srcTime), 59 | targetTime(targetTime), 60 | relative(relative), 61 | pin(pin), 62 | srcPointPoolId(-1), 63 | tarPointPoolId(-1) {} 64 | 65 | Eigen::Vector3d src; 66 | Eigen::Vector3d target; 67 | uint64_t srcTime; 68 | uint64_t targetTime; 69 | bool relative; 70 | bool pin; 71 | int srcPointPoolId; 72 | int tarPointPoolId; 73 | }; 74 | 75 | void addConstraint( 76 | const Eigen::Vector4d& src, 77 | const Eigen::Vector4d& target, 78 | const uint64_t& srcTime, 79 | const uint64_t& targetTime, 80 | const bool pinConstraints); 81 | 82 | void addConstraint(const Constraint& constraint); 83 | 84 | bool constrain( 85 | std::vector& ferns, 86 | std::vector& rawGraph, 87 | int time, 88 | const bool fernMatch, 89 | std::vector>& t_T_wc, 90 | const bool relaxGraph, 91 | std::vector* newRelativeCons = 0); 92 | 93 | Eigen::Vector4f* getRawSampledNodes_w() { 94 | return rawSampledNodes_w; 95 | } 96 | 97 | int getCount() { 98 | return int(count); 99 | } 100 | 101 | int getLastDeformTime() { 102 | return lastDeformTime; 103 | } 104 | 105 | private: 106 | DeformationGraph def; 107 | 108 | std::vector vertexTimes; 109 | std::vector pointPool; 110 | int originalPointPool; 111 | int firstGraphNode; 112 | 113 | std::shared_ptr sampleProgram; 114 | GLuint vbo; 115 | GLuint fid; 116 | const int bufferSize; 117 | GLuint countQuery; 118 | uint32_t count; 119 | Eigen::Vector4f* rawSampledNodes_w; 120 | 121 | std::vector> poseGraphPoints; 122 | std::vector graphPoseTimes; 123 | std::vector* graphPosePoints; 124 | 125 | std::vector constraints; 126 | int lastDeformTime; 127 | }; 128 | 129 | #endif /* DEFORMATION_H_ */ 130 | -------------------------------------------------------------------------------- /Core/Ferns.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef FERNS_H_ 21 | #define FERNS_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "Shaders/Resize.h" 30 | #include "Utils/Intrinsics.h" 31 | #include "Utils/RGBDOdometry.h" 32 | #include "Utils/Resolution.h" 33 | 34 | class Ferns { 35 | public: 36 | Ferns(int n, int maxDepth, const float photoThresh); 37 | virtual ~Ferns(); 38 | 39 | bool addFrame( 40 | GPUTexture* imageTexture, 41 | GPUTexture* vertexTexture, 42 | GPUTexture* normalTexture, 43 | const Sophus::SE3d& T_wc, 44 | int srcTime, 45 | const float threshold); 46 | 47 | class SurfaceConstraint { 48 | public: 49 | SurfaceConstraint(const Eigen::Vector4d& sourcePoint, const Eigen::Vector4d& targetPoint) 50 | : sourcePoint(sourcePoint), targetPoint(targetPoint) {} 51 | 52 | Eigen::Vector4d sourcePoint; 53 | Eigen::Vector4d targetPoint; 54 | }; 55 | 56 | Sophus::SE3d findFrame( 57 | std::vector& constraints, 58 | const Sophus::SE3d& T_wc, 59 | GPUTexture* vertexTexture, 60 | GPUTexture* normalTexture, 61 | GPUTexture* imageTexture, 62 | const int time, 63 | const bool lost); 64 | 65 | class Fern { 66 | public: 67 | Fern() {} 68 | 69 | Eigen::Vector2i pos; 70 | Eigen::Vector4i rgbd; 71 | std::vector ids[16]; 72 | }; 73 | 74 | std::vector conservatory; 75 | 76 | class Frame { 77 | public: 78 | Frame( 79 | int n, 80 | int id, 81 | const Sophus::SE3d& T_wc, 82 | const int srcTime, 83 | const int numPixels, 84 | uint8_t* rgb = 0, 85 | Eigen::Vector4f* verts = 0, 86 | Eigen::Vector4f* norms = 0) 87 | : goodCodes(0), 88 | id(id), 89 | T_wc(T_wc), 90 | srcTime(srcTime), 91 | initRgb(rgb), 92 | initVerts(verts), 93 | initNorms(norms) { 94 | codes = new uint8_t[n]; 95 | 96 | if (rgb) { 97 | this->initRgb = new uint8_t[numPixels * 3]; 98 | memcpy(this->initRgb, rgb, numPixels * 3); 99 | } 100 | 101 | if (verts) { 102 | this->initVerts = new Eigen::Vector4f[numPixels]; 103 | for (int i = 0; i < numPixels; i++) { 104 | this->initVerts[i] = verts[i]; 105 | } 106 | } 107 | 108 | if (norms) { 109 | this->initNorms = new Eigen::Vector4f[numPixels]; 110 | for (int i = 0; i < numPixels; i++) { 111 | this->initNorms[i] = norms[i]; 112 | } 113 | } 114 | } 115 | 116 | virtual ~Frame() { 117 | delete[] codes; 118 | 119 | if (initRgb) 120 | delete[] initRgb; 121 | 122 | if (initVerts) 123 | delete[] initVerts; 124 | 125 | if (initNorms) 126 | delete[] initNorms; 127 | } 128 | 129 | uint8_t* codes; 130 | int goodCodes; 131 | const int id; 132 | Sophus::SE3d T_wc; 133 | const int srcTime; 134 | uint8_t* initRgb; 135 | Eigen::Vector4f* initVerts; 136 | Eigen::Vector4f* initNorms; 137 | }; 138 | 139 | std::vector frames; 140 | 141 | const int num; 142 | std::mt19937 random; 143 | const int factor; 144 | const int width; 145 | const int height; 146 | const int maxDepth; 147 | const float photoThresh; 148 | std::uniform_int_distribution widthDist; 149 | std::uniform_int_distribution heightDist; 150 | std::uniform_int_distribution rgbDist; 151 | std::uniform_int_distribution dDist; 152 | 153 | int lastClosest; 154 | const uint8_t badCode; 155 | RGBDOdometry rgbd; 156 | 157 | private: 158 | void generateFerns(); 159 | 160 | float blockHD(const Frame* f1, const Frame* f2); 161 | float blockHDAware(const Frame* f1, const Frame* f2); 162 | 163 | float photometricCheck( 164 | const Img& vertSmall, 165 | const Img>& imgSmall, 166 | const Sophus::SE3d& T_wc_est, 167 | const Sophus::SE3d& T_wc_fern, 168 | const uint8_t* fernRgb); 169 | 170 | GPUTexture vertFern; 171 | GPUTexture vertCurrent; 172 | 173 | GPUTexture normFern; 174 | GPUTexture normCurrent; 175 | 176 | GPUTexture colorFern; 177 | GPUTexture colorCurrent; 178 | 179 | Resize resize; 180 | 181 | Img> imageBuff; 182 | Img vertBuff; 183 | Img normBuff; 184 | }; 185 | 186 | #endif /* FERNS_H_ */ 187 | -------------------------------------------------------------------------------- /Core/GPUTexture.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "GPUTexture.h" 21 | 22 | const std::string GPUTexture::RGB = "RGB"; 23 | const std::string GPUTexture::DEPTH_RAW = "DEPTH"; 24 | const std::string GPUTexture::DEPTH_FILTERED = "DEPTH_FILTERED"; 25 | const std::string GPUTexture::DEPTH_METRIC = "DEPTH_METRIC"; 26 | const std::string GPUTexture::DEPTH_METRIC_FILTERED = "DEPTH_METRIC_FILTERED"; 27 | const std::string GPUTexture::DEPTH_NORM = "DEPTH_NORM"; 28 | 29 | GPUTexture::GPUTexture( 30 | const int width, 31 | const int height, 32 | const GLenum internalFormat, 33 | const GLenum format, 34 | const GLenum dataType, 35 | const bool draw) 36 | : texture(new pangolin::GlTexture(width, height, internalFormat, draw, 0, format, dataType)), 37 | draw(draw), 38 | width(width), 39 | height(height), 40 | internalFormat(internalFormat), 41 | format(format), 42 | dataType(dataType) { 43 | cudaGraphicsGLRegisterImage( 44 | &cudaRes, texture->tid, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsReadOnly); 45 | } 46 | 47 | GPUTexture::~GPUTexture() { 48 | if (texture) { 49 | delete texture; 50 | } 51 | 52 | if (cudaRes) { 53 | cudaGraphicsUnregisterResource(cudaRes); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /Core/GPUTexture.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef GPUTEXTURE_H_ 21 | #define GPUTEXTURE_H_ 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | class GPUTexture { 30 | public: 31 | GPUTexture( 32 | const int width, 33 | const int height, 34 | const GLenum internalFormat, 35 | const GLenum format, 36 | const GLenum dataType, 37 | const bool draw); 38 | 39 | virtual ~GPUTexture(); 40 | 41 | static const std::string RGB, DEPTH_RAW, DEPTH_FILTERED, DEPTH_METRIC, DEPTH_METRIC_FILTERED, 42 | DEPTH_NORM; 43 | 44 | pangolin::GlTexture* texture; 45 | 46 | cudaGraphicsResource* cudaRes; 47 | 48 | const bool draw; 49 | 50 | private: 51 | GPUTexture() 52 | : texture(0), 53 | cudaRes(0), 54 | draw(false), 55 | width(0), 56 | height(0), 57 | internalFormat(0), 58 | format(0), 59 | dataType(0) {} 60 | const int width; 61 | const int height; 62 | const GLenum internalFormat; 63 | const GLenum format; 64 | const GLenum dataType; 65 | }; 66 | 67 | #endif /* GPUTEXTURE_H_ */ 68 | -------------------------------------------------------------------------------- /Core/GlobalModel.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef GLOBALMODEL_H_ 21 | #define GLOBALMODEL_H_ 22 | 23 | #include 24 | #include 25 | #include "GPUTexture.h" 26 | #include "IndexMap.h" 27 | #include "Shaders/FeedbackBuffer.h" 28 | #include "Shaders/Shaders.h" 29 | #include "Shaders/Uniform.h" 30 | #include "Utils/Intrinsics.h" 31 | #include "Utils/Resolution.h" 32 | #include "Utils/Stopwatch.h" 33 | 34 | class GlobalModel { 35 | public: 36 | GlobalModel(); 37 | virtual ~GlobalModel(); 38 | 39 | void initialise(const FeedbackBuffer& rawFeedback, const FeedbackBuffer& filteredFeedback); 40 | 41 | static const int TEXTURE_DIMENSION; 42 | static const int MAX_VERTICES; 43 | static const int NODE_TEXTURE_DIMENSION; 44 | static const int MAX_NODES; 45 | 46 | void renderPointCloud( 47 | pangolin::OpenGlMatrix mvp, 48 | const float threshold, 49 | const bool drawUnstable, 50 | const bool drawNormals, 51 | const bool drawColors, 52 | const bool drawPoints, 53 | const bool drawWindow, 54 | const bool drawTimes, 55 | const int time, 56 | const int timeDelta); 57 | 58 | const std::pair& model(); 59 | 60 | void fuse( 61 | const Sophus::SE3d& T_wc, 62 | const int& time, 63 | GPUTexture* rgb, 64 | GPUTexture* depthRaw, 65 | GPUTexture* depthFiltered, 66 | GPUTexture* indexMap, 67 | GPUTexture* vertConfMap, 68 | GPUTexture* colorTimeMap, 69 | GPUTexture* normRadMap, 70 | const float depthCutoff, 71 | const float weighting); 72 | 73 | void clean( 74 | const Sophus::SE3d& T_wc, 75 | const int& time, 76 | GPUTexture* indexMap, 77 | GPUTexture* vertConfMap, 78 | GPUTexture* colorTimeMap, 79 | GPUTexture* normRadMap, 80 | GPUTexture* depthMap, 81 | const float confThreshold, 82 | std::vector& graph, 83 | const int timeDelta, 84 | const float maxDepth, 85 | const bool isFern); 86 | 87 | uint32_t lastCount(); 88 | 89 | Eigen::Vector4f* downloadMap(); 90 | 91 | private: 92 | // First is the vbo, second is the fid 93 | std::pair* vbos; 94 | int target, renderSource; 95 | 96 | const int bufferSize; 97 | 98 | GLuint countQuery; 99 | uint32_t count; 100 | 101 | std::shared_ptr initProgram; 102 | std::shared_ptr drawProgram; 103 | std::shared_ptr drawSurfelProgram; 104 | 105 | // For supersample fusing 106 | std::shared_ptr dataProgram; 107 | std::shared_ptr updateProgram; 108 | std::shared_ptr unstableProgram; 109 | pangolin::GlRenderBuffer renderBuffer; 110 | 111 | // We render updated vertices vec3 + confidences to one texture 112 | GPUTexture updateMapVertsConfs; 113 | 114 | // We render updated colors vec3 + timestamps to another 115 | GPUTexture updateMapColorsTime; 116 | 117 | // We render updated normals vec3 + radii to another 118 | GPUTexture updateMapNormsRadii; 119 | 120 | // 16 floats stored column-major yo' 121 | GPUTexture deformationNodes; 122 | 123 | GLuint newUnstableVbo, newUnstableFid; 124 | 125 | pangolin::GlFramebuffer frameBuffer; 126 | GLuint uvo; 127 | int uvSize; 128 | }; 129 | 130 | #endif /* GLOBALMODEL_H_ */ 131 | -------------------------------------------------------------------------------- /Core/IndexMap.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef INDEXMAP_H_ 21 | #define INDEXMAP_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include "GPUTexture.h" 27 | #include "Shaders/Shaders.h" 28 | #include "Shaders/Uniform.h" 29 | #include "Shaders/Vertex.h" 30 | #include "Utils/Intrinsics.h" 31 | #include "Utils/Resolution.h" 32 | 33 | class IndexMap { 34 | public: 35 | IndexMap(); 36 | virtual ~IndexMap(); 37 | 38 | void predictIndices( 39 | const Sophus::SE3d& T_wc, 40 | const int& time, 41 | const std::pair& model, 42 | const float depthCutoff, 43 | const int timeDelta); 44 | 45 | void renderDepth(const float depthCutoff); 46 | 47 | enum Prediction { ACTIVE, INACTIVE }; 48 | 49 | void combinedPredict( 50 | const Sophus::SE3d& T_wc, 51 | const std::pair& model, 52 | const float depthCutoff, 53 | const float confThreshold, 54 | const int time, 55 | const int maxTime, 56 | const int timeDelta, 57 | IndexMap::Prediction predictionType); 58 | 59 | void synthesizeInfo( 60 | const Eigen::Matrix4f& pose, 61 | const std::pair& model, 62 | const float depthCutoff, 63 | const float confThreshold); 64 | 65 | void synthesizeDepth( 66 | const Sophus::SE3d& T_wc, 67 | const std::pair& model, 68 | const float depthCutoff, 69 | const float confThreshold, 70 | const int time, 71 | const int maxTime, 72 | const int timeDelta); 73 | 74 | GPUTexture* indexTex() { 75 | return &indexTexture; 76 | } 77 | 78 | GPUTexture* vertConfTex() { 79 | return &vertConfTexture; 80 | } 81 | 82 | GPUTexture* colorTimeTex() { 83 | return &colorTimeTexture; 84 | } 85 | 86 | GPUTexture* normalRadTex() { 87 | return &normalRadTexture; 88 | } 89 | 90 | GPUTexture* drawTex() { 91 | return &drawTexture; 92 | } 93 | 94 | GPUTexture* depthTex() { 95 | return &depthTexture; 96 | } 97 | 98 | GPUTexture* imageTex() { 99 | return &imageTexture; 100 | } 101 | 102 | GPUTexture* vertexTex() { 103 | return &vertexTexture; 104 | } 105 | 106 | GPUTexture* normalTex() { 107 | return &normalTexture; 108 | } 109 | 110 | GPUTexture* timeTex() { 111 | return &timeTexture; 112 | } 113 | 114 | GPUTexture* oldImageTex() { 115 | return &oldImageTexture; 116 | } 117 | 118 | GPUTexture* oldVertexTex() { 119 | return &oldVertexTexture; 120 | } 121 | 122 | GPUTexture* oldNormalTex() { 123 | return &oldNormalTexture; 124 | } 125 | 126 | GPUTexture* oldTimeTex() { 127 | return &oldTimeTexture; 128 | } 129 | 130 | GPUTexture* colorInfoTex() { 131 | return &colorInfoTexture; 132 | } 133 | 134 | GPUTexture* vertexInfoTex() { 135 | return &vertexInfoTexture; 136 | } 137 | 138 | GPUTexture* normalInfoTex() { 139 | return &normalInfoTexture; 140 | } 141 | 142 | static const int FACTOR; 143 | 144 | private: 145 | std::shared_ptr indexProgram; 146 | pangolin::GlFramebuffer indexFrameBuffer; 147 | pangolin::GlRenderBuffer indexRenderBuffer; 148 | GPUTexture indexTexture; 149 | GPUTexture vertConfTexture; 150 | GPUTexture colorTimeTexture; 151 | GPUTexture normalRadTexture; 152 | 153 | std::shared_ptr drawDepthProgram; 154 | pangolin::GlFramebuffer drawFrameBuffer; 155 | pangolin::GlRenderBuffer drawRenderBuffer; 156 | GPUTexture drawTexture; 157 | 158 | std::shared_ptr depthProgram; 159 | pangolin::GlFramebuffer depthFrameBuffer; 160 | pangolin::GlRenderBuffer depthRenderBuffer; 161 | GPUTexture depthTexture; 162 | 163 | std::shared_ptr combinedProgram; 164 | pangolin::GlFramebuffer combinedFrameBuffer; 165 | pangolin::GlRenderBuffer combinedRenderBuffer; 166 | GPUTexture imageTexture; 167 | GPUTexture vertexTexture; 168 | GPUTexture normalTexture; 169 | GPUTexture timeTexture; 170 | 171 | pangolin::GlFramebuffer oldFrameBuffer; 172 | pangolin::GlRenderBuffer oldRenderBuffer; 173 | GPUTexture oldImageTexture; 174 | GPUTexture oldVertexTexture; 175 | GPUTexture oldNormalTexture; 176 | GPUTexture oldTimeTexture; 177 | 178 | pangolin::GlFramebuffer infoFrameBuffer; 179 | pangolin::GlRenderBuffer infoRenderBuffer; 180 | GPUTexture colorInfoTexture; 181 | GPUTexture vertexInfoTexture; 182 | GPUTexture normalInfoTexture; 183 | }; 184 | 185 | #endif /* INDEXMAP_H_ */ 186 | -------------------------------------------------------------------------------- /Core/PoseMatch.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef POSEMATCH_H_ 21 | #define POSEMATCH_H_ 22 | 23 | #include 24 | #include "Ferns.h" 25 | 26 | class PoseMatch { 27 | public: 28 | PoseMatch( 29 | int firstId, 30 | int secondId, 31 | const Sophus::SE3d& T_wc_first, 32 | const Sophus::SE3d& T_wc_second, 33 | const std::vector& constraints, 34 | const bool& fern) 35 | : firstId(firstId), 36 | secondId(secondId), 37 | T_wc_first(T_wc_first), 38 | T_wc_second(T_wc_second), 39 | constraints(constraints), 40 | fern(fern) {} 41 | 42 | int firstId; 43 | int secondId; 44 | Sophus::SE3d T_wc_first; 45 | Sophus::SE3d T_wc_second; 46 | std::vector constraints; 47 | bool fern; 48 | }; 49 | 50 | #endif /* POSEMATCH_H_ */ 51 | -------------------------------------------------------------------------------- /Core/Shaders/ComputePack.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "ComputePack.h" 21 | 22 | const std::string ComputePack::NORM = "NORM"; 23 | const std::string ComputePack::FILTER = "FILTER"; 24 | const std::string ComputePack::METRIC = "METRIC"; 25 | const std::string ComputePack::METRIC_FILTERED = "METRIC_FILTERED"; 26 | 27 | ComputePack::ComputePack(std::shared_ptr program, pangolin::GlTexture* target) 28 | : program(program), 29 | renderBuffer(Resolution::getInstance().width(), Resolution::getInstance().height()), 30 | target(target) { 31 | frameBuffer.AttachColour(*target); 32 | frameBuffer.AttachDepth(renderBuffer); 33 | } 34 | 35 | ComputePack::~ComputePack() {} 36 | 37 | void ComputePack::compute(pangolin::GlTexture* input, const std::vector* const uniforms) { 38 | input->Bind(); 39 | 40 | frameBuffer.Bind(); 41 | 42 | glPushAttrib(GL_VIEWPORT_BIT); 43 | 44 | glViewport(0, 0, renderBuffer.width, renderBuffer.height); 45 | 46 | glClearColor(0, 0, 0, 0); 47 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 48 | 49 | program->Bind(); 50 | 51 | if (uniforms) { 52 | for (size_t i = 0; i < uniforms->size(); i++) { 53 | program->setUniform(uniforms->at(i)); 54 | } 55 | } 56 | 57 | glDrawArrays(GL_POINTS, 0, 1); 58 | 59 | frameBuffer.Unbind(); 60 | 61 | program->Unbind(); 62 | 63 | glPopAttrib(); 64 | 65 | glFinish(); 66 | } 67 | -------------------------------------------------------------------------------- /Core/Shaders/ComputePack.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef COMPUTEPACK_H_ 21 | #define COMPUTEPACK_H_ 22 | 23 | #include 24 | #include "../Utils/Resolution.h" 25 | #include "Shaders.h" 26 | #include "Uniform.h" 27 | 28 | class ComputePack { 29 | public: 30 | ComputePack(std::shared_ptr program, pangolin::GlTexture* target); 31 | 32 | virtual ~ComputePack(); 33 | 34 | static const std::string NORM, FILTER, METRIC, METRIC_FILTERED; 35 | 36 | void compute(pangolin::GlTexture* input, const std::vector* const uniforms = 0); 37 | 38 | private: 39 | std::shared_ptr program; 40 | pangolin::GlRenderBuffer renderBuffer; 41 | pangolin::GlTexture* target; 42 | pangolin::GlFramebuffer frameBuffer; 43 | }; 44 | 45 | #endif /* COMPUTEPACK_H_ */ 46 | -------------------------------------------------------------------------------- /Core/Shaders/FeedbackBuffer.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef FEEDBACKBUFFER_H_ 21 | #define FEEDBACKBUFFER_H_ 22 | 23 | #include 24 | #include 25 | #include "../Utils/Intrinsics.h" 26 | #include "../Utils/Resolution.h" 27 | #include "Shaders.h" 28 | #include "Uniform.h" 29 | #include "Vertex.h" 30 | 31 | class FeedbackBuffer { 32 | public: 33 | FeedbackBuffer(std::shared_ptr program); 34 | virtual ~FeedbackBuffer(); 35 | 36 | std::shared_ptr program; 37 | 38 | void compute( 39 | pangolin::GlTexture* color, 40 | pangolin::GlTexture* depth, 41 | const int& time, 42 | const float depthCutoff); 43 | 44 | void render( 45 | pangolin::OpenGlMatrix mvp, 46 | const Eigen::Matrix4f& pose, 47 | const bool drawNormals, 48 | const bool drawColors); 49 | 50 | static const std::string RAW, FILTERED; 51 | 52 | GLuint vbo; 53 | GLuint fid; 54 | 55 | private: 56 | std::shared_ptr drawProgram; 57 | GLuint uvo; 58 | GLuint countQuery; 59 | const int bufferSize; 60 | uint32_t count; 61 | }; 62 | 63 | #endif /* FEEDBACKBUFFER_H_ */ 64 | -------------------------------------------------------------------------------- /Core/Shaders/FillIn.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef FILLIN_H_ 21 | #define FILLIN_H_ 22 | 23 | #include "../GPUTexture.h" 24 | #include "../Utils/Intrinsics.h" 25 | #include "../Utils/Resolution.h" 26 | #include "Shaders.h" 27 | #include "Uniform.h" 28 | 29 | class FillIn { 30 | public: 31 | FillIn(); 32 | virtual ~FillIn(); 33 | 34 | void image(GPUTexture* existingRgb, GPUTexture* rawRgb, bool passthrough); 35 | void vertex(GPUTexture* existingVertex, GPUTexture* rawDepth, bool passthrough); 36 | void normal(GPUTexture* existingNormal, GPUTexture* rawDepth, bool passthrough); 37 | 38 | GPUTexture imageTexture; 39 | GPUTexture vertexTexture; 40 | GPUTexture normalTexture; 41 | 42 | std::shared_ptr imageProgram; 43 | pangolin::GlRenderBuffer imageRenderBuffer; 44 | pangolin::GlFramebuffer imageFrameBuffer; 45 | 46 | std::shared_ptr vertexProgram; 47 | pangolin::GlRenderBuffer vertexRenderBuffer; 48 | pangolin::GlFramebuffer vertexFrameBuffer; 49 | 50 | std::shared_ptr normalProgram; 51 | pangolin::GlRenderBuffer normalRenderBuffer; 52 | pangolin::GlFramebuffer normalFrameBuffer; 53 | }; 54 | 55 | #endif /* FILLIN_H_ */ 56 | -------------------------------------------------------------------------------- /Core/Shaders/Resize.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "Resize.h" 21 | 22 | Resize::Resize(int srcWidth, int srcHeight, int destWidth, int destHeight) 23 | : imageTexture(destWidth, destHeight, GL_RGBA, GL_RGB, GL_UNSIGNED_BYTE, false), 24 | vertexTexture(destWidth, destHeight, GL_RGBA32F, GL_LUMINANCE, GL_FLOAT, false), 25 | timeTexture( 26 | destWidth, 27 | destHeight, 28 | GL_LUMINANCE16UI_EXT, 29 | GL_LUMINANCE_INTEGER_EXT, 30 | GL_UNSIGNED_SHORT, 31 | false), 32 | imageProgram(loadProgramFromFile("empty.vert", "resize.frag", "quad.geom")), 33 | imageRenderBuffer(destWidth, destHeight), 34 | vertexProgram(loadProgramFromFile("empty.vert", "resize.frag", "quad.geom")), 35 | vertexRenderBuffer(destWidth, destHeight), 36 | timeProgram(loadProgramFromFile("empty.vert", "resize.frag", "quad.geom")), 37 | timeRenderBuffer(destWidth, destHeight) { 38 | imageFrameBuffer.AttachColour(*imageTexture.texture); 39 | imageFrameBuffer.AttachDepth(imageRenderBuffer); 40 | 41 | vertexFrameBuffer.AttachColour(*vertexTexture.texture); 42 | vertexFrameBuffer.AttachDepth(vertexRenderBuffer); 43 | 44 | timeFrameBuffer.AttachColour(*timeTexture.texture); 45 | timeFrameBuffer.AttachDepth(timeRenderBuffer); 46 | } 47 | 48 | Resize::~Resize() {} 49 | 50 | void Resize::image(GPUTexture* source, Img>& dest) { 51 | imageFrameBuffer.Bind(); 52 | 53 | glPushAttrib(GL_VIEWPORT_BIT); 54 | 55 | glViewport(0, 0, imageRenderBuffer.width, imageRenderBuffer.height); 56 | 57 | glClearColor(0, 0, 0, 0); 58 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 59 | 60 | imageProgram->Bind(); 61 | 62 | imageProgram->setUniform(Uniform("eSampler", 0)); 63 | 64 | glActiveTexture(GL_TEXTURE0); 65 | glBindTexture(GL_TEXTURE_2D, source->texture->tid); 66 | 67 | glDrawArrays(GL_POINTS, 0, 1); 68 | 69 | glReadPixels( 70 | 0, 0, imageRenderBuffer.width, imageRenderBuffer.height, GL_RGB, GL_UNSIGNED_BYTE, dest.data); 71 | 72 | imageFrameBuffer.Unbind(); 73 | 74 | glBindTexture(GL_TEXTURE_2D, 0); 75 | 76 | glActiveTexture(GL_TEXTURE0); 77 | 78 | imageProgram->Unbind(); 79 | 80 | glPopAttrib(); 81 | 82 | glFinish(); 83 | } 84 | 85 | void Resize::vertex(GPUTexture* source, Img& dest) { 86 | vertexFrameBuffer.Bind(); 87 | 88 | glPushAttrib(GL_VIEWPORT_BIT); 89 | 90 | glViewport(0, 0, vertexRenderBuffer.width, vertexRenderBuffer.height); 91 | 92 | glClearColor(0, 0, 0, 0); 93 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 94 | 95 | vertexProgram->Bind(); 96 | 97 | vertexProgram->setUniform(Uniform("eSampler", 0)); 98 | 99 | glActiveTexture(GL_TEXTURE0); 100 | glBindTexture(GL_TEXTURE_2D, source->texture->tid); 101 | 102 | glDrawArrays(GL_POINTS, 0, 1); 103 | 104 | glReadPixels( 105 | 0, 0, vertexRenderBuffer.width, vertexRenderBuffer.height, GL_RGBA, GL_FLOAT, dest.data); 106 | 107 | vertexFrameBuffer.Unbind(); 108 | 109 | glBindTexture(GL_TEXTURE_2D, 0); 110 | 111 | glActiveTexture(GL_TEXTURE0); 112 | 113 | vertexProgram->Unbind(); 114 | 115 | glPopAttrib(); 116 | 117 | glFinish(); 118 | } 119 | 120 | void Resize::time(GPUTexture* source, Img& dest) { 121 | timeFrameBuffer.Bind(); 122 | 123 | glPushAttrib(GL_VIEWPORT_BIT); 124 | 125 | glViewport(0, 0, timeRenderBuffer.width, timeRenderBuffer.height); 126 | 127 | glClearColor(0, 0, 0, 0); 128 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 129 | 130 | timeProgram->Bind(); 131 | 132 | timeProgram->setUniform(Uniform("eSampler", 0)); 133 | 134 | glActiveTexture(GL_TEXTURE0); 135 | glBindTexture(GL_TEXTURE_2D, source->texture->tid); 136 | 137 | glDrawArrays(GL_POINTS, 0, 1); 138 | 139 | glReadPixels( 140 | 0, 141 | 0, 142 | timeRenderBuffer.width, 143 | timeRenderBuffer.height, 144 | GL_LUMINANCE_INTEGER_EXT, 145 | GL_UNSIGNED_SHORT, 146 | dest.data); 147 | 148 | timeFrameBuffer.Unbind(); 149 | 150 | glBindTexture(GL_TEXTURE_2D, 0); 151 | 152 | glActiveTexture(GL_TEXTURE0); 153 | 154 | timeProgram->Unbind(); 155 | 156 | glPopAttrib(); 157 | 158 | glFinish(); 159 | } 160 | -------------------------------------------------------------------------------- /Core/Shaders/Resize.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef RESIZE_H_ 21 | #define RESIZE_H_ 22 | 23 | #include "../GPUTexture.h" 24 | #include "../Utils/Img.h" 25 | #include "../Utils/Intrinsics.h" 26 | #include "../Utils/Resolution.h" 27 | #include "Shaders.h" 28 | #include "Uniform.h" 29 | 30 | class Resize { 31 | public: 32 | Resize(int srcWidth, int srcHeight, int destWidth, int destHeight); 33 | virtual ~Resize(); 34 | 35 | void image(GPUTexture* source, Img>& dest); 36 | void vertex(GPUTexture* source, Img& dest); 37 | void time(GPUTexture* source, Img& dest); 38 | 39 | GPUTexture imageTexture; 40 | GPUTexture vertexTexture; 41 | GPUTexture timeTexture; 42 | 43 | std::shared_ptr imageProgram; 44 | pangolin::GlRenderBuffer imageRenderBuffer; 45 | pangolin::GlFramebuffer imageFrameBuffer; 46 | 47 | std::shared_ptr vertexProgram; 48 | pangolin::GlRenderBuffer vertexRenderBuffer; 49 | pangolin::GlFramebuffer vertexFrameBuffer; 50 | 51 | std::shared_ptr timeProgram; 52 | pangolin::GlRenderBuffer timeRenderBuffer; 53 | pangolin::GlFramebuffer timeFrameBuffer; 54 | }; 55 | 56 | #endif /* RESIZE_H_ */ 57 | -------------------------------------------------------------------------------- /Core/Shaders/Shaders.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef SHADERS_SHADERS_H_ 21 | #define SHADERS_SHADERS_H_ 22 | 23 | #include 24 | #include 25 | #include "../Utils/Parse.h" 26 | #include "Uniform.h" 27 | 28 | class Shader : public pangolin::GlSlProgram { 29 | public: 30 | Shader() {} 31 | 32 | GLuint programId() { 33 | return prog; 34 | } 35 | 36 | void setUniform(const Uniform& v) { 37 | GLuint loc = glGetUniformLocation(prog, v.id.c_str()); 38 | 39 | switch (v.t) { 40 | case Uniform::INT: 41 | glUniform1i(loc, v.i); 42 | break; 43 | case Uniform::FLOAT: 44 | glUniform1f(loc, v.f); 45 | break; 46 | case Uniform::VEC2: 47 | glUniform2f(loc, v.v2(0), v.v2(1)); 48 | break; 49 | case Uniform::VEC3: 50 | glUniform3f(loc, v.v3(0), v.v3(1), v.v3(2)); 51 | break; 52 | case Uniform::VEC4: 53 | glUniform4f(loc, v.v4(0), v.v4(1), v.v4(2), v.v4(3)); 54 | break; 55 | case Uniform::MAT4: 56 | glUniformMatrix4fv(loc, 1, false, v.m4.data()); 57 | break; 58 | default: 59 | assert(false && "Uniform type not implemented!"); 60 | break; 61 | } 62 | } 63 | }; 64 | 65 | static inline std::shared_ptr loadProgramGeomFromFile( 66 | const std::string& vertex_shader_file, 67 | const std::string& geometry_shader_file) { 68 | std::shared_ptr program = std::make_shared(); 69 | 70 | program->AddShaderFromFile( 71 | pangolin::GlSlVertexShader, 72 | Parse::get().shaderDir() + "/" + vertex_shader_file, 73 | {}, 74 | {Parse::get().shaderDir()}); 75 | program->AddShaderFromFile( 76 | pangolin::GlSlGeometryShader, 77 | Parse::get().shaderDir() + "/" + geometry_shader_file, 78 | {}, 79 | {Parse::get().shaderDir()}); 80 | program->Link(); 81 | 82 | return program; 83 | } 84 | 85 | static inline std::shared_ptr loadProgramFromFile(const std::string& vertex_shader_file) { 86 | std::shared_ptr program = std::make_shared(); 87 | 88 | program->AddShaderFromFile( 89 | pangolin::GlSlVertexShader, 90 | Parse::get().shaderDir() + "/" + vertex_shader_file, 91 | {}, 92 | {Parse::get().shaderDir()}); 93 | program->Link(); 94 | 95 | return program; 96 | } 97 | 98 | static inline std::shared_ptr loadProgramFromFile( 99 | const std::string& vertex_shader_file, 100 | const std::string& fragment_shader_file) { 101 | std::shared_ptr program = std::make_shared(); 102 | 103 | program->AddShaderFromFile( 104 | pangolin::GlSlVertexShader, 105 | Parse::get().shaderDir() + "/" + vertex_shader_file, 106 | {}, 107 | {Parse::get().shaderDir()}); 108 | program->AddShaderFromFile( 109 | pangolin::GlSlFragmentShader, 110 | Parse::get().shaderDir() + "/" + fragment_shader_file, 111 | {}, 112 | {Parse::get().shaderDir()}); 113 | program->Link(); 114 | 115 | return program; 116 | } 117 | 118 | static inline std::shared_ptr loadProgramFromFile( 119 | const std::string& vertex_shader_file, 120 | const std::string& fragment_shader_file, 121 | const std::string& geometry_shader_file) { 122 | std::shared_ptr program = std::make_shared(); 123 | 124 | program->AddShaderFromFile( 125 | pangolin::GlSlVertexShader, 126 | Parse::get().shaderDir() + "/" + vertex_shader_file, 127 | {}, 128 | {Parse::get().shaderDir()}); 129 | program->AddShaderFromFile( 130 | pangolin::GlSlGeometryShader, 131 | Parse::get().shaderDir() + "/" + geometry_shader_file, 132 | {}, 133 | {Parse::get().shaderDir()}); 134 | program->AddShaderFromFile( 135 | pangolin::GlSlFragmentShader, 136 | Parse::get().shaderDir() + "/" + fragment_shader_file, 137 | {}, 138 | {Parse::get().shaderDir()}); 139 | program->Link(); 140 | 141 | return program; 142 | } 143 | 144 | #endif /* SHADERS_SHADERS_H_ */ 145 | -------------------------------------------------------------------------------- /Core/Shaders/Uniform.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef UNIFORM_H_ 21 | #define UNIFORM_H_ 22 | 23 | #include 24 | #include 25 | 26 | class Uniform { 27 | public: 28 | Uniform(const std::string& id, const int& v) : id(id), i(v), t(INT) {} 29 | 30 | Uniform(const std::string& id, const float& v) : id(id), f(v), t(FLOAT) {} 31 | 32 | Uniform(const std::string& id, const Eigen::Vector2f& v) : id(id), v2(v), t(VEC2) {} 33 | 34 | Uniform(const std::string& id, const Eigen::Vector3f& v) : id(id), v3(v), t(VEC3) {} 35 | 36 | Uniform(const std::string& id, const Eigen::Vector4f& v) : id(id), v4(v), t(VEC4) {} 37 | 38 | Uniform(const std::string& id, const Eigen::Matrix4f& v) : id(id), m4(v), t(MAT4) {} 39 | 40 | std::string id; 41 | 42 | int i; 43 | float f; 44 | Eigen::Vector2f v2; 45 | Eigen::Vector3f v3; 46 | Eigen::Vector4f v4; 47 | Eigen::Matrix4f m4; 48 | 49 | enum Type { INT, FLOAT, VEC2, VEC3, VEC4, MAT4, NONE }; 50 | 51 | Type t; 52 | }; 53 | 54 | #endif /* UNIFORM_H_ */ 55 | -------------------------------------------------------------------------------- /Core/Shaders/Vertex.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "Vertex.h" 21 | 22 | /* 23 | * OK this is the structure 24 | * 25 | *-------------------- 26 | * vec3 position 27 | * float confidence 28 | * 29 | * float color (encoded as a 24-bit integer) 30 | * float 31 | * float initTime 32 | * float timestamp 33 | * 34 | * vec3 normal 35 | * float radius 36 | *-------------------- 37 | 38 | * Which is three vec4s 39 | */ 40 | 41 | const int Vertex::SIZE = sizeof(Eigen::Vector4f) * 3; 42 | -------------------------------------------------------------------------------- /Core/Shaders/Vertex.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef VERTEX_H_ 21 | #define VERTEX_H_ 22 | 23 | #include 24 | 25 | class Vertex { 26 | public: 27 | static const int SIZE; 28 | 29 | private: 30 | Vertex() {} 31 | }; 32 | 33 | #endif /* VERTEX_H_ */ 34 | -------------------------------------------------------------------------------- /Core/Shaders/color.glsl: -------------------------------------------------------------------------------- 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 | float encodeColor(vec3 c) 20 | { 21 | int rgb = int(round(c.x * 255.0f)); 22 | rgb = (rgb << 8) + int(round(c.y * 255.0f)); 23 | rgb = (rgb << 8) + int(round(c.z * 255.0f)); 24 | return float(rgb); 25 | } 26 | 27 | vec3 decodeColor(float c) 28 | { 29 | vec3 col; 30 | col.x = float(int(c) >> 16 & 0xFF) / 255.0f; 31 | col.y = float(int(c) >> 8 & 0xFF) / 255.0f; 32 | col.z = float(int(c) & 0xFF) / 255.0f; 33 | return col; 34 | } 35 | -------------------------------------------------------------------------------- /Core/Shaders/combo_splat.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | uniform vec4 cam; //cx, cy, fx, fy 22 | uniform float maxDepth; 23 | 24 | in vec4 position; 25 | in vec4 normRad; 26 | in vec4 colTime; 27 | 28 | layout(location = 0) out vec4 image; 29 | layout(location = 1) out vec4 vertex; 30 | layout(location = 2) out vec4 normal; 31 | layout(location = 3) out uint time; 32 | 33 | #include "color.glsl" 34 | 35 | void main() 36 | { 37 | vec3 l = normalize(vec3((vec2(gl_FragCoord) - cam.xy) / cam.zw, 1.0f)); 38 | 39 | vec3 corrected_pos = (dot(position.xyz, normRad.xyz) / dot(l, normRad.xyz)) * l; 40 | 41 | //check if the intersection is inside the surfel 42 | float sqrRad = pow(normRad.w, 2); 43 | vec3 diff = corrected_pos - position.xyz; 44 | 45 | if(dot(diff, diff) > sqrRad) 46 | { 47 | discard; 48 | } 49 | 50 | image = vec4(decodeColor(colTime.x), 1); 51 | 52 | float z = corrected_pos.z; 53 | 54 | vertex = vec4((gl_FragCoord.x - cam.x) * z * (1.f / cam.z), (gl_FragCoord.y - cam.y) * z * (1.f / cam.w), z, position.w); 55 | 56 | normal = normRad; 57 | 58 | time = uint(colTime.z); 59 | 60 | gl_FragDepth = (corrected_pos.z / (2 * maxDepth)) + 0.5f; 61 | } 62 | -------------------------------------------------------------------------------- /Core/Shaders/copy_unstable.geom: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout(points) in; 22 | layout(points, max_vertices = 1) out; 23 | 24 | in vec4 vPosition[]; 25 | in vec4 vColor[]; 26 | in vec4 vNormRad[]; 27 | flat in int test[]; 28 | 29 | out vec4 vPosition0; 30 | out vec4 vColor0; 31 | out vec4 vNormRad0; 32 | 33 | void main() 34 | { 35 | if(test[0] > 0) 36 | { 37 | vPosition0 = vPosition[0]; 38 | vColor0 = vColor[0]; 39 | vNormRad0 = vNormRad[0]; 40 | EmitVertex(); 41 | EndPrimitive(); 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /Core/Shaders/data.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec4 vPosition0; 22 | in vec4 vColor0; 23 | in vec4 vNormRad0; 24 | flat in int updateId0; 25 | 26 | layout(location = 0) out vec4 vPosition1; 27 | layout(location = 1) out vec4 vColor1; 28 | layout(location = 2) out vec4 vNormRad1; 29 | 30 | void main() 31 | { 32 | //If we have a point to update in the existing model, store that 33 | if(updateId0 == 1) 34 | { 35 | vPosition1 = vPosition0; 36 | vColor1 = vColor0; 37 | vNormRad1 = vNormRad0; 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /Core/Shaders/data.geom: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout(points) in; 22 | layout(points, max_vertices = 1) out; 23 | 24 | in vec4 vPosition[]; 25 | in vec4 vColor[]; 26 | in vec4 vNormRad[]; 27 | flat in int updateId[]; 28 | 29 | out vec4 vPosition0; 30 | out vec4 vColor0; 31 | out vec4 vNormRad0; 32 | flat out int updateId0; 33 | 34 | void main() 35 | { 36 | //Emit a vertex if either we have an update to store, or a new unstable vertex to store 37 | if(updateId[0] > 0) 38 | { 39 | vPosition0 = vPosition[0]; 40 | vColor0 = vColor[0]; 41 | vNormRad0 = vNormRad[0]; 42 | updateId0 = updateId[0]; 43 | 44 | //This will be -10, -10 (offscreen) for new unstable vertices, so they don't show in the fragment shader 45 | gl_Position = gl_in[0].gl_Position; 46 | 47 | EmitVertex(); 48 | EndPrimitive(); 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /Core/Shaders/depth_bilateral.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec2 texcoord; 22 | 23 | out uint FragColor; 24 | 25 | uniform usampler2D gSampler; 26 | uniform float cols; 27 | uniform float rows; 28 | uniform float maxD; 29 | 30 | void main() 31 | { 32 | uint value = uint(texture(gSampler, texcoord.xy)); 33 | 34 | if(value > uint(maxD * 1000.0f) || value < 300U) 35 | { 36 | FragColor = 0U; 37 | } 38 | else 39 | { 40 | int x = int(texcoord.x * cols); 41 | int y = int(texcoord.y * rows); 42 | 43 | const float sigma_space2_inv_half = 0.024691358; // 0.5 / (sigma_space * sigma_space) 44 | const float sigma_color2_inv_half = 0.000555556; // 0.5 / (sigma_color * sigma_color) 45 | 46 | const int R = 6; 47 | const int D = R * 2 + 1; 48 | 49 | int tx = min(x - D / 2 + D, int(cols)); 50 | int ty = min(y - D / 2 + D, int(rows)); 51 | 52 | float sum1 = 0; 53 | float sum2 = 0; 54 | 55 | for(int cy = max(y - D / 2, 0); cy < ty; ++cy) 56 | { 57 | for(int cx = max(x - D / 2, 0); cx < tx; ++cx) 58 | { 59 | float texX = float(cx) / cols; 60 | float texY = float(cy) / rows; 61 | 62 | uint tmp = uint(texture(gSampler, vec2(texX, texY))); 63 | 64 | float space2 = (float(x) - float(cx)) * (float(x) - float(cx)) + (float(y) - float(cy)) * (float(y) - float(cy)); 65 | float color2 = (float(value) - float(tmp)) * (float(value) - float(tmp)); 66 | 67 | float weight = exp(-(space2 * sigma_space2_inv_half + color2 * sigma_color2_inv_half)); 68 | 69 | sum1 += float(tmp) * weight; 70 | sum2 += weight; 71 | } 72 | } 73 | 74 | FragColor = uint(round(sum1/sum2)); 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /Core/Shaders/depth_metric.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec2 texcoord; 22 | 23 | out float FragColor; 24 | 25 | uniform usampler2D gSampler; 26 | uniform float maxD; 27 | 28 | void main() 29 | { 30 | uint value = uint(texture(gSampler, texcoord.xy)); 31 | 32 | if(value > uint(maxD * 1000.0f) || value < 300U) 33 | { 34 | FragColor = 0U; 35 | } 36 | else 37 | { 38 | FragColor = float(value) / 1000.0f; 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /Core/Shaders/depth_norm.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec2 texcoord; 22 | 23 | out float FragColor; 24 | 25 | uniform usampler2D gSampler; 26 | uniform float minVal; 27 | uniform float maxVal; 28 | 29 | void main() 30 | { 31 | uint value = uint(texture(gSampler, texcoord.xy)); 32 | if(value > uint(minVal) && value < uint(maxVal)) 33 | FragColor = 1.0f - (value / maxVal); 34 | else 35 | FragColor = 0; 36 | } 37 | -------------------------------------------------------------------------------- /Core/Shaders/depth_splat.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | uniform vec4 cam; //cx, cy, fx, fy 22 | uniform float maxDepth; 23 | 24 | in vec4 position; 25 | in vec4 normRad; 26 | 27 | out float FragColor; 28 | 29 | void main() 30 | { 31 | vec3 l = normalize(vec3((vec2(gl_FragCoord) - cam.xy) / cam.zw, 1.0f)); 32 | 33 | vec3 corrected_pos = (dot(position.xyz, normRad.xyz) / dot(l, normRad.xyz)) * l; 34 | 35 | //check if the intersection is inside the surfel 36 | float sqrRad = pow(normRad.w, 2); 37 | vec3 diff = corrected_pos - position.xyz; 38 | 39 | if(dot(diff, diff) > sqrRad) 40 | { 41 | discard; 42 | } 43 | 44 | FragColor = corrected_pos.z; 45 | 46 | gl_FragDepth = (corrected_pos.z / (2 * maxDepth)) + 0.5f; 47 | } 48 | -------------------------------------------------------------------------------- /Core/Shaders/draw_feedback.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec4 vColor; 22 | 23 | out vec4 FragColor; 24 | 25 | void main() 26 | { 27 | FragColor = vColor; 28 | } 29 | -------------------------------------------------------------------------------- /Core/Shaders/draw_feedback.vert: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout (location = 0) in vec4 position; 22 | layout (location = 1) in vec4 color; 23 | layout (location = 2) in vec4 normal; 24 | 25 | uniform mat4 MVP; 26 | uniform mat4 pose; 27 | uniform float threshold; 28 | uniform int colorType; 29 | 30 | out vec4 vColor; 31 | 32 | #include "color.glsl" 33 | 34 | void main() 35 | { 36 | if(position.w > threshold) 37 | { 38 | if(colorType == 1) 39 | { 40 | vColor = vec4(normal.xyz, 1.0); 41 | } 42 | else if(colorType == 2) 43 | { 44 | vColor = vec4(decodeColor(color.x), 1.0); 45 | } 46 | else 47 | { 48 | vColor = vec4((vec3(.5f, .5f, .5f) * abs(dot(normal.xyz, vec3(1.0, 1.0, 1.0)))) + vec3(0.1f, 0.1f, 0.1f), 1.0f); 49 | } 50 | gl_Position = MVP * pose * vec4(position.xyz, 1.0); 51 | } 52 | else 53 | { 54 | gl_Position = vec4(-10, -10, 0, 1); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /Core/Shaders/draw_global_surface.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec3 vColor0; 22 | in vec2 texcoord; 23 | in float radius; 24 | flat in int unstablePoint; 25 | 26 | out vec4 FragColor; 27 | 28 | void main() 29 | { 30 | if(dot(texcoord, texcoord) > 1.0) 31 | discard; 32 | 33 | FragColor = vec4(vColor0, 1.0f); 34 | 35 | if(unstablePoint == 1) 36 | { 37 | gl_FragDepth = gl_FragCoord.z + radius; 38 | } 39 | else 40 | { 41 | gl_FragDepth = gl_FragCoord.z; 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /Core/Shaders/draw_global_surface.geom: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout(points) in; 22 | layout(triangle_strip, max_vertices = 4) out; 23 | 24 | uniform float threshold; 25 | uniform float signMult; 26 | 27 | in vec4 vColor[]; 28 | in vec4 vPosition[]; 29 | in vec4 vNormRad[]; 30 | in mat4 vMVP[]; 31 | in int colorType0[]; 32 | in int drawWindow0[]; 33 | in int vTime[]; 34 | in int timeDelta0[]; 35 | 36 | out vec3 vColor0; 37 | out vec3 v; 38 | out vec3 n; 39 | out vec2 texcoord; 40 | out float radius; 41 | flat out int unstablePoint; 42 | 43 | #include "color.glsl" 44 | 45 | void main() 46 | { 47 | if(colorType0[0] != -1) 48 | { 49 | if(colorType0[0] == 1) 50 | { 51 | vColor0 = vNormRad[0].xyz; 52 | } 53 | else if(colorType0[0] == 2) 54 | { 55 | vColor0 = decodeColor(vColor[0].x); 56 | } 57 | else if(colorType0[0] == 3) 58 | { 59 | vColor0 = vec3(vColor[0].z / float(vTime[0])); 60 | 61 | float minimum = 1.0f; 62 | float maximum = float(vTime[0]); 63 | 64 | float ratio = 2 * (vColor[0].z - minimum) / (maximum - minimum); 65 | vColor0.x = max(0, (1 - ratio)); 66 | vColor0.y = max(0, (ratio - 1)); 67 | vColor0.z = 1.0f - vColor0.x - vColor0.y; 68 | 69 | vColor0.xyz *= abs(dot(vNormRad[0].xyz, vec3(1.0, 1.0, 1.0))) + vec3(0.1f, 0.1f, 0.1f); 70 | } 71 | else 72 | { 73 | vColor0 = (vec3(.5f, .5f, .5f) * abs(dot(vNormRad[0].xyz, vec3(1.0, 1.0, 1.0)))) + vec3(0.1f, 0.1f, 0.1f); 74 | } 75 | 76 | if(drawWindow0[0] == 1 && vTime[0] - vColor[0].w > timeDelta0[0]) 77 | { 78 | vColor0 *= 0.25; 79 | } 80 | 81 | unstablePoint = (vPosition[0].w <= threshold ? 1 : 0); 82 | 83 | radius = vNormRad[0].w; 84 | 85 | vec3 x = normalize(vec3((vNormRad[0].y - vNormRad[0].z), -vNormRad[0].x, vNormRad[0].x)) * vNormRad[0].w * 1.41421356; 86 | 87 | vec3 y = cross(vNormRad[0].xyz, x); 88 | 89 | n = signMult * vNormRad[0].xyz; 90 | 91 | texcoord = vec2(-1.0, -1.0); 92 | gl_Position = vMVP[0] * vec4(vPosition[0].xyz + x, 1.0); 93 | v = vPosition[0].xyz + x; 94 | EmitVertex(); 95 | 96 | texcoord = vec2(1.0, -1.0); 97 | gl_Position = vMVP[0] * vec4(vPosition[0].xyz + y, 1.0); 98 | v = vPosition[0].xyz + y; 99 | EmitVertex(); 100 | 101 | texcoord = vec2(-1.0, 1.0); 102 | gl_Position = vMVP[0] * vec4(vPosition[0].xyz - y, 1.0); 103 | v = vPosition[0].xyz - y; 104 | EmitVertex(); 105 | 106 | texcoord = vec2(1.0, 1.0); 107 | gl_Position = vMVP[0] * vec4(vPosition[0].xyz - x, 1.0); 108 | v = vPosition[0].xyz - x; 109 | EmitVertex(); 110 | EndPrimitive(); 111 | } 112 | } 113 | -------------------------------------------------------------------------------- /Core/Shaders/draw_global_surface.vert: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout (location = 0) in vec4 position; 22 | layout (location = 1) in vec4 color; 23 | layout (location = 2) in vec4 normal; 24 | 25 | uniform mat4 MVP; 26 | uniform float threshold; 27 | uniform int colorType; 28 | uniform int unstable; 29 | uniform int drawWindow; 30 | uniform int time; 31 | uniform int timeDelta; 32 | 33 | out vec4 vColor; 34 | out vec4 vPosition; 35 | out vec4 vNormRad; 36 | out mat4 vMVP; 37 | out int vTime; 38 | out int colorType0; 39 | out int drawWindow0; 40 | out int timeDelta0; 41 | 42 | void main() 43 | { 44 | if(position.w > threshold || unstable == 1) 45 | { 46 | colorType0 = colorType; 47 | drawWindow0 = drawWindow; 48 | vColor = color; 49 | vPosition = position; 50 | vNormRad = normal; 51 | vMVP = MVP; 52 | vTime = time; 53 | timeDelta0 = timeDelta; 54 | gl_Position = MVP * vec4(position.xyz, 1.0); 55 | } 56 | else 57 | { 58 | colorType0 = -1; 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /Core/Shaders/draw_global_surface_phong.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec3 n; 22 | in vec3 v; 23 | in vec3 vColor0; 24 | in vec2 texcoord; 25 | in float radius; 26 | flat in int unstablePoint; 27 | 28 | out vec4 FragColor; 29 | 30 | uniform vec3 lightpos; 31 | 32 | void main() 33 | { 34 | if(dot(texcoord, texcoord) > 1.0) 35 | discard; 36 | 37 | vec4 diffuse = vec4(0.0); 38 | vec4 specular = vec4(0.0); 39 | 40 | // ambient term 41 | vec4 ambient = vec4(0.3 * vColor0, 1); 42 | 43 | // diffuse color 44 | vec4 kd = vec4(vColor0, 1.0); 45 | 46 | // specular color 47 | vec4 ks = vec4(1.0, 1.0, 1.0, 1.0); 48 | 49 | // diffuse term 50 | vec3 lightDir = normalize(lightpos - v); 51 | float NdotL = dot(n, lightDir); 52 | 53 | if (NdotL > 0.0) 54 | diffuse = kd * NdotL; 55 | 56 | // specular term 57 | vec3 rVector = normalize(2.0 * n * dot(n, lightDir) - lightDir); 58 | vec3 viewVector = normalize(-v); 59 | float RdotV = dot(rVector, viewVector); 60 | 61 | if (RdotV > 0.0) 62 | specular = ks * pow(RdotV, 32); 63 | 64 | FragColor = ambient + diffuse + specular; 65 | 66 | if(unstablePoint == 1) 67 | { 68 | gl_FragDepth = gl_FragCoord.z + radius; 69 | } 70 | else 71 | { 72 | gl_FragDepth = gl_FragCoord.z; 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /Core/Shaders/empty.vert: -------------------------------------------------------------------------------- 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 | #version 330 20 | 21 | void main() 22 | { 23 | } 24 | -------------------------------------------------------------------------------- /Core/Shaders/fill_normal.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec2 texcoord; 22 | 23 | out vec4 FragColor; 24 | 25 | uniform sampler2D eSampler; 26 | uniform usampler2D rSampler; 27 | uniform vec4 cam; //cx, cy, 1/fx, 1/fy 28 | uniform float cols; 29 | uniform float rows; 30 | uniform int passthrough; 31 | 32 | #include "geometry.glsl" 33 | 34 | void main() 35 | { 36 | float halfPixX = 0.5 * (1.0 / cols); 37 | float halfPixY = 0.5 * (1.0 / rows); 38 | 39 | vec4 sample = textureLod(eSampler, texcoord, 0.0); 40 | 41 | if(sample.z == 0 || passthrough == 1) 42 | { 43 | vec4 vPos = vec4(getVertex(texcoord, int(texcoord.x * cols), int(texcoord.y * rows), cam, rSampler), 1); 44 | FragColor = vec4(getNormal(vPos.xyz, texcoord, int(texcoord.x * cols), int(texcoord.y * rows), cam, rSampler), 1); 45 | } 46 | else 47 | { 48 | FragColor = sample; 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /Core/Shaders/fill_rgb.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec2 texcoord; 22 | 23 | out vec4 FragColor; 24 | 25 | uniform sampler2D eSampler; 26 | uniform sampler2D rSampler; 27 | uniform int passthrough; 28 | 29 | void main() 30 | { 31 | vec4 sample = texture2D(eSampler, texcoord.xy); 32 | 33 | if(sample.x + sample.y + sample.z == 0 || passthrough == 1) 34 | FragColor = texture2D(rSampler, texcoord.xy); 35 | else 36 | FragColor = sample; 37 | } 38 | -------------------------------------------------------------------------------- /Core/Shaders/fill_vertex.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec2 texcoord; 22 | 23 | out vec4 FragColor; 24 | 25 | uniform sampler2D eSampler; 26 | uniform usampler2D rSampler; 27 | uniform vec4 cam; //cx, cy, 1/fx, 1/fy 28 | uniform float cols; 29 | uniform float rows; 30 | uniform int passthrough; 31 | 32 | vec3 getVertex(int x, int y, float z) 33 | { 34 | return vec3((x - cam.x) * z * cam.z, (y - cam.y) * z * cam.w, z); 35 | } 36 | 37 | void main() 38 | { 39 | float halfPixX = 0.5 * (1.0 / cols); 40 | float halfPixY = 0.5 * (1.0 / rows); 41 | 42 | vec4 sample = textureLod(eSampler, texcoord, 0.0); 43 | 44 | if(sample.z == 0 || passthrough == 1) 45 | { 46 | float z = float(textureLod(rSampler, texcoord, 0.0)) / 1000.0f; 47 | FragColor = vec4(getVertex(int(texcoord.x * cols), int(texcoord.y * rows), z), 1); 48 | } 49 | else 50 | { 51 | FragColor = sample; 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /Core/Shaders/fxaa.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | #define FXAA_REDUCE_MIN (1.0/ 128.0) 22 | #define FXAA_REDUCE_MUL (1.0 / 8.0) 23 | #define FXAA_SPAN_MAX 8.0 24 | 25 | uniform sampler2D tex; 26 | uniform vec2 resolution; 27 | 28 | in vec2 texcoord; 29 | 30 | out vec4 FragColor; 31 | 32 | void main() 33 | { 34 | vec4 color; 35 | 36 | vec2 inverseVP = 1.0 / resolution.xy; 37 | vec2 v_rgbNW = texcoord + (vec2(-1.0, -1.0) * inverseVP); 38 | vec2 v_rgbNE = texcoord + (vec2(1.0, -1.0) * inverseVP); 39 | vec2 v_rgbSW = texcoord + (vec2(-1.0, 1.0) * inverseVP); 40 | vec2 v_rgbSE = texcoord + (vec2(1.0, 1.0) * inverseVP); 41 | 42 | vec2 v_rgbN = texcoord + (vec2(-1.0, 0.0) * inverseVP); 43 | vec2 v_rgbE = texcoord + (vec2(1.0, 0.0) * inverseVP); 44 | vec2 v_rgbW = texcoord + (vec2(0.0, -1.0) * inverseVP); 45 | vec2 v_rgbS = texcoord + (vec2(0.0, 1.0) * inverseVP); 46 | 47 | vec2 v_rgbM = texcoord; 48 | 49 | vec3 rgbNW = texture2D(tex, v_rgbNW).xyz; 50 | vec3 rgbNE = texture2D(tex, v_rgbNE).xyz; 51 | vec3 rgbSW = texture2D(tex, v_rgbSW).xyz; 52 | vec3 rgbSE = texture2D(tex, v_rgbSE).xyz; 53 | 54 | vec3 rgbN = texture2D(tex, v_rgbN).xyz; 55 | vec3 rgbE = texture2D(tex, v_rgbE).xyz; 56 | vec3 rgbW = texture2D(tex, v_rgbW).xyz; 57 | vec3 rgbS = texture2D(tex, v_rgbS).xyz; 58 | 59 | vec3 rgbM = texture2D(tex, v_rgbM).xyz; 60 | vec3 luma = vec3(0.299, 0.587, 0.114); 61 | float lumaNW = dot(rgbNW, luma); 62 | float lumaNE = dot(rgbNE, luma); 63 | float lumaSW = dot(rgbSW, luma); 64 | float lumaSE = dot(rgbSE, luma); 65 | float lumaM = dot(rgbM, luma); 66 | float lumaMin = min(lumaM, min(min(lumaNW, lumaNE), min(lumaSW, lumaSE))); 67 | float lumaMax = max(lumaM, max(max(lumaNW, lumaNE), max(lumaSW, lumaSE))); 68 | 69 | vec2 dir; 70 | dir.x = -((lumaNW + lumaNE) - (lumaSW + lumaSE)); 71 | dir.y = ((lumaNW + lumaSW) - (lumaNE + lumaSE)); 72 | 73 | float dirReduce = max((lumaNW + lumaNE + lumaSW + lumaSE) * (0.25 * FXAA_REDUCE_MUL), FXAA_REDUCE_MIN); 74 | 75 | float rcpDirMin = 1.0 / (min(abs(dir.x), abs(dir.y)) + dirReduce); 76 | 77 | dir = min(vec2(FXAA_SPAN_MAX, FXAA_SPAN_MAX), max(vec2(-FXAA_SPAN_MAX, -FXAA_SPAN_MAX), dir * rcpDirMin)) * inverseVP; 78 | 79 | vec3 rgbA = 0.5 * (texture2D(tex, texcoord + dir * (1.0 / 3.0 - 0.5)).xyz + texture2D(tex, texcoord + dir * (2.0 / 3.0 - 0.5)).xyz); 80 | 81 | vec3 rgbB = rgbA * 0.5 + 0.25 * (texture2D(tex, texcoord + dir * -0.5).xyz + texture2D(tex, texcoord + dir * 0.5).xyz); 82 | 83 | float lumaB = dot(rgbB, luma); 84 | 85 | if ((lumaB < lumaMin) || (lumaB > lumaMax)) 86 | FragColor = vec4(rgbA, 1); 87 | else 88 | FragColor = vec4(rgbB, 1); 89 | } 90 | -------------------------------------------------------------------------------- /Core/Shaders/geometry.glsl: -------------------------------------------------------------------------------- 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 | //Central difference on floating point depth maps 20 | //Cam is //cx, cy, 1 / fx, 1 / fy 21 | vec3 getVertex(vec2 texCoord, float x, float y, vec4 cam, sampler2D depth) 22 | { 23 | float z = float(textureLod(depth, texCoord, 0.0)); 24 | return vec3((x - cam.x) * z * cam.z, (y - cam.y) * z * cam.w, z); 25 | } 26 | 27 | //Cam is //cx, cy, 1 / fx, 1 / fy 28 | vec3 getNormal(vec3 vPosition, vec2 texCoord, float x, float y, vec4 cam, sampler2D depth) 29 | { 30 | vec3 vPosition_xf = getVertex(vec2(texCoord.x + (1.0 / cols), texCoord.y), x + 1, y, cam, depth); 31 | vec3 vPosition_xb = getVertex(vec2(texCoord.x - (1.0 / cols), texCoord.y), x - 1, y, cam, depth); 32 | 33 | vec3 vPosition_yf = getVertex(vec2(texCoord.x, texCoord.y + (1.0 / rows)), x, y + 1, cam, depth); 34 | vec3 vPosition_yb = getVertex(vec2(texCoord.x, texCoord.y - (1.0 / rows)), x, y - 1, cam, depth); 35 | 36 | vec3 del_x = ((vPosition_xb + vPosition) / 2) - ((vPosition_xf + vPosition) / 2); 37 | vec3 del_y = ((vPosition_yb + vPosition) / 2) - ((vPosition_yf + vPosition) / 2); 38 | 39 | return normalize(cross(del_x, del_y)); 40 | } 41 | 42 | //Forward difference on raw depth maps still in ushort mm 43 | //Cam is //cx, cy, 1 / fx, 1 / fy 44 | vec3 getVertex(vec2 texcoord, int x, int y, vec4 cam, usampler2D depth) 45 | { 46 | float z = float(textureLod(depth, texcoord, 0.0)) / 1000.0f; 47 | return vec3((x - cam.x) * z * cam.z, (y - cam.y) * z * cam.w, z); 48 | } 49 | 50 | //Cam is //cx, cy, 1 / fx, 1 / fy 51 | vec3 getNormal(vec3 vPosition, vec2 texcoord, int x, int y, vec4 cam, usampler2D depth) 52 | { 53 | vec3 vPosition_x = getVertex(vec2(texcoord.x + (1.0 / cols), texcoord.y), x + 1, y, cam, depth); 54 | vec3 vPosition_y = getVertex(vec2(texcoord.x, texcoord.y + (1.0 / rows)), x, y + 1, cam, depth); 55 | 56 | vec3 del_x = vPosition_x - vPosition; 57 | vec3 del_y = vPosition_y - vPosition; 58 | 59 | return normalize(cross(del_x, del_y)); 60 | } 61 | -------------------------------------------------------------------------------- /Core/Shaders/index_map.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec4 vPosition0; 22 | in vec4 vColorTime0; 23 | in vec4 vNormRad0; 24 | flat in int vertexId; 25 | 26 | layout(location = 0) out int FragColor; 27 | layout(location = 1) out vec4 vPosition1; 28 | layout(location = 2) out vec4 vColorTime1; 29 | layout(location = 3) out vec4 vNormRad1; 30 | 31 | void main() 32 | { 33 | vPosition1 = vPosition0; 34 | vColorTime1 = vColorTime0; 35 | vNormRad1 = vNormRad0; 36 | FragColor = vertexId; 37 | } 38 | -------------------------------------------------------------------------------- /Core/Shaders/index_map.vert: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout (location = 0) in vec4 vPosition; 22 | layout (location = 1) in vec4 vColorTime; 23 | layout (location = 2) in vec4 vNormRad; 24 | 25 | out vec4 vPosition0; 26 | out vec4 vColorTime0; 27 | out vec4 vNormRad0; 28 | flat out int vertexId; 29 | 30 | uniform mat4 t_inv; 31 | uniform vec4 cam; //cx, cy, fx, fy 32 | uniform float cols; 33 | uniform float rows; 34 | uniform float maxDepth; 35 | uniform int time; 36 | uniform int timeDelta; 37 | 38 | void main() 39 | { 40 | vec4 vPosHome = t_inv * vec4(vPosition.xyz, 1.0); 41 | 42 | float x = 0; 43 | float y = 0; 44 | 45 | if(vPosHome.z > maxDepth || vPosHome.z < 0 || time - vColorTime.w > timeDelta) 46 | { 47 | x = -10; 48 | y = -10; 49 | vertexId = 0; 50 | } 51 | else 52 | { 53 | x = ((((cam.z * vPosHome.x) / vPosHome.z) + cam.x) - (cols * 0.5)) / (cols * 0.5); 54 | y = ((((cam.w * vPosHome.y) / vPosHome.z) + cam.y) - (rows * 0.5)) / (rows * 0.5); 55 | vertexId = gl_VertexID; 56 | } 57 | 58 | gl_Position = vec4(x, y, vPosHome.z / maxDepth, 1.0); 59 | 60 | vPosition0 = vec4(vPosHome.xyz, vPosition.w); 61 | vColorTime0 = vColorTime; 62 | vNormRad0 = vec4(normalize(mat3(t_inv) * vNormRad.xyz), vNormRad.w); 63 | } 64 | -------------------------------------------------------------------------------- /Core/Shaders/init_unstable.vert: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout (location = 0) in vec4 vPosition; 22 | layout (location = 1) in vec4 vColor; 23 | layout (location = 2) in vec4 vNormRad; 24 | 25 | out vec4 vPosition0; 26 | out vec4 vColor0; 27 | out vec4 vNormRad0; 28 | 29 | void main() 30 | { 31 | vPosition0 = vPosition; 32 | vColor0 = vColor; 33 | vColor0.y = 0; //Unused 34 | vColor0.z = 1; //This sets the vertex's initialisation time 35 | vNormRad0 = vNormRad; 36 | } 37 | -------------------------------------------------------------------------------- /Core/Shaders/quad.geom: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout(points) in; 22 | layout(triangle_strip, max_vertices = 4) out; 23 | 24 | out vec2 texcoord; 25 | 26 | void main() 27 | { 28 | gl_Position = vec4(1.0, 1.0, 0.0, 1.0); 29 | texcoord = vec2(1.0, 1.0); 30 | EmitVertex(); 31 | 32 | gl_Position = vec4(-1.0, 1.0, 0.0, 1.0); 33 | texcoord = vec2(0.0, 1.0); 34 | EmitVertex(); 35 | 36 | gl_Position = vec4(1.0,-1.0, 0.0, 1.0); 37 | texcoord = vec2(1.0, 0.0); 38 | EmitVertex(); 39 | 40 | gl_Position = vec4(-1.0,-1.0, 0.0, 1.0); 41 | texcoord = vec2(0.0, 0.0); 42 | EmitVertex(); 43 | 44 | EndPrimitive(); 45 | } 46 | -------------------------------------------------------------------------------- /Core/Shaders/resize.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | in vec2 texcoord; 22 | 23 | out vec4 FragColor; 24 | 25 | uniform sampler2D eSampler; 26 | 27 | void main() 28 | { 29 | FragColor = texture2D(eSampler, texcoord.xy); 30 | } 31 | -------------------------------------------------------------------------------- /Core/Shaders/sample.geom: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout(points) in; 22 | layout(points, max_vertices = 1) out; 23 | 24 | in vec4 vPosition0[]; 25 | in vec4 vColorTime0[]; 26 | in vec4 vNormRad0[]; 27 | flat in int id[]; 28 | 29 | out vec4 vData; 30 | 31 | void main() 32 | { 33 | if(id[0] % 5000 == 0) 34 | { 35 | vData.xyz = vPosition0[0].xyz; 36 | vData.w = vColorTime0[0].z; 37 | EmitVertex(); 38 | EndPrimitive(); 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /Core/Shaders/sample.vert: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout (location = 0) in vec4 vPosition; 22 | layout (location = 1) in vec4 vColorTime; 23 | layout (location = 2) in vec4 vNormRad; 24 | 25 | out vec4 vPosition0; 26 | out vec4 vColorTime0; 27 | out vec4 vNormRad0; 28 | flat out int id; 29 | 30 | void main() 31 | { 32 | vPosition0 = vPosition; 33 | vColorTime0 = vColorTime; 34 | vNormRad0 = vNormRad; 35 | id = gl_VertexID; 36 | } 37 | -------------------------------------------------------------------------------- /Core/Shaders/splat.vert: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout (location = 0) in vec4 vPosition; 22 | layout (location = 1) in vec4 vColor; 23 | layout (location = 2) in vec4 vNormRad; 24 | 25 | uniform mat4 t_inv; 26 | uniform vec4 cam; //cx, cy, fx, fy 27 | uniform float cols; 28 | uniform float rows; 29 | uniform float maxDepth; 30 | uniform float confThreshold; 31 | uniform int time; 32 | uniform int maxTime; 33 | uniform int timeDelta; 34 | 35 | out vec4 position; 36 | out vec4 normRad; 37 | out vec4 colTime; 38 | 39 | vec3 projectPoint(vec3 p) 40 | { 41 | return vec3(((((cam.z * p.x) / p.z) + cam.x) - (cols * 0.5)) / (cols * 0.5), 42 | ((((cam.w * p.y) / p.z) + cam.y) - (rows * 0.5)) / (rows * 0.5), 43 | p.z / maxDepth); 44 | } 45 | 46 | vec3 projectPointImage(vec3 p) 47 | { 48 | return vec3(((cam.z * p.x) / p.z) + cam.x, 49 | ((cam.w * p.y) / p.z) + cam.y, 50 | p.z); 51 | } 52 | 53 | void main() 54 | { 55 | vec4 vPosHome = t_inv * vec4(vPosition.xyz, 1.0); 56 | 57 | if(vPosHome.z > maxDepth || vPosHome.z < 0 || vPosition.w < confThreshold || time - vColor.w > timeDelta || vColor.w > maxTime) 58 | { 59 | gl_Position = vec4(1000.0f, 1000.0f, 1000.0f, 1000.0f); 60 | gl_PointSize = 0; 61 | } 62 | else 63 | { 64 | gl_Position = vec4(projectPoint(vPosHome.xyz), 1.0); 65 | 66 | colTime = vColor; 67 | position = vec4(vPosHome.xyz, vPosition.w); 68 | normRad = vec4(normalize(mat3(t_inv) * vNormRad.xyz), vNormRad.w); 69 | 70 | vec3 x1 = normalize(vec3((normRad.y - normRad.z), -normRad.x, normRad.x)) * normRad.w * 1.41421356; 71 | 72 | vec3 y1 = cross(normRad.xyz, x1); 73 | 74 | vec4 proj1 = vec4(projectPointImage(vPosHome.xyz + x1), 1.0); 75 | vec4 proj2 = vec4(projectPointImage(vPosHome.xyz + y1), 1.0); 76 | vec4 proj3 = vec4(projectPointImage(vPosHome.xyz - y1), 1.0); 77 | vec4 proj4 = vec4(projectPointImage(vPosHome.xyz - x1), 1.0); 78 | 79 | vec2 xs = vec2(min(proj1.x, min(proj2.x, min(proj3.x, proj4.x))), max(proj1.x, max(proj2.x, max(proj3.x, proj4.x)))); 80 | vec2 ys = vec2(min(proj1.y, min(proj2.y, min(proj3.y, proj4.y))), max(proj1.y, max(proj2.y, max(proj3.y, proj4.y)))); 81 | 82 | float xDiff = abs(xs.y - xs.x); 83 | float yDiff = abs(ys.y - ys.x); 84 | 85 | gl_PointSize = max(0, max(xDiff, yDiff)); 86 | } 87 | } 88 | -------------------------------------------------------------------------------- /Core/Shaders/surfels.glsl: -------------------------------------------------------------------------------- 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 | float getRadius(float depth, float norm_z) 20 | { 21 | float meanFocal = ((1.0 / abs(cam.z)) + (1.0 / abs(cam.w))) / 2.0; 22 | 23 | const float sqrt2 = 1.41421356237f; 24 | 25 | float radius = (depth / meanFocal) * sqrt2; 26 | 27 | float radius_n = radius; 28 | 29 | radius_n = radius_n / abs(norm_z); 30 | 31 | radius_n = min(2.0f * radius, radius_n); 32 | 33 | return radius_n; 34 | } 35 | 36 | float confidence(float x, float y, float weighting) 37 | { 38 | const float maxRadDist = 400; //sqrt((width * 0.5)^2 + (height * 0.5)^2) 39 | const float twoSigmaSquared = 0.72; //2*(0.6^2) from paper 40 | 41 | vec2 pixelPosCentered = vec2(x, y) - cam.xy; 42 | 43 | float radialDist = sqrt(dot(pixelPosCentered, pixelPosCentered)) / maxRadDist; 44 | 45 | return exp((-(radialDist * radialDist) / twoSigmaSquared)) * weighting; 46 | } 47 | -------------------------------------------------------------------------------- /Core/Shaders/update.vert: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout (location = 0) in vec4 vPosition; 22 | layout (location = 1) in vec4 vColor; 23 | layout (location = 2) in vec4 vNormRad; 24 | 25 | out vec4 vPosition0; 26 | out vec4 vColor0; 27 | out vec4 vNormRad0; 28 | uniform float texDim; 29 | uniform int time; 30 | 31 | uniform sampler2D vertSamp; 32 | uniform sampler2D colorSamp; 33 | uniform sampler2D normSamp; 34 | 35 | #include "color.glsl" 36 | 37 | void main() 38 | { 39 | int intY = gl_VertexID / int(texDim); 40 | int intX = gl_VertexID - (intY * int(texDim)); 41 | 42 | float halfPixel = 0.5 * (1.0f / texDim); 43 | float y = (float(intY) / texDim) + halfPixel; 44 | float x = (float(intX) / texDim) + halfPixel; 45 | 46 | vec4 newColor = textureLod(colorSamp, vec2(x, y), 0); 47 | 48 | //Do averaging here 49 | if(newColor.w == -1) 50 | { 51 | vec4 newPos = textureLod(vertSamp, vec2(x, y), 0); 52 | vec4 newNorm = textureLod(normSamp, vec2(x, y), 0); 53 | 54 | float c_k = vPosition.w; 55 | vec3 v_k = vPosition.xyz; 56 | 57 | float a = newPos.w; 58 | vec3 v_g = newPos.xyz; 59 | 60 | if(newNorm.w < (1.0 + 0.5) * vNormRad.w) 61 | { 62 | vPosition0 = vec4(((c_k * v_k) + (a * v_g)) / (c_k + a), c_k + a); 63 | 64 | vec3 oldCol = decodeColor(vColor.x); 65 | vec3 newCol = decodeColor(newColor.x); 66 | 67 | vec3 avgColor = ((c_k * oldCol.xyz) + (a * newCol.xyz)) / (c_k + a); 68 | 69 | vColor0 = vec4(encodeColor(avgColor), vColor.y, vColor.z, time); 70 | 71 | vNormRad0 = ((c_k * vNormRad) + (a * newNorm)) / (c_k + a); 72 | 73 | vNormRad0.xyz = normalize(vNormRad0.xyz); 74 | } 75 | else 76 | { 77 | vPosition0 = vPosition; 78 | vColor0 = vColor; 79 | vNormRad0 = vNormRad; 80 | 81 | vPosition0.w = c_k + a; 82 | vColor0.w = time; 83 | } 84 | } 85 | else 86 | { 87 | //This point isn't being updated, so just transfer it 88 | vPosition0 = vPosition; 89 | vColor0 = vColor; 90 | vNormRad0 = vNormRad; 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /Core/Shaders/vertex_feedback.geom: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout(points) in; 22 | layout(points, max_vertices = 1) out; 23 | 24 | in vec4 vPosition[]; 25 | in vec4 vColor[]; 26 | in vec4 vNormRad[]; 27 | in float zVal[]; 28 | 29 | out vec4 vPosition0; 30 | out vec4 vColor0; 31 | out vec4 vNormRad0; 32 | 33 | void main() 34 | { 35 | if(zVal[0] > 0) 36 | { 37 | vPosition0 = vPosition[0]; 38 | vColor0 = vColor[0]; 39 | vNormRad0 = vNormRad[0]; 40 | EmitVertex(); 41 | EndPrimitive(); 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /Core/Shaders/vertex_feedback.vert: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | layout (location = 0) in vec2 texcoord; 22 | 23 | out vec4 vPosition; 24 | out vec4 vColor; 25 | out vec4 vNormRad; 26 | out float zVal; 27 | 28 | uniform sampler2D gSampler; 29 | uniform sampler2D cSampler; 30 | uniform vec4 cam; //cx, cy, 1/fx, 1/fy 31 | uniform float cols; 32 | uniform float rows; 33 | uniform int time; 34 | uniform float maxDepth; 35 | 36 | #include "surfels.glsl" 37 | #include "color.glsl" 38 | #include "geometry.glsl" 39 | 40 | void main() 41 | { 42 | //Should be guaranteed to be in bounds 43 | float x = texcoord.x * cols; 44 | float y = texcoord.y * rows; 45 | 46 | vPosition = vec4(getVertex(texcoord.xy, x, y, cam, gSampler), 1); 47 | vColor = textureLod(cSampler, texcoord.xy, 0.0); 48 | 49 | vec3 vNormLocal = getNormal(vPosition.xyz, texcoord.xy, x, y, cam, gSampler); 50 | vNormRad = vec4(vNormLocal, getRadius(vPosition.z, vNormLocal.z)); 51 | 52 | if(vPosition.z <= 0 || vPosition.z > maxDepth) 53 | { 54 | zVal = 0; 55 | } 56 | else 57 | { 58 | zVal = vPosition.z; 59 | } 60 | 61 | vPosition.w = confidence(x, y, 1.0f); 62 | 63 | vColor.x = encodeColor(vColor.xyz); 64 | 65 | vColor.y = 0; 66 | //Timestamp 67 | vColor.w = float(time); 68 | } 69 | -------------------------------------------------------------------------------- /Core/Shaders/visualise_textures.frag: -------------------------------------------------------------------------------- 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 | #version 330 core 20 | 21 | uniform sampler2D texVerts; 22 | uniform float maxDepth; 23 | 24 | in vec2 texcoord; 25 | 26 | out vec4 FragColor; 27 | 28 | void main() 29 | { 30 | vec4 vertex = texture2D(texVerts, texcoord); 31 | 32 | if(vertex.z > maxDepth || vertex.z <= 0) 33 | { 34 | discard; 35 | } 36 | else 37 | { 38 | FragColor = 1.0f - vec4(vertex.z / maxDepth); 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /Core/Utils/CholeskyDecomp.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "CholeskyDecomp.h" 21 | 22 | CholeskyDecomp::CholeskyDecomp() : L(0) { 23 | cholmod_start(&Common); 24 | } 25 | 26 | CholeskyDecomp::~CholeskyDecomp() { 27 | cholmod_finish(&Common); 28 | } 29 | 30 | void CholeskyDecomp::freeFactor() { 31 | assert(L); 32 | cholmod_free_factor(&L, &Common); 33 | L = 0; 34 | } 35 | 36 | Eigen::VectorXd CholeskyDecomp::solve( 37 | const Jacobian& jacobian, 38 | const Eigen::VectorXd& residual, 39 | const bool firstRun) { 40 | cholmod_sparse* At = cholmod_allocate_sparse( 41 | jacobian.cols(), 42 | jacobian.rows.size(), 43 | jacobian.nonZero(), 44 | true, 45 | true, 46 | 0, 47 | CHOLMOD_REAL, 48 | &Common); 49 | 50 | int* p = (int*)At->p; 51 | int* i = (int*)At->i; 52 | double* x = (double*)At->x; 53 | int n = 0; 54 | *p = n; 55 | 56 | for (size_t r = 0; r < jacobian.rows.size(); r++) { 57 | memcpy(i, jacobian.rows.at(r)->indices, jacobian.rows.at(r)->nonZeros() * sizeof(int)); 58 | memcpy(x, jacobian.rows.at(r)->vals, jacobian.rows.at(r)->nonZeros() * sizeof(double)); 59 | 60 | i += jacobian.rows.at(r)->nonZeros(); 61 | x += jacobian.rows.at(r)->nonZeros(); 62 | n += jacobian.rows.at(r)->nonZeros(); 63 | p++; 64 | *p = n; 65 | } 66 | 67 | if (firstRun) { 68 | assert(!L); 69 | 70 | L = cholmod_analyze(At, &Common); 71 | } 72 | 73 | cholmod_factor* L_factor = cholmod_copy_factor(L, &Common); 74 | 75 | cholmod_factorize(At, L_factor, &Common); 76 | 77 | cholmod_change_factor(CHOLMOD_REAL, true, false, true, true, L_factor, &Common); 78 | 79 | cholmod_dense* Arhs = cholmod_zeros(At->ncol, 1, CHOLMOD_REAL, &Common); 80 | 81 | memcpy(Arhs->x, residual.data(), At->ncol * sizeof(double)); 82 | 83 | cholmod_dense* Atb = cholmod_zeros(At->nrow, 1, CHOLMOD_REAL, &Common); 84 | 85 | double alpha[2] = {1., 0.}; 86 | double beta[2] = {0., 0.}; 87 | 88 | cholmod_sdmult(At, 0, alpha, beta, Arhs, Atb, &Common); 89 | 90 | cholmod_dense* Atb_perm = cholmod_solve(CHOLMOD_P, L_factor, Atb, &Common); 91 | 92 | cholmod_dense* rhs = cholmod_solve(CHOLMOD_L, L_factor, Atb_perm, &Common); 93 | 94 | cholmod_dense* delta_cm = cholmod_solve(CHOLMOD_Lt, L_factor, rhs, &Common); 95 | 96 | Eigen::VectorXd delta(rhs->nrow); 97 | 98 | for (size_t i = 0; i < At->nrow; i++) { 99 | delta(((int*)L_factor->Perm)[i]) = ((double*)delta_cm->x)[i]; 100 | } 101 | 102 | cholmod_free_dense(&delta_cm, &Common); 103 | cholmod_free_dense(&Atb_perm, &Common); 104 | cholmod_free_dense(&Atb, &Common); 105 | cholmod_free_dense(&Arhs, &Common); 106 | cholmod_free_sparse(&At, &Common); 107 | cholmod_free_dense(&rhs, &Common); 108 | cholmod_free_factor(&L_factor, &Common); 109 | 110 | return delta; 111 | } 112 | -------------------------------------------------------------------------------- /Core/Utils/CholeskyDecomp.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef UTILS_CHOLESKYDECOMP_H_ 21 | #define UTILS_CHOLESKYDECOMP_H_ 22 | 23 | #include 24 | #include 25 | 26 | #include "Jacobian.h" 27 | 28 | class CholeskyDecomp { 29 | public: 30 | CholeskyDecomp(); 31 | virtual ~CholeskyDecomp(); 32 | 33 | void freeFactor(); 34 | 35 | Eigen::VectorXd 36 | solve(const Jacobian& jacobian, const Eigen::VectorXd& residual, const bool firstRun); 37 | 38 | private: 39 | cholmod_common Common; 40 | cholmod_factor* L; 41 | }; 42 | 43 | #endif /* UTILS_CHOLESKYDECOMP_H_ */ 44 | -------------------------------------------------------------------------------- /Core/Utils/DeformationGraph.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef DEFORMATIONGRAPH_H_ 21 | #define DEFORMATIONGRAPH_H_ 22 | 23 | #include 24 | #include 25 | 26 | #include "GraphNode.h" 27 | #include "Jacobian.h" 28 | #include "Stopwatch.h" 29 | 30 | /** 31 | * This is basically and object-oriented type approach. Using an array based approach would be 32 | * faster... 33 | */ 34 | 35 | class CholeskyDecomp; 36 | 37 | class DeformationGraph { 38 | public: 39 | DeformationGraph(int k, std::vector* sourceVertices); 40 | virtual ~DeformationGraph(); 41 | 42 | void initialiseGraph( 43 | std::vector* customGraph, 44 | std::vector* graphTimeMap); 45 | 46 | void appendVertices(std::vector* vertexTimeMap, uint32_t originalPointEnd); 47 | 48 | // This clears the pose map... 49 | void setPosesSeq(std::vector* poseTimeMap, const std::vector& T_wcs); 50 | 51 | // Stores a weight and node pointer for a vertex 52 | class VertexWeightMap { 53 | public: 54 | VertexWeightMap(double weight, int node) : weight(weight), node(node), relative(false) {} 55 | 56 | double weight; 57 | int node; 58 | bool relative; 59 | 60 | /** 61 | * BubblesortLOL 62 | * @param list 63 | * @param graph 64 | */ 65 | static void sort(std::vector& list, std::vector& graph) { 66 | bool done = false; 67 | 68 | int size = list.size(); 69 | 70 | while (!done) { 71 | done = true; 72 | for (int i = 0; i < size - 1; i++) { 73 | if (graph.at(list[i].node)->id > graph.at(list[i + 1].node)->id) { 74 | done = false; 75 | std::swap(list[i], list[i + 1]); 76 | } 77 | } 78 | size--; 79 | } 80 | } 81 | }; 82 | 83 | std::vector& getGraph(); 84 | std::vector& getGraphTimes(); 85 | 86 | void addConstraint(int vertexId, Eigen::Vector3d& target); 87 | void addRelativeConstraint(int vertexId, int targetId); 88 | 89 | void clearConstraints(); 90 | 91 | void applyGraphToVertices(); 92 | void applyGraphToPoses(std::vector T_wc_ptrs); 93 | 94 | bool optimiseGraphSparse( 95 | float& error, 96 | float& meanConsErr, 97 | const bool fernMatch, 98 | const uint64_t lastDeformTime); 99 | void resetGraph(); 100 | 101 | bool isInit() { 102 | return initialised; 103 | } 104 | 105 | // Number of neighbours 106 | const int k; 107 | 108 | private: 109 | bool initialised; 110 | 111 | // From paper 112 | const double wRot; 113 | const double wReg; 114 | const double wCon; 115 | 116 | static const int numVariables = 12; 117 | static const int eRotRows = 6; 118 | static const int eRegRows = 3; 119 | static const int eConRows = 3; 120 | 121 | // Graph itself 122 | std::vector graphNodes; 123 | std::vector graph; 124 | 125 | // Maps vertex indices to neighbours and weights 126 | std::vector> vertexMap; 127 | std::vector* sourceVertices; 128 | 129 | // Maps pose indices to neighbours and weights 130 | std::vector> poseMap; 131 | 132 | // Stores a vertex constraint 133 | class Constraint { 134 | public: 135 | Constraint(int vertexId, Eigen::Vector3d& targetPosition) 136 | : vertexId(vertexId), targetPosition(targetPosition), relative(false), targetId(-1) {} 137 | 138 | Constraint(int vertexId, int targetId) 139 | : vertexId(vertexId), 140 | targetPosition(Eigen::Vector3d::Zero()), 141 | relative(true), 142 | targetId(targetId) {} 143 | 144 | int vertexId; 145 | Eigen::Vector3d targetPosition; 146 | bool relative; 147 | int targetId; 148 | }; 149 | 150 | std::vector constraints; 151 | 152 | std::vector* graphCloud; 153 | std::vector sampledGraphTimes; 154 | uint32_t lastPointCount; 155 | 156 | void connectGraphSeq(); 157 | 158 | void weightVerticesSeq(std::vector* vertexTimeMap); 159 | 160 | void computeVertexPosition(int vertexId, Eigen::Vector3d& position); 161 | 162 | void sparseJacobian(Jacobian& jacobian, const int numRows, const int numCols, const int backSet); 163 | 164 | Eigen::VectorXd sparseResidual(const int maxRows); 165 | 166 | void applyDeltaSparse(Eigen::VectorXd& delta); 167 | 168 | CholeskyDecomp* cholesky; 169 | 170 | float nonRelativeConstraintError(); 171 | }; 172 | 173 | #endif /* DEFORMATIONGRAPH_H_ */ 174 | -------------------------------------------------------------------------------- /Core/Utils/GraphNode.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef GRAPHNODE_H_ 21 | #define GRAPHNODE_H_ 22 | 23 | #include 24 | 25 | class GraphNode { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | GraphNode() {} 29 | 30 | int id; 31 | Eigen::Vector3d position; 32 | Eigen::Matrix3d rotation; 33 | Eigen::Vector3d translation; 34 | std::vector neighbours; 35 | bool enabled; 36 | }; 37 | 38 | #endif /* GRAPHNODE_H_ */ 39 | -------------------------------------------------------------------------------- /Core/Utils/Img.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef UTILS_IMG_H_ 21 | #define UTILS_IMG_H_ 22 | 23 | #include 24 | 25 | template 26 | class Img { 27 | public: 28 | Img(const int rows, const int cols) 29 | : rows(rows), cols(cols), data(new uint8_t[rows * cols * sizeof(T)]), owned(true) {} 30 | 31 | Img(const int rows, const int cols, T* data) 32 | : rows(rows), cols(cols), data((uint8_t*)data), owned(false) {} 33 | 34 | virtual ~Img() { 35 | if (owned) { 36 | delete[] data; 37 | } 38 | } 39 | 40 | const int rows; 41 | const int cols; 42 | uint8_t* data; 43 | const bool owned; 44 | 45 | template 46 | inline V& at(const int i) { 47 | return ((V*)data)[i]; 48 | } 49 | 50 | template 51 | inline V& at(const int row, const int col) { 52 | return ((V*)data)[cols * row + col]; 53 | } 54 | 55 | template 56 | inline const V& at(const int row, const int col) const { 57 | return ((const V*)data)[cols * row + col]; 58 | } 59 | }; 60 | 61 | #endif /* UTILS_IMG_H_ */ 62 | -------------------------------------------------------------------------------- /Core/Utils/Intrinsics.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was written for porting ElasticFusion to windows 3 | * by Filip Srajer (filip.srajer@inf.ethz.ch). 4 | * 5 | */ 6 | 7 | #include "Intrinsics.h" 8 | 9 | const Intrinsics& Intrinsics::getInstance(float fx, float fy, float cx, float cy) { 10 | static const Intrinsics instance(fx, fy, cx, cy); 11 | return instance; 12 | } 13 | -------------------------------------------------------------------------------- /Core/Utils/Intrinsics.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef INTRINSICS_H_ 21 | #define INTRINSICS_H_ 22 | 23 | #include 24 | 25 | class Intrinsics { 26 | public: 27 | static const Intrinsics& getInstance(float fx = 0, float fy = 0, float cx = 0, float cy = 0); 28 | 29 | const float& fx() const { 30 | return fx_; 31 | } 32 | 33 | const float& fy() const { 34 | return fy_; 35 | } 36 | 37 | const float& cx() const { 38 | return cx_; 39 | } 40 | 41 | const float& cy() const { 42 | return cy_; 43 | } 44 | 45 | private: 46 | Intrinsics(float fx, float fy, float cx, float cy) : fx_(fx), fy_(fy), cx_(cx), cy_(cy) { 47 | assert(fx != 0 && fy != 0 && "You haven't initialised the Intrinsics class!"); 48 | } 49 | 50 | const float fx_, fy_, cx_, cy_; 51 | }; 52 | 53 | #endif /* INTRINSICS_H_ */ 54 | -------------------------------------------------------------------------------- /Core/Utils/Jacobian.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef UTILS_JACOBIAN_H_ 21 | #define UTILS_JACOBIAN_H_ 22 | 23 | #include 24 | 25 | #include "OrderedJacobianRow.h" 26 | 27 | class Jacobian { 28 | public: 29 | Jacobian() : columns(0) {} 30 | 31 | virtual ~Jacobian() { 32 | reset(); 33 | } 34 | 35 | void assign(std::vector& rows, const int columns) { 36 | reset(); 37 | this->rows = rows; 38 | this->columns = columns; 39 | } 40 | 41 | int cols() const { 42 | return columns; 43 | } 44 | 45 | int nonZero() const { 46 | int count = 0; 47 | for (size_t i = 0; i < rows.size(); i++) { 48 | count += rows[i]->nonZeros(); 49 | } 50 | return count; 51 | } 52 | 53 | std::vector rows; 54 | 55 | private: 56 | int columns; 57 | 58 | void reset() { 59 | for (size_t i = 0; i < rows.size(); i++) { 60 | delete rows[i]; 61 | } 62 | rows.clear(); 63 | } 64 | }; 65 | 66 | #endif /* UTILS_JACOBIAN_H_ */ 67 | -------------------------------------------------------------------------------- /Core/Utils/OdometryProvider.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef ODOMETRYPROVIDER_H_ 21 | #define ODOMETRYPROVIDER_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | class OdometryProvider { 29 | public: 30 | OdometryProvider() {} 31 | 32 | virtual ~OdometryProvider() {} 33 | 34 | static inline Eigen::Matrix rodrigues(const Eigen::Vector3d& src) { 35 | Eigen::Matrix dst = 36 | Eigen::Matrix::Identity(); 37 | 38 | double rx, ry, rz, theta; 39 | 40 | rx = src(0); 41 | ry = src(1); 42 | rz = src(2); 43 | 44 | theta = src.norm(); 45 | 46 | if (theta >= DBL_EPSILON) { 47 | const double I[] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; 48 | 49 | double c = cos(theta); 50 | double s = sin(theta); 51 | double c1 = 1. - c; 52 | double itheta = theta ? 1. / theta : 0.; 53 | 54 | rx *= itheta; 55 | ry *= itheta; 56 | rz *= itheta; 57 | 58 | double rrt[] = { 59 | rx * rx, rx * ry, rx * rz, rx * ry, ry * ry, ry * rz, rx * rz, ry * rz, rz * rz}; 60 | double _r_x_[] = {0, -rz, ry, rz, 0, -rx, -ry, rx, 0}; 61 | double R[9]; 62 | 63 | for (int k = 0; k < 9; k++) { 64 | R[k] = c * I[k] + c1 * rrt[k] + s * _r_x_[k]; 65 | } 66 | 67 | memcpy(dst.data(), &R[0], sizeof(Eigen::Matrix)); 68 | } 69 | 70 | return dst; 71 | } 72 | 73 | static inline void computeUpdateSE3( 74 | Eigen::Matrix& resultRt, 75 | const Eigen::Matrix& result, 76 | Eigen::Isometry3f& rgbOdom) { 77 | // for infinitesimal transformation 78 | Eigen::Matrix Rt = 79 | Eigen::Matrix::Identity(); 80 | 81 | Eigen::Vector3d rvec(result(3), result(4), result(5)); 82 | 83 | Eigen::Matrix R = rodrigues(rvec); 84 | 85 | Rt.topLeftCorner(3, 3) = R; 86 | Rt(0, 3) = result(0); 87 | Rt(1, 3) = result(1); 88 | Rt(2, 3) = result(2); 89 | 90 | resultRt = Rt * resultRt; 91 | 92 | Eigen::Matrix rotation = resultRt.topLeftCorner(3, 3); 93 | rgbOdom.setIdentity(); 94 | rgbOdom.rotate(rotation.cast().eval()); 95 | rgbOdom.translation() = resultRt.cast().eval().topRightCorner(3, 1); 96 | } 97 | }; 98 | 99 | #endif /* ODOMETRYPROVIDER_H_ */ 100 | -------------------------------------------------------------------------------- /Core/Utils/OrderedJacobianRow.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef UTILS_ORDEREDJACOBIANROW_H_ 21 | #define UTILS_ORDEREDJACOBIANROW_H_ 22 | 23 | #include 24 | #include 25 | 26 | class OrderedJacobianRow { 27 | public: 28 | OrderedJacobianRow(const int nonZeros) 29 | : indices(new int[nonZeros]), 30 | vals(new double[nonZeros]), 31 | lastSlot(0), 32 | lastIndex(-1), 33 | maxNonZero(nonZeros) {} 34 | 35 | virtual ~OrderedJacobianRow() { 36 | delete[] indices; 37 | delete[] vals; 38 | } 39 | 40 | // You have to use this in an ordered fashion for efficiency :) 41 | void append(const int index, const double value) { 42 | assert(index > lastIndex); 43 | indexSlotMap[index] = lastSlot; 44 | indices[lastSlot] = index; 45 | vals[lastSlot] = value; 46 | lastSlot++; 47 | lastIndex = index; 48 | } 49 | 50 | // To add to an existing and already weighted value 51 | void addTo(const int index, const double value, const double weight) { 52 | double& val = vals[indexSlotMap[index]]; 53 | val = ((val / weight) + value) * weight; 54 | } 55 | 56 | int nonZeros() { 57 | return lastSlot; 58 | } 59 | 60 | int* indices; 61 | double* vals; 62 | 63 | private: 64 | int lastSlot; 65 | int lastIndex; 66 | const int maxNonZero; 67 | std::unordered_map indexSlotMap; 68 | }; 69 | 70 | #endif /* UTILS_ORDEREDJACOBIANROW_H_ */ 71 | -------------------------------------------------------------------------------- /Core/Utils/Parse.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "Parse.h" 21 | 22 | Parse::Parse() {} 23 | 24 | const Parse& Parse::get() { 25 | static const Parse instance; 26 | return instance; 27 | } 28 | 29 | int Parse::arg(int argc, char** argv, const char* str, std::string& val) const { 30 | int index = findArg(argc, argv, str) + 1; 31 | 32 | if (index > 0 && index < argc) { 33 | val = argv[index]; 34 | } 35 | 36 | return index - 1; 37 | } 38 | 39 | int Parse::arg(int argc, char** argv, const char* str, float& val) const { 40 | int index = findArg(argc, argv, str) + 1; 41 | 42 | if (index > 0 && index < argc) { 43 | val = atof(argv[index]); 44 | } 45 | 46 | return index - 1; 47 | } 48 | 49 | int Parse::arg(int argc, char** argv, const char* str, int& val) const { 50 | int index = findArg(argc, argv, str) + 1; 51 | 52 | if (index > 0 && index < argc) { 53 | val = atoi(argv[index]); 54 | } 55 | 56 | return index - 1; 57 | } 58 | 59 | std::string Parse::shaderDir() const { 60 | std::string currentVal = STR(SHADER_DIR); 61 | 62 | assert(pangolin::FileExists(currentVal) && "Shader directory not found!"); 63 | 64 | return currentVal; 65 | } 66 | 67 | std::string Parse::baseDir() const { 68 | char buf[256]; 69 | int length = readlink("/proc/self/exe", buf, sizeof(buf)); 70 | std::string currentVal; 71 | currentVal.append((char*)&buf, length); 72 | currentVal = currentVal.substr(0, currentVal.rfind("/build/")); 73 | return currentVal; 74 | } 75 | 76 | int Parse::findArg(int argc, char** argv, const char* argument_name) const { 77 | for (int i = 1; i < argc; ++i) { 78 | // Search for the string 79 | if (strcmp(argv[i], argument_name) == 0) { 80 | return i; 81 | } 82 | } 83 | return -1; 84 | } 85 | -------------------------------------------------------------------------------- /Core/Utils/Parse.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | #ifndef PARSE_H_ 20 | #define PARSE_H_ 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #define XSTR(x) #x 30 | #define STR(x) XSTR(x) 31 | 32 | class Parse { 33 | public: 34 | static const Parse& get(); 35 | 36 | int arg(int argc, char** argv, const char* str, std::string& val) const; 37 | 38 | int arg(int argc, char** argv, const char* str, float& val) const; 39 | 40 | int arg(int argc, char** argv, const char* str, int& val) const; 41 | 42 | std::string shaderDir() const; 43 | 44 | std::string baseDir() const; 45 | 46 | private: 47 | Parse(); 48 | 49 | int findArg(int argc, char** argv, const char* argument_name) const; 50 | }; 51 | 52 | #endif /* PARSE_H_ */ 53 | -------------------------------------------------------------------------------- /Core/Utils/RGBDOdometry.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef RGBDODOMETRY_H_ 21 | #define RGBDODOMETRY_H_ 22 | 23 | #include "../Cuda/cudafuncs.cuh" 24 | #include "../GPUTexture.h" 25 | #include "OdometryProvider.h" 26 | #include "Stopwatch.h" 27 | 28 | #include 29 | #include 30 | 31 | class RGBDOdometry { 32 | public: 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | RGBDOdometry( 35 | int width, 36 | int height, 37 | float cx, 38 | float cy, 39 | float fx, 40 | float fy, 41 | float distThresh = 0.10f, 42 | float angleThresh = sin(20.f * 3.14159254f / 180.f)); 43 | 44 | virtual ~RGBDOdometry(); 45 | 46 | void initICP(GPUTexture* filteredDepth, const float depthCutoff); 47 | 48 | void initICP(GPUTexture* predictedVertices, GPUTexture* predictedNormals); 49 | 50 | void initICPModel( 51 | GPUTexture* predictedVertices, 52 | GPUTexture* predictedNormals, 53 | const Sophus::SE3d& T_wc); 54 | 55 | void initRGB(GPUTexture* rgb); 56 | 57 | void initRGBModel(GPUTexture* rgb); 58 | 59 | void initFirstRGB(GPUTexture* rgb); 60 | 61 | void getIncrementalTransformation( 62 | Sophus::SE3d& T_wc, 63 | const bool& rgbOnly, 64 | const float& icpWeight, 65 | const bool& pyramid, 66 | const bool& fastOdom, 67 | const bool& so3); 68 | 69 | Eigen::MatrixXd getCovariance(); 70 | 71 | float lastICPError; 72 | float lastICPCount; 73 | float lastRGBError; 74 | float lastRGBCount; 75 | float lastSO3Error; 76 | float lastSO3Count; 77 | 78 | Eigen::Matrix lastA; 79 | Eigen::Matrix lastb; 80 | 81 | private: 82 | void populateRGBDData( 83 | GPUTexture* rgb, 84 | DeviceArray2D* destDepths, 85 | DeviceArray2D* destImages); 86 | 87 | std::vector> depth_tmp; 88 | 89 | DeviceArray vmaps_tmp; 90 | DeviceArray nmaps_tmp; 91 | 92 | std::vector> vmaps_g_prev_; 93 | std::vector> nmaps_g_prev_; 94 | 95 | std::vector> vmaps_curr_; 96 | std::vector> nmaps_curr_; 97 | 98 | CameraModel intr; 99 | 100 | DeviceArray sumDataSE3; 101 | DeviceArray outDataSE3; 102 | DeviceArray sumResidualRGB; 103 | 104 | DeviceArray sumDataSO3; 105 | DeviceArray outDataSO3; 106 | 107 | const int sobelSize; 108 | const float sobelScale; 109 | const float maxDepthDeltaRGB; 110 | const float maxDepthRGB; 111 | 112 | std::vector pyrDims; 113 | 114 | static constexpr int NUM_PYRS = 3; 115 | 116 | DeviceArray2D lastDepth[NUM_PYRS]; 117 | DeviceArray2D lastImage[NUM_PYRS]; 118 | 119 | DeviceArray2D nextDepth[NUM_PYRS]; 120 | DeviceArray2D nextImage[NUM_PYRS]; 121 | DeviceArray2D nextdIdx[NUM_PYRS]; 122 | DeviceArray2D nextdIdy[NUM_PYRS]; 123 | 124 | DeviceArray2D lastNextImage[NUM_PYRS]; 125 | 126 | DeviceArray2D corresImg[NUM_PYRS]; 127 | 128 | DeviceArray2D pointClouds[NUM_PYRS]; 129 | 130 | std::vector iterations; 131 | std::vector minimumGradientMagnitudes; 132 | 133 | float distThres_; 134 | float angleThres_; 135 | 136 | Eigen::Matrix lastCov; 137 | 138 | const int width; 139 | const int height; 140 | const float cx, cy, fx, fy; 141 | }; 142 | 143 | #endif /* RGBDODOMETRY_H_ */ 144 | -------------------------------------------------------------------------------- /Core/Utils/Resolution.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file was written for porting ElasticFusion to windows 3 | * by Filip Srajer (filip.srajer@inf.ethz.ch). 4 | * 5 | */ 6 | 7 | #include "Resolution.h" 8 | 9 | const Resolution& Resolution::getInstance(int width, int height) { 10 | static const Resolution instance(width, height); 11 | return instance; 12 | } 13 | -------------------------------------------------------------------------------- /Core/Utils/Resolution.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef RESOLUTION_H_ 21 | #define RESOLUTION_H_ 22 | 23 | #include 24 | 25 | class Resolution { 26 | public: 27 | static const Resolution& getInstance(int width = 0, int height = 0); 28 | 29 | const int& width() const { 30 | return imgWidth; 31 | } 32 | 33 | const int& height() const { 34 | return imgHeight; 35 | } 36 | 37 | const int& cols() const { 38 | return imgWidth; 39 | } 40 | 41 | const int& rows() const { 42 | return imgHeight; 43 | } 44 | 45 | const int& numPixels() const { 46 | return imgNumPixels; 47 | } 48 | 49 | private: 50 | Resolution(int width, int height) 51 | : imgWidth(width), imgHeight(height), imgNumPixels(width * height) { 52 | assert(width > 0 && height > 0 && "You haven't initialised the Resolution class!"); 53 | } 54 | 55 | const int imgWidth; 56 | const int imgHeight; 57 | const int imgNumPixels; 58 | }; 59 | 60 | #endif /* RESOLUTION_H_ */ 61 | -------------------------------------------------------------------------------- /Main.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "MainController.h" 21 | 22 | int main(int argc, char* argv[]) { 23 | MainController mainController(argc, argv); 24 | 25 | mainController.launch(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /MainController.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "Core/ElasticFusion.h" 21 | #include "Core/Utils/Parse.h" 22 | 23 | #include "Tools/GUI.h" 24 | #include "Tools/GroundTruthOdometry.h" 25 | #include "Tools/LiveLogReader.h" 26 | #include "Tools/RawLogReader.h" 27 | 28 | #ifndef MAINCONTROLLER_H_ 29 | #define MAINCONTROLLER_H_ 30 | 31 | class MainController { 32 | public: 33 | MainController(int argc, char* argv[]); 34 | virtual ~MainController(); 35 | 36 | void launch(); 37 | 38 | private: 39 | void run(); 40 | 41 | void loadCalibration(const std::string& filename); 42 | 43 | bool good; 44 | ElasticFusion* eFusion; 45 | GUI* gui; 46 | GroundTruthOdometry* groundTruthOdometry; 47 | LogReader* logReader; 48 | 49 | bool iclnuim; 50 | std::string logFile; 51 | std::string poseFile; 52 | 53 | float confidence, depth, icp, icpErrThresh, covThresh, photoThresh, fernThresh; 54 | 55 | int timeDelta, icpCountThresh, start, end; 56 | 57 | bool fillIn, openLoop, reloc, frameskip, quiet, fastOdom, so3, rewind, frameToFrameRGB; 58 | 59 | int framesToSkip; 60 | bool streaming; 61 | bool resetButton; 62 | 63 | Resize* resizeStream; 64 | }; 65 | 66 | #endif /* MAINCONTROLLER_H_ */ 67 | -------------------------------------------------------------------------------- /Tools/CameraInterface.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ThreadMutexObject.h" 9 | 10 | class CameraInterface { 11 | public: 12 | virtual ~CameraInterface() {} 13 | 14 | virtual bool ok() = 0; 15 | virtual std::string error() = 0; 16 | 17 | static const int numBuffers = 10; 18 | ThreadMutexObject latestDepthIndex; 19 | std::pair, int64_t> frameBuffers[numBuffers]; 20 | 21 | virtual void setAutoExposure(bool value) = 0; 22 | virtual void setAutoWhiteBalance(bool value) = 0; 23 | }; 24 | -------------------------------------------------------------------------------- /Tools/GroundTruthOdometry.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "GroundTruthOdometry.h" 21 | 22 | GroundTruthOdometry::GroundTruthOdometry(const std::string& filename) : last_utime(0) { 23 | loadTrajectory(filename); 24 | } 25 | 26 | GroundTruthOdometry::~GroundTruthOdometry() {} 27 | 28 | void GroundTruthOdometry::loadTrajectory(const std::string& filename) { 29 | std::ifstream file; 30 | std::string line; 31 | file.open(filename.c_str()); 32 | while (!file.eof()) { 33 | uint64_t utime; 34 | float x, y, z, qx, qy, qz, qw; 35 | std::getline(file, line); 36 | int n = 37 | sscanf(line.c_str(), "%lu,%f,%f,%f,%f,%f,%f,%f", &utime, &x, &y, &z, &qx, &qy, &qz, &qw); 38 | 39 | if (file.eof()) 40 | break; 41 | 42 | assert(n == 8); 43 | 44 | Eigen::Quaternionf q(qw, qx, qy, qz); 45 | Eigen::Vector3f t(x, y, z); 46 | 47 | Eigen::Isometry3f T; 48 | T.setIdentity(); 49 | T.pretranslate(t).rotate(q); 50 | camera_trajectory[utime] = T; 51 | } 52 | } 53 | 54 | Eigen::Matrix4f GroundTruthOdometry::getTransformation(uint64_t timestamp) { 55 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 56 | 57 | if (last_utime != 0) { 58 | std::map::const_iterator it = camera_trajectory.find(last_utime); 59 | if (it == camera_trajectory.end()) { 60 | last_utime = timestamp; 61 | return pose; 62 | } 63 | 64 | // Poses are stored in the file in iSAM basis, undo it 65 | Eigen::Matrix4f M; 66 | M << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1; 67 | 68 | pose = M.inverse() * camera_trajectory[timestamp] * M; 69 | } else { 70 | std::map::const_iterator it = camera_trajectory.find(timestamp); 71 | Eigen::Isometry3f ident = it->second; 72 | pose = Eigen::Matrix4f::Identity(); 73 | camera_trajectory[last_utime] = ident; 74 | } 75 | 76 | last_utime = timestamp; 77 | 78 | return pose; 79 | } 80 | 81 | Eigen::MatrixXd GroundTruthOdometry::getCovariance() { 82 | Eigen::MatrixXd cov(6, 6); 83 | cov.setIdentity(); 84 | cov(0, 0) = 0.1; 85 | cov(1, 1) = 0.1; 86 | cov(2, 2) = 0.1; 87 | cov(3, 3) = 0.5; 88 | cov(4, 4) = 0.5; 89 | cov(5, 5) = 0.5; 90 | return cov; 91 | } 92 | -------------------------------------------------------------------------------- /Tools/GroundTruthOdometry.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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef GROUNDTRUTHODOMETRY_H_ 21 | #define GROUNDTRUTHODOMETRY_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include "../Core/Utils/OdometryProvider.h" 32 | 33 | class GroundTruthOdometry { 34 | public: 35 | GroundTruthOdometry(const std::string& filename); 36 | 37 | virtual ~GroundTruthOdometry(); 38 | 39 | Eigen::Matrix4f getTransformation(uint64_t timestamp); 40 | 41 | Eigen::MatrixXd getCovariance(); 42 | 43 | private: 44 | void loadTrajectory(const std::string& filename); 45 | 46 | std::map< 47 | uint64_t, 48 | Eigen::Isometry3f, 49 | std::less, 50 | Eigen::aligned_allocator>> 51 | camera_trajectory; 52 | uint64_t last_utime; 53 | }; 54 | 55 | #endif /* GROUNDTRUTHODOMETRY_H_ */ 56 | -------------------------------------------------------------------------------- /Tools/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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef TOOLS_JPEGLOADER_H_ 21 | #define TOOLS_JPEGLOADER_H_ 22 | 23 | extern "C" { 24 | #include "jpeglib.h" 25 | } 26 | 27 | #include 28 | #include 29 | 30 | static void jpegFail(j_common_ptr cinfo) { 31 | assert(false && "JPEG decoding error!"); 32 | } 33 | 34 | static void doNothing(j_decompress_ptr) {} 35 | 36 | class JPEGLoader { 37 | public: 38 | JPEGLoader() {} 39 | 40 | void readData(uint8_t* src, const int numBytes, uint8_t* data) { 41 | jpeg_decompress_struct cinfo; // IJG JPEG codec structure 42 | 43 | jpeg_error_mgr errorMgr; 44 | 45 | errorMgr.error_exit = jpegFail; 46 | 47 | cinfo.err = jpeg_std_error(&errorMgr); 48 | 49 | jpeg_create_decompress(&cinfo); 50 | 51 | jpeg_source_mgr srcMgr; 52 | 53 | cinfo.src = &srcMgr; 54 | 55 | // Prepare for suspending reader 56 | srcMgr.init_source = doNothing; 57 | srcMgr.resync_to_restart = jpeg_resync_to_restart; 58 | srcMgr.term_source = doNothing; 59 | srcMgr.next_input_byte = src; 60 | srcMgr.bytes_in_buffer = numBytes; 61 | 62 | jpeg_read_header(&cinfo, TRUE); 63 | 64 | jpeg_calc_output_dimensions(&cinfo); 65 | 66 | jpeg_start_decompress(&cinfo); 67 | 68 | int width = cinfo.output_width; 69 | int height = cinfo.output_height; 70 | 71 | JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)((j_common_ptr)&cinfo, JPOOL_IMAGE, width * 4, 1); 72 | 73 | for (; height--; data += (width * 3)) { 74 | jpeg_read_scanlines(&cinfo, buffer, 1); 75 | 76 | uint8_t* bgr = (uint8_t*)buffer[0]; 77 | uint8_t* rgb = (uint8_t*)data; 78 | 79 | for (int i = 0; i < width; i++, bgr += 3, rgb += 3) { 80 | uint8_t t0 = bgr[0], t1 = bgr[1], t2 = bgr[2]; 81 | rgb[2] = t0; 82 | rgb[1] = t1; 83 | rgb[0] = t2; 84 | } 85 | } 86 | 87 | jpeg_finish_decompress(&cinfo); 88 | 89 | jpeg_destroy_decompress(&cinfo); 90 | } 91 | }; 92 | 93 | #endif /* TOOLS_JPEGLOADER_H_ */ 94 | -------------------------------------------------------------------------------- /Tools/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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "LiveLogReader.h" 21 | 22 | #include "OpenNI2Interface.h" 23 | #include "RealSenseInterface.h" 24 | 25 | LiveLogReader::LiveLogReader(std::string file, bool flipColors, CameraType type) 26 | : LogReader(file, flipColors), lastFrameTime(-1), lastGot(-1) { 27 | std::cout << "Creating live capture... "; 28 | std::cout.flush(); 29 | 30 | if (type == CameraType::OpenNI2) 31 | cam = 32 | new OpenNI2Interface(Resolution::getInstance().width(), Resolution::getInstance().height()); 33 | else if (type == CameraType::RealSense) 34 | cam = new RealSenseInterface( 35 | Resolution::getInstance().width(), Resolution::getInstance().height()); 36 | else 37 | cam = nullptr; 38 | 39 | decompressionBufferDepth = new Bytef[Resolution::getInstance().numPixels() * 2]; 40 | 41 | decompressionBufferImage = new Bytef[Resolution::getInstance().numPixels() * 3]; 42 | 43 | if (!cam || !cam->ok()) { 44 | std::cout << "failed!" << std::endl; 45 | std::cout << cam->error(); 46 | } else { 47 | std::cout << "success!" << std::endl; 48 | 49 | std::cout << "Waiting for first frame"; 50 | std::cout.flush(); 51 | 52 | int lastDepth = cam->latestDepthIndex.getValue(); 53 | 54 | do { 55 | std::this_thread::sleep_for(std::chrono::microseconds(33333)); 56 | std::cout << "."; 57 | std::cout.flush(); 58 | lastDepth = cam->latestDepthIndex.getValue(); 59 | } while (lastDepth == -1); 60 | 61 | std::cout << " got it!" << std::endl; 62 | } 63 | } 64 | 65 | LiveLogReader::~LiveLogReader() { 66 | delete[] decompressionBufferDepth; 67 | 68 | delete[] decompressionBufferImage; 69 | 70 | delete cam; 71 | } 72 | 73 | void LiveLogReader::getNext() { 74 | int lastDepth = cam->latestDepthIndex.getValue(); 75 | 76 | assert(lastDepth != -1); 77 | 78 | int bufferIndex = lastDepth % CameraInterface::numBuffers; 79 | 80 | if (bufferIndex == lastGot) { 81 | return; 82 | } 83 | 84 | if (lastFrameTime == cam->frameBuffers[bufferIndex].second) { 85 | return; 86 | } 87 | 88 | memcpy( 89 | &decompressionBufferDepth[0], 90 | cam->frameBuffers[bufferIndex].first.first, 91 | Resolution::getInstance().numPixels() * 2); 92 | memcpy( 93 | &decompressionBufferImage[0], 94 | cam->frameBuffers[bufferIndex].first.second, 95 | Resolution::getInstance().numPixels() * 3); 96 | 97 | lastFrameTime = cam->frameBuffers[bufferIndex].second; 98 | 99 | timestamp = lastFrameTime; 100 | 101 | rgb = (uint8_t*)&decompressionBufferImage[0]; 102 | depth = (uint16_t*)&decompressionBufferDepth[0]; 103 | 104 | imageReadBuffer = 0; 105 | depthReadBuffer = 0; 106 | 107 | imageSize = Resolution::getInstance().numPixels() * 3; 108 | depthSize = Resolution::getInstance().numPixels() * 2; 109 | 110 | if (flipColors) { 111 | for (int i = 0; i < Resolution::getInstance().numPixels() * 3; i += 3) { 112 | std::swap(rgb[i + 0], rgb[i + 2]); 113 | } 114 | } 115 | } 116 | 117 | const std::string LiveLogReader::getFile() { 118 | return Parse::get().baseDir().append("live"); 119 | } 120 | 121 | int LiveLogReader::getNumFrames() { 122 | return std::numeric_limits::max(); 123 | } 124 | 125 | bool LiveLogReader::hasMore() { 126 | return true; 127 | } 128 | 129 | void LiveLogReader::setAuto(bool value) { 130 | cam->setAutoExposure(value); 131 | cam->setAutoWhiteBalance(value); 132 | } 133 | -------------------------------------------------------------------------------- /Tools/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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef LIVELOGREADER_H_ 21 | #define LIVELOGREADER_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "../Core/Utils/Parse.h" 31 | 32 | #include "CameraInterface.h" 33 | #include "LogReader.h" 34 | 35 | class LiveLogReader : public LogReader { 36 | public: 37 | enum CameraType { OpenNI2, RealSense }; 38 | 39 | LiveLogReader(std::string file, bool flipColors, CameraType type); 40 | 41 | virtual ~LiveLogReader(); 42 | 43 | void getNext(); 44 | 45 | int getNumFrames(); 46 | 47 | bool hasMore(); 48 | 49 | bool rewound() { 50 | return false; 51 | } 52 | 53 | void rewind() {} 54 | 55 | void getBack() {} 56 | 57 | void fastForward(int frame) {} 58 | 59 | const std::string getFile(); 60 | 61 | void setAuto(bool value); 62 | 63 | CameraInterface* cam; 64 | 65 | private: 66 | int64_t lastFrameTime; 67 | int lastGot; 68 | }; 69 | 70 | #endif /* LIVELOGREADER_H_ */ 71 | -------------------------------------------------------------------------------- /Tools/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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef LOGREADER_H_ 21 | #define LOGREADER_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include "../Core/Utils/Img.h" 27 | #include "../Core/Utils/Resolution.h" 28 | 29 | #include "JPEGLoader.h" 30 | 31 | class LogReader { 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 | virtual ~LogReader() {} 47 | 48 | virtual void getNext() = 0; 49 | 50 | virtual int getNumFrames() = 0; 51 | 52 | virtual bool hasMore() = 0; 53 | 54 | virtual bool rewound() = 0; 55 | 56 | virtual void rewind() = 0; 57 | 58 | virtual void getBack() = 0; 59 | 60 | virtual void fastForward(int frame) = 0; 61 | 62 | virtual const std::string getFile() = 0; 63 | 64 | virtual void setAuto(bool value) = 0; 65 | 66 | bool flipColors; 67 | int64_t timestamp; 68 | 69 | uint16_t* depth; 70 | uint8_t* rgb; 71 | int currentFrame; 72 | 73 | protected: 74 | Bytef* decompressionBufferDepth; 75 | Bytef* decompressionBufferImage; 76 | uint8_t* depthReadBuffer; 77 | uint8_t* imageReadBuffer; 78 | int32_t depthSize; 79 | int32_t imageSize; 80 | 81 | const std::string file; 82 | FILE* fp; 83 | int32_t numFrames; 84 | int width; 85 | int height; 86 | int numPixels; 87 | 88 | JPEGLoader jpeg; 89 | }; 90 | 91 | #endif /* LOGREADER_H_ */ 92 | -------------------------------------------------------------------------------- /Tools/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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #include "RawLogReader.h" 21 | 22 | RawLogReader::RawLogReader(std::string file, bool flipColors) : LogReader(file, flipColors) { 23 | assert(pangolin::FileExists(file.c_str())); 24 | 25 | fp = fopen(file.c_str(), "rb"); 26 | 27 | currentFrame = 0; 28 | 29 | auto tmp = fread(&numFrames, sizeof(int32_t), 1, fp); 30 | assert(tmp); 31 | 32 | depthReadBuffer = new uint8_t[numPixels * 2]; 33 | imageReadBuffer = new uint8_t[numPixels * 3]; 34 | decompressionBufferDepth = new Bytef[Resolution::getInstance().numPixels() * 2]; 35 | decompressionBufferImage = new Bytef[Resolution::getInstance().numPixels() * 3]; 36 | } 37 | 38 | RawLogReader::~RawLogReader() { 39 | delete[] depthReadBuffer; 40 | delete[] imageReadBuffer; 41 | delete[] decompressionBufferDepth; 42 | delete[] decompressionBufferImage; 43 | 44 | fclose(fp); 45 | } 46 | 47 | void RawLogReader::getBack() { 48 | assert(filePointers.size() > 0); 49 | 50 | fseek(fp, filePointers.top(), SEEK_SET); 51 | 52 | filePointers.pop(); 53 | 54 | getCore(); 55 | } 56 | 57 | void RawLogReader::getNext() { 58 | filePointers.push(ftell(fp)); 59 | 60 | getCore(); 61 | } 62 | 63 | void RawLogReader::getCore() { 64 | auto tmp = fread(×tamp, sizeof(int64_t), 1, fp); 65 | assert(tmp); 66 | 67 | tmp = fread(&depthSize, sizeof(int32_t), 1, fp); 68 | assert(tmp); 69 | tmp = fread(&imageSize, sizeof(int32_t), 1, fp); 70 | assert(tmp); 71 | 72 | tmp = fread(depthReadBuffer, depthSize, 1, fp); 73 | assert(tmp); 74 | 75 | if (imageSize > 0) { 76 | tmp = fread(imageReadBuffer, imageSize, 1, fp); 77 | assert(tmp); 78 | } 79 | 80 | if (depthSize == numPixels * 2) { 81 | memcpy(&decompressionBufferDepth[0], depthReadBuffer, numPixels * 2); 82 | } else { 83 | unsigned long decompLength = numPixels * 2; 84 | uncompress( 85 | &decompressionBufferDepth[0], 86 | (unsigned long*)&decompLength, 87 | (const Bytef*)depthReadBuffer, 88 | depthSize); 89 | } 90 | 91 | if (imageSize == numPixels * 3) { 92 | memcpy(&decompressionBufferImage[0], imageReadBuffer, numPixels * 3); 93 | } else if (imageSize > 0) { 94 | jpeg.readData(imageReadBuffer, imageSize, (uint8_t*)&decompressionBufferImage[0]); 95 | } else { 96 | memset(&decompressionBufferImage[0], 0, numPixels * 3); 97 | } 98 | 99 | depth = (uint16_t*)decompressionBufferDepth; 100 | rgb = (uint8_t*)&decompressionBufferImage[0]; 101 | 102 | if (flipColors) { 103 | for (int i = 0; i < Resolution::getInstance().numPixels() * 3; i += 3) { 104 | std::swap(rgb[i + 0], rgb[i + 2]); 105 | } 106 | } 107 | 108 | currentFrame++; 109 | } 110 | 111 | void RawLogReader::fastForward(int frame) { 112 | while (currentFrame < frame && hasMore()) { 113 | filePointers.push(ftell(fp)); 114 | 115 | auto tmp = fread(×tamp, sizeof(int64_t), 1, fp); 116 | assert(tmp); 117 | 118 | tmp = fread(&depthSize, sizeof(int32_t), 1, fp); 119 | assert(tmp); 120 | tmp = fread(&imageSize, sizeof(int32_t), 1, fp); 121 | assert(tmp); 122 | 123 | tmp = fread(depthReadBuffer, depthSize, 1, fp); 124 | assert(tmp); 125 | 126 | if (imageSize > 0) { 127 | tmp = fread(imageReadBuffer, imageSize, 1, fp); 128 | assert(tmp); 129 | } 130 | 131 | currentFrame++; 132 | } 133 | } 134 | 135 | int RawLogReader::getNumFrames() { 136 | return numFrames; 137 | } 138 | 139 | bool RawLogReader::hasMore() { 140 | return currentFrame + 1 < numFrames; 141 | } 142 | 143 | void RawLogReader::rewind() { 144 | if (filePointers.size() != 0) { 145 | std::stack empty; 146 | std::swap(empty, filePointers); 147 | } 148 | 149 | fclose(fp); 150 | fp = fopen(file.c_str(), "rb"); 151 | 152 | auto tmp = fread(&numFrames, sizeof(int32_t), 1, fp); 153 | assert(tmp); 154 | 155 | currentFrame = 0; 156 | } 157 | 158 | bool RawLogReader::rewound() { 159 | return filePointers.size() == 0; 160 | } 161 | 162 | const std::string RawLogReader::getFile() { 163 | return file; 164 | } 165 | 166 | void RawLogReader::setAuto(bool value) {} 167 | -------------------------------------------------------------------------------- /Tools/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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef RAWLOGREADER_H_ 21 | #define RAWLOGREADER_H_ 22 | 23 | #include 24 | #include "../Core/Utils/Resolution.h" 25 | #include "../Core/Utils/Stopwatch.h" 26 | 27 | #include "LogReader.h" 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | class RawLogReader : public LogReader { 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 rewind(); 53 | 54 | void fastForward(int frame); 55 | 56 | const std::string getFile(); 57 | 58 | void setAuto(bool value); 59 | 60 | std::stack filePointers; 61 | 62 | private: 63 | void getCore(); 64 | }; 65 | 66 | #endif /* RAWLOGREADER_H_ */ 67 | -------------------------------------------------------------------------------- /Tools/RealSenseInterface.cpp: -------------------------------------------------------------------------------- 1 | #include "RealSenseInterface.h" 2 | #include 3 | 4 | #ifdef WITH_REALSENSE 5 | RealSenseInterface::RealSenseInterface(int inWidth, int inHeight, int inFps) 6 | : width(inWidth), height(inHeight), fps(inFps), dev(nullptr), initSuccessful(true) { 7 | if (ctx.get_device_count() == 0) { 8 | errorText = "No device connected."; 9 | initSuccessful = false; 10 | return; 11 | } 12 | 13 | dev = ctx.get_device(0); 14 | dev->enable_stream(rs::stream::depth, width, height, rs::format::z16, fps); 15 | dev->enable_stream(rs::stream::color, width, height, rs::format::rgb8, fps); 16 | 17 | latestDepthIndex.assign(-1); 18 | latestRgbIndex.assign(-1); 19 | 20 | for (int i = 0; i < numBuffers; i++) { 21 | uint8_t* newImage = (uint8_t*)calloc(width * height * 3, sizeof(uint8_t)); 22 | rgbBuffers[i] = std::pair(newImage, 0); 23 | } 24 | 25 | for (int i = 0; i < numBuffers; i++) { 26 | uint8_t* newDepth = (uint8_t*)calloc(width * height * 2, sizeof(uint8_t)); 27 | uint8_t* newImage = (uint8_t*)calloc(width * height * 3, sizeof(uint8_t)); 28 | frameBuffers[i] = std::pair, int64_t>( 29 | std::pair(newDepth, newImage), 0); 30 | } 31 | 32 | setAutoExposure(true); 33 | setAutoWhiteBalance(true); 34 | 35 | rgbCallback = new RGBCallback(lastRgbTime, latestRgbIndex, rgbBuffers); 36 | 37 | depthCallback = 38 | new DepthCallback(lastDepthTime, latestDepthIndex, latestRgbIndex, rgbBuffers, frameBuffers); 39 | 40 | dev->set_frame_callback(rs::stream::depth, *depthCallback); 41 | dev->set_frame_callback(rs::stream::color, *rgbCallback); 42 | 43 | dev->start(); 44 | } 45 | 46 | RealSenseInterface::~RealSenseInterface() { 47 | if (initSuccessful) { 48 | dev->stop(); 49 | 50 | for (int i = 0; i < numBuffers; i++) { 51 | free(rgbBuffers[i].first); 52 | } 53 | 54 | for (int i = 0; i < numBuffers; i++) { 55 | free(frameBuffers[i].first.first); 56 | free(frameBuffers[i].first.second); 57 | } 58 | 59 | delete rgbCallback; 60 | delete depthCallback; 61 | } 62 | } 63 | 64 | void RealSenseInterface::setAutoExposure(bool value) { 65 | dev->set_option(rs::option::color_enable_auto_exposure, value); 66 | } 67 | 68 | void RealSenseInterface::setAutoWhiteBalance(bool value) { 69 | dev->set_option(rs::option::color_enable_auto_white_balance, value); 70 | } 71 | 72 | bool RealSenseInterface::getAutoExposure() { 73 | return dev->get_option(rs::option::color_enable_auto_exposure); 74 | } 75 | 76 | bool RealSenseInterface::getAutoWhiteBalance() { 77 | return dev->get_option(rs::option::color_enable_auto_white_balance); 78 | } 79 | #else 80 | 81 | RealSenseInterface::RealSenseInterface(int inWidth, int inHeight, int inFps) 82 | : width(inWidth), height(inHeight), fps(inFps), initSuccessful(false) { 83 | errorText = "Compiled without Intel RealSense library"; 84 | } 85 | 86 | RealSenseInterface::~RealSenseInterface() {} 87 | 88 | void RealSenseInterface::setAutoExposure(bool value) {} 89 | 90 | void RealSenseInterface::setAutoWhiteBalance(bool value) {} 91 | 92 | bool RealSenseInterface::getAutoExposure() { 93 | return false; 94 | } 95 | 96 | bool RealSenseInterface::getAutoWhiteBalance() { 97 | return false; 98 | } 99 | #endif 100 | -------------------------------------------------------------------------------- /Tools/RealSenseInterface.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #ifdef WITH_REALSENSE 9 | #include "librealsense/rs.hpp" 10 | #endif 11 | 12 | #include "CameraInterface.h" 13 | #include "ThreadMutexObject.h" 14 | 15 | class RealSenseInterface : public CameraInterface { 16 | public: 17 | RealSenseInterface(int width = 640, int height = 480, int fps = 30); 18 | virtual ~RealSenseInterface(); 19 | 20 | const int width, height, fps; 21 | 22 | bool getAutoExposure(); 23 | bool getAutoWhiteBalance(); 24 | virtual void setAutoExposure(bool value); 25 | virtual void setAutoWhiteBalance(bool value); 26 | 27 | virtual bool ok() { 28 | return initSuccessful; 29 | } 30 | 31 | virtual std::string error() { 32 | return errorText; 33 | } 34 | 35 | #ifdef WITH_REALSENSE 36 | struct RGBCallback { 37 | public: 38 | RGBCallback( 39 | int64_t& lastRgbTime, 40 | ThreadMutexObject& latestRgbIndex, 41 | std::pair* rgbBuffers) 42 | : lastRgbTime(lastRgbTime), latestRgbIndex(latestRgbIndex), rgbBuffers(rgbBuffers) {} 43 | 44 | void operator()(rs::frame frame) { 45 | lastRgbTime = std::chrono::duration_cast( 46 | std::chrono::system_clock::now().time_since_epoch()) 47 | .count(); 48 | 49 | int bufferIndex = (latestRgbIndex.getValue() + 1) % numBuffers; 50 | 51 | memcpy( 52 | rgbBuffers[bufferIndex].first, 53 | frame.get_data(), 54 | frame.get_width() * frame.get_height() * 3); 55 | 56 | rgbBuffers[bufferIndex].second = lastRgbTime; 57 | 58 | latestRgbIndex++; 59 | } 60 | 61 | private: 62 | int64_t& lastRgbTime; 63 | ThreadMutexObject& latestRgbIndex; 64 | std::pair* rgbBuffers; 65 | }; 66 | 67 | struct DepthCallback { 68 | public: 69 | DepthCallback( 70 | int64_t& lastDepthTime, 71 | ThreadMutexObject& latestDepthIndex, 72 | ThreadMutexObject& latestRgbIndex, 73 | std::pair* rgbBuffers, 74 | std::pair, int64_t>* frameBuffers) 75 | : lastDepthTime(lastDepthTime), 76 | latestDepthIndex(latestDepthIndex), 77 | latestRgbIndex(latestRgbIndex), 78 | rgbBuffers(rgbBuffers), 79 | frameBuffers(frameBuffers) {} 80 | 81 | void operator()(rs::frame frame) { 82 | lastDepthTime = std::chrono::duration_cast( 83 | std::chrono::system_clock::now().time_since_epoch()) 84 | .count(); 85 | 86 | int bufferIndex = (latestDepthIndex.getValue() + 1) % numBuffers; 87 | 88 | // The multiplication by 2 is here because the depth is actually uint16_t 89 | memcpy( 90 | frameBuffers[bufferIndex].first.first, 91 | frame.get_data(), 92 | frame.get_width() * frame.get_height() * 2); 93 | 94 | frameBuffers[bufferIndex].second = lastDepthTime; 95 | 96 | int lastImageVal = latestRgbIndex.getValue(); 97 | 98 | if (lastImageVal == -1) { 99 | return; 100 | } 101 | 102 | lastImageVal %= numBuffers; 103 | 104 | memcpy( 105 | frameBuffers[bufferIndex].first.second, 106 | rgbBuffers[lastImageVal].first, 107 | frame.get_width() * frame.get_height() * 3); 108 | 109 | latestDepthIndex++; 110 | } 111 | 112 | private: 113 | int64_t& lastDepthTime; 114 | ThreadMutexObject& latestDepthIndex; 115 | ThreadMutexObject& latestRgbIndex; 116 | 117 | std::pair* rgbBuffers; 118 | std::pair, int64_t>* frameBuffers; 119 | }; 120 | #endif 121 | 122 | private: 123 | #ifdef WITH_REALSENSE 124 | rs::device* dev; 125 | rs::context ctx; 126 | 127 | RGBCallback* rgbCallback; 128 | DepthCallback* depthCallback; 129 | #endif 130 | 131 | bool initSuccessful; 132 | std::string errorText; 133 | 134 | ThreadMutexObject latestRgbIndex; 135 | std::pair rgbBuffers[numBuffers]; 136 | 137 | int64_t lastRgbTime; 138 | int64_t lastDepthTime; 139 | }; 140 | -------------------------------------------------------------------------------- /Tools/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 | * 12 | * unless explicitly stated. By downloading this file you agree to 13 | * comply with these terms. 14 | * 15 | * If you wish to use any of this code for commercial purposes then 16 | * please email researchcontracts.engineering@imperial.ac.uk. 17 | * 18 | */ 19 | 20 | #ifndef THREADMUTEXOBJECT_H_ 21 | #define THREADMUTEXOBJECT_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | template 29 | class ThreadMutexObject { 30 | public: 31 | ThreadMutexObject() {} 32 | 33 | ThreadMutexObject(T initialValue) : object(initialValue), lastCopy(initialValue) {} 34 | 35 | void assign(T newValue) { 36 | mutex.lock(); 37 | 38 | object = lastCopy = newValue; 39 | 40 | mutex.unlock(); 41 | } 42 | 43 | std::mutex& getMutex() { 44 | return mutex; 45 | } 46 | 47 | T& getReference() { 48 | return object; 49 | } 50 | 51 | void assignAndNotifyAll(T newValue) { 52 | mutex.lock(); 53 | 54 | object = newValue; 55 | 56 | signal.notify_all(); 57 | 58 | mutex.unlock(); 59 | } 60 | 61 | void notifyAll() { 62 | mutex.lock(); 63 | 64 | signal.notify_all(); 65 | 66 | mutex.unlock(); 67 | } 68 | 69 | T getValue() { 70 | mutex.lock(); 71 | 72 | lastCopy = object; 73 | 74 | mutex.unlock(); 75 | 76 | return lastCopy; 77 | } 78 | 79 | T waitForSignal() { 80 | mutex.lock(); 81 | 82 | signal.wait(mutex); 83 | 84 | lastCopy = object; 85 | 86 | mutex.unlock(); 87 | 88 | return lastCopy; 89 | } 90 | 91 | T getValueWait(int wait = 33000) { 92 | std::this_thread::sleep_for(std::chrono::microseconds(wait)); 93 | 94 | mutex.lock(); 95 | 96 | lastCopy = object; 97 | 98 | mutex.unlock(); 99 | 100 | return lastCopy; 101 | } 102 | 103 | T& getReferenceWait(int wait = 33000) { 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 | void operator++(int) { 116 | mutex.lock(); 117 | 118 | object++; 119 | 120 | mutex.unlock(); 121 | } 122 | 123 | private: 124 | T object; 125 | T lastCopy; 126 | std::mutex mutex; 127 | std::condition_variable_any signal; 128 | }; 129 | 130 | #endif /* THREADMUTEXOBJECT_H_ */ 131 | --------------------------------------------------------------------------------