├── src ├── extern │ ├── .gitignore │ └── README.txt ├── util │ ├── img.png │ ├── timer.h │ ├── video_clip.cc │ ├── cuda_gl_interp.cc │ ├── debugger.h │ ├── block_visualizer.cc │ └── debugger.cc ├── mapping │ ├── update_convex.cu │ ├── update_convex.h │ ├── allocate.h │ ├── update_simple.h │ ├── update_bayesian.h │ ├── recycle.h │ ├── update_simple.cu │ └── allocate.cu ├── engine │ ├── localizing_engine.cc │ ├── localizing_engine.h │ ├── mapping_engine.cc │ ├── mapping_engine.h │ ├── logging_engine.h │ ├── main_engine.h │ └── visualizing_engine.h ├── visualization │ ├── colorize.h │ ├── compress_mesh.h │ ├── extract_bounding_box.h │ ├── bounding_box.h │ ├── trajectory.h │ ├── bounding_box.cu │ ├── color_util.h │ ├── compact_mesh.h │ ├── colorize.cu │ ├── extract_bounding_box.cu │ ├── ray_caster.h │ ├── trajectory.cu │ ├── compact_mesh.cu │ └── compress_mesh.cu ├── core │ ├── triangle.h │ ├── vertex.h │ ├── hash_entry.h │ ├── collect_block_array.h │ ├── block.h │ ├── entry_array.h │ ├── block_array.h │ ├── common.h │ ├── block_array.cu │ ├── entry_array.cu │ ├── params.h │ ├── voxel.h │ ├── mesh.h │ ├── collect_block_array.cu │ └── mesh.cu ├── io │ ├── mesh_writer.h │ ├── config_manager.h │ └── mesh_writer.cc ├── localizing │ ├── point_to_psdf.h │ └── point_to_psdf.cu ├── meshing │ └── marching_cubes.h ├── sensor │ ├── preprocess.h │ ├── rgbd_data_provider.h │ ├── rgbd_sensor.h │ ├── rgbd_data_provider.cc │ ├── preprocess.cu │ └── rgbd_sensor.cu ├── optimize │ ├── primal_dual.h │ ├── linear_equations.h │ ├── linear_equations.cu │ └── primal_dual.cu ├── README.md ├── geometry │ ├── isosurface_intersection.h │ ├── spatial_query.h │ ├── voxel_query.h │ └── geometry_helper.h └── app │ ├── reconstruction.cc │ ├── slam.cc │ ├── block_analysis.cc │ └── orb_slam2.cc ├── config ├── lights.yaml ├── args.yml ├── ICL.yml ├── PKU.yml ├── SUN3D.yml ├── TUM2.yml ├── TUM1.yml ├── TUM3.yml ├── SUN3D_ORIGINAL.yml └── ORB │ ├── TUM3.yaml │ ├── ICL.yaml │ ├── SUN3D.yaml │ ├── PKU.yaml │ ├── SUN3D_ORIGINAL.yaml │ ├── TUM1.yaml │ └── TUM2.yaml ├── README.md ├── .gitignore ├── cmake_modules ├── FindGLEW.cmake ├── FindGLFW3.cmake ├── FindGlog.cmake └── FindEigen3.cmake └── CMakeLists.txt /src/extern/.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | opengl-wrapper/ 3 | orb_slam2/ -------------------------------------------------------------------------------- /src/util/img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cafe/MeshHashing/master/src/util/img.png -------------------------------------------------------------------------------- /src/mapping/update_convex.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-29. 3 | // 4 | 5 | #include "update_convex.h" 6 | -------------------------------------------------------------------------------- /src/engine/localizing_engine.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-12-23. 3 | // 4 | 5 | #include "localizing_engine.h" 6 | -------------------------------------------------------------------------------- /src/visualization/colorize.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-25. 3 | // 4 | 5 | #ifndef MESH_HASHING_POSTPRESS_H 6 | #define MESH_HASHING_POSTPRESS_H 7 | 8 | 9 | #endif //MESH_HASHING_POSTPRESS_H 10 | -------------------------------------------------------------------------------- /src/mapping/update_convex.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-29. 3 | // 4 | 5 | #ifndef MESH_HASHING_UPDATE_CONVEX_H 6 | #define MESH_HASHING_UPDATE_CONVEX_H 7 | 8 | 9 | #endif //MESH_HASHING_UPDATE_CONVEX_H 10 | -------------------------------------------------------------------------------- /config/lights.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | positions: 4 | - {x: 0.0, y: -2.0, z: 0.0} 5 | - {x: 4.0, y: -2.0, z: 0.0} 6 | 7 | ambient_coeff: {r: 0.0, g: 0.0, b: 0.0} 8 | specular_coeff: {r: 0.0, g: 0.0, b: 0.0} 9 | # Khaki 10 | diffuse_color: {r: 0.94, g: 0.90, b: 0.54} 11 | light_color: {r: 1.0, g: 1.0, b: 1.0} 12 | power: 7.0 -------------------------------------------------------------------------------- /src/extern/README.txt: -------------------------------------------------------------------------------- 1 | For easier maintainence, opengl functions are separately hosted in another repository. 2 | Please clone and modify the repository in the 'src' directory. 3 | https://theNded@bitbucket.org/theNded/opengl-wrapper.git 4 | 5 | Also, ORB_SLAM2 could be integrated 6 | https://theNded@bitbucket.org/theNded/orb_slam2.git -------------------------------------------------------------------------------- /src/engine/localizing_engine.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-12-23. 3 | // 4 | 5 | #ifndef MESH_HASHING_LOCALIZING_ENGINE_H 6 | #define MESH_HASHING_LOCALIZING_ENGINE_H 7 | 8 | #include 9 | 10 | class LocalizingEngine { 11 | public: 12 | 13 | private: 14 | 15 | }; 16 | 17 | #endif //MESH_HASHING_LOCALIZING_ENGINE_H 18 | -------------------------------------------------------------------------------- /src/core/triangle.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #ifndef CORE_TRIANGLE_H 6 | #define CORE_TRIANGLE_H 7 | 8 | #include "core/common.h" 9 | #include "helper_math.h" 10 | 11 | struct __ALIGN__(4) Triangle { 12 | int3 vertex_ptrs; 13 | 14 | __device__ 15 | void Clear() { 16 | vertex_ptrs = make_int3(-1, -1, -1); 17 | } 18 | }; 19 | 20 | #endif //MESH_HASHING_TRIANGLE_H 21 | -------------------------------------------------------------------------------- /src/io/mesh_writer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #ifndef MESH_HASHING_MESH_WRITER_H 6 | #define MESH_HASHING_MESH_WRITER_H 7 | 8 | #include 9 | #include "core/common.h" 10 | #include "visualization/compact_mesh.h" 11 | 12 | void SaveObj(CompactMesh& compact_mesh, std::string path); 13 | void SavePly(CompactMesh& compact_mesh, std::string path); 14 | 15 | #endif //MESH_HASHING_MESH_WRITER_H 16 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | MeshHashing 2 | ------ 3 | Code (partly) for our 4 | - ICRA paper (**Accepted**): *An Efficient Volumetric Mesh Representation for 5 | Real-time Scene Reconstruction using Spatial-hashing* 6 | - ECCV paper (**Accepted**): *PSDF Fusion: Probabilistic Signed Distance Function for On-the-fly 3D Data Fusion and Scene Reconstruction* 7 | 8 | **This project is deprecated. It is now being migrated to Open3D with CUDA components. Please refer to [this fork of Open3D](https://github.com/theNded/Open3D/tree/cuda).** 9 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | # CLion 35 | cmake-build-release/ 36 | cmake-build-debug/ 37 | bin/ 38 | lib/ 39 | .idea/ 40 | result/ 41 | -------------------------------------------------------------------------------- /src/visualization/compress_mesh.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #ifndef MESH_HASHING_COMPRESS_MESH_H 6 | #define MESH_HASHING_COMPRESS_MESH_H 7 | 8 | #include "core/common.h" 9 | #include "core/entry_array.h" 10 | #include "core/block_array.h" 11 | #include "core/mesh.h" 12 | #include "visualization/compact_mesh.h" 13 | 14 | void CompressMesh(EntryArray& candidate_entries, BlockArray& blocks, 15 | Mesh& mesh, 16 | CompactMesh & compact_mesh, int3& stats); 17 | 18 | #endif //MESH_HASHING_COMPRESS_MESH_H 19 | -------------------------------------------------------------------------------- /src/core/vertex.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #ifndef CORE_VERTEX_H 6 | #define CORE_VERTEX_H 7 | 8 | #include "core/common.h" 9 | #include "helper_math.h" 10 | 11 | struct __ALIGN__(4) Vertex { 12 | float3 pos; 13 | float3 normal; 14 | float3 color; 15 | float radius; 16 | int ref_count; 17 | 18 | __device__ 19 | void Clear() { 20 | pos = make_float3(0.0); 21 | normal = make_float3(0.0); 22 | color = make_float3(0); 23 | radius = 0; 24 | ref_count = 0; 25 | } 26 | }; 27 | 28 | #endif //MESH_HASHING_VERTEX_H 29 | -------------------------------------------------------------------------------- /src/util/timer.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-7-4. 3 | // 4 | 5 | #ifndef VOXEL_HASHING_TIMER_H 6 | #define VOXEL_HASHING_TIMER_H 7 | 8 | #include 9 | 10 | class Timer { 11 | private: 12 | std::chrono::time_point start_, end_; 13 | 14 | public: 15 | void Tick() { 16 | start_ = std::chrono::system_clock::now(); 17 | } 18 | double Tock() { 19 | end_ = std::chrono::system_clock::now(); 20 | std::chrono::duration seconds = end_ - start_; 21 | return seconds.count(); 22 | } 23 | }; 24 | #endif //VOXEL_HASHING_TIMER_H 25 | -------------------------------------------------------------------------------- /src/mapping/allocate.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #ifndef MESH_HASHING_ALLOCATE_H 6 | #define MESH_HASHING_ALLOCATE_H 7 | 8 | #include "core/hash_table.h" 9 | #include "geometry/geometry_helper.h" 10 | #include "sensor/rgbd_sensor.h" 11 | 12 | // @function 13 | // See what entries of @param hash_table 14 | // was affected by @param sensor 15 | // with the help of @param geometry_helper 16 | double AllocBlockArray( 17 | HashTable& hash_table, 18 | Sensor& sensor, 19 | GeometryHelper& geometry_helper 20 | ); 21 | 22 | #endif //MESH_HASHING_ALLOCATE_H 23 | -------------------------------------------------------------------------------- /src/visualization/extract_bounding_box.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-23. 3 | // 4 | 5 | #ifndef MESH_HASHING_EXTRACT_BOUNDING_BOX_H 6 | #define MESH_HASHING_EXTRACT_BOUNDING_BOX_H 7 | 8 | #include "core/common.h" 9 | #include "core/entry_array.h" 10 | 11 | #include "visualization/bounding_box.h" 12 | #include "visualization/extract_bounding_box.h" 13 | #include "geometry/geometry_helper.h" 14 | 15 | void ExtractBoundingBox(EntryArray& candidate_entries, 16 | BoundingBox& bounding_box, 17 | GeometryHelper& geometry_helper); 18 | 19 | #endif //MESH_HASHING_EXTRACT_BOUNDING_BOX_H 20 | -------------------------------------------------------------------------------- /config/args.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # 0 - ICL 3 | # 1~3 - TUM1~3 4 | # 4 - SUN3D 5 | # 5 - SUN3D_ORIGINAL 6 | # 6 - PKU 7 | dataset_type: 4 8 | 9 | # ----------- 10 | enable_bayesian_update: 1 11 | 12 | enable_sdf_gradient: 1 13 | enable_polygon_mode: 0 14 | 15 | enable_color: 0 16 | enable_navigation: 1 17 | enable_global_mesh: 1 18 | enable_bounding_box: 0 19 | enable_trajectory: 0 20 | enable_ray_casting: 0 21 | 22 | enable_video_recording: 1 23 | enable_ply_saving: 1 24 | 25 | filename_prefix: "burghers" 26 | time_profile: "tum3-nhm" 27 | memo_profile: "tum3-nhm" 28 | 29 | # Others 30 | run_frames: -1 -------------------------------------------------------------------------------- /src/localizing/point_to_psdf.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-12-23. 3 | // 4 | 5 | #ifndef MESH_HASHING_POINT_TO_PSDF_H 6 | #define MESH_HASHING_POINT_TO_PSDF_H 7 | 8 | #include "core/common.h" 9 | #include "core/hash_table.h" 10 | #include "core/mesh.h" 11 | #include "core/entry_array.h" 12 | #include "core/block_array.h" 13 | #include "sensor/rgbd_sensor.h" 14 | #include "geometry/geometry_helper.h" 15 | 16 | float PointToSurface( 17 | BlockArray &blocks, 18 | Sensor &sensor, 19 | HashTable &hash_table, 20 | GeometryHelper &geometry_helper, 21 | mat6x6 &A, 22 | mat6x1 &b, 23 | int& count); 24 | #endif //MESH_HASHING_POINT_TO_PSDF_H 25 | -------------------------------------------------------------------------------- /src/meshing/marching_cubes.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #ifndef MESH_HASHING_MARCHING_CUBES_H 6 | #define MESH_HASHING_MARCHING_CUBES_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include "mc_tables.h" 14 | #include "util/timer.h" 15 | #include "engine/main_engine.h" 16 | #include "core/collect_block_array.h" 17 | 18 | float MarchingCubes( 19 | EntryArray& candidate_entries, 20 | BlockArray& blocks, 21 | Mesh& mesh, 22 | HashTable& hash_table, 23 | GeometryHelper& geometry_helper, 24 | bool enable_sdf_gradient); 25 | #endif //MESH_HASHING_MARCHING_CUBES_H 26 | -------------------------------------------------------------------------------- /src/sensor/preprocess.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-25. 3 | // 4 | 5 | #ifndef MESH_HASHING_PREPROCESS_H 6 | #define MESH_HASHING_PREPROCESS_H 7 | 8 | #include 9 | #include 10 | #include "core/params.h" 11 | 12 | __host__ 13 | void ResetInlierRatio( 14 | float* inlier_ratio, 15 | SensorParams& params 16 | ); 17 | 18 | __host__ 19 | void ConvertDepthFormat( 20 | cv::Mat& depth_img, 21 | short* depth_buffer, 22 | float* depth_data, 23 | SensorParams& params 24 | ); 25 | 26 | __host__ 27 | void ConvertColorFormat( 28 | cv::Mat &color_img, 29 | uchar4* color_buffer, 30 | float4* color_data, 31 | SensorParams& params 32 | ); 33 | 34 | 35 | 36 | #endif //MESH_HASHING_PREPROCESS_H 37 | -------------------------------------------------------------------------------- /src/visualization/bounding_box.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #ifndef MESH_HASHING_BOUNDING_BOX_H 6 | #define MESH_HASHING_BOUNDING_BOX_H 7 | 8 | class BoundingBox { 9 | public: 10 | BoundingBox() = default; 11 | //~BoundingBox(); 12 | void Alloc(int max_vertex_count); 13 | void Free(); 14 | 15 | uint vertex_count(); 16 | void Resize(int max_vertex_count); 17 | void Reset(); 18 | 19 | __device__ 20 | float3* vertices() { 21 | return vertices_; 22 | } 23 | __device__ 24 | uint* vertex_counter() { 25 | return vertex_counter_; 26 | } 27 | private: 28 | bool is_allocated_on_gpu_ = false; 29 | 30 | float3* vertices_; 31 | uint* vertex_counter_; 32 | 33 | int max_vertex_count_; 34 | }; 35 | 36 | #endif //MESH_HASHING_BOUNDING_BOX_H 37 | -------------------------------------------------------------------------------- /src/optimize/primal_dual.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-26. 3 | // 4 | 5 | #ifndef MESH_HASHING_PRIMAL_DUAL_H 6 | #define MESH_HASHING_PRIMAL_DUAL_H 7 | 8 | #include "core/common.h" 9 | #include "core/block_array.h" 10 | #include "core/entry_array.h" 11 | #include "core/hash_table.h" 12 | #include "geometry/geometry_helper.h" 13 | 14 | void PrimalDualInit( 15 | EntryArray& candidate_entries, 16 | BlockArray& blocks, 17 | HashTable& hash_table, 18 | GeometryHelper& geometry_helper 19 | ); 20 | 21 | void PrimalDualIterate( 22 | EntryArray& candidate_entries, 23 | BlockArray& blocks, 24 | HashTable& hash_table, 25 | GeometryHelper& geometry_helper, 26 | const float lambda, 27 | const float sigma, 28 | const float tau 29 | ); 30 | #endif //MESH_HASHING_PRIMAL_DUAL_H 31 | -------------------------------------------------------------------------------- /src/core/hash_entry.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #ifndef CORE_HASH_ENTRY_H 6 | #define CORE_HASH_ENTRY_H 7 | 8 | #include "core/common.h" 9 | #include "helper_math.h" 10 | 11 | struct __ALIGN__(8) HashEntry { 12 | int3 pos; // block position (lower left corner of SDFBlock)) 13 | int ptr; // pointer into heap to SDFBlock 14 | uint offset; // offset for linked lists 15 | 16 | __device__ 17 | void operator=(const struct HashEntry& e) { 18 | ((long long*)this)[0] = ((const long long*)&e)[0]; 19 | ((long long*)this)[1] = ((const long long*)&e)[1]; 20 | ((int*)this)[4] = ((const int*)&e)[4]; 21 | } 22 | 23 | __device__ 24 | void Clear() { 25 | pos = make_int3(0); 26 | ptr = FREE_ENTRY; 27 | offset = 0; 28 | } 29 | }; 30 | 31 | #endif //MESH_HASHING_HASH_ENTRY_H 32 | -------------------------------------------------------------------------------- /src/mapping/update_simple.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #ifndef MESH_HASHING_FUSE_H 6 | #define MESH_HASHING_FUSE_H 7 | 8 | #include "core/hash_table.h" 9 | #include "core/block_array.h" 10 | #include "core/entry_array.h" 11 | #include "core/mesh.h" 12 | #include "sensor/rgbd_sensor.h" 13 | #include "geometry/geometry_helper.h" 14 | 15 | // @function 16 | // Enumerate @param candidate_entries 17 | // change the value of @param blocks 18 | // according to the existing @param mesh 19 | // and input @param sensor data 20 | // with the help of hash_table and geometry_helper 21 | double UpdateBlocksSimple( 22 | EntryArray& candidate_entries, 23 | BlockArray& blocks, 24 | Sensor& sensor, 25 | HashTable& hash_table, 26 | GeometryHelper& geometry_helper 27 | ); 28 | 29 | #endif //MESH_HASHING_FUSE_H 30 | -------------------------------------------------------------------------------- /src/engine/mapping_engine.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-26. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "mapping_engine.h" 11 | #include "optimize/primal_dual.h" 12 | 13 | MappingEngine::MappingEngine( 14 | int sensor_width, 15 | int sensor_height, 16 | bool enable_bayesian_update 17 | ) { 18 | Init(sensor_width, sensor_height, enable_bayesian_update); 19 | } 20 | 21 | void MappingEngine::Init( 22 | int sensor_width, int sensor_height, 23 | bool enable_bayesian_update 24 | ) { 25 | enable_bayesian_update_ = enable_bayesian_update; 26 | if (enable_bayesian_update) { 27 | linear_equations_.Alloc(sensor_width, sensor_height); 28 | } 29 | } 30 | 31 | MappingEngine::~MappingEngine() { 32 | linear_equations_.Free(); 33 | } -------------------------------------------------------------------------------- /src/visualization/trajectory.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-24. 3 | // 4 | 5 | #ifndef MESH_HASHING_TRAJECTORY_H 6 | #define MESH_HASHING_TRAJECTORY_H 7 | 8 | #include 9 | #include "helper_math.h" 10 | #include "matrix.h" 11 | 12 | class Trajectory { 13 | public: 14 | Trajectory() = default; 15 | explicit Trajectory(uint max_vertex_count); 16 | void Alloc(uint max_vertex_count); 17 | void Free(); 18 | 19 | void AddPose(float4x4 wTc); 20 | std::vector MakeFrustum(float4x4 wTc); 21 | 22 | uint vertex_count() { 23 | return vertex_count_ + frustum_.size(); 24 | } 25 | float3* vertices() { 26 | return vertices_; 27 | } 28 | 29 | private: 30 | bool is_allocated_on_gpu_ = false; 31 | uint max_vertex_count_; 32 | int vertex_count_ = -1; 33 | float3 prev_position_; 34 | float3* vertices_; 35 | std::vector frustum_; 36 | }; 37 | 38 | 39 | #endif //MESH_HASHING_TRAJECTORY_H 40 | -------------------------------------------------------------------------------- /src/engine/mapping_engine.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-26. 3 | // 4 | 5 | #ifndef MESH_HASHING_MAPPING_ENGINE_H 6 | #define MESH_HASHING_MAPPING_ENGINE_H 7 | 8 | #include "optimize/linear_equations.h" 9 | #include "helper_math.h" 10 | 11 | class MappingEngine { 12 | public: 13 | MappingEngine() = default; 14 | void Init( 15 | int sensor_width, 16 | int sensor_height, 17 | bool enable_bayesian_update 18 | ); 19 | MappingEngine( 20 | int sensor_width, 21 | int sensor_height, 22 | bool enable_bayesian_update 23 | ); 24 | ~MappingEngine(); 25 | 26 | 27 | bool enable_bayesian_update() { 28 | return enable_bayesian_update_; 29 | } 30 | SensorLinearEquations& linear_equations() { 31 | return linear_equations_; 32 | } 33 | private: 34 | bool enable_bayesian_update_ = false; 35 | SensorLinearEquations linear_equations_; 36 | }; 37 | 38 | 39 | #endif //MESH_HASHING_MAPPING_ENGINE_H 40 | -------------------------------------------------------------------------------- /src/util/video_clip.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-7-11. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | int main() { 10 | cv::VideoCapture reader; 11 | reader.open("/home/wei/Documents/reconstruction/lounge-shading.avi"); 12 | 13 | cv::Mat frame; 14 | int i = 0; 15 | while (1) { 16 | ++i; 17 | //std::cout << i++ << std::endl; 18 | reader >> frame; 19 | 20 | // if (i == 100 || i == 400 || i == 800 || i == 1400 || i == 2450) { 21 | // std::stringstream ss; 22 | // ss << "tum-color-" << i << ".png"; 23 | // cv::imwrite(ss.str(), frame); 24 | // } 25 | cv::imshow("video", frame); 26 | int key = cv::waitKey(10); 27 | if (key == 27) break; 28 | if (key == 32 || i == 242 || i == 921 || i == 1939 || i == 2951) { 29 | std::stringstream ss; 30 | ss << "lounge-all-" << i << ".png"; 31 | cv::imwrite(ss.str(), frame); 32 | } 33 | } 34 | } -------------------------------------------------------------------------------- /cmake_modules/FindGLEW.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find GLEW 2 | # Once done, this will define 3 | # 4 | # GLEW_FOUND - system has GLEW 5 | # GLEW_INCLUDE_DIR - the GLEW include directory 6 | # GLEW_LIBRARY - the GLEW library 7 | 8 | 9 | # Use PkgConfig if available to get hints about glew location 10 | find_package(PkgConfig) 11 | if (PKG_CONFIG_FOUND) 12 | pkg_check_modules(PKG_GLEW QUIET glew) 13 | endif() 14 | 15 | # Search for library objects in known paths and optional hint from pkg-config 16 | find_library(GLEW_LIBRARY 17 | NAMES GLEW 18 | HINTS ${PKG_GLEW_LIBRARY_DIRS} ${PKG_GLEW_LIBDIR} 19 | ) 20 | 21 | # Search for header files in known paths and optional hint from pkg-config 22 | find_path(GLEW_INCLUDE_DIR 23 | GL/glew.h 24 | HINTS ${PKG_GLEW_INCLUDE_DIRS} ${PKG_GLEW_INCLUDEDIR} 25 | ) 26 | 27 | include(FindPackageHandleStandardArgs) 28 | find_package_handle_standard_args(GLEW REQUIRED_VARS GLEW_LIBRARY GLEW_INCLUDE_DIR) 29 | mark_as_advanced(GLEW_LIBRARY GLEW_INCLUDE_DIR) -------------------------------------------------------------------------------- /src/core/collect_block_array.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #ifndef CORE_COLLECT_H 6 | #define CORE_COLLECT_H 7 | 8 | #include "core/entry_array.h" 9 | #include "core/hash_table.h" 10 | #include "sensor/rgbd_sensor.h" 11 | #include "geometry/geometry_helper.h" 12 | 13 | // @function 14 | // Read the entries in @param hash_table 15 | // Write to the @param candidate_entries (for parallel computation) 16 | void CollectAllBlocks( 17 | HashTable &hash_table, 18 | EntryArray &candidate_entries 19 | ); 20 | 21 | // @function 22 | // Read the entries in @param hash_table 23 | // Filter the positions with @param sensor info (pose and params), 24 | // and @param geometry helper 25 | // Write to the @param candidate_entries (for parallel computation) 26 | double CollectBlocksInFrustum( 27 | HashTable &hash_table, 28 | Sensor &sensor, 29 | GeometryHelper &geometry_helper, 30 | EntryArray &candidate_entries 31 | ); 32 | 33 | #endif //CORE_COLLECT_H 34 | -------------------------------------------------------------------------------- /config/ICL.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | dataset_path: "/home/wei/Work/data/ICL/office1/" 3 | 4 | bucket_count: 40000 5 | bucket_size: 10 6 | count: 400000 7 | linked_list_size: 7 8 | value_capacity: 100000 9 | 10 | # SDF params 11 | voxel_size: 0.008 12 | sdf_upper_bound: 4.0 13 | truncation_distance_scale: 0.008 14 | truncation_distance: 0.012 15 | weight_sample: 10 16 | weight_upper_bound: 255 17 | 18 | # Sensor params 19 | fx: 481.2 20 | fy: 480 21 | cx: 319.5 22 | cy: 239.5 23 | min_depth_range: 0.5 24 | max_depth_range: 4.0 25 | range_factor: 0.0002 26 | height: 480 27 | width: 640 28 | 29 | # Raycaster params 30 | min_raycast_depth: 0.5 31 | max_raycast_depth: 5.5 32 | raycast_step: 0.016 33 | sample_sdf_threshold: 0.808 34 | sdf_threshold: 0.800 35 | enable_gradient: 1 36 | 37 | max_vertex_count: 8000000 38 | max_triangle_count: 8000000 39 | -------------------------------------------------------------------------------- /src/sensor/rgbd_data_provider.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #ifndef MESH_HASHING_RGBD_LOCAL_SEQUENCE_H 6 | #define MESH_HASHING_RGBD_LOCAL_SEQUENCE_H 7 | 8 | #include "io/config_manager.h" 9 | 10 | struct RGBDDataProvider { 11 | /// Read from Disk 12 | size_t frame_id = 0; 13 | std::vector depth_image_list; 14 | std::vector color_image_list; 15 | std::vector wTcs; 16 | 17 | void LoadDataset(DatasetType dataset_type); 18 | 19 | void LoadDataset(std::string dataset_path, 20 | DatasetType dataset_type); 21 | void LoadDataset(Dataset dataset); 22 | 23 | /// If read from disk, then provide mat at frame_id 24 | /// If read from network/USB, then wait until a mat comes; 25 | /// a while loop might be inside 26 | bool ProvideData(cv::Mat &depth, cv::Mat &color); 27 | bool ProvideData(cv::Mat &depth, cv::Mat &color, float4x4 &wTc); 28 | }; 29 | 30 | #endif //MESH_HASHING_RGBD_LOCAL_SEQUENCE_H 31 | -------------------------------------------------------------------------------- /config/PKU.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | dataset_path: "/home/wei/Work/data/3DVCR/lab4/" 4 | 5 | # Hash params 6 | bucket_count: 800000 7 | bucket_size: 10 8 | count: 8000000 9 | linked_list_size: 7 10 | value_capacity: 200000 11 | 12 | # SDF params 13 | voxel_size: 0.04 14 | sdf_upper_bound: 4.0 15 | truncation_distance_scale: 0.02 16 | truncation_distance: 0.04 17 | weight_sample: 10 18 | weight_upper_bound: 255 19 | 20 | # Sensor params 21 | fx: 350.9285 22 | fy: 350.2863 23 | cx: 315.0930 24 | cy: 173.4571 25 | min_depth_range: 0.5 26 | max_depth_range: 3.5 27 | range_factor: 0.001 28 | height: 360 29 | width: 640 30 | 31 | # Raycaster params 32 | min_raycast_depth: 0.5 33 | max_raycast_depth: 5.5 34 | raycast_step: 0.016 35 | sample_sdf_threshold: 0.808 36 | sdf_threshold: 0.800 37 | enable_gradient: 1 38 | 39 | max_vertex_count: 10000000 40 | max_triangle_count: 10000000 -------------------------------------------------------------------------------- /config/SUN3D.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | dataset_path: "/home/wei/Work/data/SUN3D/burghers/" 4 | 5 | # Hash params 6 | bucket_count: 200000 7 | bucket_size: 10 8 | count: 2000000 9 | linked_list_size: 7 10 | value_capacity: 50000 11 | 12 | # SDF params 13 | voxel_size: 0.008 14 | sdf_upper_bound: 14.0 15 | truncation_distance_scale: 0.008 16 | truncation_distance: 0.024 17 | weight_sample: 10 18 | weight_upper_bound: 255 19 | 20 | # Sensor params 21 | fx: 525.0 22 | fy: 525.0 23 | cx: 319.5 24 | cy: 239.5 25 | min_depth_range: 0.5 26 | max_depth_range: 2.0 27 | range_factor: 0.001 28 | height: 480 29 | width: 640 30 | 31 | # Raycaster params 32 | min_raycast_depth: 0.5 33 | max_raycast_depth: 5.5 34 | raycast_step: 0.016 35 | sample_sdf_threshold: 0.808 36 | sdf_threshold: 0.800 37 | enable_gradient: 1 38 | 39 | max_vertex_count: 8000000 40 | max_triangle_count: 8000000 -------------------------------------------------------------------------------- /src/core/block.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-5-21. 3 | // 4 | 5 | #ifndef CORE_BLOCK_H 6 | #define CORE_BLOCK_H 7 | 8 | #include "core/common.h" 9 | #include "core/voxel.h" 10 | 11 | #include 12 | 13 | #define BLOCK_LIFE 3 14 | // Typically Block is a 8x8x8 voxel array 15 | struct __ALIGN__(8) Block { 16 | int inner_surfel_count; 17 | int boundary_surfel_count; 18 | int life_count_down; 19 | 20 | Voxel voxels[BLOCK_SIZE]; 21 | MeshUnit mesh_units[BLOCK_SIZE]; 22 | PrimalDualVariables primal_dual_variables[BLOCK_SIZE]; 23 | 24 | __host__ __device__ 25 | void Clear() { 26 | inner_surfel_count = 0; 27 | boundary_surfel_count = 0; 28 | life_count_down = BLOCK_LIFE; 29 | 30 | #ifdef __CUDA_ARCH__ // __CUDA_ARCH__ is only defined for __device__ 31 | #pragma unroll 8 32 | #endif 33 | for (int i = 0; i < BLOCK_SIZE; ++i) { 34 | 35 | voxels[i].Clear(); 36 | mesh_units[i].Clear(); 37 | primal_dual_variables[i].Clear(); 38 | } 39 | } 40 | }; 41 | 42 | #endif // CORE_BLOCK_H 43 | -------------------------------------------------------------------------------- /config/TUM2.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | dataset_path: "/home/wei/Work/data/TUM/rgbd_dataset_freiburg2_xyz/" 4 | 5 | # Hash params 6 | bucket_count: 100000 7 | bucket_size: 10 8 | count: 1000000 9 | linked_list_size: 7 10 | value_capacity: 40000 11 | 12 | # SDF params 13 | voxel_size: 0.008 14 | sdf_upper_bound: 4.0 15 | truncation_distance_scale: 0.008 16 | truncation_distance: 0.016 17 | weight_sample: 10 18 | weight_upper_bound: 255 19 | 20 | # Sensor params 21 | fx: 520.9 22 | fy: 521.0 23 | cx: 325.1 24 | cy: 249.7 25 | min_depth_range: 0.5 26 | max_depth_range: 5.0 27 | range_factor: 0.0002 28 | height: 480 29 | width: 640 30 | 31 | # Raycaster params 32 | min_raycast_depth: 0.5 33 | max_raycast_depth: 5.5 34 | raycast_step: 0.016 35 | sample_sdf_threshold: 0.808 36 | sdf_threshold: 0.800 37 | enable_gradient: 1 38 | 39 | max_vertex_count: 1000000 40 | max_triangle_count: 1000000 -------------------------------------------------------------------------------- /config/TUM1.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | dataset_path: "/home/wei/Work/data/TUM/rgbd_dataset_freiburg1_xyz/" 4 | 5 | # Hash params 6 | bucket_count: 100000 7 | bucket_size: 10 8 | count: 1000000 9 | linked_list_size: 7 10 | value_capacity: 40000 11 | 12 | # SDF params 13 | voxel_size: 0.008 14 | sdf_upper_bound: 4.0 15 | truncation_distance_scale: 0.008 16 | truncation_distance: 0.016 17 | weight_sample: 10 18 | weight_upper_bound: 255 19 | 20 | # Sensor params 21 | fx: 517.3 22 | fy: 516.5 23 | cx: 318.6 24 | cy: 255.3 25 | min_depth_range: 0.5 26 | max_depth_range: 3.0 27 | range_factor: 0.0002 28 | height: 480 29 | width: 640 30 | 31 | # Raycaster params 32 | min_raycast_depth: 0.5 33 | max_raycast_depth: 5.5 34 | raycast_step: 0.016 35 | sample_sdf_threshold: 0.808 36 | sdf_threshold: 0.800 37 | enable_gradient: 1 38 | 39 | max_vertex_count: 1000000 40 | max_triangle_count: 1500000 -------------------------------------------------------------------------------- /config/TUM3.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | dataset_path: "/home/wei/Work/data/TUM/rgbd_dataset_freiburg3_long_office_household/" 4 | 5 | # Hash params 6 | bucket_count: 40000 7 | bucket_size: 10 8 | count: 400000 9 | linked_list_size: 7 10 | value_capacity: 50000 11 | 12 | # SDF params 13 | voxel_size: 0.008 14 | sdf_upper_bound: 4.0 15 | truncation_distance_scale: 0.008 16 | truncation_distance: 0.016 17 | weight_sample: 10 18 | weight_upper_bound: 255 19 | 20 | # Sensor params 21 | fx: 535.4 22 | fy: 539.2 23 | cx: 320.1 24 | cy: 247.6 25 | min_depth_range: 0.5 26 | max_depth_range: 3.0 27 | range_factor: 0.0002 28 | height: 480 29 | width: 640 30 | 31 | # Raycaster params 32 | min_raycast_depth: 0.5 33 | max_raycast_depth: 5.5 34 | raycast_step: 0.016 35 | sample_sdf_threshold: 0.808 36 | sdf_threshold: 0.800 37 | enable_gradient: 1 38 | 39 | max_vertex_count: 4000000 40 | max_triangle_count: 8000000 -------------------------------------------------------------------------------- /config/SUN3D_ORIGINAL.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | dataset_path: "/home/wei/Work/data/SUN3D-Princeton/hotel_umd/maryland_hotel3/" 4 | 5 | # Hash params 6 | bucket_count: 400000 7 | bucket_size: 10 8 | count: 4000000 9 | linked_list_size: 7 10 | value_capacity: 100000 11 | 12 | # SDF params 13 | voxel_size: 0.008 14 | sdf_upper_bound: 4.0 15 | truncation_distance_scale: 0.01 16 | truncation_distance: 0.02 17 | weight_sample: 10 18 | weight_upper_bound: 255 19 | 20 | # Sensor params 21 | fx: 570.34 22 | fy: 570.34 23 | cx: 320.0 24 | cy: 240.0 25 | min_depth_range: 0.5 26 | max_depth_range: 3.0 27 | range_factor: 0.0002 28 | height: 480 29 | width: 640 30 | 31 | # Raycaster params 32 | min_raycast_depth: 0.5 33 | max_raycast_depth: 5.5 34 | raycast_step: 0.016 35 | sample_sdf_threshold: 0.808 36 | sdf_threshold: 0.800 37 | enable_gradient: 1 38 | 39 | max_vertex_count: 10000000 40 | max_triangle_count: 10000000 -------------------------------------------------------------------------------- /src/optimize/linear_equations.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-25. 3 | // 4 | 5 | #ifndef MESH_HASHING_LINEAR_LEAST_SQUARES_H 6 | #define MESH_HASHING_LINEAR_LEAST_SQUARES_H 7 | 8 | #include "core/common.h" 9 | #include "sensor/rgbd_sensor.h" 10 | #include "geometry/geometry_helper.h" 11 | #include "helper_math.h" 12 | #include "matrix.h" 13 | 14 | class SensorLinearEquations { 15 | public: 16 | // Ax = b for each pixel 17 | float3x3* A; 18 | float3* b; 19 | 20 | void Alloc(int width, int height); 21 | void Free(); 22 | void Reset(); 23 | 24 | #ifdef __CUDACC__ 25 | __device__ 26 | void atomicAddfloat3x3(int idx, float3x3& dA) { 27 | #pragma unroll 1 28 | for (int i = 0; i < 9; ++i) { 29 | atomicAdd(&(A[idx].entries[i]), dA.entries[i]); 30 | } 31 | } 32 | __device__ 33 | void atomicAddfloat3(int idx, float3& db) { 34 | atomicAdd(&b[idx].x, db.x); 35 | atomicAdd(&b[idx].y, db.y); 36 | atomicAdd(&b[idx].z, db.z); 37 | } 38 | #endif 39 | 40 | private: 41 | bool is_allocated_on_gpu_ = false; 42 | int width_; 43 | int height_; 44 | }; 45 | 46 | void SolveSensorDataEquation( 47 | SensorLinearEquations& linear_equations, 48 | Sensor& sensor, 49 | GeometryHelper& geometry_helper 50 | ); 51 | 52 | #endif //MESH_HASHING_LINEAR_LEAST_SQUARES_H 53 | -------------------------------------------------------------------------------- /src/core/entry_array.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #ifndef CORE_ENTRY_ARRAY_H 6 | #define CORE_ENTRY_ARRAY_H 7 | 8 | #include "hash_entry.h" 9 | 10 | class EntryArray { 11 | public: 12 | __host__ EntryArray() = default; 13 | __host__ explicit EntryArray(uint entry_count); 14 | // __host__ ~EntryArray(); 15 | 16 | __host__ void Alloc(uint entry_count); 17 | __host__ void Resize(uint entry_count); 18 | __host__ void Free(); 19 | 20 | __host__ uint count(); 21 | __host__ void reset_count(); 22 | 23 | __host__ void Reset(); 24 | 25 | __host__ __device__ 26 | HashEntry& operator [] (int i) { 27 | return entries_[i]; 28 | } 29 | __host__ __device__ uchar& flag(int i) { 30 | return flags_[i]; 31 | } 32 | __host__ __device__ int& counter() { 33 | return counter_[0]; 34 | } 35 | 36 | __host__ HashEntry* GetGPUPtr() const{ 37 | return entries_; 38 | } 39 | private: 40 | bool is_allocated_on_gpu_ = false; 41 | // @param const element 42 | uint entry_count_; 43 | // @param array 44 | HashEntry *entries_; 45 | // @param read-write element 46 | int *counter_; /// atomic counter 47 | // @param array 48 | uchar *flags_; /// used in garbage collection 49 | 50 | }; 51 | 52 | #endif //MESH_HASHING_ENTRY_ARRAY_H 53 | -------------------------------------------------------------------------------- /src/core/block_array.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #ifndef CORE_BLOCK_ARRAY_H 6 | #define CORE_BLOCK_ARRAY_H 7 | 8 | #include "core/block.h" 9 | 10 | // Pre-allocated blocks to store the map 11 | class BlockArray { 12 | public: 13 | __host__ BlockArray() = default; 14 | __host__ explicit BlockArray(uint block_count); 15 | 16 | // We have to pass VALUE instead of REFERENCE to GPU, 17 | // therefore destructor will be called after a kernel launch, 18 | // and improper Free() will be triggered. 19 | // So if on GPU, disable destructor (temporarily), 20 | // and call Free() manually. 21 | // TODO: when CPU version is implemented, let it decide when to call Free() 22 | //__host__ ~BlockArray(); 23 | 24 | __host__ void Alloc(uint block_count); 25 | __host__ void Resize(uint block_count); 26 | __host__ void Free(); 27 | 28 | __host__ void Reset(); 29 | 30 | __host__ __device__ Block& operator[] (uint i) { 31 | return blocks_[i]; 32 | } 33 | __host__ __device__ const Block& operator[] (uint i) const { 34 | return blocks_[i]; 35 | } 36 | 37 | __host__ Block* GetGPUPtr() const{ 38 | return blocks_; 39 | } 40 | private: 41 | bool is_allocated_on_gpu_ = false; 42 | // @param array 43 | Block* blocks_; 44 | // @param const element 45 | uint block_count_; 46 | }; 47 | 48 | #endif // CORE_BLOCK_ARRAY_H 49 | -------------------------------------------------------------------------------- /src/core/common.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-3-12. 3 | // 4 | #ifndef CORE_COMMON_H 5 | #define CORE_COMMON_H 6 | 7 | /// Type redefinitions 8 | #ifndef sint 9 | typedef signed int sint; 10 | #endif 11 | 12 | #ifndef uint 13 | typedef unsigned int uint; 14 | #endif 15 | 16 | #ifndef slong 17 | typedef signed long slong; 18 | #endif 19 | 20 | #ifndef ulong 21 | typedef unsigned long ulong; 22 | #endif 23 | 24 | #ifndef uchar 25 | typedef unsigned char uchar; 26 | #endif 27 | 28 | #ifndef schar 29 | typedef signed char schar; 30 | #endif 31 | 32 | /// Useful macros 33 | #if defined(__CUDACC__) 34 | #define __ALIGN__(n) __align__(n) 35 | #else 36 | #define __ALIGN__(n) __attribute__((aligned(n))) 37 | #include 38 | #endif 39 | 40 | #ifndef PINF 41 | #define PINF __int_as_float(0x7f800000) 42 | #endif 43 | 44 | /// Enable linked list in the hash table 45 | #define HANDLE_COLLISIONS 46 | 47 | /// Block size in voxel unit 48 | #define BLOCK_SIDE_LENGTH 8 49 | #define BLOCK_SIZE (BLOCK_SIDE_LENGTH * BLOCK_SIDE_LENGTH * BLOCK_SIDE_LENGTH) 50 | #define MEMORY_LIMIT 100 51 | 52 | /// Entry state 53 | #define LOCK_ENTRY -1 54 | #define FREE_ENTRY -2 55 | #define NO_OFFSET 0 56 | 57 | #define FREE_PTR -2 58 | 59 | #define N_VERTEX 3 60 | #define N_TRIANGLE 5 61 | 62 | #define EPSILON 1e-6 63 | 64 | //#define STATS 65 | 66 | #endif //_VH_COMMON_H_ 67 | -------------------------------------------------------------------------------- /src/visualization/bounding_box.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #include "bounding_box.h" 6 | #include "core/common.h" 7 | #include "core/entry_array.h" 8 | #include "geometry/geometry_helper.h" 9 | #include "helper_cuda.h" 10 | 11 | //BoundingBox::~BoundingBox() { 12 | // Free(); 13 | //} 14 | 15 | void BoundingBox::Alloc(int max_vertex_count) { 16 | if (!is_allocated_on_gpu_) { 17 | checkCudaErrors(cudaMalloc(&vertex_counter_, sizeof(uint))); 18 | checkCudaErrors(cudaMalloc(&vertices_, sizeof(float3) * max_vertex_count)); 19 | is_allocated_on_gpu_ = true; 20 | } 21 | } 22 | 23 | void BoundingBox::Free() { 24 | if (is_allocated_on_gpu_) { 25 | checkCudaErrors(cudaFree(vertex_counter_)); 26 | checkCudaErrors(cudaFree(vertices_)); 27 | is_allocated_on_gpu_ = false; 28 | } 29 | } 30 | 31 | void BoundingBox::Resize(int max_vertex_count) { 32 | max_vertex_count_ = max_vertex_count; 33 | if (is_allocated_on_gpu_) { 34 | Free(); 35 | } 36 | Alloc(max_vertex_count); 37 | Reset(); 38 | } 39 | 40 | void BoundingBox::Reset() { 41 | checkCudaErrors(cudaMemset(vertex_counter_, 0, sizeof(uint))); 42 | } 43 | 44 | uint BoundingBox::vertex_count() { 45 | uint vertex_count; 46 | checkCudaErrors(cudaMemcpy(&vertex_count, 47 | vertex_counter_, 48 | sizeof(uint), cudaMemcpyDeviceToHost)); 49 | return vertex_count; 50 | } -------------------------------------------------------------------------------- /src/mapping/update_bayesian.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-25. 3 | // 4 | 5 | #ifndef MAPPING_UPDATE_PROBABILISTIC_H 6 | #define MAPPING_UPDATE_PROBABILISTIC_H 7 | 8 | #include "core/hash_table.h" 9 | #include "core/block_array.h" 10 | #include "core/entry_array.h" 11 | #include "core/mesh.h" 12 | #include "sensor/rgbd_sensor.h" 13 | #include "geometry/geometry_helper.h" 14 | 15 | #include "optimize/linear_equations.h" 16 | 17 | // @function 18 | // Enumerate @param candidate_entries 19 | // change the value of @param blocks 20 | // according to the existing @param mesh 21 | // and input @param sensor data 22 | // with the help of hash_table and geometry_helper 23 | void BuildSensorDataEquation( 24 | EntryArray &candidate_entries, 25 | BlockArray &blocks, 26 | Mesh &mesh, 27 | Sensor &sensor, 28 | HashTable &hash_table, 29 | GeometryHelper &geometry_helper, 30 | SensorLinearEquations &linear_equations 31 | ); 32 | 33 | float UpdateBlocksBayesian( 34 | EntryArray &candidate_entries, 35 | BlockArray &blocks, 36 | Sensor &sensor, 37 | HashTable &hash_table, 38 | GeometryHelper &geometry_helper 39 | ); 40 | 41 | float PredictOutlierRatio( 42 | EntryArray& candidate_entries, 43 | BlockArray& blocks, 44 | Mesh& mesh, 45 | Sensor& sensor, 46 | HashTable& hash_table, 47 | GeometryHelper& geometry_helper); 48 | 49 | #endif //MESH_HASHING_UPDATE_PROBABILISTIC_H 50 | -------------------------------------------------------------------------------- /src/mapping/recycle.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #ifndef MESH_HASHING_RECYCLE_H 6 | #define MESH_HASHING_RECYCLE_H 7 | 8 | #include "core/common.h" 9 | #include "core/hash_table.h" 10 | #include "core/mesh.h" 11 | #include "core/entry_array.h" 12 | #include "core/block_array.h" 13 | #include "geometry/geometry_helper.h" 14 | 15 | // @function 16 | // Enumerate @param candidate_entries 17 | // operate over correspondent @param blocks 18 | void StarveOccupiedBlockArray( 19 | EntryArray& candidate_entries, 20 | BlockArray& blocks 21 | ); 22 | // @function 23 | // Enumerate @param candidate_entries 24 | // operate over correspondent @param blocks 25 | // set flag for incoming @param candidate_entries 26 | void CollectGarbageBlockArray( 27 | EntryArray& candidate_entries, 28 | BlockArray& blocks, 29 | GeometryHelper& geometry_helper 30 | ); 31 | 32 | void CollectLowSurfelBlocks( 33 | EntryArray& candidate_entries, 34 | BlockArray& blocks, 35 | HashTable& hash_table, 36 | GeometryHelper& geometry_helper); 37 | 38 | 39 | // TODO(wei): Check vertex / triangles in detail 40 | // @function 41 | // Enumerate @param candidate_entries 42 | // recycle correspondent @param blocks 43 | // and @param mesh 44 | // also free entry in @param hash_table if needed 45 | void RecycleGarbageBlockArray( 46 | EntryArray &candidate_entries, 47 | BlockArray& blocks, 48 | Mesh& mesh, 49 | HashTable& hash_table 50 | ); 51 | 52 | #endif //MESH_HASHING_RECYCLE_H 53 | -------------------------------------------------------------------------------- /src/README.md: -------------------------------------------------------------------------------- 1 | # Project Structure 2 | 3 | ## app 4 | Target executables 5 | 6 | ## Framework 7 | ### core 8 | Basic data structures, including 9 | - parameters and settings 10 | - **hash_table** 11 | - **block**, holding its minimum unit, **voxel** 12 | - **mesh**, holding **vertices** and **triangles** 13 | 14 | These structs are designed to be usable on both CPU and GPU. The protocols for a `struct` are: 15 | - Array should be stored by *ptr, so that it can be correctly allocated on memory either on CPU or GPU; 16 | - Non-const elements should behave the same as array, so that when changed on GPU, they can be accesed by CPU via memcpy; 17 | - Const elements such as `int` should be stored as they are, so that they can be directly passed from device to host, vice versa. 18 | 19 | ### engine 20 | Higher level wrappers for the data structures in `core`, basically 21 | - **main_engine**, managing **hash_table**, **blocks**, and **mesh** 22 | - **visualizing_engine**, manage **compact_mesh**, **bounding_box**, and **trajectory** 23 | - **logging_engine**, record profiles, etc 24 | 25 | ## Function 26 | ### fusion 27 | Using naive/modified algorithms to fuse data into map. 28 | 29 | ### meshing 30 | Generate **mesh** from **blocks** using modified Marching Cubes. 31 | 32 | ### geometry 33 | - Conversion of coordinates between *world*, *block*, and *voxel*. 34 | - Conversion of systems between *camera* and *world* 35 | - Projections 36 | 37 | ### visualization 38 | - **ray_caster** 39 | - **mesh_renderer** 40 | 41 | ### io 42 | Read and write images/data/map/mesh 43 | 44 | ## Util 45 | Backup garbages here. 46 | 47 | ## TODO 48 | - Separate .cu && .cc (functions and methods) 49 | - Integrate statistics 50 | - Involve time profile 51 | - Minor argument configurations -------------------------------------------------------------------------------- /src/geometry/isosurface_intersection.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-25. 3 | // 4 | 5 | #ifndef GEOMETRY_ISOSURFACE_INTERSECTION_H 6 | #define GEOMETRY_ISOSURFACE_INTERSECTION_H 7 | 8 | #include "core/common.h" 9 | #include "geometry/spatial_query.h" 10 | 11 | __device__ 12 | /// sdf_near: -, sdf_far: + 13 | inline float LinearIntersection(float t_near, float t_far, 14 | float sdf_near, float sdf_far) { 15 | return t_near + (sdf_near / (sdf_near - sdf_far)) * (t_far - t_near); 16 | } 17 | 18 | // d0 near, d1 far 19 | __device__ 20 | /// Iteratively 21 | inline bool BisectionIntersection( 22 | const float3 &world_cam_pos, 23 | const float3 &world_cam_dir, 24 | float sdf_near, float t_near, 25 | float sdf_far, float t_far, 26 | const BlockArray &blocks, 27 | const HashTable &hash_table, 28 | GeometryHelper& geometry_helper, 29 | float &t, uchar3 &color) { 30 | float l = t_near, r = t_far, m = (l + r) * 0.5f; 31 | float l_sdf = sdf_near, r_sdf = sdf_far; 32 | 33 | const uint kIterations = 3; 34 | Voxel voxel_query; 35 | #pragma unroll 1 36 | for (uint i = 0; i < kIterations; i++) { 37 | m = LinearIntersection(l, r, l_sdf, r_sdf); 38 | if (!GetSpatialValue(world_cam_pos + m * world_cam_dir, 39 | blocks, 40 | hash_table, 41 | geometry_helper, 42 | &voxel_query)) 43 | return false; 44 | float m_sdf = voxel_query.sdf; 45 | if (l_sdf * m_sdf > 0.0) { 46 | l = m; 47 | l_sdf = m_sdf; 48 | } else { 49 | r = m; 50 | r_sdf = m_sdf; 51 | } 52 | } 53 | t = m; 54 | return true; 55 | } 56 | 57 | #endif //MESH_HASHING_ISOSURFACE_INTERSECTION_H 58 | -------------------------------------------------------------------------------- /src/visualization/color_util.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-6-6. 3 | // 4 | 5 | #ifndef VH_COLOR_UTIL_H 6 | #define VH_COLOR_UTIL_H 7 | 8 | #include "../core/common.h" 9 | #include "helper_math.h" 10 | 11 | // http://paulbourke.net/texture_colour/colourspace/ 12 | __host__ __device__ 13 | inline float3 ValToRGB(float v, const float vmin, const float vmax) { 14 | float3 c = make_float3(1.0f); 15 | 16 | float dv; 17 | if (v < vmin) 18 | v = vmin; 19 | if (v > vmax) 20 | v = vmax; 21 | dv = vmax - vmin; 22 | 23 | if (v < (vmin + 0.25 * dv)) { 24 | c.x = 0; 25 | c.y = 4 * (v - vmin) / dv; 26 | } else if (v < (vmin + 0.5 * dv)) { 27 | c.x = 0; 28 | c.z = 1 + 4 * (vmin + 0.25 * dv - v) / dv; 29 | } else if (v < (vmin + 0.75 * dv)) { 30 | c.x = 4 * (v - vmin - 0.5 * dv) / dv; 31 | c.z = 0; 32 | } else { 33 | c.y = 1 + 4 * (vmin + 0.75 * dv - v) / dv; 34 | c.z = 0; 35 | } 36 | return c; 37 | }; 38 | 39 | /// Util: Depth to RGB 40 | __device__ 41 | inline float3 HSVToRGB(const float3 &hsv) { 42 | float H = hsv.x; 43 | float S = hsv.y; 44 | float V = hsv.z; 45 | 46 | float hd = H / 60.0f; 47 | uint h = (uint) hd; 48 | float f = hd - h; 49 | 50 | float p = V * (1.0f - S); 51 | float q = V * (1.0f - S * f); 52 | float t = V * (1.0f - S * (1.0f - f)); 53 | 54 | if (h == 0 || h == 6) { 55 | return make_float3(V, t, p); 56 | } else if (h == 1) { 57 | return make_float3(q, V, p); 58 | } else if (h == 2) { 59 | return make_float3(p, V, t); 60 | } else if (h == 3) { 61 | return make_float3(p, q, V); 62 | } else if (h == 4) { 63 | return make_float3(t, p, V); 64 | } else { 65 | return make_float3(V, p, q); 66 | } 67 | } 68 | 69 | 70 | #endif //VH_COLOR_UTIL_H 71 | -------------------------------------------------------------------------------- /src/util/cuda_gl_interp.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-3-19. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "../renderer.h" 10 | 11 | int main() { 12 | Renderer renderer("frame", 640, 480); 13 | std::vector uniform_names; 14 | uniform_names.push_back("texture_sampler"); 15 | 16 | FrameObject frame(640, 480); 17 | frame.CompileShader("../shader/frame_vertex.glsl", 18 | "../shader/frame_fragment.glsl", 19 | uniform_names); 20 | renderer.AddObject(&frame); 21 | 22 | ////////// Load data here 23 | float * cpu_mem; 24 | float4* cuda_mem; 25 | cv::Mat im = cv::imread("../util/img.png"); 26 | 27 | cv::resize(im, im, cv::Size(640, 480)); 28 | cpu_mem = new float[4 * sizeof(float) * 640 * 480]; 29 | for (int i = 0; i < im.rows; ++i) { 30 | for (int j = 0; j < im.cols; ++j) { 31 | int data_idx = i * im.cols + j; 32 | cv::Vec3b bgr = im.at(i, j); 33 | cpu_mem[4 * data_idx + 0] = bgr[2] / 255.0f; 34 | cpu_mem[4 * data_idx + 1] = bgr[1] / 255.0f; 35 | cpu_mem[4 * data_idx + 2] = bgr[0] / 255.0f; 36 | cpu_mem[4 * data_idx + 3] = 1; 37 | } 38 | } 39 | 40 | /// Alloc and memcpy CUDA data here 41 | checkCudaErrors(cudaMalloc(&cuda_mem, sizeof(float4) * 640 * 480)); 42 | checkCudaErrors(cudaMemcpy(cuda_mem, cpu_mem, sizeof(float4) * 640 * 480, 43 | cudaMemcpyHostToDevice)); 44 | 45 | LOG(INFO) << "Begin Loop"; 46 | do { 47 | frame.SetData(cuda_mem); 48 | 49 | float4x4 dmy; 50 | renderer.Render(dmy); 51 | } while( glfwGetKey(renderer.window(), GLFW_KEY_ESCAPE ) != GLFW_PRESS && 52 | glfwWindowShouldClose(renderer.window()) == 0); 53 | } 54 | -------------------------------------------------------------------------------- /src/util/debugger.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-6-4. 3 | // 4 | 5 | /// !!!! NOW a test only version !!! 6 | /// Rewrite it entirely !!! 7 | #ifndef VOXEL_HASHING_DEBUGGER_H 8 | #define VOXEL_HASHING_DEBUGGER_H 9 | 10 | #include 11 | #include 12 | #include "../core/hash_table.h" 13 | #include "../core/block.h" 14 | #include "../core/mesh.h" 15 | 16 | 17 | struct Hash3D { 18 | const static int bucket_count = 1000000; 19 | 20 | std::size_t operator()(const int3 &pos) const { 21 | const int p0 = 73856093; 22 | const int p1 = 19349669; 23 | const int p2 = 83492791; 24 | 25 | int res = ((pos.x * p0) ^ (pos.y * p1) ^ (pos.z * p2)) 26 | % (bucket_count); 27 | if (res < 0) res += (bucket_count); 28 | return (size_t) res; 29 | } 30 | }; 31 | 32 | class Debugger { 33 | private: 34 | std::unordered_map block_map_; 35 | 36 | HashEntry *entries_; 37 | uint *heap_; 38 | uint *heap_counter__; 39 | /// | 40 | /// v 41 | Block *blocks_; 42 | /// | 43 | /// v 44 | uint *vertex_heap__; 45 | uint *vertex_heap_counter__; 46 | Vertex *vertices_; 47 | 48 | uint *triangle_heap__; 49 | uint *triangle_heap_counter__; 50 | Triangle *triangles_; 51 | 52 | int entry_count_; 53 | int block_count_; 54 | int vertex_count_; 55 | int triangle_count_; 56 | 57 | float voxel_size_; 58 | 59 | public: 60 | Debugger(int entry_count, int block_count, 61 | int vertex_count, int triangle_count, 62 | float voxel_size); 63 | ~Debugger(); 64 | 65 | void CoreDump(EntryArrayGPU &hash_table); 66 | 67 | void CoreDump(BlockGPUMemory &blocks); 68 | 69 | void CoreDump(MeshGPU &mesh); 70 | 71 | void DebugAll(); 72 | }; 73 | 74 | 75 | #endif //VOXEL_HASHING_DEBUGGER_H 76 | -------------------------------------------------------------------------------- /cmake_modules/FindGLFW3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find GLFW3 2 | # 3 | # If no pkgconfig, define GLFW_ROOT to installation tree 4 | # Will define the following: 5 | # GLFW3_FOUND 6 | # GLFW3_INCLUDE_DIRS 7 | # GLFW3_LIBRARIES 8 | 9 | IF(PKG_CONFIG_FOUND) 10 | IF(APPLE) 11 | # homebrew or macports pkgconfig locations 12 | SET(ENV{PKG_CONFIG_PATH} "/usr/local/opt/glfw3/lib/pkgconfig:/opt/local/lib/pkgconfig") 13 | ENDIF() 14 | SET(ENV{PKG_CONFIG_PATH} "${DEPENDS_DIR}/glfw/lib/pkgconfig:$ENV{PKG_CONFIG_PATH}") 15 | PKG_CHECK_MODULES(GLFW3 glfw3) 16 | 17 | FIND_LIBRARY(GLFW3_LIBRARY 18 | NAMES ${GLFW3_LIBRARIES} 19 | HINTS ${GLFW3_LIBRARY_DIRS} 20 | ) 21 | SET(GLFW3_LIBRARIES ${GLFW3_LIBRARY}) 22 | 23 | RETURN() 24 | ENDIF() 25 | 26 | FIND_PATH(GLFW3_INCLUDE_DIRS 27 | GLFW/glfw3.h 28 | DOC "GLFW include directory " 29 | PATHS 30 | "${DEPENDS_DIR}/glfw" 31 | "$ENV{ProgramW6432}/glfw" 32 | ENV GLFW_ROOT 33 | PATH_SUFFIXES 34 | include 35 | ) 36 | 37 | # directories in the official binary package 38 | IF(MINGW) 39 | SET(_SUFFIX lib-mingw) 40 | ELSEIF(MSVC11) 41 | SET(_SUFFIX lib-vc2012) 42 | ELSEIF(MSVC12) 43 | SET(_SUFFIX lib-vc2013) 44 | ELSEIF(MSVC14) 45 | SET(_SUFFIX lib-vc2015) 46 | ELSEIF(MSVC) 47 | SET(_SUFFIX lib-vc2012) 48 | ENDIF() 49 | 50 | FIND_LIBRARY(GLFW3_LIBRARIES 51 | NAMES glfw3dll glfw3 52 | PATHS 53 | "${DEPENDS_DIR}/glfw" 54 | "$ENV{ProgramW6432}/glfw" 55 | ENV GLFW_ROOT 56 | PATH_SUFFIXES 57 | lib 58 | ${_SUFFIX} 59 | ) 60 | 61 | IF(WIN32) 62 | FIND_FILE(GLFW3_DLL 63 | glfw3.dll 64 | PATHS 65 | "${DEPENDS_DIR}/glfw" 66 | "$ENV{ProgramW6432}/glfw" 67 | ENV GLFW_ROOT 68 | PATH_SUFFIXES 69 | ${_SUFFIX} 70 | ) 71 | ENDIF() 72 | 73 | INCLUDE(FindPackageHandleStandardArgs) 74 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(GLFW3 "Could NOT find GLFW3 - try adding GLFW_ROOT in enviroment variables." GLFW3_LIBRARIES GLFW3_INCLUDE_DIRS) 75 | -------------------------------------------------------------------------------- /src/core/block_array.cu: -------------------------------------------------------------------------------- 1 | #include "core/block_array.h" 2 | #include "helper_cuda.h" 3 | 4 | #include 5 | 6 | //////////////////// 7 | /// Device code 8 | //////////////////// 9 | __global__ 10 | void BlockArrayResetKernel( 11 | Block* blocks, 12 | int block_count 13 | ) { 14 | const uint block_idx = blockIdx.x * blockDim.x + threadIdx.x; 15 | 16 | if (block_idx < block_count) { 17 | blocks[block_idx].Clear(); 18 | } 19 | } 20 | 21 | //////////////////// 22 | /// Host code 23 | ////////////////////// 24 | __host__ 25 | BlockArray::BlockArray(uint block_count) { 26 | Resize(block_count); 27 | } 28 | 29 | //BlockArray::~BlockArray() { 30 | // Free(); 31 | //} 32 | 33 | __host__ 34 | void BlockArray::Alloc(uint block_count) { 35 | if (! is_allocated_on_gpu_) { 36 | block_count_ = block_count; 37 | checkCudaErrors(cudaMalloc(&blocks_, sizeof(Block) * block_count)); 38 | is_allocated_on_gpu_ = true; 39 | } 40 | } 41 | 42 | __host__ 43 | void BlockArray::Free() { 44 | if (is_allocated_on_gpu_) { 45 | checkCudaErrors(cudaFree(blocks_)); 46 | block_count_ = 0; 47 | blocks_ = NULL; 48 | is_allocated_on_gpu_ = false; 49 | } 50 | } 51 | 52 | __host__ 53 | void BlockArray::Resize(uint block_count) { 54 | if (is_allocated_on_gpu_) { 55 | Free(); 56 | } 57 | Alloc(block_count); 58 | Reset(); 59 | } 60 | 61 | __host__ 62 | void BlockArray::Reset() { 63 | const uint threads_per_block = 64; 64 | 65 | if (block_count_ == 0) return; 66 | 67 | // NOTE: this block is the parallel unit in CUDA, not the data structure Block 68 | const uint blocks = (block_count_ + threads_per_block - 1) / threads_per_block; 69 | 70 | const dim3 grid_size(blocks, 1); 71 | const dim3 block_size(threads_per_block, 1); 72 | 73 | BlockArrayResetKernel<<>>(blocks_, block_count_); 74 | checkCudaErrors(cudaDeviceSynchronize()); 75 | checkCudaErrors(cudaGetLastError()); 76 | } -------------------------------------------------------------------------------- /src/visualization/compact_mesh.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #ifndef VISUALIZATION_COMPACT_MESH_H 6 | #define VISUALIZATION_COMPACT_MESH_H 7 | 8 | #include "core/common.h" 9 | #include "core/params.h" 10 | #include "core/vertex.h" 11 | #include "core/triangle.h" 12 | 13 | class CompactMesh { 14 | public: 15 | CompactMesh() = default; 16 | //~CompactMesh(); 17 | void Alloc(const MeshParams& mesh_params); 18 | void Free(); 19 | 20 | void Resize(const MeshParams &mesh_params); 21 | void Reset(); 22 | 23 | uint vertex_count(); 24 | uint triangle_count(); 25 | 26 | __device__ __host__ 27 | int* vertex_remapper() { 28 | return vertex_remapper_; 29 | } 30 | __device__ __host__ 31 | float3* vertices() { 32 | return vertices_; 33 | } 34 | __device__ __host__ 35 | float3* normals() { 36 | return normals_; 37 | } 38 | __device__ __host__ 39 | float3* colors() { 40 | return colors_; 41 | } 42 | __device__ __host__ 43 | int3* triangles() { 44 | return triangles_; 45 | } 46 | __device__ __host__ 47 | int* triangles_ref_count() { 48 | return triangles_ref_count_; 49 | } 50 | __device__ __host__ 51 | int* vertices_ref_count() { 52 | return vertices_ref_count_; 53 | } 54 | __device__ __host__ 55 | uint* triangle_counter() { 56 | return triangle_counter_; 57 | } 58 | __device__ __host__ 59 | uint* vertex_counter() { 60 | return vertex_counter_; 61 | } 62 | 63 | private: 64 | bool is_allocated_on_gpu_ = false; 65 | int* vertex_remapper_; 66 | 67 | // They are decoupled so as to be separately assigned to the rendering pipeline 68 | float3* vertices_; 69 | float3* normals_; 70 | float3* colors_; 71 | int* vertices_ref_count_; 72 | uint* vertex_counter_; 73 | 74 | int3* triangles_; 75 | int* triangles_ref_count_; 76 | uint* triangle_counter_; 77 | MeshParams mesh_params_; 78 | }; 79 | 80 | 81 | #endif //MESH_HASHING_COMPACT_MESH_H 82 | -------------------------------------------------------------------------------- /src/sensor/rgbd_sensor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-3-20. 3 | // 4 | 5 | #ifndef VH_SENSOR_H 6 | #define VH_SENSOR_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "core/params.h" 13 | #include "sensor/preprocess.h" 14 | #include 15 | /// At first get rid of CUDARGBDAdaptor and Sensor, use it directly 16 | struct SensorData { 17 | /// sensor data raw 18 | short* depth_buffer; 19 | uchar4* color_buffer; 20 | 21 | /// Reformatted data 22 | float* depth_data; 23 | float* filtered_depth_data; 24 | float* inlier_ratio; 25 | float4* color_data; 26 | float3* normal_data; 27 | 28 | /// Texture-binded data 29 | cudaArray* depth_array; 30 | cudaArray* color_array; 31 | cudaArray* normal_array; 32 | 33 | cudaTextureObject_t depth_texture; 34 | cudaTextureObject_t color_texture; 35 | cudaTextureObject_t normal_texture; 36 | 37 | cudaChannelFormatDesc depth_channel_desc; 38 | cudaChannelFormatDesc color_channel_desc; 39 | cudaChannelFormatDesc normal_channel_desc; 40 | }; 41 | 42 | class Sensor { 43 | public: 44 | Sensor() = default; 45 | explicit Sensor(SensorParams ¶ms); 46 | ~Sensor(); 47 | void BindCUDATexture(); 48 | 49 | int Process(cv::Mat &depth, cv::Mat &color); 50 | 51 | void set_transform(float4x4 wTc) { 52 | wTc_ = wTc; 53 | cTw_ = wTc_.getInverse(); 54 | } 55 | const float4x4& wTc() const { 56 | return wTc_; 57 | } 58 | const float4x4& cTw() const { 59 | return cTw_; 60 | } 61 | uint width() const { 62 | return params_.width; 63 | } 64 | uint height() const { 65 | return params_.height; 66 | } 67 | const SensorData& data() const { 68 | return data_; 69 | } 70 | const SensorParams& sensor_params() const { 71 | return params_; 72 | } 73 | 74 | private: 75 | bool is_allocated_on_gpu_ = false; 76 | 77 | /// sensor data 78 | SensorData data_; 79 | SensorParams params_; 80 | 81 | float4x4 wTc_; // camera -> world 82 | float4x4 cTw_; 83 | }; 84 | 85 | #endif //VH_SENSOR_H 86 | -------------------------------------------------------------------------------- /cmake_modules/FindGlog.cmake: -------------------------------------------------------------------------------- 1 | # Sets: 2 | # GLOG_FOUND: TRUE if Glog is found. 3 | # GLOG_INCLUDE_DIRS: Include directories for Glog. 4 | # GLOG_LIBRARIES: Libraries required to link Glog. 5 | # 6 | # The following variables control the behavior of this module: 7 | # 8 | # GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to 9 | # search for Glog includes. 10 | # GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to 11 | # search for Glog libraries. 12 | 13 | include(FindPackageHandleStandardArgs) 14 | 15 | list(APPEND GLOG_CHECK_INCLUDE_DIRS 16 | /usr/local/include 17 | /usr/local/homebrew/include 18 | /opt/local/var/macports/software 19 | /opt/local/include 20 | /usr/include) 21 | list(APPEND GLOG_CHECK_PATH_SUFFIXES 22 | glog/include 23 | glog/Include 24 | Glog/include 25 | Glog/Include 26 | src/windows) 27 | 28 | list(APPEND GLOG_CHECK_LIBRARY_DIRS 29 | /usr/local/lib 30 | /usr/local/homebrew/lib 31 | /opt/local/lib 32 | /usr/lib) 33 | list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES 34 | glog/lib 35 | glog/Lib 36 | Glog/lib 37 | Glog/Lib 38 | x64/Release) 39 | 40 | find_path(GLOG_INCLUDE_DIRS 41 | NAMES 42 | glog/logging.h 43 | PATHS 44 | ${GLOG_INCLUDE_DIR_HINTS} 45 | ${GLOG_CHECK_INCLUDE_DIRS} 46 | PATH_SUFFIXES 47 | ${GLOG_CHECK_PATH_SUFFIXES}) 48 | find_library(GLOG_LIBRARIES 49 | NAMES 50 | glog 51 | libglog 52 | PATHS 53 | ${GLOG_LIBRARY_DIR_HINTS} 54 | ${GLOG_CHECK_LIBRARY_DIRS} 55 | PATH_SUFFIXES 56 | ${GLOG_CHECK_LIBRARY_SUFFIXES}) 57 | 58 | if (GLOG_INCLUDE_DIRS AND GLOG_LIBRARIES) 59 | set(GLOG_FOUND TRUE) 60 | message(STATUS "Found Glog") 61 | message(STATUS " Includes : ${GLOG_INCLUDE_DIRS}") 62 | message(STATUS " Libraries : ${GLOG_LIBRARIES}") 63 | else() 64 | if(Glog_FIND_REQUIRED) 65 | message(FATAL_ERROR "Could not find Glog") 66 | endif() 67 | endif() -------------------------------------------------------------------------------- /src/visualization/colorize.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "core/params.h" 6 | #include "visualization/colorize.h" 7 | #include "visualization/color_util.h" 8 | 9 | __device__ 10 | float3 DepthToRGB(float depth, float depthMin, float depthMax) { 11 | float normalized_depth = (depth - depthMin)/(depthMax - depthMin); 12 | float x = 1.0f-normalized_depth; 13 | if (x < 0.0f) x = 0.0f; 14 | if (x > 1.0f) x = 1.0f; 15 | 16 | x = 360.0f*x - 120.0f; 17 | if (x < 0.0f) x += 359.0f; 18 | return HSVToRGB(make_float3(x, 1.0f, 0.5f)); 19 | } 20 | 21 | __global__ 22 | void ColorizeDepthKernel(float4 *dst, float *src, 23 | uint width, uint height, 24 | float min_depth_range, 25 | float max_depth_range) { 26 | const int x = blockIdx.x*blockDim.x + threadIdx.x; 27 | const int y = blockIdx.y*blockDim.y + threadIdx.y; 28 | 29 | if (x >= 0 && x < width && y >= 0 && y < height) { 30 | 31 | float depth = src[y*width + x]; 32 | if (depth != MINF && depth != 0.0f 33 | && depth >= min_depth_range 34 | && depth <= max_depth_range) { 35 | float3 c = DepthToRGB(depth, min_depth_range, max_depth_range); 36 | dst[y*width + x] = make_float4(c, 1.0f); 37 | } else { 38 | dst[y*width + x] = make_float4(0.0f); 39 | } 40 | } 41 | } 42 | 43 | void ColorizeDepth(float4* colorized_depth, float* depth, SensorParams& params) { 44 | const uint threads_per_block = 16; 45 | const dim3 grid_size((params.width + threads_per_block - 1)/threads_per_block, 46 | (params.height + threads_per_block - 1)/threads_per_block); 47 | const dim3 block_size(threads_per_block, threads_per_block); 48 | 49 | ColorizeDepthKernel<<>>( 50 | colorized_depth, depth, 51 | params.width, params.height, 52 | params.min_depth_range, 53 | params.max_depth_range); 54 | checkCudaErrors(cudaDeviceSynchronize()); 55 | checkCudaErrors(cudaGetLastError()); 56 | } 57 | -------------------------------------------------------------------------------- /config/ORB/TUM3.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 535.4 9 | Camera.fy: 539.2 10 | Camera.cx: 320.1 11 | Camera.cy: 247.6 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 640 19 | Camera.height: 480 20 | 21 | # Camera frames per second 22 | Camera.fps: 30.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 5000.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | 70 | -------------------------------------------------------------------------------- /config/ORB/ICL.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 481.2 9 | Camera.fy: 480 10 | Camera.cx: 319.5 11 | Camera.cy: 239.5 12 | 13 | Camera.k1: 0 14 | Camera.k2: 0 15 | Camera.p1: 0 16 | Camera.p2: 0 17 | Camera.k3: 0 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 40.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5000.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /config/ORB/SUN3D.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 525.0 9 | Camera.fy: 525.0 10 | Camera.cx: 319.5 11 | Camera.cy: 239.5 12 | 13 | Camera.k1: 0 14 | Camera.k2: 0 15 | Camera.p1: 0 16 | Camera.p2: 0 17 | Camera.k3: 0 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 40.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5000.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /src/engine/logging_engine.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-24. 3 | // 4 | 5 | #ifndef ENGINE_LOGGING_ENGINE_H 6 | #define ENGINE_LOGGING_ENGINE_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | class Int3Sort { 13 | public: 14 | bool operator()(int3 const &a, int3 const &b) const { 15 | if (a.x != b.x) return a.x < b.x; 16 | if (a.y != b.y) return a.y < b.y; 17 | return a.z < b.z; 18 | } 19 | }; 20 | typedef std::map BlockMap; 21 | 22 | class LoggingEngine { 23 | public: 24 | LoggingEngine() = default; 25 | explicit LoggingEngine(std::string path) 26 | : base_path_(path) {}; 27 | void Init(std::string path); 28 | ~LoggingEngine(); 29 | 30 | void ConfigVideoWriter(int width, int height); 31 | void ConfigPlyWriter(); 32 | void WriteVideo(cv::Mat& mat); 33 | void WritePly(CompactMesh& mesh); 34 | void WriteLocalizationError(float error); 35 | void WriteMappingTimeStamp(double alloc_time, double collect_time, double update_time, 36 | int frame_idx); 37 | void WriteMappingTimeStamp(float alloc_time, float collect_time, float predict_time, float update_time, 38 | int frame_idx); 39 | void WriteMeshingTimeStamp(float time, int frame_idx); 40 | void WriteMeshStats(int vtx_count, int tri_count); 41 | 42 | BlockMap RecordBlockToMemory( 43 | const Block *block_gpu, uint block_num, 44 | const HashEntry *candidate_entry_gpu, uint entry_num 45 | ); 46 | void WriteFormattedBlocks(const BlockMap &blocks, std::string filename); 47 | BlockMap ReadFormattedBlocks(std::string filename); 48 | void WriteRawBlocks(const BlockMap &blocks, std::string filename); 49 | BlockMap ReadRawBlocks(std::string filename); 50 | 51 | bool enable_video() { 52 | return enable_video_; 53 | } 54 | bool enable_ply() { 55 | return enable_ply_; 56 | } 57 | private: 58 | bool enable_video_ = false; 59 | bool enable_ply_ = false; 60 | 61 | std::string base_path_; 62 | std::string prefix_; 63 | cv::VideoWriter video_writer_; 64 | std::ofstream time_stamp_file_; 65 | std::ofstream meshing_time_file_; 66 | std::ofstream mesh_stats_file_; 67 | std::ofstream localization_err_file_; 68 | }; 69 | 70 | 71 | #endif //MESH_HASHING_LOGGING_ENGINE_H 72 | -------------------------------------------------------------------------------- /config/ORB/PKU.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 350.9285 9 | Camera.fy: 350.2863 10 | Camera.cx: 315.0930 11 | Camera.cy: 173.4571 12 | 13 | Camera.k1: 0 14 | Camera.k2: 0 15 | Camera.p1: 0 16 | Camera.p2: 0 17 | Camera.k3: 0 18 | 19 | Camera.width: 640 20 | Camera.height: 360 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 50.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 1000.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /config/ORB/SUN3D_ORIGINAL.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 570.34 9 | Camera.fy: 570.34 10 | Camera.cx: 320.0 11 | Camera.cy: 240.0 12 | 13 | Camera.k1: 0 14 | Camera.k2: 0 15 | Camera.p1: 0 16 | Camera.p2: 0 17 | Camera.k3: 0 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 50.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 1000.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /config/ORB/TUM1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 517.306408 9 | Camera.fy: 516.469215 10 | Camera.cx: 318.643040 11 | Camera.cy: 255.313989 12 | 13 | Camera.k1: 0.262383 14 | Camera.k2: -0.953104 15 | Camera.p1: -0.005358 16 | Camera.p2: 0.002628 17 | Camera.k3: 1.163314 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 40.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5000.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /config/ORB/TUM2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 520.908620 9 | Camera.fy: 521.007327 10 | Camera.cx: 325.141442 11 | Camera.cy: 249.701764 12 | 13 | Camera.k1: 0.231222 14 | Camera.k2: -0.784899 15 | Camera.p1: -0.003257 16 | Camera.p2: -0.000105 17 | Camera.k3: 0.917205 18 | 19 | Camera.width: 640 20 | Camera.height: 480 21 | 22 | # Camera frames per second 23 | Camera.fps: 30.0 24 | 25 | # IR projector baseline times fx (aprox.) 26 | Camera.bf: 40.0 27 | 28 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 29 | Camera.RGB: 1 30 | 31 | # Close/Far threshold. Baseline times. 32 | ThDepth: 40.0 33 | 34 | # Deptmap values factor 35 | DepthMapFactor: 5208.0 36 | 37 | #-------------------------------------------------------------------------------------------- 38 | # ORB Parameters 39 | #-------------------------------------------------------------------------------------------- 40 | 41 | # ORB Extractor: Number of features per image 42 | ORBextractor.nFeatures: 1000 43 | 44 | # ORB Extractor: Scale factor between levels in the scale pyramid 45 | ORBextractor.scaleFactor: 1.2 46 | 47 | # ORB Extractor: Number of levels in the scale pyramid 48 | ORBextractor.nLevels: 8 49 | 50 | # ORB Extractor: Fast threshold 51 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 52 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 53 | # You can lower these values if your images have low contrast 54 | ORBextractor.iniThFAST: 20 55 | ORBextractor.minThFAST: 7 56 | 57 | #-------------------------------------------------------------------------------------------- 58 | # Viewer Parameters 59 | #-------------------------------------------------------------------------------------------- 60 | Viewer.KeyFrameSize: 0.05 61 | Viewer.KeyFrameLineWidth: 1 62 | Viewer.GraphLineWidth: 0.9 63 | Viewer.PointSize:2 64 | Viewer.CameraSize: 0.08 65 | Viewer.CameraLineWidth: 3 66 | Viewer.ViewpointX: 0 67 | Viewer.ViewpointY: -0.7 68 | Viewer.ViewpointZ: -1.8 69 | Viewer.ViewpointF: 500 70 | 71 | -------------------------------------------------------------------------------- /src/visualization/extract_bounding_box.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-23. 3 | // 4 | 5 | #include "core/common.h" 6 | #include "core/entry_array.h" 7 | 8 | #include "visualization/bounding_box.h" 9 | #include "visualization/extract_bounding_box.h" 10 | 11 | #include "geometry/geometry_helper.h" 12 | 13 | #include "helper_cuda.h" 14 | 15 | __device__ 16 | const static int3 kEdgeOffsets[24] = { 17 | {0, 0, 0}, {0, 0, 1}, 18 | {0, 0, 1}, {1, 0, 1}, 19 | {1, 0, 1}, {1, 0, 0}, 20 | {1, 0, 0}, {0, 0, 0}, 21 | 22 | {0, 1, 0}, {0, 1, 1}, 23 | {0, 1, 1}, {1, 1, 1}, 24 | {1, 1, 1}, {1, 1, 0}, 25 | {1, 1, 0}, {0, 1, 0}, 26 | 27 | {0, 0, 0}, {0, 1, 0}, 28 | {0, 0, 1}, {0, 1, 1}, 29 | {1, 0, 1}, {1, 1, 1}, 30 | {1, 0, 0}, {1, 1, 0} 31 | }; 32 | 33 | __global__ 34 | void ExtractBoundingBoxKernel( 35 | EntryArray candidate_entries, 36 | BoundingBox bounding_box, 37 | GeometryHelper geometry_helper) { 38 | const uint idx = blockIdx.x * blockDim.x + threadIdx.x; 39 | HashEntry& entry = candidate_entries[idx]; 40 | 41 | int3 voxel_base_pos = geometry_helper.BlockToVoxel(entry.pos); 42 | float3 world_base_pos = geometry_helper.VoxelToWorld(voxel_base_pos) 43 | - make_float3(0.5f) * geometry_helper.voxel_size; 44 | 45 | float s = geometry_helper.voxel_size * BLOCK_SIDE_LENGTH; 46 | int addr = atomicAdd(bounding_box.vertex_counter(), 24); 47 | for (int i = 0; i < 24; i ++) { 48 | bounding_box.vertices()[addr + i] 49 | = world_base_pos + s * make_float3(kEdgeOffsets[i]); 50 | } 51 | } 52 | 53 | void ExtractBoundingBox(EntryArray& candidate_entries, 54 | BoundingBox& bounding_box, 55 | GeometryHelper& geometry_helper) { 56 | bounding_box.Reset(); 57 | int occupied_block_count = candidate_entries.count(); 58 | if (occupied_block_count <= 0) return; 59 | 60 | { 61 | const uint threads_per_block = BLOCK_SIZE; 62 | const dim3 grid_size((occupied_block_count + threads_per_block - 1) 63 | / threads_per_block, 1); 64 | const dim3 block_size(threads_per_block, 1); 65 | 66 | ExtractBoundingBoxKernel <<< grid_size, block_size >>> ( 67 | candidate_entries, bounding_box, geometry_helper); 68 | checkCudaErrors(cudaDeviceSynchronize()); 69 | checkCudaErrors(cudaGetLastError()); 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /src/io/config_manager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-4-30. 3 | // 4 | 5 | #ifndef VH_DATA_MANAGER 6 | #define VH_DATA_MANAGER 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "matrix.h" 13 | #include "../core/params.h" 14 | 15 | enum DatasetType { 16 | ICL = 0, 17 | TUM1 = 1, 18 | TUM2 = 2, 19 | TUM3 = 3, 20 | SUN3D = 4, 21 | SUN3D_ORIGINAL = 5, 22 | PKU = 6 23 | }; 24 | 25 | // Deprecated: 26 | struct Dataset { 27 | DatasetType type; 28 | std::string path; 29 | }; 30 | 31 | void LoadRuntimeParams(std::string path, RuntimeParams ¶ms); 32 | void LoadHashParams(std::string path, HashParams& params); 33 | void LoadMeshParams(std::string path, MeshParams ¶ms); 34 | void LoadVolumeParams(std::string path, VolumeParams& params); 35 | void LoadSensorParams(std::string path, SensorParams& params); 36 | void LoadRayCasterParams(std::string path, RayCasterParams& params); 37 | 38 | /// 1-1-1 correspondences 39 | void LoadICL( 40 | std::string dataset_path, 41 | std::vector &depth_image_list, 42 | std::vector &color_image_list, 43 | std::vector& wTcs); 44 | void LoadSUN3D( 45 | std::string dataset_path, 46 | std::vector &depth_img_list, 47 | std::vector &color_img_list, 48 | std::vector &wTcs); 49 | void LoadSUN3DOriginal( 50 | std::string dataset_path, 51 | std::vector &depth_img_list, 52 | std::vector &color_img_list, 53 | std::vector &wTcs); 54 | void Load3DVCR( 55 | std::string dataset_path, 56 | std::vector &depth_image_list, 57 | std::vector &color_image_list, 58 | std::vector &wTcs); 59 | 60 | /// no 1-1-1 correspondences 61 | void LoadTUM( 62 | std::string dataset_path, 63 | std::vector &depth_image_list, 64 | std::vector &color_image_list, 65 | std::vector &wTcs); 66 | 67 | struct ConfigManager { 68 | HashParams hash_params; 69 | MeshParams mesh_params; 70 | VolumeParams sdf_params; 71 | SensorParams sensor_params; 72 | RayCasterParams ray_caster_params; 73 | 74 | void LoadConfig(DatasetType dataset_type); 75 | void LoadConfig(std::string config_path); 76 | }; 77 | 78 | 79 | 80 | #endif //VH_DATA_MANAGER 81 | -------------------------------------------------------------------------------- /src/core/entry_array.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #include "core/entry_array.h" 6 | #include "helper_cuda.h" 7 | #include 8 | 9 | //////////////////// 10 | /// Device code 11 | //////////////////// 12 | __global__ 13 | void EntryArrayResetKernel( 14 | HashEntry* entries, 15 | uint entry_count 16 | ) { 17 | const uint idx = blockIdx.x * blockDim.x + threadIdx.x; 18 | if (idx < entry_count) { 19 | entries[idx].Clear(); 20 | } 21 | } 22 | 23 | //////////////////// 24 | /// Host code 25 | //////////////////// 26 | /// Life cycle 27 | __host__ 28 | EntryArray::EntryArray(uint entry_count) { 29 | Resize(entry_count); 30 | } 31 | //EntryArray::~EntryArray() { 32 | // Free(); 33 | //} 34 | 35 | __host__ 36 | void EntryArray::Alloc(uint entry_count) { 37 | if (! is_allocated_on_gpu_) { 38 | entry_count_ = entry_count; 39 | checkCudaErrors(cudaMalloc(&entries_, sizeof(HashEntry) * entry_count)); 40 | checkCudaErrors(cudaMalloc(&counter_, sizeof(int))); 41 | checkCudaErrors(cudaMalloc(&flags_, sizeof(uchar) * entry_count)); 42 | is_allocated_on_gpu_ = true; 43 | } 44 | } 45 | 46 | __host__ 47 | void EntryArray::Free() { 48 | if (is_allocated_on_gpu_) { 49 | checkCudaErrors(cudaFree(entries_)); 50 | checkCudaErrors(cudaFree(counter_)); 51 | checkCudaErrors(cudaFree(flags_)); 52 | entry_count_ = 0; 53 | entries_ = NULL; 54 | flags_ = NULL; 55 | counter_ = NULL; 56 | is_allocated_on_gpu_ = false; 57 | } 58 | } 59 | 60 | __host__ 61 | void EntryArray::Resize(uint entry_count) { 62 | if (is_allocated_on_gpu_) { 63 | Free(); 64 | } 65 | Alloc(entry_count); 66 | Reset(); 67 | } 68 | 69 | __host__ 70 | void EntryArray::Reset() { 71 | const int threads_per_block = 64; 72 | const dim3 grid_size((entry_count_ + threads_per_block - 1) 73 | / threads_per_block, 1); 74 | const dim3 block_size(threads_per_block, 1); 75 | 76 | EntryArrayResetKernel<<>>(entries_, entry_count_); 77 | checkCudaErrors(cudaDeviceSynchronize()); 78 | checkCudaErrors(cudaGetLastError()); 79 | } 80 | 81 | __host__ 82 | uint EntryArray::count(){ 83 | uint count; 84 | checkCudaErrors(cudaMemcpy(&count, 85 | counter_, 86 | sizeof(uint), 87 | cudaMemcpyDeviceToHost)); 88 | return count; 89 | } 90 | 91 | void EntryArray::reset_count() { 92 | checkCudaErrors(cudaMemset(counter_, 0, sizeof(uint))); 93 | } -------------------------------------------------------------------------------- /src/visualization/ray_caster.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-3-17. 3 | // 4 | 5 | #ifndef VH_RAY_CASTER_H 6 | #define VH_RAY_CASTER_H 7 | 8 | #include "core/common.h" 9 | #include "core/params.h" 10 | #include "core/hash_table.h" 11 | #include "core/block_array.h" 12 | #include "geometry/geometry_helper.h" 13 | #include "sensor/rgbd_sensor.h" 14 | 15 | struct RayCasterSample { 16 | float entropy; 17 | float sdf; 18 | float t; 19 | uint weight; 20 | }; 21 | 22 | struct RayCasterData { 23 | float4 *depth; 24 | float4 *vertex; 25 | float4 *normal; 26 | float4 *color; 27 | float4 *surface; 28 | }; 29 | 30 | class RayCaster { 31 | public: 32 | RayCaster() = default; 33 | void Alloc(const RayCasterParams ¶ms); 34 | RayCaster(const RayCasterParams& params); 35 | void Free(); 36 | 37 | void Cast(HashTable& hash_table, 38 | BlockArray& blocks, 39 | RayCasterData &ray_caster_data, 40 | GeometryHelper& geometry_helper, 41 | const float4x4& c_T_w); 42 | 43 | const cv::Mat& depth_image() { 44 | return depth_image_; 45 | } 46 | const cv::Mat& normal_image() { 47 | return normal_image_; 48 | } 49 | const cv::Mat& color_image() { 50 | return color_image_; 51 | } 52 | const cv::Mat& surface_image() { 53 | return surface_image_; 54 | } 55 | const RayCasterParams& ray_caster_params() const { 56 | return ray_caster_params_; 57 | } 58 | RayCasterData& data() { 59 | return ray_caster_data_; 60 | } 61 | 62 | /// To write images into a video, use this function 63 | static cv::Mat Mat4fToMat3b(const cv::Mat &mat4f) { 64 | cv::Mat mat3b = cv::Mat(mat4f.rows, mat4f.cols, CV_8UC3); 65 | for (int i = 0; i < mat4f.rows; ++i) { 66 | for (int j = 0; j < mat4f.cols; ++j) { 67 | cv::Vec4f cf = mat4f.at(i, j); 68 | if (std::isinf(cf[0])) { 69 | mat3b.at(i, j) = cv::Vec3b(0, 0, 0); 70 | } else { 71 | mat3b.at(i, j) = cv::Vec3b(255 * fabs(cf[0]), 72 | 255 * fabs(cf[1]), 73 | 255 * fabs(cf[2])); 74 | } 75 | } 76 | } 77 | return mat3b; 78 | } 79 | 80 | private: 81 | bool is_allocated_on_gpu_ = false; 82 | RayCasterParams ray_caster_params_; 83 | 84 | cv::Mat depth_image_; 85 | cv::Mat normal_image_; 86 | cv::Mat color_image_; 87 | cv::Mat surface_image_; 88 | 89 | RayCasterData ray_caster_data_; 90 | }; 91 | 92 | #endif //VH_RAY_CASTER_H 93 | -------------------------------------------------------------------------------- /src/core/params.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-3-12. 3 | // 4 | 5 | /// Parameters for HashTable 6 | /// Shared in __constant__ form 7 | /// Update it ONLY in hash_table 8 | // TODO(wei): put rigid transform elsewhere 9 | 10 | #ifndef CORE_PARAMS_H 11 | #define CORE_PARAMS_H 12 | 13 | #include "core/common.h" 14 | #include 15 | 16 | struct HashParams { 17 | uint bucket_count; // 500000 18 | uint bucket_size; // 10 (entries) 19 | 20 | uint entry_count; // bucket_count * bucket_size 21 | uint linked_list_size; // 7 22 | 23 | uint value_capacity; // 1000000 24 | }; 25 | 26 | struct MeshParams { 27 | uint max_vertex_count; 28 | uint max_triangle_count; 29 | }; 30 | 31 | struct __ALIGN__(16) VolumeParams { 32 | float voxel_size; // 0.004 (m) 33 | 34 | float truncation_distance_scale; // 0.01 (m / m) 35 | float truncation_distance; // 0.02 (m) 36 | float sdf_upper_bound; // 4.0 (m) 37 | 38 | uint weight_sample; // 10, TODO(wei): change it dynamically! 39 | uint weight_upper_bound; // 255 40 | }; 41 | 42 | struct RayCasterParams { 43 | float fx; 44 | float fy; 45 | float cx; 46 | float cy; 47 | 48 | uint width; /// 640 49 | uint height; /// 480 50 | 51 | float min_raycast_depth; 52 | float max_raycast_depth; 53 | float raycast_step; /// 0.8f * SDF_Truncation 54 | 55 | float sample_sdf_threshold; /// 50.5f * s_rayIncrement 56 | float sdf_threshold; /// 50.0f * s_rayIncrement 57 | bool enable_gradients; 58 | }; 59 | 60 | /// We may generate a virtual camera for LiDAR 61 | /// where many points are null 62 | struct SensorParams { 63 | float fx; /// Set manually 64 | float fy; 65 | float cx; 66 | float cy; 67 | 68 | uint width; /// 640 69 | uint height; /// 480 70 | 71 | float min_depth_range; /// 0.5f 72 | float max_depth_range; /// 5.0f, might need modify for LiDAR 73 | float range_factor; /// 1/5000 for TUM and ICL, 1/1000 for SUN3D 74 | }; 75 | 76 | /// Just a supersede of argv editing... 77 | struct RuntimeParams { 78 | int dataset_type; 79 | bool enable_bayesian_update; 80 | 81 | bool enable_navigation; 82 | bool enable_polygon_mode; 83 | bool enable_global_mesh; 84 | bool enable_sdf_gradient; 85 | bool enable_color; 86 | 87 | bool enable_bounding_box; 88 | bool enable_trajectory; 89 | bool enable_ray_casting; 90 | 91 | bool enable_video_recording; 92 | bool enable_ply_saving; 93 | 94 | std::string filename_prefix; 95 | std::string time_profile; 96 | std::string memo_profile; 97 | 98 | int run_frames; 99 | }; 100 | 101 | #endif //VH_PARAMS_H 102 | -------------------------------------------------------------------------------- /src/engine/main_engine.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-4-5. 3 | // 4 | // MainEngine: managing HashTable and might be other structs later 5 | 6 | #ifndef ENGINE_MAIN_ENGINE_H 7 | #define ENGINE_MAIN_ENGINE_H 8 | 9 | #include "core/hash_table.h" 10 | #include "core/block_array.h" 11 | #include "core/entry_array.h" 12 | #include "core/mesh.h" 13 | 14 | #include "engine/visualizing_engine.h" 15 | #include "engine/logging_engine.h" 16 | #include "visualization/compact_mesh.h" 17 | #include "visualization/bounding_box.h" 18 | #include "sensor/rgbd_sensor.h" 19 | #include "mapping_engine.h" 20 | 21 | class MainEngine { 22 | public: 23 | // configure main data 24 | MainEngine( 25 | const HashParams& hash_params, 26 | const VolumeParams& volume_params, 27 | const MeshParams& mesh_params, 28 | const SensorParams& sensor_params, 29 | const RayCasterParams& ray_caster_params 30 | ); 31 | ~MainEngine(); 32 | void Reset(); 33 | 34 | // configure engines 35 | void ConfigMappingEngine( 36 | bool enable_bayesian_update 37 | ); 38 | 39 | void ConfigLocalizingEngine(); 40 | void ConfigVisualizingEngine( 41 | gl::Light& light, 42 | bool enable_navigation, 43 | bool enable_global_mesh, 44 | bool enable_bounding_box, 45 | bool enable_trajectory, 46 | bool enable_polygon_mode, 47 | bool enable_ray_caster, 48 | bool enable_color 49 | ); 50 | 51 | void ConfigLoggingEngine( 52 | std::string path, 53 | bool enable_video, 54 | bool enable_ply 55 | ); 56 | 57 | void Localizing(Sensor &sensor, int iters, float4x4& gt); 58 | void Mapping(Sensor &sensor); 59 | void Meshing(); 60 | void Recycle(); 61 | int Visualize(float4x4 view); 62 | int Visualize(float4x4 view, float4x4 view_gt); 63 | 64 | void Log(); 65 | void RecordBlocks(std::string prefix = ""); 66 | void FinalLog(); 67 | 68 | const int& frame_count() { 69 | return integrated_frame_count_; 70 | } 71 | bool& enable_sdf_gradient() { 72 | return enable_sdf_gradient_; 73 | } 74 | 75 | private: 76 | // Engines 77 | MappingEngine map_engine_; 78 | VisualizingEngine vis_engine_; 79 | LoggingEngine log_engine_; 80 | 81 | // Core 82 | HashTable hash_table_; 83 | BlockArray blocks_; 84 | EntryArray candidate_entries_; 85 | 86 | // Meshing 87 | Mesh mesh_; 88 | 89 | // Geometry 90 | GeometryHelper geometry_helper_; 91 | 92 | int integrated_frame_count_ = 0; 93 | bool enable_sdf_gradient_; 94 | 95 | HashParams hash_params_; 96 | VolumeParams volume_params_; 97 | MeshParams mesh_params_; 98 | SensorParams sensor_params_; 99 | RayCasterParams ray_caster_params_; 100 | }; 101 | 102 | 103 | #endif //VH_MAP_H 104 | -------------------------------------------------------------------------------- /src/visualization/trajectory.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-24. 3 | // 4 | 5 | #include 6 | #include 7 | #include "trajectory.h" 8 | 9 | Trajectory::Trajectory(uint max_vertex_count) { 10 | Alloc(max_vertex_count); 11 | } 12 | 13 | // Free() 14 | void Trajectory::Alloc(uint max_vertex_count) { 15 | if (! is_allocated_on_gpu_) { 16 | max_vertex_count_ = max_vertex_count; 17 | checkCudaErrors(cudaMalloc(&vertices_, sizeof(float3) * max_vertex_count)); 18 | is_allocated_on_gpu_ = true; 19 | } 20 | } 21 | 22 | void Trajectory::Free() { 23 | if (is_allocated_on_gpu_) { 24 | checkCudaErrors(cudaFree(vertices_)); 25 | is_allocated_on_gpu_ = true; 26 | } 27 | } 28 | 29 | void Trajectory::AddPose(float4x4 wTc) { 30 | float3 curr_position = make_float3(wTc.m14, wTc.m24, wTc.m34); 31 | if (vertex_count_ == -1) { 32 | vertex_count_ = 0; 33 | } else { 34 | checkCudaErrors(cudaMemcpy(vertices_ + vertex_count_, 35 | &prev_position_, 36 | sizeof(float3), 37 | cudaMemcpyHostToDevice)); 38 | vertex_count_++; 39 | checkCudaErrors(cudaMemcpy(vertices_ + vertex_count_, 40 | &curr_position, 41 | sizeof(float3), 42 | cudaMemcpyHostToDevice)); 43 | vertex_count_++; 44 | 45 | frustum_ = MakeFrustum(wTc); 46 | for (size_t i = 0; i < frustum_.size(); ++i) { 47 | checkCudaErrors(cudaMemcpy(vertices_ + vertex_count_ + i, 48 | &frustum_[i], 49 | sizeof(float3), 50 | cudaMemcpyHostToDevice)); 51 | } 52 | } 53 | prev_position_ = curr_position; 54 | } 55 | 56 | std::vector Trajectory::MakeFrustum(float4x4 wTc) { 57 | float3 camera_position = make_float3(wTc.m14, wTc.m24, wTc.m34); 58 | 59 | float length = 0.25; 60 | 61 | float4 v04 = wTc * make_float4(length, length, 2*length, 1); 62 | float4 v14 = wTc * make_float4(length, -length, 2*length, 1); 63 | float4 v24 = wTc * make_float4(-length, length, 2*length, 1); 64 | float4 v34 = wTc * make_float4(-length, -length, 2*length, 1); 65 | float3 v0 = make_float3(v04.x, v04.y, v04.z); 66 | float3 v1 = make_float3(v14.x, v14.y, v14.z); 67 | float3 v2 = make_float3(v24.x, v24.y, v24.z); 68 | float3 v3 = make_float3(v34.x, v34.y, v34.z); 69 | 70 | std::vector vertices = {camera_position, v0, 71 | camera_position, v1, 72 | camera_position, v2, 73 | camera_position, v3, 74 | v0, v1, 75 | v1, v3, 76 | v3, v2, 77 | v2, v0}; 78 | return vertices; 79 | } -------------------------------------------------------------------------------- /src/app/reconstruction.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-3-26. 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "util/timer.h" 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "sensor/rgbd_data_provider.h" 26 | #include "sensor/rgbd_sensor.h" 27 | #include "visualization/ray_caster.h" 28 | 29 | #include "io/config_manager.h" 30 | #include "core/collect_block_array.h" 31 | #include "glwrapper.h" 32 | 33 | #define DEBUG_ 34 | 35 | int main(int argc, char **argv) { 36 | /// Use this to substitute tedious argv parsing 37 | RuntimeParams args; 38 | LoadRuntimeParams("../config/args.yml", args); 39 | 40 | ConfigManager config; 41 | RGBDDataProvider rgbd_local_sequence; 42 | 43 | DatasetType dataset_type = DatasetType(args.dataset_type); 44 | config.LoadConfig(dataset_type); 45 | rgbd_local_sequence.LoadDataset(dataset_type); 46 | Sensor sensor(config.sensor_params); 47 | 48 | MainEngine main_engine( 49 | config.hash_params, 50 | config.sdf_params, 51 | config.mesh_params, 52 | config.sensor_params, 53 | config.ray_caster_params 54 | ); 55 | 56 | main_engine.ConfigMappingEngine( 57 | args.enable_bayesian_update 58 | ); 59 | 60 | gl::Light light; 61 | light.Load("../config/lights.yaml"); 62 | main_engine.ConfigVisualizingEngine( 63 | light, 64 | args.enable_navigation, 65 | args.enable_global_mesh, 66 | args.enable_bounding_box, 67 | args.enable_trajectory, 68 | args.enable_polygon_mode, 69 | args.enable_ray_casting, 70 | args.enable_color 71 | ); 72 | main_engine.ConfigLoggingEngine( 73 | ".", 74 | args.enable_video_recording, 75 | args.enable_ply_saving 76 | ); 77 | main_engine.enable_sdf_gradient() = args.enable_sdf_gradient; 78 | 79 | cv::Mat color, depth; 80 | float4x4 wTc, cTw; 81 | int frame_count = 0; 82 | while (rgbd_local_sequence.ProvideData(depth, color, wTc)) { 83 | frame_count++; 84 | if (args.run_frames > 0 && frame_count > args.run_frames) 85 | break; 86 | 87 | // Preprocess data 88 | sensor.Process(depth, color); 89 | sensor.set_transform(wTc); 90 | cTw = wTc.getInverse(); 91 | 92 | main_engine.Mapping(sensor); 93 | main_engine.Meshing(); 94 | if (main_engine.Visualize(cTw)) 95 | break; 96 | 97 | main_engine.Log(); 98 | //main_engine.RecordBlocks(); 99 | main_engine.Recycle(); 100 | } 101 | 102 | main_engine.FinalLog(); 103 | 104 | return 0; 105 | } -------------------------------------------------------------------------------- /src/app/slam.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-12-23. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "util/timer.h" 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "sensor/rgbd_data_provider.h" 27 | #include "sensor/rgbd_sensor.h" 28 | #include "visualization/ray_caster.h" 29 | 30 | #include "io/config_manager.h" 31 | #include "core/collect_block_array.h" 32 | #include "glwrapper.h" 33 | 34 | #include 35 | #define DEBUG_ 36 | 37 | int main(int argc, char **argv) { 38 | /// Use this to substitute tedious argv parsing 39 | RuntimeParams args; 40 | LoadRuntimeParams("../config/args.yml", args); 41 | 42 | ConfigManager config; 43 | RGBDDataProvider rgbd_local_sequence; 44 | 45 | DatasetType dataset_type = DatasetType(args.dataset_type); 46 | config.LoadConfig(dataset_type); 47 | rgbd_local_sequence.LoadDataset(dataset_type); 48 | Sensor sensor(config.sensor_params); 49 | float4x4 I; I.setIdentity(); 50 | sensor.set_transform(I); 51 | 52 | MainEngine main_engine( 53 | config.hash_params, 54 | config.sdf_params, 55 | config.mesh_params, 56 | config.sensor_params, 57 | config.ray_caster_params 58 | ); 59 | 60 | main_engine.ConfigMappingEngine( 61 | args.enable_bayesian_update 62 | ); 63 | 64 | gl::Light light; 65 | light.Load("../config/lights.yaml"); 66 | main_engine.ConfigVisualizingEngine( 67 | light, 68 | args.enable_navigation, 69 | args.enable_global_mesh, 70 | args.enable_bounding_box, 71 | args.enable_trajectory, 72 | args.enable_polygon_mode, 73 | args.enable_ray_casting, 74 | args.enable_color 75 | ); 76 | main_engine.ConfigLoggingEngine( 77 | ".", 78 | args.enable_video_recording, 79 | args.enable_ply_saving 80 | ); 81 | 82 | main_engine.enable_sdf_gradient() = args.enable_sdf_gradient; 83 | 84 | cv::Mat color, depth; 85 | float4x4 wTc, cTw; 86 | int frame_count = 0; 87 | while (rgbd_local_sequence.ProvideData(depth, color, wTc)) { 88 | frame_count++; 89 | if (args.run_frames > 0 && frame_count > args.run_frames) 90 | break; 91 | 92 | // Preprocess data 93 | sensor.Process(depth, color); 94 | 95 | main_engine.Localizing(sensor, 20, wTc); 96 | //sensor.set_transform(wTc); 97 | //main_engine.Localizing(sensor, 10, wTc); 98 | 99 | main_engine.Mapping(sensor); 100 | main_engine.Meshing(); 101 | 102 | cTw = sensor.cTw(); 103 | if (main_engine.Visualize(cTw)) 104 | break; 105 | 106 | main_engine.Log(); 107 | //main_engine.RecordBlocks(); 108 | main_engine.Recycle(); 109 | } 110 | 111 | main_engine.FinalLog(); 112 | 113 | return 0; 114 | } -------------------------------------------------------------------------------- /src/sensor/rgbd_data_provider.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #include "rgbd_data_provider.h" 6 | #include 7 | 8 | const std::string kConfigPaths[] = { 9 | "../config/ICL.yml", 10 | "../config/TUM1.yml", 11 | "../config/TUM2.yml", 12 | "../config/TUM3.yml", 13 | "../config/SUN3D.yml", 14 | "../config/SUN3D_ORIGINAL.yml", 15 | "../config/PKU.yml" 16 | }; 17 | 18 | void RGBDDataProvider::LoadDataset( 19 | DatasetType dataset_type 20 | ) { 21 | std::string config_path = kConfigPaths[dataset_type]; 22 | cv::FileStorage fs(config_path, cv::FileStorage::READ); 23 | std::string dataset_path = (std::string)fs["dataset_path"]; 24 | LoadDataset(dataset_path, dataset_type); 25 | } 26 | 27 | void RGBDDataProvider::LoadDataset(Dataset dataset) { 28 | LoadDataset(dataset.path, dataset.type); 29 | } 30 | 31 | void RGBDDataProvider::LoadDataset( 32 | std::string dataset_path, 33 | DatasetType dataset_type 34 | ) { 35 | switch (dataset_type) { 36 | case ICL: 37 | LoadICL(dataset_path, depth_image_list, color_image_list, wTcs); 38 | break; 39 | case SUN3D: 40 | LoadSUN3D(dataset_path, depth_image_list, color_image_list, wTcs); 41 | break; 42 | case SUN3D_ORIGINAL: 43 | LoadSUN3DOriginal(dataset_path, depth_image_list, color_image_list, wTcs); 44 | break; 45 | case TUM1: 46 | LoadTUM(dataset_path, depth_image_list, color_image_list, wTcs); 47 | break; 48 | case TUM2: 49 | LoadTUM(dataset_path, depth_image_list, color_image_list, wTcs); 50 | break; 51 | case TUM3: 52 | LoadTUM(dataset_path, depth_image_list, color_image_list, wTcs); 53 | break; 54 | case PKU: 55 | Load3DVCR(dataset_path, depth_image_list, color_image_list, wTcs); 56 | break; 57 | } 58 | } 59 | 60 | bool RGBDDataProvider::ProvideData( 61 | cv::Mat &depth, 62 | cv::Mat &color 63 | ) { 64 | if (frame_id > depth_image_list.size()) { 65 | LOG(ERROR) << "All images provided!"; 66 | return false; 67 | } 68 | depth = cv::imread(depth_image_list[frame_id], CV_LOAD_IMAGE_UNCHANGED); 69 | color = cv::imread(color_image_list[frame_id]); 70 | if (color.channels() == 3) { 71 | cv::cvtColor(color, color, CV_BGR2BGRA); 72 | } 73 | ++frame_id; 74 | 75 | return true; 76 | // TODO: Network situation 77 | } 78 | 79 | bool RGBDDataProvider::ProvideData(cv::Mat &depth, 80 | cv::Mat &color, 81 | float4x4 &wTc) { 82 | if (frame_id >= depth_image_list.size()) { 83 | LOG(ERROR) << "All images provided!"; 84 | return false; 85 | } 86 | 87 | { 88 | LOG(INFO) << frame_id << "/" << depth_image_list.size(); 89 | depth = cv::imread(depth_image_list[frame_id], CV_LOAD_IMAGE_UNCHANGED); 90 | color = cv::imread(color_image_list[frame_id]); 91 | if (color.channels() == 3) { 92 | cv::cvtColor(color, color, CV_BGR2BGRA); 93 | } 94 | 95 | wTc = wTcs[0].getInverse() * wTcs[frame_id]; 96 | ++frame_id; 97 | } ///while (frame_id >= 1960 && frame_id <= 1985); 98 | 99 | return true; 100 | // TODO: Network situation 101 | } -------------------------------------------------------------------------------- /src/geometry/spatial_query.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_SPATIAL_QUERY_H 2 | #define GEOMETRY_SPATIAL_QUERY_H 3 | 4 | #include 5 | #include "geometry_helper.h" 6 | 7 | #include "core/hash_table.h" 8 | #include "core/block_array.h" 9 | #include "geometry/voxel_query.h" 10 | 11 | __device__ 12 | inline float frac(float val) { 13 | return (val - floorf(val)); 14 | } 15 | 16 | __device__ 17 | inline float3 frac(const float3 &val) { 18 | return make_float3(frac(val.x), frac(val.y), frac(val.z)); 19 | } 20 | 21 | // TODO: simplify this code 22 | // @function with tri-linear interpolation 23 | __device__ 24 | inline bool GetSpatialValue( 25 | const float3 &pos, 26 | const BlockArray &blocks, 27 | const HashTable &hash_table, 28 | GeometryHelper &geometry_helper, 29 | Voxel* voxel 30 | ) { 31 | const float offset = geometry_helper.voxel_size; 32 | const float3 pos_corner = pos - 0.5f * offset; 33 | float3 ratio = frac(geometry_helper.WorldToVoxelf(pos)); 34 | 35 | Voxel voxel_query; 36 | float sdf = 0.0f; 37 | float3 colorf = make_float3(0.0f, 0.0f, 0.0f); 38 | float a = 0.0f; 39 | float b = 0.0f; 40 | float radius = 0.0f; 41 | 42 | #pragma unroll 1 43 | for (int i = 0; i < 8; ++i) { 44 | float3 mask = make_float3((i & 4) > 0, (i & 2) > 0, (i & 1) > 0); 45 | // 0 --> 1 - r, 1 --> r 46 | float3 r = (make_float3(1.0f) - mask) * (make_float3(1.0) - ratio) 47 | + (mask) * ratio; 48 | bool valid = GetVoxelValue(pos_corner + mask * offset, blocks, hash_table, 49 | geometry_helper, &voxel_query); 50 | if (! valid) return false; 51 | float w = r.x * r.y * r.z; 52 | sdf += w * voxel_query.sdf; 53 | colorf += w * make_float3(voxel_query.color); 54 | a += w * voxel_query.a; 55 | b += w * voxel_query.b; 56 | radius += w * sqrtf(1.0f / voxel_query.inv_sigma2); 57 | // TODO: Interpolation of stats 58 | } 59 | 60 | voxel->sdf = sdf; 61 | voxel->color = make_uchar3(colorf.x, colorf.y, colorf.z); 62 | voxel->a = a; 63 | voxel->b = b; 64 | voxel->inv_sigma2 = 1.0f / squaref(radius); 65 | return true; 66 | } 67 | 68 | __device__ 69 | inline bool GetSpatialSDFGradient( 70 | const float3 &pos, 71 | const BlockArray &blocks, 72 | const HashTable &hash_table, 73 | GeometryHelper &geometry_helper, 74 | float3* grad 75 | ) { 76 | const float3 grad_masks[3] = {{0.5, 0, 0}, {0, 0.5, 0}, {0, 0, 0.5}}; 77 | const float3 offset = make_float3(geometry_helper.voxel_size); 78 | 79 | bool valid = true; 80 | float sdfp[3], sdfn[3]; 81 | Voxel voxel_query; 82 | #pragma unroll 1 83 | for (int i = 0; i < 3; ++i) { 84 | float3 dpos = grad_masks[i] * offset; 85 | valid = valid && GetSpatialValue(pos - dpos, blocks, hash_table, 86 | geometry_helper, &voxel_query); 87 | sdfn[i] = voxel_query.sdf; 88 | valid = valid && GetSpatialValue(pos + dpos, blocks, hash_table, 89 | geometry_helper, &voxel_query); 90 | sdfp[i] = voxel_query.sdf; 91 | } 92 | 93 | *grad = make_float3((sdfp[0] - sdfn[0]) / offset.x, 94 | (sdfp[1] - sdfn[1]) / offset.y, 95 | (sdfp[2] - sdfn[2]) / offset.z); 96 | return valid; 97 | } 98 | 99 | #endif -------------------------------------------------------------------------------- /src/core/voxel.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #ifndef CORE_VOXEL_H 6 | #define CORE_VOXEL_H 7 | 8 | #include "core/common.h" 9 | #include "helper_math.h" 10 | 11 | // Statistics typically reserved for Voxels 12 | // float: *Laplacian* and *entropy* are intuitive statistics 13 | // float: *duration* is time-interval that the voxel exists 14 | struct __ALIGN__(4) Stat { 15 | float laplacian; 16 | float entropy; 17 | float duration; 18 | 19 | __host__ __device__ 20 | void Clear() { 21 | laplacian = 0; 22 | entropy = 0; 23 | duration = 0; 24 | } 25 | }; 26 | 27 | struct __ALIGN__(4) MeshUnit { 28 | // mesh 29 | int vertex_ptrs [N_VERTEX]; // 3 30 | int vertex_mutexes[N_VERTEX]; // 3 31 | int triangle_ptrs [N_TRIANGLE];// 32 | short curr_cube_idx, prev_cube_idx; 33 | 34 | __host__ __device__ 35 | void ResetMutexes() { 36 | vertex_mutexes[0] = FREE_PTR; 37 | vertex_mutexes[1] = FREE_PTR; 38 | vertex_mutexes[2] = FREE_PTR; 39 | } 40 | 41 | __host__ __device__ 42 | int GetVertex(int idx) { 43 | return vertex_ptrs[idx]; 44 | } 45 | 46 | __host__ __device__ 47 | void Clear() { 48 | vertex_ptrs[0] = vertex_mutexes[0] = FREE_PTR; 49 | vertex_ptrs[1] = vertex_mutexes[1] = FREE_PTR; 50 | vertex_ptrs[2] = vertex_mutexes[2] = FREE_PTR; 51 | 52 | triangle_ptrs[0] = FREE_PTR; 53 | triangle_ptrs[1] = FREE_PTR; 54 | triangle_ptrs[2] = FREE_PTR; 55 | triangle_ptrs[3] = FREE_PTR; 56 | triangle_ptrs[4] = FREE_PTR; 57 | 58 | curr_cube_idx = prev_cube_idx = 0; 59 | } 60 | }; 61 | 62 | struct __ALIGN__(4) PrimalDualVariables { 63 | bool mask; 64 | float sdf0, sdf_bar, inv_sigma2; 65 | float3 p; 66 | 67 | __host__ __device__ 68 | void operator = (const PrimalDualVariables& pdv) { 69 | mask = pdv.mask; 70 | sdf0 = pdv.sdf0; 71 | sdf_bar = pdv.sdf_bar; 72 | p = pdv.p; 73 | } 74 | 75 | __host__ __device__ 76 | void Clear() { 77 | mask = false; 78 | sdf0 = sdf_bar = 0; 79 | inv_sigma2 = 0; 80 | p = make_float3(0); 81 | } 82 | }; 83 | 84 | struct __ALIGN__(4) Voxel { 85 | float sdf; // signed distance function, mu 86 | float inv_sigma2; // sigma 87 | float a, b; 88 | uchar3 color; // color 89 | 90 | __host__ __device__ 91 | void operator = (const Voxel& v) { 92 | sdf = v.sdf; 93 | inv_sigma2 = v.inv_sigma2; 94 | color = v.color; 95 | a = v.a; 96 | b = v.b; 97 | } 98 | 99 | __host__ __device__ 100 | void Clear() { 101 | sdf = inv_sigma2 = 0.0f; 102 | color = make_uchar3(0, 0, 0); 103 | a = b = 0; 104 | } 105 | 106 | __host__ __device__ 107 | void Update(const Voxel &delta) { 108 | float3 c_prev = make_float3(color.x, color.y, color.z); 109 | float3 c_delta = make_float3(delta.color.x, delta.color.y, delta.color.z); 110 | float3 c_curr = 0.5f * c_prev + 0.5f * c_delta; 111 | color = make_uchar3(c_curr.x + 0.5f, c_curr.y + 0.5f, c_curr.z + 0.5f); 112 | 113 | sdf = (sdf * inv_sigma2 + delta.sdf * delta.inv_sigma2) / (inv_sigma2 + delta.inv_sigma2); 114 | inv_sigma2 = inv_sigma2 + delta.inv_sigma2; 115 | } 116 | }; 117 | 118 | #endif // CORE_VOXEL_H 119 | -------------------------------------------------------------------------------- /src/core/mesh.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-5-21. 3 | // 4 | 5 | #ifndef CORE_MESH_H 6 | #define CORE_MESH_H 7 | 8 | #include "core/common.h" 9 | #include "core/params.h" 10 | #include "core/vertex.h" 11 | #include "core/triangle.h" 12 | 13 | #include 14 | #include 15 | 16 | class Mesh { 17 | public: 18 | __host__ Mesh() = default; 19 | // __host__ ~Mesh(); 20 | 21 | __host__ void Alloc(const MeshParams &mesh_params); 22 | __host__ void Resize(const MeshParams &mesh_params); 23 | __host__ void Free(); 24 | __host__ void Reset(); 25 | 26 | const MeshParams& params() { 27 | return mesh_params_; 28 | } 29 | 30 | __device__ __host__ Vertex& vertex(uint i) { 31 | return vertices[i]; 32 | } 33 | __device__ __host__ Triangle& triangle(uint i) { 34 | return triangles[i]; 35 | } 36 | 37 | __host__ uint vertex_heap_count(); 38 | __host__ uint triangle_heap_count(); 39 | 40 | private: 41 | bool is_allocated_on_gpu_ = false; 42 | uint* vertex_heap_; 43 | uint* vertex_heap_counter_; 44 | Vertex* vertices; 45 | 46 | uint* triangle_heap_; 47 | uint* triangle_heap_counter_; 48 | Triangle* triangles; 49 | 50 | #ifdef __CUDACC__ 51 | public: 52 | __device__ 53 | uint AllocVertex() { 54 | uint addr = atomicSub(&vertex_heap_counter_[0], 1); 55 | if (addr < MEMORY_LIMIT) { 56 | printf("vertex heap: %d -> %d\n", addr, vertex_heap_[addr]); 57 | } 58 | return vertex_heap_[addr]; 59 | } 60 | __device__ 61 | void FreeVertex(uint ptr) { 62 | uint addr = atomicAdd(&vertex_heap_counter_[0], 1); 63 | vertex_heap_[addr + 1] = ptr; 64 | } 65 | 66 | __device__ 67 | uint AllocTriangle() { 68 | uint addr = atomicSub(&triangle_heap_counter_[0], 1); 69 | if (addr < MEMORY_LIMIT) { 70 | printf("triangle heap: %d -> %d\n", addr, triangle_heap_[addr]); 71 | } 72 | return triangle_heap_[addr]; 73 | } 74 | __device__ 75 | void FreeTriangle(uint ptr) { 76 | uint addr = atomicAdd(&triangle_heap_counter_[0], 1); 77 | triangle_heap_[addr + 1] = ptr; 78 | } 79 | 80 | /// Release is NOT always a FREE operation 81 | __device__ 82 | void ReleaseTriangle(Triangle& triangle) { 83 | int3 vertex_ptrs = triangle.vertex_ptrs; 84 | atomicSub(&vertices[vertex_ptrs.x].ref_count, 1); 85 | atomicSub(&vertices[vertex_ptrs.y].ref_count, 1); 86 | atomicSub(&vertices[vertex_ptrs.z].ref_count, 1); 87 | } 88 | 89 | __device__ 90 | void AssignTriangle(Triangle& triangle, int3 vertex_ptrs) { 91 | triangle.vertex_ptrs = vertex_ptrs; 92 | atomicAdd(&vertices[vertex_ptrs.x].ref_count, 1); 93 | atomicAdd(&vertices[vertex_ptrs.y].ref_count, 1); 94 | atomicAdd(&vertices[vertex_ptrs.z].ref_count, 1); 95 | } 96 | 97 | __device__ 98 | void ComputeTriangleNormal(Triangle& triangle) { 99 | int3 vertex_ptrs = triangle.vertex_ptrs; 100 | float3 p0 = vertices[vertex_ptrs.x].pos; 101 | float3 p1 = vertices[vertex_ptrs.y].pos; 102 | float3 p2 = vertices[vertex_ptrs.z].pos; 103 | float3 n = normalize(cross(p2 - p0, p1 - p0)); 104 | vertices[vertex_ptrs.x].normal = n; 105 | vertices[vertex_ptrs.y].normal = n; 106 | vertices[vertex_ptrs.z].normal = n; 107 | } 108 | #endif // __CUDACC__ 109 | MeshParams mesh_params_; 110 | 111 | }; 112 | 113 | #endif //VOXEL_HASHING_MESH_H 114 | -------------------------------------------------------------------------------- /src/optimize/linear_equations.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-25. 3 | // 4 | 5 | #include 6 | #include "linear_equations.h" 7 | 8 | 9 | __global__ 10 | void SolveSensorDataEquationKernel( 11 | SensorLinearEquations linear_equations, 12 | const SensorData sensor_data, 13 | const SensorParams params, 14 | float4x4 wTc, 15 | GeometryHelper geometry_helper 16 | ) { 17 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 18 | const int y = blockIdx.y * blockDim.y + threadIdx.y; 19 | 20 | const int idx = y * params.width + x; 21 | float depth = tex2D(sensor_data.depth_texture, x, y); 22 | if (depth == MINF || depth == 0.0f || depth > geometry_helper.sdf_upper_bound) 23 | return; 24 | 25 | float3 input_pos = geometry_helper.ImageReprojectToCamera(x, y, depth, 26 | params.fx, params.fy, 27 | params.cx, params.cy); 28 | input_pos = wTc * input_pos; 29 | float3 predict_pos = (float3x3::getIdentity() + linear_equations.A[idx]).getInverse() 30 | * (input_pos + linear_equations.b[idx]); 31 | //float3x3 A = linear_equations.A[idx]; 32 | //float3 b = linear_equations.b[idx]; 33 | // TODO: low support -> outlier 34 | float diff = length(input_pos - predict_pos); 35 | linear_equations.b[idx] = wTc.getInverse() * predict_pos; 36 | // if (diff < 1e-6) return; 37 | // printf("(%d %d) -> \n %f %f %f, %f\n %f %f %f, %f\n %f %f %f, %f\n" 38 | // "-diff:%f, (%f %f %f) -> (%f %f %f)\n", 39 | // x, y, 40 | // A.entries[0],A.entries[1],A.entries[2], b.x, 41 | // A.entries[3],A.entries[4],A.entries[5], b.y, 42 | // A.entries[6],A.entries[7],A.entries[8], b.z, 43 | // diff, 44 | // input_pos.x, input_pos.y, input_pos.z, 45 | // predict_pos.x, predict_pos.y, predict_pos.z); 46 | } 47 | 48 | 49 | void SolveSensorDataEquation( 50 | SensorLinearEquations& linear_equations, 51 | Sensor& sensor, 52 | GeometryHelper& geometry_helper 53 | ) { 54 | const SensorParams& params = sensor.sensor_params(); 55 | const SensorData& data = sensor.data(); 56 | uint width = params.width; 57 | uint height = params.height; 58 | 59 | const int threads_per_block = 16; 60 | const dim3 grid_size((width + threads_per_block - 1)/threads_per_block, 61 | (height + threads_per_block - 1)/threads_per_block); 62 | const dim3 block_size(threads_per_block, threads_per_block); 63 | 64 | SolveSensorDataEquationKernel <<>> ( 65 | linear_equations, 66 | data, 67 | params, 68 | sensor.wTc(), 69 | geometry_helper); 70 | } 71 | 72 | void SensorLinearEquations::Alloc(int width, int height) { 73 | width_ = width; 74 | height_ = height; 75 | 76 | if (!is_allocated_on_gpu_) { 77 | cudaMalloc(&A, sizeof(float3x3) * width*height); 78 | cudaMalloc(&b, sizeof(float3) * width*height); 79 | is_allocated_on_gpu_ = true; 80 | } 81 | } 82 | 83 | void SensorLinearEquations::Free() { 84 | if (is_allocated_on_gpu_) { 85 | cudaFree(A); 86 | cudaFree(b); 87 | is_allocated_on_gpu_ = false; 88 | } 89 | } 90 | 91 | void SensorLinearEquations::Reset() { 92 | if (is_allocated_on_gpu_) { 93 | cudaMemset(A, 0, sizeof(float3x3)*width_*height_); 94 | cudaMemset(b, 0, sizeof(float3)*width_*height_); 95 | } 96 | } -------------------------------------------------------------------------------- /cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /src/visualization/compact_mesh.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-21. 3 | // 4 | 5 | #include "compact_mesh.h" 6 | #include "helper_cuda.h" 7 | //////////////////// 8 | /// class CompactMesh 9 | //////////////////// 10 | 11 | /// Life cycle 12 | //CompactMesh::~CompactMesh() { 13 | // Free(); 14 | //} 15 | 16 | void CompactMesh::Alloc(const MeshParams &mesh_params) { 17 | if (! is_allocated_on_gpu_) { 18 | checkCudaErrors(cudaMalloc(&vertex_remapper_, 19 | sizeof(int) * mesh_params.max_vertex_count)); 20 | 21 | checkCudaErrors(cudaMalloc(&vertex_counter_, 22 | sizeof(uint))); 23 | checkCudaErrors(cudaMalloc(&vertices_ref_count_, 24 | sizeof(int) * mesh_params.max_vertex_count)); 25 | checkCudaErrors(cudaMalloc(&vertices_, 26 | sizeof(float3) * mesh_params.max_vertex_count)); 27 | checkCudaErrors(cudaMalloc(&normals_, 28 | sizeof(float3) * mesh_params.max_vertex_count)); 29 | checkCudaErrors(cudaMalloc(&colors_, 30 | sizeof(float3) * mesh_params.max_vertex_count)); 31 | 32 | checkCudaErrors(cudaMalloc(&triangle_counter_, 33 | sizeof(uint))); 34 | checkCudaErrors(cudaMalloc(&triangles_ref_count_, 35 | sizeof(int) * mesh_params.max_triangle_count)); 36 | checkCudaErrors(cudaMalloc(&triangles_, 37 | sizeof(int3) * mesh_params.max_triangle_count)); 38 | is_allocated_on_gpu_ = true; 39 | } 40 | } 41 | 42 | void CompactMesh::Free() { 43 | if (is_allocated_on_gpu_) { 44 | checkCudaErrors(cudaFree(vertex_remapper_)); 45 | 46 | checkCudaErrors(cudaFree(vertex_counter_)); 47 | checkCudaErrors(cudaFree(vertices_ref_count_)); 48 | checkCudaErrors(cudaFree(vertices_)); 49 | checkCudaErrors(cudaFree(normals_)); 50 | checkCudaErrors(cudaFree(colors_)); 51 | 52 | checkCudaErrors(cudaFree(triangle_counter_)); 53 | checkCudaErrors(cudaFree(triangles_ref_count_)); 54 | checkCudaErrors(cudaFree(triangles_)); 55 | } 56 | } 57 | 58 | void CompactMesh::Resize(const MeshParams &mesh_params) { 59 | mesh_params_ = mesh_params; 60 | if (is_allocated_on_gpu_) { 61 | Free(); 62 | } 63 | Alloc(mesh_params); 64 | Reset(); 65 | } 66 | 67 | /// Reset 68 | void CompactMesh::Reset() { 69 | checkCudaErrors(cudaMemset(vertex_remapper_, 0xff, 70 | sizeof(int) * mesh_params_.max_vertex_count)); 71 | checkCudaErrors(cudaMemset(vertices_ref_count_, 0, 72 | sizeof(int) * mesh_params_.max_vertex_count)); 73 | checkCudaErrors(cudaMemset(vertex_counter_, 74 | 0, sizeof(uint))); 75 | checkCudaErrors(cudaMemset(triangles_ref_count_, 0, 76 | sizeof(int) * mesh_params_.max_triangle_count)); 77 | checkCudaErrors(cudaMemset(triangle_counter_, 78 | 0, sizeof(uint))); 79 | } 80 | 81 | uint CompactMesh::vertex_count() { 82 | uint compact_vertex_count; 83 | checkCudaErrors(cudaMemcpy(&compact_vertex_count, 84 | vertex_counter_, 85 | sizeof(uint), cudaMemcpyDeviceToHost)); 86 | return compact_vertex_count; 87 | } 88 | 89 | uint CompactMesh::triangle_count() { 90 | uint compact_triangle_count; 91 | checkCudaErrors(cudaMemcpy(&compact_triangle_count, 92 | triangle_counter_, 93 | sizeof(uint), cudaMemcpyDeviceToHost)); 94 | return compact_triangle_count; 95 | } 96 | -------------------------------------------------------------------------------- /src/engine/visualizing_engine.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #ifndef ENGINE_VISUALIZING_ENGINE_H 6 | #define ENGINE_VISUALIZING_ENGINE_H 7 | 8 | #include 9 | #include 10 | #include "glwrapper.h" 11 | #include "visualization/compact_mesh.h" 12 | #include "visualization/ray_caster.h" 13 | #include "visualization/bounding_box.h" 14 | 15 | // TODO: setup a factory 16 | class VisualizingEngine { 17 | public: 18 | VisualizingEngine() = default; 19 | void Init(std::string, int width, int height); 20 | VisualizingEngine(std::string window_name, int width, int height); 21 | ~VisualizingEngine(); 22 | 23 | void set_interaction_mode(bool is_free); 24 | void update_view_matrix(); 25 | void set_view_matrix(glm::mat4 view); 26 | 27 | int Render(); 28 | cv::Mat Capture() { 29 | return window_.CaptureRGB(); 30 | } 31 | // Call set_lights before this 32 | void set_light(gl::Light& light); 33 | void LoadLight(std::string path); 34 | void BindMainProgram( 35 | uint max_vertices, 36 | uint max_triangles, 37 | bool enable_global_mesh, 38 | bool enable_polygon_mode, 39 | bool enable_color 40 | ); 41 | void BindMainUniforms(); 42 | void BindMainData(); 43 | void RenderMain(); 44 | 45 | // At current assume all kinds of data uses the same program 46 | void BuildHelperProgram(); 47 | void BindHelperUniforms(); 48 | void RenderHelper(); 49 | 50 | void InitBoundingBoxData(uint max_vertices); 51 | void BindBoundingBoxData(); 52 | void InitTrajectoryData(uint max_vertices); 53 | void BindTrajectoryData(); 54 | 55 | void BuildRayCaster(const RayCasterParams& ray_caster_params); 56 | 57 | // @method 58 | // at @param view look at the @param blocks 59 | // find neighbors in @param hash_table 60 | // and compute with the @param geometry_helper 61 | void RenderRayCaster( 62 | float4x4 view, 63 | BlockArray& blocks, 64 | HashTable& hash_table, 65 | GeometryHelper& geometry_helper 66 | ) ; 67 | 68 | bool enable_interaction() { 69 | return enable_interaction_; 70 | } 71 | bool enable_global_mesh() { 72 | return enable_global_mesh_; 73 | } 74 | bool enable_polygon_mode() { 75 | return enable_polygon_mode_; 76 | } 77 | bool enable_ray_casting() { 78 | return enable_ray_casting_; 79 | } 80 | bool enable_bounding_box() { 81 | return enable_bounding_box_; 82 | } 83 | bool enable_trajectory() { 84 | return enable_trajectory_; 85 | } 86 | CompactMesh& compact_mesh() { 87 | return compact_mesh_; 88 | } 89 | BoundingBox& bounding_box() { 90 | return bounding_box_; 91 | } 92 | Trajectory& trajectory() { 93 | return trajectory_; 94 | } 95 | 96 | private: 97 | bool enable_interaction_ = false; 98 | bool enable_global_mesh_ = false; 99 | bool enable_ray_casting_ = false; 100 | bool enable_bounding_box_ = false; 101 | bool enable_trajectory_ = false; 102 | bool enable_polygon_mode_ = false; 103 | bool enable_color_ = false; 104 | 105 | // Lighting conditions 106 | gl::Light light_; 107 | 108 | // Shared viewpoint 109 | glm::mat4 mvp_; 110 | glm::mat4 view_; 111 | gl::Window window_; 112 | gl::Camera camera_; 113 | 114 | // Main shader 115 | gl::Program main_program_; 116 | gl::Uniforms main_uniforms_; 117 | gl::Args main_args_; 118 | 119 | gl::Program helper_program_; 120 | gl::Uniforms helper_uniforms_; 121 | 122 | gl::Args box_args_; 123 | gl::Args trajectory_args_; 124 | 125 | // Raycaster 126 | RayCaster ray_caster_; 127 | CompactMesh compact_mesh_; 128 | BoundingBox bounding_box_; 129 | Trajectory trajectory_; 130 | }; 131 | 132 | 133 | #endif //MESH_HASHING_VISUALIZING_ENGINE_H 134 | -------------------------------------------------------------------------------- /src/app/block_analysis.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wqy on 17-10-30. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "sensor/rgbd_data_provider.h" 18 | 19 | int main(int argc, char **argv) { 20 | LoggingEngine log_engine_; 21 | log_engine_.Init("."); 22 | 23 | int selected_frame_idx = 98; 24 | int iter = 0; 25 | float truncation = 0.01f; 26 | BlockMap blocks; 27 | std::stringstream ss(""); 28 | ss << "primal_dual_" << iter << "_" << selected_frame_idx; 29 | LOG(INFO) << "Reading block from disk"; 30 | blocks = log_engine_.ReadRawBlocks(ss.str()); 31 | 32 | std::vector pos; 33 | std::vector sdf; 34 | pos.reserve(blocks.size() * BLOCK_SIZE); 35 | sdf.reserve(blocks.size() * BLOCK_SIZE); 36 | for (auto &&block:blocks) { 37 | float delta = 1.0f; 38 | for (int i = 0; i < BLOCK_SIDE_LENGTH; ++i) { 39 | for (int j = 0; j < BLOCK_SIDE_LENGTH; ++j) { 40 | for (int k = 0; k < BLOCK_SIDE_LENGTH; ++k) { 41 | int index = i * 8 * 8 + j * 8 + k; 42 | float3 voxel_pos = {block.first.x + delta * i, 43 | block.first.y + delta * j, 44 | block.first.z + delta * k}; 45 | if (block.second.voxels[index].inv_sigma2 > 0) { 46 | pos.emplace_back(voxel_pos); 47 | sdf.emplace_back(ValToRGB(block.second.voxels[index].sdf, 48 | -truncation, 49 | truncation)); 50 | } 51 | } 52 | } 53 | } 54 | } 55 | LOG(INFO) << "Block info loaded"; 56 | 57 | gl::Window window("BlockAnalysis", 640, 480); 58 | gl::Camera camera(window.visual_width(), window.visual_height()); 59 | camera.SwitchInteraction(true); 60 | glm::mat4 model = glm::mat4(1.0); 61 | model[1][1] = -1; 62 | model[2][2] = -1; 63 | camera.set_model(model); 64 | gl::Program program; 65 | program.Load("../src/extern/opengl-wrapper/shader/block_analysis_vertex.glsl", 66 | gl::kVertexShader); 67 | program.Load("../src/extern/opengl-wrapper/shader/block_analysis_fragment.glsl", 68 | gl::kFragmentShader); 69 | program.Build(); 70 | gl::Uniforms uniforms; 71 | uniforms.GetLocation(program.id(), "mvp", gl::kMatrix4f); 72 | 73 | gl::Args args(2); 74 | args.BindBuffer(0, {GL_ARRAY_BUFFER, sizeof(float), 3, GL_FLOAT}, 75 | pos.size(), pos.data()); 76 | args.BindBuffer(1, {GL_ARRAY_BUFFER, sizeof(float), 3, GL_FLOAT}, 77 | sdf.size(), sdf.data()); 78 | 79 | glfwPollEvents(); 80 | glClearColor(1.0f, 1.0f, 1.0f, 0.0f); 81 | glEnable(GL_DEPTH_TEST); 82 | glDepthFunc(GL_LESS); 83 | 84 | std::string base = ss.str(); 85 | ss.str(""); 86 | int idx = 0; 87 | do { 88 | // Clear the screen 89 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 90 | 91 | camera.UpdateView(window); 92 | glm::mat4 mvp = camera.mvp(); 93 | 94 | glUseProgram(program.id()); 95 | uniforms.Bind("mvp", &mvp, 1); 96 | glBindVertexArray(args.vao()); 97 | glEnable(GL_PROGRAM_POINT_SIZE); 98 | glDrawArrays(GL_POINTS, 0, pos.size()); 99 | glBindVertexArray(0); 100 | 101 | window.swap_buffer(); 102 | if (window.get_key(GLFW_KEY_ENTER) == GLFW_PRESS) { 103 | ss << idx++; 104 | cv::imwrite(base + ss.str() + ".png" , window.CaptureRGB()); 105 | } 106 | } while (window.get_key(GLFW_KEY_ESCAPE) != GLFW_PRESS && 107 | window.should_close() == 0); 108 | 109 | glfwTerminate(); 110 | return 0; 111 | } -------------------------------------------------------------------------------- /src/mapping/update_simple.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "core/block_array.h" 5 | #include "mapping/update_simple.h" 6 | #include "engine/main_engine.h" 7 | #include "sensor/rgbd_sensor.h" 8 | #include "geometry/spatial_query.h" 9 | 10 | //////////////////// 11 | /// Device code 12 | //////////////////// 13 | __global__ 14 | void UpdateBlocksSimpleKernel( 15 | EntryArray candidate_entries, 16 | BlockArray blocks, 17 | SensorData sensor_data, 18 | SensorParams sensor_params, 19 | float4x4 cTw, 20 | HashTable hash_table, 21 | GeometryHelper geometry_helper 22 | ) { 23 | 24 | //TODO check if we should load this in shared memory (entries) 25 | /// 1. Select voxel 26 | const HashEntry &entry = candidate_entries[blockIdx.x]; 27 | int3 voxel_base_pos = geometry_helper.BlockToVoxel(entry.pos); 28 | uint local_idx = threadIdx.x; //inside of an SDF block 29 | int3 voxel_pos = voxel_base_pos + make_int3(geometry_helper.DevectorizeIndex(local_idx)); 30 | 31 | Voxel &this_voxel = blocks[entry.ptr].voxels[local_idx]; 32 | /// 2. Project to camera 33 | float3 world_pos = geometry_helper.VoxelToWorld(voxel_pos); 34 | float3 camera_pos = cTw * world_pos; 35 | uint2 image_pos = make_uint2( 36 | geometry_helper.CameraProjectToImagei(camera_pos, 37 | sensor_params.fx, sensor_params.fy, 38 | sensor_params.cx, sensor_params.cy)); 39 | if (image_pos.x >= sensor_params.width 40 | || image_pos.y >= sensor_params.height) 41 | return; 42 | 43 | /// 3. Find correspondent depth observation 44 | float depth = tex2D(sensor_data.depth_texture, image_pos.x, image_pos.y); 45 | if (depth == MINF || depth == 0.0f || depth >= geometry_helper.sdf_upper_bound) 46 | return; 47 | 48 | float sdf = depth - camera_pos.z; 49 | float normalized_depth = geometry_helper.NormalizeDepth( 50 | depth, 51 | sensor_params.min_depth_range, 52 | sensor_params.max_depth_range 53 | ); 54 | float inv_sigma2 = fmaxf(10 * geometry_helper.weight_sample * (1.0f - normalized_depth), 55 | 1.0f); 56 | float truncation = geometry_helper.truncate_distance(depth); 57 | if (sdf <= -truncation) 58 | return; 59 | if (sdf >= 0.0f) { 60 | sdf = fminf(truncation, sdf); 61 | } else { 62 | sdf = fmaxf(-truncation, sdf); 63 | } 64 | 65 | /// 5. Update 66 | Voxel delta; 67 | delta.sdf = sdf; 68 | delta.inv_sigma2 = inv_sigma2; 69 | 70 | if (sensor_data.color_data) { 71 | float4 color = tex2D(sensor_data.color_texture, image_pos.x, image_pos.y); 72 | delta.color = make_uchar3(255 * color.x, 255 * color.y, 255 * color.z); 73 | } else { 74 | delta.color = make_uchar3(0, 255, 0); 75 | } 76 | this_voxel.Update(delta); 77 | } 78 | 79 | double UpdateBlocksSimple( 80 | EntryArray &candidate_entries, 81 | BlockArray &blocks, 82 | Sensor &sensor, 83 | HashTable &hash_table, 84 | GeometryHelper &geometry_helper 85 | ) { 86 | 87 | Timer timer; 88 | timer.Tick(); 89 | const uint threads_per_block = BLOCK_SIZE; 90 | 91 | uint candidate_entry_count = candidate_entries.count(); 92 | if (candidate_entry_count <= 0) 93 | return timer.Tock(); 94 | 95 | const dim3 grid_size(candidate_entry_count, 1); 96 | const dim3 block_size(threads_per_block, 1); 97 | UpdateBlocksSimpleKernel << < grid_size, block_size >> > ( 98 | candidate_entries, 99 | blocks, 100 | sensor.data(), 101 | sensor.sensor_params(), 102 | sensor.cTw(), 103 | hash_table, 104 | geometry_helper); 105 | checkCudaErrors(cudaDeviceSynchronize()); 106 | checkCudaErrors(cudaGetLastError()); 107 | return timer.Tock(); 108 | } -------------------------------------------------------------------------------- /src/util/block_visualizer.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-6-10. 3 | // 4 | 5 | #include "../renderer.h" 6 | #include "../core/block.h" 7 | #include "../visualization/color_util.h" 8 | 9 | int main() { 10 | Renderer renderer("Block", 640, 480); 11 | renderer.enable_navigation() = true; 12 | 13 | std::ifstream in("../result/statistics/(1,1,5).txt"); 14 | int block_side_length, block_voxel_count; 15 | in >> block_side_length >> block_voxel_count; 16 | LOG(INFO) << "Sizes: " << block_side_length << " " << block_voxel_count; 17 | 18 | LOG(INFO) << block_side_length << " " << block_voxel_count; 19 | PointObject voxel_points(block_voxel_count); 20 | float3 *voxel_centers = new float3[block_voxel_count]; 21 | float3 *voxel_colors = new float3[block_voxel_count]; 22 | 23 | for (int i = 0; i < block_voxel_count; ++i) { 24 | float3 voxel_pos; float sdf; uint inv_sigma2; 25 | in >> voxel_pos.x >> voxel_pos.y >> voxel_pos.z; 26 | in >> sdf >> inv_sigma2; 27 | voxel_centers[i] = voxel_pos; 28 | voxel_colors[i] = (inv_sigma2 == 0) ? make_float3(1) : ValToRGB(sdf, -0.1f, 0.1f); 29 | } 30 | 31 | float3* cuda_voxel_centers, *cuda_voxel_colors; 32 | checkCudaErrors(cudaMalloc(&cuda_voxel_centers, sizeof(float3) * block_voxel_count)); 33 | checkCudaErrors(cudaMalloc(&cuda_voxel_colors, sizeof(float3) * block_voxel_count)); 34 | 35 | checkCudaErrors(cudaMemcpy(cuda_voxel_centers, voxel_centers, 36 | sizeof(float3) * block_voxel_count, 37 | cudaMemcpyHostToDevice)); 38 | checkCudaErrors(cudaMemcpy(cuda_voxel_colors, voxel_colors, 39 | sizeof(float3) * block_voxel_count, 40 | cudaMemcpyHostToDevice)); 41 | 42 | voxel_points.SetData(cuda_voxel_centers, block_voxel_count, 43 | cuda_voxel_colors, block_voxel_count); 44 | renderer.AddObject(&voxel_points); 45 | 46 | 47 | MeshObject mesh_object(block_voxel_count * 3, block_voxel_count * 5, kColor); 48 | int vertex_count; 49 | in >> vertex_count; 50 | LOG(INFO) << "Vertex count: " << vertex_count; 51 | std::vector vertex_positions; 52 | std::vector vertex_colors; 53 | for (int i = 0; i < vertex_count; ++i) { 54 | float3 pos, color; 55 | in >> pos.x >> pos.y >> pos.z >> color.x >> color.y >> color.z; 56 | vertex_positions.push_back(pos); 57 | vertex_colors.push_back(color); 58 | } 59 | 60 | int triangle_count; 61 | in >> triangle_count; 62 | LOG(INFO) << "Triangle count: " << triangle_count; 63 | std::vector triangles; 64 | for (int i = 0; i < triangle_count; ++i) { 65 | int3 triangle; 66 | in >> triangle.x >> triangle.y >> triangle.z; 67 | triangles.push_back(triangle); 68 | } 69 | 70 | float3* cuda_vertex_positions; 71 | float3 *cuda_vertex_colors; 72 | int3 *cuda_triangle_indices; 73 | checkCudaErrors(cudaMalloc(&cuda_vertex_positions, sizeof(float3) * vertex_positions.size())); 74 | checkCudaErrors(cudaMalloc(&cuda_vertex_colors, sizeof(float3) * vertex_colors.size())); 75 | checkCudaErrors(cudaMalloc(&cuda_triangle_indices, sizeof(int3) * triangles.size())); 76 | 77 | checkCudaErrors(cudaMemcpy(cuda_vertex_positions, vertex_positions.data(), 78 | sizeof(float3) * vertex_positions.size(), 79 | cudaMemcpyHostToDevice)); 80 | checkCudaErrors(cudaMemcpy(cuda_vertex_colors, vertex_colors.data(), 81 | sizeof(float3) * vertex_colors.size(), 82 | cudaMemcpyHostToDevice)); 83 | checkCudaErrors(cudaMemcpy(cuda_triangle_indices, triangles.data(), 84 | sizeof(int3) * triangles.size(), 85 | cudaMemcpyHostToDevice)); 86 | 87 | mesh_object.ploygon_mode() = 1; 88 | mesh_object.SetData(cuda_vertex_positions, vertex_positions.size(), 89 | NULL, 0, 90 | cuda_vertex_colors, vertex_colors.size(), 91 | cuda_triangle_indices, triangles.size()); 92 | renderer.AddObject(&mesh_object); 93 | 94 | while (true) { 95 | float4x4 dummy; 96 | renderer.Render(dummy); 97 | } 98 | 99 | return 0; 100 | } 101 | -------------------------------------------------------------------------------- /src/geometry/voxel_query.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-25. 3 | // 4 | 5 | #ifndef GEOMETRY_VOXEL_QUERY_H 6 | #define GEOMETRY_VOXEL_QUERY_H 7 | 8 | #include 9 | #include "geometry_helper.h" 10 | 11 | #include "core/hash_table.h" 12 | #include "core/block_array.h" 13 | 14 | 15 | // TODO(wei): refine it 16 | // function: 17 | // at @param world_pos 18 | // get Voxel in @param blocks 19 | // with the help of @param hash_table and geometry_helper 20 | __device__ 21 | 22 | // function: 23 | // block-pos @param curr_entry -> voxel-pos @param voxel_local_pos 24 | // get Voxel in @param blocks 25 | // with the help of @param hash_table and geometry_helper 26 | __device__ 27 | inline Voxel &GetVoxelRef( 28 | const HashEntry &curr_entry, 29 | const int3 voxel_pos, 30 | BlockArray &blocks, 31 | const HashTable &hash_table, 32 | GeometryHelper &geometry_helper 33 | ) { 34 | int3 block_pos = geometry_helper.VoxelToBlock(voxel_pos); 35 | uint3 offset = geometry_helper.VoxelToOffset(block_pos, voxel_pos); 36 | 37 | if (curr_entry.pos == block_pos) { 38 | uint i = geometry_helper.VectorizeOffset(offset); 39 | return blocks[curr_entry.ptr].voxels[i]; 40 | } else { 41 | HashEntry entry = hash_table.GetEntry(block_pos); 42 | if (entry.ptr == FREE_ENTRY) { 43 | printf("GetVoxelRef: should never reach here!\n"); 44 | } 45 | uint i = geometry_helper.VectorizeOffset(offset); 46 | return blocks[entry.ptr].voxels[i]; 47 | } 48 | } 49 | 50 | __device__ 51 | inline MeshUnit &GetMeshUnitRef( 52 | const HashEntry &curr_entry, 53 | const int3 voxel_pos, 54 | BlockArray &blocks, 55 | const HashTable &hash_table, 56 | GeometryHelper &geometry_helper 57 | ) { 58 | int3 block_pos = geometry_helper.VoxelToBlock(voxel_pos); 59 | uint3 offset = geometry_helper.VoxelToOffset(block_pos, voxel_pos); 60 | 61 | if (curr_entry.pos == block_pos) { 62 | uint i = geometry_helper.VectorizeOffset(offset); 63 | return blocks[curr_entry.ptr].mesh_units[i]; 64 | } else { 65 | HashEntry entry = hash_table.GetEntry(block_pos); 66 | if (entry.ptr == FREE_ENTRY) { 67 | printf("GetVoxelRef: should never reach here!\n"); 68 | } 69 | uint i = geometry_helper.VectorizeOffset(offset); 70 | return blocks[entry.ptr].mesh_units[i]; 71 | } 72 | } 73 | 74 | // function: 75 | // block-pos @param curr_entry -> voxel-pos @param voxel_local_pos 76 | // get SDF in @param blocks 77 | // with the help of @param hash_table and geometry_helper 78 | __device__ 79 | inline bool GetVoxelValue( 80 | const HashEntry &curr_entry, 81 | const int3 voxel_pos, 82 | const BlockArray &blocks, 83 | const HashTable &hash_table, 84 | GeometryHelper &geometry_helper, 85 | Voxel* voxel) { 86 | int3 block_pos = geometry_helper.VoxelToBlock(voxel_pos); 87 | uint3 offset = geometry_helper.VoxelToOffset(block_pos, voxel_pos); 88 | 89 | if (curr_entry.pos == block_pos) { 90 | uint i = geometry_helper.VectorizeOffset(offset); 91 | *voxel = blocks[curr_entry.ptr].voxels[i]; 92 | } else { 93 | HashEntry entry = hash_table.GetEntry(block_pos); 94 | if (entry.ptr == FREE_ENTRY) return false; 95 | uint i = geometry_helper.VectorizeOffset(offset); 96 | *voxel = blocks[entry.ptr].voxels[i]; 97 | } 98 | return true; 99 | } 100 | 101 | __device__ 102 | inline bool GetVoxelValue( 103 | const float3 world_pos, 104 | const BlockArray &blocks, 105 | const HashTable &hash_table, 106 | GeometryHelper &geometry_helper, 107 | Voxel* voxel 108 | ) { 109 | int3 voxel_pos = geometry_helper.WorldToVoxeli(world_pos); 110 | int3 block_pos = geometry_helper.VoxelToBlock(voxel_pos); 111 | uint3 offset = geometry_helper.VoxelToOffset(block_pos, voxel_pos); 112 | 113 | HashEntry entry = hash_table.GetEntry(block_pos); 114 | if (entry.ptr == FREE_ENTRY) { 115 | voxel->sdf = 0; 116 | voxel->inv_sigma2 = 0; 117 | voxel->color = make_uchar3(0,0,0); 118 | return false; 119 | } else { 120 | uint i = geometry_helper.VectorizeOffset(offset); 121 | const Voxel& v = blocks[entry.ptr].voxels[i]; 122 | voxel->sdf = v.sdf; 123 | voxel->inv_sigma2 = v.inv_sigma2; 124 | voxel->color = v.color; 125 | voxel->a = v.a; 126 | voxel->b = v.b; 127 | return true; 128 | } 129 | } 130 | 131 | #endif //MESH_HASHING_SPATIAL_QUERY_H 132 | -------------------------------------------------------------------------------- /src/app/orb_slam2.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include "core/params.h" 34 | #include "io/config_manager.h" 35 | #include "engine/main_engine.h" 36 | #include "sensor/rgbd_sensor.h" 37 | #include "visualization/ray_caster.h" 38 | #include "glwrapper.h" 39 | 40 | const std::string orb_configs[] = { 41 | "../config/ORB/ICL.yaml", 42 | "../config/ORB/TUM1.yaml", 43 | "../config/ORB/TUM2.yaml", 44 | "../config/ORB/TUM3.yaml", 45 | "../config/ORB/SUN3D.yaml", 46 | "../config/ORB/SUN3D_ORIGINAL.yaml", 47 | "../config/ORB/PKU.yaml" 48 | }; 49 | 50 | std::string path_to_vocabulary = "../src/extern/orb_slam2/Vocabulary/ORBvoc.bin"; 51 | 52 | Light light = { 53 | { 54 | glm::vec3(0, -2, 0), 55 | glm::vec3(4, -2, 0) 56 | }, 57 | glm::vec3(1, 1, 1), 58 | 3.0f 59 | }; 60 | 61 | float4x4 MatTofloat4x4(cv::Mat m) { 62 | float4x4 T; 63 | T.setIdentity(); 64 | for (int i = 0; i < 4; ++i) 65 | for (int j = 0; j < 4; ++j) 66 | T.entries2[i][j] = (float)m.at(i, j); 67 | return T; 68 | } 69 | 70 | int main(int argc, char **argv) { 71 | /// Use this to substitute tedious argv parsing 72 | RuntimeParams args; 73 | LoadRuntimeParams("../config/args.yml", args); 74 | 75 | ConfigManager config; 76 | RGBDDataProvider rgbd_local_sequence; 77 | 78 | DatasetType dataset_type = DatasetType(args.dataset_type); 79 | config.LoadConfig(dataset_type); 80 | rgbd_local_sequence.LoadDataset(dataset_type); 81 | Sensor sensor(config.sensor_params); 82 | 83 | MainEngine main_engine( 84 | config.hash_params, 85 | config.sdf_params, 86 | config.mesh_params, 87 | config.sensor_params, 88 | config.ray_caster_params 89 | ); 90 | main_engine.ConfigMappingEngine( 91 | false 92 | ); 93 | main_engine.ConfigVisualizingEngine( 94 | light, 95 | args.enable_navigation, 96 | args.enable_global_mesh, 97 | args.enable_bounding_box, 98 | args.enable_trajectory, 99 | args.enable_polygon_mode, 100 | args.enable_ray_casting 101 | ); 102 | main_engine.ConfigLoggingEngine( 103 | ".", 104 | args.enable_video_recording, 105 | args.enable_ply_saving 106 | ); 107 | main_engine.enable_sdf_gradient() = args.enable_sdf_gradient; 108 | 109 | ORB_SLAM2::System orb_slam_engine( 110 | path_to_vocabulary, 111 | orb_configs[dataset_type], 112 | ORB_SLAM2::System::RGBD, 113 | true); 114 | 115 | double tframe; 116 | cv::Mat color, depth; 117 | float4x4 wTc, cTw; 118 | int frame_count = 0; 119 | while (rgbd_local_sequence.ProvideData(depth, color, wTc)) { 120 | frame_count ++; 121 | if (args.run_frames > 0 && frame_count > args.run_frames) 122 | break; 123 | 124 | cv::Mat color_slam = color.clone(); 125 | cv::Mat depth_slam = depth.clone(); 126 | cv::Mat cTw_orb = orb_slam_engine.TrackRGBD(color_slam, depth_slam, tframe); 127 | if (cTw_orb.empty()) continue; 128 | cTw = MatTofloat4x4(cTw_orb); 129 | wTc = cTw.getInverse(); 130 | 131 | sensor.Process(depth, color); // abandon wTc 132 | sensor.set_transform(wTc); 133 | cTw = wTc.getInverse(); 134 | 135 | main_engine.Mapping(sensor); 136 | main_engine.Meshing(); 137 | main_engine.Visualize(cTw); 138 | 139 | main_engine.Log(); 140 | main_engine.Recycle(); 141 | } 142 | 143 | main_engine.FinalLog(); 144 | orb_slam_engine.Shutdown(); 145 | return 0; 146 | } -------------------------------------------------------------------------------- /src/sensor/preprocess.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "core/params.h" 6 | #include "preprocess.h" 7 | 8 | #define MINF __int_as_float(0xff800000) 9 | 10 | __global__ 11 | void ResetInlierRatioKernel( 12 | float* inlier_ratio, 13 | int width, 14 | int height 15 | ) { 16 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 17 | const int y = blockIdx.y * blockDim.y + threadIdx.y; 18 | 19 | if (x >= width || y >= height) return; 20 | const int idx = y * width + x; 21 | /// Convert mm -> m 22 | inlier_ratio[idx] = 0.1f; 23 | } 24 | 25 | __global__ 26 | void ConvertDepthFormatKernel( 27 | float *dst, short *src, 28 | uint width, uint height, 29 | float range_factor, 30 | float min_depth_range, 31 | float max_depth_range 32 | ) { 33 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 34 | const int y = blockIdx.y * blockDim.y + threadIdx.y; 35 | 36 | if (x >= width || y >= height) return; 37 | const int idx = y * width + x; 38 | /// Convert mm -> m 39 | const float depth = range_factor * src[idx]; 40 | bool is_valid = (depth >= min_depth_range && depth <= max_depth_range); 41 | dst[idx] = is_valid ? depth : MINF; 42 | } 43 | 44 | __global__ 45 | void ConvertColorFormatKernel(float4 *dst, uchar4 *src, 46 | uint width, uint height) { 47 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 48 | const int y = blockIdx.y * blockDim.y + threadIdx.y; 49 | 50 | if (x >= width || y >= height) return; 51 | const int idx = y * width + x; 52 | 53 | uchar4 c = src[idx]; 54 | bool is_valid = (c.x != 0 && c.y != 0 && c.z != 0); 55 | dst[idx] = is_valid ? make_float4(c.z / 255.0f, c.y / 255.0f, 56 | c.x / 255.0f, c.w / 255.0f) 57 | : make_float4(MINF, MINF, MINF, MINF); 58 | } 59 | 60 | ////////// 61 | /// Member function: (CPU calling GPU kernels) 62 | __host__ 63 | void ResetInlierRatio( 64 | float* inlier_ratio, 65 | SensorParams& params 66 | ) { 67 | uint width = params.width; 68 | uint height = params.height; 69 | 70 | const uint threads_per_block = 16; 71 | const dim3 grid_size((width + threads_per_block - 1)/threads_per_block, 72 | (height + threads_per_block - 1)/threads_per_block); 73 | const dim3 block_size(threads_per_block, threads_per_block); 74 | ResetInlierRatioKernel<<>>( 75 | inlier_ratio, width, height); 76 | } 77 | 78 | __host__ 79 | void ConvertDepthFormat( 80 | cv::Mat& depth_img, 81 | short* depth_buffer, 82 | float* depth_data, 83 | SensorParams& params 84 | ) { 85 | /// First copy cpu data in to cuda short 86 | uint width = params.width; 87 | uint height = params.height; 88 | uint image_size = width * height; 89 | 90 | checkCudaErrors(cudaMemcpy(depth_buffer, (short *)depth_img.data, 91 | sizeof(short) * image_size, 92 | cudaMemcpyHostToDevice)); 93 | 94 | const uint threads_per_block = 16; 95 | const dim3 grid_size((width + threads_per_block - 1)/threads_per_block, 96 | (height + threads_per_block - 1)/threads_per_block); 97 | const dim3 block_size(threads_per_block, threads_per_block); 98 | ConvertDepthFormatKernel<<>>( 99 | depth_data, 100 | depth_buffer, 101 | width, height, 102 | params.range_factor, 103 | params.min_depth_range, 104 | params.max_depth_range); 105 | } 106 | 107 | __host__ 108 | void ConvertColorFormat( 109 | cv::Mat &color_img, 110 | uchar4* color_buffer, 111 | float4* color_data, 112 | SensorParams& params 113 | ) { 114 | 115 | uint width = params.width; 116 | uint height = params.height; 117 | uint image_size = width * height; 118 | 119 | checkCudaErrors(cudaMemcpy(color_buffer, color_img.data, 120 | sizeof(uchar4) * image_size, 121 | cudaMemcpyHostToDevice)); 122 | 123 | const int threads_per_block = 16; 124 | const dim3 grid_size((width + threads_per_block - 1)/threads_per_block, 125 | (height + threads_per_block - 1)/threads_per_block); 126 | const dim3 block_size(threads_per_block, threads_per_block); 127 | 128 | ConvertColorFormatKernel <<>>( 129 | color_data, 130 | color_buffer, 131 | width, 132 | height); 133 | } 134 | 135 | -------------------------------------------------------------------------------- /src/core/collect_block_array.cu: -------------------------------------------------------------------------------- 1 | #include "matrix.h" 2 | 3 | #include "engine/main_engine.h" 4 | #include "sensor/rgbd_sensor.h" 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "meshing/mc_tables.h" 16 | 17 | 18 | #define PINF __int_as_float(0x7f800000) 19 | 20 | //////////////////// 21 | /// class MappingEngine - compress, recycle 22 | //////////////////// 23 | 24 | /// Condition: IsBlockInCameraFrustum 25 | __global__ 26 | void CollectBlocksInFrustumKernel( 27 | HashTable hash_table, 28 | SensorParams sensor_params, 29 | float4x4 c_T_w, 30 | GeometryHelper geometry_helper, 31 | EntryArray candidate_entries 32 | ) { 33 | const uint idx = blockIdx.x * blockDim.x + threadIdx.x; 34 | 35 | __shared__ int local_counter; 36 | if (threadIdx.x == 0) local_counter = 0; 37 | __syncthreads(); 38 | 39 | int addr_local = -1; 40 | if (idx < hash_table.entry_count 41 | && hash_table.entry(idx).ptr != FREE_ENTRY 42 | && geometry_helper.IsBlockInCameraFrustum(c_T_w, hash_table.entry(idx).pos, 43 | sensor_params)) { 44 | addr_local = atomicAdd(&local_counter, 1); 45 | } 46 | __syncthreads(); 47 | 48 | __shared__ int addr_global; 49 | if (threadIdx.x == 0 && local_counter > 0) { 50 | addr_global = atomicAdd(&candidate_entries.counter(), 51 | local_counter); 52 | } 53 | __syncthreads(); 54 | 55 | if (addr_local != -1) { 56 | const uint addr = addr_global + addr_local; 57 | candidate_entries[addr] = hash_table.entry(idx); 58 | } 59 | } 60 | 61 | __global__ 62 | void CollectAllBlocksKernel( 63 | HashTable hash_table, 64 | EntryArray candidate_entries 65 | ) { 66 | const uint idx = blockIdx.x * blockDim.x + threadIdx.x; 67 | 68 | __shared__ int local_counter; 69 | if (threadIdx.x == 0) local_counter = 0; 70 | __syncthreads(); 71 | 72 | int addr_local = -1; 73 | if (idx < hash_table.entry_count 74 | && hash_table.entry(idx).ptr != FREE_ENTRY) { 75 | addr_local = atomicAdd(&local_counter, 1); 76 | } 77 | 78 | __syncthreads(); 79 | 80 | __shared__ int addr_global; 81 | if (threadIdx.x == 0 && local_counter > 0) { 82 | addr_global = atomicAdd(&candidate_entries.counter(), 83 | local_counter); 84 | } 85 | __syncthreads(); 86 | 87 | if (addr_local != -1) { 88 | const uint addr = addr_global + addr_local; 89 | candidate_entries[addr] = hash_table.entry(idx); 90 | } 91 | } 92 | 93 | //////////////////// 94 | /// Host code 95 | /////////////////// 96 | 97 | /// Compress discrete hash table entries 98 | void CollectAllBlocks( 99 | HashTable &hash_table, 100 | EntryArray &candidate_entries 101 | ) { 102 | const uint threads_per_block = 256; 103 | 104 | uint entry_count = hash_table.entry_count; 105 | const dim3 grid_size((entry_count + threads_per_block - 1) 106 | / threads_per_block, 1); 107 | const dim3 block_size(threads_per_block, 1); 108 | 109 | candidate_entries.reset_count(); 110 | CollectAllBlocksKernel <<>>( 111 | hash_table, 112 | candidate_entries); 113 | checkCudaErrors(cudaDeviceSynchronize()); 114 | checkCudaErrors(cudaGetLastError()); 115 | 116 | LOG(INFO) << "Block count in all: " 117 | << candidate_entries.count(); 118 | } 119 | 120 | double CollectBlocksInFrustum( 121 | HashTable &hash_table, 122 | Sensor &sensor, 123 | GeometryHelper &geometry_helper, 124 | EntryArray &candidate_entries 125 | ) { 126 | 127 | Timer timer; 128 | timer.Tick(); 129 | const uint threads_per_block = 256; 130 | 131 | uint entry_count = hash_table.entry_count; 132 | 133 | const dim3 grid_size((entry_count + threads_per_block - 1) 134 | / threads_per_block, 1); 135 | const dim3 block_size(threads_per_block, 1); 136 | 137 | candidate_entries.reset_count(); 138 | CollectBlocksInFrustumKernel <<>>( 139 | hash_table, 140 | sensor.sensor_params(), 141 | sensor.cTw(), 142 | geometry_helper, 143 | candidate_entries); 144 | 145 | checkCudaErrors(cudaDeviceSynchronize()); 146 | checkCudaErrors(cudaGetLastError()); 147 | 148 | LOG(INFO) << "Block count in view frustum: " 149 | << candidate_entries.count(); 150 | return timer.Tock(); 151 | } 152 | 153 | -------------------------------------------------------------------------------- /src/core/mesh.cu: -------------------------------------------------------------------------------- 1 | #include "mesh.h" 2 | 3 | #include 4 | #include 5 | #include "params.h" 6 | #include 7 | 8 | //////////////////// 9 | /// class Mesh 10 | //////////////////// 11 | 12 | //////////////////// 13 | /// Device code 14 | //////////////////// 15 | __global__ 16 | void MeshResetVerticesKernel(uint* vertex_heap, Vertex* vertices, int max_vertex_count) { 17 | const uint idx = blockIdx.x * blockDim.x + threadIdx.x; 18 | 19 | if (idx < max_vertex_count) { 20 | vertex_heap[idx] = max_vertex_count - idx - 1; 21 | vertices[idx].Clear(); 22 | } 23 | } 24 | 25 | __global__ 26 | void MeshResetTrianglesKernel(uint* triangle_heap, Triangle* triangles, int max_triangle_count) { 27 | const uint idx = blockIdx.x * blockDim.x + threadIdx.x; 28 | 29 | if (idx < max_triangle_count) { 30 | triangle_heap[idx] = max_triangle_count - idx - 1; 31 | triangles[idx].Clear(); 32 | } 33 | } 34 | 35 | //////////////////// 36 | /// Host code 37 | //////////////////// 38 | // Mesh::~Mesh() { 39 | //Free(); 40 | //} 41 | 42 | __host__ 43 | void Mesh::Alloc(const MeshParams &mesh_params) { 44 | if (!is_allocated_on_gpu_) { 45 | checkCudaErrors(cudaMalloc(&vertex_heap_, 46 | sizeof(uint) * mesh_params.max_vertex_count)); 47 | checkCudaErrors(cudaMalloc(&vertex_heap_counter_, sizeof(uint))); 48 | checkCudaErrors(cudaMalloc(&vertices, 49 | sizeof(Vertex) * mesh_params.max_vertex_count)); 50 | 51 | checkCudaErrors(cudaMalloc(&triangle_heap_, 52 | sizeof(uint) * mesh_params.max_triangle_count)); 53 | checkCudaErrors(cudaMalloc(&triangle_heap_counter_, sizeof(uint))); 54 | checkCudaErrors(cudaMalloc(&triangles, 55 | sizeof(Triangle) * mesh_params.max_triangle_count)); 56 | is_allocated_on_gpu_ = true; 57 | } 58 | } 59 | 60 | void Mesh::Free() { 61 | if (is_allocated_on_gpu_) { 62 | checkCudaErrors(cudaFree(vertex_heap_)); 63 | checkCudaErrors(cudaFree(vertex_heap_counter_)); 64 | checkCudaErrors(cudaFree(vertices)); 65 | 66 | checkCudaErrors(cudaFree(triangle_heap_)); 67 | checkCudaErrors(cudaFree(triangle_heap_counter_)); 68 | checkCudaErrors(cudaFree(triangles)); 69 | 70 | is_allocated_on_gpu_ = false; 71 | } 72 | } 73 | 74 | void Mesh::Resize(const MeshParams &mesh_params) { 75 | mesh_params_ = mesh_params; 76 | if (is_allocated_on_gpu_) { 77 | Free(); 78 | } 79 | Alloc(mesh_params); 80 | Reset(); 81 | } 82 | 83 | void Mesh::Reset() { 84 | uint val; 85 | 86 | val = mesh_params_.max_vertex_count - 1; 87 | checkCudaErrors(cudaMemcpy(vertex_heap_counter_, 88 | &val, 89 | sizeof(uint), 90 | cudaMemcpyHostToDevice)); 91 | 92 | val = mesh_params_.max_triangle_count - 1; 93 | checkCudaErrors(cudaMemcpy(triangle_heap_counter_, 94 | &val, 95 | sizeof(uint), 96 | cudaMemcpyHostToDevice)); 97 | 98 | { 99 | const int threads_per_block = 64; 100 | const dim3 grid_size((mesh_params_.max_vertex_count + threads_per_block - 1) 101 | / threads_per_block, 1); 102 | const dim3 block_size(threads_per_block, 1); 103 | 104 | MeshResetVerticesKernel<<< grid_size, block_size >>> (vertex_heap_, vertices, 105 | mesh_params_.max_vertex_count); 106 | checkCudaErrors(cudaDeviceSynchronize()); 107 | checkCudaErrors(cudaGetLastError()); 108 | } 109 | 110 | { 111 | const int threads_per_block = 64; 112 | const dim3 grid_size((mesh_params_.max_triangle_count + threads_per_block - 1) 113 | / threads_per_block, 1); 114 | const dim3 block_size(threads_per_block, 1); 115 | 116 | MeshResetTrianglesKernel<<>> (triangle_heap_, triangles, 117 | mesh_params_.max_triangle_count); 118 | checkCudaErrors(cudaDeviceSynchronize()); 119 | checkCudaErrors(cudaGetLastError()); 120 | } 121 | } 122 | 123 | uint Mesh::vertex_heap_count() { 124 | uint vertex_heap_count; 125 | checkCudaErrors(cudaMemcpy(&vertex_heap_count, 126 | vertex_heap_counter_, 127 | sizeof(uint), cudaMemcpyDeviceToHost)); 128 | return vertex_heap_count; 129 | } 130 | 131 | uint Mesh::triangle_heap_count() { 132 | uint triangle_heap_count; 133 | checkCudaErrors(cudaMemcpy(&triangle_heap_count, 134 | triangle_heap_counter_, 135 | sizeof(uint), cudaMemcpyDeviceToHost)); 136 | return triangle_heap_count; 137 | } -------------------------------------------------------------------------------- /src/mapping/allocate.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #include 6 | #include "mapping/allocate.h" 7 | 8 | __global__ 9 | void AllocBlockArrayKernel(HashTable hash_table, 10 | SensorData sensor_data, 11 | SensorParams sensor_params, 12 | float4x4 w_T_c, 13 | GeometryHelper geometry_helper) { 14 | 15 | const uint x = blockIdx.x * blockDim.x + threadIdx.x; 16 | const uint y = blockIdx.y * blockDim.y + threadIdx.y; 17 | 18 | if (x >= sensor_params.width || y >= sensor_params.height) 19 | return; 20 | 21 | /// TODO(wei): change it here 22 | /// 1. Get observed data 23 | float depth = tex2D(sensor_data.depth_texture, x, y); 24 | if (depth == MINF || depth == 0.0f 25 | || depth >= geometry_helper.sdf_upper_bound) 26 | return; 27 | 28 | float truncation = geometry_helper.truncate_distance(depth); 29 | float near_depth = fminf(geometry_helper.sdf_upper_bound, depth - truncation); 30 | float far_depth = fminf(geometry_helper.sdf_upper_bound, depth + truncation); 31 | if (near_depth >= far_depth) return; 32 | 33 | float3 camera_pos_near = geometry_helper.ImageReprojectToCamera(x, y, near_depth, 34 | sensor_params.fx, sensor_params.fy, 35 | sensor_params.cx, sensor_params.cy); 36 | float3 camera_pos_far = geometry_helper.ImageReprojectToCamera(x, y, far_depth, 37 | sensor_params.fx, sensor_params.fy, 38 | sensor_params.cx, sensor_params.cy); 39 | 40 | /// 2. Set range where blocks are allocated 41 | float3 world_pos_near = w_T_c * camera_pos_near; 42 | float3 world_pos_far = w_T_c * camera_pos_far; 43 | float3 world_ray_dir = normalize(world_pos_far - world_pos_near); 44 | 45 | int3 block_pos_near = geometry_helper.WorldToBlock(world_pos_near); 46 | int3 block_pos_far = geometry_helper.WorldToBlock(world_pos_far); 47 | float3 block_step = make_float3(sign(world_ray_dir)); 48 | 49 | /// 3. Init zig-zag steps 50 | float3 world_pos_nearest_voxel_center 51 | = geometry_helper.BlockToWorld(block_pos_near + make_int3(clamp(block_step, 0.0, 1.0f))) 52 | - 0.5f * geometry_helper.voxel_size; 53 | float3 t = (world_pos_nearest_voxel_center - world_pos_near) / world_ray_dir; 54 | float3 dt = (block_step * BLOCK_SIDE_LENGTH * geometry_helper.voxel_size) / world_ray_dir; 55 | int3 block_pos_bound = make_int3(make_float3(block_pos_far) + block_step); 56 | 57 | if (world_ray_dir.x == 0.0f) { 58 | t.x = PINF; 59 | dt.x = PINF; 60 | } 61 | if (world_ray_dir.y == 0.0f) { 62 | t.y = PINF; 63 | dt.y = PINF; 64 | } 65 | if (world_ray_dir.z == 0.0f) { 66 | t.z = PINF; 67 | dt.z = PINF; 68 | } 69 | 70 | int3 block_pos_curr = block_pos_near; 71 | /// 4. Go a zig-zag path to ensure all voxels are visited 72 | const uint kMaxIterTime = 1024; 73 | #pragma unroll 1 74 | for (uint iter = 0; iter < kMaxIterTime; ++iter) { 75 | if (geometry_helper.IsBlockInCameraFrustum( 76 | w_T_c.getInverse(), 77 | block_pos_curr, 78 | sensor_params)) { 79 | /// Disable streaming at current 80 | // && !isSDFBlockStreamedOut(idCurrentVoxel, hash_table, is_streamed_mask)) { 81 | hash_table.AllocEntry(block_pos_curr); 82 | } 83 | 84 | // Traverse voxel grid 85 | if (t.x < t.y && t.x < t.z) { 86 | block_pos_curr.x += block_step.x; 87 | if (block_pos_curr.x == block_pos_bound.x) return; 88 | t.x += dt.x; 89 | } else if (t.y < t.z) { 90 | block_pos_curr.y += block_step.y; 91 | if (block_pos_curr.y == block_pos_bound.y) return; 92 | t.y += dt.y; 93 | } else { 94 | block_pos_curr.z += block_step.z; 95 | if (block_pos_curr.z == block_pos_bound.z) return; 96 | t.z += dt.z; 97 | } 98 | } 99 | } 100 | 101 | double AllocBlockArray( 102 | HashTable& hash_table, 103 | Sensor& sensor, 104 | GeometryHelper& geometry_helper 105 | ) { 106 | Timer timer; 107 | timer.Tick(); 108 | hash_table.ResetMutexes(); 109 | 110 | const uint threads_per_block = 8; 111 | const dim3 grid_size((sensor.sensor_params().width + threads_per_block - 1) 112 | /threads_per_block, 113 | (sensor.sensor_params().height + threads_per_block - 1) 114 | /threads_per_block); 115 | const dim3 block_size(threads_per_block, threads_per_block); 116 | 117 | AllocBlockArrayKernel<<>>( 118 | hash_table, 119 | sensor.data(), 120 | sensor.sensor_params(), sensor.wTc(), 121 | geometry_helper); 122 | checkCudaErrors(cudaDeviceSynchronize()); 123 | checkCudaErrors(cudaGetLastError()); 124 | return timer.Tock(); 125 | } 126 | -------------------------------------------------------------------------------- /src/util/debugger.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-6-4. 3 | // 4 | 5 | #include 6 | #include "debugger.h" 7 | 8 | Debugger::Debugger(int entry_count, int block_count, 9 | int vertex_count, int triangle_count, 10 | float voxel_size) { 11 | entry_count_ = entry_count; 12 | entries_ = new HashEntry[entry_count]; 13 | heap_ = new uint[block_count]; 14 | heap_counter__ = new uint[1]; 15 | 16 | blocks_ = new Block[block_count]; 17 | block_count_ = block_count; 18 | 19 | vertex_count_ = vertex_count; 20 | vertices_ = new Vertex[vertex_count]; 21 | vertex_heap__ = new uint[vertex_count]; 22 | vertex_heap_counter__ = new uint[1]; 23 | 24 | triangle_count_ = triangle_count; 25 | triangles_ = new Triangle[triangle_count]; 26 | triangle_heap__ = new uint[triangle_count]; 27 | triangle_heap_counter__ = new uint[1]; 28 | 29 | voxel_size_ = voxel_size; 30 | } 31 | 32 | Debugger::~Debugger() { 33 | delete[] entries_; 34 | delete[] heap_; 35 | delete[] heap_counter__; 36 | delete[] blocks_; 37 | } 38 | 39 | void Debugger::CoreDump(EntryArrayGPU &hash_table) { 40 | checkCudaErrors(cudaMemcpy(heap_counter__, hash_table.counter, 41 | sizeof(int), 42 | cudaMemcpyDeviceToHost)); 43 | LOG(INFO) << *heap_counter__; 44 | checkCudaErrors(cudaMemcpy(entries_, hash_table.entries, 45 | sizeof(HashEntry) * (*heap_counter__), 46 | cudaMemcpyDeviceToHost)); 47 | } 48 | 49 | /// BlockArray are not compactified, so we have to dump all of them 50 | void Debugger::CoreDump(BlockGPUMemory &blocks) { 51 | LOG(INFO) << block_count_; 52 | checkCudaErrors(cudaMemcpy(blocks_, blocks, 53 | sizeof(Block) * block_count_, 54 | cudaMemcpyDeviceToHost)); 55 | } 56 | 57 | void Debugger::CoreDump(MeshGPU &mesh) { 58 | checkCudaErrors(cudaMemcpy(triangle_heap_counter__, mesh.triangle_heap_counter_, 59 | sizeof(uint), 60 | cudaMemcpyDeviceToHost)); 61 | checkCudaErrors(cudaMemcpy(vertex_heap_counter__, mesh.vertex_heap_counter_, 62 | sizeof(uint), 63 | cudaMemcpyDeviceToHost)); 64 | 65 | LOG(INFO) << "Triangles: " << *triangle_heap_counter__; 66 | LOG(INFO) << "Vertices: " << *vertex_heap_counter__; 67 | 68 | checkCudaErrors(cudaMemcpy(triangles_, mesh.triangles, 69 | sizeof(Triangle) * triangle_count_, 70 | cudaMemcpyDeviceToHost)); 71 | checkCudaErrors(cudaMemcpy(vertices_, mesh.vertices, 72 | sizeof(Vertex) * vertex_count_, 73 | cudaMemcpyDeviceToHost)); 74 | } 75 | 76 | void Debugger::DebugAll() { 77 | /// entry -> block -> (sdf, mesh) 78 | 79 | std::stringstream ss; 80 | for (int e = 0; e < *heap_counter__; ++e) { 81 | LOG(INFO) << "Entry " << e; 82 | 83 | HashEntry &entry = entries_[e]; 84 | CHECK(entry.ptr != FREE_ENTRY) << "Invalid compacted entry!"; 85 | Block &block = blocks_[entry.ptr]; 86 | 87 | ss.str(""); 88 | ss << "../result/statistics/(" << entry.pos.x << "," << entry.pos.y << "," << entry.pos.z << ").txt"; 89 | std::ofstream out(ss.str()); 90 | LOG(INFO) << "Writing to " << ss.str(); 91 | 92 | out << BLOCK_SIDE_LENGTH << " " << BLOCK_SIZE << "\n"; 93 | int3 voxel_pos = entry.pos * BLOCK_SIDE_LENGTH; 94 | 95 | std::vector vertices; 96 | std::vector colors; 97 | std::vector triangles; 98 | for (int i = 0; i < BLOCK_SIZE; ++i) { 99 | /// SDF info 100 | uint3 local_pos = DevectorizeIndex(i); 101 | float3 world_pos = voxel_size_ * make_float3(voxel_pos + make_int3(local_pos)); 102 | 103 | out << world_pos.x << " " << world_pos.y << " " << world_pos.z << " " 104 | << block.voxels[i].sdf << " " << (int) block.voxels[i].weight 105 | << "\n"; 106 | 107 | /// Mesh info 108 | for (int j = 0; j < 5; ++j) { 109 | int triangle_ptr = block.voxels[i].triangle_ptrs[j]; 110 | if (triangle_ptr >= 0) { 111 | int3 vertex_ptrs = triangles_[triangle_ptr].vertex_ptrs; 112 | vertices.push_back(vertices_[vertex_ptrs.x].pos); 113 | vertices.push_back(vertices_[vertex_ptrs.y].pos); 114 | vertices.push_back(vertices_[vertex_ptrs.z].pos); 115 | 116 | colors.push_back(vertices_[vertex_ptrs.x].color); 117 | colors.push_back(vertices_[vertex_ptrs.y].color); 118 | colors.push_back(vertices_[vertex_ptrs.z].color); 119 | 120 | int n = int(triangles.size()); 121 | triangles.push_back(make_int3(3 * n, 3 * n + 1, 3 * n + 2)); 122 | } 123 | } 124 | } 125 | 126 | out << vertices.size() << "\n"; 127 | for (int i = 0; i < vertices.size(); ++i) { 128 | float3 pos = vertices[i]; 129 | float3 color = colors[i]; 130 | out << pos.x << " " << pos.y << " " << pos.z << " " 131 | << color.x << " " << color.y << " " << color.z << "\n"; 132 | } 133 | 134 | out << triangles.size() << "\n"; 135 | for (auto &triangle : triangles) { 136 | out << triangle.x << " " << triangle.y << " " << triangle.z << "\n"; 137 | } 138 | } 139 | } 140 | -------------------------------------------------------------------------------- /src/localizing/point_to_psdf.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-12-23. 3 | // 4 | 5 | #include 6 | #include "point_to_psdf.h" 7 | #include "geometry/spatial_query.h" 8 | 9 | template 10 | __device__ 11 | void AtomicAdd(matNxM *A, const matNxM dA) { 12 | const int n = N * M; 13 | #pragma unroll 1 14 | for (int i = 0; i < n; ++i) { 15 | atomicAdd(&(A->entries[i]), dA.entries[i]); 16 | } 17 | } 18 | 19 | __global__ 20 | void PointToSurfaceKernel( 21 | BlockArray blocks, 22 | SensorData sensor_data, 23 | SensorParams sensor_params, 24 | float4x4 wTc, 25 | HashTable hash_table, 26 | GeometryHelper geometry_helper, 27 | mat6x6 *A, 28 | mat6x1 *b, 29 | float *err, 30 | int *count 31 | ) { 32 | const int x = blockIdx.x * blockDim.x + threadIdx.x; 33 | const int y = blockIdx.y * blockDim.y + threadIdx.y; 34 | if (x >= sensor_params.width || y >= sensor_params.height) { 35 | return; 36 | } 37 | if (x % 2 == 0 || y % 2 == 0) 38 | return; 39 | 40 | float depth = tex2D(sensor_data.depth_texture, x, y); 41 | if (depth == MINF || depth == 0.0f 42 | || depth >= geometry_helper.sdf_upper_bound) 43 | return; 44 | 45 | float3 point_cam = geometry_helper.ImageReprojectToCamera(x, 46 | y, 47 | depth, 48 | sensor_params.fx, 49 | sensor_params.fy, 50 | sensor_params.cx, 51 | sensor_params.cy); 52 | float3 point_world = wTc * point_cam; 53 | Voxel voxel; 54 | bool valid = GetSpatialValue(point_world, blocks, hash_table, 55 | geometry_helper, &voxel); 56 | if (!valid) return; 57 | float d = voxel.sdf; 58 | 59 | float3 grad; 60 | valid = GetSpatialSDFGradient(point_world, blocks, hash_table, 61 | geometry_helper, &grad); 62 | if (! valid) return; 63 | 64 | // A = \sum 65 | // \nabla D * [- y_x | I] 66 | // [dDx, dDy, dDz] * [ 1 0 0 | 0 z -y ] 67 | // [ 0 1 0 | -z 0 x ] 68 | // [ 0 0 1 | y -x 0 ] 69 | float dDdx1_data[6] = { 70 | grad.x, grad.y, grad.x, 71 | (-grad.y * point_world.z + grad.z * point_world.y), 72 | (-grad.z * point_world.x + grad.x * point_world.z), 73 | (-grad.x * point_world.y + grad.y * point_world.x) 74 | }; 75 | mat6x1 dDdxi(dDdx1_data); 76 | mat6x6 dA = dDdxi * dDdxi.getTranspose(); 77 | AtomicAdd<6, 6>(A, dA); 78 | AtomicAdd<6, 1>(b, d * dDdxi); 79 | atomicAdd(err, d * d); 80 | //printf("%d %d -> %f\n", x, y, d); 81 | atomicAdd(count, 1); 82 | } 83 | 84 | float PointToSurface(BlockArray &blocks, 85 | Sensor &sensor, 86 | HashTable &hash_table, 87 | GeometryHelper &geometry_helper, 88 | mat6x6 &cpu_A, 89 | mat6x1 &cpu_b, 90 | int &cpu_count) { 91 | const uint threads_per_block = 16; 92 | 93 | const dim3 94 | grid_size((sensor.width() + threads_per_block - 1) / threads_per_block, 95 | (sensor.height() + threads_per_block - 1) / threads_per_block); 96 | const dim3 block_size(threads_per_block, threads_per_block); 97 | 98 | mat6x6 *A; 99 | mat6x1 *b; 100 | float *err, cpu_err = 0; 101 | int *count; 102 | cpu_A.setZero(); 103 | cpu_b.setZero(); 104 | cpu_count = 0; 105 | 106 | checkCudaErrors(cudaMalloc(&A, sizeof(mat6x6))); 107 | checkCudaErrors(cudaMemcpy(A, &cpu_A, sizeof(mat6x6), 108 | cudaMemcpyHostToDevice)); 109 | 110 | checkCudaErrors(cudaMalloc(&b, sizeof(mat6x1))); 111 | checkCudaErrors(cudaMemcpy(b, &cpu_b, sizeof(mat6x1), 112 | cudaMemcpyHostToDevice)); 113 | 114 | checkCudaErrors(cudaMalloc(&count, sizeof(int))); 115 | checkCudaErrors(cudaMemcpy(count, &cpu_count, sizeof(int), 116 | cudaMemcpyHostToDevice)); 117 | 118 | checkCudaErrors(cudaMalloc(&err, sizeof(float))); 119 | checkCudaErrors(cudaMemcpy(err, &cpu_err, sizeof(float), 120 | cudaMemcpyHostToDevice)); 121 | 122 | PointToSurfaceKernel << < grid_size, block_size >> > ( 123 | blocks, 124 | sensor.data(), 125 | sensor.sensor_params(), 126 | sensor.wTc(), 127 | hash_table, 128 | geometry_helper, 129 | A, 130 | b, 131 | err, 132 | count 133 | ); 134 | checkCudaErrors(cudaDeviceSynchronize()); 135 | checkCudaErrors(cudaGetLastError()); 136 | 137 | checkCudaErrors(cudaMemcpy(&cpu_A, A, sizeof(mat6x6), 138 | cudaMemcpyDeviceToHost)); 139 | checkCudaErrors(cudaMemcpy(&cpu_b, b, sizeof(mat6x1), 140 | cudaMemcpyDeviceToHost)); 141 | checkCudaErrors(cudaMemcpy(&cpu_count, count, sizeof(int), 142 | cudaMemcpyDeviceToHost)); 143 | checkCudaErrors(cudaMemcpy(&cpu_err, err, sizeof(float), 144 | cudaMemcpyDeviceToHost)); 145 | 146 | return cpu_err; 147 | } 148 | -------------------------------------------------------------------------------- /src/io/mesh_writer.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #include 6 | #include "io/mesh_writer.h" 7 | #include "helper_cuda.h" 8 | #include "visualization/compact_mesh.h" 9 | 10 | 11 | //CollectAllBlocks(candidate_entries_, hash_table_); 12 | //int3 stats; 13 | //CompressMesh(stats); 14 | void SaveObj(CompactMesh& compact_mesh, std::string path) { 15 | LOG(INFO) << "Copying data from GPU"; 16 | 17 | 18 | uint compact_vertex_count = compact_mesh.vertex_count(); 19 | uint compact_triangle_count = compact_mesh.triangle_count(); 20 | LOG(INFO) << "Vertices: " << compact_vertex_count; 21 | LOG(INFO) << "Triangles: " << compact_triangle_count; 22 | 23 | float3* vertices = new float3[compact_vertex_count]; 24 | float3* normals = new float3[compact_vertex_count]; 25 | int3* triangles = new int3 [compact_triangle_count]; 26 | checkCudaErrors(cudaMemcpy(vertices, compact_mesh.vertices(), 27 | sizeof(float3) * compact_vertex_count, 28 | cudaMemcpyDeviceToHost)); 29 | checkCudaErrors(cudaMemcpy(normals, compact_mesh.normals(), 30 | sizeof(float3) * compact_vertex_count, 31 | cudaMemcpyDeviceToHost)); 32 | checkCudaErrors(cudaMemcpy(triangles, compact_mesh.triangles(), 33 | sizeof(int3) * compact_triangle_count, 34 | cudaMemcpyDeviceToHost)); 35 | 36 | std::ofstream out(path); 37 | std::stringstream ss; 38 | LOG(INFO) << "Writing vertices"; 39 | for (uint i = 0; i < compact_vertex_count; ++i) { 40 | ss.str(""); 41 | ss << "v " << vertices[i].x << " " 42 | << vertices[i].y << " " 43 | << vertices[i].z << "\n"; 44 | out << ss.str(); 45 | } 46 | 47 | LOG(INFO) << "Writing normals"; 48 | for (uint i = 0; i < compact_vertex_count; ++i) { 49 | ss.str(""); 50 | ss << "vn " << normals[i].x << " " 51 | << normals[i].y << " " 52 | << normals[i].z << "\n"; 53 | out << ss.str(); 54 | } 55 | 56 | LOG(INFO) << "Writing faces"; 57 | for (uint i = 0; i < compact_triangle_count; ++i) { 58 | ss.str(""); 59 | int3 idx = triangles[i] + make_int3(1); 60 | ss << "f " << idx.x << "//" << idx.x << " " 61 | << idx.y << "//" << idx.y << " " 62 | << idx.z << "//" << idx.z << "\n"; 63 | out << ss.str(); 64 | } 65 | 66 | LOG(INFO) << "Finishing vertices"; 67 | delete[] vertices; 68 | LOG(INFO) << "Finishing normals"; 69 | delete[] normals; 70 | LOG(INFO) << "Finishing triangles"; 71 | delete[] triangles; 72 | } 73 | 74 | 75 | void SavePly(CompactMesh& compact_mesh, std::string path) { 76 | LOG(INFO) << "Copying data from GPU"; 77 | 78 | uint compact_vertex_count = compact_mesh.vertex_count(); 79 | uint compact_triangle_count = compact_mesh.triangle_count(); 80 | LOG(INFO) << "Vertices: " << compact_vertex_count; 81 | LOG(INFO) << "Triangles: " << compact_triangle_count; 82 | 83 | float3* vertices = new float3[compact_vertex_count]; 84 | float3* normals = new float3[compact_vertex_count]; 85 | float3* colors = new float3[compact_vertex_count]; 86 | int3* triangles = new int3 [compact_triangle_count]; 87 | checkCudaErrors(cudaMemcpy(vertices, compact_mesh.vertices(), 88 | sizeof(float3) * compact_vertex_count, 89 | cudaMemcpyDeviceToHost)); 90 | checkCudaErrors(cudaMemcpy(normals, compact_mesh.normals(), 91 | sizeof(float3) * compact_vertex_count, 92 | cudaMemcpyDeviceToHost)); 93 | checkCudaErrors(cudaMemcpy(colors, compact_mesh.colors(), 94 | sizeof(float3) * compact_vertex_count, 95 | cudaMemcpyDeviceToHost)); 96 | checkCudaErrors(cudaMemcpy(triangles, compact_mesh.triangles(), 97 | sizeof(int3) * compact_triangle_count, 98 | cudaMemcpyDeviceToHost)); 99 | 100 | std::ofstream out(path); 101 | std::stringstream ss; 102 | ////// Header 103 | ss.str(""); 104 | ss << "ply\n" 105 | "format ascii 1.0\n"; 106 | ss << "element vertex " << compact_vertex_count << "\n"; 107 | ss << "property float x\n" 108 | "property float y\n" 109 | "property float z\n" 110 | "property float nx\n" 111 | "property float ny\n" 112 | "property float nz\n" 113 | "property uchar red\n" 114 | "property uchar green\n" 115 | "property uchar blue\n"; 116 | ss << "element face " << compact_triangle_count << "\n"; 117 | ss << "property list uchar int vertex_index\n"; 118 | ss << "end_header\n"; 119 | out << ss.str(); 120 | 121 | LOG(INFO) << "Writing vertices"; 122 | for (uint i = 0; i < compact_vertex_count; ++i) { 123 | ss.str(""); 124 | ss << vertices[i].x << " " 125 | << vertices[i].y << " " 126 | << vertices[i].z << " " 127 | << normals[i].x << " " 128 | << normals[i].y << " " 129 | << normals[i].z << " " 130 | << int(255.0f * colors[i].x) << " " 131 | << int(255.0f * colors[i].y) << " " 132 | << int(255.0f * colors[i].z) << "\n"; 133 | out << ss.str(); 134 | } 135 | 136 | LOG(INFO) << "Writing faces"; 137 | for (uint i = 0; i < compact_triangle_count; ++i) { 138 | ss.str(""); 139 | int3 idx = triangles[i]; 140 | ss << "3 " << idx.x << " " << idx.y << " " << idx.z << "\n"; 141 | out << ss.str(); 142 | } 143 | 144 | LOG(INFO) << "Finishing vertices"; 145 | delete[] vertices; 146 | LOG(INFO) << "Finishing normals"; 147 | delete[] normals; 148 | LOG(INFO) << "Finishing triangles"; 149 | delete[] triangles; 150 | } 151 | -------------------------------------------------------------------------------- /src/sensor/rgbd_sensor.cu: -------------------------------------------------------------------------------- 1 | /// 16 threads per block 2 | 3 | #include "rgbd_sensor.h" 4 | #include "geometry/geometry_helper.h" 5 | #include "visualization/color_util.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "sensor/preprocess.h" 12 | 13 | 14 | /// Member functions: (CPU code) 15 | Sensor::Sensor(SensorParams &sensor_params) { 16 | const uint image_size = sensor_params.height * sensor_params.width; 17 | 18 | params_ = sensor_params; // Is it copy constructing? 19 | checkCudaErrors(cudaMalloc(&data_.depth_buffer, sizeof(short) * image_size)); 20 | checkCudaErrors(cudaMalloc(&data_.color_buffer, sizeof(uchar4) * image_size)); 21 | 22 | checkCudaErrors(cudaMalloc(&data_.depth_data, sizeof(float) * image_size)); 23 | checkCudaErrors(cudaMalloc(&data_.inlier_ratio, sizeof(float) * image_size)); 24 | checkCudaErrors(cudaMalloc(&data_.filtered_depth_data, sizeof(float) * image_size)); 25 | checkCudaErrors(cudaMalloc(&data_.color_data, sizeof(float4) * image_size)); 26 | checkCudaErrors(cudaMalloc(&data_.normal_data, sizeof(float4) * image_size)); 27 | 28 | data_.depth_channel_desc = cudaCreateChannelDesc(); 29 | checkCudaErrors(cudaMallocArray(&data_.depth_array, 30 | &data_.depth_channel_desc, 31 | params_.width, params_.height)); 32 | 33 | data_.color_channel_desc = cudaCreateChannelDesc(); 34 | checkCudaErrors(cudaMallocArray(&data_.color_array, 35 | &data_.color_channel_desc, 36 | params_.width, params_.height)); 37 | 38 | data_.normal_channel_desc = cudaCreateChannelDesc(); 39 | checkCudaErrors(cudaMallocArray(&data_.normal_array, 40 | &data_.normal_channel_desc, 41 | params_.width, params_.height)); 42 | 43 | data_.depth_texture = 0; 44 | data_.color_texture = 0; 45 | data_.normal_texture = 0; 46 | 47 | BindCUDATexture(); 48 | is_allocated_on_gpu_ = true; 49 | } 50 | 51 | Sensor::~Sensor() { 52 | if (is_allocated_on_gpu_) { 53 | checkCudaErrors(cudaFree(data_.depth_buffer)); 54 | checkCudaErrors(cudaFree(data_.color_buffer)); 55 | 56 | checkCudaErrors(cudaFree(data_.depth_data)); 57 | checkCudaErrors(cudaFree(data_.inlier_ratio)); 58 | checkCudaErrors(cudaFree(data_.filtered_depth_data)); 59 | checkCudaErrors(cudaFree(data_.color_data)); 60 | checkCudaErrors(cudaFree(data_.normal_data)); 61 | 62 | checkCudaErrors(cudaFreeArray(data_.depth_array)); 63 | checkCudaErrors(cudaFreeArray(data_.color_array)); 64 | checkCudaErrors(cudaFreeArray(data_.normal_array)); 65 | } 66 | } 67 | 68 | void Sensor::BindCUDATexture() { 69 | cudaResourceDesc depth_resource; 70 | memset(&depth_resource, 0, sizeof(depth_resource)); 71 | depth_resource.resType = cudaResourceTypeArray; 72 | depth_resource.res.array.array = data_.depth_array; 73 | 74 | cudaTextureDesc depth_tex_desc; 75 | memset(&depth_tex_desc, 0, sizeof(depth_tex_desc)); 76 | depth_tex_desc.readMode = cudaReadModeElementType; 77 | 78 | if (data_.depth_texture != 0) 79 | checkCudaErrors(cudaDestroyTextureObject(data_.depth_texture)); 80 | checkCudaErrors(cudaCreateTextureObject(&data_.depth_texture, 81 | &depth_resource, 82 | &depth_tex_desc, 83 | NULL)); 84 | 85 | cudaResourceDesc color_resource; 86 | memset(&color_resource, 0, sizeof(color_resource)); 87 | color_resource.resType = cudaResourceTypeArray; 88 | color_resource.res.array.array = data_.color_array; 89 | 90 | cudaTextureDesc color_tex_desc; 91 | memset(&color_tex_desc, 0, sizeof(color_tex_desc)); 92 | color_tex_desc.readMode = cudaReadModeElementType; 93 | 94 | if (data_.color_texture != 0) 95 | checkCudaErrors(cudaDestroyTextureObject(data_.color_texture)); 96 | checkCudaErrors(cudaCreateTextureObject(&data_.color_texture, 97 | &color_resource, 98 | &color_tex_desc, 99 | NULL)); 100 | 101 | cudaResourceDesc normal_resource; 102 | memset(&normal_resource, 0, sizeof(normal_resource)); 103 | normal_resource.resType = cudaResourceTypeArray; 104 | normal_resource.res.array.array = data_.normal_array; 105 | 106 | cudaTextureDesc normal_tex_desc; 107 | memset(&normal_tex_desc, 0, sizeof(normal_tex_desc)); 108 | normal_tex_desc.readMode = cudaReadModeElementType; 109 | 110 | if (data_.normal_texture != 0) 111 | checkCudaErrors(cudaDestroyTextureObject(data_.normal_texture)); 112 | checkCudaErrors(cudaCreateTextureObject(&data_.normal_texture, 113 | &normal_resource, 114 | &normal_tex_desc, 115 | NULL)); 116 | } 117 | 118 | int Sensor::Process(cv::Mat &depth, cv::Mat &color) { 119 | // TODO(wei): deal with distortion 120 | /// Disable all filters at current 121 | ConvertDepthFormat(depth, data_.depth_buffer, data_.depth_data, params_); 122 | ConvertColorFormat(color, data_.color_buffer, data_.color_data, params_); 123 | 124 | ResetInlierRatio(data_.inlier_ratio, params_); 125 | 126 | /// Array used as texture in mapper 127 | checkCudaErrors(cudaMemcpyToArray(data_.depth_array, 0, 0, 128 | data_.depth_data, 129 | sizeof(float)*params_.height*params_.width, 130 | cudaMemcpyDeviceToDevice)); 131 | checkCudaErrors(cudaMemcpyToArray(data_.color_array, 0, 0, 132 | data_.color_data, 133 | sizeof(float4)*params_.height*params_.width, 134 | cudaMemcpyDeviceToDevice)); 135 | BindCUDATexture(); 136 | return 0; 137 | } 138 | 139 | 140 | 141 | -------------------------------------------------------------------------------- /src/visualization/compress_mesh.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-22. 3 | // 4 | 5 | #include "visualization/compress_mesh.h" 6 | #include 7 | 8 | //////////////////////////////// 9 | /// Compress discrete vertices and triangles 10 | __global__ 11 | void CollectVerticesAndTrianglesKernel( 12 | EntryArray candidate_entries, 13 | BlockArray blocks, 14 | Mesh mesh, 15 | CompactMesh compact_mesh) { 16 | const HashEntry &entry = candidate_entries[blockIdx.x]; 17 | MeshUnit &mesh_unit = blocks[entry.ptr].mesh_units[threadIdx.x]; 18 | 19 | for (int i = 0; i < N_TRIANGLE; ++i) { 20 | int triangle_ptrs = mesh_unit.triangle_ptrs[i]; 21 | if (triangle_ptrs != FREE_PTR) { 22 | int3& triangle = mesh.triangle(triangle_ptrs).vertex_ptrs; 23 | atomicAdd(&compact_mesh.triangles_ref_count()[triangle_ptrs], 1); 24 | atomicAdd(&compact_mesh.vertices_ref_count()[triangle.x], 1); 25 | atomicAdd(&compact_mesh.vertices_ref_count()[triangle.y], 1); 26 | atomicAdd(&compact_mesh.vertices_ref_count()[triangle.z], 1); 27 | } 28 | } 29 | } 30 | 31 | __global__ 32 | void CompressVerticesKernel(Mesh mesh, 33 | CompactMesh compact_mesh, 34 | uint max_vertex_count, 35 | uint* vertex_ref_count) { 36 | const uint idx = blockIdx.x * blockDim.x + threadIdx.x; 37 | 38 | __shared__ int local_counter; 39 | if (threadIdx.x == 0) local_counter = 0; 40 | __syncthreads(); 41 | 42 | int addr_local = -1; 43 | if (idx < max_vertex_count && compact_mesh.vertices_ref_count()[idx] > 0) { 44 | addr_local = atomicAdd(&local_counter, 1); 45 | } 46 | __syncthreads(); 47 | 48 | __shared__ int addr_global; 49 | if (threadIdx.x == 0 && local_counter > 0) { 50 | addr_global = atomicAdd(compact_mesh.vertex_counter(), local_counter); 51 | } 52 | __syncthreads(); 53 | 54 | if (addr_local != -1) { 55 | const uint addr = addr_global + addr_local; 56 | compact_mesh.vertex_remapper()[idx] = addr; 57 | compact_mesh.vertices()[addr] = mesh.vertex(idx).pos; 58 | compact_mesh.normals()[addr] = mesh.vertex(idx).normal; 59 | compact_mesh.colors()[addr] = mesh.vertex(idx).color; 60 | 61 | atomicAdd(vertex_ref_count, mesh.vertex(idx).ref_count); 62 | } 63 | } 64 | 65 | __global__ 66 | void CompressTrianglesKernel(Mesh mesh, 67 | CompactMesh compact_mesh, 68 | uint max_triangle_count) { 69 | const uint idx = blockIdx.x * blockDim.x + threadIdx.x; 70 | 71 | __shared__ int local_counter; 72 | if (threadIdx.x == 0) local_counter = 0; 73 | __syncthreads(); 74 | 75 | int addr_local = -1; 76 | if (idx < max_triangle_count && compact_mesh.triangles_ref_count()[idx] > 0) { 77 | addr_local = atomicAdd(&local_counter, 1); 78 | } 79 | __syncthreads(); 80 | 81 | __shared__ int addr_global; 82 | if (threadIdx.x == 0 && local_counter > 0) { 83 | addr_global = atomicAdd(compact_mesh.triangle_counter(), local_counter); 84 | } 85 | __syncthreads(); 86 | 87 | if (addr_local != -1) { 88 | const uint addr = addr_global + addr_local; 89 | int3 vertex_ptrs = mesh.triangle(idx).vertex_ptrs; 90 | compact_mesh.triangles()[addr].x = compact_mesh.vertex_remapper()[vertex_ptrs.x]; 91 | compact_mesh.triangles()[addr].y = compact_mesh.vertex_remapper()[vertex_ptrs.y]; 92 | compact_mesh.triangles()[addr].z = compact_mesh.vertex_remapper()[vertex_ptrs.z]; 93 | } 94 | } 95 | 96 | void CompressMesh(EntryArray& candidate_entries, 97 | BlockArray& blocks, 98 | Mesh& mesh, 99 | CompactMesh & compact_mesh, int3& stats) { 100 | compact_mesh.Reset(); 101 | 102 | int occupied_block_count = candidate_entries.count(); 103 | if (occupied_block_count <= 0) return; 104 | 105 | { 106 | const uint threads_per_block = BLOCK_SIZE; 107 | const dim3 grid_size(occupied_block_count, 1); 108 | const dim3 block_size(threads_per_block, 1); 109 | 110 | LOG(INFO) << "Before: " << compact_mesh.vertex_count() 111 | << " " << compact_mesh.triangle_count(); 112 | CollectVerticesAndTrianglesKernel <<< grid_size, block_size >>> ( 113 | candidate_entries, 114 | blocks, 115 | mesh, 116 | compact_mesh); 117 | checkCudaErrors(cudaDeviceSynchronize()); 118 | checkCudaErrors(cudaGetLastError()); 119 | } 120 | 121 | { 122 | const uint threads_per_block = 256; 123 | const dim3 grid_size((mesh.params().max_vertex_count 124 | + threads_per_block - 1) 125 | / threads_per_block, 1); 126 | const dim3 block_size(threads_per_block, 1); 127 | 128 | uint* vertex_ref_count; 129 | checkCudaErrors(cudaMalloc(&vertex_ref_count, sizeof(uint))); 130 | checkCudaErrors(cudaMemset(vertex_ref_count, 0, sizeof(uint))); 131 | 132 | CompressVerticesKernel <<< grid_size, block_size >>> ( 133 | mesh, 134 | compact_mesh, 135 | mesh.params().max_vertex_count, 136 | vertex_ref_count); 137 | 138 | checkCudaErrors(cudaDeviceSynchronize()); 139 | checkCudaErrors(cudaGetLastError()); 140 | 141 | uint vertex_ref_count_cpu; 142 | checkCudaErrors(cudaMemcpy(&vertex_ref_count_cpu, vertex_ref_count, sizeof(uint), 143 | cudaMemcpyDeviceToHost)); 144 | checkCudaErrors(cudaFree(vertex_ref_count)); 145 | 146 | LOG(INFO) << vertex_ref_count_cpu; 147 | stats.z = vertex_ref_count_cpu; 148 | } 149 | 150 | { 151 | const uint threads_per_block = 256; 152 | const dim3 grid_size((mesh.params().max_triangle_count 153 | + threads_per_block - 1) 154 | / threads_per_block, 1); 155 | const dim3 block_size(threads_per_block, 1); 156 | 157 | CompressTrianglesKernel <<< grid_size, block_size >>> ( 158 | mesh, 159 | compact_mesh, 160 | mesh.params().max_triangle_count); 161 | checkCudaErrors(cudaDeviceSynchronize()); 162 | checkCudaErrors(cudaGetLastError()); 163 | } 164 | 165 | LOG(INFO) << "Vertices: " << compact_mesh.vertex_count() 166 | << "/" << (mesh.params().max_vertex_count - mesh.vertex_heap_count()); 167 | stats.y = compact_mesh.vertex_count(); 168 | 169 | LOG(INFO) << "Triangles: " << compact_mesh.triangle_count() 170 | << "/" << (mesh.params().max_triangle_count - mesh.triangle_heap_count()); 171 | stats.x = compact_mesh.triangle_count(); 172 | } 173 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | PROJECT(mesh-hashing) 3 | 4 | #---------- 5 | # C++ version setting 6 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 7 | SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -Wall -g -ggdb") 8 | SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -Wall") 9 | 10 | #---------- 11 | # Project variable configurations 12 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) 13 | SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 14 | 15 | #---------- 16 | # Find packages 17 | FIND_PACKAGE(PkgConfig) 18 | LIST(APPEND 19 | CMAKE_MODULE_PATH 20 | ${PROJECT_SOURCE_DIR}/cmake_modules) 21 | 22 | # Graphics dependent 23 | FIND_PACKAGE(GLFW3 REQUIRED) 24 | FIND_PACKAGE(GLEW REQUIRED) 25 | FIND_PACKAGE(OpenGL REQUIRED) 26 | # CV utilities 27 | FIND_PACKAGE(OpenCV REQUIRED) 28 | FIND_PACKAGE(Eigen3 REQUIRED) 29 | # Parallel computation 30 | FIND_PACKAGE(CUDA REQUIRED) 31 | 32 | # Log utilities 33 | FIND_PACKAGE(Glog REQUIRED) 34 | 35 | #---------- 36 | ## Found Headers 37 | GET_PROPERTY(included_dirs 38 | DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 39 | PROPERTY INCLUDE_DIRECTORIES) 40 | MESSAGE(STATUS "Found include paths:") 41 | foreach(included_dir ${included_dirs}) 42 | MESSAGE(STATUS ${included_dir}) 43 | endforeach() 44 | 45 | #---------- 46 | ## Found Libs 47 | SET(OPENGL_DEPENDENCIES "") 48 | LIST(APPEND 49 | OPENGL_DEPENDENCIES 50 | ${OPENGL_LIBRARY} 51 | ${GLEW_LIBRARY}) 52 | if (APPLE) 53 | LIST(APPEND OPENGL_DEPENDENCIES 54 | ${GLFW3_LIBRARIES}) 55 | else () 56 | LIST(APPEND OPENGL_DEPENDENCIES 57 | ${GLFW3_STATIC_LIBRARIES}) 58 | endif() 59 | 60 | SET(CUDA_DEPENDENCIES "") 61 | LIST(APPEND 62 | CUDA_DEPENDENCIES 63 | ${CUDA_CUDART_LIBRARY} 64 | ${CUDA_CUDA_LIBRARY}) 65 | 66 | MESSAGE(STATUS "Found libraries:") 67 | foreach(library ${OPENGL_DEPENDENCIES}) 68 | MESSAGE(STATUS ${library}) 69 | endforeach() 70 | foreach(library ${CUDA_DEPENDENCIES}) 71 | MESSAGE(STATUS ${library}) 72 | endforeach() 73 | foreach(library ${OTHER_DEPENDENCIES}) 74 | MESSAGE(STATUS ${library}) 75 | endforeach() 76 | #--------- 77 | 78 | #---------- 79 | SET(VH ${PROJECT_SOURCE_DIR}/src) 80 | SET(GL_WRAPPER ${VH}/extern/opengl-wrapper) 81 | 82 | INCLUDE_DIRECTORIES( 83 | ${VH} 84 | ${VH}/extern/cuda 85 | ${GL_WRAPPER}/include 86 | ${EIGEN3_INCLUDE_DIR} 87 | ${GLOG_INCLUDE_DIRS} 88 | ${CUDA_TOOLKIT_INCLUDE}) 89 | 90 | #---------- 91 | ## Building Libraries 92 | ### 1. OpenGL util 93 | ADD_LIBRARY(gl-util 94 | ${GL_WRAPPER}/src/core/args.cc 95 | ${GL_WRAPPER}/src/core/program.cc 96 | ${GL_WRAPPER}/src/core/window.cc 97 | ${GL_WRAPPER}/src/core/uniforms.cc 98 | ${GL_WRAPPER}/src/core/camera.cc) 99 | SET_TARGET_PROPERTIES(gl-util 100 | PROPERTIES 101 | COMPILE_DEFINITIONS USE_CUDA_GL) 102 | TARGET_LINK_LIBRARIES(gl-util 103 | ${OPENGL_DEPENDENCIES} 104 | ${GLOG_LIBRARIES}) 105 | 106 | ### 2. CUDA 107 | # Don't know exactly how it should be configured 108 | LIST(APPEND CUDA_NVCC_FLAGS -gencode=arch=compute_50,code=sm_60) 109 | CUDA_ADD_LIBRARY(mesh-hashing-cuda 110 | ${VH}/core/hash_table.cu 111 | ${VH}/core/entry_array.cu 112 | ${VH}/core/block_array.cu 113 | ${VH}/core/mesh.cu 114 | ${VH}/core/collect_block_array.cu 115 | 116 | ${VH}/sensor/rgbd_sensor.cu 117 | ${VH}/sensor/preprocess.cu 118 | 119 | ${VH}/localizing/point_to_psdf.cu 120 | ${VH}/mapping/allocate.cu 121 | ${VH}/mapping/update_simple.cu 122 | ${VH}/mapping/update_bayesian.cu 123 | ${VH}/mapping/recycle.cu 124 | 125 | ${VH}/optimize/linear_equations.cu 126 | ${VH}/optimize/primal_dual.cu 127 | 128 | ${VH}/meshing/marching_cubes.cu 129 | 130 | ${VH}/visualization/colorize.cu 131 | ${VH}/visualization/compact_mesh.cu 132 | ${VH}/visualization/bounding_box.cu 133 | ${VH}/visualization/trajectory.cu 134 | ${VH}/visualization/compress_mesh.cu 135 | ${VH}/visualization/extract_bounding_box.cu 136 | ${VH}/visualization/ray_caster.cu) 137 | 138 | TARGET_LINK_LIBRARIES(mesh-hashing-cuda 139 | ${CUDA_DEPENDENCIES} 140 | ${GLOG_LIBRARIES} 141 | -lopencv_core -lopencv_highgui -lopencv_imgproc) 142 | 143 | ### 3. C++ 144 | ADD_LIBRARY(mesh-hashing 145 | ${VH}/engine/main_engine.cc 146 | ${VH}/engine/mapping_engine.cc 147 | ${VH}/engine/logging_engine.cc 148 | ${VH}/engine/visualizing_engine.cc 149 | 150 | ${VH}/io/config_manager.cc 151 | ${VH}/io/mesh_writer.cc 152 | 153 | ${VH}/sensor/rgbd_data_provider.cc) 154 | #${VH}/tool/cpp/debugger.cc) 155 | SET_TARGET_PROPERTIES(mesh-hashing 156 | PROPERTIES 157 | COMPILE_DEFINITIONS USE_CUDA_GL) 158 | TARGET_LINK_LIBRARIES(mesh-hashing 159 | mesh-hashing-cuda 160 | gl-util) 161 | 162 | #---------- 163 | ### Loop over 164 | MESSAGE(STATUS ${GLOG_LIBRARIES}) 165 | ADD_EXECUTABLE(reconstruction src/app/reconstruction.cc) 166 | SET_TARGET_PROPERTIES(reconstruction 167 | PROPERTIES 168 | COMPILE_DEFINITIONS USE_CUDA_GL) 169 | TARGET_LINK_LIBRARIES(reconstruction 170 | mesh-hashing 171 | -lopencv_core -lopencv_highgui 172 | ${GLOG_LIBRARIES}) 173 | 174 | ADD_EXECUTABLE(slam src/app/slam.cc) 175 | SET_TARGET_PROPERTIES(slam 176 | PROPERTIES 177 | COMPILE_DEFINITIONS USE_CUDA_GL) 178 | TARGET_LINK_LIBRARIES(slam 179 | mesh-hashing 180 | -lopencv_core -lopencv_highgui 181 | ${GLOG_LIBRARIES}) 182 | 183 | ADD_EXECUTABLE(block_analysis src/app/block_analysis.cc) 184 | SET_TARGET_PROPERTIES(block_analysis 185 | PROPERTIES 186 | COMPILE_DEFINITIONS USE_CUDA_GL) 187 | TARGET_LINK_LIBRARIES(block_analysis 188 | gl-util 189 | mesh-hashing 190 | -lopencv_core -lopencv_highgui 191 | ${GLOG_LIBRARIES}) 192 | 193 | ### An ORB app 194 | #OPTION(WITH_ORBSLAM2 "Build with orb slam" ON) 195 | #if (WITH_ORBSLAM2) 196 | # SET(ORB_SLAM2_PATH ${VH}/extern/orb_slam2) 197 | # MESSAGE(STATUS "Build with ORB-SLAM2") 198 | # 199 | # FIND_PACKAGE(Pangolin REQUIRED) 200 | # FIND_PACKAGE(Eigen3 REQUIRED) 201 | # INCLUDE_DIRECTORIES( 202 | # ${ORB_SLAM2_PATH} 203 | # ${ORB_SLAM2_PATH}/include 204 | # ${EIGEN3_INCLUDE_DIR}) 205 | # LINK_DIRECTORIES(${ORB_SLAM2_PATH}/lib) 206 | # 207 | # ADD_EXECUTABLE(orb_slam2 src/app/orb_slam2.cc) 208 | # TARGET_LINK_LIBRARIES(orb_slam2 209 | # mesh-hashing 210 | # ORB_SLAM2 211 | # ${OpenCV_LIBS} 212 | # ${Pangolin_LIBRARIES} 213 | # ${GLOG_LIBRARIES}) 214 | # 215 | # SET_TARGET_PROPERTIES(orb_slam2 216 | # PROPERTIES 217 | # COMPILE_DEFINITIONS USE_CUDA_GL) 218 | #endif(WITH_ORBSLAM2) 219 | -------------------------------------------------------------------------------- /src/geometry/geometry_helper.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-4-2. 3 | // 4 | // Geometric utilities: 5 | // 1. Transformation of Coordinate systems with different units 6 | // 2. Projection, reprojection and viewing frustum determination 7 | #ifndef VH_GEOMETRY_UTIL_H 8 | #define VH_GEOMETRY_UTIL_H 9 | 10 | #include "../core/common.h" 11 | #include "../core/params.h" 12 | 13 | /// There are 3 kinds of positions (pos) 14 | /// 1. world pos, unit: meter 15 | /// 2. voxel pos, unit: voxel (typically 0.004m) 16 | /// 3. block pos, unit: block (typically 8 voxels) 17 | 18 | ////////// 19 | /// Transforms between world, voxel, and block coordinate systems 20 | /// Semantic: A pos To B pos; A, B in {world, voxel, block} 21 | /// float is only used to do interpolation 22 | 23 | /// World <-> Voxel 24 | 25 | __device__ __host__ 26 | __inline__ float squaref(float a) { 27 | return a*a; 28 | } 29 | 30 | __device__ __host__ 31 | __inline__ float gaussian(float x, float u, float squared_sigma) { 32 | return 1 / (sqrt(2*M_PI)*sqrt(squared_sigma)) 33 | * exp(- squaref(x - u) / squared_sigma); 34 | } 35 | 36 | struct GeometryHelper { 37 | float voxel_size; 38 | float truncation_distance; 39 | float truncation_distance_scale; 40 | float sdf_upper_bound; 41 | float weight_sample; 42 | 43 | GeometryHelper() = default; 44 | void Init(const VolumeParams ¶ms) { 45 | voxel_size = params.voxel_size; 46 | truncation_distance_scale = params.truncation_distance_scale; 47 | truncation_distance = params.truncation_distance; 48 | sdf_upper_bound = params.sdf_upper_bound; 49 | weight_sample = params.weight_sample; 50 | } 51 | GeometryHelper(const VolumeParams ¶ms) { 52 | Init(params); 53 | } 54 | 55 | __host__ __device__ 56 | inline 57 | float3 WorldToVoxelf(const float3 world_pos) { 58 | return world_pos / voxel_size; 59 | } 60 | 61 | __host__ __device__ 62 | inline 63 | int3 WorldToVoxeli(const float3 world_pos) { 64 | const float3 p = world_pos / voxel_size; 65 | return make_int3(p + make_float3(sign(p)) * 0.5f); 66 | } 67 | 68 | __host__ __device__ 69 | inline // no offset here: value is stored 90on the corner of a voxel 70 | float3 VoxelToWorld(const int3 voxel_pos) { 71 | return make_float3(voxel_pos) * voxel_size; 72 | } 73 | 74 | /// Voxel <-> Block 75 | // Corner voxel with smallest xyz 76 | __host__ __device__ 77 | inline 78 | int3 BlockToVoxel(const int3 block_pos) { 79 | return block_pos * BLOCK_SIDE_LENGTH; 80 | } 81 | 82 | __host__ __device__ 83 | inline 84 | int3 VoxelToBlock(const int3 voxel_pos) { 85 | int3 pos = voxel_pos; 86 | pos.x -= (voxel_pos.x < 0) * (BLOCK_SIDE_LENGTH - 1); 87 | pos.y -= (voxel_pos.y < 0) * (BLOCK_SIDE_LENGTH - 1); 88 | pos.z -= (voxel_pos.z < 0) * (BLOCK_SIDE_LENGTH - 1); 89 | return pos / BLOCK_SIDE_LENGTH; 90 | } 91 | 92 | __host__ __device__ 93 | inline 94 | uint3 VoxelToOffset(const int3 block_pos, const int3 voxel_pos) { 95 | int3 offset = voxel_pos - BlockToVoxel(block_pos); 96 | return make_uint3(offset); 97 | } 98 | 99 | /// Block <-> World 100 | __host__ __device__ 101 | inline 102 | float3 BlockToWorld(const int3 block_pos) { 103 | return VoxelToWorld(BlockToVoxel(block_pos)); 104 | } 105 | 106 | __host__ __device__ 107 | inline 108 | int3 WorldToBlock(const float3 world_pos) { 109 | return VoxelToBlock(WorldToVoxeli(world_pos)); 110 | } 111 | 112 | ////////// 113 | /// Transforms between coordinates and indices 114 | /// Idx means local idx inside a block \in [0, 512) 115 | __host__ __device__ 116 | inline 117 | uint3 DevectorizeIndex(const uint idx) { 118 | uint x = idx % BLOCK_SIDE_LENGTH; 119 | uint y = (idx % (BLOCK_SIDE_LENGTH * BLOCK_SIDE_LENGTH)) / BLOCK_SIDE_LENGTH; 120 | uint z = idx / (BLOCK_SIDE_LENGTH * BLOCK_SIDE_LENGTH); 121 | return make_uint3(x, y, z); 122 | } 123 | 124 | /// Computes the linearized index of a local virtual voxel pos; pos \in [0, 8)^3 125 | __host__ __device__ 126 | inline 127 | uint VectorizeOffset(const uint3 voxel_local_pos) { 128 | return voxel_local_pos.z * BLOCK_SIDE_LENGTH * BLOCK_SIDE_LENGTH + 129 | voxel_local_pos.y * BLOCK_SIDE_LENGTH + 130 | voxel_local_pos.x; 131 | } 132 | 133 | ////////// 134 | /// Truncating distance 135 | __host__ __device__ 136 | inline 137 | float truncate_distance(const float z) { 138 | return truncation_distance +truncation_distance_scale * z; 139 | } 140 | 141 | ////////// 142 | /// Projections and reprojections 143 | /// Between the Camera coordinate system and the image plane 144 | /// Projection 145 | __host__ __device__ 146 | inline 147 | float2 CameraProjectToImagef(const float3 camera_pos, 148 | float fx, float fy, float cx, float cy) { 149 | return make_float2(camera_pos.x * fx / camera_pos.z + cx, 150 | camera_pos.y * fy / camera_pos.z + cy); 151 | } 152 | 153 | __host__ __device__ 154 | inline 155 | int2 CameraProjectToImagei( 156 | const float3 camera_pos, 157 | float fx, float fy, float cx, float cy) { 158 | float2 uv = CameraProjectToImagef(camera_pos, fx, fy, cx, cy); 159 | return make_int2(uv + make_float2(0.5f, 0.5f)); 160 | } 161 | 162 | __host__ __device__ 163 | inline 164 | float3 ImageReprojectToCamera( 165 | uint ux, uint uy, float depth, 166 | float fx, float fy, float cx, float cy) { 167 | const float x = ((float) ux - cx) / fx; 168 | const float y = ((float) uy - cy) / fy; 169 | return make_float3(depth * x, depth * y, depth); 170 | } 171 | 172 | /// R^3 -> [0, 1]^3 173 | /// maybe used for rendering 174 | __host__ __device__ 175 | inline 176 | float NormalizeDepth(float z, float min_depth, float max_depth) { 177 | return (z - min_depth) / (max_depth - min_depth); 178 | } 179 | 180 | inline 181 | float DenormalizeDepth(float z, float min_depth, float max_depth) { 182 | return z * (max_depth - min_depth) + min_depth; 183 | } 184 | 185 | /// View frustum test 186 | __host__ __device__ 187 | inline 188 | bool IsPointInCameraFrustum(const float4x4 &c_T_w, 189 | const float3 world_pos, 190 | const SensorParams &sensor_params) { 191 | float3 camera_pos = c_T_w * world_pos; 192 | float2 uv = CameraProjectToImagef(camera_pos, 193 | sensor_params.fx, sensor_params.fy, 194 | sensor_params.cx, sensor_params.cy); 195 | float3 normalized_p = make_float3( 196 | (2.0f * uv.x - (sensor_params.width - 1.0f)) / (sensor_params.width - 1.0f), 197 | ((sensor_params.height - 1.0f) - 2.0f * uv.y) / (sensor_params.height - 1.0f), 198 | NormalizeDepth(camera_pos.z, 199 | sensor_params.min_depth_range, 200 | sensor_params.max_depth_range)); 201 | 202 | normalized_p *= 0.95; 203 | return !(normalized_p.x < -1.0f || normalized_p.x > 1.0f 204 | || normalized_p.y < -1.0f || normalized_p.y > 1.0f 205 | || normalized_p.z < 0.0f || normalized_p.z > 1.0f); 206 | } 207 | 208 | __host__ __device__ 209 | inline 210 | bool IsBlockInCameraFrustum(float4x4 c_T_w, 211 | const int3 block_pos, 212 | const SensorParams &sensor_params) { 213 | float3 world_pos = VoxelToWorld(BlockToVoxel(block_pos)) 214 | + voxel_size * 0.5f * (BLOCK_SIDE_LENGTH - 1.0f); 215 | return IsPointInCameraFrustum(c_T_w, world_pos, sensor_params); 216 | } 217 | }; 218 | 219 | #endif //VH_GEOMETRY_UTIL_H 220 | -------------------------------------------------------------------------------- /src/optimize/primal_dual.cu: -------------------------------------------------------------------------------- 1 | // 2 | // Created by wei on 17-10-26. 3 | // 4 | 5 | #include "primal_dual.h" 6 | #include 7 | #include 8 | #include "geometry/primal_dual_query.h" 9 | 10 | __device__ 11 | inline float Huber(float x, float alpha) { 12 | return (fabsf(x) < alpha) ? 0.5f * (x*x)/alpha : (fabsf(x) - 0.5f*alpha); 13 | } 14 | 15 | __global__ 16 | void PrimalDualInitKernel( 17 | EntryArray candidate_entries, 18 | BlockArray blocks, 19 | HashTable hash_table, 20 | GeometryHelper geometry_helper 21 | ) { 22 | const HashEntry& entry = candidate_entries[blockIdx.x]; 23 | Block& block = blocks[entry.ptr]; 24 | PrimalDualVariables& primal_dual_variables 25 | = block.primal_dual_variables[threadIdx.x]; 26 | Voxel& voxel = block.voxels[threadIdx.x]; 27 | 28 | if (voxel.inv_sigma2 < EPSILON) 29 | return; 30 | 31 | int3 voxel_base_pos = geometry_helper.BlockToVoxel(entry.pos); 32 | uint3 offset = geometry_helper.DevectorizeIndex(threadIdx.x); 33 | int3 voxel_pos = voxel_base_pos + make_int3(offset); 34 | float3 gradient; 35 | GetInitSDFGradient(entry, voxel_pos, 36 | blocks, hash_table, 37 | geometry_helper, &gradient); 38 | // primal 39 | primal_dual_variables.Clear(); 40 | primal_dual_variables.inv_sigma2 = expf(length(gradient) / geometry_helper.voxel_size); 41 | primal_dual_variables.sdf0 = voxel.sdf; 42 | primal_dual_variables.mask = true; 43 | } 44 | 45 | __global__ 46 | /** 47 | * Primal dual: dual step 48 | * @param candidate_entries 49 | * @param blocks 50 | * @param hash_table 51 | * @param geometry_helper 52 | * @param lambda 53 | * @param sigma 54 | * @param tau 55 | */ 56 | void PrimalDualIteratePass1Kernel( 57 | EntryArray candidate_entries, 58 | BlockArray blocks, 59 | HashTable hash_table, 60 | GeometryHelper geometry_helper, 61 | float lambda, 62 | float sigma, 63 | float tau, 64 | float* err_data, 65 | float* err_tv 66 | ) { 67 | const float alpha = 0.02; 68 | 69 | const HashEntry &entry = candidate_entries[blockIdx.x]; 70 | Block& block = blocks[entry.ptr]; 71 | 72 | Voxel &voxel = block.voxels[threadIdx.x]; 73 | PrimalDualVariables &primal_dual_variable = block.primal_dual_variables[threadIdx.x]; 74 | if (voxel.inv_sigma2 < EPSILON) return; 75 | 76 | int3 voxel_base_pos = geometry_helper.BlockToVoxel(entry.pos); 77 | uint3 offset = geometry_helper.DevectorizeIndex(threadIdx.x); 78 | int3 voxel_pos = voxel_base_pos + make_int3(offset); 79 | 80 | // Compute error 81 | float data_diff = fabsf(voxel.sdf - primal_dual_variable.sdf0); 82 | data_diff *= data_diff; 83 | if (voxel.inv_sigma2 > EPSILON) { 84 | atomicAdd(err_data, data_diff); 85 | } 86 | 87 | float3 gradient; 88 | GetSDFGradient(entry, voxel_pos, 89 | blocks, hash_table, 90 | geometry_helper, &gradient); 91 | atomicAdd(err_tv, Huber(length(gradient), alpha)); 92 | 93 | // Dual step 94 | // p_{n+1} = prox_F* (p_{n} + \sigma \nabla x_bar{n}) 95 | // prox_F* (y) = \delta (y) (projection function) 96 | float3 primal_gradient; 97 | GetPrimalGradient( 98 | entry, voxel_pos, 99 | blocks, hash_table, 100 | geometry_helper, 101 | &primal_gradient 102 | ); 103 | 104 | //float tv_diff = 105 | primal_dual_variable.p = primal_dual_variable.p + sigma * primal_gradient; 106 | // huber 107 | primal_dual_variable.p /= (1 + sigma * alpha); 108 | primal_dual_variable.p /= fmaxf(1, length(primal_dual_variable.p)); 109 | } 110 | 111 | __global__ 112 | void PrimalDualIteratePass2Kernel( 113 | EntryArray candidate_entries, 114 | BlockArray blocks, 115 | HashTable hash_table, 116 | GeometryHelper geometry_helper, 117 | float lambda, 118 | float sigma, 119 | float tau 120 | ) { 121 | const HashEntry &entry = candidate_entries[blockIdx.x]; 122 | Block& block = blocks[entry.ptr]; 123 | Voxel &voxel = block.voxels[threadIdx.x]; 124 | PrimalDualVariables& primal_dual_variables = block.primal_dual_variables[threadIdx.x]; 125 | if (voxel.inv_sigma2 < EPSILON) 126 | return; 127 | 128 | int3 voxel_base_pos = geometry_helper.BlockToVoxel(entry.pos); 129 | uint3 offset = geometry_helper.DevectorizeIndex(threadIdx.x); 130 | int3 voxel_pos = voxel_base_pos + make_int3(offset); 131 | 132 | float voxel_sdf_prev = voxel.sdf; 133 | // Primal step 134 | // x_{n+1} = prox_G (x_{n} - \tau -Div p_{n+1}) 135 | // prox_G = (1 + \lambda y) / (1 + \lambda) 136 | float dual_divergence = 0; 137 | GetDualDivergence( 138 | entry, voxel_pos, 139 | blocks, hash_table, 140 | geometry_helper, &dual_divergence 141 | ); 142 | 143 | lambda *= primal_dual_variables.inv_sigma2; 144 | voxel.sdf = voxel.sdf - tau * dual_divergence; 145 | voxel.sdf = (voxel.sdf + lambda * tau * primal_dual_variables.sdf0) 146 | / (1 + lambda * tau); 147 | if (primal_dual_variables.inv_sigma2 > 2.0f) 148 | voxel.sdf = primal_dual_variables.sdf0; 149 | // Extrapolation 150 | primal_dual_variables.sdf_bar = 2 * voxel.sdf - voxel_sdf_prev; 151 | } 152 | 153 | void PrimalDualInit( 154 | EntryArray& candidate_entries, 155 | BlockArray& blocks, 156 | HashTable& hash_table, 157 | GeometryHelper& geometry_helper 158 | ) { 159 | const uint threads_per_block = BLOCK_SIZE; 160 | 161 | uint candidate_entry_count = candidate_entries.count(); 162 | if (candidate_entry_count <= 0) 163 | return; 164 | 165 | const dim3 grid_size(candidate_entry_count, 1); 166 | const dim3 block_size(threads_per_block, 1); 167 | 168 | PrimalDualInitKernel<<>> (candidate_entries, 169 | blocks, hash_table, geometry_helper); 170 | checkCudaErrors(cudaDeviceSynchronize()); 171 | checkCudaErrors(cudaGetLastError()); 172 | } 173 | 174 | void PrimalDualIterate( 175 | EntryArray& candidate_entries, 176 | BlockArray& blocks, 177 | HashTable& hash_table, 178 | GeometryHelper& geometry_helper, 179 | const float lambda, 180 | const float sigma, 181 | const float tau 182 | ) { 183 | const uint threads_per_block = BLOCK_SIZE; 184 | 185 | uint candidate_entry_count = candidate_entries.count(); 186 | if (candidate_entry_count <= 0) 187 | return; 188 | 189 | const dim3 grid_size(candidate_entry_count, 1); 190 | const dim3 block_size(threads_per_block, 1); 191 | 192 | float* err_data, *err_tv; 193 | checkCudaErrors(cudaMalloc(&err_data, sizeof(float))); 194 | checkCudaErrors(cudaMemset(err_data, 0, sizeof(float))); 195 | checkCudaErrors(cudaMalloc(&err_tv, sizeof(float))); 196 | checkCudaErrors(cudaMemset(err_tv, 0, sizeof(float))); 197 | 198 | PrimalDualIteratePass1Kernel <<>> ( 199 | candidate_entries, 200 | blocks, hash_table, 201 | geometry_helper, 202 | lambda, sigma, tau, 203 | err_data, err_tv); 204 | checkCudaErrors(cudaDeviceSynchronize()); 205 | checkCudaErrors(cudaGetLastError()); 206 | 207 | float err_data_cpu, err_tv_cpu; 208 | checkCudaErrors(cudaMemcpy(&err_data_cpu, err_data, sizeof(float), 209 | cudaMemcpyDeviceToHost)); 210 | checkCudaErrors(cudaMemcpy(&err_tv_cpu, err_tv, sizeof(float), 211 | cudaMemcpyDeviceToHost)); 212 | std::cout << err_data_cpu * lambda / 2 + err_tv_cpu << " " 213 | << err_data_cpu << " " 214 | << err_tv_cpu << std::endl; 215 | 216 | PrimalDualIteratePass2Kernel <<>> ( 217 | candidate_entries, 218 | blocks, hash_table, 219 | geometry_helper, 220 | lambda, sigma, tau); 221 | checkCudaErrors(cudaDeviceSynchronize()); 222 | checkCudaErrors(cudaGetLastError()); 223 | } --------------------------------------------------------------------------------