├── .gitattributes ├── .gitignore ├── CMakeLists.txt ├── LICENCE ├── README.md ├── cmake └── Modules │ ├── FindGLEW.cmake │ ├── FindGLFW.cmake │ └── FindOpenCL.cmake ├── dependencies ├── glew-1.9.0-win32.zip └── glfw-2.7.6.bin.WIN32.zip ├── include ├── config.h.in ├── data_packet.h ├── definitions.h ├── gpu_manager.h ├── miroir_manager.h ├── miroir_module.h ├── miroir_module_manager.h ├── profiler.h ├── raycast_module.h ├── surface_measurement_module.h ├── tracking_module.h ├── update_reconstruction_module.h ├── util.h └── window_listener.h └── src ├── OpenCL ├── TSDF.cl ├── bilateral.cl ├── gpu_def.h ├── raycast.cl ├── test.cl ├── tracking_correspondence.cl └── tracking_sum.cl ├── Shaders ├── PhongShading.fs ├── PhongShading.vs ├── frag.glsl └── vert.glsl ├── data_packet.cpp ├── gpu_manager.cpp ├── miroir.cpp ├── miroir_manager.cpp ├── miroir_module_manager.cpp ├── profiler.cpp ├── raycast_module.cpp ├── surface_measurement_module.cpp ├── tracking_module.cpp └── update_reconstruction_module.cpp /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | *.sln merge=union 7 | *.csproj merge=union 8 | *.vbproj merge=union 9 | *.fsproj merge=union 10 | *.dbproj merge=union 11 | 12 | # Standard to msysgit 13 | *.doc diff=astextplain 14 | *.DOC diff=astextplain 15 | *.docx diff=astextplain 16 | *.DOCX diff=astextplain 17 | *.dot diff=astextplain 18 | *.DOT diff=astextplain 19 | *.pdf diff=astextplain 20 | *.PDF diff=astextplain 21 | *.rtf diff=astextplain 22 | *.RTF diff=astextplain 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ################# 2 | ## Eclipse 3 | ################# 4 | 5 | *.pydevproject 6 | .project 7 | .metadata 8 | bin/ 9 | tmp/ 10 | *.tmp 11 | *.bak 12 | *.swp 13 | *~.nib 14 | local.properties 15 | .classpath 16 | .settings/ 17 | .loadpath 18 | 19 | # External tool builders 20 | .externalToolBuilders/ 21 | 22 | # Locally stored "Eclipse launch configurations" 23 | *.launch 24 | 25 | # CDT-specific 26 | .cproject 27 | 28 | # PDT-specific 29 | .buildpath 30 | 31 | 32 | ################# 33 | ## Visual Studio 34 | ################# 35 | 36 | ## Ignore Visual Studio temporary files, build results, and 37 | ## files generated by popular Visual Studio add-ons. 38 | 39 | # User-specific files 40 | *.suo 41 | *.user 42 | *.sln.docstates 43 | 44 | # Build results 45 | [Dd]ebug/ 46 | [Rr]elease/ 47 | *_i.c 48 | *_p.c 49 | *.ilk 50 | *.meta 51 | *.obj 52 | *.pch 53 | *.pdb 54 | *.pgc 55 | *.pgd 56 | *.rsp 57 | *.sbr 58 | *.tlb 59 | *.tli 60 | *.tlh 61 | *.tmp 62 | *.vspscc 63 | .builds 64 | *.dotCover 65 | 66 | ## TODO: If you have NuGet Package Restore enabled, uncomment this 67 | #packages/ 68 | 69 | # Visual C++ cache files 70 | ipch/ 71 | *.aps 72 | *.ncb 73 | *.opensdf 74 | *.sdf 75 | 76 | # Visual Studio profiler 77 | *.psess 78 | *.vsp 79 | 80 | # ReSharper is a .NET coding add-in 81 | _ReSharper* 82 | 83 | # Installshield output folder 84 | [Ee]xpress 85 | 86 | # DocProject is a documentation generator add-in 87 | DocProject/buildhelp/ 88 | DocProject/Help/*.HxT 89 | DocProject/Help/*.HxC 90 | DocProject/Help/*.hhc 91 | DocProject/Help/*.hhk 92 | DocProject/Help/*.hhp 93 | DocProject/Help/Html2 94 | DocProject/Help/html 95 | 96 | # Click-Once directory 97 | publish 98 | 99 | # Others 100 | [Bb]in 101 | [Oo]bj 102 | sql 103 | TestResults 104 | *.Cache 105 | ClientBin 106 | stylecop.* 107 | ~$* 108 | *.dbmdl 109 | Generated_Code #added for RIA/Silverlight projects 110 | 111 | # Backup & report files from converting an old project file to a newer 112 | # Visual Studio version. Backup files are not needed, because we have git ;-) 113 | _UpgradeReport_Files/ 114 | Backup*/ 115 | UpgradeLog*.XML 116 | 117 | 118 | 119 | ############ 120 | ## Windows 121 | ############ 122 | 123 | # Windows image file caches 124 | Thumbs.db 125 | 126 | # Folder config file 127 | Desktop.ini 128 | 129 | 130 | ############# 131 | ## Python 132 | ############# 133 | 134 | *.py[co] 135 | 136 | # Packages 137 | *.egg 138 | *.egg-info 139 | dist 140 | build 141 | eggs 142 | parts 143 | bin 144 | var 145 | sdist 146 | develop-eggs 147 | .installed.cfg 148 | 149 | # Installer logs 150 | pip-log.txt 151 | 152 | # Unit test / coverage reports 153 | .coverage 154 | .tox 155 | 156 | #Translations 157 | *.mo 158 | 159 | #Mr Developer 160 | .mr.developer.cfg 161 | 162 | # Mac crap 163 | .DS_Store 164 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6 FATAL_ERROR) 2 | project(MIRROIR) 3 | 4 | find_package(PCL 1.3 REQUIRED COMPONENTS common io visualization) 5 | find_package(Boost COMPONENTS timer REQUIRED chrono REQUIRED) 6 | 7 | # TODO : figure out good compiler options 8 | if(CMAKE_COMPILER_IS_GNUCXX) 9 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") 10 | endif() 11 | 12 | file(GLOB incs include/*.h) 13 | file(GLOB srcs src/*.cpp) 14 | file(GLOB krnls src/OpenCL/*.cl) 15 | 16 | include_directories(${PCL_INCLUDE_DIRS}) 17 | link_directories(${PCL_LIBRARY_DIRS}) 18 | add_definitions(${PCL_DEFINITIONS}) 19 | 20 | include_directories(${CMAKE_SOURCE_DIR}/include) 21 | 22 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/Modules/") 23 | 24 | # path to cl kernel files 25 | set( KERNEL_PATH "\"${PROJECT_SOURCE_DIR}/src/OpenCL/\"" ) 26 | configure_file ( "${PROJECT_SOURCE_DIR}/include/config.h.in" "${PROJECT_BINARY_DIR}/config.h" ) 27 | include_directories("${PROJECT_BINARY_DIR}") 28 | 29 | #OpenCL 30 | find_package(OPENCL REQUIRED) 31 | find_package(GLEW REQUIRED) 32 | find_package(GLFW REQUIRED) 33 | include_directories(${OPENCL_INCLUDE_DIRS} ${GLFW_INCLUDE_PATH} ${GLEW_INCLUDE_PATH}) 34 | 35 | if( OPENCL_HAS_CPP_BINDINGS ) 36 | message( "OpenCL has CPP bindings. Full include is: " ${OPENCL_INCLUDE_DIRS} ) 37 | else( OPENCL_HAS_CPP_BINDINGS ) 38 | message( "No OpenCL CPP bindings found" ) 39 | endif( OPENCL_HAS_CPP_BINDINGS ) 40 | 41 | add_executable(miroir ${srcs} ${incs} ${krnls}) 42 | target_link_libraries(miroir ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_VISUALIZATION_LIBRARIES} ${OPENCL_LIBRARIES} ${GLEW_LIBRARY} ${GLFW_LIBRARY} ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY}) 43 | #add_subdirectory(src) -------------------------------------------------------------------------------- /LICENCE: -------------------------------------------------------------------------------- 1 | Copyright 2013 Anthony Chansavang and other contributors 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining 4 | a copy of this software and associated documentation files (the 5 | "Software"), to deal in the Software without restriction, including 6 | without limitation the rights to use, copy, modify, merge, publish, 7 | distribute, sublicense, and/or sell copies of the Software, and to 8 | permit persons to whom the Software is furnished to do so, subject to 9 | the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be 12 | included in all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 15 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 16 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 17 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 18 | LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 19 | OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 20 | WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | MiroirFusion 2 | ============ 3 | 4 | This is an OpenCL version of the KinectFusion (http://research.microsoft.com/en-us/projects/surfacerecon/). 5 | The OpenCL code is mainly inspired by the CUDA implementation from the PCL project (http://pointclouds.org/). 6 | 7 | As I don't have a lot of time to dedicate to this project anymore, I just decided to release it to the public and maybe you guys will contribute to improving it :) 8 | 9 | The application uses the PCL visualizer and can also use GLFW to show the raycast from the camera (uncomment GL_INTEROP in gpu_def.h). 10 | 11 | Compiling 12 | --------- 13 | CMake included. 14 | Requires PCL, GLFW, Boost, GLEW. First install PCL and all its dependencies (Boost, Qt) then unzip GLFW and GLEW. Use CMake to point to GLFW and GLEW's library and include folder. Generate the solution files with CMake. 15 | 16 | Commands (for PCL visualizer only) 17 | ----------------------------------- 18 | F1 : show the raw depth map from depth sensor 19 | F2 : show the result of bilateral filtering on raw depth map 20 | F3 : show the TSDF 21 | F4 : show the raycasted surface normal map 22 | F5 : show the corresponding vertices from filtered depth map and raycasted surface 23 | 24 | -------------------------------------------------------------------------------- /cmake/Modules/FindGLEW.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Try to find GLEW library and include path. 3 | # Once done this will define 4 | # 5 | # GLEW_FOUND 6 | # GLEW_INCLUDE_PATH 7 | # GLEW_LIBRARY 8 | # 9 | 10 | IF (WIN32) 11 | FIND_PATH( GLEW_INCLUDE_PATH GL/glew.h 12 | PATHS $ENV{PROGRAMFILES}/GLEW/include 13 | ${PROJECT_SOURCE_DIR}/src/nvgl/glew/include 14 | DOC "The directory where GL/glew.h resides") 15 | FIND_LIBRARY( GLEW_LIBRARY 16 | NAMES glew GLEW glew32 glew32s 17 | PATHS 18 | $ENV{PROGRAMFILES}/GLEW/lib 19 | ${PROJECT_SOURCE_DIR}/src/nvgl/glew/bin 20 | ${PROJECT_SOURCE_DIR}/src/nvgl/glew/lib 21 | DOC "The GLEW library") 22 | ELSE (WIN32) 23 | FIND_PATH( GLEW_INCLUDE_PATH GL/glew.h 24 | /usr/include 25 | /usr/local/include 26 | /sw/include 27 | /opt/local/include 28 | DOC "The directory where GL/glew.h resides") 29 | FIND_LIBRARY( GLEW_LIBRARY 30 | NAMES GLEW glew 31 | PATHS 32 | /usr/lib64 33 | /usr/lib 34 | /usr/local/lib64 35 | /usr/local/lib 36 | /sw/lib 37 | /opt/local/lib 38 | DOC "The GLEW library") 39 | ENDIF (WIN32) 40 | 41 | IF (GLEW_INCLUDE_PATH) 42 | SET( GLEW_FOUND 1 CACHE STRING "Set to 1 if GLEW is found, 0 otherwise") 43 | ELSE (GLEW_INCLUDE_PATH) 44 | SET( GLEW_FOUND 0 CACHE STRING "Set to 1 if GLEW is found, 0 otherwise") 45 | ENDIF (GLEW_INCLUDE_PATH) 46 | 47 | MARK_AS_ADVANCED( GLEW_FOUND ) -------------------------------------------------------------------------------- /cmake/Modules/FindGLFW.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Try to find GLFW library and include path. 3 | # Once done this will define 4 | # 5 | # GLFW_FOUND 6 | # GLFW_INCLUDE_PATH 7 | # GLFW_LIBRARY 8 | # 9 | 10 | IF (WIN32) 11 | FIND_PATH( GLFW_INCLUDE_PATH GL/glfw.h 12 | $ENV{PROGRAMFILES}/GLFW/include 13 | ${PROJECT_SOURCE_DIR}/src/nvgl/glfw/include 14 | DOC "The directory where GL/glfw.h resides") 15 | FIND_LIBRARY( GLFW_LIBRARY 16 | NAMES glfw GLFW glfw32 glfw32s 17 | PATHS 18 | $ENV{PROGRAMFILES}/GLFW/lib 19 | ${PROJECT_SOURCE_DIR}/src/nvgl/glfw/bin 20 | ${PROJECT_SOURCE_DIR}/src/nvgl/glfw/lib 21 | DOC "The GLFW library") 22 | ELSE (WIN32) 23 | FIND_PATH( GLFW_INCLUDE_PATH GL/glfw.h 24 | /usr/include 25 | /usr/local/include 26 | /sw/include 27 | /opt/local/include 28 | DOC "The directory where GL/glfw.h resides") 29 | FIND_LIBRARY( GLFW_LIBRARY 30 | NAMES GLFW glfw 31 | PATHS 32 | /usr/lib64 33 | /usr/lib 34 | /usr/local/lib64 35 | /usr/local/lib 36 | /sw/lib 37 | /opt/local/lib 38 | DOC "The GLFW library") 39 | ENDIF (WIN32) 40 | 41 | IF (GLFW_INCLUDE_PATH) 42 | SET( GLFW_FOUND 1 CACHE STRING "Set to 1 if GLFW is found, 0 otherwise") 43 | ELSE (GLFW_INCLUDE_PATH) 44 | SET( GLFW_FOUND 0 CACHE STRING "Set to 1 if GLFW is found, 0 otherwise") 45 | ENDIF (GLFW_INCLUDE_PATH) 46 | 47 | MARK_AS_ADVANCED( GLFW_FOUND ) -------------------------------------------------------------------------------- /cmake/Modules/FindOpenCL.cmake: -------------------------------------------------------------------------------- 1 | # Taken from http://gitorious.org/findopencl/findopencl 6/15/2010 2 | # Says it's public domain 3 | 4 | # - Try to find OpenCL 5 | # This module tries to find an OpenCL implementation on your system. It supports 6 | # AMD / ATI, Apple and NVIDIA implementations, but shoudl work, too. 7 | # 8 | # Once done this will define 9 | # OPENCL_FOUND - system has OpenCL 10 | # OPENCL_INCLUDE_DIRS - the OpenCL include directory 11 | # OPENCL_LIBRARIES - link these to use OpenCL 12 | # 13 | 14 | find_package( PackageHandleStandardArgs ) 15 | 16 | set(OPENCL_VERSION_STRING "0.1.0") 17 | set(OPENCL_VERSION_MAJOR 0) 18 | set(OPENCL_VERSION_MINOR 1) 19 | set(OPENCL_VERSION_PATCH 0) 20 | 21 | if(APPLE) 22 | 23 | FIND_LIBRARY(OPENCL_LIBRARIES OpenCL DOC "OpenCL lib for OSX") 24 | FIND_PATH(OPENCL_INCLUDE_DIRS OpenCL/cl.h DOC "Include for OpenCL on OSX") 25 | FIND_PATH(_OPENCL_CPP_INCLUDE_DIRS OpenCL/cl.hpp DOC "Include for OpenCL CPP bindings on OSX") 26 | 27 | else(APPLE) 28 | 29 | if(WIN32) 30 | if(NVIDIA_OPENCL) 31 | # We could use CUDA_LIB_PATH, but this breaks when compiling 32 on 64 32 | if(SYSTEM_IS_64) 33 | set(OPENCL_LIB_DIR "$ENV{CUDA_PATH}/lib/x64/") 34 | else() 35 | set(OPENCL_LIB_DIR "$ENV{CUDA_PATH}/lib/Win32") 36 | endif() 37 | set(_OPENCL_INC_CAND "$ENV{CUDA_INC_PATH}") 38 | elseif(AMD_OPENCL) 39 | # The AMD SDK currently installs both x86 and x86_64 libraries 40 | if(SYSTEM_IS_64) 41 | set(OPENCL_LIB_DIR "$ENV{AMDAPPSDKROOT}/lib/x86_64") 42 | else() 43 | set(OPENCL_LIB_DIR "$ENV{AMDAPPSDKROOT}/lib/x86") 44 | endif() 45 | get_filename_component(_OPENCL_INC_CAND ${OPENCL_LIB_DIR}/../../include ABSOLUTE) 46 | endif() 47 | 48 | if(SYSTEM_IS_64) 49 | find_library(OPENCL_LIBRARIES OpenCL.lib ${OPENCL_LIB_DIR} "$ENV{CUDA_PATH}/lib/x64/" "$ENV{AMDAPPSDKROOT}/lib/x86_64") 50 | else() 51 | find_library(OPENCL_LIBRARIES OpenCL.lib ${OPENCL_LIB_DIR} "$ENV{CUDA_PATH}/lib/Win32" "$ENV{AMDAPPSDKROOT}/lib/x86") 52 | endif() 53 | 54 | # On Win32 search relative to the library 55 | # If we are forcing AMD or Nvidia OpenCL, those should be found before trying both 56 | find_path(OPENCL_INCLUDE_DIRS CL/cl.h PATHS "${_OPENCL_INC_CAND}" "$ENV{AMDAPPSDKROOT}/include" "$ENV{CUDA_INC_PATH}") 57 | find_path(_OPENCL_CPP_INCLUDE_DIRS CL/cl.hpp PATHS "${_OPENCL_INC_CAND}") 58 | else(WIN32) 59 | if(NVIDIA_OPENCL) 60 | find_library(OPENCL_LIBRARIES OpenCL 61 | /usr/local/cuda) 62 | elseif(AMD_OPENCL) 63 | find_library(OPENCL_LIBRARIES OpenCL 64 | $ENV{AMDAPPSDKROOT}/lib/x86_64 65 | $ENV{AMDAPPSDKROOT}/lib/x86) 66 | else() 67 | find_library(OPENCL_LIBRARIES OpenCL 68 | $ENV{AMDAPPSDKROOT}/lib/x86_64 69 | $ENV{AMDAPPSDKROOT}/lib/x86 70 | /usr/local/cuda) 71 | endif() 72 | get_filename_component(OPENCL_LIB_DIR ${OPENCL_LIBRARIES} PATH) 73 | get_filename_component(_OPENCL_INC_CAND ${OPENCL_LIB_DIR}/../../include ABSOLUTE) 74 | 75 | # The AMD SDK currently does not place its headers 76 | # in /usr/include, therefore also search relative 77 | # to the library 78 | find_path(OPENCL_INCLUDE_DIRS CL/cl.h 79 | PATHS 80 | ${_OPENCL_INC_CAND} 81 | $ENV{AMDAPPSDKROOT}/include 82 | /usr/local/cuda/include) 83 | find_path(_OPENCL_CPP_INCLUDE_DIRS CL/cl.hpp 84 | PATHS 85 | ${_OPENCL_INC_CAND}) 86 | endif (WIN32) 87 | 88 | endif(APPLE) 89 | 90 | find_package_handle_standard_args( OpenCL DEFAULT_MSG OPENCL_LIBRARIES OPENCL_INCLUDE_DIRS ) 91 | 92 | if( _OPENCL_CPP_INCLUDE_DIRS ) 93 | set( OPENCL_HAS_CPP_BINDINGS TRUE ) 94 | list( APPEND OPENCL_INCLUDE_DIRS ${_OPENCL_CPP_INCLUDE_DIRS} ) 95 | # This is often the same, so clean up 96 | list( REMOVE_DUPLICATES OPENCL_INCLUDE_DIRS ) 97 | endif( _OPENCL_CPP_INCLUDE_DIRS ) 98 | 99 | mark_as_advanced( 100 | OPENCL_INCLUDE_DIRS 101 | ) 102 | 103 | if(OPENCL_INCLUDE_DIRS AND OPENCL_LIBRARIES) 104 | set(OPENCL_FOUND TRUE) 105 | else() 106 | set(OPENCL_FOUND FALSE) 107 | endif() 108 | 109 | -------------------------------------------------------------------------------- /dependencies/glew-1.9.0-win32.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achsvg/MiroirFusion/4c728d1620f791493903f795536ce7a1e2af78c3/dependencies/glew-1.9.0-win32.zip -------------------------------------------------------------------------------- /dependencies/glfw-2.7.6.bin.WIN32.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achsvg/MiroirFusion/4c728d1620f791493903f795536ce7a1e2af78c3/dependencies/glfw-2.7.6.bin.WIN32.zip -------------------------------------------------------------------------------- /include/config.h.in: -------------------------------------------------------------------------------- 1 | #define KERNEL_PATH @KERNEL_PATH@ -------------------------------------------------------------------------------- /include/data_packet.h: -------------------------------------------------------------------------------- 1 | #ifndef DATA_PACKET_H 2 | #define DATA_PACKET_H 3 | 4 | #include 5 | 6 | /** \brief Data encapsulation for the modules. 7 | * \author Anthony Chansavang 8 | */ 9 | template< typename T > class DataPacket 10 | { 11 | private: 12 | boost::shared_ptr data; 13 | public: 14 | 15 | DataPacket() 16 | { 17 | } 18 | 19 | DataPacket( T* d ) 20 | { 21 | data = boost::shared_ptr(d); 22 | } 23 | 24 | DataPacket( const boost::shared_ptr& d ) : data(d) 25 | { 26 | } 27 | 28 | virtual ~DataPacket() 29 | { 30 | } 31 | 32 | boost::shared_ptr getData() 33 | { 34 | return data; 35 | } 36 | 37 | DataPacket& operator=( const DataPacket& d ) 38 | { 39 | data = d.data; 40 | return *this; 41 | } 42 | }; 43 | 44 | #endif -------------------------------------------------------------------------------- /include/definitions.h: -------------------------------------------------------------------------------- 1 | #ifndef DEFINITIONS_H 2 | #define DEFINITIONS_H 3 | 4 | // TODO : make this into a precompiled header 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "config.h" 13 | 14 | //#include "surface_measurement_module.h" 15 | 16 | #define RESX 512.0f 17 | #define RESY 512.0f 18 | #define RESZ 512.0f 19 | 20 | #define VOLUMEX 1500.0f // volume of scanned area in mm 21 | #define VOLUMEY 1500.0f 22 | #define VOLUMEZ 1500.0f 23 | 24 | #define SENSOR_ZOFFSET 400.0f // blind spot in mm 25 | 26 | #define NPIX 307200 27 | 28 | #define MU 30.0f // truncated distance in mm 29 | 30 | #define VOLUME VOLUMEX 31 | 32 | #define ROTATION_THRES 0.00314159265359f // in rad 33 | #define TRANSLATION_THRES 5 // in mm 34 | 35 | namespace pcl 36 | { 37 | struct PointXYZ; 38 | struct PointXYZRGBA; 39 | class Grabber; 40 | } 41 | 42 | namespace openni_wrapper 43 | { 44 | class DepthImage; 45 | } 46 | 47 | struct MemObj 48 | { 49 | cl_mem o; 50 | }; 51 | 52 | class SurfaceMeasurementModule; 53 | class TrackingModule; 54 | class UpdateReconstructionModule; 55 | class RaycastModule; 56 | 57 | //typedef std::tuple< cl_mem, cl_float4** > RaycastIn; // 58 | 59 | //typedef RaycastIn UROut; 60 | typedef cl_float3* VertexMap; 61 | typedef cl_float3* NormalMap; 62 | 63 | //typedef MemObj SurfaceMeasurementIn; // mem obj for the depth map 64 | //typedef SurfaceMeasurementIn URIn; 65 | //typedef std::tuple< VertexMap, NormalMap > SurfaceMeasurementOut; 66 | // 67 | //typedef std::tuple< VertexMap, VertexMap, NormalMap, NormalMap > PoseEstimationIn; 68 | //typedef int PoseEstimationOut; /* TODO : here it is a matrix */ 69 | 70 | // explicit instantiation declaration of templates 71 | // extern template MiroirModule<> ... 72 | 73 | #endif -------------------------------------------------------------------------------- /include/gpu_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef GPU_MANAGER_H 2 | #define GPU_MANAGER_H 3 | 4 | #include 5 | #include 6 | #include "../src/OpenCL/gpu_def.h" 7 | #include 8 | #if defined (__APPLE__) || defined(MACOSX) 9 | static const char* CL_GL_SHARING_EXT = "cl_APPLE_gl_sharing"; 10 | #else 11 | static const char* CL_GL_SHARING_EXT = "cl_khr_gl_sharing"; 12 | #endif 13 | 14 | /** \brief Manages the gpu programs 15 | * \author Anthony Chansavang 16 | */ 17 | class GpuManager 18 | { 19 | public: 20 | 21 | static cl_context context; 22 | static cl_command_queue queue; 23 | static cl_device_id device; 24 | static cl_int error; 25 | static cl_platform_id platforms; 26 | 27 | static cl_ulong max_alloc_size; 28 | 29 | static void initCL(bool kernel_recompile); 30 | static void cleanCL(); 31 | static char* loadProgSource(const char* filename); 32 | 33 | static GLuint createProgram(char* vert_filename, char* frag_filename, GLuint& vshader, GLuint& fshader); 34 | static cl_kernel createKernel(const char* filename, const char* kernel_name, bool fastMath=false); 35 | static cl_mem createSharedBuffer(GLsizeiptr size, const void* data, cl_mem_flags flags); 36 | static void createSharedTexture(int width, int height, cl_mem_flags flags, cl_mem& cl_tex, GLuint& gl_tex); 37 | 38 | static void __stdcall contextNotify(const char *errinfo, const void *private_info, size_t cb, void *user_data); 39 | static void __stdcall buildProgramNotify(cl_program program, void* user_data); 40 | 41 | static boost::mutex& getMutex() { return mutex; } 42 | private: 43 | static bool cl_init; 44 | static bool gl_init; 45 | 46 | static boost::mutex mutex; 47 | }; 48 | 49 | #endif -------------------------------------------------------------------------------- /include/miroir_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef MIROIR_MANAGER_H 2 | #define MIROIR_MANAGER_H 3 | 4 | //#include 5 | #include 6 | #include "definitions.h" 7 | 8 | 9 | /** \brief Manager for the whole application. 10 | * \author Anthony Chansavang 11 | */ 12 | 13 | class MiroirWindowListener; 14 | 15 | enum ShowMode { 16 | NONE, 17 | DEPTH_MAP_BIL, 18 | DEPTH_MAP_RAW, 19 | TSDF, 20 | RAYCAST, 21 | CORRESP 22 | }; 23 | 24 | class MiroirManager 25 | { 26 | private: 27 | static MiroirManager& miroir; 28 | pcl::Grabber* grabber; // frame or point cloud grabber 29 | pcl::PointCloud::ConstPtr raw_cloud; 30 | boost::shared_ptr depth_image; // current point cloud 31 | static std::vector windowListeners; 32 | 33 | // modules 34 | SurfaceMeasurementModule* surf_measurement; 35 | TrackingModule* pose_estimation; 36 | UpdateReconstructionModule* reconstruct; 37 | RaycastModule* rayc; 38 | 39 | static boost::mutex miroir_mutex; 40 | 41 | int w_width; // window's width 42 | int w_height; 43 | int sensor_width; // depth sensors' width 44 | int sensor_height; 45 | float focal_length; 46 | cl_float2 principal_point; 47 | bool isRunning; 48 | bool runTSDF; 49 | 50 | unsigned long frame; 51 | 52 | cl_mem width_d; 53 | cl_mem height_d; 54 | cl_mem fl_d; 55 | cl_mem tsdf_params_d; 56 | 57 | ShowMode mode; 58 | 59 | MiroirManager(); 60 | 61 | /** \brief set the current depth img, used as a callback for the grabber. 62 | * \param[in] depthImg depth img. 63 | */ 64 | void setCurrentDepthImage(const boost::shared_ptr& depthImg); 65 | 66 | /** \brief set the current depth img, used as a callback for the grabber. 67 | * \param[in] depthImg depth img. 68 | */ 69 | void setRawCloud(const pcl::PointCloud::ConstPtr& raw) 70 | { 71 | raw_cloud = raw; 72 | } 73 | 74 | public: 75 | 76 | ~MiroirManager(); 77 | 78 | inline static MiroirManager& getSingleton() 79 | { 80 | return miroir; 81 | } 82 | 83 | /** \brief retrieve the current point cloud. 84 | * \return current point cloud. 85 | */ 86 | //pcl::PointCloud::Ptr getCurrentCloud() 87 | //{ 88 | // boost::mutex::scoped_lock lock(miroir_mutex); 89 | // return current_cloud; 90 | //} 91 | 92 | pcl::PointCloud::ConstPtr getOutputCloud(); 93 | 94 | void step(); 95 | 96 | /** \brief run the machine. 97 | */ 98 | void run(); 99 | 100 | /** \brief assembles the different modules, using the module manager. 101 | */ 102 | void buildMiroir(); 103 | 104 | static void stop(){ MiroirManager::miroir.isRunning = false; } 105 | 106 | static pcl::Grabber* getGrabber() { return MiroirManager::getSingleton().grabber; } 107 | 108 | void setWindowDimensions(int width, int height); 109 | 110 | static void getSensorDimensions(int& w, int& h); 111 | static void getWindowDimensions(int& w, int& h); 112 | static float getFocalLength(); 113 | static cl_float2 getPrincipalPoint(); 114 | 115 | static cl_mem getCLWidth(){ return MiroirManager::getSingleton().width_d; } 116 | static cl_mem getCLHeight(){ return MiroirManager::getSingleton().height_d; } 117 | static cl_mem getCLFocalLength(){ return MiroirManager::getSingleton().fl_d; } 118 | static cl_mem getCLTSDFParams(){ return MiroirManager::getSingleton().tsdf_params_d; } 119 | 120 | static void setShowMode(ShowMode smode){ MiroirManager::getSingleton().mode = smode; } 121 | static ShowMode getShowMode(){ return MiroirManager::getSingleton().mode; } 122 | 123 | static void initGlobalBuffers(); 124 | 125 | static void addWindowListener(MiroirWindowListener* listener){ windowListeners.push_back(listener); } 126 | 127 | static cl_float4* getCameraPose(); 128 | 129 | static boost::mutex& getMutex() { return miroir_mutex; } 130 | 131 | static void setRunTSDF(bool enable); 132 | 133 | static bool isTSDFRunning(); 134 | }; 135 | 136 | #endif -------------------------------------------------------------------------------- /include/miroir_module.h: -------------------------------------------------------------------------------- 1 | #ifndef MIROIR_MODULE_H 2 | #define MIROIR_MODULE_H 3 | 4 | #include "data_packet.h" 5 | 6 | /** \brief A module gets an input, applies operations to it and produces an output. 7 | * Modules can be linked to each other by connecting their input and output provided the types match. 8 | * \author Anthony Chansavang 9 | */ 10 | class MiroirModule 11 | { 12 | public: 13 | 14 | MiroirModule() 15 | { 16 | } 17 | 18 | virtual ~MiroirModule() 19 | { 20 | } 21 | 22 | virtual void processData() = 0; 23 | }; 24 | 25 | #endif -------------------------------------------------------------------------------- /include/miroir_module_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef MIROIR_MODULE_MANAGER_H 2 | #define MIROIR_MODULE_MANAGER_H 3 | 4 | #include "miroir_module.h" 5 | 6 | /** \brief Manager for the modules. 7 | * \author Anthony Chansavang 8 | */ 9 | class MiroirModuleManager 10 | { 11 | private: 12 | static MiroirModuleManager& singleton; 13 | 14 | MiroirModuleManager(); 15 | 16 | public: 17 | virtual ~MiroirModuleManager(); 18 | 19 | static MiroirModuleManager& getSingleton() 20 | { 21 | return singleton; 22 | } 23 | 24 | /** \brief Link two modules by their output and input. 25 | * \param[in] from output module. 26 | * \param[in] to input module. 27 | */ 28 | void plug( const MiroirModule& from, const MiroirModule& to ) 29 | { 30 | // do some type checking 31 | } 32 | 33 | /** \brief feed data to the first module. 34 | * \param[in] data 35 | */ 36 | template < typename T > void 37 | feed( const DataPacket< T >& data ) 38 | { 39 | 40 | } 41 | 42 | /** \brief Manually feed data to a module. 43 | * \param[in] module 44 | * \param[in] data 45 | */ 46 | //void feed( const MiroirModule& module, const DataPacket< T >& data ) 47 | //{ 48 | // // TODO : do type checking 49 | // //module. 50 | //} 51 | 52 | /** \brief Iterate one step. 53 | */ 54 | void step(); 55 | 56 | /** \brief Start the machine. 57 | */ 58 | void start(); 59 | 60 | /** \brief Stop the machine. 61 | */ 62 | void stop(); 63 | }; 64 | 65 | #endif -------------------------------------------------------------------------------- /include/profiler.h: -------------------------------------------------------------------------------- 1 | #ifndef MIROIR_PROFILER 2 | #define MIROIR_PROFILER 3 | 4 | #include 5 | 6 | namespace boost 7 | { 8 | namespace timer{ class cpu_timer; } 9 | } 10 | class MiroirProfiler 11 | { 12 | public: 13 | //MiroirProfiler& getSingleton(); 14 | static void enable(){enabled = true;} 15 | static void disable(){enabled = false;} 16 | static void start( const std::string& name ); 17 | static void stop( const std::string& name ); 18 | private: 19 | //static MiroirProfiler& profiler; 20 | static std::map start_time; 21 | static bool enabled; 22 | 23 | MiroirProfiler(); 24 | }; 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /include/raycast_module.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYCAST_MODULE_H 2 | #define RAYCAST_MODULE_H 3 | 4 | #include "definitions.h" 5 | #include "miroir_module.h" 6 | #include "window_listener.h" 7 | 8 | typedef int RaycastOut; 9 | 10 | class RaycastModule : public MiroirModule, public MiroirWindowListener 11 | { 12 | private: 13 | boost::mutex raycast_mutex; 14 | 15 | cl_kernel raycast_kernel; 16 | 17 | cl_float3* vmap_h; 18 | cl_float3* nmap_h; 19 | 20 | cl_mem vmap_d; 21 | cl_mem nmap_d; 22 | cl_mem tex_d; 23 | unsigned int gl_tex; 24 | 25 | int tex_dim[2]; 26 | 27 | pcl::PointCloud< pcl::PointXYZRGBA >::Ptr cloud; 28 | 29 | // to be set by tsdf module 30 | cl_mem tsdf; 31 | cl_float4 cam_pose[4]; 32 | //cl_float* tsdf_h; 33 | 34 | public: 35 | RaycastModule(); 36 | ~RaycastModule(); 37 | 38 | void raycast(cl_mem TSDF, cl_float4 cam_pos[4]); 39 | 40 | void processData(); 41 | 42 | pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getCloud() const { return cloud; } 43 | 44 | void onWindowResized(int width, int height); 45 | 46 | void setTSDF(const cl_mem& buf) 47 | { 48 | tsdf = buf; 49 | } 50 | 51 | void setCamPose(const cl_float4 pose[4]) 52 | { 53 | cam_pose[0] = pose[0]; 54 | cam_pose[1] = pose[1]; 55 | cam_pose[2] = pose[2]; 56 | cam_pose[3] = pose[3]; 57 | } 58 | 59 | cl_mem getVertexMap() const 60 | { 61 | return vmap_d; 62 | } 63 | 64 | cl_mem getNormalMap() const 65 | { 66 | return nmap_d; 67 | } 68 | }; 69 | 70 | #endif -------------------------------------------------------------------------------- /include/surface_measurement_module.h: -------------------------------------------------------------------------------- 1 | #ifndef SURFACE_MEASUREMENT_MODULE_H 2 | #define SURFACE_MEASUREMENT_MODULE_H 3 | 4 | #include "definitions.h" 5 | 6 | #include "miroir_module.h" 7 | 8 | #include 9 | #include 10 | 11 | class SurfaceMeasurementModule : public MiroirModule 12 | { 13 | private: 14 | unsigned short* dmap_h; 15 | cl_float3* vmap_h; 16 | cl_float3* nmap_h; 17 | 18 | cl_mem vmap_d; 19 | cl_mem nmap_d; 20 | 21 | pcl::PointCloud< pcl::PointXYZRGBA >::Ptr cloud_filtered; 22 | 23 | cl_kernel measurement_vertices; 24 | cl_kernel measurement_normals; 25 | 26 | void bilateralFilter( cl_mem raw_dmap ); 27 | 28 | cl_mem sigma_s_d; 29 | cl_mem sigma_r_d; 30 | 31 | // to be set 32 | cl_mem dmap; 33 | public: 34 | 35 | SurfaceMeasurementModule( int w, int h, float focal_length ); 36 | 37 | ~SurfaceMeasurementModule() 38 | { 39 | clReleaseKernel(measurement_vertices); 40 | clReleaseKernel(measurement_normals); 41 | clReleaseMemObject(sigma_s_d); 42 | clReleaseMemObject(sigma_r_d); 43 | clReleaseMemObject(vmap_d); 44 | clReleaseMemObject(nmap_d); 45 | delete [] dmap_h; 46 | delete [] vmap_h; 47 | delete [] nmap_h; 48 | } 49 | 50 | void processData(); 51 | 52 | pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr getFilteredCloud() 53 | { 54 | return cloud_filtered; 55 | } 56 | 57 | inline void setDepthMap( const cl_mem& depth_map ) 58 | { 59 | dmap = depth_map; 60 | } 61 | 62 | inline cl_mem getVertexMap() const 63 | { 64 | return vmap_d; 65 | } 66 | 67 | inline cl_mem getNormalMap() const 68 | { 69 | return nmap_d; 70 | } 71 | }; 72 | 73 | //typedef std::shared_ptr< SurfaceMeasurementModule > SurfaceMeasurementModulePtr; 74 | 75 | #endif -------------------------------------------------------------------------------- /include/tracking_module.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSOR_POSE_MODULE_H 2 | #define SENSOR_POSE_MODULE_H 3 | 4 | #include "definitions.h" 5 | #include "miroir_module.h" 6 | 7 | class TrackingModule : public MiroirModule 8 | { 9 | private: 10 | cl_mem vmap_raycast; 11 | cl_mem vmap_sensor; 12 | cl_mem nmap_raycast; 13 | cl_mem nmap_sensor; 14 | cl_mem corresp_d; // buffer for found correspondences 15 | cl_float4 cam_pose[4]; 16 | 17 | cl_float2* corresp_h; 18 | cl_uint corresp_count; 19 | 20 | cl_kernel find_corresp; 21 | cl_kernel compute_sum; 22 | cl_kernel compute_block_sum; 23 | 24 | pcl::PointCloud< pcl::PointXYZRGBA >::Ptr cloud; 25 | 26 | void findCorrespondences(const cl_float4 cam_pose_inv[4]); 27 | void sumLinearSystem(cl_float A_h[36], cl_float B_h[6], const cl_float4 cam_est_pose[4]); 28 | public: 29 | TrackingModule(); 30 | 31 | void processData(); 32 | 33 | inline void setParams(const cl_mem& vraycast, const cl_mem& vsensor, const cl_mem& nraycast, const cl_mem& nsensor) 34 | { 35 | vmap_raycast = vraycast; 36 | vmap_sensor = vsensor; 37 | nmap_raycast = nraycast; 38 | nmap_sensor = nsensor; 39 | } 40 | 41 | inline cl_float4* getCamPose() const 42 | { 43 | return (cl_float4*)cam_pose; 44 | } 45 | 46 | pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getCloud() const { return cloud; } 47 | }; 48 | 49 | //typedef std::shared_ptr< TrackingModule > TrackingModulePtr; 50 | 51 | #endif -------------------------------------------------------------------------------- /include/update_reconstruction_module.h: -------------------------------------------------------------------------------- 1 | #ifndef UPDATE_RECONSTRUCTION_MODULE_H 2 | #define UPDATE_RECONSTRUCTION_MODULE_H 3 | 4 | #include "definitions.h" 5 | #include "miroir_module.h" 6 | 7 | namespace openni_wrapper 8 | { 9 | class DepthImage; 10 | } 11 | 12 | 13 | 14 | class UpdateReconstructionModule : public MiroirModule 15 | { 16 | private: 17 | cl_kernel kernel; 18 | cl_ulong current_frame; 19 | cl_mem TSDF_d; 20 | cl_float4* cam_pose; 21 | pcl::PointCloud< pcl::PointXYZRGBA >::Ptr cloud; 22 | 23 | cl_mem dmap; 24 | 25 | bool enable; 26 | 27 | public: 28 | 29 | UpdateReconstructionModule(); 30 | ~UpdateReconstructionModule(); 31 | 32 | void TSDF(); 33 | 34 | pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getCloud() const { return cloud; } 35 | 36 | void processData(); 37 | 38 | inline void setCamPose( const cl_float4 pose[4] ) 39 | { 40 | cam_pose[0] = pose[0]; 41 | cam_pose[1] = pose[1]; 42 | cam_pose[2] = pose[2]; 43 | cam_pose[3] = pose[3]; 44 | } 45 | 46 | inline void setDepthMap( const cl_mem& depth_map ) 47 | { 48 | dmap = depth_map; 49 | } 50 | 51 | inline cl_mem getTSDF() const 52 | { 53 | return TSDF_d; 54 | } 55 | 56 | inline cl_float4* getCamPose() const 57 | { 58 | return cam_pose; 59 | } 60 | 61 | inline void setEnable(bool b) 62 | { 63 | enable = b; 64 | } 65 | 66 | inline bool isEnabled() const 67 | { 68 | return enable; 69 | } 70 | }; 71 | 72 | #endif -------------------------------------------------------------------------------- /include/util.h: -------------------------------------------------------------------------------- 1 | #ifndef MIROIR_UTIL_H 2 | #define MIROIR_UTIL_H 3 | 4 | #include 5 | 6 | namespace MiroirUtil 7 | { 8 | inline float angleFromRotMatrix(const cl_float3 rot[3]) 9 | { 10 | return std::atan( std::sqrt((rot[2].s[1] - rot[1].s[2])*(rot[2].s[1] - rot[1].s[2]) + (rot[0].s[2] - rot[2].s[0])*(rot[0].s[2] - rot[2].s[0]) + (rot[1].s[0] - rot[0].s[1])*(rot[1].s[0] - rot[0].s[1])) 11 | / (rot[0].s[0] + rot[1].s[1] + rot[2].s[2] - 1.0f) ); 12 | } 13 | 14 | inline void invertCamTransform( const cl_float4 cam_pose[4], cl_float4 cam_pose_inv[4] ) 15 | { 16 | // transpose rotation matrix 17 | cam_pose_inv[0].s[0] = cam_pose[0].s[0]; 18 | cam_pose_inv[0].s[1] = cam_pose[1].s[0]; 19 | cam_pose_inv[0].s[2] = cam_pose[2].s[0]; 20 | cam_pose_inv[0].s[3] = 0.0f; 21 | 22 | cam_pose_inv[1].s[0] = cam_pose[0].s[1]; 23 | cam_pose_inv[1].s[1] = cam_pose[1].s[1]; 24 | cam_pose_inv[1].s[2] = cam_pose[2].s[1]; 25 | cam_pose_inv[1].s[3] = 0.0f; 26 | 27 | cam_pose_inv[2].s[0] = cam_pose[0].s[2]; 28 | cam_pose_inv[2].s[1] = cam_pose[1].s[2]; 29 | cam_pose_inv[2].s[2] = cam_pose[0].s[0]; 30 | cam_pose_inv[2].s[3] = 0.0f; 31 | 32 | // negate translation vector and multiply by rotation inverse 33 | /*cam_pose_inv[3].s[0] = -(cam_pose[3].s[0] * cam_pose_inv[0].s[0] + cam_pose[3].s[1] * cam_pose_inv[1].s[0] + cam_pose[3].s[2] * cam_pose_inv[2].s[0]); 34 | cam_pose_inv[3].s[1] = -(cam_pose[3].s[0] * cam_pose_inv[0].s[1] + cam_pose[3].s[1] * cam_pose_inv[1].s[1] + cam_pose[3].s[2] * cam_pose_inv[2].s[1]); 35 | cam_pose_inv[3].s[2] = -(cam_pose[3].s[0] * cam_pose_inv[0].s[2] + cam_pose[3].s[1] * cam_pose_inv[1].s[2] + cam_pose[3].s[2] * cam_pose_inv[2].s[2]); 36 | cam_pose_inv[3].s[3] = 1.0f;*/ 37 | cam_pose_inv[3].s[0] = -(cam_pose[3].s[0]); 38 | cam_pose_inv[3].s[1] = -(cam_pose[3].s[1]); 39 | cam_pose_inv[3].s[2] = -(cam_pose[3].s[2]); 40 | cam_pose_inv[3].s[3] = 1.0f; 41 | } 42 | 43 | inline void mat3Mult(const cl_float3 A[3], const cl_float3 B[3], cl_float3 res[3]) 44 | { 45 | res[0].s[0] = A[0].s[0] * B[0].s[0] + A[1].s[0] * B[0].s[1] + A[2].s[0] * B[0].s[2]; 46 | res[0].s[1] = A[0].s[1] * B[0].s[0] + A[1].s[1] * B[0].s[1] + A[2].s[1] * B[0].s[2]; 47 | res[0].s[2] = A[0].s[2] * B[0].s[0] + A[1].s[2] * B[0].s[1] + A[2].s[2] * B[0].s[2]; 48 | 49 | res[1].s[0] = A[0].s[0] * B[1].s[0] + A[1].s[0] * B[1].s[1] + A[2].s[0] * B[1].s[2]; 50 | res[1].s[1] = A[0].s[1] * B[1].s[0] + A[1].s[1] * B[1].s[1] + A[2].s[1] * B[1].s[2]; 51 | res[1].s[2] = A[0].s[2] * B[1].s[0] + A[1].s[2] * B[1].s[1] + A[2].s[2] * B[1].s[2]; 52 | 53 | res[2].s[0] = A[0].s[0] * B[2].s[0] + A[1].s[0] * B[2].s[1] + A[2].s[0] * B[2].s[2]; 54 | res[2].s[1] = A[0].s[1] * B[2].s[0] + A[1].s[1] * B[2].s[1] + A[2].s[1] * B[2].s[2]; 55 | res[2].s[2] = A[0].s[2] * B[2].s[0] + A[1].s[2] * B[2].s[1] + A[2].s[2] * B[2].s[2]; 56 | } 57 | 58 | inline void mat4Mult( const cl_float4 A[4], const cl_float4 B[4], cl_float4 res[4]) 59 | { 60 | res[0].s[0] = A[0].s[0] * B[0].s[0] + A[1].s[0] * B[0].s[1] + A[2].s[0] * B[0].s[2] + A[3].s[0] * B[0].s[3]; 61 | res[0].s[1] = A[0].s[1] * B[0].s[0] + A[1].s[1] * B[0].s[1] + A[2].s[1] * B[0].s[2] + A[3].s[1] * B[0].s[3]; 62 | res[0].s[2] = A[0].s[2] * B[0].s[0] + A[1].s[2] * B[0].s[1] + A[2].s[2] * B[0].s[2] + A[3].s[2] * B[0].s[3];; 63 | res[0].s[3] = A[0].s[3] * B[0].s[0] + A[1].s[3] * B[0].s[1] + A[2].s[3] * B[0].s[2] + A[3].s[3] * B[0].s[3]; 64 | 65 | res[1].s[0] = A[0].s[0] * B[1].s[0] + A[1].s[0] * B[1].s[1] + A[2].s[0] * B[1].s[2] + A[3].s[0] * B[1].s[3]; 66 | res[1].s[1] = A[0].s[1] * B[1].s[0] + A[1].s[1] * B[1].s[1] + A[2].s[1] * B[1].s[2] + A[3].s[1] * B[1].s[3]; 67 | res[1].s[2] = A[0].s[2] * B[1].s[0] + A[1].s[2] * B[1].s[1] + A[2].s[2] * B[1].s[2] + A[3].s[2] * B[1].s[3]; 68 | res[1].s[3] = A[0].s[3] * B[1].s[0] + A[1].s[3] * B[1].s[1] + A[2].s[3] * B[1].s[2] + A[3].s[3] * B[1].s[3]; 69 | 70 | res[2].s[0] = A[0].s[0] * B[2].s[0] + A[1].s[0] * B[2].s[1] + A[2].s[0] * B[2].s[2] + A[3].s[0] * B[0].s[3]; 71 | res[2].s[1] = A[0].s[1] * B[2].s[0] + A[1].s[1] * B[2].s[1] + A[2].s[1] * B[2].s[2] + A[3].s[1] * B[0].s[3]; 72 | res[2].s[2] = A[0].s[2] * B[2].s[0] + A[1].s[2] * B[2].s[1] + A[2].s[2] * B[2].s[2] + A[3].s[2] * B[0].s[3]; 73 | res[2].s[3] = A[0].s[3] * B[2].s[0] + A[1].s[3] * B[2].s[1] + A[2].s[3] * B[2].s[2] + A[3].s[3] * B[0].s[3]; 74 | 75 | res[3].s[0] = A[0].s[0] * B[3].s[0] + A[1].s[0] * B[3].s[1] + A[2].s[0] * B[3].s[2] + A[3].s[0] * B[3].s[3]; 76 | res[3].s[1] = A[0].s[1] * B[3].s[0] + A[1].s[1] * B[3].s[1] + A[2].s[1] * B[3].s[2] + A[3].s[1] * B[3].s[3]; 77 | res[3].s[2] = A[0].s[2] * B[3].s[0] + A[1].s[2] * B[3].s[1] + A[2].s[2] * B[3].s[2] + A[3].s[2] * B[3].s[3]; 78 | res[3].s[3] = A[0].s[3] * B[3].s[0] + A[1].s[3] * B[3].s[1] + A[2].s[3] * B[3].s[2] + A[3].s[3] * B[3].s[3]; 79 | } 80 | 81 | //Cholesky decomposition of matrix A 82 | inline void cholesky(int n, cl_float* A, cl_float* decomp) 83 | { 84 | /*double sum1 = 0.0; 85 | double sum2 = 0.0; 86 | double sum3 = 0.0; 87 | 88 | decomp[0] = sqrt(A[0]); 89 | for (int j = 1; j <= n-1; j++) 90 | decomp[j] = A[j]/decomp[0]; 91 | 92 | for (int i = 1; i <= (n-2); i++) 93 | { 94 | for (int k = 0; k <= (i-1); k++) 95 | sum1 += pow(decomp[i * n + k], 2); 96 | decomp[i * n + i]= sqrt(A[i * n + i]-sum1); 97 | for (int j = (i+1); j <= (n-1); j++) 98 | { 99 | for (int k = 0; k <= (i-1); k++) 100 | sum2 += decomp[j * n + k]*decomp[i * n + k]; 101 | decomp[j * n + i]= (A[j * n + i]-sum2)/decomp[i * n + i]; 102 | } 103 | } 104 | for (int k = 0; k <= (n-2); k++) 105 | sum3 += pow(decomp[(n-1)* n +k], 2); 106 | decomp[(n-1)* n + n-1] = sqrt(A[(n-1)* n + n-1]-sum3);*/ 107 | 108 | for (int i = 0; i < n; i++) 109 | for (int j = 0; j < (i+1); j++) { 110 | double s = 0; 111 | for (int k = 0; k < j; k++) 112 | s += decomp[i * n + k] * decomp[j * n + k]; 113 | decomp[i * n + j] = (i == j) ? sqrt(A[i * n + i] - s) : (1.0 / decomp[j * n + j] * (A[i * n + j] - s)); 114 | } 115 | } 116 | 117 | inline double determinant(cl_float* A, int n) 118 | { 119 | /* const int dim is the dimensionality of the matrix */ 120 | double det = 0.0; 121 | for (int i = 0; i < n; i++) 122 | { 123 | double a = 1.0, b = 1.0; 124 | for (int row = 0; row < n; row++) 125 | { 126 | a *= A[row*n+((i+row)%n)]; 127 | b *= A[row*n+((n-1) - (i+row)%n)]; 128 | } 129 | det += a - b; 130 | } 131 | return det; 132 | } 133 | } 134 | 135 | #endif -------------------------------------------------------------------------------- /include/window_listener.h: -------------------------------------------------------------------------------- 1 | #ifndef MIROIR_WINDOW_LISTENER 2 | #define MIROIR_WINDOW_LISTENER 3 | 4 | #include "miroir_manager.h" 5 | 6 | class MiroirWindowListener 7 | { 8 | public: 9 | MiroirWindowListener(){ MiroirManager::addWindowListener(this); }; 10 | virtual void onWindowResized(int width, int height) = 0; 11 | }; 12 | 13 | #endif -------------------------------------------------------------------------------- /src/OpenCL/TSDF.cl: -------------------------------------------------------------------------------- 1 | #define _GPU_ 2 | 3 | #include "gpu_def.h" 4 | 5 | __kernel void TSDF( __global short2* F, 6 | __global const int* width, 7 | __global const int* height, 8 | //__global float4 cam_pos[4], 9 | __global float4* cam_pos_inv, 10 | __global ushort* dmap, 11 | __global ulong* k, // current frame 12 | __global const float* fl, // focal length 13 | __constant struct TsdfParams* params 14 | ) 15 | { 16 | // go through all the depth value and update their associated voxel TSDF 17 | const int voxel_x = get_global_id(0); 18 | const int voxel_y = get_global_id(1); 19 | float focal_length = *fl; 20 | //ulong frame = *k; 21 | 22 | float4 cam_pos_inv_l[4] = { cam_pos_inv[0], cam_pos_inv[1], cam_pos_inv[2], cam_pos_inv[3] }; 23 | int w = *width; 24 | int h = *height; 25 | 26 | struct TsdfParams l_params = *params; 27 | 28 | int3 resolution = {convert_int(l_params.resolution[0]), convert_int(l_params.resolution[1]), convert_int(l_params.resolution[2])}; 29 | float mu = l_params.mu; 30 | 31 | float3 cellsize = { l_params.volume[0] / l_params.resolution[0], l_params.volume[1] / l_params.resolution[1], l_params.volume[2] / l_params.resolution[2] }; 32 | 33 | int xyIdx = voxel_x*resolution.z + voxel_y*resolution.x*resolution.z; 34 | 35 | // OPTIMIZATION TRICK: get first voxel to precompute some of the matrix-vector product since all these voxels share the same x & y coordinates 36 | float4 voxel0 = getVoxelGlobal(convert_float(voxel_x), convert_float(voxel_y), 0.0f, cellsize); 37 | voxel0.x -= l_params.volume[0] / 2.0f; 38 | voxel0.y -= l_params.volume[1] / 2.0f; 39 | voxel0.w = 0.0f; 40 | //float4 baseVoxel = mul(cam_pos_inv_l, voxel0); // current cam space 41 | float4 baseVoxel = voxel0 + cam_pos_inv_l[3]; 42 | baseVoxel.z = 0.0f; // will contribute later in the loop 43 | baseVoxel.w = 0.0f; 44 | baseVoxel = mul(cam_pos_inv_l, baseVoxel); 45 | 46 | for(int voxel_z = 0; voxel_z < resolution.z; voxel_z++) 47 | { 48 | float voxel_z_g = (convert_float(voxel_z) + 0.5f) * cellsize.z; 49 | 50 | //float4 voxel_c = mul(cam_pos_inv_l,voxel_g); // voxel in camera space, bottleneck 51 | float4 voxel_c = { baseVoxel.x + cam_pos_inv_l[2].x * (voxel_z_g + cam_pos_inv_l[3].z), 52 | baseVoxel.y + cam_pos_inv_l[2].y * (voxel_z_g + cam_pos_inv_l[3].z), 53 | baseVoxel.z + cam_pos_inv_l[2].z * (voxel_z_g + cam_pos_inv_l[3].z), 54 | baseVoxel.w + cam_pos_inv_l[2].w * (voxel_z_g + cam_pos_inv_l[3].z) 55 | }; // optimization trick 56 | 57 | // if(voxel_c.z <= 0.0f) // behind camera 58 | // break; 59 | 60 | int2 pix = { convert_int((voxel_c.x / voxel_c.z) * focal_length + convert_float(w)/2.0f), convert_int((voxel_c.y / voxel_c.z) * focal_length + convert_float(h)/2.0f)}; 61 | int idx = voxel_z + xyIdx; 62 | 63 | if( pix.x >= 0 && pix.y >= 0 && pix.x < w && pix.y < h && idx >= 0 && idx < resolution.x * resolution.y * resolution.z ) 64 | { 65 | ushort depth = dmap[pix.y * w + pix.x]; 66 | 67 | if(depth != 0) 68 | { 69 | float x = (convert_float(pix.x) - convert_float(w)/2.0f)/focal_length; 70 | float y = (convert_float(pix.y) - convert_float(h)/2.0f)/focal_length; 71 | float lambda_inv = sqrt( x*x + y*y + 1.0f ); 72 | 73 | float3 voxel3_c = {voxel_c.x, voxel_c.y, voxel_c.z}; 74 | 75 | float dist = sqrt(voxel3_c.x * voxel3_c.x + voxel3_c.y * voxel3_c.y + voxel3_c.z * voxel3_c.z); // distance(t, voxel3_g) is slow? 76 | float arg = convert_float(depth) - dist / lambda_inv; 77 | 78 | if(arg >= -mu) 79 | { 80 | float F_Rk = arg/mu; 81 | F_Rk = max( -1.0f, min( 1.0f, F_Rk ) ); 82 | 83 | float2 cur_val = unpack_tsdf(F[idx]); // bottleneck 84 | float weight = cur_val.y; 85 | if(weight < convert_float(SHORT_MAX * 2)) // weight limit 86 | { 87 | float val = ((weight * cur_val.x) + F_Rk) / (weight+1.0f); 88 | pack_tsdf(max( -1.0f, min( 1.0f, val ) ), weight+1.0f, F, idx); // bottleneck 89 | } 90 | } 91 | } 92 | } 93 | } 94 | } -------------------------------------------------------------------------------- /src/OpenCL/bilateral.cl: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * This kernel performs bilateral filtering of the depth map and outputs the vertex map and normal map 4 | */ 5 | 6 | __kernel void measurement_vertices( __global const ushort* src, 7 | __global const int* width, 8 | __global const int* height, 9 | __global const float* fl, // focal length 10 | //__global ushort* dst, 11 | __global float3* vmap, 12 | __global const float* sigma_s, 13 | __global const float* sigma_r 14 | ) 15 | { 16 | /* get_global_id(0) returns the ID of the thread in execution. 17 | As many threads are launched at the same time, executing the same kernel, 18 | each one will receive a different ID, and consequently perform a different computation.*/ 19 | 20 | const int x = get_global_id(0); 21 | const int y = get_global_id(1); 22 | const float s = *sigma_s; 23 | const float r = *sigma_r; 24 | 25 | const int radius = 8; 26 | int w = *width; // buffer into shared memory? 27 | int h = *height; 28 | float focal_length = *fl; 29 | 30 | int tlx = max(x-radius, 0); 31 | int tly = max(y-radius, 0); 32 | int brx = min(x+radius, w); 33 | int bry = min(y+radius, h); 34 | 35 | int idx = w * y + x; 36 | //depth_l[idx] = src[idx]; 37 | float sum = 0; 38 | float wp = 0; // normalizing constant 39 | 40 | //barrier(CLK_LOCAL_MEM_FENCE); 41 | float src_depth = src[idx]; 42 | 43 | float s2 = s*s; 44 | float r2 = r*r; 45 | 46 | if(src_depth != 0) 47 | { 48 | for(int i=tlx; i< brx; i++) 49 | { 50 | for(int j=tly; j> NUM_BANKS + (n) >> (2 * LOG_NUM_BANKS)) 21 | 22 | // m is column wise, i.e m[0] is column 0, m[1] column 1, etc. 23 | bool _isnan3(float3 v) 24 | { 25 | return isnan(v.x) || isnan(v.y) || isnan(v.z); 26 | } 27 | 28 | bool _isnan4(float4 v) 29 | { 30 | return isnan(v.x) || isnan(v.y) || isnan(v.z) || isnan(v.w); 31 | } 32 | 33 | bool _isnan2(float2 v) 34 | { 35 | return isnan(v.x) || isnan(v.y); 36 | } 37 | 38 | // short between -32767 and 32767 39 | void pack_tsdf(float val, float weight, __global short2* tsdf, int idx) 40 | { 41 | //tsdf[idx].x = convert_short(val * convert_float(SHORT_MAX)); 42 | //tsdf[idx].y = convert_short(weight)-SHORT_MAX; 43 | short2 res = {convert_short(val * convert_float(SHORT_MAX)), convert_short(weight)-SHORT_MAX}; 44 | tsdf[idx] = res; 45 | } 46 | 47 | float2 unpack_tsdf(short2 tsdf) 48 | { 49 | float2 res = {convert_float(tsdf.x) / convert_float(SHORT_MAX), convert_float(tsdf.y)+convert_float(SHORT_MAX)}; 50 | return res; 51 | } 52 | 53 | float4 mul( float4 m[4], float4 v ) 54 | { 55 | float4 res = { m[0].x*v.x + m[1].x*v.y + m[2].x*v.z + m[3].x*v.w, 56 | m[0].y*v.x + m[1].y*v.y + m[2].y*v.z + m[3].y*v.w, 57 | m[0].z*v.x + m[1].z*v.y + m[2].z*v.z + m[3].z*v.w, 58 | m[0].w*v.x + m[1].w*v.y + m[2].w*v.z + m[3].w*v.w }; 59 | return res; 60 | } 61 | 62 | float3 mul_homogenize( float4 m[4], float3 v ) 63 | { 64 | float3 res = { m[0].x*v.x + m[1].x*v.y + m[2].x*v.z + m[3].x*1.0f, 65 | m[0].y*v.x + m[1].y*v.y + m[2].y*v.z + m[3].y*1.0f, 66 | m[0].z*v.x + m[1].z*v.y + m[2].z*v.z + m[3].z*1.0f 67 | }; 68 | return res; 69 | } 70 | 71 | float4 getVoxelGlobal( float voxel_x, float voxel_y, float voxel_z, float3 cellsize ) 72 | { 73 | float4 voxel = {(voxel_x + 0.5f) * cellsize.x, (voxel_y + 0.5f) * cellsize.y, (voxel_z + 0.5f) * cellsize.z, 1.0f }; 74 | return voxel; 75 | } 76 | 77 | bool isNull(float3 v) 78 | { 79 | return v.x == 0 && v.y == 0 && v.z == 0; 80 | } 81 | 82 | // bool isNull(float4 v) 83 | // { 84 | // return v.x == 0 && v.y == 0 && v.z == 0 && v.z == 0; 85 | // } 86 | 87 | #endif 88 | #endif -------------------------------------------------------------------------------- /src/OpenCL/raycast.cl: -------------------------------------------------------------------------------- 1 | #define _GPU_ 2 | 3 | #include "gpu_def.h" 4 | 5 | int3 getVoxel(float4 p, struct TsdfParams params) 6 | { 7 | int3 res = { convert_int(p.x*params.resolution[0]/params.volume[0] + params.resolution[0]/2.0f), 8 | convert_int(p.y*params.resolution[1]/params.volume[1] + params.resolution[1]/2.0f), 9 | convert_int(p.z*params.resolution[2]/params.volume[2]) }; 10 | // int3 res = { ((p.x)*params.resolution[0]/params.volume[0] + params.resolution[0]/2.0f), 11 | // ((p.y)*params.resolution[1]/params.volume[1] + params.resolution[1]/2.0f), 12 | // (p.z*params.resolution[2]/params.volume[2]) }; 13 | return res; 14 | } 15 | 16 | float3 reflect(float3 V, float3 N){ 17 | return V - 2.0f * dot( V, N ) * N; 18 | } 19 | 20 | float getTSDF( 21 | __global short2* F, 22 | int3 resolution, 23 | int3 voxel, 24 | int x_offset, 25 | int y_offset, 26 | int z_offset 27 | ) 28 | { 29 | int ox = (voxel.x + x_offset); 30 | int oy = (voxel.y + y_offset); 31 | int oz = (voxel.z + z_offset); 32 | 33 | 34 | //int3 resolution = { (l_params.resolution[0]), (l_params.resolution[1]), (l_params.resolution[2]) }; 35 | 36 | int idx = ox*resolution.z + oy*resolution.x*resolution.z + oz; 37 | if( ox < 0 || ox >= resolution.x || 38 | oy < 0 || oy >= resolution.y || 39 | oz < 0 || oz >= resolution.z ) 40 | return NAN; 41 | 42 | return unpack_tsdf(F[idx]).x; 43 | } 44 | 45 | float interpolateTrilinearly( 46 | __global short2* F, 47 | float4 point, 48 | int3 voxel, 49 | float3 cellsize, 50 | struct TsdfParams params 51 | ) 52 | { 53 | int3 resolution = { convert_int(params.resolution[0]), convert_int(params.resolution[1]), convert_int(params.resolution[2]) }; 54 | 55 | if (voxel.x <= 0 || voxel.x >= resolution.x - 1) 56 | return NAN; 57 | 58 | if (voxel.y <= 0 || voxel.y >= resolution.y - 1) 59 | return NAN; 60 | 61 | if (voxel.z <= 0 || voxel.z >= resolution.z - 1) 62 | return NAN; 63 | 64 | float vx = ((float)voxel.x + 0.5f) * cellsize.x - params.volume[0]/2.0f; 65 | float vy = ((float)voxel.y + 0.5f) * cellsize.y - params.volume[1]/2.0f; 66 | float vz = ((float)voxel.z + 0.5f) * cellsize.z; 67 | 68 | voxel.x = (point.x < vx) ? (voxel.x - 1) : voxel.x; 69 | voxel.y = (point.y < vy) ? (voxel.y - 1) : voxel.y; 70 | voxel.z = (point.z < vz) ? (voxel.z - 1) : voxel.z; 71 | 72 | float a = (point.x - ((convert_float(voxel.x) + 0.5f) * cellsize.x - params.volume[0]/2.0f)) / cellsize.x; 73 | float b = (point.y - ((convert_float(voxel.y) + 0.5f) * cellsize.y - params.volume[1]/2.0f)) / cellsize.y; 74 | float c = (point.z - (convert_float(voxel.z) + 0.5f) * cellsize.z) / cellsize.z; 75 | 76 | float V000 = getTSDF(F, resolution, voxel, 0, 0, 0); 77 | float V001 = getTSDF(F, resolution, voxel, 0, 0, 1); 78 | float V010 = getTSDF(F, resolution, voxel, 0, 1, 0); 79 | float V011 = getTSDF(F, resolution, voxel, 0, 1, 1); 80 | float V100 = getTSDF(F, resolution, voxel, 1, 0, 0); 81 | float V101 = getTSDF(F, resolution, voxel, 1, 0, 1); 82 | float V110 = getTSDF(F, resolution, voxel, 1, 1, 0); 83 | float V111 = getTSDF(F, resolution, voxel, 1, 1, 1); 84 | 85 | if(isnan(V000) || isnan(V001) || isnan(V010) || isnan(V011) || isnan(V100) || isnan(V101) || isnan(V110) || isnan(V111)) 86 | return NAN; 87 | 88 | float res = V000 * (1.0f - a) * (1.0f - b) * (1.0f - c) + 89 | V001 * (1.0f - a) * (1.0f - b) * c + 90 | V010 * (1.0f - a) * b * (1.0f - c) + 91 | V011 * (1.0f - a) * b * c + 92 | V100 * a * (1.0f - b) * (1.0f - c) + 93 | V101 * a * (1.0f- b) * c + 94 | V110 * a * b * (1.0f - c) + 95 | V111 * a * b * c; 96 | return res; 97 | } 98 | 99 | __kernel void raycast( 100 | __global short2* F, 101 | __global const int* width, 102 | __global const int* height, 103 | __global const float4* cam_pos, 104 | __global const float* fl, // focal length 105 | __global const float2* ppt, // principal point 106 | __global float3* vmap, 107 | __global float3* nmap, 108 | #ifdef MIROIR_GL_INTEROP 109 | __write_only image2d_t frameBuffer, 110 | #endif 111 | __constant struct TsdfParams* params 112 | ) 113 | { 114 | // cast a ray for each pixel (u,v) 115 | const int u = get_global_id(0); 116 | const int v = get_global_id(1); 117 | 118 | int w = *width; 119 | int h = *height; 120 | 121 | float4 cam_pos_l[4] = { cam_pos[0], cam_pos[1], cam_pos[2], cam_pos[3] }; 122 | 123 | float depth_march = 0.0f; 124 | float focal_length = *fl; 125 | 126 | // TODO : start ray where the actual volume starts! (ie intersect ray with volume) 127 | float4 ray_start = {convert_float(u-w/2.0f)/focal_length, convert_float(v-h/2.0f)/focal_length,1.0f,1.0f}; // in camera space 128 | ray_start = mul(cam_pos_l, ray_start); // in global space 129 | float4 vertex_4 = ray_start; 130 | 131 | float4 ray_dir = normalize(vertex_4 - cam_pos_l[3]); 132 | 133 | //ensure that it isn't a degenerate case 134 | // ray_dir.x = (ray_dir.x == 0.f) ? 1e-15 : ray_dir.x; 135 | // ray_dir.y = (ray_dir.y == 0.f) ? 1e-15 : ray_dir.y; 136 | // ray_dir.z = (ray_dir.z == 0.f) ? 1e-15 : ray_dir.z; 137 | 138 | struct TsdfParams l_params = *params; 139 | 140 | int pix_idx = w*v+u; 141 | 142 | // kinect sensor range is ~[0.4,8] 143 | float step = l_params.mu*0.8f; 144 | 145 | float4 ray_step = ray_dir*step; 146 | 147 | float3 cellsize = { l_params.volume[0] / l_params.resolution[0], l_params.volume[1] / l_params.resolution[1], l_params.volume[2] / l_params.resolution[2] }; 148 | 149 | float tsdf = NAN; 150 | int idx = 0; 151 | float prev_tsdf; 152 | 153 | int3 resolution = { convert_int(l_params.resolution[0]), convert_int(l_params.resolution[1]), convert_int(l_params.resolution[2]) }; 154 | //int3 resolution = { (l_params.resolution[0]), (l_params.resolution[1]), (l_params.resolution[2]) }; 155 | 156 | #ifdef MIROIR_GL_INTEROP 157 | int2 coord = {u, v}; 158 | float4 color = {0.0, 0.0, 0.0, 1.0}; 159 | write_imagef(frameBuffer, coord, color); 160 | #endif 161 | 162 | while(depth_march < l_params.volume[0]) 163 | { 164 | vertex_4 += ray_step; 165 | depth_march += step; 166 | int3 voxel = getVoxel(vertex_4, l_params); 167 | 168 | if( voxel.z < 0 || voxel.z >= resolution.z || 169 | voxel.y < 0 || voxel.y >= resolution.y || 170 | voxel.x < 0 || voxel.x >= resolution.x ) 171 | { 172 | continue; 173 | } 174 | 175 | int tmp_idx = voxel.z + voxel.x * resolution.z + voxel.y * resolution.x * resolution.z; 176 | //idx = voxel.z + voxel.x * resolution.z + voxel.y * resolution.x * resolution.z; 177 | if(tmp_idx == idx) 178 | continue; 179 | idx = tmp_idx; 180 | 181 | prev_tsdf = tsdf; 182 | tsdf = F[idx].x; // bottleneck 183 | 184 | if(isnan(tsdf) || isnan(prev_tsdf)) 185 | continue; 186 | 187 | if(prev_tsdf < 0.0f && tsdf > 0.0f) 188 | break; 189 | 190 | if(prev_tsdf >= 0.0f && tsdf <= 0.0f) 191 | { // zero crossing 192 | 193 | //float Ftdt = tsdf; 194 | float Ftdt = interpolateTrilinearly(F, vertex_4, voxel, cellsize, l_params); 195 | if(isnan(Ftdt)) 196 | break; 197 | 198 | float4 prev_vertex = vertex_4 - ray_step; 199 | int3 prev_voxel = getVoxel(prev_vertex, l_params); 200 | //float Ft = prev_tsdf; 201 | float Ft = interpolateTrilinearly(F, prev_vertex, prev_voxel, cellsize, l_params); 202 | if(isnan(Ft)) 203 | break; 204 | 205 | if(Ftdt > 0.0f && Ft > 0.0f) 206 | continue; 207 | if(Ftdt < 0.0f && Ft < 0.0f) 208 | break; 209 | 210 | float Ts = (depth_march-step) - step * Ft / (Ftdt - Ft); 211 | 212 | float4 v_res = ray_start + ray_dir * Ts; 213 | 214 | vmap[pix_idx].x = v_res.x; 215 | vmap[pix_idx].y = v_res.y; 216 | vmap[pix_idx].z = v_res.z; 217 | 218 | float4 t; 219 | float3 n; 220 | 221 | t = v_res; 222 | t.x += cellsize.x; 223 | float Fx1 = interpolateTrilinearly(F,t,getVoxel(t, l_params), cellsize, l_params); 224 | 225 | if(isnan(Fx1)) 226 | break; 227 | 228 | t = v_res; 229 | t.x -= cellsize.x; 230 | float Fx2 = interpolateTrilinearly(F,t,getVoxel(t, l_params), cellsize, l_params); 231 | 232 | if(isnan(Fx2)) 233 | break; 234 | 235 | n.x = (Fx2 - Fx1);// / (2 * cellsize.x); 236 | 237 | t = v_res; 238 | t.y += cellsize.y; 239 | float Fy1 = interpolateTrilinearly(F,t,getVoxel(t, l_params), cellsize, l_params); 240 | 241 | if(isnan(Fy1)) 242 | break; 243 | 244 | t = v_res; 245 | t.y -= cellsize.y; 246 | float Fy2 = interpolateTrilinearly(F,t,getVoxel(t, l_params), cellsize, l_params); 247 | 248 | if(isnan(Fy2)) 249 | break; 250 | 251 | n.y = (Fy2 - Fy1);// / (2 * cellsize.y); 252 | 253 | t = v_res; 254 | t.z += cellsize.z; 255 | float Fz1 = interpolateTrilinearly(F,t,getVoxel(t, l_params), cellsize, l_params); 256 | 257 | if(isnan(Fz1)) 258 | break; 259 | 260 | t = v_res; 261 | t.z -= cellsize.z; 262 | float Fz2 = interpolateTrilinearly(F,t,getVoxel(t, l_params), cellsize, l_params); 263 | 264 | if(isnan(Fz2)) 265 | break; 266 | 267 | n.z = (Fz2 - Fz1);// / (2 * cellsize.z); 268 | 269 | n = normalize(n); 270 | 271 | nmap[pix_idx] = n; 272 | 273 | // phong shading 274 | 275 | // float3 lightPos = {1.0, 10.0, -1.0}; 276 | // float3 lightDir = lightPos - v_res; 277 | // float4 lightDiffuse = {0.5, 0.5, 0.5, 1.0}; 278 | // float4 lightSpec = {0.0, 0.0, 0.0, 1.0}; 279 | // float4 matDiffuse = lightDiffuse; 280 | // float4 matSpec = lightSpec; 281 | // float matShininess = 1.0; 282 | // float attenuation = 1.0; 283 | // lightDir = normalize(lightDir); 284 | // float NdotL = max(dot(n, lightDir), 0.0); 285 | // float3 eyeVec = {-ray_dir.x, -ray_dir.y, -ray_dir.z}; 286 | // if(NdotL > 0.0) 287 | // { 288 | // color += lightDiffuse * matDiffuse * NdotL * attenuation; 289 | // float3 R = normalize( -reflect(lightDir, n) ); 290 | // float specular = pow( max(dot(R, eyeVec), 0.0), matShininess ); 291 | // color += lightSpec * matSpec * specular * attenuation; 292 | // } 293 | 294 | #ifdef MIROIR_GL_INTEROP 295 | color.x = n.x * 0.5 + 0.5f; 296 | color.y = n.y * 0.5 + 0.5f; 297 | color.z = n.z * 0.5 + 0.5f; 298 | write_imagef(frameBuffer, coord, color); 299 | #endif 300 | 301 | return; 302 | } 303 | } 304 | 305 | vmap[pix_idx] = NAN; 306 | nmap[pix_idx] = NAN; 307 | 308 | } 309 | -------------------------------------------------------------------------------- /src/OpenCL/test.cl: -------------------------------------------------------------------------------- 1 | __kernel void test() 2 | { 3 | const int x = get_global_id(0); 4 | const int y = get_global_id(1); 5 | } -------------------------------------------------------------------------------- /src/OpenCL/tracking_correspondence.cl: -------------------------------------------------------------------------------- 1 | #define _GPU_ 2 | 3 | #include "gpu_def.h" 4 | 5 | // Projective point-plane data association 6 | // Finds corresponding vertices between raycasted volume and current measurement 7 | // given an estimated camera pose (initialised with previous camera pose). 8 | 9 | // __kernel void find_correspondences 10 | // ( 11 | // __global const int* width, 12 | // __global const int* height, 13 | // __global const float* fl, 14 | // __global const float4* cam_pose, 15 | // __global const float4* cam_est_pose, // estimated transform for the new frame 16 | // __global const float4* cam_inc_trans, // transform from current pose to estimated pose 17 | // __global const float3* vmap_raycast, // in global space 18 | // __global const float3* vmap_sensor, // in camera space 19 | // __global const float3* nmap_raycast, 20 | // __global const float3* nmap_sensor, 21 | // __global float2* vcorresp, 22 | // __global uint* counter 23 | // ) 24 | // { 25 | // // for each image pixel (x,y) 26 | // const int x = get_global_id(0); 27 | // const int y = get_global_id(1); 28 | 29 | // int w = *width; 30 | // int h = *height; 31 | // float focal_length = *fl; 32 | 33 | // int idx = w * y + x; 34 | // //float3 vprev_tmp = vmap_raycast[idx]; 35 | // //float4 vprev = { vprev_tmp.x, vprev_tmp.y, vprev_tmp.z, 1 }; 36 | // float3 v_tmp = vmap_sensor[idx]; 37 | // float4 v = { v_tmp.x, v_tmp.y, v_tmp.z, 1 }; 38 | 39 | // if(!_isnan(v)) 40 | // { 41 | 42 | // float4 inc_trans_l[4] = { cam_inc_trans[0], cam_inc_trans[1], cam_inc_trans[2], cam_inc_trans[3] } ; 43 | // float4 p = mul(inc_trans_l, v); 44 | 45 | // p.x = focal_length * p.x / p.z + convert_float(w)/2.0f; 46 | // p.y = focal_length * p.y / p.z + convert_float(h)/2.0f; 47 | 48 | // int px = round(p.x); 49 | // int py = round(p.y); 50 | 51 | // if(px >= 0.0f && py >= 0.0f && px < w && py < h) 52 | // { 53 | // float4 est_pose_l[4] = { cam_est_pose[0], cam_est_pose[1], cam_est_pose[2], cam_est_pose[3] }; 54 | // float4 cam_pose_l[4] = { cam_pose[0], cam_pose[1], cam_pose[2], cam_pose[3] }; 55 | // float3 v_rc_tmp = vmap_raycast[w * py + px]; 56 | 57 | // if(!_isnan(v_rc_tmp)) 58 | // { 59 | // float4 v_rc = {v_rc_tmp.x, v_rc_tmp.y, v_rc_tmp.z, 1.0f}; 60 | 61 | // float4 diff = mul(est_pose_l, v) - mul(cam_pose_l, v_rc); 62 | // float dist = sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z); 63 | 64 | // float3 norm_s = nmap_sensor[idx]; 65 | // float3 rot_norm = 66 | // { 67 | // est_pose_l[0].x * norm_s.x + est_pose_l[1].x * norm_s.y + est_pose_l[2].x * norm_s.z, 68 | // est_pose_l[0].y * norm_s.x + est_pose_l[1].y * norm_s.y + est_pose_l[2].y * norm_s.z, 69 | // est_pose_l[0].z * norm_s.x + est_pose_l[1].z * norm_s.y + est_pose_l[2].z * norm_s.z 70 | // }; 71 | 72 | // float3 norm_rc = nmap_raycast[w * py + px]; 73 | // float4 norm_rc_tmp = {norm_rc.x, norm_rc.y, norm_rc.z, 0.0f}; 74 | // norm_rc_tmp = mul(cam_pose_l, norm_rc_tmp); 75 | // norm_rc.x = norm_rc_tmp.x; 76 | // norm_rc.y = norm_rc_tmp.y; 77 | // norm_rc.z = norm_rc_tmp.z; 78 | 79 | // if(dist < 10.0f && fabs((float)dot(normalize(rot_norm), norm_rc)) > 0.5f) // in mm 80 | // { 81 | // vcorresp[idx].x = px; 82 | // vcorresp[idx].y = py; 83 | // atom_inc(counter); 84 | // return; 85 | // } 86 | // } 87 | // } 88 | // } 89 | // vcorresp[idx].x = NAN; 90 | // vcorresp[idx].y = NAN; 91 | // } 92 | 93 | #define ANGLE_THRES 0.006092310f // sin(20*pi/180) 94 | #define DIST_THRES 10.0f 95 | 96 | __kernel void find_correspondences 97 | ( 98 | __global const int* width, 99 | __global const int* height, 100 | __global const float* fl, 101 | __global const float4* cam_pose_inv_p, 102 | __global const float3* vmap_raycast, // in global space 103 | __global const float3* vmap_sensor, // in camera space 104 | __global const float3* nmap_raycast, 105 | __global const float3* nmap_sensor, 106 | __global float2* vcorresp, 107 | __global uint* counter 108 | ) 109 | { 110 | // for each image pixel (x,y) 111 | const int x = get_global_id(0); 112 | const int y = get_global_id(1); 113 | 114 | int w = *width; 115 | int h = *height; 116 | float focal_length = *fl; 117 | 118 | int idx = w * y + x; 119 | //float3 vprev_tmp = vmap_raycast[idx]; 120 | //float4 vprev = { vprev_tmp.x, vprev_tmp.y, vprev_tmp.z, 1 }; 121 | float3 v_tmp = vmap_raycast[idx]; 122 | float4 v_prev = { v_tmp.x, v_tmp.y, v_tmp.z, 0.0f }; 123 | 124 | if(!_isnan4(v_prev)) 125 | { 126 | float4 cam_pose_inv[4] = {cam_pose_inv_p[0], cam_pose_inv_p[1], cam_pose_inv_p[2], cam_pose_inv_p[3]}; 127 | 128 | float4 v_prev_cs = v_prev + cam_pose_inv[3]; 129 | v_prev_cs.w = 0.0f; 130 | v_prev_cs = mul(cam_pose_inv, v_prev_cs); // to cam space 131 | 132 | float2 p = { focal_length * v_prev_cs.x / v_prev_cs.z + convert_float(w)/2.0f, // perspective project 133 | focal_length * v_prev_cs.y / v_prev_cs.z + convert_float(h)/2.0f }; 134 | 135 | int px = round(p.x); 136 | int py = round(p.y); 137 | 138 | if(px >= 0.0f && py >= 0.0f && px < w && py < h) 139 | { 140 | float3 v_corresp_tmp = vmap_sensor[w * py + px]; 141 | 142 | if(!_isnan3(v_corresp_tmp)) 143 | { 144 | float4 v_corresp_cs = {v_corresp_tmp.x, v_corresp_tmp.y, v_corresp_tmp.z, 1.0f}; // in cam space 145 | 146 | float4 diff = v_corresp_cs - v_prev_cs; 147 | float dist = sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z); 148 | 149 | float3 norm_rc = nmap_raycast[idx]; 150 | float4 norm_rc_4 = {norm_rc.x, norm_rc.y, norm_rc.z, 0.0f}; 151 | float4 rot_norm_4 = mul(cam_pose_inv, norm_rc_4); // to cam space 152 | float3 rot_norm = {rot_norm_4.x, rot_norm_4.y, rot_norm_4.z}; 153 | float3 norm_s = nmap_sensor[w * py + px]; 154 | 155 | 156 | if(dist < DIST_THRES && fabs((float)dot(normalize(rot_norm), norm_s)) > ANGLE_THRES) // in mm 157 | { 158 | vcorresp[idx].x = px; 159 | vcorresp[idx].y = py; 160 | //atom_inc(counter); 161 | return; 162 | } 163 | } 164 | } 165 | } 166 | vcorresp[idx].x = NAN; 167 | vcorresp[idx].y = NAN; 168 | } -------------------------------------------------------------------------------- /src/OpenCL/tracking_sum.cl: -------------------------------------------------------------------------------- 1 | #define _GPU_ 2 | 3 | #include "gpu_def.h" 4 | 5 | #define WORK_GROUP_SIZE 64 6 | #define NUM_WORK_GROUP 153600 / WORK_GROUP_SIZE 7 | #define A_SIZE 21 8 | #define B_SIZE 6 9 | 10 | /// Parallel prefix sum - Compute the sum within 1 work group 11 | /// TODO : avoid bank conflicts 12 | 13 | void summand(int px, int idx, int w, 14 | __global const float4* cam_est_pose, // current estimate 15 | __global const float2* corresp, 16 | __global const float3* vmap, // in cam space (sensor) 17 | __global const float3* vmap_prev, // in global space (raycast) 18 | __global const float3* nmap_prev, 19 | __local float* tempA, __local float* tempB) 20 | { 21 | //int px = 2^thid;//group_idx * 2 + idx; 22 | 23 | float2 corresp_p = corresp[px]; // corresponding px in sensor vmap 24 | float3 v_prev = vmap_prev[px]; 25 | float3 n_prev = nmap_prev[px]; 26 | bool ok = true; 27 | 28 | ok = !_isnan2(corresp_p) && !_isnan3(v_prev) && !_isnan3(n_prev); 29 | 30 | int corresp_idx = ok?(w * convert_int(corresp_p.y) + convert_int(corresp_p.x)):0; 31 | float3 corresp_v = vmap[corresp_idx]; 32 | 33 | ok = ok && !_isnan3(corresp_v) && !isNull(corresp_v); 34 | 35 | if(ok) 36 | { 37 | float4 est_pose_loc[4] = {cam_est_pose[0], cam_est_pose[1], cam_est_pose[2], cam_est_pose[3]}; 38 | //float4 cam_pose_inv_loc[4] = {cam_pose_inv[0], cam_pose_inv[1], cam_pose_inv[2], cam_pose_inv[3]}; 39 | 40 | corresp_v = mul_homogenize(est_pose_loc, corresp_v); // to global coordinates 41 | 42 | // // compute and add transpose(A) = transpose(G) * nmap_prev(x, y) 43 | float3 c = cross(n_prev, corresp_v); 44 | float A[6] = { 45 | c.x, //corresp_v.z * n_prev.y - corresp_v.y * n_prev.z, 46 | c.y, //-corresp_v.z * n_prev.x + corresp_v.x * n_prev.z, 47 | c.z, //corresp_v.y * n_prev.x - corresp_v.x * n_prev.y, 48 | n_prev.x, 49 | n_prev.y, 50 | n_prev.z 51 | }; 52 | 53 | int k = 0; 54 | for(int i = 0; i < 6; i++) // cols 55 | { 56 | for(int j = i; j < 6; j++) // rows 57 | tempA[idx*A_SIZE + (k++)] = A[i] * A[j]; // 21 elements of the upper triangular 6x6 matrix 58 | } 59 | 60 | // compute b = transpose(nmap_prev(x,y)) * (vmap_prev(x,y) - vmap(x,y)) 61 | // add sumB += transpose(A) * b 62 | float b = dot(n_prev, v_prev - corresp_v); 63 | 64 | for(int i = 0; i < 6; i++) 65 | tempB[idx*B_SIZE + i] = A[i] * b; // B is 6x1 66 | } 67 | else 68 | { 69 | for(int k = 0; k < A_SIZE; k++) 70 | tempA[idx*A_SIZE + k] = 0.0f; 71 | for(int i = 0; i < B_SIZE; i++) 72 | tempB[idx*B_SIZE + i] = 0.0f; 73 | } 74 | } 75 | 76 | void reduce 77 | ( 78 | int local_id, 79 | int thid, 80 | __local float* tempA, 81 | __local float* tempB, 82 | __global float* sumA, 83 | __global float* sumB, 84 | int local_size, 85 | int n 86 | ) 87 | { 88 | int offset = 1; 89 | 90 | for(int d = n >> 1; d > 0; d >>= 1) 91 | { 92 | barrier(CLK_LOCAL_MEM_FENCE); 93 | 94 | if(local_id < d) 95 | { 96 | int ai = offset * (2 * local_id + 1) - 1; 97 | int bi = offset * (2 * local_id + 2) - 1; 98 | 99 | for(int i = 0; i < A_SIZE; i++) 100 | tempA[bi*A_SIZE+i] += tempA[ai*A_SIZE+i]; 101 | for(int i = 0; i < B_SIZE; i++) 102 | tempB[bi*B_SIZE+i] += tempB[ai*B_SIZE+i]; 103 | } 104 | offset *= 2; 105 | } 106 | 107 | barrier(CLK_LOCAL_MEM_FENCE); 108 | 109 | if(local_id == 0) 110 | { 111 | int group = thid/local_size; 112 | 113 | // the last elements in tempA and tempB contain the sum of the work-group 114 | for(int i = 0; i < A_SIZE; i++) 115 | sumA[group*A_SIZE+i] = tempA[(n-1)*A_SIZE+i]; 116 | for(int i = 0; i < B_SIZE; i++) 117 | sumB[group*B_SIZE+i] = tempB[(n-1)*B_SIZE+i]; 118 | } 119 | } 120 | 121 | __kernel 122 | void compute_sums 123 | ( 124 | __global int* size, 125 | __global float* ABlocks, // size*A_SIZE 126 | __global float* BBlocks, // size*B_SIZE 127 | __global float* A, // (size / 2 + pad) / WORK_GROUP_SIZE 128 | __global float* B 129 | ) 130 | { 131 | __local float tempA[2*WORK_GROUP_SIZE*A_SIZE]; 132 | __local float tempB[2*WORK_GROUP_SIZE*B_SIZE]; 133 | 134 | const int local_id = get_local_id(0); 135 | const int thid = get_global_id(0); 136 | //int group_idx = 2 * thid / WORK_GROUP_SIZE * WORK_GROUP_SIZE; 137 | 138 | if(thid < ((*size) / 2)) // max thid = 1215, size = 2400, size/2 = 1200 139 | { 140 | for(int i = 0; i < A_SIZE; i++) 141 | { 142 | tempA[2*local_id*A_SIZE+i] = ABlocks[(thid*2)*A_SIZE+i]; 143 | tempA[(2*local_id+1)*A_SIZE+i] = ABlocks[(thid*2+1)*A_SIZE+i]; 144 | } 145 | 146 | for(int i = 0; i < B_SIZE; i++) 147 | { 148 | tempB[2*local_id*B_SIZE+i] = BBlocks[(thid*2)*B_SIZE+i]; 149 | tempB[(2*local_id+1)*B_SIZE+i] = BBlocks[(thid*2+1)*B_SIZE+i]; 150 | } 151 | } 152 | else 153 | { 154 | for(int i = 0; i < A_SIZE; i++) 155 | { 156 | tempA[2*local_id*A_SIZE+i] = 0.0f; 157 | tempA[(2*local_id+1)*A_SIZE+i] = 0.0f; 158 | } 159 | 160 | for(int i = 0; i < B_SIZE; i++) 161 | { 162 | tempB[2*local_id*B_SIZE+i] = 0.0f; 163 | tempB[(2*local_id+1)*B_SIZE+i] = 0.0f; 164 | } 165 | } 166 | 167 | barrier(CLK_LOCAL_MEM_FENCE); 168 | 169 | reduce(local_id, thid, tempA, tempB, A, B, WORK_GROUP_SIZE, WORK_GROUP_SIZE*2); 170 | 171 | 172 | } 173 | 174 | __kernel 175 | void compute_block_sums 176 | ( 177 | __global const int* width, 178 | __global const float4* cam_est_pose, 179 | __global const float2* corresp, 180 | __global const float3* vmap, // in cam space 181 | //__global const float3* nmap, 182 | __global const float3* vmap_prev, // in global space 183 | __global const float3* nmap_prev, 184 | __global float* sumA, // array of size NUM_WORK_GROUP*A_SIZE 185 | __global float* sumB // array of size NUM_WORK_GROUP*B_SIZE 186 | ) 187 | { 188 | __local float tempA[2*WORK_GROUP_SIZE*A_SIZE]; // hold the prefix sum for the current work group 189 | __local float tempB[2*WORK_GROUP_SIZE*B_SIZE]; 190 | const int local_id = get_local_id(0); 191 | const int thid = get_global_id(0); 192 | 193 | int w = *width; 194 | //int group_idx = thid / WORK_GROUP_SIZE * WORK_GROUP_SIZE;// - 1;//thid - (thid%WORK_GROUP_SIZE); 195 | 196 | summand(thid*2, 2*local_id, w, cam_est_pose, corresp, vmap, vmap_prev, nmap_prev, tempA, tempB); 197 | summand(thid*2+1, 2*local_id+1, w, cam_est_pose, corresp, vmap, vmap_prev, nmap_prev, tempA, tempB); 198 | 199 | reduce(local_id, thid, tempA, tempB, sumA, sumB, WORK_GROUP_SIZE, WORK_GROUP_SIZE*2); 200 | } -------------------------------------------------------------------------------- /src/Shaders/PhongShading.fs: -------------------------------------------------------------------------------- 1 | varying vec3 normal; 2 | varying vec3 lightDir; 3 | varying vec2 textCoord; 4 | varying vec3 eyeVec; 5 | varying float att; 6 | uniform sampler2D texture; 7 | 8 | void main() 9 | { 10 | vec4 textColor = texture2D( texture,textCoord ); 11 | vec4 finalColor = ( gl_FrontLightModelProduct.sceneColor * gl_FrontMaterial.ambient ) + 12 | ( gl_LightSource[0].ambient * gl_FrontMaterial.ambient * att ); 13 | float alpha = gl_FrontMaterial.diffuse.a * textColor.a; 14 | 15 | vec3 N = normalize(normal); 16 | vec3 L = normalize(lightDir); 17 | 18 | float NdotL = max( dot( N, L ), 0.0 ); 19 | 20 | if(NdotL > 0.0) 21 | { 22 | finalColor += gl_LightSource[0].diffuse * 23 | gl_FrontMaterial.diffuse * 24 | NdotL * att; 25 | 26 | vec3 E = normalize(eyeVec); 27 | vec3 R = normalize( -reflect(L, N) ); 28 | float specular = pow( max(dot(R, E), 0.0), 29 | gl_FrontMaterial.shininess ); 30 | finalColor += gl_LightSource[0].specular * 31 | gl_FrontMaterial.specular * 32 | specular * att; 33 | } 34 | finalColor *= textColor; 35 | gl_FragColor = vec4( finalColor.xyz,alpha ); 36 | } -------------------------------------------------------------------------------- /src/Shaders/PhongShading.vs: -------------------------------------------------------------------------------- 1 | varying vec3 normal; 2 | varying vec3 lightDir; 3 | varying vec3 eyeVec; 4 | varying vec2 textCoord; 5 | varying float att; 6 | 7 | void main() 8 | { 9 | normal = gl_NormalMatrix * gl_Normal; 10 | vec3 vVertex = vec3(gl_ModelViewMatrix * gl_Vertex); 11 | lightDir = vec3(gl_LightSource[0].position - vVertex); 12 | eyeVec = -vVertex; 13 | 14 | float d = length(lightDir); 15 | 16 | att = 1.0 / ( gl_LightSource[0].constantAttenuation + 17 | (gl_LightSource[0].linearAttenuation*d) + 18 | (gl_LightSource[0].quadraticAttenuation*d*d) ); 19 | 20 | textCoord = vec2( gl_MultiTexCoord0 ); 21 | 22 | gl_Position = ftransform(); 23 | } -------------------------------------------------------------------------------- /src/Shaders/frag.glsl: -------------------------------------------------------------------------------- 1 | varying vec2 texCoord; 2 | uniform sampler2D texture; 3 | 4 | void main() 5 | { 6 | //gl_FragColor = vec4(1.0,0.0,0.5,0.5); 7 | gl_FragColor = texture2D(texture, texCoord); 8 | } 9 | 10 | -------------------------------------------------------------------------------- /src/Shaders/vert.glsl: -------------------------------------------------------------------------------- 1 | varying vec2 texCoord; 2 | 3 | void main() 4 | { 5 | gl_Position = ftransform(); 6 | 7 | vec2 inPos = sign(gl_Vertex.xy); 8 | 9 | texCoord = (vec2(inPos.x, -inPos.y) + 1.0)/2.0; 10 | } -------------------------------------------------------------------------------- /src/data_packet.cpp: -------------------------------------------------------------------------------- 1 | #include "data_packet.h" 2 | #include "definitions.h" 3 | 4 | // explicit template instantiations 5 | 6 | //template class DataPacket< std::tuple< VertexMap, NormalMap > >; -------------------------------------------------------------------------------- /src/gpu_manager.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achsvg/MiroirFusion/4c728d1620f791493903f795536ce7a1e2af78c3/src/gpu_manager.cpp -------------------------------------------------------------------------------- /src/miroir.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #define GLFW_NO_GLU 9 | 10 | #include 11 | #include 12 | 13 | #include "miroir_manager.h" 14 | #include "gpu_manager.h" 15 | 16 | #include 17 | #include 18 | //#ifdef _WIN32 19 | // #define sleep(x) Sleep((x)*1000) 20 | //#endif 21 | 22 | bool stop = false; 23 | GLuint vao; 24 | GLuint quad_vertexbuffer; 25 | 26 | void GLFWCALL onWindowResized( int width, int height ) 27 | { 28 | MiroirManager::getSingleton().setWindowDimensions(width, height); 29 | } 30 | 31 | void keyboardCallback(const pcl::visualization::KeyboardEvent &evt, void *) 32 | { 33 | if(evt.getKeySym() == "F1") 34 | { 35 | MiroirManager::setShowMode(DEPTH_MAP_RAW); 36 | } 37 | else if(evt.getKeySym() == "F2") 38 | { 39 | MiroirManager::setShowMode(DEPTH_MAP_BIL); 40 | } 41 | else if(evt.getKeySym() == "F3") 42 | { 43 | MiroirManager::setShowMode(TSDF); 44 | } 45 | else if(evt.getKeySym() == "F4") 46 | { 47 | MiroirManager::setShowMode(RAYCAST); 48 | } 49 | else if(evt.getKeySym() == "F5") 50 | { 51 | MiroirManager::setShowMode(CORRESP); 52 | } 53 | else if(evt.getKeySym() == "t" && evt.keyDown()) 54 | { 55 | MiroirManager::setRunTSDF(!MiroirManager::isTSDFRunning()); 56 | } 57 | else if(evt.getKeySym() == "a" && evt.keyDown()) 58 | { 59 | //MiroirManager::setRunTSDF(!MiroirManager::isTSDFRunning()); 60 | } 61 | } 62 | 63 | void GLFWCALL keyboardCallback(int key, int action) 64 | { 65 | /*if(key == GLFW_KEY_F1) 66 | { 67 | MiroirManager::setShowMode(DEPTH_MAP); 68 | } 69 | else if(key == GLFW_KEY_F2) 70 | { 71 | MiroirManager::setShowMode(TSDF); 72 | } 73 | else if(key == GLFW_KEY_F3) 74 | { 75 | MiroirManager::setShowMode(RAYCAST); 76 | }*/ 77 | 78 | if(key == 84 && action) // t 79 | { 80 | MiroirManager::setRunTSDF(!MiroirManager::isTSDFRunning()); 81 | } 82 | } 83 | 84 | void draw() 85 | { 86 | glFinish(); 87 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT); // Clear required buffers 88 | 89 | glBindVertexArray(vao); 90 | 91 | glDrawArrays(GL_TRIANGLES, 0, 6); // Starting from vertex 0; 3 vertices total -> 1 triangle 92 | 93 | glBindVertexArray(0); 94 | 95 | glfwSwapBuffers(); 96 | } 97 | 98 | bool initGL() 99 | { 100 | if(!glfwInit()) 101 | { 102 | std::cerr << "Failed to initialize GLFW\n"; 103 | return false; 104 | } 105 | 106 | glfwOpenWindowHint(GLFW_FSAA_SAMPLES, 4); // 4x antialiasing 107 | glfwOpenWindowHint(GLFW_OPENGL_VERSION_MAJOR, 3); // We want OpenGL 3.3 108 | glfwOpenWindowHint(GLFW_OPENGL_VERSION_MINOR, 3); 109 | glfwOpenWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); //We don't want the old OpenGL 110 | glfwOpenWindowHint(GLFW_WINDOW_NO_RESIZE, GL_TRUE); 111 | // Open a window and create its OpenGL context 112 | if( !glfwOpenWindow( 640, 480, 0,0,0,0, 32,0, GLFW_WINDOW ) ) 113 | { 114 | std::cerr << "Failed to open GLFW window\n"; 115 | glfwTerminate(); 116 | return false; 117 | } 118 | 119 | GLenum err = glGetError(); 120 | assert(err == GL_NO_ERROR); 121 | 122 | glfwSetWindowTitle("MIROIR"); 123 | glfwEnable( GLFW_STICKY_KEYS ); 124 | glfwSetWindowSizeCallback( onWindowResized ); 125 | glfwSetKeyCallback(keyboardCallback); 126 | err = glGetError(); 127 | assert(err == GL_NO_ERROR); 128 | 129 | glewExperimental = GL_TRUE; 130 | if(glewInit() != GLEW_OK) 131 | { 132 | std::cerr << "Failed to open GLEW\n"; 133 | return false; 134 | } 135 | glGetError(); 136 | 137 | // background color 138 | glClearColor(0.0f, 0.0f, 0.0f, 0.0f); 139 | 140 | // fullscreen quad 141 | static const GLfloat g_quad_vertex_buffer_data[] = { 142 | -1.0f, -1.0f, 0.0f, 143 | 1.0f, -1.0f, 0.0f, 144 | -1.0f, 1.0f, 0.0f, 145 | -1.0f, 1.0f, 0.0f, 146 | 1.0f, -1.0f, 0.0f, 147 | 1.0f, 1.0f, 0.0f, 148 | }; 149 | 150 | glGenVertexArrays(1, &vao); 151 | glBindVertexArray(vao); 152 | 153 | glGenBuffers(1, &quad_vertexbuffer); 154 | glBindBuffer(GL_ARRAY_BUFFER, quad_vertexbuffer); 155 | glBufferData(GL_ARRAY_BUFFER, sizeof(g_quad_vertex_buffer_data), g_quad_vertex_buffer_data, GL_STATIC_DRAW); 156 | glVertexAttribPointer( 157 | 0, // attribute 0. No particular reason for 0, but must match the layout in the shader. 158 | 3, // size 159 | GL_FLOAT, // type 160 | GL_FALSE, // normalized? 161 | 0, // stride 162 | (void*)0 // array buffer offset 163 | ); 164 | 165 | glEnableVertexAttribArray(0); // Disable our Vertex Array Object 166 | glBindVertexArray(0); // Disable our Vertex Buffer Object 167 | 168 | // shaders 169 | GLuint vshader, fshader; 170 | GLuint program = GpuManager::createProgram("../src/Shaders/vert.glsl", "../src/Shaders/frag.glsl", vshader, fshader); 171 | 172 | glUseProgram(program); 173 | GLint texLoc = glGetUniformLocation(fshader, "texture"); 174 | glUniform1i(texLoc, 0); // TODO pass texture! 175 | 176 | int isValid; 177 | glValidateProgram(vshader); 178 | glGetProgramiv(vshader, GL_VALIDATE_STATUS, &isValid); 179 | assert(isValid); 180 | 181 | return true; 182 | } 183 | 184 | int main( int argc, char ** argv ) 185 | { 186 | #ifdef MIROIR_GL_INTEROP 187 | if(!initGL()) 188 | return -1; 189 | #endif 190 | bool recompile_kernel = (argc == 2)&&(!strcmp(argv[1],"-kc")); 191 | 192 | GpuManager::initCL(recompile_kernel); 193 | 194 | MiroirManager& miroir = MiroirManager::getSingleton(); 195 | 196 | miroir.initGlobalBuffers(); 197 | miroir.buildMiroir(); 198 | 199 | #ifdef MIROIR_GL_INTEROP 200 | miroir.getGrabber()->start(); 201 | do 202 | { 203 | draw(); 204 | 205 | miroir.step(); 206 | 207 | } while(glfwGetKey( GLFW_KEY_ESC ) != GLFW_PRESS && glfwGetWindowParam( GLFW_OPENED )); 208 | 209 | #else 210 | boost::thread miroir_t( &MiroirManager::run, &miroir ); 211 | boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer ("MIROIR")); 212 | viewer->setBackgroundColor (0, 0, 0); 213 | viewer->addCoordinateSystem (1.0); 214 | viewer->initCameraParameters(); 215 | 216 | viewer->registerKeyboardCallback(&keyboardCallback); 217 | viewer->setCameraPosition(0.0, 0.0, -1.0, 0.0, -1.0, 0.0); 218 | 219 | miroir.setWindowDimensions(640, 480); 220 | 221 | //camera visualization 222 | //viewer->addCube(-0.05, 0.05, -0.05, 0.05, -0.05, 0.05); 223 | 224 | while(!viewer->wasStopped() || (glfwGetKey( GLFW_KEY_ESC ) != GLFW_PRESS && glfwGetWindowParam( GLFW_OPENED ))) 225 | { 226 | pcl::PointCloud::ConstPtr cloud = miroir.getOutputCloud(); 227 | if( cloud != NULL ) 228 | { 229 | //viewer.showCloud(cloud); 230 | if(!viewer->updatePointCloud(cloud)) 231 | { 232 | viewer->addPointCloud(cloud); 233 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1); 234 | } 235 | viewer->spinOnce (100); 236 | } 237 | 238 | cl_float4* camPose = miroir.getCameraPose(); 239 | //viewer->addCube(camPose[3].s[0]*0.001-0.05, camPose[3].s[0]*0.001+0.05, camPose[3].s[1]*0.001-0.05, camPose[3].s[1]*0.001+0.05, camPose[3].s[2]*0.001-0.05, camPose[3].s[2]*0.001+0.05); 240 | 241 | pcl::ModelCoefficients coeffs; 242 | // translation 243 | coeffs.values.push_back (camPose[3].s[0]*0.001); 244 | coeffs.values.push_back (camPose[3].s[1]*0.001); 245 | coeffs.values.push_back (camPose[3].s[2]*0.001); 246 | // rotation 247 | float b1 = 0.5f * std::sqrt(1.0f + camPose[0].s[0] + camPose[1].s[1] + camPose[2].s[2]); 248 | coeffs.values.push_back (b1); 249 | coeffs.values.push_back ((camPose[2].s[1] - camPose[1].s[2]) / (4.0f*b1)); 250 | coeffs.values.push_back ((camPose[0].s[2] - camPose[2].s[0]) / (4.0f*b1)); 251 | coeffs.values.push_back ((camPose[1].s[0] - camPose[0].s[1]) / (4.0f*b1)); 252 | // size 253 | coeffs.values.push_back (0.05); 254 | coeffs.values.push_back (0.05); 255 | coeffs.values.push_back (0.05); 256 | viewer->removeShape ("cube"); 257 | viewer->addCube (coeffs, "cube"); 258 | //boost::this_thread::sleep (boost::posix_time::microseconds (100000)); 259 | //sleep(1); 260 | } 261 | 262 | 263 | MiroirManager::stop(); 264 | miroir_t.join(); 265 | #endif 266 | 267 | GpuManager::cleanCL(); 268 | 269 | return 0; 270 | } -------------------------------------------------------------------------------- /src/miroir_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "miroir_manager.h" 2 | 3 | #include "miroir_module_manager.h" 4 | #include "tracking_module.h" 5 | #include "surface_measurement_module.h" 6 | #include "update_reconstruction_module.h" 7 | #include "raycast_module.h" 8 | #include "gpu_manager.h" 9 | #include "../src/OpenCL/gpu_def.h" 10 | #include "window_listener.h" 11 | #include "profiler.h" 12 | #include "util.h" 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | //#define PROFILER 20 | 21 | MiroirManager& MiroirManager::miroir = MiroirManager(); 22 | std::vector MiroirManager::windowListeners; 23 | boost::mutex MiroirManager::miroir_mutex; 24 | 25 | MiroirManager::MiroirManager() : mode(DEPTH_MAP_BIL), frame(0) 26 | { 27 | // setting up the point cloud grabber for Kinect device 28 | grabber = new pcl::OpenNIGrabber(); 29 | //grabber = new pcl::ONIGrabber("C:/Program Files (x86)/OpenNI/Samples/Bin/Release/20120808-133730.oni",true,false); 30 | 31 | boost::function&)> callback = 32 | boost::bind(&MiroirManager::setCurrentDepthImage, this, _1); 33 | grabber->registerCallback(callback); 34 | 35 | boost::function::ConstPtr&)> f = 36 | boost::bind (&MiroirManager::setRawCloud, this, _1); 37 | 38 | grabber->registerCallback(f); 39 | 40 | boost::shared_ptr< openni_wrapper::OpenNIDevice > device = static_cast(grabber)->getDevice(); 41 | sensor_width = device->getDefaultDepthMode().nXRes; 42 | sensor_height = device->getDefaultDepthMode().nYRes; 43 | focal_length = device->getDepthFocalLength(); 44 | principal_point.s[0] = 320.0f;//width * 0.5f; 45 | principal_point.s[1] = 240.0f;//height * 0.5f; 46 | } 47 | 48 | MiroirManager::~MiroirManager() 49 | { 50 | //delete grabber; 51 | delete surf_measurement; 52 | delete reconstruct; 53 | delete rayc; 54 | clReleaseMemObject(width_d); 55 | clReleaseMemObject(height_d); 56 | clReleaseMemObject(fl_d); 57 | } 58 | 59 | void MiroirManager::setRunTSDF(bool enable) 60 | { 61 | MiroirManager::getSingleton().reconstruct->setEnable(enable); 62 | } 63 | 64 | bool MiroirManager::isTSDFRunning() 65 | { 66 | return MiroirManager::getSingleton().reconstruct->isEnabled(); 67 | } 68 | 69 | void MiroirManager::buildMiroir() 70 | { 71 | //SurfaceMeasurementModulePtr 72 | surf_measurement = new SurfaceMeasurementModule(sensor_width, sensor_height, focal_length); 73 | reconstruct = new UpdateReconstructionModule(); 74 | rayc = new RaycastModule(); 75 | pose_estimation = new TrackingModule(); 76 | MiroirModuleManager::getSingleton().plug(*surf_measurement, *pose_estimation); 77 | } 78 | 79 | void MiroirManager::initGlobalBuffers() 80 | { 81 | cl_int err; 82 | TsdfParams params; 83 | params.resolution[0] = RESX; 84 | params.resolution[1] = RESY; 85 | params.resolution[2] = RESZ; 86 | params.volume[0] = VOLUMEX; 87 | params.volume[1] = VOLUMEY; 88 | params.volume[2] = VOLUMEZ; 89 | params.mu = MU; 90 | 91 | miroir.width_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(int), &miroir.sensor_width, &err); 92 | assert(err == CL_SUCCESS); 93 | miroir.height_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(int), &miroir.sensor_height, &err); 94 | assert(err == CL_SUCCESS); 95 | miroir.fl_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(float), &miroir.focal_length, &err); 96 | assert(err == CL_SUCCESS); 97 | miroir.tsdf_params_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(TsdfParams), ¶ms, &err); 98 | assert(err == CL_SUCCESS); 99 | } 100 | 101 | int bli = 0; 102 | 103 | void MiroirManager::step() 104 | { 105 | if( depth_image == NULL ) 106 | return; 107 | 108 | cl_int err; 109 | cl_mem depth_map_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(unsigned short)*NPIX, (void*)(depth_image->getDepthMetaData().Data()), &err); 110 | assert(err == CL_SUCCESS); 111 | 112 | /// begin Test 113 | /*cl_mem prev_surf = clCreateBuffer(GpuManager::context, CL_MEM_READ_WRITE, sizeof(cl_float3)*NPIX, 0, &err); 114 | if(bli != 0) 115 | clEnqueueCopyBuffer(GpuManager::queue, surf_measurement->getVertexMap(), prev_surf, 0, 0, sizeof(cl_float3)*NPIX, 0, 0, 0);*/ 116 | /// end Test 117 | clFinish(GpuManager::queue); 118 | 119 | surf_measurement->setDepthMap(depth_map_d); 120 | MiroirProfiler::start("surface measurement"); 121 | surf_measurement->processData(); 122 | MiroirProfiler::stop("surface measurement"); 123 | 124 | /*if(bli == 0) 125 | clEnqueueCopyBuffer(GpuManager::queue, surf_measurement->getVertexMap(), prev_surf, 0, 0, sizeof(cl_float3)*NPIX, 0, 0, 0); 126 | clFinish(GpuManager::queue);*/ 127 | 128 | // check if camera moved if yes integrate volume 129 | cl_float4 t; 130 | cl_float4* camprev = reconstruct->getCamPose(); 131 | cl_float4* camcurr = pose_estimation->getCamPose(); 132 | cl_float4 camcurr_inv[4]; 133 | cl_float3 rot_inc[3]; 134 | 135 | t.s[0] = camcurr[3].s[0] - camprev[3].s[0]; 136 | t.s[1] = camcurr[3].s[1] - camprev[3].s[1]; 137 | t.s[2] = camcurr[3].s[2] - camprev[3].s[2]; 138 | t.s[3] = camcurr[3].s[3] - camprev[3].s[3]; 139 | 140 | cl_float tnorm = std::sqrt(t.s[0]*t.s[0] + t.s[1]*t.s[1] + t.s[2]*t.s[2] + t.s[3]*t.s[3]); 141 | MiroirUtil::invertCamTransform(camcurr,camcurr_inv); 142 | MiroirUtil::mat3Mult(camcurr_inv, camprev, rot_inc); 143 | cl_float r = MiroirUtil::angleFromRotMatrix(rot_inc); 144 | ////////////////////////////////////////////////////////////// 145 | 146 | //if((r > ROTATION_THRES && tnorm > TRANSLATION_THRES) || frame < 20) 147 | //{ 148 | MiroirProfiler::start("tsdf"); 149 | reconstruct->setDepthMap(depth_map_d); 150 | reconstruct->setCamPose(pose_estimation->getCamPose()); 151 | reconstruct->processData(); 152 | MiroirProfiler::stop("tsdf"); 153 | //} 154 | 155 | clReleaseMemObject(depth_map_d); 156 | 157 | MiroirProfiler::start("raycast"); 158 | //rayc->input( reconstruct->output() ); 159 | rayc->setTSDF(reconstruct->getTSDF()); 160 | rayc->setCamPose(pose_estimation->getCamPose()); 161 | rayc->processData(); 162 | MiroirProfiler::stop("raycast"); 163 | 164 | MiroirProfiler::start("Pose estimation"); 165 | pose_estimation->setParams(rayc->getVertexMap(), surf_measurement->getVertexMap(), rayc->getNormalMap(), surf_measurement->getNormalMap()); 166 | pose_estimation->processData(); 167 | MiroirProfiler::stop("Pose estimation"); 168 | frame++; 169 | // clReleaseMemObject(prev_surf); 170 | 171 | // bli++; 172 | //MiroirModuleManager::getSingleton().feed( *surf_measurement, data ); 173 | //MiroirModuleManager::getSingleton().step(); 174 | } 175 | 176 | #define PROFILING 177 | 178 | void MiroirManager::run() 179 | { 180 | #if defined(PROFILER) 181 | MiroirProfiler::enable(); 182 | #endif 183 | 184 | 185 | grabber->start(); 186 | isRunning = true; 187 | while( isRunning ) 188 | { 189 | MiroirProfiler::start("main loop"); 190 | #ifdef PROFILING 191 | LARGE_INTEGER g_PerfFrequency; 192 | LARGE_INTEGER g_PerformanceCountNDRangeStart; 193 | LARGE_INTEGER g_PerformanceCountNDRangeStop; 194 | QueryPerformanceFrequency(&g_PerfFrequency); 195 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStart); 196 | #endif 197 | step(); 198 | #ifdef PROFILING 199 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStop); 200 | float seconds = (float)((g_PerformanceCountNDRangeStop.QuadPart - g_PerformanceCountNDRangeStart.QuadPart)/(float)g_PerfFrequency.QuadPart); 201 | #endif 202 | MiroirProfiler::stop("main loop"); 203 | //std::cout << std::endl; 204 | } 205 | } 206 | 207 | void MiroirManager::setCurrentDepthImage(const boost::shared_ptr& depthImg) 208 | { 209 | depth_image = depthImg; 210 | } 211 | 212 | //void MiroirManager::setCurrentCloud(const pcl::PointCloud::Ptr& cloud) 213 | //{ 214 | // boost::mutex::scoped_lock lock(miroir_mutex); 215 | // current_cloud = cloud; 216 | //} 217 | 218 | pcl::PointCloud::ConstPtr MiroirManager::getOutputCloud() 219 | { 220 | boost::mutex::scoped_lock lock(miroir_mutex); 221 | if(mode == DEPTH_MAP_RAW) 222 | return raw_cloud; 223 | if(mode == DEPTH_MAP_BIL) 224 | return surf_measurement->getFilteredCloud(); 225 | if(mode == TSDF) 226 | return reconstruct->getCloud(); 227 | if(mode == RAYCAST) 228 | return rayc->getCloud(); 229 | if(mode == CORRESP) 230 | return pose_estimation->getCloud(); 231 | return pcl::PointCloud::ConstPtr(); 232 | } 233 | 234 | void MiroirManager::setWindowDimensions(int width, int height) 235 | { 236 | MiroirManager::getSingleton().w_width = width; 237 | MiroirManager::getSingleton().w_height = height; 238 | 239 | for(auto it = windowListeners.begin(); it != windowListeners.end(); ++it) 240 | { 241 | (*it)->onWindowResized(width, height); 242 | } 243 | } 244 | 245 | void MiroirManager::getSensorDimensions(int& w, int& h) 246 | { 247 | w = MiroirManager::getSingleton().sensor_width; 248 | h = MiroirManager::getSingleton().sensor_height; 249 | } 250 | 251 | void MiroirManager::getWindowDimensions(int& w, int& h) 252 | { 253 | w = MiroirManager::getSingleton().w_width; 254 | h = MiroirManager::getSingleton().w_height; 255 | } 256 | 257 | float MiroirManager::getFocalLength() 258 | { 259 | return MiroirManager::getSingleton().focal_length; 260 | } 261 | 262 | cl_float2 MiroirManager::getPrincipalPoint() 263 | { 264 | return MiroirManager::getSingleton().principal_point; 265 | } 266 | 267 | cl_float4* MiroirManager::getCameraPose() 268 | { 269 | boost::mutex::scoped_lock lock(MiroirManager::getSingleton().miroir_mutex); 270 | return MiroirManager::getSingleton().pose_estimation->getCamPose(); 271 | } 272 | -------------------------------------------------------------------------------- /src/miroir_module_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "miroir_module_manager.h" 2 | 3 | MiroirModuleManager& MiroirModuleManager::singleton = MiroirModuleManager(); 4 | 5 | MiroirModuleManager::MiroirModuleManager() 6 | { 7 | } 8 | 9 | MiroirModuleManager::~MiroirModuleManager() 10 | { 11 | } 12 | -------------------------------------------------------------------------------- /src/profiler.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "profiler.h" 3 | 4 | //MiroirProfiler& MiroirProfiler::profiler = MiroirProfiler(); 5 | 6 | using boost::timer::cpu_timer; 7 | 8 | std::map MiroirProfiler::start_time = std::map(); 9 | bool MiroirProfiler::enabled = false; 10 | 11 | MiroirProfiler::MiroirProfiler() 12 | { 13 | } 14 | 15 | //MiroirProfiler& MiroirProfiler::getSingleton() 16 | //{ 17 | // return profiler; 18 | //} 19 | 20 | void MiroirProfiler::start(const std::string& name) 21 | { 22 | if(enabled) 23 | { 24 | cpu_timer* timer = new cpu_timer(); 25 | timer->start(); 26 | start_time[name] = timer; 27 | } 28 | } 29 | 30 | void MiroirProfiler::stop(const std::string& name) 31 | { 32 | if(enabled) 33 | { 34 | auto pos = start_time.find(name); 35 | if( pos == start_time.end()) 36 | return; 37 | 38 | cpu_timer* timer = start_time[name]; 39 | start_time[name]->stop(); 40 | std::cout << name << " elapsed time : " << boost::timer::format(timer->elapsed(), 5, "%t") << std::endl; 41 | 42 | delete timer; 43 | start_time.erase(pos); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /src/raycast_module.cpp: -------------------------------------------------------------------------------- 1 | #include "raycast_module.h" 2 | #include "gpu_manager.h" 3 | #include "miroir_manager.h" 4 | #include "../src/OpenCL/gpu_def.h" 5 | #include 6 | #include 7 | 8 | RaycastModule::RaycastModule() 9 | { 10 | raycast_kernel = GpuManager::createKernel((std::string(KERNEL_PATH)+std::string("raycast.cl")).c_str(), "raycast"); 11 | 12 | int width, height; 13 | MiroirManager::getSensorDimensions(width, height); 14 | 15 | int npixel = width*height; 16 | int err; 17 | 18 | vmap_h = new cl_float3[npixel]; 19 | nmap_h = new cl_float3[npixel]; 20 | 21 | vmap_d = clCreateBuffer(GpuManager::context, CL_MEM_WRITE_ONLY, sizeof(cl_float3)*npixel, NULL, &err); 22 | nmap_d = clCreateBuffer(GpuManager::context, CL_MEM_WRITE_ONLY, sizeof(cl_float3)*npixel, NULL, &err); 23 | assert(err == CL_SUCCESS); 24 | 25 | #ifdef MIROIR_GL_INTEROP 26 | GpuManager::createSharedTexture(width, height, CL_MEM_WRITE_ONLY, tex_d, gl_tex); 27 | #endif 28 | 29 | tex_dim[0] = width; 30 | tex_dim[1] = height; 31 | } 32 | 33 | RaycastModule::~RaycastModule() 34 | { 35 | clReleaseMemObject(vmap_d); 36 | clReleaseMemObject(nmap_d); 37 | clReleaseKernel(raycast_kernel); 38 | delete [] vmap_h; 39 | delete [] nmap_h; 40 | } 41 | 42 | //#define PROFILING 43 | 44 | void RaycastModule::raycast(cl_mem TSDF, cl_float4 cam_pos[4]) 45 | { 46 | boost::mutex::scoped_lock lock(raycast_mutex); 47 | cl_int err; 48 | 49 | int width, height; 50 | MiroirManager::getWindowDimensions(width, height); 51 | int npixel = width*height; 52 | 53 | if(width != tex_dim[0] || height != tex_dim[1]) 54 | return; 55 | 56 | cl_float2 princ_pt = MiroirManager::getPrincipalPoint(); 57 | 58 | cl_mem tsdf_params = MiroirManager::getCLTSDFParams(); 59 | 60 | cl_mem cam_pos_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_float4)*4, cam_pos, &err); 61 | assert(err == CL_SUCCESS); 62 | cl_mem width_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_int), &width,&err); 63 | assert(err == CL_SUCCESS); 64 | cl_mem height_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_int), &height,&err); 65 | assert(err == CL_SUCCESS); 66 | cl_mem fl_d = MiroirManager::getCLFocalLength();//clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_float), &fl, &err); 67 | //assert(err == CL_SUCCESS); 68 | cl_mem ppt_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_float2), &princ_pt, &err); 69 | assert(err == CL_SUCCESS); 70 | 71 | cl_uint j = 0; 72 | 73 | // GL-CL sync 74 | glFlush(); 75 | glFinish(); 76 | 77 | // All pending GL calls have finished -> safe to acquire the buffer in CL 78 | //clEnqueueAcquireGLObjects(GpuManager::queue, 1, &vmap_d, 0,0,0); 79 | //clEnqueueAcquireGLObjects(GpuManager::queue, 1, &nmap_d, 0,0,0); 80 | 81 | err |= clSetKernelArg(raycast_kernel, j++, sizeof(cl_mem), (void*)&TSDF); 82 | err |= clSetKernelArg(raycast_kernel, j++, sizeof(cl_mem), (void*)&width_d); 83 | err |= clSetKernelArg(raycast_kernel, j++, sizeof(cl_mem), (void*)&height_d); 84 | err |= clSetKernelArg(raycast_kernel, j++, sizeof(cl_mem), (void*)&cam_pos_d); 85 | err |= clSetKernelArg(raycast_kernel, j++, sizeof(cl_mem), (void*)&fl_d); 86 | err |= clSetKernelArg(raycast_kernel, j++, sizeof(cl_mem), (void*)&ppt_d); 87 | err |= clSetKernelArg(raycast_kernel, j++, sizeof(cl_mem), (void*)&vmap_d); 88 | err |= clSetKernelArg(raycast_kernel, j++, sizeof(cl_mem), (void*)&nmap_d); 89 | #ifdef MIROIR_GL_INTEROP 90 | 91 | err = clEnqueueAcquireGLObjects(GpuManager::queue, 1, &tex_d, 0,0,0); 92 | assert(err == CL_SUCCESS); 93 | err |= clSetKernelArg(raycast_kernel, j++, sizeof(cl_mem), &tex_d); 94 | 95 | #endif 96 | err |= clSetKernelArg(raycast_kernel, j++, sizeof(cl_mem), (void*)&tsdf_params); 97 | 98 | assert(err == CL_SUCCESS); 99 | 100 | size_t global_ws[2] = { width, height }; 101 | 102 | #ifdef PROFILING 103 | LARGE_INTEGER g_PerfFrequency; 104 | LARGE_INTEGER g_PerformanceCountNDRangeStart; 105 | LARGE_INTEGER g_PerformanceCountNDRangeStop; 106 | QueryPerformanceFrequency(&g_PerfFrequency); 107 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStart); 108 | #endif 109 | 110 | err = clEnqueueNDRangeKernel(GpuManager::queue, raycast_kernel, 2, 0, global_ws, NULL, 0, NULL, NULL); 111 | clFinish(GpuManager::queue); 112 | 113 | #ifdef PROFILING 114 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStop); 115 | float seconds = (float)((g_PerformanceCountNDRangeStop.QuadPart - g_PerformanceCountNDRangeStart.QuadPart)/(float)g_PerfFrequency.QuadPart); 116 | #endif 117 | 118 | assert(err == CL_SUCCESS); 119 | 120 | // 121 | //cl_float3* test = new cl_float3[307200]; 122 | //err = clEnqueueReadBuffer(GpuManager::queue, vmap_d, CL_TRUE, 0, sizeof(cl_float3)*npixel, vmap_h, 0, NULL, NULL); 123 | //err = clEnqueueReadBuffer(GpuManager::queue, nmap_d, CL_TRUE, 0, sizeof(cl_float3)*npixel, nmap_h, 0, NULL, NULL); 124 | #ifdef MIROIR_GL_INTEROP 125 | err = clEnqueueReleaseGLObjects(GpuManager::queue, 1, &tex_d, 0,0,0); 126 | //err = clEnqueueReleaseGLObjects(GpuManager::queue, 1, &vmap_d, 0,0,0); 127 | //err |= clEnqueueReleaseGLObjects(GpuManager::queue, 1, &nmap_d, 0,0,0); 128 | assert(err == CL_SUCCESS); 129 | #endif 130 | clFinish(GpuManager::queue); 131 | 132 | clReleaseMemObject(cam_pos_d); 133 | //clReleaseMemObject(width_d); 134 | //clReleaseMemObject(fl_d); 135 | clReleaseMemObject(ppt_d); 136 | //clReleaseMemObject(vmap_d); 137 | //clReleaseMemObject(nmap_d); 138 | } 139 | 140 | void RaycastModule::processData() 141 | { 142 | raycast( tsdf, cam_pose ); 143 | int width, height; 144 | MiroirManager::getSensorDimensions(width, height); 145 | int npixel = width*height; 146 | 147 | if(MiroirManager::getShowMode() == RAYCAST) 148 | { 149 | boost::mutex::scoped_lock lock(MiroirManager::getMutex()); 150 | clEnqueueReadBuffer(GpuManager::queue, vmap_d, CL_TRUE, 0, sizeof(cl_float3)*npixel, vmap_h, 0, NULL, NULL); 151 | clEnqueueReadBuffer(GpuManager::queue, nmap_d, CL_TRUE, 0, sizeof(cl_float3)*npixel, nmap_h, 0, NULL, NULL); 152 | clFinish(GpuManager::queue); 153 | 154 | pcl::OpenNIGrabber* grabber = static_cast(MiroirManager::getGrabber()); 155 | cloud = pcl::PointCloud::Ptr(new pcl::PointCloud ); 156 | cloud->height = grabber->getDevice()->getDepthOutputMode().nYRes; 157 | cloud->width = grabber->getDevice()->getDepthOutputMode().nXRes; 158 | cloud->is_dense = false; 159 | 160 | cloud->points.resize (cloud->height * cloud->width); 161 | 162 | if (grabber->getDevice()->isDepthRegistered ()) 163 | cloud->header.frame_id = "/openni_rgb_optical_frame"; 164 | else 165 | cloud->header.frame_id = "/openni_depth_optical_frame"; 166 | 167 | for(int i=0; iheight * cloud->width; i++) 168 | { 169 | pcl::PointXYZRGBA& pt = cloud->points[i]; 170 | 171 | if(vmap_h[i].s[0] != vmap_h[i].s[0]) 172 | { 173 | pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); 174 | continue; 175 | } 176 | 177 | pt.z = vmap_h[i].s[2] * 0.001; 178 | pt.x = vmap_h[i].s[0] * 0.001; 179 | pt.y = vmap_h[i].s[1] * 0.001; 180 | pt.r = pt.g = pt.b = 127; 181 | pt.r = 255 * (nmap_h[i].s[0] + 1) / 2; 182 | pt.g = 255 * (nmap_h[i].s[1] + 1) / 2; 183 | pt.b = 255 * (nmap_h[i].s[2] + 1) / 2; 184 | 185 | } 186 | 187 | cloud->sensor_origin_.setZero (); 188 | cloud->sensor_orientation_.w () = 0.0f; 189 | cloud->sensor_orientation_.x () = 1.0f; 190 | cloud->sensor_orientation_.y () = 0.0f; 191 | cloud->sensor_orientation_.z () = 0.0f; 192 | } 193 | } 194 | 195 | void RaycastModule::onWindowResized(int width, int height) 196 | { 197 | boost::mutex::scoped_lock lock(raycast_mutex); 198 | //ost::mutex::scoped_lock lock(MiroirManager::getMutex()); 199 | 200 | tex_dim[0] = width; 201 | tex_dim[1] = height; 202 | 203 | #ifdef MIROIR_GL_INTEROP 204 | //glDeleteTextures(1, &gl_tex); 205 | 206 | //// creating the texture to render full screen quad 207 | //glActiveTexture(GL_TEXTURE0); 208 | //GpuManager::createSharedTexture(width, height, CL_MEM_WRITE_ONLY, tex_d, gl_tex); 209 | 210 | //int npixel = width*height; 211 | 212 | //clReleaseMemObject(vmap_d); 213 | //clReleaseMemObject(nmap_d); 214 | 215 | //int err; 216 | //vmap_d = clCreateBuffer(GpuManager::context, CL_MEM_WRITE_ONLY, sizeof(cl_float3)*npixel, NULL, &err); 217 | //nmap_d = clCreateBuffer(GpuManager::context, CL_MEM_WRITE_ONLY, sizeof(cl_float3)*npixel, NULL, &err); 218 | 219 | //delete [] vmap_h; 220 | //delete [] nmap_h; 221 | 222 | //vmap_h = new cl_float3[npixel]; 223 | //nmap_h = new cl_float3[npixel]; 224 | #endif 225 | } -------------------------------------------------------------------------------- /src/surface_measurement_module.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "surface_measurement_module.h" 5 | #include "miroir_manager.h" 6 | #include "gpu_manager.h" 7 | #include "../src/OpenCL/gpu_def.h" 8 | 9 | #include 10 | #include 11 | 12 | 13 | // helper function TODO : put it somewhere 14 | pcl::PointCloud::Ptr 15 | convertToXYZPointCloud (pcl::OpenNIGrabber* grabber, const boost::shared_ptr& depth_image) 16 | { 17 | boost::shared_ptr< openni_wrapper::OpenNIDevice > device_ = grabber->getDevice(); 18 | XnMapOutputMode depth_md = device_->getDepthOutputMode(); 19 | std::string rgb_frame_id_ = "/openni_rgb_optical_frame"; 20 | std::string depth_frame_id_ = "/openni_depth_optical_frame"; 21 | unsigned depth_width_ = depth_md.nXRes; 22 | unsigned depth_height_ = depth_md.nYRes; 23 | 24 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud ); 25 | 26 | cloud->height = depth_height_; 27 | cloud->width = depth_width_; 28 | cloud->is_dense = false; 29 | 30 | cloud->points.resize (cloud->height * cloud->width); 31 | 32 | register float constant = 1.0f / grabber->getDevice()->getDepthFocalLength (depth_width_); 33 | 34 | if (device_->isDepthRegistered ()) 35 | cloud->header.frame_id = rgb_frame_id_; 36 | else 37 | cloud->header.frame_id = depth_frame_id_; 38 | 39 | register int centerX = (cloud->width >> 1); 40 | int centerY = (cloud->height >> 1); 41 | 42 | float bad_point = std::numeric_limits::quiet_NaN (); 43 | 44 | // we have to use Data, since operator[] uses assert -> Debug-mode very slow! 45 | register const unsigned short* depth_map = depth_image->getDepthMetaData ().Data (); 46 | if (depth_image->getWidth() != depth_width_ || depth_image->getHeight () != depth_height_) 47 | { 48 | static unsigned buffer_size = 0; 49 | static boost::shared_array depth_buffer (0); 50 | 51 | if (buffer_size < depth_width_ * depth_height_) 52 | { 53 | buffer_size = depth_width_ * depth_height_; 54 | depth_buffer.reset (new unsigned short [buffer_size]); 55 | } 56 | depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ()); 57 | depth_map = depth_buffer.get (); 58 | } 59 | 60 | register int depth_idx = 0; 61 | for (int v = -centerY; v < centerY; ++v) 62 | { 63 | for (register int u = -centerX; u < centerX; ++u, ++depth_idx) 64 | { 65 | pcl::PointXYZ& pt = cloud->points[depth_idx]; 66 | // Check for invalid measurements 67 | if (depth_map[depth_idx] == 0 || 68 | depth_map[depth_idx] == depth_image->getNoSampleValue () || 69 | depth_map[depth_idx] == depth_image->getShadowValue ()) 70 | { 71 | // not valid 72 | pt.x = pt.y = pt.z = bad_point; 73 | continue; 74 | } 75 | pt.z = depth_map[depth_idx] * 0.001f; 76 | pt.x = static_cast (u) * pt.z * constant; 77 | pt.y = static_cast (v) * pt.z * constant; 78 | 79 | } 80 | } 81 | cloud->sensor_origin_.setZero (); 82 | cloud->sensor_orientation_.w () = 0.0f; 83 | cloud->sensor_orientation_.x () = 1.0f; 84 | cloud->sensor_orientation_.y () = 0.0f; 85 | cloud->sensor_orientation_.z () = 0.0f; 86 | return (cloud); 87 | } 88 | 89 | SurfaceMeasurementModule::SurfaceMeasurementModule( int w, int h, float focal_length ) 90 | { 91 | cloud_filtered = pcl::PointCloud< pcl::PointXYZRGBA >::Ptr( new pcl::PointCloud< pcl::PointXYZRGBA >() ); 92 | cl_int err; 93 | 94 | // gpu programs 95 | measurement_vertices = GpuManager::createKernel((std::string(KERNEL_PATH)+std::string("bilateral.cl")).c_str(), "measurement_vertices"); 96 | measurement_normals = GpuManager::createKernel((std::string(KERNEL_PATH)+std::string("bilateral.cl")).c_str(), "measurement_normals"); 97 | 98 | dmap_h = new unsigned short[NPIX]; 99 | vmap_h = new cl_float3[NPIX]; 100 | nmap_h = new cl_float3[NPIX]; 101 | 102 | float sig_s = 100.0f; 103 | float sig_r = 200.0f; 104 | /*int w = MiroirManager::getDepthWidth(); 105 | int h = MiroirManager::getDepthHeight(); 106 | float fl = MiroirManager::getFocalLength();*/ 107 | 108 | sigma_s_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(float), &sig_s, &err); 109 | assert(err == CL_SUCCESS); 110 | sigma_r_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(float), &sig_r, &err); 111 | assert(err == CL_SUCCESS); 112 | 113 | vmap_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_WRITE, sizeof(cl_float3)*NPIX, NULL, &err); 114 | assert(err == CL_SUCCESS); 115 | nmap_d = clCreateBuffer(GpuManager::context, CL_MEM_WRITE_ONLY, sizeof(cl_float3)*NPIX, NULL, &err); 116 | assert(err == CL_SUCCESS); 117 | } 118 | 119 | //#define PROFILING 120 | 121 | void SurfaceMeasurementModule::bilateralFilter( cl_mem raw_dmap ) 122 | { 123 | //const XnDepthPixel* depth_map = src.getDepthMetaData().Data(); 124 | //int npixel = src.getWidth()*src.getHeight(); 125 | //unsigned short* buffer = new unsigned short[npixel]; 126 | cl_int err; 127 | 128 | //cl_mem res_d = clCreateBuffer(GpuManager::context, CL_MEM_WRITE_ONLY, sizeof(unsigned short)*NPIX, NULL, &err); 129 | //assert(err == CL_SUCCESS); 130 | 131 | cl_mem width_d = MiroirManager::getCLWidth(); 132 | cl_mem height_d = MiroirManager::getCLHeight(); 133 | cl_mem fl_d = MiroirManager::getCLFocalLength(); 134 | 135 | cl_uint j = 0; 136 | err = clSetKernelArg(measurement_vertices, j++, sizeof(raw_dmap), (void*)&raw_dmap); 137 | err |= clSetKernelArg(measurement_vertices, j++, sizeof(width_d), (void*)&width_d); 138 | err |= clSetKernelArg(measurement_vertices, j++, sizeof(height_d), (void*)&height_d); 139 | err |= clSetKernelArg(measurement_vertices, j++, sizeof(fl_d), (void*)&fl_d); 140 | //err |= clSetKernelArg(measurement_vertices, j++, sizeof(res_d), (void*)&res_d); 141 | err |= clSetKernelArg(measurement_vertices, j++, sizeof(vmap_d), (void*)&vmap_d); 142 | err |= clSetKernelArg(measurement_vertices, j++, sizeof(sigma_s_d), (void*)&sigma_s_d); 143 | err |= clSetKernelArg(measurement_vertices, j++, sizeof(sigma_r_d), (void*)&sigma_r_d); 144 | 145 | assert(err == CL_SUCCESS); 146 | 147 | int width, height; 148 | MiroirManager::getSensorDimensions(width, height); 149 | 150 | size_t global_ws[2] = { width, height }; 151 | 152 | #ifdef PROFILING 153 | LARGE_INTEGER g_PerfFrequency; 154 | LARGE_INTEGER g_PerformanceCountNDRangeStart; 155 | LARGE_INTEGER g_PerformanceCountNDRangeStop; 156 | QueryPerformanceFrequency(&g_PerfFrequency); 157 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStart); 158 | #endif 159 | 160 | err = clEnqueueNDRangeKernel(GpuManager::queue, measurement_vertices, 2, 0, global_ws, NULL, 0, NULL, NULL); 161 | 162 | clFinish(GpuManager::queue); 163 | 164 | #ifdef PROFILING 165 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStop); 166 | float seconds = (float)((g_PerformanceCountNDRangeStop.QuadPart - g_PerformanceCountNDRangeStart.QuadPart)/(float)g_PerfFrequency.QuadPart); 167 | #endif 168 | 169 | assert(err == CL_SUCCESS); 170 | // compute normals 171 | j = 0; 172 | err = clSetKernelArg(measurement_normals, j++, sizeof(width_d), (void*)&width_d); 173 | err |= clSetKernelArg(measurement_normals, j++, sizeof(height_d), (void*)&height_d); 174 | err |= clSetKernelArg(measurement_normals, j++, sizeof(vmap_d), (void*)&vmap_d); 175 | err |= clSetKernelArg(measurement_normals, j++, sizeof(nmap_d), (void*)&nmap_d); 176 | assert(err == CL_SUCCESS); 177 | 178 | #ifdef PROFILING 179 | QueryPerformanceFrequency(&g_PerfFrequency); 180 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStart); 181 | #endif 182 | 183 | err = clEnqueueNDRangeKernel(GpuManager::queue, measurement_normals, 2, 0, global_ws, NULL, 0, NULL, NULL); 184 | 185 | clFinish(GpuManager::queue); 186 | 187 | #ifdef PROFILING 188 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStop); 189 | seconds = (float)((g_PerformanceCountNDRangeStop.QuadPart - g_PerformanceCountNDRangeStart.QuadPart)/(float)g_PerfFrequency.QuadPart); 190 | #endif 191 | 192 | assert(err == CL_SUCCESS); 193 | } 194 | 195 | void SurfaceMeasurementModule::processData() 196 | { 197 | /* 1 - bilateral filtering of the raw depth map 198 | 2 - back project the filtered depth map to sensor's frame of reference to obtain thwe VertexMap -> convertToXYZPointCloud 199 | 3 - compute NormalMap from neighbouring vertices 200 | 201 | Note : use vertex validity mask Mk(u) = 1 if depth measurement is valid, Mk(u) = 0, if 202 | depth measurement if missing. 203 | 204 | 4 - compute L = 3 "mimaps" of VertexMap and NormalMap by block averaging and subsampling to half 205 | the resolution of previous level. 206 | */ 207 | 208 | bilateralFilter( dmap ); 209 | 210 | // create the cloud from the vertex map 211 | //cloud_filtered = convertToXYZPointCloud( static_cast(MiroirManager::getGrabber()), dm_filtered ); 212 | #ifndef MIROIR_GL_INTEROP 213 | if(MiroirManager::getShowMode() == DEPTH_MAP_BIL) 214 | { 215 | boost::mutex::scoped_lock lock(MiroirManager::getMutex()); 216 | clEnqueueReadBuffer(GpuManager::queue, vmap_d, CL_TRUE, 0, sizeof(cl_float3)*NPIX, vmap_h, 0, NULL, NULL); 217 | clEnqueueReadBuffer(GpuManager::queue, nmap_d, CL_TRUE, 0, sizeof(cl_float3)*NPIX, nmap_h, 0, NULL, NULL); 218 | pcl::OpenNIGrabber* grabber = static_cast(MiroirManager::getGrabber()); 219 | cloud_filtered = pcl::PointCloud::Ptr(new pcl::PointCloud ); 220 | cloud_filtered->height = grabber->getDevice()->getDepthOutputMode().nYRes; 221 | cloud_filtered->width = grabber->getDevice()->getDepthOutputMode().nXRes; 222 | cloud_filtered->is_dense = false; 223 | 224 | cloud_filtered->points.resize (cloud_filtered->height * cloud_filtered->width); 225 | 226 | if (grabber->getDevice()->isDepthRegistered ()) 227 | cloud_filtered->header.frame_id = "/openni_rgb_optical_frame"; 228 | else 229 | cloud_filtered->header.frame_id = "/openni_depth_optical_frame"; 230 | 231 | for(int i=0; ipoints[i]; 234 | 235 | if(vmap_h[i].s[2] == 0) 236 | { 237 | pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); 238 | continue; 239 | } 240 | 241 | pt.x = vmap_h[i].s[0] * 0.001; 242 | pt.y = vmap_h[i].s[1] * 0.001; 243 | pt.z = vmap_h[i].s[2] * 0.001; 244 | pt.r = 255 * (nmap_h[i].s[0] + 1) / 2; 245 | pt.g = 255 * (nmap_h[i].s[1] + 1) / 2; 246 | pt.b = 255 * (nmap_h[i].s[2] + 1) / 2; 247 | } 248 | 249 | cloud_filtered->sensor_origin_.setZero (); 250 | /*cloud_filtered->sensor_orientation_.w () = 1.0f; 251 | cloud_filtered->sensor_orientation_.x () = 0.0f; 252 | cloud_filtered->sensor_orientation_.y () = 0.0f; 253 | cloud_filtered->sensor_orientation_.z () = 0.0f;*/ 254 | } 255 | #endif 256 | } 257 | -------------------------------------------------------------------------------- /src/tracking_module.cpp: -------------------------------------------------------------------------------- 1 | #include "tracking_module.h" 2 | #include "gpu_manager.h" 3 | #include "miroir_manager.h" 4 | #include "util.h" 5 | 6 | #include 7 | 8 | TrackingModule::TrackingModule() 9 | { 10 | cl_float4 c1; 11 | cl_float4 c2; 12 | cl_float4 c3; 13 | cl_float4 c4; 14 | 15 | c1.s[0] = c2.s[1] = c3.s[2] = c4.s[3] = 1.0f; 16 | c1.s[1] = c1.s[2] = c1.s[3] = c2.s[0] = c2.s[2] = c2.s[3] = c3.s[0] = c3.s[1] = c3.s[3] = c4.s[0] = c4.s[1] = c4.s[2] = 0.0f; 17 | 18 | cam_pose[0] = c1; 19 | cam_pose[1] = c2; 20 | cam_pose[2] = c3; 21 | cam_pose[3] = c4; 22 | 23 | find_corresp = GpuManager::createKernel((std::string(KERNEL_PATH)+std::string("tracking_correspondence.cl")).c_str(), "find_correspondences"); 24 | compute_block_sum = GpuManager::createKernel((std::string(KERNEL_PATH)+std::string("tracking_sum.cl")).c_str(), "compute_block_sums"); 25 | compute_sum = GpuManager::createKernel((std::string(KERNEL_PATH)+std::string("tracking_sum.cl")).c_str(), "compute_sums"); 26 | 27 | corresp_h = new cl_float2[NPIX]; 28 | 29 | for(int i = 0; i < NPIX; i++) 30 | { 31 | corresp_h[i].s[0] = corresp_h[i].s[1] = std::numeric_limits::quiet_NaN (); 32 | } 33 | 34 | cl_int err; 35 | corresp_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, sizeof(cl_float2)*NPIX, corresp_h, &err); 36 | assert(err == CL_SUCCESS); 37 | 38 | delete [] corresp_h; 39 | } 40 | 41 | //#define PROFILING 42 | 43 | void TrackingModule::findCorrespondences(const cl_float4 cam_pose_inv[4]) 44 | { 45 | cl_int err; 46 | 47 | cl_mem cam_pose_inv_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_float4)*4, (void*)cam_pose_inv, &err); 48 | assert(err == CL_SUCCESS); 49 | 50 | // test 51 | unsigned int c=0; 52 | cl_mem counter = clCreateBuffer(GpuManager::context, CL_MEM_READ_WRITE, sizeof(cl_uint), 0, &err); 53 | assert(err == CL_SUCCESS); 54 | err = clEnqueueWriteBuffer(GpuManager::queue, counter, true, 0, sizeof(cl_int), &c, 0, NULL, NULL); 55 | assert(err == CL_SUCCESS); 56 | // 57 | 58 | cl_mem width_d = MiroirManager::getCLWidth(); 59 | cl_mem height_d = MiroirManager::getCLHeight(); 60 | cl_mem fl_d = MiroirManager::getCLFocalLength(); 61 | 62 | cl_uint j = 0; 63 | err = clSetKernelArg(find_corresp, j++, sizeof(cl_mem), (void*)&width_d); 64 | err |= clSetKernelArg(find_corresp, j++, sizeof(cl_mem), (void*)&height_d); 65 | err |= clSetKernelArg(find_corresp, j++, sizeof(cl_mem), (void*)&fl_d); 66 | err |= clSetKernelArg(find_corresp, j++, sizeof(cl_mem), (void*)&cam_pose_inv_d); 67 | err |= clSetKernelArg(find_corresp, j++, sizeof(cl_mem), (void*)&vmap_raycast); 68 | err |= clSetKernelArg(find_corresp, j++, sizeof(cl_mem), (void*)&vmap_sensor); 69 | err |= clSetKernelArg(find_corresp, j++, sizeof(cl_mem), (void*)&nmap_raycast); 70 | err |= clSetKernelArg(find_corresp, j++, sizeof(cl_mem), (void*)&nmap_sensor); 71 | err = clSetKernelArg(find_corresp, j++, sizeof(cl_mem), (void*)&corresp_d); 72 | clSetKernelArg(find_corresp, j++, sizeof(counter), &counter); 73 | 74 | assert(err == CL_SUCCESS); 75 | 76 | int width, height; 77 | MiroirManager::getSensorDimensions(width, height); 78 | 79 | size_t global_ws[2] = { width, height }; 80 | //size_t local_ws[2] = { 16, 12 }; 81 | 82 | #ifdef PROFILING 83 | LARGE_INTEGER g_PerfFrequency; 84 | LARGE_INTEGER g_PerformanceCountNDRangeStart; 85 | LARGE_INTEGER g_PerformanceCountNDRangeStop; 86 | QueryPerformanceFrequency(&g_PerfFrequency); 87 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStart); 88 | #endif 89 | 90 | err = clEnqueueNDRangeKernel(GpuManager::queue, find_corresp, 2, 0, global_ws, NULL, 0, NULL, NULL); 91 | clFinish(GpuManager::queue); 92 | 93 | #ifdef PROFILING 94 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStop); 95 | float seconds = (float)((g_PerformanceCountNDRangeStop.QuadPart - g_PerformanceCountNDRangeStart.QuadPart)/(float)g_PerfFrequency.QuadPart); 96 | #endif 97 | 98 | assert(err == CL_SUCCESS); 99 | 100 | if(MiroirManager::getShowMode() == ShowMode::CORRESP) 101 | { 102 | clEnqueueReadBuffer(GpuManager::queue, counter, CL_TRUE, 0, sizeof(cl_uint), &corresp_count, 0, NULL, NULL); 103 | corresp_h = new cl_float2[NPIX]; 104 | clEnqueueReadBuffer(GpuManager::queue, corresp_d, CL_TRUE, 0, sizeof(cl_float2)*NPIX, corresp_h, 0, NULL, NULL); 105 | } 106 | else 107 | corresp_h = NULL; 108 | 109 | clReleaseMemObject(cam_pose_inv_d); 110 | } 111 | 112 | #define WORK_GROUP_SIZE 64 113 | #define NUM_WORK_GROUP NPIX /2 / WORK_GROUP_SIZE 114 | #define A_SIZE 21 115 | #define B_SIZE 6 116 | 117 | // TODO fix memory leak here 118 | void TrackingModule::sumLinearSystem(cl_float A_h[36], cl_float B_h[6], const cl_float4 est_pose[4]) 119 | { 120 | cl_int err; 121 | 122 | // step 1 : calculate the summands and get first intermediate sums 123 | cl_mem A_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_WRITE, sizeof(cl_float)*NUM_WORK_GROUP*A_SIZE, NULL, &err); 124 | assert(err == CL_SUCCESS); 125 | cl_mem B_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_WRITE, sizeof(cl_float)*NUM_WORK_GROUP*B_SIZE, NULL, &err); 126 | assert(err == CL_SUCCESS); 127 | cl_mem est_pose_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_float4)*4, (void*)est_pose, &err); 128 | assert(err == CL_SUCCESS); 129 | 130 | cl_mem width_d = MiroirManager::getCLWidth(); 131 | cl_mem height_d = MiroirManager::getCLHeight(); 132 | cl_mem fl_d = MiroirManager::getCLFocalLength(); 133 | 134 | cl_uint j = 0; 135 | err = clSetKernelArg(compute_block_sum, j++, sizeof(cl_mem), (void*)&width_d); 136 | err |= clSetKernelArg(compute_block_sum, j++, sizeof(cl_mem), (void*)&est_pose_d); 137 | err |= clSetKernelArg(compute_block_sum, j++, sizeof(cl_mem), (void*)&corresp_d); 138 | err |= clSetKernelArg(compute_block_sum, j++, sizeof(cl_mem), (void*)&vmap_sensor); 139 | err |= clSetKernelArg(compute_block_sum, j++, sizeof(cl_mem), (void*)&vmap_raycast); 140 | err |= clSetKernelArg(compute_block_sum, j++, sizeof(cl_mem), (void*)&nmap_raycast); 141 | err |= clSetKernelArg(compute_block_sum, j++, sizeof(cl_mem), (void*)&A_d); 142 | err |= clSetKernelArg(compute_block_sum, j++, sizeof(cl_mem), (void*)&B_d); 143 | assert(err == CL_SUCCESS); 144 | 145 | int width, height; 146 | MiroirManager::getSensorDimensions(width, height); 147 | 148 | size_t global_ws[1] = { width * height / 2 }; // each thread sums 2 elements 149 | size_t local_ws[1] = { WORK_GROUP_SIZE }; 150 | 151 | #ifdef PROFILING 152 | LARGE_INTEGER g_PerfFrequency; 153 | LARGE_INTEGER g_PerformanceCountNDRangeStart; 154 | LARGE_INTEGER g_PerformanceCountNDRangeStop; 155 | QueryPerformanceFrequency(&g_PerfFrequency); 156 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStart); 157 | #endif 158 | err = clEnqueueNDRangeKernel(GpuManager::queue, compute_block_sum, 1, 0, global_ws, local_ws, 0, NULL, NULL); 159 | clFinish(GpuManager::queue); 160 | #ifdef PROFILING 161 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStop); 162 | float seconds = (float)((g_PerformanceCountNDRangeStop.QuadPart - g_PerformanceCountNDRangeStart.QuadPart)/(float)g_PerfFrequency.QuadPart); 163 | #endif 164 | 165 | assert(err == CL_SUCCESS); 166 | 167 | // cl_float3* test = new cl_float3[307200]; 168 | // cl_float* test = new cl_float[NUM_WORK_GROUP*B_SIZE]; 169 | // clEnqueueReadBuffer(GpuManager::queue, B_d, CL_TRUE, 0, sizeof(cl_float)*NUM_WORK_GROUP*B_SIZE, test, 0, NULL, NULL); 170 | // clEnqueueReadBuffer(GpuManager::queue, vmap_raycast, CL_TRUE, 0, sizeof(cl_float3)*307200, test, 0, NULL, NULL); 171 | 172 | clFinish(GpuManager::queue); 173 | 174 | // step 2 : sum intermediate sums 175 | j = 0; 176 | int size = (float)global_ws[0] / (float)local_ws[0] ; 177 | int nblocks = std::ceil( size / 2.0f / (float)WORK_GROUP_SIZE); 178 | int nitems = nblocks * WORK_GROUP_SIZE; 179 | cl_mem A2_d = clCreateBuffer(GpuManager::context, CL_MEM_WRITE_ONLY, sizeof(cl_float) * nblocks * A_SIZE, NULL, &err); 180 | assert(err == CL_SUCCESS); 181 | cl_mem B2_d = clCreateBuffer(GpuManager::context, CL_MEM_WRITE_ONLY, sizeof(cl_float) * nblocks * B_SIZE, NULL, &err); 182 | assert(err == CL_SUCCESS); 183 | cl_mem size_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_int), &size, &err); 184 | assert(err == CL_SUCCESS); 185 | 186 | err = clSetKernelArg(compute_sum, j++, sizeof(cl_mem), (void*)&size_d); 187 | err |= clSetKernelArg(compute_sum, j++, sizeof(cl_mem), (void*)&A_d); 188 | err |= clSetKernelArg(compute_sum, j++, sizeof(cl_mem), (void*)&B_d); 189 | err |= clSetKernelArg(compute_sum, j++, sizeof(cl_mem), (void*)&A2_d); 190 | err |= clSetKernelArg(compute_sum, j++, sizeof(cl_mem), (void*)&B2_d); 191 | assert(err == CL_SUCCESS); 192 | 193 | global_ws[0] = nitems; 194 | local_ws[0] = WORK_GROUP_SIZE; 195 | 196 | #ifdef PROFILING 197 | QueryPerformanceFrequency(&g_PerfFrequency); 198 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStart); 199 | #endif 200 | err = clEnqueueNDRangeKernel(GpuManager::queue, compute_sum, 1, 0, global_ws, local_ws, 0, NULL, NULL); 201 | clFinish(GpuManager::queue); 202 | #ifdef PROFILING 203 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStop); 204 | seconds = (float)((g_PerformanceCountNDRangeStop.QuadPart - g_PerformanceCountNDRangeStart.QuadPart)/(float)g_PerfFrequency.QuadPart); 205 | #endif 206 | 207 | assert(err == CL_SUCCESS); 208 | 209 | cl_float* ABlocks = new cl_float[nblocks*A_SIZE]; 210 | cl_float* BBlocks = new cl_float[nblocks*B_SIZE]; 211 | clEnqueueReadBuffer(GpuManager::queue, A2_d, CL_TRUE, 0, sizeof(cl_float) * nblocks * A_SIZE, ABlocks, 0, NULL, NULL); 212 | clEnqueueReadBuffer(GpuManager::queue, B2_d, CL_TRUE, 0, sizeof(cl_float) * nblocks * B_SIZE, BBlocks, 0, NULL, NULL); 213 | 214 | clFinish(GpuManager::queue); 215 | 216 | // get final A and B 217 | int col = 0; 218 | int pad = 0; 219 | int buf = 0; 220 | for(int i = 0; i < nblocks * A_SIZE; i++) 221 | { 222 | A_h[i%A_SIZE+pad] += ABlocks[i]; 223 | if((i+1 - buf) % (6 - col) == 0){ col++; pad += col; buf = i+1; } 224 | if(col == 6){ col = 0; pad = 0; } 225 | } 226 | for(int i = 0; i < nblocks * B_SIZE; i++) 227 | B_h[i%B_SIZE] += BBlocks[i]; 228 | 229 | clReleaseMemObject(A_d); 230 | clReleaseMemObject(B_d); 231 | clReleaseMemObject(A2_d); 232 | clReleaseMemObject(B2_d); 233 | clReleaseMemObject(est_pose_d); 234 | clReleaseMemObject(size_d); 235 | delete [] ABlocks; 236 | delete [] BBlocks; 237 | } 238 | 239 | void TrackingModule::processData() 240 | { 241 | cl_float4* est_pose = new cl_float4[4]; 242 | cl_float4* inc_trans = new cl_float4[4]; 243 | cl_float4* pose_inv = new cl_float4[4]; 244 | cl_float4* cam_pose_prev = new cl_float4[4]; 245 | cl_float x[6]; 246 | 247 | est_pose[0] = cam_pose[0]; 248 | est_pose[1] = cam_pose[1]; 249 | est_pose[2] = cam_pose[2]; 250 | est_pose[3] = cam_pose[3]; 251 | 252 | MiroirUtil::invertCamTransform(cam_pose, pose_inv); 253 | MiroirUtil::mat4Mult(pose_inv, est_pose, inc_trans); 254 | 255 | // 1. for each pixel find vertex correspondences 256 | findCorrespondences(pose_inv); 257 | 258 | for(int i=0; i<15; i++) 259 | { 260 | 261 | // 2. sum matrix A and vector b of the linear system 262 | cl_float A_h[36], B_h[6]; 263 | cl_float decomp[36]; 264 | // init A_h and B_h 265 | for(int k = 0; k < 36; k++) 266 | decomp[k] = A_h[k] = 0.0f; 267 | for(int k = 0; k < 6; k++) 268 | B_h[k] = 0.0f; 269 | for(int k = 0; k < 6; k++) 270 | A_h[k*6+k] = 1.0f; 271 | 272 | sumLinearSystem(A_h, B_h, est_pose); 273 | 274 | for(int k = 0; k < 6; k++) 275 | { 276 | for(int j = k+1; j<6; j++) 277 | A_h[j * 6 + k] = A_h[k * 6 + j]; 278 | } 279 | 280 | // checking nullspace 281 | double det = MiroirUtil::determinant(A_h, 6); 282 | if(std::fabs(det) < 1e-15 || det != det) 283 | { 284 | // lost tracking 285 | break; 286 | } 287 | 288 | MiroirUtil::cholesky(6, A_h, decomp); 289 | 290 | // 3. find solution of linear system on CPU using Cholesky decomposition 291 | // L*y = b, y = trans(L)*x 292 | cl_float y[6]; 293 | for(int k = 0; k < 6; k++) 294 | x[k] = y[k] = 0; 295 | for(int k = 0; k < 6; k++) 296 | { 297 | y[k] = B_h[k] / decomp[k * 6 + k]; 298 | for(int j = 0; j < k; j++) 299 | y[k] -= decomp[k * 6 + j] * y[j] / decomp[k * 6 + k]; 300 | } 301 | for(int k = 5; k >= 0; k--) 302 | { 303 | x[k] = y[k] / decomp[k * 6 + k]; 304 | for(int j = 0; j < k; j++) 305 | x[k] -= decomp[j * 6 + k] * x[k] / decomp[k * 6 + k]; 306 | } 307 | 308 | if(std::sqrt(x[3]*x[3]+x[4]*x[4]+x[5]*x[5]) > 100) // lost tracking 309 | break; 310 | 311 | cam_pose_prev[0] = est_pose[0]; 312 | cam_pose_prev[1] = est_pose[1]; 313 | cam_pose_prev[2] = est_pose[2]; 314 | cam_pose_prev[3] = est_pose[3]; 315 | 316 | inc_trans[0].s[0] = inc_trans[1].s[1] = inc_trans[2].s[2] = 1.0f; 317 | inc_trans[0].s[1] = -x[2]; 318 | inc_trans[0].s[2] = x[1]; 319 | inc_trans[1].s[0] = x[2]; 320 | inc_trans[1].s[2] = -x[0]; 321 | inc_trans[2].s[0] = -x[1]; 322 | inc_trans[2].s[1] = x[0]; 323 | 324 | inc_trans[3].s[0] = x[3]; 325 | inc_trans[3].s[1] = x[4]; 326 | inc_trans[3].s[2] = x[5]; 327 | 328 | MiroirUtil::mat4Mult(inc_trans, cam_pose_prev, est_pose); 329 | } 330 | 331 | cam_pose[0] = est_pose[0]; 332 | cam_pose[1] = est_pose[1]; 333 | cam_pose[2] = est_pose[2]; 334 | cam_pose[3] = est_pose[3]; 335 | 336 | delete [] pose_inv; 337 | delete [] cam_pose_prev; 338 | 339 | if(corresp_h != NULL) 340 | { 341 | boost::mutex::scoped_lock lock(MiroirManager::getMutex()); 342 | cl_float3* vmap_raycast_h = new cl_float3[NPIX]; 343 | cl_float3* vmap_sensor_h = new cl_float3[NPIX]; 344 | clEnqueueReadBuffer(GpuManager::queue, vmap_raycast, CL_TRUE, 0, sizeof(cl_float3)*NPIX, vmap_raycast_h, 0, NULL, NULL); 345 | clEnqueueReadBuffer(GpuManager::queue, vmap_sensor, CL_TRUE, 0, sizeof(cl_float3)*NPIX, vmap_sensor_h, 0, NULL, NULL); 346 | clFinish(GpuManager::queue); 347 | 348 | pcl::OpenNIGrabber* grabber = static_cast(MiroirManager::getGrabber()); 349 | cloud = pcl::PointCloud::Ptr(new pcl::PointCloud ); 350 | cloud->height = grabber->getDevice()->getDepthOutputMode().nYRes; 351 | cloud->width = grabber->getDevice()->getDepthOutputMode().nXRes; 352 | cloud->is_dense = false; 353 | cloud->points.resize(corresp_count*2); 354 | 355 | int width, height; 356 | MiroirManager::getSensorDimensions(width, height); 357 | 358 | int count = 0; 359 | for(int i = 0; i < NPIX; i++) 360 | { 361 | // if is nan 362 | if(corresp_h[i].s[0] != corresp_h[i].s[0]) 363 | continue; 364 | 365 | pcl::PointXYZRGBA& pt = cloud->points[count++]; 366 | int idx = width * corresp_h[i].s[1] + corresp_h[i].s[0]; 367 | pt.x = 0.001 * vmap_raycast_h[idx].s[0]; 368 | pt.y = 0.001 * vmap_raycast_h[idx].s[1]; 369 | /*pt.x = 0.001 * i * VOLUME/RESX ; 370 | pt.y = 0.001 * j * VOLUME/RESY;*/ 371 | pt.z = 0.001 * vmap_raycast_h[idx].s[2]; 372 | pt.r = 127; 373 | pt.g = 0; 374 | pt.b = pt.g; 375 | 376 | pcl::PointXYZRGBA& pt2 = cloud->points[count++]; 377 | 378 | pt2.x = 0.001 * vmap_sensor_h[i].s[0]; 379 | pt2.y = 0.001 * vmap_sensor_h[i].s[1]; 380 | /*pt.x = 0.001 * i * VOLUME/RESX ; 381 | pt.y = 0.001 * j * VOLUME/RESY;*/ 382 | pt2.z = 0.001 * vmap_sensor_h[i].s[2]; 383 | pt2.r = 0; 384 | pt2.g = 0; 385 | pt2.b = 127; 386 | } 387 | 388 | delete [] corresp_h; 389 | delete [] vmap_raycast_h; 390 | delete [] vmap_sensor_h; 391 | } 392 | } -------------------------------------------------------------------------------- /src/update_reconstruction_module.cpp: -------------------------------------------------------------------------------- 1 | #include "definitions.h" 2 | #include "util.h" 3 | #include "update_reconstruction_module.h" 4 | #include "gpu_manager.h" 5 | #include "miroir_manager.h" 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | UpdateReconstructionModule::UpdateReconstructionModule() : current_frame(0), enable(true) 12 | { 13 | kernel = GpuManager::createKernel((std::string(KERNEL_PATH)+std::string("TSDF.cl")).c_str(), "TSDF"); 14 | 15 | // TSDF stored in GPU 16 | cl_int err; 17 | 18 | cl_short2* buf = new cl_short2[(int)(RESX*RESY*RESZ)]; 19 | 20 | for(int i=0; i::quiet_NaN(); 23 | buf[i].s[1] = -SHORT_MAX; 24 | } 25 | 26 | TSDF_d = clCreateBuffer(GpuManager::context, CL_MEM_USE_HOST_PTR | CL_MEM_READ_WRITE, sizeof(cl_short2)*RESX*RESY*RESZ, buf, &err); 27 | assert(err == CL_SUCCESS); 28 | clEnqueueWriteBuffer(GpuManager::queue, TSDF_d, true, 0, sizeof(cl_short2)*RESX*RESY*RESZ, buf, 0, NULL, NULL); 29 | 30 | delete [] buf; 31 | 32 | cam_pose = new cl_float4[4]; 33 | } 34 | 35 | UpdateReconstructionModule::~UpdateReconstructionModule() 36 | { 37 | clReleaseMemObject(TSDF_d); 38 | clReleaseKernel(kernel); 39 | } 40 | 41 | //#define PROFILING 42 | 43 | void UpdateReconstructionModule::TSDF() 44 | { 45 | cl_int err; 46 | 47 | cl_float4 cam_pos_inv_h[4];// = {*(cam_pos_inv[0]), *(cam_pos_inv[1]), *(cam_pos_inv[2]), *(cam_pos_inv[3])}; 48 | 49 | // transpose rotation matrix to get inverse 50 | MiroirUtil::invertCamTransform(cam_pose, cam_pos_inv_h); 51 | 52 | //cl_mem cam_pos_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_float4)*4, cam_pose, &err); 53 | //assert(err == CL_SUCCESS); 54 | cl_mem cam_pos_inv_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_float4)*4, cam_pos_inv_h, &err); 55 | assert(err == CL_SUCCESS); 56 | cl_mem k_d = clCreateBuffer(GpuManager::context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(cl_ulong), ¤t_frame, &err); 57 | assert(err == CL_SUCCESS); 58 | 59 | cl_mem width_d = MiroirManager::getCLWidth(); 60 | cl_mem height_d = MiroirManager::getCLHeight(); 61 | cl_mem fl_d = MiroirManager::getCLFocalLength(); 62 | cl_mem tsdf_params = MiroirManager::getCLTSDFParams(); 63 | 64 | cl_uint j = 0; 65 | err = clSetKernelArg(kernel, j++, sizeof(TSDF_d), (void*)&TSDF_d); 66 | err |= clSetKernelArg(kernel, j++, sizeof(width_d), (void*)&width_d); 67 | err |= clSetKernelArg(kernel, j++, sizeof(height_d), (void*)&height_d); 68 | //err |= clSetKernelArg(kernel, j++, sizeof(cam_pos_d), (void*)&cam_pos_d); 69 | err |= clSetKernelArg(kernel, j++, sizeof(cam_pos_inv_d), (void*)&cam_pos_inv_d); 70 | err |= clSetKernelArg(kernel, j++, sizeof(dmap), (void*)&dmap); 71 | err |= clSetKernelArg(kernel, j++, sizeof(k_d), (void*)&k_d); 72 | err |= clSetKernelArg(kernel, j++, sizeof(fl_d), (void*)&fl_d); 73 | err |= clSetKernelArg(kernel, j++, sizeof(tsdf_params), (void*)&tsdf_params); 74 | 75 | assert(err == CL_SUCCESS); 76 | 77 | size_t global_ws[2] = { RESX, RESY }; 78 | 79 | #ifdef PROFILING 80 | LARGE_INTEGER g_PerfFrequency; 81 | LARGE_INTEGER g_PerformanceCountNDRangeStart; 82 | LARGE_INTEGER g_PerformanceCountNDRangeStop; 83 | QueryPerformanceFrequency(&g_PerfFrequency); 84 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStart); 85 | #endif 86 | 87 | err = clEnqueueNDRangeKernel(GpuManager::queue, kernel, 2, 0, global_ws, NULL, 0, NULL, NULL); 88 | clFinish(GpuManager::queue); 89 | 90 | #ifdef PROFILING 91 | QueryPerformanceCounter(&g_PerformanceCountNDRangeStop); 92 | float seconds = (float)((g_PerformanceCountNDRangeStop.QuadPart - g_PerformanceCountNDRangeStart.QuadPart)/(float)g_PerfFrequency.QuadPart); 93 | #endif 94 | 95 | assert(err == CL_SUCCESS); 96 | 97 | //clReleaseMemObject(cam_pos_d); 98 | err = clReleaseMemObject(cam_pos_inv_d); 99 | assert(err == CL_SUCCESS); 100 | err = clReleaseMemObject(k_d); 101 | assert(err == CL_SUCCESS); 102 | } 103 | 104 | void UpdateReconstructionModule::processData() 105 | { 106 | current_frame++; 107 | 108 | if(enable) 109 | TSDF(); 110 | 111 | /*UROut* packet = new UROut(TSDF_d, cam_pos_inv); 112 | out = DataPacket(packet);*/ 113 | 114 | cl_short flex = 1000; 115 | 116 | // count number of valid points in the tsdf 117 | if(MiroirManager::getShowMode() == ShowMode::TSDF) 118 | { 119 | cl_short2* tsdf_h = new cl_short2[(int)(RESX*RESY*RESZ)]; 120 | clEnqueueReadBuffer(GpuManager::queue, TSDF_d, CL_TRUE, 0, sizeof(cl_short2)*RESX*RESY*RESZ, tsdf_h, 0, NULL, NULL); 121 | 122 | //boost::mutex::scoped_lock lock(MiroirManager::getMutex()); 123 | int nPoints = 0; 124 | for(int i=0; i<(RESX*RESY*RESZ); i++) 125 | { 126 | cl_short tsdf = tsdf_h[i].s[0]; 127 | if(tsdf == SHORT_NAN || tsdf > flex || tsdf < -flex) 128 | { 129 | continue; 130 | } 131 | nPoints++; 132 | 133 | } 134 | 135 | pcl::OpenNIGrabber* grabber = static_cast(MiroirManager::getGrabber()); 136 | cloud = pcl::PointCloud::Ptr(new pcl::PointCloud ); 137 | cloud->height = grabber->getDevice()->getDepthOutputMode().nYRes; 138 | cloud->width = grabber->getDevice()->getDepthOutputMode().nXRes; 139 | cloud->is_dense = false; 140 | 141 | cloud->points.resize(nPoints); 142 | 143 | /*if (grabber->getDevice()->isDepthRegistered ()) 144 | cloud->header.frame_id = "/openni_rgb_optical_frame"; 145 | else 146 | cloud->header.frame_id = "/openni_depth_optical_frame";*/ 147 | 148 | int count = 0; 149 | 150 | for(int j=0; j= nPoints || tsdf_val == SHORT_NAN || tsdf_val > flex || tsdf_val < -flex ) 160 | { 161 | //pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); 162 | continue; 163 | } 164 | 165 | 166 | pcl::PointXYZRGBA& pt = cloud->points[count++]; 167 | pt.x = 0.001 * (i * VOLUME/RESX - VOLUME/2 ); 168 | pt.y = 0.001 * (j * VOLUME/RESY - VOLUME/2 ); 169 | /*pt.x = 0.001 * i * VOLUME/RESX ; 170 | pt.y = 0.001 * j * VOLUME/RESY;*/ 171 | pt.z = 0.001 * k * VOLUME/RESZ; 172 | pt.r = 255; 173 | pt.g = tsdf_val<0?0:255; 174 | pt.b = pt.g; 175 | } 176 | 177 | } 178 | } 179 | 180 | delete [] tsdf_h; 181 | 182 | cloud->sensor_origin_.setZero (); 183 | cloud->sensor_orientation_.w () = 0.0f; 184 | cloud->sensor_orientation_.x () = 1.0f; 185 | cloud->sensor_orientation_.y () = 0.0f; 186 | cloud->sensor_orientation_.z () = 0.0f; 187 | } 188 | /*else 189 | tsdf_h = NULL;*/ 190 | 191 | } --------------------------------------------------------------------------------