├── GPUTest ├── 1c.png ├── 1d.png ├── 2c.png ├── 2d.png └── src │ └── CMakeLists.txt ├── Tips.md ├── .gitignore ├── Tag_Note.md ├── GUI └── src │ ├── FindRealSense.cmake │ ├── .vscode │ ├── c_cpp_properties.json │ └── settings.json │ ├── Findefusion.cmake │ ├── Main.cpp │ ├── Tools │ ├── CameraInterface.h │ ├── GroundTruthOdometry.h │ ├── RawLogReader.h │ ├── LiveLogReader.h │ ├── RealSenseInterface.cpp │ ├── GroundTruthOdometry.cpp │ ├── RealSenseInterface.h │ ├── LogReader.h │ ├── ThreadMutexObject.h │ └── JPEGLoader.h │ ├── FindOpenNI2.cmake │ ├── CMakeLists.txt │ ├── MainController.h │ └── FindSuiteSparse.cmake ├── Core └── src │ ├── Utils │ ├── Resolution.cpp │ ├── Intrinsics.cpp │ ├── WindowsExtras.h │ ├── CholeskyDecomp.h │ ├── WindowsExtras.cpp │ ├── GraphNode.h │ ├── Jacobian.h │ ├── Img.h │ ├── Intrinsics.h │ ├── OrderedJacobianRow.h │ ├── Resolution.h │ ├── Parse.cpp │ ├── CholeskyDecomp.cpp │ ├── Parse.h │ └── RGBDOdometry.h │ ├── Defines.h │ ├── Shaders │ ├── empty.vert │ ├── draw_feedback.frag │ ├── resize.frag │ ├── Vertex.h │ ├── depth_norm.frag │ ├── sample.vert │ ├── depth_metric.frag │ ├── visualise_textures.frag │ ├── fill_rgb.frag │ ├── sample.geom │ ├── Vertex.cpp │ ├── color.glsl │ ├── init_unstable.vert │ ├── index_map.frag │ ├── draw_global_surface.frag │ ├── data.frag │ ├── copy_unstable.geom │ ├── vertex_feedback.geom │ ├── quad.geom │ ├── depth_splat.frag │ ├── ComputePack.h │ ├── data.geom │ ├── fill_normal.frag │ ├── surfels.glsl │ ├── fill_vertex.frag │ ├── draw_feedback.vert │ ├── draw_global_surface.vert │ ├── combo_splat.frag │ ├── FillIn.h │ ├── index_map.vert │ ├── vertex_feedback.vert │ ├── draw_global_surface_phong.frag │ ├── ComputePack.cpp │ ├── FeedbackBuffer.h │ ├── depth_bilateral.frag │ ├── geometry.glsl │ ├── update.vert │ ├── Resize.h │ ├── splat.vert │ ├── fxaa.frag │ ├── draw_global_surface.geom │ └── Uniform.h │ ├── PoseMatch.h │ ├── CudaComputeTargetFlags.cmake │ ├── GPUTexture.cpp │ ├── GPUTexture.h │ ├── Cuda │ ├── convenience.cuh │ ├── containers │ │ ├── device_memory_impl.hpp │ │ └── kernel_containers.hpp │ └── operators.cuh │ ├── CMakeLists.txt │ ├── Deformation.h │ └── FindSuiteSparse.cmake ├── .VSCodeCounter └── results.md └── .vscode └── settings.json /GPUTest/1c.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DreamWaterFound/ElasticFusion-annotation/HEAD/GPUTest/1c.png -------------------------------------------------------------------------------- /GPUTest/1d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DreamWaterFound/ElasticFusion-annotation/HEAD/GPUTest/1d.png -------------------------------------------------------------------------------- /GPUTest/2c.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DreamWaterFound/ElasticFusion-annotation/HEAD/GPUTest/2c.png -------------------------------------------------------------------------------- /GPUTest/2d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DreamWaterFound/ElasticFusion-annotation/HEAD/GPUTest/2d.png -------------------------------------------------------------------------------- /Tips.md: -------------------------------------------------------------------------------- 1 | 使用CUDA 10. 2 | 3 | NVCC的编译选项仅编译了计算能力６.1。有需要可以在CMakeLists.txt中进行相应的改动 4 | 5 | 不过CUDA10中有些特性已经被deprecated，方便起见屏蔽警告。 6 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | deps/ 3 | .project 4 | .cproject 5 | *.user 6 | **/.VSCodeCounter/** 7 | **/sourcetrail/** 8 | GUI/*.freiburg 9 | GUI/*.ply 10 | -------------------------------------------------------------------------------- /Tag_Note.md: -------------------------------------------------------------------------------- 1 | # Tag Note 2 | 3 | 主要用来备忘, 模块计时模块中的字符串描述符都对应的是什么过程. 4 | 5 | - LogRead 从记录文件中读取一张图像的时间 6 | - Run 执行eFusion->processFrame()一次(也就是送一帧图像, 进行处理所耗费的时间) 7 | - GUI 绘制GUI界面, 更新各种模型所耗费的时间 8 | - Global 绘制 global model 所耗费的时间 9 | 10 | 11 | -------------------------------------------------------------------------------- /GUI/src/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 | -------------------------------------------------------------------------------- /Core/src/Utils/Resolution.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Resolution.cpp 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 分辨率对象的功能实现 5 | * @version 0.1 6 | * @date 2020-01-03 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file was written for porting ElasticFusion to windows 14 | * by Filip Srajer (filip.srajer@inf.ethz.ch). 15 | * 16 | */ 17 | 18 | #include "Resolution.h" 19 | 20 | // 获取给定分辨率的分辨率对象实例 21 | const Resolution & Resolution::getInstance(int width,int height) 22 | { 23 | static const Resolution instance(width,height); 24 | return instance; 25 | } 26 | 27 | -------------------------------------------------------------------------------- /Core/src/Utils/Intrinsics.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Intrinsics.cpp 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 相机内参类对象的实现 5 | * @version 0.1 6 | * @date 2020-01-03 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file was written for porting ElasticFusion to windows 14 | * by Filip Srajer (filip.srajer@inf.ethz.ch). 15 | * 16 | */ 17 | 18 | #include "Intrinsics.h" 19 | 20 | // 使用给定内参生成内参类对象 21 | const Intrinsics & Intrinsics::getInstance(float fx,float fy,float cx,float cy) 22 | { 23 | static const Intrinsics instance(fx,fy,cx,cy); 24 | return instance; 25 | } 26 | 27 | -------------------------------------------------------------------------------- /Core/src/Utils/WindowsExtras.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file WindowsExtras.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 由于 windows 下没有Windows下的一些函数, 所以这里撰写了用于 windows 下工作的同名替代函数 5 | * @version 0.1 6 | * @date 2020-01-07 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file was written for porting ElasticFusion to windows 14 | * by Filip Srajer (filip.srajer@inf.ethz.ch). 15 | * 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "../Defines.h" 21 | 22 | #ifdef WIN32 23 | 24 | EFUSION_API int gettimeofday(struct timeval * tp,struct timezone * tzp); 25 | 26 | EFUSION_API void *mempcpy(void *dest,const void *src,size_t n); 27 | 28 | #endif // WIN32 -------------------------------------------------------------------------------- /Core/src/Defines.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Defines.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 一些定义 5 | * @version 0.1 6 | * @date 2020-01-06 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file was written for porting ElasticFusion to windows 14 | * by Filip Srajer (filip.srajer@inf.ethz.ch). 15 | * 16 | */ 17 | 18 | #pragma once 19 | 20 | #ifndef WIN32 21 | // Linux 下就没有这么多事情 22 | # define EFUSION_API 23 | #else 24 | // windows 下编译的 *.dll 文件有权限限制, 外部程序如果想使用 dll 中的内容, 它自己需要 dllimport(其实不用也可以); 25 | // 同样我们必须需要给暴露给外部程序的函数前面添加这个 dllexport 26 | // ref:[https://blog.csdn.net/huangyimo/article/details/81748939] 27 | # ifdef efusion_EXPORTS 28 | # define EFUSION_API __declspec(dllexport) 29 | # else 30 | # define EFUSION_API __declspec(dllimport) 31 | # endif 32 | #endif 33 | -------------------------------------------------------------------------------- /GUI/src/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "${workspaceFolder}/../../Core/src", 8 | "${workspaceFolder}/tools", 9 | "/home/guoqing/libraries/Pangolin_ElasticFusion/include", 10 | "/usr/include/openni2", 11 | "/usr/local/cuda-10.0/include", 12 | "/home/guoqing/libraries/eigen_333/include/eigen3" 13 | ], 14 | "defines": ["USE_OPENNI_PANGOLIN"], 15 | "compilerPath": "/usr/bin/gcc", 16 | "cStandard": "c11", 17 | "cppStandard": "c++17", 18 | "intelliSenseMode": "clang-x64" 19 | } 20 | ], 21 | "version": 4 22 | } 23 | -------------------------------------------------------------------------------- /Core/src/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/src/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/src/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/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef VERTEX_H_ 20 | #define VERTEX_H_ 21 | 22 | #include 23 | 24 | #include "../Defines.h" 25 | 26 | class Vertex 27 | { 28 | public: 29 | EFUSION_API static const int SIZE; 30 | 31 | private: 32 | Vertex(){} 33 | }; 34 | 35 | 36 | #endif /* VERTEX_H_ */ 37 | -------------------------------------------------------------------------------- /GUI/src/Findefusion.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Find efusion 3 | # 4 | # This sets the following variables: 5 | # EFUSION_FOUND - True if EFUSION was found. 6 | # EFUSION_INCLUDE_DIRS - Directories containing the EFUSION include files. 7 | # EFUSION_LIBRARIES - Libraries needed to use EFUSION. 8 | 9 | find_path(EFUSION_INCLUDE_DIR ElasticFusion.h 10 | PATHS 11 | ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/src 12 | PATH_SUFFIXES Core 13 | ) 14 | 15 | find_library(EFUSION_LIBRARY 16 | NAMES libefusion.so 17 | PATHS 18 | ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/build 19 | ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/src/build 20 | PATH_SUFFIXES ${EFUSION_PATH_SUFFIXES} 21 | ) 22 | 23 | set(EFUSION_INCLUDE_DIRS ${EFUSION_INCLUDE_DIR}) 24 | set(EFUSION_LIBRARIES ${EFUSION_LIBRARY}) 25 | 26 | include(FindPackageHandleStandardArgs) 27 | find_package_handle_standard_args(EFUSION DEFAULT_MSG EFUSION_LIBRARY EFUSION_INCLUDE_DIR) 28 | 29 | if(NOT WIN32) 30 | mark_as_advanced(EFUSION_LIBRARY EFUSION_INCLUDE_DIR) 31 | endif() 32 | -------------------------------------------------------------------------------- /GPUTest/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6.0) 2 | 3 | project(GPUTest) 4 | 5 | find_package(Pangolin 0.1 REQUIRED) 6 | find_package(CUDA REQUIRED) 7 | 8 | set(efusion_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../../Core/src" CACHE PATH "Where ElasticFusion.h lives") 9 | set(efusion_LIBRARY "${CMAKE_CURRENT_SOURCE_DIR}/../../Core/build/libefusion.so" CACHE FILEPATH "Where libefusion.so lives") 10 | 11 | include_directories(/usr/include/suitesparse) 12 | include_directories(${CUDA_INCLUDE_DIRS}) 13 | include_directories(${EIGEN_INCLUDE_DIRS}) 14 | include_directories(${Pangolin_INCLUDE_DIRS}) 15 | include_directories(${efusion_INCLUDE_DIR}) 16 | 17 | file(GLOB srcs *.cpp) 18 | 19 | set(CMAKE_CXX_FLAGS "-O3 -msse2 -msse3 -Wall -std=c++11") 20 | #set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} " -g -Wall") 21 | 22 | add_executable(GPUTest 23 | ${srcs} 24 | ) 25 | 26 | target_link_libraries(GPUTest 27 | ${efusion_LIBRARY} 28 | ${Eigen_LIBRARIES} 29 | ${Pangolin_LIBRARIES} 30 | ${CUDA_LIBRARIES} 31 | ${SUITESPARSE_LIBRARIES} 32 | ) 33 | 34 | -------------------------------------------------------------------------------- /Core/src/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/src/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/src/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/src/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/src/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/src/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/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "Vertex.h" 20 | 21 | /* 22 | * OK this is the structure 23 | * 24 | *-------------------- 25 | * vec3 position 26 | * float confidence 27 | * 28 | * float color (encoded as a 24-bit integer) 29 | * float 30 | * float initTime 31 | * float timestamp 32 | * 33 | * vec3 normal 34 | * float radius 35 | *-------------------- 36 | 37 | * Which is three vec4s 38 | */ 39 | 40 | const int Vertex::SIZE = sizeof(Eigen::Vector4f) * 3; 41 | -------------------------------------------------------------------------------- /Core/src/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 | -------------------------------------------------------------------------------- /GUI/src/Main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Main.cpp 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief GUI 界面启动器 5 | * @version 0.1 6 | * @date 2020-01-03 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | 31 | #include "MainController.h" 32 | 33 | // 说是主要文件, 但是主要的内容还都是在 MainController 这个类中实现的 34 | int main(int argc, char * argv[]) 35 | { 36 | MainController mainController(argc, argv); 37 | 38 | mainController.launch(); 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /Core/src/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/src/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/src/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/src/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/src/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/src/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 | -------------------------------------------------------------------------------- /.VSCodeCounter/results.md: -------------------------------------------------------------------------------- 1 | # Summary 2 | 3 | Date : 2019-12-30 11:23:43 4 | 5 | Directory /home/guoqing/SLAM/ElasticFusion/ElasticFusion-run 6 | 7 | Total : 89 files, 11200 codes, 2014 comments, 3350 blanks, all 16564 lines 8 | 9 | [details](details.md) 10 | 11 | ## Languages 12 | | language | files | code | comment | blank | total | 13 | | :--- | ---: | ---: | ---: | ---: | ---: | 14 | | C++ | 32 | 5,660 | 905 | 1,790 | 8,355 | 15 | | CUDA C++ | 46 | 4,253 | 1,104 | 1,357 | 6,714 | 16 | | CMake | 8 | 1,093 | 0 | 136 | 1,229 | 17 | | Markdown | 2 | 141 | 0 | 60 | 201 | 18 | | Shell Script | 1 | 53 | 5 | 7 | 65 | 19 | 20 | ## Directories 21 | | path | files | code | comment | blank | total | 22 | | :--- | ---: | ---: | ---: | ---: | ---: | 23 | | . | 89 | 11,200 | 2,014 | 3,350 | 16,564 | 24 | | Core | 61 | 7,795 | 1,727 | 2,473 | 11,995 | 25 | | Core/src | 61 | 7,795 | 1,727 | 2,473 | 11,995 | 26 | | Core/src/Cuda | 12 | 2,006 | 747 | 673 | 3,426 | 27 | | Core/src/Cuda/containers | 6 | 444 | 430 | 205 | 1,079 | 28 | | Core/src/Shaders | 12 | 640 | 222 | 257 | 1,119 | 29 | | Core/src/Utils | 21 | 2,235 | 357 | 689 | 3,281 | 30 | | GPUTest | 1 | 234 | 17 | 83 | 334 | 31 | | GPUTest/src | 1 | 234 | 17 | 83 | 334 | 32 | | GUI | 24 | 2,977 | 265 | 727 | 3,969 | 33 | | GUI/src | 24 | 2,977 | 265 | 727 | 3,969 | 34 | | GUI/src/Tools | 15 | 1,523 | 214 | 485 | 2,222 | 35 | 36 | [details](details.md) -------------------------------------------------------------------------------- /Core/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef UTILS_CHOLESKYDECOMP_H_ 20 | #define UTILS_CHOLESKYDECOMP_H_ 21 | 22 | #include 23 | #include 24 | 25 | #include "Jacobian.h" 26 | 27 | class CholeskyDecomp 28 | { 29 | public: 30 | CholeskyDecomp(); 31 | virtual ~CholeskyDecomp(); 32 | 33 | void freeFactor(); 34 | 35 | Eigen::VectorXd solve(const Jacobian & jacobian, const Eigen::VectorXd & residual, const bool firstRun); 36 | 37 | private: 38 | cholmod_common Common; 39 | cholmod_factor * L; 40 | }; 41 | 42 | #endif /* UTILS_CHOLESKYDECOMP_H_ */ 43 | -------------------------------------------------------------------------------- /Core/src/Utils/WindowsExtras.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file WindowsExtras.cpp 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 由于 windows 下没有Windows下的一些函数, 所以这里撰写了用于 windows 下工作的同名替代函数 5 | * @version 0.1 6 | * @date 2020-01-07 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file was written for porting ElasticFusion to windows 14 | * by Filip Srajer (filip.srajer@inf.ethz.ch). 15 | * 16 | */ 17 | 18 | #include "WindowsExtras.h" 19 | 20 | #ifdef WIN32 21 | 22 | #include 23 | #include 24 | #define far 25 | #include 26 | #include 27 | 28 | int gettimeofday(struct timeval * tp,struct timezone * tzp) 29 | { 30 | // Note: some broken versions only have 8 trailing zero's, the correct epoch has 9 trailing zero's 31 | static const uint64_t EPOCH = ((uint64_t)116444736000000000ULL); 32 | 33 | SYSTEMTIME system_time; 34 | FILETIME file_time; 35 | uint64_t time; 36 | 37 | GetSystemTime(&system_time); 38 | SystemTimeToFileTime(&system_time,&file_time); 39 | time = ((uint64_t)file_time.dwLowDateTime); 40 | time += ((uint64_t)file_time.dwHighDateTime) << 32; 41 | 42 | tp->tv_sec = (long)((time - EPOCH) / 10000000L); 43 | tp->tv_usec = (long)(system_time.wMilliseconds * 1000); 44 | return 0; 45 | } 46 | 47 | void *mempcpy(void *dest,const void *src,size_t n) 48 | { 49 | return (char *)memcpy(dest,src,n) + n; 50 | } 51 | 52 | #endif // WIN32 -------------------------------------------------------------------------------- /Core/src/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 | -------------------------------------------------------------------------------- /GUI/src/Tools/CameraInterface.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file CameraInterface.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 相机接口虚基类 5 | * @version 0.1 6 | * @date 2020-01-04 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #pragma once 13 | 14 | // STL 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | // 加强的线程互斥锁对象 21 | #include "ThreadMutexObject.h" 22 | 23 | /** @brief 相机接口虚基类 */ 24 | class CameraInterface 25 | { 26 | public: 27 | /** @brief 默认析构函数 */ 28 | virtual ~CameraInterface() {} 29 | 30 | /** 31 | * @brief 判断相机是否正确初始化 32 | * @return true 33 | * @return false 34 | */ 35 | virtual bool ok() = 0; 36 | 37 | /** 38 | * @brief 获取错误描述字符串 39 | * @return std::string 错误描述字符串 40 | */ 41 | virtual std::string error() = 0; 42 | 43 | static const int numBuffers = 10; ///< 图像缓冲区大小, 指的是缓冲的图像的个数 44 | 45 | ThreadMutexObject latestDepthIndex; ///< 被线程锁保护的变量, 记录最近一帧的深度图id 46 | std::pair,int64_t> frameBuffers[numBuffers]; ///< 帧图像缓冲区, 前两个应该分别是深度图和彩色图, 最后是深度图像的时间戳 47 | 48 | /** 49 | * @brief 设置自动曝光 50 | * @param[in] value 是否 51 | */ 52 | virtual void setAutoExposure(bool value) = 0; 53 | 54 | /** 55 | * @brief 设置自动白平衡 56 | * @param[in] value 是否 57 | */ 58 | virtual void setAutoWhiteBalance(bool value) = 0; 59 | }; 60 | -------------------------------------------------------------------------------- /Core/src/Utils/GraphNode.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GraphNode.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief Deformation Node Type 5 | * @version 0.1 6 | * @date 2020-01-09 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef GRAPHNODE_H_ 31 | #define GRAPHNODE_H_ 32 | 33 | #include 34 | 35 | /** @brief Deformation Graph Node */ 36 | class GraphNode 37 | { 38 | public: 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | GraphNode() 41 | {} 42 | 43 | int id; 44 | Eigen::Vector3f position; 45 | Eigen::Matrix3f rotation; 46 | Eigen::Vector3f translation; 47 | std::vector neighbours; 48 | bool enabled; 49 | }; 50 | 51 | #endif /* GRAPHNODE_H_ */ 52 | -------------------------------------------------------------------------------- /Core/src/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/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef COMPUTEPACK_H_ 20 | #define COMPUTEPACK_H_ 21 | 22 | #include "Shaders.h" 23 | #include "../Utils/Resolution.h" 24 | #include "Uniform.h" 25 | #include 26 | 27 | class ComputePack 28 | { 29 | public: 30 | ComputePack(std::shared_ptr program, 31 | pangolin::GlTexture * target); 32 | 33 | virtual ~ComputePack(); 34 | 35 | static const std::string NORM, FILTER, METRIC, METRIC_FILTERED; 36 | 37 | void compute(pangolin::GlTexture * input, const std::vector * const uniforms = 0); 38 | 39 | private: 40 | std::shared_ptr program; 41 | pangolin::GlRenderBuffer renderBuffer; 42 | pangolin::GlTexture * target; 43 | pangolin::GlFramebuffer frameBuffer; 44 | }; 45 | 46 | #endif /* COMPUTEPACK_H_ */ 47 | -------------------------------------------------------------------------------- /Core/src/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/src/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/src/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/src/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 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "ostream": "cpp", 4 | "cctype": "cpp", 5 | "clocale": "cpp", 6 | "cmath": "cpp", 7 | "cstdarg": "cpp", 8 | "cstddef": "cpp", 9 | "cstdio": "cpp", 10 | "cstdlib": "cpp", 11 | "cstring": "cpp", 12 | "ctime": "cpp", 13 | "cwchar": "cpp", 14 | "cwctype": "cpp", 15 | "array": "cpp", 16 | "atomic": "cpp", 17 | "strstream": "cpp", 18 | "*.tcc": "cpp", 19 | "chrono": "cpp", 20 | "complex": "cpp", 21 | "condition_variable": "cpp", 22 | "cstdint": "cpp", 23 | "deque": "cpp", 24 | "unordered_map": "cpp", 25 | "vector": "cpp", 26 | "exception": "cpp", 27 | "fstream": "cpp", 28 | "functional": "cpp", 29 | "initializer_list": "cpp", 30 | "iomanip": "cpp", 31 | "iosfwd": "cpp", 32 | "iostream": "cpp", 33 | "istream": "cpp", 34 | "limits": "cpp", 35 | "mutex": "cpp", 36 | "new": "cpp", 37 | "numeric": "cpp", 38 | "ratio": "cpp", 39 | "sstream": "cpp", 40 | "stdexcept": "cpp", 41 | "streambuf": "cpp", 42 | "system_error": "cpp", 43 | "thread": "cpp", 44 | "cinttypes": "cpp", 45 | "tuple": "cpp", 46 | "type_traits": "cpp", 47 | "utility": "cpp", 48 | "typeinfo": "cpp", 49 | "algorithm": "cpp", 50 | "optional": "cpp", 51 | "string_view": "cpp", 52 | "memory": "cpp", 53 | "iterator": "cpp", 54 | "map": "cpp", 55 | "memory_resource": "cpp", 56 | "random": "cpp", 57 | "string": "cpp", 58 | "typeindex": "cpp" 59 | } 60 | } -------------------------------------------------------------------------------- /Core/src/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/src/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 | -------------------------------------------------------------------------------- /GUI/src/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "array": "cpp", 4 | "*.tcc": "cpp", 5 | "bitset": "cpp", 6 | "fstream": "cpp", 7 | "functional": "cpp", 8 | "iomanip": "cpp", 9 | "istream": "cpp", 10 | "limits": "cpp", 11 | "mutex": "cpp", 12 | "ostream": "cpp", 13 | "numeric": "cpp", 14 | "streambuf": "cpp", 15 | "tuple": "cpp", 16 | "utility": "cpp", 17 | "memory": "cpp", 18 | "sstream": "cpp", 19 | "thread": "cpp", 20 | "*.txx": "cpp", 21 | "cctype": "cpp", 22 | "clocale": "cpp", 23 | "cmath": "cpp", 24 | "cstdarg": "cpp", 25 | "cstddef": "cpp", 26 | "cstdio": "cpp", 27 | "cstdlib": "cpp", 28 | "cstring": "cpp", 29 | "ctime": "cpp", 30 | "cwchar": "cpp", 31 | "cwctype": "cpp", 32 | "atomic": "cpp", 33 | "strstream": "cpp", 34 | "chrono": "cpp", 35 | "complex": "cpp", 36 | "condition_variable": "cpp", 37 | "cstdint": "cpp", 38 | "deque": "cpp", 39 | "unordered_map": "cpp", 40 | "vector": "cpp", 41 | "exception": "cpp", 42 | "initializer_list": "cpp", 43 | "iosfwd": "cpp", 44 | "iostream": "cpp", 45 | "new": "cpp", 46 | "ratio": "cpp", 47 | "stdexcept": "cpp", 48 | "system_error": "cpp", 49 | "cinttypes": "cpp", 50 | "type_traits": "cpp", 51 | "typeindex": "cpp", 52 | "typeinfo": "cpp", 53 | "algorithm": "cpp", 54 | "optional": "cpp", 55 | "string_view": "cpp", 56 | "iterator": "cpp", 57 | "map": "cpp", 58 | "memory_resource": "cpp", 59 | "random": "cpp", 60 | "string": "cpp", 61 | "cfloat": "cpp" 62 | } 63 | } -------------------------------------------------------------------------------- /Core/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef POSEMATCH_H_ 20 | #define POSEMATCH_H_ 21 | 22 | #include 23 | #include "Ferns.h" 24 | 25 | /** @brief // ? 由于 Local Loop 和 Graph Loop 带来的配准对齐的约束? */ 26 | class PoseMatch 27 | { 28 | public: 29 | PoseMatch(int firstId, 30 | int secondId, 31 | const Eigen::Matrix4f & first, 32 | const Eigen::Matrix4f & second, 33 | const std::vector & constraints, 34 | const bool & fern) 35 | : firstId(firstId), 36 | secondId(secondId), 37 | first(first), 38 | second(second), 39 | constraints(constraints), 40 | fern(fern) 41 | {} 42 | 43 | int firstId; ///? 什么的id? 44 | int secondId; ///? 同上? 45 | Eigen::Matrix4f first; 46 | Eigen::Matrix4f second; 47 | std::vector constraints; ///? 约束, 一对点一对点 48 | bool fern; ///< 是否是由 Global Loop 导致的 49 | }; 50 | 51 | #endif /* POSEMATCH_H_ */ 52 | -------------------------------------------------------------------------------- /Core/src/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/src/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() -------------------------------------------------------------------------------- /Core/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef FILLIN_H_ 20 | #define FILLIN_H_ 21 | 22 | #include "Shaders.h" 23 | #include "Uniform.h" 24 | #include "../Utils/Resolution.h" 25 | #include "../Utils/Intrinsics.h" 26 | #include "../GPUTexture.h" 27 | 28 | class FillIn 29 | { 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 | 57 | -------------------------------------------------------------------------------- /Core/src/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/src/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/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef UTILS_JACOBIAN_H_ 20 | #define UTILS_JACOBIAN_H_ 21 | 22 | #include 23 | 24 | #include "OrderedJacobianRow.h" 25 | 26 | class Jacobian 27 | { 28 | public: 29 | Jacobian() 30 | : columns(0) 31 | {} 32 | 33 | virtual ~Jacobian() 34 | { 35 | reset(); 36 | } 37 | 38 | void assign(std::vector & rows, const int columns) 39 | { 40 | reset(); 41 | this->rows = rows; 42 | this->columns = columns; 43 | } 44 | 45 | int cols() const 46 | { 47 | return columns; 48 | } 49 | 50 | int nonZero() const 51 | { 52 | int count = 0; 53 | for(size_t i = 0; i < rows.size(); i++) 54 | { 55 | count += rows[i]->nonZeros(); 56 | } 57 | return count; 58 | } 59 | 60 | std::vector rows; 61 | 62 | private: 63 | int columns; 64 | 65 | void reset() 66 | { 67 | for(size_t i = 0; i < rows.size(); i++) 68 | { 69 | delete rows[i]; 70 | } 71 | rows.clear(); 72 | } 73 | }; 74 | 75 | 76 | 77 | #endif /* UTILS_JACOBIAN_H_ */ 78 | -------------------------------------------------------------------------------- /Core/src/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/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef UTILS_IMG_H_ 20 | #define UTILS_IMG_H_ 21 | 22 | #include 23 | 24 | template 25 | class Img 26 | { 27 | public: 28 | Img(const int rows, const int cols) 29 | : rows(rows), 30 | cols(cols), 31 | data(new unsigned char[rows * cols * sizeof(T)]), 32 | owned(true) 33 | {} 34 | 35 | Img(const int rows, const int cols, T * data) 36 | : rows(rows), 37 | cols(cols), 38 | data((unsigned char *)data), 39 | owned(false) 40 | {} 41 | 42 | virtual ~Img() 43 | { 44 | if(owned) 45 | { 46 | delete [] data; 47 | } 48 | } 49 | 50 | const int rows; 51 | const int cols; 52 | unsigned char * data; 53 | const bool owned; 54 | 55 | template inline 56 | V & at(const int i) 57 | { 58 | return ((V*)data)[i]; 59 | } 60 | 61 | template inline 62 | V & at(const int row, const int col) 63 | { 64 | return ((V*)data)[cols * row + col]; 65 | } 66 | 67 | template inline const 68 | V & at(const int row, const int col) const 69 | { 70 | return ((const V*)data)[cols * row + col]; 71 | } 72 | }; 73 | 74 | #endif /* UTILS_IMG_H_ */ 75 | -------------------------------------------------------------------------------- /Core/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "ComputePack.h" 20 | 21 | const std::string ComputePack::NORM = "NORM"; 22 | const std::string ComputePack::FILTER = "FILTER"; 23 | const std::string ComputePack::METRIC = "METRIC"; 24 | const std::string ComputePack::METRIC_FILTERED = "METRIC_FILTERED"; 25 | 26 | ComputePack::ComputePack(std::shared_ptr program, 27 | pangolin::GlTexture * target) 28 | : program(program), 29 | renderBuffer(Resolution::getInstance().width(), Resolution::getInstance().height()), 30 | target(target) 31 | { 32 | frameBuffer.AttachColour(*target); 33 | frameBuffer.AttachDepth(renderBuffer); 34 | } 35 | 36 | ComputePack::~ComputePack() 37 | { 38 | 39 | } 40 | 41 | void ComputePack::compute(pangolin::GlTexture * input, const std::vector * const uniforms) 42 | { 43 | input->Bind(); 44 | 45 | frameBuffer.Bind(); 46 | 47 | glPushAttrib(GL_VIEWPORT_BIT); 48 | 49 | glViewport(0, 0, renderBuffer.width, renderBuffer.height); 50 | 51 | glClearColor(0, 0, 0, 0); 52 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 53 | 54 | program->Bind(); 55 | 56 | if(uniforms) 57 | { 58 | for(size_t i = 0; i < uniforms->size(); i++) 59 | { 60 | program->setUniform(uniforms->at(i)); 61 | } 62 | } 63 | 64 | glDrawArrays(GL_POINTS, 0, 1); 65 | 66 | frameBuffer.Unbind(); 67 | 68 | program->Unbind(); 69 | 70 | glPopAttrib(); 71 | 72 | glFinish(); 73 | } 74 | -------------------------------------------------------------------------------- /Core/src/Utils/Intrinsics.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Intrinsics.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 相机内参对象, 包含 fx fy cx cy 5 | * @version 0.1 6 | * @date 2020-01-03 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef INTRINSICS_H_ 31 | #define INTRINSICS_H_ 32 | 33 | #include 34 | 35 | #include "../Defines.h" 36 | 37 | /** @brief 保存相机内参 fx fy cx cy 的类 */ 38 | class Intrinsics 39 | { 40 | public: 41 | /** 42 | * @brief 使用给定的内参数, 获取内参类对象 静态成员函数 43 | * @param[in] fx 44 | * @param[in] fy 45 | * @param[in] cx 46 | * @param[in] cy 47 | * @return EFUSION_API const& getInstance 内参类对象的引用 48 | */ 49 | EFUSION_API static const Intrinsics & getInstance(float fx = 0,float fy = 0,float cx = 0,float cy = 0); 50 | 51 | const float & fx() const 52 | { 53 | return fx_; 54 | } 55 | 56 | const float & fy() const 57 | { 58 | return fy_; 59 | } 60 | 61 | const float & cx() const 62 | { 63 | return cx_; 64 | } 65 | 66 | const float & cy() const 67 | { 68 | return cy_; 69 | } 70 | 71 | private: 72 | Intrinsics(float fx, float fy, float cx, float cy) 73 | : fx_(fx), 74 | fy_(fy), 75 | cx_(cx), 76 | cy_(cy) 77 | { 78 | assert(fx != 0 && fy != 0 && "You haven't initialised the Intrinsics class!"); 79 | } 80 | 81 | const float fx_, fy_, cx_, cy_; 82 | }; 83 | 84 | #endif /* INTRINSICS_H_ */ 85 | -------------------------------------------------------------------------------- /Core/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef FEEDBACKBUFFER_H_ 20 | #define FEEDBACKBUFFER_H_ 21 | 22 | #include "Shaders.h" 23 | #include "Uniform.h" 24 | #include "Vertex.h" 25 | #include "../Utils/Resolution.h" 26 | #include "../Utils/Intrinsics.h" 27 | #include 28 | #include 29 | 30 | #include "../Defines.h" 31 | 32 | /** @brief //? 疑似和绘制点云有关的类型 */ 33 | class FeedbackBuffer 34 | { 35 | public: 36 | FeedbackBuffer(std::shared_ptr program); 37 | virtual ~FeedbackBuffer(); 38 | 39 | std::shared_ptr program; 40 | 41 | void compute(pangolin::GlTexture * color, 42 | pangolin::GlTexture * depth, 43 | const int & time, 44 | const float depthCutoff); 45 | 46 | /** 47 | * @brief // ? 渲染点云? 48 | * @param[in] mvp 虚拟观察相机所在的位姿 49 | * @param[in] pose 当前相机的位姿 50 | * @param[in] drawNormals 是否使用法向贴图 51 | * @param[in] drawColors 是否使用RGB纹理贴图 52 | * @return EFUSION_API render 53 | */ 54 | EFUSION_API void render(pangolin::OpenGlMatrix mvp, const Eigen::Matrix4f & pose, const bool drawNormals, const bool drawColors); 55 | 56 | EFUSION_API static const std::string RAW, FILTERED; 57 | 58 | GLuint vbo; 59 | GLuint fid; 60 | 61 | private: 62 | std::shared_ptr drawProgram; 63 | GLuint uvo; 64 | GLuint countQuery; 65 | const int bufferSize; 66 | unsigned int count; 67 | }; 68 | 69 | #endif /* FEEDBACKBUFFER_H_ */ 70 | -------------------------------------------------------------------------------- /Core/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef UTILS_ORDEREDJACOBIANROW_H_ 20 | #define UTILS_ORDEREDJACOBIANROW_H_ 21 | 22 | #include 23 | #include 24 | 25 | class OrderedJacobianRow 26 | { 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 | 36 | virtual ~OrderedJacobianRow() 37 | { 38 | delete [] indices; 39 | delete [] vals; 40 | } 41 | 42 | //You have to use this in an ordered fashion for efficiency :) 43 | void append(const int index, const double value) 44 | { 45 | assert(index > lastIndex); 46 | indexSlotMap[index] = lastSlot; 47 | indices[lastSlot] = index; 48 | vals[lastSlot] = value; 49 | lastSlot++; 50 | lastIndex = index; 51 | } 52 | 53 | //To add to an existing and already weighted value 54 | void addTo(const int index, const double value, const double weight) 55 | { 56 | double & val = vals[indexSlotMap[index]]; 57 | val = ((val / weight) + value) * weight; 58 | } 59 | 60 | int nonZeros() 61 | { 62 | return lastSlot; 63 | } 64 | 65 | int * indices; 66 | double * vals; 67 | 68 | private: 69 | int lastSlot; 70 | int lastIndex; 71 | const int maxNonZero; 72 | std::unordered_map indexSlotMap; 73 | }; 74 | 75 | 76 | #endif /* UTILS_ORDEREDJACOBIANROW_H_ */ 77 | -------------------------------------------------------------------------------- /Core/src/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/src/GPUTexture.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GPUTexture.cpp 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief GPU中的纹理对象 5 | * @version 0.1 6 | * @date 2020-01-06 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #include "GPUTexture.h" 31 | 32 | const std::string GPUTexture::RGB = "RGB"; 33 | const std::string GPUTexture::DEPTH_RAW = "DEPTH"; 34 | const std::string GPUTexture::DEPTH_FILTERED = "DEPTH_FILTERED"; 35 | const std::string GPUTexture::DEPTH_METRIC = "DEPTH_METRIC"; 36 | const std::string GPUTexture::DEPTH_METRIC_FILTERED = "DEPTH_METRIC_FILTERED"; 37 | const std::string GPUTexture::DEPTH_NORM = "DEPTH_NORM"; 38 | 39 | // 构造函数 40 | GPUTexture::GPUTexture(const int width, 41 | const int height, 42 | const GLenum internalFormat, 43 | const GLenum format, 44 | const GLenum dataType, 45 | const bool draw, 46 | const bool cuda) 47 | : texture(new pangolin::GlTexture(width, height, internalFormat, draw, 0, format, dataType)), 48 | draw(draw), 49 | width(width), 50 | height(height), 51 | internalFormat(internalFormat), 52 | format(format), 53 | dataType(dataType) 54 | { 55 | if(cuda) 56 | { 57 | // 如果使用 OpenGL 和 CUDA 互操作, 这里就需要在CUDA中注册 OpenGL 的纹理对象 58 | cudaGraphicsGLRegisterImage(&cudaRes, texture->tid, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsReadOnly); 59 | } 60 | else 61 | { 62 | // 不使用的话设置空指针, 这里直接设置成为0了 63 | cudaRes = 0; 64 | } 65 | } 66 | 67 | // 析构函数 68 | GPUTexture::~GPUTexture() 69 | { 70 | // 释放OpenGL 纹理对象 71 | if(texture) 72 | { 73 | delete texture; 74 | } 75 | 76 | // 取消 CUDA 中的注册 77 | if(cudaRes) 78 | { 79 | cudaGraphicsUnregisterResource(cudaRes); 80 | } 81 | } 82 | -------------------------------------------------------------------------------- /Core/src/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 | -------------------------------------------------------------------------------- /GUI/src/Tools/GroundTruthOdometry.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GroundTruthOdometry.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 轨迹真值对象 5 | * @version 0.1 6 | * @date 2020-01-06 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef GROUNDTRUTHODOMETRY_H_ 31 | #define GROUNDTRUTHODOMETRY_H_ 32 | 33 | #ifdef WIN32 34 | # include 35 | #endif 36 | 37 | // Eigen 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | // STL 45 | #include 46 | #include 47 | #include 48 | 49 | // 位姿计算和转换的小工具 50 | #include 51 | 52 | /** @brief 提供相机位姿真值数据读取的接口 */ 53 | class GroundTruthOdometry 54 | { 55 | public: 56 | /** 57 | * @brief 构造函数 58 | * @param[in] filename 位姿真值文件 59 | */ 60 | GroundTruthOdometry(const std::string & filename); 61 | 62 | /** @brief 析构函数 */ 63 | virtual ~GroundTruthOdometry(); 64 | 65 | /** 66 | * @brief 获取指定时间戳时, 相机位姿 Tcw 67 | * @param[in] timestamp 给定时间戳, 单位 ns 68 | * @return Eigen::Matrix4f Tcw 69 | */ 70 | Eigen::Matrix4f getTransformation(uint64_t timestamp); 71 | 72 | /** 73 | * @brief 获取位姿真值的协方差, 恒对角阵 74 | * @return Eigen::MatrixXd 协方差矩阵 75 | */ 76 | Eigen::MatrixXd getCovariance(); 77 | 78 | private: 79 | 80 | /** 81 | * @brief 从外部文件中加载相机的运动真值 82 | * @param[in] filename 83 | */ 84 | void loadTrajectory(const std::string & filename); 85 | 86 | std::map, 89 | Eigen::aligned_allocator< 90 | std::pair 91 | > 92 | > camera_trajectory; ///< 保存相机位姿的 vector 93 | 94 | 95 | uint64_t last_utime; ///< 上一次查询的时间戳 96 | }; 97 | 98 | #endif /* GROUNDTRUTHODOMETRY_H_ */ 99 | -------------------------------------------------------------------------------- /Core/src/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/src/Utils/Resolution.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Resolution.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 分辨率对象 5 | * @version 0.1 6 | * @date 2020-01-03 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef RESOLUTION_H_ 31 | #define RESOLUTION_H_ 32 | 33 | #include 34 | 35 | #include "../Defines.h" 36 | 37 | /** @brief 分辨率对象 */ 38 | class Resolution 39 | { 40 | public: 41 | /** 42 | * @brief 获取给定分辨率的分辨率对象实例, 静态函数 43 | * @param[in] width 给定宽度 44 | * @param[in] height 给定高度 45 | * @return EFUSION_API const& getInstance 分辨率对象实例 46 | */ 47 | EFUSION_API static const Resolution & getInstance(int width = 0,int height = 0); 48 | 49 | /** 50 | * @brief 获取宽度 51 | * @return const int& 52 | */ 53 | const int & width() const 54 | { 55 | return imgWidth; 56 | } 57 | 58 | /** 59 | * @brief 获取高度 60 | * @return const int& 61 | */ 62 | const int & height() const 63 | { 64 | return imgHeight; 65 | } 66 | 67 | const int & cols() const 68 | { 69 | return imgWidth; 70 | } 71 | 72 | const int & rows() const 73 | { 74 | return imgHeight; 75 | } 76 | 77 | const int & numPixels() const 78 | { 79 | return imgNumPixels; 80 | } 81 | 82 | private: 83 | /** 84 | * @brief 构造函数 85 | * @param[in] width 给定宽度 86 | * @param[in] height 给定高度 87 | */ 88 | Resolution(int width, int height) 89 | : imgWidth(width), 90 | imgHeight(height), 91 | imgNumPixels(width * height) 92 | { 93 | // 不允许为0x0的图像存在 94 | assert(width > 0 && height > 0 && "You haven't initialised the Resolution class!"); 95 | } 96 | 97 | // 分辨率信息 98 | const int imgWidth; ///< 图像宽度 99 | const int imgHeight; ///< 图像高度 100 | const int imgNumPixels; ///< 图像中所包含的像素个数 101 | }; 102 | 103 | #endif /* RESOLUTION_H_ */ 104 | -------------------------------------------------------------------------------- /Core/src/Shaders/Resize.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Resize.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 对图像进行快捷缩放的类 5 | * @version 0.1 6 | * @date 2020-01-06 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef RESIZE_H_ 31 | #define RESIZE_H_ 32 | 33 | // GLSL 支持 34 | #include "Shaders.h" 35 | // ? 36 | #include "Uniform.h" 37 | // 分辨率对象 38 | #include "../Utils/Resolution.h" 39 | // 相机内参对象 40 | #include "../Utils/Intrinsics.h" 41 | // ? GPU 纹理? 42 | #include "../GPUTexture.h" 43 | // ? 44 | #include "../Utils/Img.h" 45 | 46 | // DLL 相关定义 47 | #include "../Defines.h" 48 | 49 | /** @brief 缩放图像的对象 */ 50 | class Resize 51 | { 52 | public: 53 | /** 54 | * @brief 构造函数 55 | * @param[in] srcWidth 源图像宽度 56 | * @param[in] srcHeight 源图像高度 57 | * @param[in] destWidth 目标图像宽度 58 | * @param[in] destHeight 目标图像高度 59 | */ 60 | EFUSION_API Resize(int srcWidth, 61 | int srcHeight, 62 | int destWidth, 63 | int destHeight); 64 | 65 | /** @brief 析构函数 */ 66 | virtual ~Resize(); 67 | 68 | void image(GPUTexture * source, Img> & dest); 69 | void vertex(GPUTexture * source, Img & dest); 70 | void time(GPUTexture * source, Img & dest); 71 | 72 | GPUTexture imageTexture; ///< 彩色图像纹理 73 | GPUTexture vertexTexture; ///< 顶点伪装纹理 74 | GPUTexture timeTexture; ///? 时间戳伪装纹理 75 | 76 | std::shared_ptr imageProgram; ///< 缩放图像的 Shader, 直接将缩放图像的工作伪装成为了着色器的工作 77 | pangolin::GlRenderBuffer imageRenderBuffer; ///< 渲染 buffer 78 | pangolin::GlFramebuffer imageFrameBuffer; ///< 帧 buffer 79 | 80 | std::shared_ptr vertexProgram; ///? 缩放/处理顶点的 Shader 81 | pangolin::GlRenderBuffer vertexRenderBuffer; ///< 渲染 buffer 82 | pangolin::GlFramebuffer vertexFrameBuffer; ///< 帧 buffer 83 | 84 | std::shared_ptr timeProgram; ///? 缩放/处理时间戳的 Shader 85 | pangolin::GlRenderBuffer timeRenderBuffer; ///< 渲染 buffer 86 | pangolin::GlFramebuffer timeFrameBuffer; ///< 帧 buffer 87 | }; 88 | 89 | #endif /* RESIZE_H_ */ 90 | -------------------------------------------------------------------------------- /Core/src/GPUTexture.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GPUTexture.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief GPU 纹理对象 5 | * @version 0.1 6 | * @date 2020-01-06 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef GPUTEXTURE_H_ 31 | #define GPUTEXTURE_H_ 32 | 33 | // Pangolin 34 | #include 35 | // CUDA 36 | #include 37 | #include 38 | #include 39 | // DLL 40 | #include "Defines.h" 41 | 42 | /** @brief GPU 纹理的类 */ 43 | class GPUTexture 44 | { 45 | public: 46 | /** 47 | * @brief 构造函数 48 | * @param[in] width 纹理图像的宽度 49 | * @param[in] height 纹理图像的高度 50 | * @param[in] internalFormat 纹理的数据格式 51 | * @param[in] format 对纹理格式的处理 52 | * @param[in] dataType 每个通道的数据类型 53 | * @param[in] draw // ? 54 | * @param[in] cuda 使用CUDA支持 55 | * @return EFUSION_API 56 | */ 57 | EFUSION_API GPUTexture(const int width, 58 | const int height, 59 | const GLenum internalFormat, 60 | const GLenum format, 61 | const GLenum dataType, 62 | const bool draw = false, 63 | const bool cuda = false); 64 | 65 | /** @brief 析构函数 */ 66 | virtual ~GPUTexture(); 67 | 68 | ///? 几个字符串, 标记当前纹理的状态 69 | EFUSION_API static const std::string RGB, DEPTH_RAW, DEPTH_FILTERED, DEPTH_METRIC, DEPTH_METRIC_FILTERED, DEPTH_NORM; 70 | 71 | 72 | pangolin::GlTexture * texture; ///< OpenGL 中的纹理对象句柄 73 | cudaGraphicsResource * cudaRes; ///? OpenGL 纹理对象注册到CUDA中后的资源句柄 74 | 75 | const bool draw; ///? 是否绘制? 还是是否可以绘制? 感觉这个纹理也会存储一些不可绘制的对象, 比如顶点, 比如法向 76 | 77 | private: 78 | /** @brief 无参构造函数 */ 79 | GPUTexture() : texture(0), cudaRes(0), draw(false), width(0), height(0), internalFormat(0), format(0), dataType(0) {} 80 | const int width; ///< 图像宽度 81 | const int height; ///< 图像高度 82 | const GLenum internalFormat; ///< 纹理本身的数据格式 83 | const GLenum format; ///< 对纹理格式的处理 84 | const GLenum dataType; ///< 每个通道的数据类型 85 | }; 86 | 87 | #endif /* GPUTEXTURE_H_ */ 88 | -------------------------------------------------------------------------------- /Core/src/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 | * 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 | * Software License Agreement (BSD License) 18 | * 19 | * Copyright (c) 2011, Willow Garage, Inc. 20 | * All rights reserved. 21 | * 22 | * Redistribution and use in source and binary forms, with or without 23 | * modification, are permitted provided that the following conditions 24 | * are met: 25 | * 26 | * * Redistributions of source code must retain the above copyright 27 | * notice, this list of conditions and the following disclaimer. 28 | * * Redistributions in binary form must reproduce the above 29 | * copyright notice, this list of conditions and the following 30 | * disclaimer in the documentation and/or other materials provided 31 | * with the distribution. 32 | * * Neither the name of Willow Garage, Inc. nor the names of its 33 | * contributors may be used to endorse or promote products derived 34 | * from this software without specific prior written permission. 35 | * 36 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 37 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 38 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 39 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 40 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 41 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 42 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 43 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 44 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 45 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 46 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 47 | * POSSIBILITY OF SUCH DAMAGE. 48 | * 49 | * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com) 50 | */ 51 | 52 | #ifndef CUDA_CONVENIENCE_CUH_ 53 | #define CUDA_CONVENIENCE_CUH_ 54 | 55 | #include 56 | #include 57 | #include 58 | 59 | static inline int getGridDim(int x, int y) 60 | { 61 | return (x + y - 1) / y; 62 | } 63 | 64 | static inline void cudaSafeCall(cudaError_t err) 65 | { 66 | if(cudaSuccess != err) 67 | { 68 | std::cout << "Error: " << cudaGetErrorString(err) << ": " << __FILE__ << ":" << __LINE__ << std::endl; 69 | exit(0); 70 | } 71 | } 72 | 73 | #endif /* CUDA_CONVENIENCE_CUH_ */ 74 | -------------------------------------------------------------------------------- /Core/src/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 | -------------------------------------------------------------------------------- /GUI/src/FindOpenNI2.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Find OpenNI2 3 | # 4 | # This sets the following variables: 5 | # OPENNI2_FOUND - True if OPENNI was found. 6 | # OPENNI2_INCLUDE_DIRS - Directories containing the OPENNI include files. 7 | # OPENNI2_LIBRARIES - Libraries needed to use OPENNI. 8 | 9 | find_package(PkgConfig) 10 | if(${CMAKE_VERSION} VERSION_LESS 2.8.2) 11 | pkg_check_modules(PC_OPENNI openni2-dev) 12 | else() 13 | pkg_check_modules(PC_OPENNI QUIET openni2-dev) 14 | endif() 15 | 16 | set(OPENNI2_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER}) 17 | 18 | #add a hint so that it can find it without the pkg-config 19 | find_path(OPENNI2_INCLUDE_DIR OpenNI.h 20 | HINTS 21 | ${PC_OPENNI_INCLUDEDIR} 22 | ${PC_OPENNI_INCLUDE_DIRS} 23 | PATHS 24 | "${PROGRAM_FILES}/OpenNI2/Include" 25 | "${CMAKE_SOURCE_DIR}/../OpenNI2/Include" 26 | "${CMAKE_SOURCE_DIR}/../../OpenNI2/Include" 27 | "${CMAKE_SOURCE_DIR}/../../../OpenNI2/Include" 28 | "${CMAKE_SOURCE_DIR}/../../../../OpenNI2/Include" 29 | "${CMAKE_SOURCE_DIR}/../../../code/OpenNI2/Include" 30 | "${CMAKE_SOURCE_DIR}/../../../../code/OpenNI2/Include" 31 | "${CMAKE_SOURCE_DIR}/../deps/OpenNI2/Include" 32 | "${CMAKE_SOURCE_DIR}/../../deps/OpenNI2/Include" 33 | "${CMAKE_SOURCE_DIR}/../../../deps/OpenNI2/Include" 34 | "${CMAKE_SOURCE_DIR}/../../../../deps/OpenNI2/Include" 35 | /usr/include 36 | /user/include 37 | PATH_SUFFIXES openni2 ni2 38 | ) 39 | 40 | if(${CMAKE_CL_64}) 41 | set(OPENNI_PATH_SUFFIXES lib64) 42 | else() 43 | set(OPENNI_PATH_SUFFIXES lib) 44 | endif() 45 | 46 | #add a hint so that it can find it without the pkg-config 47 | find_library(OPENNI2_LIBRARY 48 | NAMES OpenNI2 49 | HINTS 50 | ${PC_OPENNI_LIBDIR} 51 | ${PC_OPENNI_LIBRARY_DIRS} 52 | PATHS 53 | "${PROGRAM_FILES}}/OpenNI2/Redist" 54 | "${PROGRAM_FILES}/OpenNI2" 55 | "${CMAKE_SOURCE_DIR}/../OpenNI2/Bin/x64-Release" 56 | "${CMAKE_SOURCE_DIR}/../../OpenNI2/Bin/x64-Release" 57 | "${CMAKE_SOURCE_DIR}/../../../OpenNI2/Bin/x64-Release" 58 | "${CMAKE_SOURCE_DIR}/../../../../OpenNI2/Bin/x64-Release" 59 | "${CMAKE_SOURCE_DIR}/../../../code/OpenNI2/Bin/x64-Release" 60 | "${CMAKE_SOURCE_DIR}/../../../../code/OpenNI2/Bin/x64-Release" 61 | "${CMAKE_SOURCE_DIR}/../deps/OpenNI2/Bin/x64-Release" 62 | "${CMAKE_SOURCE_DIR}/../../deps/OpenNI2/Bin/x64-Release" 63 | "${CMAKE_SOURCE_DIR}/../../../deps/OpenNI2/Bin/x64-Release" 64 | "${CMAKE_SOURCE_DIR}/../../../../deps/OpenNI2/Bin/x64-Release" 65 | /usr/lib 66 | /user/lib 67 | PATH_SUFFIXES ${OPENNI_PATH_SUFFIXES} 68 | ) 69 | 70 | set(OPENNI2_INCLUDE_DIRS ${OPENNI2_INCLUDE_DIR}) 71 | set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY}) 72 | 73 | include(FindPackageHandleStandardArgs) 74 | find_package_handle_standard_args(OpenNI2 DEFAULT_MSG 75 | OPENNI2_LIBRARY OPENNI2_INCLUDE_DIR) 76 | 77 | mark_as_advanced(OPENNI2_LIBRARY OPENNI2_INCLUDE_DIR) 78 | -------------------------------------------------------------------------------- /Core/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6.0) 2 | 3 | project(libefusion) 4 | 5 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}") 6 | 7 | find_package(Pangolin 0.1 REQUIRED) 8 | find_package(CUDA REQUIRED) 9 | find_package(SuiteSparse REQUIRED) 10 | 11 | set(efusion_SHADER_DIR "${CMAKE_CURRENT_SOURCE_DIR}/Shaders" CACHE PATH "Where the shaders live") 12 | 13 | include_directories(${Pangolin_INCLUDE_DIRS}) 14 | include_directories(${CUDA_INCLUDE_DIRS}) 15 | include_directories(${EIGEN_INCLUDE_DIRS}) 16 | include_directories(${SUITESPARSE_INCLUDE_DIRS}) 17 | 18 | file(GLOB srcs *.cpp) 19 | file(GLOB utils_srcs Utils/*.cpp) 20 | file(GLOB shader_srcs Shaders/*.cpp) 21 | file(GLOB cuda Cuda/*.cu) 22 | file(GLOB containers Cuda/containers/*.cpp) 23 | 24 | if(WIN32) 25 | file(GLOB hdrs *.h) 26 | file(GLOB utils_hdrs Utils/*.h) 27 | file(GLOB shader_hdrs Shaders/*.h) 28 | file(GLOB cuda_hdrs Cuda/*.cuh) 29 | file(GLOB containers_hdrs Cuda/containers/*.hpp) 30 | endif() 31 | 32 | # HACK 可以在这里修改支持的计算能力 33 | set(CUDA_ARCH_BIN "61" CACHE STRING "Specify 'real' GPU arch to build binaries for, BIN(PTX) format is supported. Example: 1.3 2.1(1.3) or 13 21(13)") 34 | set(CUDA_ARCH_PTX "" CACHE STRING "Specify 'virtual' PTX arch to build PTX intermediate code for. Example: 1.0 1.2 or 10 12") 35 | 36 | SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}) 37 | include(CudaComputeTargetFlags.cmake) 38 | APPEND_TARGET_ARCH_FLAGS() 39 | 40 | set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-Xcompiler;-fPIC;") 41 | set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "--ftz=true;--prec-div=false;--prec-sqrt=false") 42 | # HACK 用于屏蔽编译器警告 43 | set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "-Wno-deprecated-declarations") 44 | 45 | 46 | 47 | 48 | CUDA_COMPILE(cuda_objs ${cuda}) 49 | 50 | if(WIN32) 51 | set(ADDITIONAL_CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS}) 52 | endif() 53 | 54 | set(CMAKE_CXX_FLAGS ${ADDITIONAL_CMAKE_CXX_FLAGS} "-O3 -msse2 -msse3 -Wall -std=c++11 -DSHADER_DIR=${efusion_SHADER_DIR}") 55 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -Wall -std=c++11 -DSHADER_DIR=${efusion_SHADER_DIR}") 56 | 57 | if(WIN32) 58 | add_definitions(-DWIN32_LEAN_AND_MEAN) 59 | add_definitions(-DNOMINMAX) 60 | set (EXTRA_WINDOWS_LIBS ${EXTRA_WINDOWS_LIBS} ws2_32) 61 | endif() 62 | 63 | add_library(efusion SHARED 64 | ${srcs} 65 | ${utils_srcs} 66 | ${shader_srcs} 67 | ${cuda} 68 | ${cuda_objs} 69 | ${containers} 70 | ${hdrs} 71 | ${utils_hdrs} 72 | ${shader_hdrs} 73 | ${cuda_hdrs} 74 | ${containers_hdrs} 75 | ) 76 | 77 | target_link_libraries(efusion 78 | ${Eigen_LIBRARIES} 79 | ${Pangolin_LIBRARIES} 80 | ${CUDA_LIBRARIES} 81 | ${SUITESPARSE_LIBRARIES} 82 | ${EXTRA_WINDOWS_LIBS} 83 | ) 84 | 85 | # HACK 86 | message(STATUS "==> Eigen_LIBRARIES = " ${Eigen_LIBRARIES}) 87 | message(STATUS "==> Pangolin_LIBRARIES = " ${Pangolin_LIBRARIES}) 88 | message(STATUS "==> CUDA_LIBRARIES = " ${CUDA_LIBRARIES}) 89 | message(STATUS "==> SUITESPARSE_LIBRARIES = " ${SUITESPARSE_LIBRARIES}) 90 | message(STATUS "==> EXTRA_WINDOWS_LIBS = " ${EXTRA_WINDOWS_LIBS}) 91 | 92 | INSTALL(TARGETS efusion 93 | RUNTIME DESTINATION bin 94 | LIBRARY DESTINATION lib 95 | ARCHIVE DESTINATION lib 96 | ) 97 | -------------------------------------------------------------------------------- /Core/src/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/src/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 | -------------------------------------------------------------------------------- /GUI/src/Tools/RawLogReader.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RawLogReader.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 记录文件读取器 5 | * @version 0.1 6 | * @date 2020-01-03 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef RAWLOGREADER_H_ 31 | #define RAWLOGREADER_H_ 32 | 33 | // 分辨率对象 34 | #include 35 | // 时间统计对象 36 | #include 37 | 38 | // Pangolin 39 | #include 40 | 41 | // 父类 42 | #include "LogReader.h" 43 | 44 | // C & C++ STL 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | 53 | /* 记录文件的数据格式: 54 | * 0x..00 int32_t 当前数据集中的帧数 55 | * 0x..04 int64_t 第0帧数据的时间戳 56 | * 0x..0C int32_t -深度图像数据长度 depthSize 57 | * 0x..10 int32_t -彩色图像数据长度 imageSize 58 | * 0x..14 depthSize -深度图像数据 59 | * .. imageSize -彩色图像数据(不一定有) 60 | * .. int64_t 第1帧数据的时间戳 61 | */ 62 | 63 | 64 | /** @brief 原始记录文件读取器, 这个读取器负责读取原始记录文件 */ 65 | class RawLogReader : public LogReader 66 | { 67 | public: 68 | 69 | /** 70 | * @brief 构造函数 71 | * @param[in] file 记录文件位置 72 | * @param[in] flipColors 是否要翻转图像 73 | */ 74 | RawLogReader(std::string file, bool flipColors); 75 | 76 | /** @brief 析构函数, 释放缓冲区内容, 关闭文件 */ 77 | virtual ~RawLogReader(); 78 | 79 | /** @brief 获取下一帧图像数据, 保存到缓冲区中 */ 80 | void getNext(); 81 | 82 | /** @brief 设置文件指针返回到上一帧图像开始的位置 */ 83 | void getBack(); 84 | 85 | /** 86 | * @brief 获取获取记录文件中的帧的个数 87 | * @return int 帧的个数 88 | */ 89 | int getNumFrames(); 90 | 91 | /* @brief 判断记录文件中的内容是否已经读取完毕 */ 92 | bool hasMore(); 93 | 94 | /** 95 | * @brief 判断是否需要倒带 -- 即记录文件倒序播放到头后, 再正序播放 96 | * @return 是否 97 | */ 98 | bool rewound(); 99 | 100 | /** @brief 重新设置记录文件的读取 */ 101 | void rewind(); 102 | 103 | /** 104 | * @brief 快进到第 frame 帧 105 | * @param[in] frame 要快进到的帧id 106 | */ 107 | void fastForward(int frame); 108 | 109 | /** 110 | * @brief 获取记录文件路径 111 | * @return const std::string 记录文件路径 112 | */ 113 | const std::string getFile(); 114 | 115 | /** 116 | * @brief 自动曝光/白平衡参数设置, 对于记录文件无法设置, 空函数 117 | * @param[in] value 是否 118 | */ 119 | void setAuto(bool value); 120 | 121 | std::stack filePointers; ///< 存储每一帧图像的数据相对于文件头部的偏移量 122 | 123 | private: 124 | /** @brief 获取真正的帧数据到缓冲区中 */ 125 | void getCore(); 126 | }; 127 | 128 | #endif /* RAWLOGREADER_H_ */ 129 | -------------------------------------------------------------------------------- /Core/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "Parse.h" 20 | 21 | // 空构造函数 22 | Parse::Parse() 23 | { 24 | 25 | } 26 | 27 | // 获取当前解析器的一个实例 28 | const Parse & Parse::get() 29 | { 30 | static const Parse instance; 31 | return instance; 32 | } 33 | 34 | // 对给出的字符型参数进行解析工作 35 | int Parse::arg(int argc, char** argv, const char* str, std::string &val) const 36 | { 37 | // 寻找这个参数在的参数项id 38 | int index = findArg(argc, argv, str) + 1; 39 | 40 | if(index > 0 && index < argc) 41 | { 42 | // 说明找到了, 获取这个参数项的完整内容 43 | val = argv[index]; 44 | } 45 | 46 | // 如果找不到, 这里就是负数 47 | return index - 1; 48 | } 49 | 50 | // 对给出的 float 型参数进行解析工作 51 | int Parse::arg(int argc, char** argv, const char* str, float &val) const 52 | { 53 | int index = findArg(argc, argv, str) + 1; 54 | 55 | if(index > 0 && index < argc) 56 | { 57 | // 将对应的字符串转换成为浮点数 58 | val = atof(argv[index]); 59 | } 60 | 61 | return index - 1; 62 | } 63 | 64 | // 对给出的 int 型参数进行解析工作 65 | int Parse::arg(int argc, char** argv, const char* str, int &val) const 66 | { 67 | 68 | int index = findArg(argc, argv, str) + 1; 69 | 70 | if(index > 0 && index < argc) 71 | { 72 | val = atoi(argv[index]); 73 | } 74 | 75 | return index - 1; 76 | } 77 | 78 | // 获取着色器程序的存放路径 79 | std::string Parse::shaderDir() const 80 | { 81 | // ? STR 就是 #SHADER_DIR 82 | // SHADER_DIR 这个宏在 /Core/src/CMakeLists.txt 中被定义 83 | std::string currentVal = STR(SHADER_DIR); 84 | // 判断给定的文件是否存在, 否则就发生断言错误; 后面的字符串是在发生断言错误的时候用于提示我们的 85 | assert(pangolin::FileExists(currentVal) && "Shader directory not found!"); 86 | return currentVal; 87 | } 88 | 89 | // 获取当前程序运行的基路径(可执行文件所在的路径, 除去 build) 90 | std::string Parse::baseDir() const 91 | { 92 | char buf[256]; 93 | #ifdef WIN32 94 | int length = GetModuleFileName(NULL,buf,sizeof(buf)); 95 | #else 96 | // linux 97 | // 获取当前程序的运行名称(含路径). readlink()是boost提供的函数. 关于神奇的 /proc 目录: 98 | // ref:[https://www.cnblogs.com/liushui-sky/p/9354536.html] 99 | int length = readlink("/proc/self/exe",buf,sizeof(buf)); 100 | #endif 101 | 102 | std::string currentVal; 103 | currentVal.append((char *)&buf, length); 104 | 105 | currentVal = currentVal.substr(0, currentVal 106 | // windows 和 linux 下的斜杠不同, 这里的目的是删去 build 后的目录 107 | #ifdef WIN32 108 | .rfind("\\build\\")); 109 | #else 110 | .rfind("/build/")); 111 | #endif 112 | return currentVal; 113 | } 114 | 115 | // 寻找给定的参数名称出现在的参数id 116 | int Parse::findArg(int argc, char** argv, const char* argument_name) const 117 | { 118 | for(int i = 1; i < argc; ++i) 119 | { 120 | // Search for the string 121 | if(strcmp(argv[i], argument_name) == 0) 122 | { 123 | return i; 124 | } 125 | } 126 | return -1; 127 | } 128 | -------------------------------------------------------------------------------- /Core/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #include "CholeskyDecomp.h" 20 | 21 | CholeskyDecomp::CholeskyDecomp() 22 | : L(0) 23 | { 24 | cholmod_start(&Common); 25 | } 26 | 27 | CholeskyDecomp::~CholeskyDecomp() 28 | { 29 | cholmod_finish(&Common); 30 | } 31 | 32 | void CholeskyDecomp::freeFactor() 33 | { 34 | assert(L); 35 | cholmod_free_factor(&L, &Common); 36 | L = 0; 37 | } 38 | 39 | Eigen::VectorXd CholeskyDecomp::solve(const Jacobian & jacobian, const Eigen::VectorXd & residual, const bool firstRun) 40 | { 41 | cholmod_sparse * At = cholmod_allocate_sparse(jacobian.cols(), jacobian.rows.size(), jacobian.nonZero(), true, true, 0, CHOLMOD_REAL, &Common); 42 | 43 | int* p = (int*) At->p; 44 | int* i = (int*) At->i; 45 | double* x = (double*) At->x; 46 | int n = 0; 47 | *p = n; 48 | 49 | for(size_t r = 0; r < jacobian.rows.size(); r++) 50 | { 51 | memcpy(i, jacobian.rows.at(r)->indices, jacobian.rows.at(r)->nonZeros() * sizeof(int)); 52 | memcpy(x, jacobian.rows.at(r)->vals, jacobian.rows.at(r)->nonZeros()* sizeof(double)); 53 | 54 | i += jacobian.rows.at(r)->nonZeros(); 55 | x += jacobian.rows.at(r)->nonZeros(); 56 | n += jacobian.rows.at(r)->nonZeros(); 57 | p++; 58 | *p = n; 59 | } 60 | 61 | if(firstRun) 62 | { 63 | assert(!L); 64 | 65 | L = cholmod_analyze(At, &Common); 66 | } 67 | 68 | cholmod_factor * L_factor = cholmod_copy_factor(L, &Common); 69 | 70 | cholmod_factorize(At, L_factor, &Common); 71 | 72 | cholmod_change_factor(CHOLMOD_REAL, true, false, true, true, L_factor, &Common); 73 | 74 | cholmod_dense* Arhs = cholmod_zeros(At->ncol, 1, CHOLMOD_REAL, &Common); 75 | 76 | memcpy(Arhs->x, residual.data(), At->ncol * sizeof(double)); 77 | 78 | cholmod_dense* Atb = cholmod_zeros(At->nrow, 1, CHOLMOD_REAL, &Common); 79 | 80 | double alpha[2] = { 1., 0. }; 81 | double beta[2] = { 0., 0. }; 82 | 83 | cholmod_sdmult(At, 0, alpha, beta, Arhs, Atb, &Common); 84 | 85 | cholmod_dense* Atb_perm = cholmod_solve(CHOLMOD_P, L_factor, Atb, &Common); 86 | 87 | cholmod_dense * rhs = cholmod_solve(CHOLMOD_L, L_factor, Atb_perm, &Common); 88 | 89 | cholmod_dense* delta_cm = cholmod_solve(CHOLMOD_Lt, L_factor, rhs, &Common); 90 | 91 | Eigen::VectorXd delta(rhs->nrow); 92 | 93 | for(size_t i = 0; i < At->nrow; i++) 94 | { 95 | delta(((int *)L_factor->Perm)[i]) = ((double*)delta_cm->x)[i]; 96 | } 97 | 98 | cholmod_free_dense(&delta_cm, &Common); 99 | cholmod_free_dense(&Atb_perm, &Common); 100 | cholmod_free_dense(&Atb, &Common); 101 | cholmod_free_dense(&Arhs, &Common); 102 | cholmod_free_sparse(&At, &Common); 103 | cholmod_free_dense(&rhs, &Common); 104 | cholmod_free_factor(&L_factor, &Common); 105 | 106 | return delta; 107 | } 108 | -------------------------------------------------------------------------------- /GUI/src/Tools/LiveLogReader.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file LiveLogReader.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 读取实时传感器数据 5 | * @version 0.1 6 | * @date 2020-01-04 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | 31 | #ifndef LIVELOGREADER_H_ 32 | #define LIVELOGREADER_H_ 33 | 34 | // C STL 35 | #include 36 | #include 37 | #ifndef WIN32 38 | # include 39 | #endif 40 | // Linux 41 | #include 42 | // C++ STL 43 | #include 44 | #include 45 | 46 | // 命令行解析器 47 | #include 48 | 49 | // 父类, 记录文件加载器 50 | #include "LogReader.h" 51 | // 相机接口 52 | #include "CameraInterface.h" 53 | 54 | /** @brief 读取实时数据的类 */ 55 | class LiveLogReader : public LogReader 56 | { 57 | public: 58 | /** 59 | * @brief 摄像头类型就两种, OpenNI支持的系列, 以及Realsense系列 60 | * 61 | */ 62 | enum CameraType 63 | { 64 | OpenNI2,RealSense 65 | }; 66 | 67 | /** 68 | * @brief 构造函数 69 | * @param[in] file 记录文件, 这里实际上只用于构造父类对象, 并没有真正使用 70 | * @param[in] flipColors 是否左右翻转图像 71 | * @param[in] type 摄像头的类型 72 | */ 73 | LiveLogReader(std::string file, bool flipColors, CameraType type); 74 | 75 | /** @brief 析构函数, 释放缓冲区, 析构相机对象 */ 76 | virtual ~LiveLogReader(); 77 | 78 | /** @brief 获取摄像头的下一帧图像到 LogReader 的缓冲区中 */ 79 | void getNext(); 80 | 81 | /** 82 | * @brief 获取可以从相机读取的帧数的最大值 83 | * @return int 帧数最大值 84 | */ 85 | int getNumFrames(); 86 | 87 | /** @brief 对于实时的摄像头, 数据是永远都会有的, 一直返回 true */ 88 | bool hasMore(); 89 | 90 | /** 91 | * @brief 是否需要"倒带"读取, 从逆序读取转换成为正序读取 92 | * @note 由于 live camera 不存在这个问题, 所以一直范围 false 93 | * @return false 94 | */ 95 | bool rewound() 96 | { 97 | return false; 98 | } 99 | 100 | /** @brief 如果是跑实际的摄像头, 就没有必要重新读取记录文件了, 这里设置为空操作 */ 101 | void rewind(){ } 102 | 103 | /** @brief 对于实际摄像头没有重新读取的概念, 所以这个倒序读取图像的函数并不实现 */ 104 | void getBack(){ } 105 | 106 | /** 107 | * @brief 本来是在记录文件中快速确定每一帧相对于文件开头的位置的, 但是对于 live camera 没有什么用, 函数本身并不实现 108 | * @param[in] frame 执行处理的最大帧 109 | */ 110 | void fastForward(int frame){ } 111 | 112 | /** 113 | * @brief 获取记录文件路径 114 | * @return const std::string 不过对于实时摄像头而言, 返回的是当前的基路径+"live" 115 | */ 116 | const std::string getFile(); 117 | 118 | /** 119 | * @brief 设置自动曝光和自动白平衡 120 | * @param[in] value 是否 121 | */ 122 | void setAuto(bool value); 123 | 124 | 125 | CameraInterface * cam; ///< 保存摄像头的接口对象指针 126 | 127 | private: 128 | int64_t lastFrameTime; /// 上一帧(结构体, 包含彩色图像和深度图像)的时间戳 129 | int lastGot; /// 上一帧深度图像在图像缓冲区的id 130 | }; 131 | 132 | #endif /* LIVELOGREADER_H_ */ 133 | -------------------------------------------------------------------------------- /Core/src/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 | * 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 | #ifndef PARSE_H_ 19 | #define PARSE_H_ 20 | 21 | // windows 平台下的 22 | #ifdef WIN32 23 | # include 24 | #else 25 | // linux 下的目录操作 26 | # include 27 | #endif 28 | 29 | // STL 30 | #include 31 | // C 32 | #include 33 | 34 | // windows 35 | #ifndef WIN32 36 | # include 37 | #endif 38 | 39 | // C 40 | #include 41 | 42 | // Pangolin 43 | #include 44 | 45 | // 一些用于动态链接库本身属性的宏定义 46 | #include "../Defines.h" 47 | 48 | // 用于 Shader 程序的定位, 将给定的宏的内容转换成为字符串. 49 | // 如果只是一步替换, 那么只是将宏的名称转换成为字符串 50 | // ref: [https://www.cnblogs.com/Flying-Cloud/p/3913565.html] 51 | #define XSTR(x) #x 52 | #define STR(x) XSTR(x) 53 | 54 | /** @brief 命令行参数解析 */ 55 | class Parse 56 | { 57 | public: 58 | /** 59 | * @brief 获取当前解析器的一个实例. 静态成员函数 60 | * @return EFUSION_API const& get 实例引用 61 | */ 62 | EFUSION_API static const Parse & get(); 63 | 64 | /** 65 | * @brief 对给出的字符型参数进行解析操作 66 | * @param[in] argc 命令行参数个数 67 | * @param[in] argv 命令行参数表列 68 | * @param[in] str 要寻找的参数名称 69 | * @param[in] val 找到的命令行参数项 70 | * @return 参数项的id. 负数表示没有找到 71 | */ 72 | EFUSION_API int arg(int argc, char** argv, const char* str, std::string &val) const; 73 | 74 | /** 75 | * @brief 对给出的 float 型参数进行解析操作 76 | * @param[in] argc 命令行参数个数 77 | * @param[in] argv 命令行参数表列 78 | * @param[in] str 要寻找的参数名称 79 | * @param[in] val 找到的命令行参数项 80 | * @return 参数项的id. 负数表示没有找到 81 | */ 82 | EFUSION_API int arg(int argc, char** argv, const char* str, float &val) const; 83 | 84 | /** 85 | * @brief 对给出的 int 型参数进行解析操作 86 | * @param[in] argc 命令行参数个数 87 | * @param[in] argv 命令行参数表列 88 | * @param[in] str 要寻找的参数名称 89 | * @param[in] val 找到的命令行参数项 90 | * @return 参数项的id. 负数表示没有找到 91 | */ 92 | EFUSION_API int arg(int argc, char** argv, const char* str, int &val) const; 93 | 94 | /** 95 | * @brief 获取着色器程序的存放路径 96 | * @return shaderDir 路径 97 | */ 98 | EFUSION_API std::string shaderDir() const; 99 | 100 | /** 101 | * @brief 获取当前程序的运行目录, 除去了build的那种 102 | * @return baseDir 路径 103 | */ 104 | EFUSION_API std::string baseDir() const; 105 | 106 | private: 107 | 108 | /** @brief 空构造函数 */ 109 | EFUSION_API Parse(); 110 | 111 | /** 112 | * @brief 寻找给定的参数名称出现在的参数id 113 | * @param[in] argc 命令行参数个数 114 | * @param[in] argv 命令行参数表列 115 | * @param[in] argument_name 要寻找的参数名称 116 | * @return id. 如果找不到返回-1 117 | */ 118 | EFUSION_API int findArg(int argc,char** argv,const char* argument_name) const; 119 | }; 120 | 121 | #endif /* PARSE_H_ */ 122 | -------------------------------------------------------------------------------- /Core/src/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 | -------------------------------------------------------------------------------- /GUI/src/Tools/RealSenseInterface.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RealSenseInterface.cpp 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief RealSence 相机的接口. 由于使用的接口太老了, 这里就不分析了 5 | * @version 0.1 6 | * @date 2020-01-09 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #include "RealSenseInterface.h" 13 | #include 14 | 15 | #ifdef WITH_REALSENSE 16 | RealSenseInterface::RealSenseInterface(int inWidth,int inHeight,int inFps) 17 | : width(inWidth), 18 | height(inHeight), 19 | fps(inFps), 20 | dev(nullptr), 21 | initSuccessful(true) 22 | { 23 | if(ctx.get_device_count() == 0) 24 | { 25 | errorText = "No device connected."; 26 | initSuccessful = false; 27 | return; 28 | } 29 | 30 | dev = ctx.get_device(0); 31 | dev->enable_stream(rs::stream::depth,width,height,rs::format::z16,fps); 32 | dev->enable_stream(rs::stream::color,width,height,rs::format::rgb8,fps); 33 | 34 | latestDepthIndex.assign(-1); 35 | latestRgbIndex.assign(-1); 36 | 37 | for(int i = 0; i < numBuffers; i++) 38 | { 39 | uint8_t * newImage = (uint8_t *)calloc(width * height * 3,sizeof(uint8_t)); 40 | rgbBuffers[i] = std::pair(newImage,0); 41 | } 42 | 43 | for(int i = 0; i < numBuffers; i++) 44 | { 45 | uint8_t * newDepth = (uint8_t *)calloc(width * height * 2,sizeof(uint8_t)); 46 | uint8_t * newImage = (uint8_t *)calloc(width * height * 3,sizeof(uint8_t)); 47 | frameBuffers[i] = std::pair,int64_t>(std::pair(newDepth,newImage),0); 48 | } 49 | 50 | setAutoExposure(true); 51 | setAutoWhiteBalance(true); 52 | 53 | rgbCallback = new RGBCallback(lastRgbTime, 54 | latestRgbIndex, 55 | rgbBuffers); 56 | 57 | depthCallback = new DepthCallback(lastDepthTime, 58 | latestDepthIndex, 59 | latestRgbIndex, 60 | rgbBuffers, 61 | frameBuffers); 62 | 63 | dev->set_frame_callback(rs::stream::depth,*depthCallback); 64 | dev->set_frame_callback(rs::stream::color,*rgbCallback); 65 | 66 | dev->start(); 67 | } 68 | 69 | RealSenseInterface::~RealSenseInterface() 70 | { 71 | if(initSuccessful) 72 | { 73 | dev->stop(); 74 | 75 | for(int i = 0; i < numBuffers; i++) 76 | { 77 | free(rgbBuffers[i].first); 78 | } 79 | 80 | for(int i = 0; i < numBuffers; i++) 81 | { 82 | free(frameBuffers[i].first.first); 83 | free(frameBuffers[i].first.second); 84 | } 85 | 86 | delete rgbCallback; 87 | delete depthCallback; 88 | } 89 | } 90 | 91 | void RealSenseInterface::setAutoExposure(bool value) 92 | { 93 | dev->set_option(rs::option::color_enable_auto_exposure,value); 94 | } 95 | 96 | void RealSenseInterface::setAutoWhiteBalance(bool value) 97 | { 98 | dev->set_option(rs::option::color_enable_auto_white_balance,value); 99 | } 100 | 101 | bool RealSenseInterface::getAutoExposure() 102 | { 103 | return dev->get_option(rs::option::color_enable_auto_exposure); 104 | } 105 | 106 | bool RealSenseInterface::getAutoWhiteBalance() 107 | { 108 | return dev->get_option(rs::option::color_enable_auto_white_balance); 109 | } 110 | #else 111 | 112 | RealSenseInterface::RealSenseInterface(int inWidth,int inHeight,int inFps) 113 | : width(inWidth), 114 | height(inHeight), 115 | fps(inFps), 116 | initSuccessful(false) 117 | { 118 | errorText = "Compiled without Intel RealSense library"; 119 | } 120 | 121 | RealSenseInterface::~RealSenseInterface() 122 | { 123 | } 124 | 125 | void RealSenseInterface::setAutoExposure(bool value) 126 | { 127 | } 128 | 129 | void RealSenseInterface::setAutoWhiteBalance(bool value) 130 | { 131 | } 132 | 133 | bool RealSenseInterface::getAutoExposure() 134 | { 135 | return false; 136 | } 137 | 138 | bool RealSenseInterface::getAutoWhiteBalance() 139 | { 140 | return false; 141 | } 142 | #endif -------------------------------------------------------------------------------- /Core/src/Shaders/Uniform.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Uniform.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 管理 uniform 数据块, 负责给 GLSL 程序传递数据的类, 统一了不同数据之间的接口 5 | * @version 0.1 6 | * @date 2020-01-09 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef UNIFORM_H_ 31 | #define UNIFORM_H_ 32 | 33 | // C++ STL 34 | #include 35 | // Eigen 36 | #include 37 | 38 | /** @brief 统一各种数据类型, 方便给 GLSL 程序传递参数的类 */ 39 | class Uniform 40 | { 41 | public: 42 | /** 43 | * @brief 构造函数 - int 44 | * @param[in] id 变量的字符串名字, 也作为其唯一的标识符 45 | * @param[in] v 变量内容 46 | */ 47 | Uniform(const std::string & id, const int & v) 48 | : id(id), 49 | i(v), 50 | t(INT) 51 | {} 52 | 53 | /** 54 | * @brief 构造函数 - float 55 | * @param[in] id 变量的字符串名字, 也作为其唯一的标识符 56 | * @param[in] v 变量内容 57 | */ 58 | Uniform(const std::string & id, const float & v) 59 | : id(id), 60 | f(v), 61 | t(FLOAT) 62 | {} 63 | 64 | /** 65 | * @brief 构造函数 - Vector2f 66 | * @param[in] id 变量的字符串名字, 也作为其唯一的标识符 67 | * @param[in] v 变量内容 68 | */ 69 | Uniform(const std::string & id, const Eigen::Vector2f & v) 70 | : id(id), 71 | v2(v), 72 | t(VEC2) 73 | {} 74 | 75 | /** 76 | * @brief 构造函数 - Vector3f 77 | * @param[in] id 变量的字符串名字, 也作为其唯一的标识符 78 | * @param[in] v 变量内容 79 | */ 80 | Uniform(const std::string & id, const Eigen::Vector3f & v) 81 | : id(id), 82 | v3(v), 83 | t(VEC3) 84 | {} 85 | 86 | /** 87 | * @brief 构造函数 - Vector4f 88 | * @param[in] id 变量的字符串名字, 也作为其唯一的标识符 89 | * @param[in] v 变量内容 90 | */ 91 | Uniform(const std::string & id, const Eigen::Vector4f & v) 92 | : id(id), 93 | v4(v), 94 | t(VEC4) 95 | {} 96 | 97 | /** 98 | * @brief 构造函数 - Matrix4f 99 | * @param[in] id 变量的字符串名字, 也作为其唯一的标识符 100 | * @param[in] v 变量内容 101 | */ 102 | Uniform(const std::string & id, const Eigen::Matrix4f & v) 103 | : id(id), 104 | m4(v), 105 | t(MAT4) 106 | {} 107 | 108 | std::string id; ///< 变量名的字符串描述, 也是作为其标志 109 | 110 | int i; ///< 整型 111 | float f; ///< 浮点型 112 | Eigen::Vector2f v2; ///< 2x1 向量 113 | Eigen::Vector3f v3; ///< 3x1 向量 114 | Eigen::Vector4f v4; ///< 4x1 向量 115 | Eigen::Matrix4f m4; ///< 4x4 矩阵 116 | 117 | // ==== 当前类支持的数据类型 ==== 118 | enum Type 119 | { 120 | INT, 121 | FLOAT, 122 | VEC2, 123 | VEC3, 124 | VEC4, 125 | MAT4, 126 | NONE 127 | }; 128 | 129 | Type t; ///< 数据类型 130 | }; 131 | 132 | 133 | #endif /* UNIFORM_H_ */ 134 | -------------------------------------------------------------------------------- /Core/src/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 | * 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 | * Software License Agreement (BSD License) 18 | * 19 | * Copyright (c) 2011, Willow Garage, Inc. 20 | * All rights reserved. 21 | * 22 | * Redistribution and use in source and binary forms, with or without 23 | * modification, are permitted provided that the following conditions 24 | * are met: 25 | * 26 | * * Redistributions of source code must retain the above copyright 27 | * notice, this list of conditions and the following disclaimer. 28 | * * Redistributions in binary form must reproduce the above 29 | * copyright notice, this list of conditions and the following 30 | * disclaimer in the documentation and/or other materials provided 31 | * with the distribution. 32 | * * Neither the name of Willow Garage, Inc. nor the names of its 33 | * contributors may be used to endorse or promote products derived 34 | * from this software without specific prior written permission. 35 | * 36 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 37 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 38 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 39 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 40 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 41 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 42 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 43 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 44 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 45 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 46 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 47 | * POSSIBILITY OF SUCH DAMAGE. 48 | * 49 | * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com) 50 | */ 51 | 52 | #include 53 | 54 | #ifndef CUDA_OPERATORS_CUH_ 55 | #define CUDA_OPERATORS_CUH_ 56 | 57 | __device__ __host__ __forceinline__ float3 operator-(const float3& a, const float3& b) 58 | { 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 | { 64 | return make_float3(a.x + b.x, a.y + b.y, a.z + b.z); 65 | } 66 | 67 | __device__ __host__ __forceinline__ float3 cross(const float3& a, const float3& b) 68 | { 69 | 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); 70 | } 71 | 72 | __device__ __host__ __forceinline__ float dot(const float3& a, const float3& b) 73 | { 74 | return a.x * b.x + a.y * b.y + a.z * b.z; 75 | } 76 | 77 | __device__ __host__ __forceinline__ float norm(const float3& a) 78 | { 79 | return sqrtf(dot(a, a)); 80 | } 81 | 82 | __device__ __host__ __forceinline__ float3 normalized(const float3& a) 83 | { 84 | const float rn = rsqrtf(dot(a, a)); 85 | return make_float3(a.x * rn, a.y * rn, a.z * rn); 86 | } 87 | 88 | __device__ __forceinline__ float3 operator*(const mat33& m, const float3& a) 89 | { 90 | return make_float3(dot(m.data[0], a), dot(m.data[1], a), dot(m.data[2], a)); 91 | } 92 | 93 | #endif /* CUDA_OPERATORS_CUH_ */ 94 | -------------------------------------------------------------------------------- /Core/src/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 | -------------------------------------------------------------------------------- /GUI/src/Tools/GroundTruthOdometry.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file GroundTruthOdometry.cpp 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 轨迹真值对象 5 | * @version 0.1 6 | * @date 2020-01-06 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #include "GroundTruthOdometry.h" 31 | 32 | // 构造函数, 从给定的文件中加载相机路径 33 | GroundTruthOdometry::GroundTruthOdometry(const std::string & filename) 34 | : last_utime(0) 35 | { 36 | loadTrajectory(filename); 37 | } 38 | 39 | // 空析构函数 40 | GroundTruthOdometry::~GroundTruthOdometry() 41 | { 42 | 43 | } 44 | 45 | // 从给定的文件中加载相机轨迹文件路径 46 | void GroundTruthOdometry::loadTrajectory(const std::string & filename) 47 | { 48 | // step 0 打开文件 49 | std::ifstream file; 50 | std::string line; 51 | file.open(filename.c_str()); 52 | 53 | while (!file.eof()) 54 | { 55 | // step 1 读取其中一行的数据 56 | unsigned long long int utime; // 时间戳 57 | float x, y, z, // 位移 58 | qx, qy, qz, qw; // 旋转 59 | std::getline(file, line); 60 | int n = sscanf(line.c_str(), "%llu,%f,%f,%f,%f,%f,%f,%f", &utime, &x, &y, &z, &qx, &qy, &qz, &qw); 61 | 62 | // step 2 验证: 文件没有结束, 并且的确得到了8个数据 63 | if(file.eof()) 64 | break; 65 | 66 | assert(n == 8); 67 | 68 | // step 3 构造当前帧的位姿(Eigen格式)并加入到 69 | Eigen::Quaternionf q(qw, qx, qy, qz); 70 | Eigen::Vector3f t(x, y, z); 71 | 72 | Eigen::Isometry3f T; 73 | T.setIdentity(); 74 | T.pretranslate(t).rotate(q); 75 | camera_trajectory[utime] = T; 76 | } 77 | } 78 | 79 | // 获取指定时间戳时, 相机位姿 Tcw 80 | Eigen::Matrix4f GroundTruthOdometry::getTransformation(uint64_t timestamp) 81 | { 82 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 83 | 84 | // 不是第一次读取 85 | if(last_utime != 0) 86 | { 87 | // 找 88 | std::map::const_iterator it = camera_trajectory.find(last_utime); 89 | if (it == camera_trajectory.end()) 90 | { 91 | // 没找到? 就返回单位阵 92 | last_utime = timestamp; 93 | return pose; 94 | } 95 | 96 | //Poses are stored in the file in iSAM basis, undo it -- 有一个格式上的转化 97 | Eigen::Matrix4f M; 98 | M << 0, 0, 1, 0, 99 | -1, 0, 0, 0, 100 | 0, -1, 0, 0, 101 | 0, 0, 0, 1; 102 | 103 | // ? 但是这里给的位姿是在原始相机位姿文件中给的坐标系表示的, 而下面返回的位姿却是在 EasticFusion 起始帧的相机坐标系下表示的? 104 | pose = M.inverse() * camera_trajectory[timestamp] * M; 105 | } 106 | else 107 | { 108 | // 是第一次读取相机的位姿 109 | std::map::const_iterator it = camera_trajectory.find(timestamp); 110 | // 注意, 这里获得的一般不是单位阵, 但是后面返回pose却是单位阵 111 | Eigen::Isometry3f ident = it->second; 112 | pose = Eigen::Matrix4f::Identity(); 113 | // 记录这个 0 对应的相机位姿, 相当于以后所有的返回的相机位姿真值是在 ElasticFusion 自己坐标系下的表示 114 | camera_trajectory[last_utime] = ident; 115 | } 116 | 117 | // Update 118 | last_utime = timestamp; 119 | 120 | return pose; 121 | } 122 | 123 | // 获取相机位姿的协方差, 恒对角阵 124 | Eigen::MatrixXd GroundTruthOdometry::getCovariance() 125 | { 126 | Eigen::MatrixXd cov(6, 6); 127 | cov.setIdentity(); 128 | // 平移 129 | cov(0, 0) = 0.1; 130 | cov(1, 1) = 0.1; 131 | cov(2, 2) = 0.1; 132 | // 旋转 133 | cov(3, 3) = 0.5; 134 | cov(4, 4) = 0.5; 135 | cov(5, 5) = 0.5; 136 | return cov; 137 | } 138 | -------------------------------------------------------------------------------- /GUI/src/Tools/RealSenseInterface.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file RealSenseInterface.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief RealSence 相机的接口. 由于使用的接口太老了, 这里就不分析了 5 | * @version 0.1 6 | * @date 2020-01-09 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #ifdef WITH_REALSENSE 20 | #include "librealsense/rs.hpp" 21 | #endif 22 | 23 | #include "ThreadMutexObject.h" 24 | #include "CameraInterface.h" 25 | 26 | class RealSenseInterface : public CameraInterface 27 | { 28 | public: 29 | RealSenseInterface(int width = 640,int height = 480,int fps = 30); 30 | virtual ~RealSenseInterface(); 31 | 32 | const int width,height,fps; 33 | 34 | bool getAutoExposure(); 35 | bool getAutoWhiteBalance(); 36 | virtual void setAutoExposure(bool value); 37 | virtual void setAutoWhiteBalance(bool value); 38 | 39 | virtual bool ok() 40 | { 41 | return initSuccessful; 42 | } 43 | 44 | virtual std::string error() 45 | { 46 | return errorText; 47 | } 48 | 49 | #ifdef WITH_REALSENSE 50 | struct RGBCallback 51 | { 52 | public: 53 | RGBCallback(int64_t & lastRgbTime, 54 | ThreadMutexObject & latestRgbIndex, 55 | std::pair * rgbBuffers) 56 | : lastRgbTime(lastRgbTime), 57 | latestRgbIndex(latestRgbIndex), 58 | rgbBuffers(rgbBuffers) 59 | { 60 | } 61 | 62 | void operator()(rs::frame frame) 63 | { 64 | lastRgbTime = std::chrono::duration_cast( 65 | std::chrono::system_clock::now().time_since_epoch()).count(); 66 | 67 | int bufferIndex = (latestRgbIndex.getValue() + 1) % numBuffers; 68 | 69 | memcpy(rgbBuffers[bufferIndex].first,frame.get_data(), 70 | frame.get_width() * frame.get_height() * 3); 71 | 72 | rgbBuffers[bufferIndex].second = lastRgbTime; 73 | 74 | latestRgbIndex++; 75 | } 76 | 77 | private: 78 | int64_t & lastRgbTime; 79 | ThreadMutexObject & latestRgbIndex; 80 | std::pair * rgbBuffers; 81 | }; 82 | 83 | struct DepthCallback 84 | { 85 | public: 86 | DepthCallback(int64_t & lastDepthTime, 87 | ThreadMutexObject & latestDepthIndex, 88 | ThreadMutexObject & latestRgbIndex, 89 | std::pair * rgbBuffers, 90 | std::pair,int64_t> * frameBuffers) 91 | : lastDepthTime(lastDepthTime), 92 | latestDepthIndex(latestDepthIndex), 93 | latestRgbIndex(latestRgbIndex), 94 | rgbBuffers(rgbBuffers), 95 | frameBuffers(frameBuffers) 96 | { 97 | } 98 | 99 | void operator()(rs::frame frame) 100 | { 101 | lastDepthTime = std::chrono::duration_cast( 102 | std::chrono::system_clock::now().time_since_epoch()).count(); 103 | 104 | int bufferIndex = (latestDepthIndex.getValue() + 1) % numBuffers; 105 | 106 | // The multiplication by 2 is here because the depth is actually uint16_t 107 | memcpy(frameBuffers[bufferIndex].first.first,frame.get_data(), 108 | frame.get_width() * frame.get_height() * 2); 109 | 110 | frameBuffers[bufferIndex].second = lastDepthTime; 111 | 112 | int lastImageVal = latestRgbIndex.getValue(); 113 | 114 | if(lastImageVal == -1) 115 | { 116 | return; 117 | } 118 | 119 | lastImageVal %= numBuffers; 120 | 121 | memcpy(frameBuffers[bufferIndex].first.second,rgbBuffers[lastImageVal].first, 122 | frame.get_width() * frame.get_height() * 3); 123 | 124 | latestDepthIndex++; 125 | } 126 | 127 | private: 128 | int64_t & lastDepthTime; 129 | ThreadMutexObject & latestDepthIndex; 130 | ThreadMutexObject & latestRgbIndex; 131 | 132 | std::pair * rgbBuffers; 133 | std::pair,int64_t> * frameBuffers; 134 | }; 135 | #endif 136 | 137 | private: 138 | #ifdef WITH_REALSENSE 139 | rs::device *dev; 140 | rs::context ctx; 141 | 142 | RGBCallback * rgbCallback; 143 | DepthCallback * depthCallback; 144 | #endif 145 | 146 | bool initSuccessful; 147 | std::string errorText; 148 | 149 | ThreadMutexObject latestRgbIndex; 150 | std::pair rgbBuffers[numBuffers]; 151 | 152 | int64_t lastRgbTime; 153 | int64_t lastDepthTime; 154 | 155 | }; 156 | -------------------------------------------------------------------------------- /GUI/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6.0) 2 | 3 | project(ElasticFusion) 4 | 5 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}") 6 | 7 | if(WIN32) 8 | macro(CANONIFY_BOOL var) 9 | if(${var}) 10 | set(${var} TRUE) 11 | else() 12 | set(${var} FALSE) 13 | endif() 14 | endmacro() 15 | endif() 16 | 17 | if(WIN32) 18 | find_package(JPEG REQUIRED) 19 | endif() 20 | 21 | find_package(LAPACK REQUIRED) 22 | find_package(BLAS REQUIRED) 23 | find_package(ZLIB REQUIRED) 24 | 25 | # HACK 为了选择使用支持OpenNI2的Pangolin 26 | add_definitions(-DUSE_OPENNI_PANGOLIN) 27 | set(Pangolin_DIR "/home/guoqing/libraries/Pangolin_ElasticFusion/lib/cmake/Pangolin") 28 | find_package(Pangolin 0.1 REQUIRED) 29 | message(STATUS "==> Pangolin_INCLUDE_DIRS = " ${Pangolin_INCLUDE_DIRS}) 30 | 31 | 32 | find_package(CUDA REQUIRED) 33 | # 下面直接手动包含了 34 | # find_package(OpenNI2 REQUIRED) 35 | 36 | find_package(efusion REQUIRED) 37 | find_package(SuiteSparse REQUIRED) 38 | 39 | if(WIN32) 40 | find_package(RealSense QUIET) 41 | CANONIFY_BOOL(REALSENSE_FOUND) 42 | message(STATUS "librealsense found: ${REALSENSE_FOUND}") 43 | option(WITH_REALSENSE "Build with Intel RealSense support?" ${REALSENSE_FOUND}) 44 | endif() 45 | 46 | if(WIN32) 47 | include_directories(${JPEG_INCLUDE_DIR}) 48 | endif() 49 | 50 | include_directories(${ZLIB_INCLUDE_DIR}) 51 | include_directories(${EIGEN_INCLUDE_DIRS}) 52 | include_directories(${Pangolin_INCLUDE_DIRS}) 53 | include_directories(${CUDA_INCLUDE_DIRS}) 54 | include_directories(${EFUSION_INCLUDE_DIR}) 55 | # include_directories(${OPENNI2_INCLUDE_DIR}) 56 | include_directories("/usr/include/openni2") 57 | 58 | 59 | # HACK 60 | set(WITH_REALSENSE false) 61 | 62 | if(WITH_REALSENSE) 63 | # HACK 64 | find_package(realsense2 REQUIRED) 65 | if (realsense2_FOUND) 66 | set("realsense_INCLUDE_DIR" "/usr/include/librealsense2") 67 | include_directories("${realsense_INCLUDE_DIR}") 68 | message(STATUS "==> ElasticFusion Found realsense2 " ${realsense_INCLUDE_DIR}) 69 | endif (realsense2_FOUND) 70 | 71 | 72 | # include_directories(${REALSENSE_INCLUDE_DIR}) 73 | # add_definitions(-DWITH_REALSENSE) 74 | # set(EXTRA_LIBS ${EXTRA_LIBS} ${REALSENSE_LIBRARY}) 75 | set(EXTRA_LIBS ${EXTRA_LIBS} ${realsense2_LIBRARY}) 76 | 77 | endif() 78 | 79 | file(GLOB srcs *.cpp) 80 | file(GLOB tools_srcs Tools/*.cpp) 81 | 82 | if(WIN32) 83 | file(GLOB hdrs *.h) 84 | file(GLOB tools_hdrs Tools/*.h) 85 | endif() 86 | 87 | if(WIN32) 88 | add_definitions(-Dlinux=0) 89 | add_definitions(-DWIN32_LEAN_AND_MEAN) 90 | add_definitions(-DNOMINMAX) 91 | set(ADDITIONAL_CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS}) 92 | set(EXTRA_WINDOWS_LIBS ${EXTRA_WINDOWS_LIBS} ws2_32 ${JPEG_LIBRARY}) 93 | else() 94 | add_definitions(-Dlinux=1) 95 | endif() 96 | 97 | set(CMAKE_CXX_FLAGS ${ADDITIONAL_CMAKE_CXX_FLAGS} "-O3 -msse2 -msse3 -Wall -std=c++11") 98 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -Wall -std=c++11") 99 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-int-in-bool-context") 100 | 101 | 102 | 103 | 104 | add_executable(ElasticFusion 105 | ${srcs} 106 | ${tools_srcs} 107 | ${hdrs} 108 | ${tools_hdrs} 109 | ) 110 | 111 | target_link_libraries(ElasticFusion 112 | ${EXTRA_WINDOWS_LIBS} 113 | ${ZLIB_LIBRARY} 114 | ${Pangolin_LIBRARIES} 115 | ${CUDA_LIBRARIES} 116 | ${EXTRA_LIBS} 117 | ${EFUSION_LIBRARY} 118 | # ${OPENNI2_LIBRARY} 119 | "/home/guoqing/software/OpenNI2_other/baidwangpan/OpenNI-Linux-x64-2.2/Tools/libOpenNI2.so" 120 | ${SUITESPARSE_LIBRARIES} 121 | ${BLAS_LIBRARIES} 122 | ${LAPACK_LIBRARIES} 123 | ) 124 | 125 | message(STATUS "==> EXTRA_WINDOWS_LIBS = " ${EXTRA_WINDOWS_LIBS}) 126 | message(STATUS "==> ZLIB_LIBRARY = " ${ZLIB_LIBRARY}) 127 | message(STATUS "==> Pangolin_LIBRARIES = " ${Pangolin_LIBRARIES}) 128 | message(STATUS "==> CUDA_LIBRARIES = " ${CUDA_LIBRARIES}) 129 | message(STATUS "==> EXTRA_LIBS = " ${EXTRA_LIBS}) 130 | message(STATUS "==> EFUSION_LIBRARY = " ${EFUSION_LIBRARY}) 131 | message(STATUS "==> SUITESPARSE_LIBRARIES = " ${SUITESPARSE_LIBRARIES}) 132 | message(STATUS "==> BLAS_LIBRARIES = " ${BLAS_LIBRARIES}) 133 | message(STATUS "==> LAPACK_LIBRARIES = " ${LAPACK_LIBRARIES}) 134 | 135 | 136 | INSTALL(TARGETS ElasticFusion 137 | RUNTIME DESTINATION bin 138 | LIBRARY DESTINATION lib 139 | ARCHIVE DESTINATION lib 140 | ) 141 | -------------------------------------------------------------------------------- /GUI/src/Tools/LogReader.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file LogReader.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 记录文件读取器 5 | * @version 0.1 6 | * @date 2020-01-03 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef LOGREADER_H_ 31 | #define LOGREADER_H_ 32 | 33 | #ifdef WIN32 34 | # include 35 | #endif 36 | #include 37 | #if (defined WIN32) && (defined FAR) 38 | # undef FAR 39 | #endif 40 | #include 41 | #ifndef WIN32 42 | # include 43 | #endif 44 | 45 | // 这个文件并不是必要的 46 | // #include 47 | #include 48 | 49 | // JPEG 解码器支持 50 | #include "JPEGLoader.h" 51 | 52 | /** @brief 记录文件读取器 */ 53 | class LogReader 54 | { 55 | public: 56 | /** 57 | * @brief 构造函数 58 | * @param[in] file 记录文件路径 59 | * @param[in] flipColors 是否翻转图像RGB/BGR 60 | */ 61 | LogReader(std::string file, bool flipColors) 62 | : flipColors(flipColors), 63 | timestamp(0), 64 | depth(0), 65 | rgb(0), 66 | currentFrame(0), 67 | decompressionBufferDepth(0), 68 | decompressionBufferImage(0), 69 | file(file), 70 | width(Resolution::getInstance().width()), 71 | height(Resolution::getInstance().height()), 72 | numPixels(width * height) 73 | {} 74 | 75 | /** @brief 空析构函数, 由子类实现 */ 76 | virtual ~LogReader() 77 | {} 78 | 79 | /** @brief 正序获取下一帧的图像数据, 保存在缓冲区中 */ 80 | virtual void getNext() = 0; 81 | 82 | /** 83 | * @brief 获取当前记录文件中的帧数 84 | * @return int 帧数 85 | */ 86 | virtual int getNumFrames() = 0; 87 | 88 | /** 89 | * @brief 记录文件中的帧数据是否全部读取完成 90 | * @return 是否 91 | */ 92 | virtual bool hasMore() = 0; 93 | 94 | /** 95 | * @brief 如果逆序读取, 现在是否已经需要"倒带"转变成为正序读取? 96 | * @return true 97 | * @return false 98 | */ 99 | virtual bool rewound() = 0; 100 | 101 | /** @brief 重置整个记录文件读取器 */ 102 | virtual void rewind() = 0; 103 | 104 | /** @brief 倒序读取记录文件 */ 105 | virtual void getBack() = 0; 106 | 107 | /** 108 | * @brief 快进到第 frame 帧 109 | * @param[in] frame 帧id 110 | */ 111 | virtual void fastForward(int frame) = 0; 112 | 113 | /** 114 | * @brief 获取记录文件的路径 115 | * @return const std::string 路径 116 | */ 117 | virtual const std::string getFile() = 0; 118 | 119 | /** 120 | * @brief 设置自动曝光/白平衡参数 121 | * @param[in] value 是否设置 122 | */ 123 | virtual void setAuto(bool value) = 0; 124 | 125 | bool flipColors; ///< 是否翻转图像 RGB、BGR 126 | int64_t timestamp; ///< 帧的时间戳, 单位为 ns 127 | 128 | unsigned short * depth; ///< 可以直接使用的深度图像首地址, 注意和下面的 decompressionBufferDepth 指针数据类型不同 129 | unsigned char * rgb; ///< 可以直接使用的彩色图像首地址 130 | int currentFrame; ///< 当前帧的计数 131 | 132 | protected: 133 | Bytef * decompressionBufferDepth; ///< 解压缩后可以直接使用的深度图像 134 | Bytef * decompressionBufferImage; ///< 解压缩后可以直接使用的深度图像 135 | unsigned char * depthReadBuffer; ///< 从外部文件中读取到的深度图像数据将会被暂时存储到这里 136 | unsigned char * imageReadBuffer; ///< 从外部文件中读取到的彩色图像数据将会被暂时存储到这里 137 | int32_t depthSize; ///< 深度图像的数据长度(byte) 138 | int32_t imageSize; ///< 彩色图像的数据长度(byte) 139 | 140 | const std::string file; ///< 记录文件的路径 141 | FILE * fp; ///< C 风格的文件指针, 用于指向打开的记录文件 142 | int32_t numFrames; ///< 记录文件中帧的个数 143 | int width; ///< 输入图像的宽度 144 | int height; ///< 输入图像的高度 145 | int numPixels; ///< 输入图像的像素数目 146 | 147 | JPEGLoader jpeg; ///< Jpeg 编解码器 148 | }; 149 | 150 | #endif /* LOGREADER_H_ */ 151 | -------------------------------------------------------------------------------- /Core/src/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 | * unless explicitly stated. By downloading this file you agree to 12 | * comply with these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then 15 | * please email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | 19 | #ifndef RGBDODOMETRY_H_ 20 | #define RGBDODOMETRY_H_ 21 | 22 | #include "Stopwatch.h" 23 | #include "../GPUTexture.h" 24 | #include "../Cuda/cudafuncs.cuh" 25 | #include "OdometryProvider.h" 26 | #include "GPUConfig.h" 27 | 28 | #include 29 | #include 30 | 31 | class RGBDOdometry 32 | { 33 | public: 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | RGBDOdometry(int width, 36 | int height, 37 | float cx, float cy, float fx, float fy, 38 | float distThresh = 0.10f, 39 | float angleThresh = sin(20.f * 3.14159254f / 180.f)); 40 | 41 | virtual ~RGBDOdometry(); 42 | 43 | void initICP(GPUTexture * filteredDepth, const float depthCutoff); 44 | 45 | void initICP(GPUTexture * predictedVertices, GPUTexture * predictedNormals, const float depthCutoff); 46 | 47 | void initICPModel(GPUTexture * predictedVertices, GPUTexture * predictedNormals, const float depthCutoff, const Eigen::Matrix4f & modelPose); 48 | 49 | void initRGB(GPUTexture * rgb); 50 | 51 | void initRGBModel(GPUTexture * rgb); 52 | 53 | void initFirstRGB(GPUTexture * rgb); 54 | 55 | void getIncrementalTransformation(Eigen::Vector3f & trans, 56 | Eigen::Matrix & rot, 57 | const bool & rgbOnly, 58 | const float & icpWeight, 59 | const bool & pyramid, 60 | const bool & fastOdom, 61 | const bool & so3); 62 | 63 | Eigen::MatrixXd getCovariance(); 64 | 65 | float lastICPError; ///< ICP 匹配过程最后的残差 66 | float lastICPCount; ///< ICP 过程中的内点个数 67 | float lastRGBError; 68 | float lastRGBCount; 69 | float lastSO3Error; 70 | float lastSO3Count; 71 | 72 | Eigen::Matrix lastA; 73 | Eigen::Matrix lastb; 74 | 75 | private: 76 | void populateRGBDData(GPUTexture * rgb, 77 | DeviceArray2D * destDepths, 78 | DeviceArray2D * destImages); 79 | 80 | std::vector > depth_tmp; 81 | 82 | DeviceArray vmaps_tmp; 83 | DeviceArray nmaps_tmp; 84 | 85 | std::vector > vmaps_g_prev_; 86 | std::vector > nmaps_g_prev_; 87 | 88 | std::vector > vmaps_curr_; 89 | std::vector > nmaps_curr_; 90 | 91 | CameraModel intr; 92 | 93 | DeviceArray sumDataSE3; 94 | DeviceArray outDataSE3; 95 | DeviceArray sumResidualRGB; 96 | 97 | DeviceArray sumDataSO3; 98 | DeviceArray outDataSO3; 99 | 100 | const int sobelSize; 101 | const float sobelScale; 102 | const float maxDepthDeltaRGB; 103 | const float maxDepthRGB; 104 | 105 | std::vector pyrDims; 106 | 107 | static const int NUM_PYRS = 3; 108 | 109 | DeviceArray2D lastDepth[NUM_PYRS]; 110 | DeviceArray2D lastImage[NUM_PYRS]; 111 | 112 | DeviceArray2D nextDepth[NUM_PYRS]; 113 | DeviceArray2D nextImage[NUM_PYRS]; 114 | DeviceArray2D nextdIdx[NUM_PYRS]; 115 | DeviceArray2D nextdIdy[NUM_PYRS]; 116 | 117 | DeviceArray2D lastNextImage[NUM_PYRS]; 118 | 119 | DeviceArray2D corresImg[NUM_PYRS]; 120 | 121 | DeviceArray2D pointClouds[NUM_PYRS]; 122 | 123 | std::vector iterations; 124 | std::vector minimumGradientMagnitudes; 125 | 126 | float distThres_; 127 | float angleThres_; 128 | 129 | Eigen::Matrix lastCov; 130 | 131 | const int width; 132 | const int height; 133 | const float cx, cy, fx, fy; 134 | }; 135 | 136 | #endif /* RGBDODOMETRY_H_ */ 137 | -------------------------------------------------------------------------------- /GUI/src/MainController.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file MainController.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 定义 MainController 类 5 | * @version 0.1 6 | * @date 2020-01-03 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | // ElasticFusion 对象 31 | #include 32 | // 命令行参数解析 33 | #include 34 | 35 | // GUI 界面 36 | #include "Tools/GUI.h" 37 | // 加载真值比较用的 38 | #include "Tools/GroundTruthOdometry.h" 39 | // 数据源Base 40 | #include "Tools/RawLogReader.h" 41 | // 实时摄像头数据 42 | #include "Tools/LiveLogReader.h" 43 | 44 | #ifndef MAINCONTROLLER_H_ 45 | #define MAINCONTROLLER_H_ 46 | 47 | // GUI 接口的实现的类 48 | class MainController 49 | { 50 | public: 51 | /** 52 | * @brief 构造函数 53 | * @param[in] argc 命令行参数个数 54 | * @param[in] argv 命令行参数内容 55 | */ 56 | MainController(int argc, char * argv[]); 57 | 58 | /** @brief 析构函数 */ 59 | virtual ~MainController(); 60 | 61 | /** @brief 运行 */ 62 | void launch(); 63 | 64 | private: 65 | /* @brief 主循环, 实现给 ElasticFusion 喂图像的工作 */ 66 | void run(); 67 | 68 | /** 69 | * @brief 加载指定文件中的相机内参信息 70 | * @param[in] filename 文件名 71 | */ 72 | void loadCalibration(const std::string & filename); 73 | 74 | bool good; ///< 当前是否已经正确地初始化数据源了 75 | ElasticFusion * eFusion; ///< ElasticFusion Core 对象指针 76 | GUI * gui; ///< GUI 窗口对象 77 | GroundTruthOdometry * groundTruthOdometry; ///< 轨迹真值文件对象句柄 78 | LogReader * logReader; ///< 记录文件读取器的句柄, 根据情况也会是实时相机的接口 79 | 80 | bool iclnuim; ///< 是否使用 ICL-NUIM 数据集 81 | std::string logFile; ///< 命令行中指定的记录文件路径 82 | std::string poseFile; ///< 命令行参数中指定的真值文件 83 | 84 | float confidence, ///< Surfel confidence threshold 85 | depth, ///< 深度切断值. 这个值是考虑到过远的深度值测量不准确, 误差较大, 所以直接将距离相机过远的深度值"切掉",我们直接不用了 86 | icp, ///< Relative ICP/RGB tracking weight 87 | icpErrThresh, ///< Local loop closure residual threshold 88 | covThresh, ///< Local loop closure covariance threshold 89 | photoThresh, ///< Global loop closure photometric threshold 90 | fernThresh; ///< Fern encoding threshold 91 | 92 | int timeDelta, ///< Time window length 93 | icpCountThresh, ///< Local loop closure inlier threshold 94 | start, ///< Frames to skip at start of log 跳过头几帧， 存储的数字为帧id 95 | end; ///< Cut off frame of log 执行多少帧， 存储的为最后一帧处理的id 96 | 97 | bool fillIn, 98 | openLoop, ///< Open loop mode, 不使用闭环 99 | reloc, ///< Enable relocalisation 100 | frameskip, ///< Frame skip if processing a log to simulate real-time. 就是如果处理比较慢， 那么就根据时间戳跳帧 101 | quiet, ///< Quit when finished a log 播放记录文件完成后自动退出 102 | fastOdom, ///< Fast odometry (single level pyramid) 103 | so3, ///< Disables SO(3) pre-alignment in tracking 104 | rewind, ///< Rewind and loop log forever 设置数据记录文件是否需要反复循环读取 105 | frameToFrameRGB; ///< Do frame-to-frame RGB tracking 106 | 107 | int framesToSkip; ///< 为了仿真"实时处理数据", 当处理速度跟不上帧率的时候会产生的跳帧的个数 108 | bool streaming; 109 | bool resetButton; ///< 复位请求 110 | 111 | Resize * resizeStream; ///< 使用 Shader 对原始图像进行降采样的对象 112 | }; 113 | 114 | #endif /* MAINCONTROLLER_H_ */ 115 | -------------------------------------------------------------------------------- /Core/src/Deformation.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Deformation.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief Deformation 对象, 包含 Deformation Graph 对象 5 | * @version 0.1 6 | * @date 2020-01-09 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef DEFORMATION_H_ 31 | #define DEFORMATION_H_ 32 | 33 | #include "Utils/DeformationGraph.h" 34 | #include "Shaders/Shaders.h" 35 | #include "Shaders/Uniform.h" 36 | #include "Shaders/Vertex.h" 37 | #include "GPUTexture.h" 38 | #include "Utils/Resolution.h" 39 | #include "Utils/Intrinsics.h" 40 | #include "Ferns.h" 41 | #include "Defines.h" 42 | 43 | #include 44 | 45 | /** @brief Deformation 对象, 包含 Deformation Graph 对象 */ 46 | class Deformation 47 | { 48 | public: 49 | Deformation(); 50 | virtual ~Deformation(); 51 | 52 | /** 53 | * @brief 获取 Deformation Graph 的节点 54 | * @return getGraph Nodes 55 | */ 56 | EFUSION_API std::vector & getGraph(); 57 | 58 | void getRawGraph(std::vector & graph); 59 | 60 | void sampleGraphModel(const std::pair & model); 61 | 62 | void sampleGraphFrom(Deformation & other); 63 | 64 | class Constraint 65 | { 66 | public: 67 | Constraint(const Eigen::Vector3f & src, 68 | const Eigen::Vector3f & target, 69 | const uint64_t & srcTime, 70 | const uint64_t & targetTime, 71 | const bool relative, 72 | const bool pin = false) 73 | : src(src), 74 | target(target), 75 | srcTime(srcTime), 76 | targetTime(targetTime), 77 | relative(relative), 78 | pin(pin), 79 | srcPointPoolId(-1), 80 | tarPointPoolId(-1) 81 | {} 82 | 83 | Eigen::Vector3f src; 84 | Eigen::Vector3f target; 85 | uint64_t srcTime; 86 | uint64_t targetTime; 87 | bool relative; 88 | bool pin; 89 | int srcPointPoolId; 90 | int tarPointPoolId; 91 | }; 92 | 93 | void addConstraint(const Eigen::Vector4f & src, 94 | const Eigen::Vector4f & target, 95 | const uint64_t & srcTime, 96 | const uint64_t & targetTime, 97 | const bool pinConstraints); 98 | 99 | void addConstraint(const Constraint & constraint); 100 | 101 | bool constrain(std::vector & ferns, 102 | std::vector & rawGraph, 103 | int time, 104 | const bool fernMatch, 105 | std::vector > & poseGraph, 106 | const bool relaxGraph, 107 | std::vector * newRelativeCons = 0); 108 | 109 | Eigen::Vector4f * getVertices() 110 | { 111 | return vertices; 112 | } 113 | 114 | int getCount() 115 | { 116 | return int(count); 117 | } 118 | 119 | int getLastDeformTime() 120 | { 121 | return lastDeformTime; 122 | } 123 | 124 | private: 125 | DeformationGraph def; ///? 真正的 Deformation Graph? 126 | 127 | std::vector vertexTimes; 128 | std::vector pointPool; 129 | int originalPointPool; 130 | int firstGraphNode; 131 | 132 | std::shared_ptr sampleProgram; 133 | GLuint vbo; 134 | GLuint fid; 135 | const int bufferSize; 136 | GLuint countQuery; 137 | unsigned int count; 138 | Eigen::Vector4f * vertices; 139 | 140 | std::vector > poseGraphPoints; 141 | std::vector graphPoseTimes; 142 | std::vector * graphPosePoints; 143 | 144 | std::vector constraints; 145 | int lastDeformTime; 146 | }; 147 | 148 | #endif /* DEFORMATION_H_ */ 149 | -------------------------------------------------------------------------------- /GUI/src/Tools/ThreadMutexObject.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ThreadMutexObject.h 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief 疑似是用于执行互斥锁相关功能的模板类 5 | * @version 0.1 6 | * @date 2020-01-04 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef THREADMUTEXOBJECT_H_ 31 | #define THREADMUTEXOBJECT_H_ 32 | 33 | // C++ STL 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | /** 40 | * @brief 使用线程锁封装好的变量类 41 | * @tparam T 被封装好的变量的类型 42 | */ 43 | template 44 | class ThreadMutexObject 45 | { 46 | public: 47 | /** @brief 默认构造函数 */ 48 | ThreadMutexObject() 49 | {} 50 | 51 | /** 52 | * @brief 给定被保护变量初始值的构造函数 53 | * @param[in] initialValue 初始值 54 | */ 55 | ThreadMutexObject(T initialValue) 56 | : object(initialValue), 57 | lastCopy(initialValue) 58 | {} 59 | 60 | /** 61 | * @brief 给这个变量赋值 62 | * @param[in] newValue 新值 63 | */ 64 | void assign(T newValue) 65 | { 66 | // 上锁 - 赋值 - 解锁 67 | mutex.lock(); 68 | object = lastCopy = newValue; 69 | mutex.unlock(); 70 | } 71 | 72 | /** 73 | * @brief 获取互斥锁 74 | * @return std::mutex& 保护这个变量的互斥锁 75 | */ 76 | std::mutex & getMutex() 77 | { 78 | return mutex; 79 | } 80 | 81 | /** 82 | * @brief 获取被保护变量的引用 83 | * @note 但是目测这个方法是不安全的, 获取的引用的操作将不再受互斥锁保护 84 | * @return T& 引用 85 | */ 86 | T & getReference() 87 | { 88 | return object; 89 | } 90 | 91 | /** 92 | * @brief 赋值并唤醒关联的其他线程 93 | * @param[in] newValue 新值 94 | */ 95 | void assignAndNotifyAll(T newValue) 96 | { 97 | mutex.lock(); 98 | 99 | object = newValue; 100 | 101 | signal.notify_all(); 102 | 103 | mutex.unlock(); 104 | } 105 | 106 | 107 | /** @brief 唤醒关联的其他线程 */ 108 | void notifyAll() 109 | { 110 | mutex.lock(); 111 | 112 | signal.notify_all(); 113 | 114 | mutex.unlock(); 115 | } 116 | 117 | /** @brief 获取被保护对象的值 */ 118 | T getValue() 119 | { 120 | mutex.lock(); 121 | 122 | lastCopy = object; 123 | 124 | mutex.unlock(); 125 | 126 | return lastCopy; 127 | } 128 | 129 | /** 130 | * @brief 当前线程进入挂起状态知道被保护的变量更新 131 | * @return T 被保护变量更新后的值 132 | */ 133 | T waitForSignal() 134 | { 135 | mutex.lock(); 136 | 137 | signal.wait(mutex); 138 | 139 | lastCopy = object; 140 | 141 | mutex.unlock(); 142 | 143 | return lastCopy; 144 | } 145 | 146 | /** 147 | * @brief 当前线程挂起一段时间后获取被保护变量的值 148 | * @param[in] wait 挂起的时间(ms) 149 | * @return T 被保护变量的值 150 | */ 151 | T getValueWait(int wait = 33000) 152 | { 153 | std::this_thread::sleep_for(std::chrono::microseconds(wait)); 154 | 155 | mutex.lock(); 156 | 157 | lastCopy = object; 158 | 159 | mutex.unlock(); 160 | 161 | return lastCopy; 162 | } 163 | 164 | /** 165 | * @brief 当前线程挂起一段时间后获取被保护变量的引用 166 | * @param[in] wait 挂起的时间(ms) 167 | * @return T 被保护变量的引用 168 | */ 169 | T & getReferenceWait(int wait = 33000) 170 | { 171 | std::this_thread::sleep_for(std::chrono::microseconds(wait)); 172 | 173 | mutex.lock(); 174 | 175 | lastCopy = object; 176 | 177 | mutex.unlock(); 178 | 179 | return lastCopy; 180 | } 181 | 182 | /** 183 | * @brief 对被保护的变量进行自增操作 184 | * @note 但是目测这样只对为整型的被保护变量才有用 185 | */ 186 | void operator++(int) 187 | { 188 | mutex.lock(); 189 | 190 | object++; 191 | 192 | mutex.unlock(); 193 | } 194 | 195 | private: 196 | T object; ///< 被线程锁保护的对象 197 | T lastCopy; ///< 用于缓存最近一次访问被保护对象的值 198 | 199 | std::mutex mutex; ///< 用于保护这个变量的锁 200 | std::condition_variable_any signal; ///< 条件变量, 通知其他线程时使用 201 | }; 202 | 203 | #endif /* THREADMUTEXOBJECT_H_ */ 204 | -------------------------------------------------------------------------------- /GUI/src/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 | FIND_LIBRARY( SUITESPARSE_SUITESPARSECONFIG_LIBRARY 65 | NAMES suitesparseconfig 66 | PATHS ${SUITESPARSE_LIBRARY_DIR} ) 67 | IF ( WIN32 ) 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/src/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 | -------------------------------------------------------------------------------- /GUI/src/Tools/JPEGLoader.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file JPEGLoader.cpp 3 | * @author guoqing (1337841346@qq.com) 4 | * @brief JPEG 编解码支持 5 | * @version 0.1 6 | * @date 2020-01-03 7 | * 8 | * @copyright Copyright (c) 2020 9 | * 10 | */ 11 | 12 | /* 13 | * This file is part of ElasticFusion. 14 | * 15 | * Copyright (C) 2015 Imperial College London 16 | * 17 | * The use of the code within this file and all code within files that 18 | * make up the software that is ElasticFusion is permitted for 19 | * non-commercial purposes only. The full terms and conditions that 20 | * apply to the code within this file are detailed within the LICENSE.txt 21 | * file and at 22 | * unless explicitly stated. By downloading this file you agree to 23 | * comply with these terms. 24 | * 25 | * If you wish to use any of this code for commercial purposes then 26 | * please email researchcontracts.engineering@imperial.ac.uk. 27 | * 28 | */ 29 | 30 | #ifndef TOOLS_JPEGLOADER_H_ 31 | #define TOOLS_JPEGLOADER_H_ 32 | 33 | extern "C" 34 | { 35 | // jpeg 编解码支持 36 | #include "jpeglib.h" 37 | } 38 | 39 | // C STL 40 | #include 41 | #include 42 | 43 | /** 44 | * @brief 发生 JPEG 错误的时候的回调函数 45 | * @param[in] cinfo 解码器对象 46 | */ 47 | static void jpegFail(j_common_ptr cinfo) 48 | { 49 | // 直接给出断言错误 50 | assert(false && "JPEG decoding error!"); 51 | } 52 | 53 | /** @brief 空函数, 什么也不做 */ 54 | static void doNothing(j_decompress_ptr) 55 | { 56 | 57 | } 58 | 59 | /** @brief JPEG 编解码支持 */ 60 | class JPEGLoader 61 | { 62 | public: 63 | /** @brief 空构造函数 */ 64 | JPEGLoader() 65 | {} 66 | 67 | /** 68 | * @brief 从JPEG编码压缩的图像中恢复出原始图像 69 | * @param[in] src 从文件中读取的原始图像存放的缓冲区 70 | * @param[in] numBytes 原始编码格式的图像长度 71 | * @param[out] data 解码之后存放图像的缓冲区 72 | */ 73 | void readData(unsigned char * src, const int numBytes, unsigned char * data) 74 | { 75 | // step 0 准备数据 76 | jpeg_decompress_struct cinfo; // IJG JPEG codec structure 解压缩器对象 77 | jpeg_error_mgr errorMgr; // 错误发生的时候的管理器 78 | 79 | // step 1 错误对象设置 80 | errorMgr.error_exit = jpegFail; // 设置发生解码错误的时候回调的函数 81 | cinfo.err = jpeg_std_error(&errorMgr); // 设置错误管理器 82 | jpeg_create_decompress(&cinfo); // 创建对象 83 | 84 | // step 2 数据源对象设置 85 | jpeg_source_mgr srcMgr; // 数据源管理器 86 | cinfo.src = &srcMgr; 87 | 88 | // Prepare for suspending reader 89 | srcMgr.init_source = doNothing; // ? 不知道是干嘛的, 设置为了一个空函数; 感觉可能是这个库之前仅支持从文件中获取数据有关 90 | srcMgr.resync_to_restart = jpeg_resync_to_restart; // 好像是处理数据源发生的时间不同步问题, 使用了 libJpeg 的默认处理函数 91 | srcMgr.term_source = doNothing; // ? 同上 92 | srcMgr.next_input_byte = src; // 设置需要解压缩图像缓冲区首指针 93 | srcMgr.bytes_in_buffer = numBytes; // 设置需要解压缩图像缓冲区的长度 94 | 95 | // 启动解压过程, 查看数据流的内容, 从数据流的头部获取关于图像的大小/通道数等信息 96 | jpeg_read_header(&cinfo, // 解压对象 97 | TRUE); // 是否获取图像 98 | 99 | // 计算解压缩输出图像的维度 100 | jpeg_calc_output_dimensions(&cinfo); 101 | 102 | // 开始解压缩过程 103 | jpeg_start_decompress(&cinfo); 104 | 105 | // 获取输出图像的大小 106 | int width = cinfo.output_width; 107 | int height = cinfo.output_height; 108 | 109 | // JSAMPARRAY 本质就是 uint8_t* 110 | // 由于 libJPEG 使用C编写, 考虑到动态内存的申请相对耗时, 并且容易忘记释放, libJPEG 中使用 "pool" 的概念来处理, 有点线程池的感觉 111 | // 说白了就是申请一大段内存, 然后重复使用. 下面函数参数中的 JPOOL_IMAGE 就表示一旦处理完之后就自动放弃对原 pool 中对应空间的占有 112 | JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)( // 调用解压对象的 mem manger, 在 pool 中获取一段 buffer 113 | (j_common_ptr)&cinfo, // j_common_ptr 对象指针 114 | JPOOL_IMAGE, // pool_id, lasts until done with image/datastream 115 | width * 4, // samplesperrow, 一个像素占四个字节, RGB32格式, ref: [https://blog.csdn.net/byhook/article/details/84262330] 116 | 1); // 一行图像大小就够了, 因为后面的 jpeg_read_scanlines() 函数只能一行行地读取计算结果 117 | 118 | // 将解码结果复制到输出缓冲区 119 | for(; height--; data += (width * 3)) 120 | { 121 | // 从解压结果中获取一行 122 | jpeg_read_scanlines( 123 | &cinfo, // 解压对象 124 | buffer, // 输出位置 125 | 1); // 读取一行解压图像 126 | 127 | // 直接读取得到的戒烟图像颜色通道是 BGR, RGB32 格式中是这样定义的, 所以接下来还要进行颜色通道的转换 128 | unsigned char * bgr = (unsigned char *)buffer[0]; 129 | unsigned char * rgb = (unsigned char *)data; 130 | 131 | // BGR => RGB 132 | for(int i = 0; i < width; i++, bgr += 3, rgb += 3) 133 | { 134 | unsigned char t0 = bgr[0], t1 = bgr[1], t2 = bgr[2]; 135 | rgb[2] = t0; rgb[1] = t1; rgb[0] = t2; 136 | } 137 | } 138 | 139 | // 解压缩过程结束, 释放对象 140 | jpeg_finish_decompress(&cinfo); 141 | jpeg_destroy_decompress(&cinfo); 142 | } 143 | }; 144 | 145 | 146 | #endif /* TOOLS_JPEGLOADER_H_ */ 147 | --------------------------------------------------------------------------------