├── .clang-format ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── data ├── GT.ply ├── mask_00000.png ├── mask_00001.png ├── mask_00002.png ├── mask_00003.png ├── mask_00004.png ├── mask_00005.png └── tumpose.txt ├── examples.cc ├── include └── vacancy │ ├── camera.h │ ├── common.h │ ├── image.h │ ├── log.h │ ├── mesh.h │ └── voxel_carver.h ├── rebuild.bat ├── reconfigure.bat └── src └── vacancy ├── camera.cc ├── extract_voxel.cc ├── extract_voxel.h ├── image.cc ├── log.cc ├── marching_cubes.cc ├── marching_cubes.h ├── marching_cubes_lut.cc ├── marching_cubes_lut.h ├── mesh.cc ├── stb.cc ├── timer.h └── voxel_carver.cc /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # 2 | /win_build/ 3 | /bin/ 4 | /lib/ 5 | 6 | # Prerequisites 7 | *.d 8 | 9 | # Compiled Object files 10 | *.slo 11 | *.lo 12 | *.o 13 | *.obj 14 | 15 | # Precompiled Headers 16 | *.gch 17 | *.pch 18 | 19 | # Compiled Dynamic libraries 20 | *.so 21 | *.dylib 22 | *.dll 23 | 24 | # Fortran module files 25 | *.mod 26 | *.smod 27 | 28 | # Compiled Static libraries 29 | *.lai 30 | *.la 31 | *.a 32 | *.lib 33 | 34 | # Executables 35 | *.exe 36 | *.out 37 | *.app -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "third_party/stb"] 2 | path = third_party/stb 3 | url = https://github.com/nothings/stb 4 | [submodule "third_party/eigen"] 5 | path = third_party/eigen 6 | url = https://gitlab.com/libeigen/eigen.git 7 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | 3 | set(PROJECT_NAME vacancy) 4 | project(${PROJECT_NAME} LANGUAGES CXX VERSION 0.0.1 DESCRIPTION "Vacancy: A Voxel Carving implementation in C++") 5 | 6 | set(CMAKE_VERBOSE_MAKEFILE TRUE) 7 | 8 | # .lib 9 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/lib) 10 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/lib) 11 | 12 | # .dll and .exe 13 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/bin) 14 | 15 | if (WIN32) 16 | # option for Visual Studio 17 | # -EHsc (enable proper Exxeption Handling) needs to avoid C4530 18 | # -Wall is too noisy so that set -W4. 19 | # https://docs.microsoft.com/en-us/cpp/build/reference/compiler-option-warning-level?view=vs-2017 20 | # "However, for a new project, it may be best to use /W4 in all compilations; 21 | # this will ensure the fewest possible hard-to-find code defects." 22 | set(CMAKE_CXX_FLAGS "/std:c++14 /W4 /EHsc /MP") 23 | 24 | 25 | else() 26 | # g++ option for *nix 27 | 28 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 29 | set(CMAKE_CXX_EXTENSIONS OFF) #Set this to ON if you want to use GNU++ 30 | set(CMAKE_CXX_STANDARD 11) 31 | 32 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wall -O2") 33 | 34 | 35 | endif() 36 | 37 | # third_party directries 38 | if(NOT DEFINED STB_INSTALL_DIR) 39 | set(STB_INSTALL_DIR "third_party/stb" CACHE PATH "stb installed directory") 40 | endif() 41 | if(NOT DEFINED EIGEN_INSTALL_DIR) 42 | set(EIGEN_INSTALL_DIR "third_party/eigen" CACHE PATH "eigen installed directory") 43 | message("EIGEN_INSTALL_DIR: ${EIGEN_INSTALL_DIR}") 44 | endif() 45 | 46 | # switch for optional libraries 47 | option(VACANCY_USE_STB "Use stb to enable image i/o" ON) 48 | message("VACANCY_USE_STB: ${VACANCY_USE_STB}") 49 | if(VACANCY_USE_STB) 50 | option(VACANCY_USE_STB_AS_STATIC_LIB "Use stb as static lib" OFF) 51 | message("VACANCY_USE_STB_AS_STATIC_LIB: ${VACANCY_USE_STB_AS_STATIC_LIB}") 52 | if(VACANCY_USE_STB_AS_STATIC_LIB) 53 | add_library(stb 54 | STATIC 55 | src/vacancy/stb.cc) 56 | set(STB_LIB_NAME stb) 57 | target_include_directories(${STB_LIB_NAME} PUBLIC third_party) 58 | target_compile_definitions(${STB_LIB_NAME} PUBLIC -DVACANCY_USE_STB) 59 | else() 60 | set(VACANCY_STB_IMPLEMENTATION_CC src/vacancy/stb.cc) 61 | endif() 62 | endif() 63 | 64 | option(VACANCY_USE_OPENMP "Use OpenMP to enable parallelization" ON) 65 | message("VACANCY_USE_OPENMP: ${VACANCY_USE_OPENMP}") 66 | if(VACANCY_USE_OPENMP) 67 | add_definitions(-DVACANCY_USE_OPENMP) 68 | endif() 69 | 70 | set(PUBLIC_LIB_NAME ${PROJECT_NAME}) 71 | add_library(${PUBLIC_LIB_NAME} 72 | STATIC 73 | include/vacancy/common.h 74 | include/vacancy/camera.h 75 | include/vacancy/mesh.h 76 | include/vacancy/image.h 77 | include/vacancy/log.h 78 | include/vacancy/voxel_carver.h 79 | 80 | src/vacancy/camera.cc 81 | src/vacancy/mesh.cc 82 | src/vacancy/image.cc 83 | src/vacancy/timer.h 84 | src/vacancy/log.cc 85 | src/vacancy/voxel_carver.cc 86 | src/vacancy/marching_cubes.h 87 | src/vacancy/marching_cubes.cc 88 | src/vacancy/marching_cubes_lut.h 89 | src/vacancy/marching_cubes_lut.cc 90 | src/vacancy/extract_voxel.h 91 | src/vacancy/extract_voxel.cc 92 | 93 | # implementations of header-only library 94 | ${VACANCY_STB_IMPLEMENTATION_CC} 95 | ) 96 | 97 | if (VACANCY_USE_STB) 98 | target_compile_definitions(${PUBLIC_LIB_NAME} PUBLIC -DVACANCY_USE_STB) 99 | endif() 100 | 101 | # For OpenMP 102 | if(VACANCY_USE_OPENMP) 103 | find_package(OpenMP REQUIRED) 104 | if(OpenMP_FOUND) 105 | target_compile_options(${PUBLIC_LIB_NAME} PUBLIC 106 | $<$:${OPENMP_CXX_FLAGS}> 107 | $<$:${OPENMP_C_FLAGS}>) 108 | endif() 109 | endif() 110 | 111 | # set folders 112 | set_property(GLOBAL PROPERTY USE_FOLDERS ON) 113 | 114 | SOURCE_GROUP("Public header files" FILES 115 | include/vacancy/common.h 116 | include/vacancy/camera.h 117 | include/vacancy/mesh.h 118 | include/vacancy/image.h 119 | include/vacancy/log.h 120 | include/vacancy/voxel_carver.h 121 | ) 122 | 123 | SOURCE_GROUP("Private header files" FILES 124 | src/vacancy/timer.h 125 | src/vacancy/marching_cubes.h 126 | src/vacancy/marching_cubes_lut.h 127 | src/vacancy/extract_voxel.h 128 | ) 129 | 130 | SOURCE_GROUP("Source files" FILES 131 | src/vacancy/camera.cc 132 | src/vacancy/mesh.cc 133 | src/vacancy/image.cc 134 | src/vacancy/log.cc 135 | src/vacancy/voxel_carver.cc 136 | src/vacancy/marching_cubes.cc 137 | src/vacancy/marching_cubes_lut.cc 138 | src/vacancy/extract_voxel.cc 139 | ) 140 | 141 | target_include_directories(${PUBLIC_LIB_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) 142 | target_include_directories(${PUBLIC_LIB_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src) 143 | target_include_directories(${PUBLIC_LIB_NAME} PUBLIC third_party) 144 | target_include_directories(${PUBLIC_LIB_NAME} PUBLIC ${EIGEN_INSTALL_DIR}) 145 | target_include_directories(${PUBLIC_LIB_NAME} PUBLIC ${STB_INSTALL_DIR}) 146 | 147 | set_target_properties(${PUBLIC_LIB_NAME} PROPERTIES VERSION ${PROJECT_VERSION}) 148 | 149 | set(EXAMPLES_EXE vacancy_examples) 150 | add_executable(${EXAMPLES_EXE} 151 | examples.cc) 152 | target_link_libraries(${EXAMPLES_EXE} 153 | ${PUBLIC_LIB_NAME} 154 | ${STB_LIB_NAME} 155 | ) 156 | if (WIN32) 157 | set_property(DIRECTORY PROPERTY VS_STARTUP_PROJECT ${EXAMPLES_EXE}) 158 | # suppress C2338 for eigen 159 | add_definitions(-D_ENABLE_EXTENDED_ALIGNED_STORAGE) 160 | endif() 161 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, unclearness 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Vacancy: A Voxel Carving implementation in C++ 2 | **Vacancy** is a *Voxel Carving* (a.k.a. *Visual Hull* or *Shape from Silhouette*) implementaion in C++. Inputs are 2D silhouettes (binary mask) of target objects, corresponding camera parameters (both intrinsic and extrinsic) and 3D bounding box to roughly specify the position of the objects. Output is the reconstructed 3D model. In addition to naive one, supports KinectFusion like robust TSDF (Truncated Signed Distance Function) fusion. 3 | 4 | # Algorithm Overview 5 | 6 | 7 | - Initialize 3D voxels according to the input bounding box 8 | - Compute SDF (Signed Distance Function/Field) from silhouette in 2D 9 | - Volmetrically fuse 2D SDF values into the 3D voxels by using camera parameters 10 | - Extract explicit 3D mesh from implicit surface representation in the 3D voxels 11 | 12 | # Output mesh 13 | Two mesh extraction methods are implemented: voxel and marching cubes. Marching cubes are much better in practice while voxel representation is suitable to understand how the algorithm works. 14 | 15 | |voxel|marching cubes| 16 | |---|---| 17 | ||| 18 | 19 | # Build 20 | To build sample bunny executable, use cmake with `CMakeLists.txt` in the top directory. 21 | You can integrate **Vacancy** to your own projects as static library by cmake `add_subdirectory()` command. 22 | 23 | # Dependencies 24 | ## Mandatory 25 | - Eigen 26 | https://github.com/eigenteam/eigen-git-mirror 27 | - Math 28 | ## Optional (can be disabled by cmake) 29 | - stb 30 | https://github.com/nothings/stb 31 | - Image I/O 32 | - OpenMP (if supported by your compiler) 33 | - Multi-thread accelaration 34 | -------------------------------------------------------------------------------- /data/mask_00000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unclearness/vacancy/4af8aecf25b7048451e21d20d6b602dab501beb4/data/mask_00000.png -------------------------------------------------------------------------------- /data/mask_00001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unclearness/vacancy/4af8aecf25b7048451e21d20d6b602dab501beb4/data/mask_00001.png -------------------------------------------------------------------------------- /data/mask_00002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unclearness/vacancy/4af8aecf25b7048451e21d20d6b602dab501beb4/data/mask_00002.png -------------------------------------------------------------------------------- /data/mask_00003.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unclearness/vacancy/4af8aecf25b7048451e21d20d6b602dab501beb4/data/mask_00003.png -------------------------------------------------------------------------------- /data/mask_00004.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unclearness/vacancy/4af8aecf25b7048451e21d20d6b602dab501beb4/data/mask_00004.png -------------------------------------------------------------------------------- /data/mask_00005.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unclearness/vacancy/4af8aecf25b7048451e21d20d6b602dab501beb4/data/mask_00005.png -------------------------------------------------------------------------------- /data/tumpose.txt: -------------------------------------------------------------------------------- 1 | 00000 -39.163902 -48.510956 -718.163391 0.000000 0.000000 0.000000 1.000000 2 | 00001 -39.163902 -48.510956 781.836609 0.000000 1.000000 0.000000 0.000000 3 | 00002 710.836121 -48.510956 31.836634 0.000000 -0.707107 0.000000 0.707107 4 | 00003 -789.163879 -48.510956 31.836634 0.000000 0.707107 0.000000 0.707107 5 | 00004 -39.163902 -798.510986 31.836634 -0.707107 0.000000 0.000000 0.707107 6 | 00005 -39.163902 701.489014 31.836634 0.707107 0.000000 0.000000 0.707107 7 | -------------------------------------------------------------------------------- /examples.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include "vacancy/voxel_carver.h" 11 | 12 | std::vector Split(const std::string& input, char delimiter) { 13 | std::istringstream stream(input); 14 | std::string field; 15 | std::vector result; 16 | while (std::getline(stream, field, delimiter)) { 17 | result.push_back(field); 18 | } 19 | return result; 20 | } 21 | 22 | bool LoadTumFormat(const std::string& path, 23 | std::vector>* poses) { 24 | poses->clear(); 25 | 26 | std::ifstream ifs(path); 27 | 28 | std::string line; 29 | while (std::getline(ifs, line)) { 30 | std::vector splited = Split(line, ' '); 31 | if (splited.size() != 8) { 32 | vacancy::LOGE("wrong tum format\n"); 33 | return false; 34 | } 35 | 36 | std::pair pose; 37 | pose.first = std::atoi(splited[0].c_str()); 38 | 39 | Eigen::Translation3d t; 40 | t.x() = std::atof(splited[1].c_str()); 41 | t.y() = std::atof(splited[2].c_str()); 42 | t.z() = std::atof(splited[3].c_str()); 43 | 44 | Eigen::Quaterniond q; 45 | q.x() = std::atof(splited[4].c_str()); 46 | q.y() = std::atof(splited[5].c_str()); 47 | q.z() = std::atof(splited[6].c_str()); 48 | q.w() = std::atof(splited[7].c_str()); 49 | 50 | pose.second = t * q; 51 | 52 | poses->push_back(pose); 53 | } 54 | 55 | return true; 56 | } 57 | 58 | bool LoadTumFormat(const std::string& path, 59 | std::vector* poses) { 60 | std::vector> tmp_poses; 61 | bool ret = LoadTumFormat(path, &tmp_poses); 62 | if (!ret) { 63 | return false; 64 | } 65 | 66 | poses->clear(); 67 | for (const auto& pose_pair : tmp_poses) { 68 | poses->push_back(pose_pair.second); 69 | } 70 | 71 | return true; 72 | } 73 | 74 | // test by bunny data with 6 views 75 | int main(int argc, char* argv[]) { 76 | (void)argc; 77 | (void)argv; 78 | 79 | std::string data_dir{"../data/"}; 80 | std::vector poses; 81 | LoadTumFormat(data_dir + "tumpose.txt", &poses); 82 | 83 | vacancy::VoxelCarver carver; 84 | vacancy::VoxelCarverOption option; 85 | 86 | // exact mesh bounding box computed in advacne 87 | option.bb_min = Eigen::Vector3f(-250.000000f, -344.586151f, -129.982697f); 88 | option.bb_max = Eigen::Vector3f(250.000000f, 150.542343f, 257.329224f); 89 | 90 | // add offset to the bounding box to keep boundary clean 91 | float bb_offset = 20.0f; 92 | option.bb_min[0] -= bb_offset; 93 | option.bb_min[1] -= bb_offset; 94 | option.bb_min[2] -= bb_offset; 95 | 96 | option.bb_max[0] += bb_offset; 97 | option.bb_max[1] += bb_offset; 98 | option.bb_max[2] += bb_offset; 99 | 100 | // voxel resolution is 10mm 101 | option.resolution = 10.0f; 102 | 103 | carver.set_option(option); 104 | 105 | carver.Init(); 106 | 107 | // image size and intrinsic parameters 108 | int width = 320; 109 | int height = 240; 110 | Eigen::Vector2f principal_point(159.3f, 127.65f); 111 | Eigen::Vector2f focal_length(258.65f, 258.25f); 112 | std::shared_ptr camera = 113 | std::make_shared(width, height, 114 | Eigen::Affine3d::Identity(), 115 | principal_point, focal_length); 116 | 117 | for (size_t i = 0; i < 6; i++) { 118 | camera->set_c2w(poses[i]); 119 | 120 | std::string num = vacancy::zfill(i); 121 | 122 | vacancy::Image1b silhouette; 123 | silhouette.Load(data_dir + "/mask_" + num + ".png"); 124 | 125 | vacancy::Image1f sdf; 126 | // Carve() is the main process to update voxels. Corresponds to the fusion 127 | // step in KinectFusion 128 | carver.Carve(*camera, silhouette, &sdf); 129 | 130 | // save SDF visualization 131 | vacancy::Image3b vis_sdf; 132 | vacancy::SignedDistance2Color(sdf, &vis_sdf, -1.0f, 1.0f); 133 | vis_sdf.WritePng(data_dir + "/sdf_" + num + ".png"); 134 | 135 | vacancy::Mesh mesh; 136 | // voxel extraction 137 | // slow for algorithm itself and saving to disk 138 | carver.ExtractVoxel(&mesh); 139 | mesh.WritePly(data_dir + "/voxel_" + num + ".ply"); 140 | 141 | // marching cubes 142 | // smoother and faster 143 | carver.ExtractIsoSurface(&mesh, 0.0); 144 | mesh.WritePly(data_dir + "/surface_" + num + ".ply"); 145 | 146 | // No linear interpolation for marhching cubes, angular surface 147 | carver.ExtractIsoSurface(&mesh, 0.0, false); 148 | mesh.WritePly(data_dir + "/surface_nointerp_" + num + ".ply"); 149 | } 150 | 151 | return 0; 152 | } 153 | -------------------------------------------------------------------------------- /include/vacancy/camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | /* 7 | * right-handed coordinate system 8 | * z:forward, y:down, x:right 9 | * same to OpenCV 10 | */ 11 | 12 | #pragma once 13 | 14 | #include "vacancy/common.h" 15 | 16 | namespace vacancy { 17 | class Camera { 18 | protected: 19 | int width_; 20 | int height_; 21 | Eigen::Affine3d c2w_; // camera -> world, sometimes called as "pose" 22 | Eigen::Affine3d w2c_; 23 | 24 | public: 25 | Camera(); 26 | virtual ~Camera(); 27 | Camera(int width, int height); 28 | Camera(int width, int height, const Eigen::Affine3d& c2w); 29 | int width() const; 30 | int height() const; 31 | const Eigen::Affine3d& c2w() const; 32 | const Eigen::Affine3d& w2c() const; 33 | void set_size(int width, int height); 34 | void set_c2w(const Eigen::Affine3d& c2w); 35 | 36 | // camera -> image conversion 37 | virtual void Project(const Eigen::Vector3f& camera_p, 38 | Eigen::Vector3f* image_p) const = 0; 39 | virtual void Project(const Eigen::Vector3f& camera_p, 40 | Eigen::Vector2f* image_p) const = 0; 41 | virtual void Project(const Eigen::Vector3f& camera_p, 42 | Eigen::Vector2f* image_p, float* d) const = 0; 43 | 44 | // image -> camera conversion 45 | // need depth value as input 46 | virtual void Unproject(const Eigen::Vector3f& image_p, 47 | Eigen::Vector3f* camera_p) const = 0; 48 | virtual void Unproject(const Eigen::Vector2f& image_p, float d, 49 | Eigen::Vector3f* camera_p) const = 0; 50 | 51 | // position emmiting ray 52 | virtual void org_ray_c(float x, float y, Eigen::Vector3f* org) const = 0; 53 | virtual void org_ray_w(float x, float y, Eigen::Vector3f* org) const = 0; 54 | 55 | // ray direction 56 | virtual void ray_c( 57 | float x, float y, 58 | Eigen::Vector3f* dir) const = 0; // ray in camera coordinate 59 | virtual void ray_w( 60 | float x, float y, 61 | Eigen::Vector3f* dir) const = 0; // ray in world coordinate 62 | }; 63 | 64 | // Pinhole camera model with pixel-scale principal point and focal length 65 | // Widely used in computer vision community as perspective camera model 66 | // Valid only if FoV is much less than 180 deg. 67 | class PinholeCamera : public Camera { 68 | Eigen::Vector2f principal_point_; 69 | Eigen::Vector2f focal_length_; 70 | 71 | public: 72 | PinholeCamera(); 73 | ~PinholeCamera(); 74 | PinholeCamera(int width, int height); 75 | PinholeCamera(int width, int height, float fov_y_deg); 76 | PinholeCamera(int width, int height, const Eigen::Affine3d& c2w); 77 | PinholeCamera(int width, int height, const Eigen::Affine3d& c2w, 78 | float fov_y_deg); 79 | PinholeCamera(int width, int height, const Eigen::Affine3d& c2w, 80 | const Eigen::Vector2f& principal_point, 81 | const Eigen::Vector2f& focal_length); 82 | 83 | // FoV (Field of View) in degree interface is provided for convenience 84 | float fov_x() const; 85 | float fov_y() const; 86 | void set_fov_x(float fov_x_deg); 87 | void set_fov_y(float fov_y_deg); 88 | 89 | // pixel-scale principal point and focal length 90 | const Eigen::Vector2f& principal_point() const; 91 | const Eigen::Vector2f& focal_length() const; 92 | void set_principal_point(const Eigen::Vector2f& principal_point); 93 | void set_focal_length(const Eigen::Vector2f& focal_length); 94 | 95 | void Project(const Eigen::Vector3f& camera_p, 96 | Eigen::Vector3f* image_p) const override; 97 | void Project(const Eigen::Vector3f& camera_p, 98 | Eigen::Vector2f* image_p) const override; 99 | void Project(const Eigen::Vector3f& camera_p, Eigen::Vector2f* image_p, 100 | float* d) const override; 101 | void Unproject(const Eigen::Vector3f& image_p, 102 | Eigen::Vector3f* camera_p) const override; 103 | void Unproject(const Eigen::Vector2f& image_p, float d, 104 | Eigen::Vector3f* camera_p) const override; 105 | void org_ray_c(float x, float y, Eigen::Vector3f* org) const override; 106 | void org_ray_w(float x, float y, Eigen::Vector3f* org) const override; 107 | void ray_c(float x, float y, Eigen::Vector3f* dir) const override; 108 | void ray_w(float x, float y, Eigen::Vector3f* dir) const override; 109 | }; 110 | 111 | // Orthographic/orthogonal projection camera with no perspective 112 | // Image coordinate is translated camera coordinate 113 | // Different from pinhole camera in particular x and y coordinate in image 114 | class OrthoCamera : public Camera { 115 | public: 116 | OrthoCamera(); 117 | ~OrthoCamera(); 118 | OrthoCamera(int width, int height); 119 | OrthoCamera(int width, int height, const Eigen::Affine3d& c2w); 120 | 121 | void Project(const Eigen::Vector3f& camera_p, 122 | Eigen::Vector3f* image_p) const override; 123 | void Project(const Eigen::Vector3f& camera_p, 124 | Eigen::Vector2f* image_p) const override; 125 | void Project(const Eigen::Vector3f& camera_p, Eigen::Vector2f* image_p, 126 | float* d) const override; 127 | void Unproject(const Eigen::Vector3f& image_p, 128 | Eigen::Vector3f* camera_p) const override; 129 | void Unproject(const Eigen::Vector2f& image_p, float d, 130 | Eigen::Vector3f* camera_p) const override; 131 | void org_ray_c(float x, float y, Eigen::Vector3f* org) const override; 132 | void org_ray_w(float x, float y, Eigen::Vector3f* org) const override; 133 | void ray_c(float x, float y, Eigen::Vector3f* dir) const override; 134 | void ray_w(float x, float y, Eigen::Vector3f* dir) const override; 135 | }; 136 | 137 | } // namespace vacancy 138 | -------------------------------------------------------------------------------- /include/vacancy/common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #ifdef _WIN32 14 | #pragma warning(push) 15 | #pragma warning(disable : 4127) 16 | #pragma warning(disable : 4819) 17 | #pragma warning(disable : 26451) 18 | #pragma warning(disable : 26495) 19 | #pragma warning(disable : 26812) 20 | #endif 21 | #include "Eigen/Geometry" 22 | #ifdef _WIN32 23 | #pragma warning(pop) 24 | #endif 25 | 26 | #include "vacancy/log.h" 27 | 28 | namespace vacancy { 29 | 30 | // borrow from glm 31 | // radians 32 | template 33 | genType radians(genType degrees) { 34 | // "'radians' only accept floating-point input" 35 | assert(std::numeric_limits::is_iec559); 36 | 37 | return degrees * static_cast(0.01745329251994329576923690768489); 38 | } 39 | 40 | // degrees 41 | template 42 | genType degrees(genType radians) { 43 | // "'degrees' only accept floating-point input" 44 | assert(std::numeric_limits::is_iec559); 45 | 46 | return radians * static_cast(57.295779513082320876798154814105); 47 | } 48 | 49 | // https://stackoverflow.com/questions/13768423/setting-up-projection-model-and-view-transformations-for-vertex-shader-in-eige 50 | template 51 | void c2w(const Eigen::Matrix& position, 52 | const Eigen::Matrix& target, const Eigen::Matrix& up, 53 | Eigen::Matrix* R) { 54 | assert(std::numeric_limits::is_iec559); 55 | 56 | R->col(2) = (target - position).normalized(); 57 | R->col(0) = R->col(2).cross(up).normalized(); 58 | R->col(1) = R->col(2).cross(R->col(0)); 59 | } 60 | 61 | template 62 | void c2w(const Eigen::Matrix& position, 63 | const Eigen::Matrix& target, 64 | const Eigen::Matrix& up, 65 | Eigen::Matrix* T) { 66 | assert(std::numeric_limits::is_iec559); 67 | 68 | *T = Eigen::Matrix::Identity(); 69 | 70 | Eigen::Matrix R; 71 | c2w(position, target, up, &R); 72 | 73 | T->topLeftCorner(3, 3) = R; 74 | T->topRightCorner(3, 1) = position; 75 | } 76 | 77 | template 78 | std::string zfill(const T& val, int num = 5) { 79 | std::ostringstream sout; 80 | sout << std::setfill('0') << std::setw(num) << val; 81 | return sout.str(); 82 | } 83 | 84 | } // namespace vacancy 85 | -------------------------------------------------------------------------------- /include/vacancy/image.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "vacancy/common.h" 14 | 15 | #ifdef VACANCY_USE_STB 16 | #include "stb/stb_image.h" 17 | #include "stb/stb_image_write.h" 18 | #endif 19 | 20 | namespace vacancy { 21 | 22 | template 23 | class Image { 24 | std::vector data_; 25 | int width_{-1}; 26 | int height_{-1}; 27 | const int bit_depth_{sizeof(T)}; 28 | const int channel_{N}; 29 | 30 | public: 31 | Image() {} 32 | ~Image() {} 33 | Image(int width, int height) { Init(width, height); } 34 | Image(int width, int height, T val) { Init(width, height, val); } 35 | int width() const { return width_; } 36 | int height() const { return height_; } 37 | int channel() const { return channel_; } 38 | const std::vector& data() const { return data_; } 39 | std::vector* data_ptr() { return &data_; } 40 | void Clear() { 41 | data_.clear(); 42 | width_ = -1; 43 | height_ = -1; 44 | } 45 | bool empty() const { 46 | if (width_ < 0 || height_ < 0 || data_.empty()) { 47 | return true; 48 | } 49 | return false; 50 | } 51 | void Init(int width, int height, T val = 0) { 52 | Clear(); 53 | width_ = width; 54 | height_ = height; 55 | data_.resize(height_ * width_ * channel_, val); 56 | } 57 | T* at(int x, int y) { 58 | assert(0 <= x && x < width_ && 0 <= y && y < height_); 59 | return &data_[0] + (width_ * channel_ * y + x * channel_); 60 | } 61 | const T* at(int x, int y) const { 62 | assert(0 <= x && x < width_ && 0 <= y && y < height_); 63 | return &data_[0] + (width_ * channel_ * y + x * channel_); 64 | } 65 | T& at(int x, int y, int c) { 66 | assert(0 <= x && x < width_ && 0 <= y && y < height_ && 0 <= c && 67 | c < channel_); 68 | return data_[width_ * channel_ * y + x * channel_ + c]; 69 | } 70 | const T& at(int x, int y, int c) const { 71 | assert(0 <= x && x < width_ && 0 <= y && y < height_ && 0 <= c && 72 | c < channel_); 73 | return data_[width_ * channel_ * y + x * channel_ + c]; 74 | } 75 | 76 | #ifdef VACANCY_USE_STB 77 | bool Load(const std::string& path) { 78 | unsigned char* in_pixels_tmp; 79 | int width; 80 | int height; 81 | int bpp; 82 | 83 | in_pixels_tmp = stbi_load(path.c_str(), &width, &height, &bpp, 0); 84 | 85 | if (bpp != channel_) { 86 | delete in_pixels_tmp; 87 | LOGE("desired channel %d, actual %d\n", channel_, bpp); 88 | return false; 89 | } 90 | 91 | width_ = width; 92 | height_ = height; 93 | 94 | data_.resize(static_cast(height_) * static_cast(width_) * 95 | static_cast(channel_)); 96 | std::memcpy(&data_[0], in_pixels_tmp, 97 | sizeof(T) * channel_ * width_ * height_); 98 | 99 | delete in_pixels_tmp; 100 | return true; 101 | } 102 | 103 | bool WritePng(const std::string& path) const { 104 | if (bit_depth_ != 1) { 105 | LOGE("1 byte per channel is required to save by stb_image: actual %d\n", 106 | bit_depth_); 107 | return false; 108 | } 109 | 110 | if (width_ < 0 || height_ < 0) { 111 | LOGE("image is empty\n"); 112 | return false; 113 | } 114 | 115 | stbi_write_png(path.c_str(), width_, height_, channel_, &data_[0], 116 | width_ * channel_ * sizeof(T)); 117 | return true; 118 | } 119 | #else 120 | bool Load(const std::string& path) { 121 | (void)path; 122 | LOGE("This method is not supported by the current configuration\n"); 123 | return false; 124 | } 125 | bool WritePng(const std::string& path) const { 126 | (void)path; 127 | LOGE("This method is not supported by the current configuration\n"); 128 | return false; 129 | } 130 | #endif 131 | 132 | template 133 | bool ConvertTo(Image* dst, float scale = 1.0f) const { 134 | if (channel_ != dst->channel()) { 135 | LOGE("ConvertTo failed src channel %d, dst channel %d\n", channel_, 136 | dst->channel()); 137 | return false; 138 | } 139 | 140 | dst->Init(width_, height_); 141 | 142 | for (int y = 0; y < height_; y++) { 143 | for (int x = 0; x < width_; x++) { 144 | for (int c = 0; c < N; c++) { 145 | dst->at(x, y, c) = static_cast(scale * at(x, y, c)); 146 | } 147 | } 148 | } 149 | return true; 150 | } 151 | 152 | bool CopyTo(Image* dst) const { return ConvertTo(dst, 1.0f); } 153 | }; 154 | 155 | using Image1b = Image; // For gray image. 156 | using Image3b = Image; // For color image. RGB order. 157 | using Image1w = Image; // For depth image with 16 bit (unsigned 158 | // short) mm-scale format 159 | using Image1i = 160 | Image; // For face visibility. face id is within int32_t 161 | using Image1f = Image; // For depth image with any scale 162 | using Image3f = Image; // For normal or point cloud. XYZ order. 163 | 164 | void Depth2Gray(const Image1f& depth, Image1b* vis_depth, float min_d = 200.0f, 165 | float max_d = 1500.0f); 166 | 167 | void Normal2Color(const Image3f& normal, Image3b* vis_normal); 168 | 169 | void FaceId2RandomColor(const Image1i& face_id, Image3b* vis_face_id); 170 | 171 | } // namespace vacancy 172 | -------------------------------------------------------------------------------- /include/vacancy/log.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #pragma once 7 | 8 | namespace vacancy { 9 | 10 | enum class LogLevel { 11 | kVerbose = 0, 12 | kDebug = 1, 13 | kInfo = 2, 14 | kWarning = 3, 15 | kError = 4, 16 | kNone = 5 17 | }; 18 | 19 | void set_log_level(LogLevel level); 20 | LogLevel get_log_level(); 21 | void LOGD(const char *format, ...); 22 | void LOGI(const char *format, ...); 23 | void LOGW(const char *format, ...); 24 | void LOGE(const char *format, ...); 25 | 26 | } // namespace vacancy 27 | -------------------------------------------------------------------------------- /include/vacancy/mesh.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "vacancy/common.h" 13 | #include "vacancy/image.h" 14 | 15 | namespace vacancy { 16 | 17 | struct MeshStats { 18 | Eigen::Vector3f center; 19 | Eigen::Vector3f bb_min; 20 | Eigen::Vector3f bb_max; 21 | }; 22 | 23 | class Mesh { 24 | std::vector vertices_; 25 | std::vector vertex_colors_; // optional, RGB order 26 | std::vector vertex_indices_; // face 27 | 28 | std::vector normals_; // normal per vertex 29 | std::vector face_normals_; // normal per face 30 | std::vector normal_indices_; 31 | 32 | std::vector uv_; 33 | std::vector uv_indices_; 34 | 35 | std::string diffuse_texname_; 36 | std::string diffuse_texpath_; 37 | Image3b diffuse_tex_; 38 | MeshStats stats_; 39 | 40 | public: 41 | Mesh(); 42 | ~Mesh(); 43 | Mesh(const Mesh& src); 44 | void Clear(); 45 | 46 | // get average normal per vertex from face normal 47 | // caution: this does not work for cube with 8 vertices unless vertices are 48 | // splitted (24 vertices) 49 | void CalcNormal(); 50 | 51 | void CalcFaceNormal(); 52 | void CalcStats(); 53 | 54 | void RemoveDuplicatedVertices(); 55 | 56 | void Rotate(const Eigen::Matrix3f& R); 57 | void Translate(const Eigen::Vector3f& t); 58 | void Transform(const Eigen::Matrix3f& R, const Eigen::Vector3f& t); 59 | void Scale(float scale); 60 | void Scale(float x_scale, float y_scale, float z_scale); 61 | const std::vector& vertices() const; 62 | const std::vector& vertex_colors() const; 63 | const std::vector& vertex_indices() const; 64 | const std::vector& normals() const; 65 | const std::vector& face_normals() const; 66 | const std::vector& normal_indices() const; 67 | const std::vector& uv() const; 68 | const std::vector& uv_indices() const; 69 | const MeshStats& stats() const; 70 | const Image3b& diffuse_tex() const; 71 | 72 | bool set_vertices(const std::vector& vertices); 73 | bool set_vertex_colors(const std::vector& vertex_colors); 74 | bool set_vertex_indices(const std::vector& vertex_indices); 75 | bool set_normals(const std::vector& normals); 76 | bool set_face_normals(const std::vector& face_normals); 77 | bool set_normal_indices(const std::vector& normal_indices); 78 | bool set_uv(const std::vector& uv); 79 | bool set_uv_indices(const std::vector& uv_indices); 80 | bool set_diffuse_tex(const Image3b& diffuse_tex); 81 | 82 | #ifdef VACANCY_USE_TINYOBJLOADER 83 | bool LoadObj(const std::string& obj_path, const std::string& mtl_dir); 84 | #endif 85 | bool LoadPly(const std::string& ply_path); 86 | bool WritePly(const std::string& ply_path) const; 87 | #ifdef VACANCY_USE_STB 88 | bool WriteObj(const std::string& obj_dir, const std::string& obj_basename, 89 | const std::string& mtl_basename = "", 90 | const std::string& tex_basename = "") const; 91 | #endif 92 | }; 93 | 94 | // make cube with 24 vertices 95 | std::shared_ptr MakeCube(const Eigen::Vector3f& length, 96 | const Eigen::Matrix3f& R, 97 | const Eigen::Vector3f& t); 98 | std::shared_ptr MakeCube(const Eigen::Vector3f& length); 99 | std::shared_ptr MakeCube(float length, const Eigen::Matrix3f& R, 100 | const Eigen::Vector3f& t); 101 | std::shared_ptr MakeCube(float length); 102 | 103 | void SetRandomVertexColor(std::shared_ptr mesh, int seed = 0); 104 | 105 | } // namespace vacancy 106 | -------------------------------------------------------------------------------- /include/vacancy/voxel_carver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "vacancy/camera.h" 13 | #include "vacancy/common.h" 14 | #include "vacancy/image.h" 15 | #include "vacancy/mesh.h" 16 | 17 | namespace vacancy { 18 | 19 | // Voxel update type 20 | enum class VoxelUpdate { 21 | kMax = 0, // take max. naive voxel carving 22 | kWeightedAverage = 1 // weighted average like KinectFusion. truncation is 23 | // necessary to get good result 24 | }; 25 | 26 | // Interpolation method for 2D SDF 27 | enum class SdfInterpolation { 28 | kNn = 0, // Nearest Neigbor 29 | kBilinear = 1 // Bilinear interpolation 30 | }; 31 | 32 | // The way to update voxels which are projected to outside of the current image 33 | enum class UpdateOutsideImage { 34 | kNone = 0, // Do nothing 35 | kMax = 1 // Fill by max sdf of the current image. This is valid only when 36 | // silhouette is not protruding over the current image edge 37 | }; 38 | 39 | struct InvalidSdf { 40 | static const float kVal; 41 | }; 42 | 43 | struct VoxelUpdateOption { 44 | VoxelUpdate voxel_update{VoxelUpdate::kMax}; 45 | SdfInterpolation sdf_interp{SdfInterpolation::kBilinear}; 46 | UpdateOutsideImage update_outside{UpdateOutsideImage::kNone}; 47 | int voxel_max_update_num{ 48 | 255}; // After updating voxel_max_update_num, no sdf update 49 | float voxel_update_weight{1.0f}; // only valid if kWeightedAverage is set 50 | bool use_truncation{false}; 51 | float truncation_band{0.1f}; // only positive value is valid 52 | }; 53 | 54 | struct VoxelCarverOption { 55 | Eigen::Vector3f bb_max; 56 | Eigen::Vector3f bb_min; 57 | float resolution{0.1f}; // default is 10cm if input is m-scale 58 | bool sdf_minmax_normalize{true}; 59 | VoxelUpdateOption update_option; 60 | }; 61 | 62 | struct Voxel { 63 | Eigen::Vector3i index{-1, -1, -1}; // voxel index 64 | int id{-1}; 65 | Eigen::Vector3f pos{0.0f, 0.0f, 0.0f}; // center of voxel 66 | float sdf{0.0f}; // Signed Distance Function (SDF) value 67 | int update_num{0}; 68 | bool outside{false}; 69 | bool on_surface{false}; 70 | Voxel(); 71 | ~Voxel(); 72 | }; 73 | 74 | class VoxelGrid { 75 | std::vector voxels_; 76 | Eigen::Vector3f bb_max_; 77 | Eigen::Vector3f bb_min_; 78 | float resolution_{-1.0f}; 79 | Eigen::Vector3i voxel_num_{0, 0, 0}; 80 | int xy_slice_num_{0}; 81 | 82 | public: 83 | VoxelGrid(); 84 | ~VoxelGrid(); 85 | bool Init(const Eigen::Vector3f& bb_max, const Eigen::Vector3f& bb_min, 86 | float resolution); 87 | const Eigen::Vector3i& voxel_num() const; 88 | const Voxel& get(int x, int y, int z) const; 89 | Voxel* get_ptr(int x, int y, int z); 90 | float resolution() const; 91 | void ResetOnSurface(); 92 | bool initialized() const; 93 | }; 94 | 95 | class VoxelCarver { 96 | VoxelCarverOption option_; 97 | std::unique_ptr voxel_grid_; 98 | 99 | public: 100 | VoxelCarver(); 101 | ~VoxelCarver(); 102 | explicit VoxelCarver(VoxelCarverOption option); 103 | void set_option(VoxelCarverOption option); 104 | bool Init(); 105 | bool Carve(const Camera& camera, const Image1b& silhouette, 106 | const Eigen::Vector2i& roi_min, const Eigen::Vector2i& roi_max, 107 | Image1f* sdf); 108 | bool Carve(const Camera& camera, const Eigen::Vector2i& roi_min, 109 | const Eigen::Vector2i& roi_max, const Image1f& sdf); 110 | bool Carve(const Camera& camera, const Image1b& silhouette, Image1f* sdf); 111 | bool Carve(const Camera& camera, const Image1b& silhouette); 112 | bool Carve(const Camera& camera, const Image1f& sdf); 113 | bool Carve(const std::vector& cameras, 114 | const std::vector& silhouettes); 115 | void ExtractVoxel(Mesh* mesh, bool inside_empty = false); 116 | void ExtractIsoSurface(Mesh* mesh, double iso_level = 0.0, 117 | bool linear_interp = true); 118 | }; 119 | 120 | void DistanceTransformL1(const Image1b& mask, const Eigen::Vector2i& roi_min, 121 | const Eigen::Vector2i& roi_max, Image1f* dist); 122 | void MakeSignedDistanceField(const Image1b& mask, 123 | const Eigen::Vector2i& roi_min, 124 | const Eigen::Vector2i& roi_max, Image1f* dist, 125 | bool minmax_normalize, bool use_truncation, 126 | float truncation_band); 127 | void SignedDistance2Color(const Image1f& sdf, Image3b* vis_sdf, 128 | float min_negative_d, float max_positive_d); 129 | 130 | } // namespace vacancy 131 | -------------------------------------------------------------------------------- /rebuild.bat: -------------------------------------------------------------------------------- 1 | cmake --build win_build -------------------------------------------------------------------------------- /reconfigure.bat: -------------------------------------------------------------------------------- 1 | rd /s /q win_build 2 | md win_build 3 | cd win_build 4 | 5 | IF EXIST "C:\\Program Files (x86)\\Microsoft Visual Studio\\2019\\" ( 6 | cmake -G "Visual Studio 16 2019" .. 7 | ) ELSE IF EXIST "C:\\Program Files (x86)\\Microsoft Visual Studio\\2017\\" ( 8 | cmake -G "Visual Studio 15 2017 Win64" .. 9 | ) 10 | 11 | pause 12 | -------------------------------------------------------------------------------- /src/vacancy/camera.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #include "vacancy/camera.h" 7 | 8 | namespace vacancy { 9 | 10 | Camera::Camera() 11 | : width_(-1), 12 | height_(-1), 13 | c2w_(Eigen::Affine3d::Identity()), 14 | w2c_(Eigen::Affine3d::Identity()) {} 15 | 16 | Camera::~Camera() {} 17 | 18 | Camera::Camera(int width, int height) 19 | : width_(width), 20 | height_(height), 21 | c2w_(Eigen::Affine3d::Identity()), 22 | w2c_(Eigen::Affine3d::Identity()) {} 23 | 24 | Camera::Camera(int width, int height, const Eigen::Affine3d& c2w) 25 | : width_(width), height_(height), c2w_(c2w), w2c_(c2w_.inverse()) {} 26 | 27 | int Camera::width() const { return width_; } 28 | 29 | int Camera::height() const { return height_; } 30 | 31 | const Eigen::Affine3d& Camera::c2w() const { return c2w_; } 32 | 33 | const Eigen::Affine3d& Camera::w2c() const { return w2c_; } 34 | 35 | void Camera::set_size(int width, int height) { 36 | width_ = width; 37 | height_ = height; 38 | } 39 | void Camera::set_c2w(const Eigen::Affine3d& c2w) { 40 | c2w_ = c2w; 41 | w2c_ = c2w_.inverse(); 42 | } 43 | 44 | PinholeCamera::PinholeCamera() 45 | : Camera(), principal_point_(-1, -1), focal_length_(-1, -1) {} 46 | 47 | PinholeCamera::~PinholeCamera() {} 48 | 49 | PinholeCamera::PinholeCamera(int width, int height) 50 | : Camera(width, height), principal_point_(-1, -1), focal_length_(-1, -1) {} 51 | 52 | PinholeCamera::PinholeCamera(int width, int height, float fov_y_deg) 53 | : Camera(width, height) { 54 | principal_point_[0] = width_ * 0.5f - 0.5f; 55 | principal_point_[1] = height_ * 0.5f - 0.5f; 56 | 57 | set_fov_y(fov_y_deg); 58 | } 59 | 60 | PinholeCamera::PinholeCamera(int width, int height, const Eigen::Affine3d& c2w) 61 | : Camera(width, height, c2w), 62 | principal_point_(-1, -1), 63 | focal_length_(-1, -1) {} 64 | 65 | PinholeCamera::PinholeCamera(int width, int height, const Eigen::Affine3d& c2w, 66 | float fov_y_deg) 67 | : Camera(width, height, c2w) { 68 | principal_point_[0] = width_ * 0.5f - 0.5f; 69 | principal_point_[1] = height_ * 0.5f - 0.5f; 70 | 71 | set_fov_y(fov_y_deg); 72 | } 73 | 74 | PinholeCamera::PinholeCamera(int width, int height, const Eigen::Affine3d& c2w, 75 | const Eigen::Vector2f& principal_point, 76 | const Eigen::Vector2f& focal_length) 77 | : Camera(width, height, c2w), 78 | principal_point_(principal_point), 79 | focal_length_(focal_length) {} 80 | 81 | float PinholeCamera::fov_x() const { 82 | return degrees(2 * std::atan(width_ * 0.5f / focal_length_[0])); 83 | } 84 | 85 | float PinholeCamera::fov_y() const { 86 | return degrees(2 * std::atan(height_ * 0.5f / focal_length_[1])); 87 | } 88 | 89 | const Eigen::Vector2f& PinholeCamera::principal_point() const { 90 | return principal_point_; 91 | } 92 | 93 | const Eigen::Vector2f& PinholeCamera::focal_length() const { 94 | return focal_length_; 95 | } 96 | 97 | void PinholeCamera::set_principal_point( 98 | const Eigen::Vector2f& principal_point) { 99 | principal_point_ = principal_point; 100 | } 101 | 102 | void PinholeCamera::set_focal_length(const Eigen::Vector2f& focal_length) { 103 | focal_length_ = focal_length; 104 | } 105 | 106 | void PinholeCamera::set_fov_x(float fov_x_deg) { 107 | // same focal length per pixel for x and y 108 | focal_length_[0] = 109 | width_ * 0.5f / 110 | static_cast(std::tan(radians(fov_x_deg) * 0.5)); 111 | focal_length_[1] = focal_length_[0]; 112 | } 113 | 114 | void PinholeCamera::set_fov_y(float fov_y_deg) { 115 | // same focal length per pixel for x and y 116 | focal_length_[1] = 117 | height_ * 0.5f / 118 | static_cast(std::tan(radians(fov_y_deg) * 0.5)); 119 | focal_length_[0] = focal_length_[1]; 120 | } 121 | 122 | void PinholeCamera::Project(const Eigen::Vector3f& camera_p, 123 | Eigen::Vector3f* image_p) const { 124 | (*image_p)[0] = 125 | focal_length_[0] / camera_p[2] * camera_p[0] + principal_point_[0]; 126 | (*image_p)[1] = 127 | focal_length_[1] / camera_p[2] * camera_p[1] + principal_point_[1]; 128 | (*image_p)[2] = camera_p[2]; 129 | } 130 | 131 | void PinholeCamera::Project(const Eigen::Vector3f& camera_p, 132 | Eigen::Vector2f* image_p) const { 133 | (*image_p)[0] = 134 | focal_length_[0] / camera_p[2] * camera_p[0] + principal_point_[0]; 135 | (*image_p)[1] = 136 | focal_length_[1] / camera_p[2] * camera_p[1] + principal_point_[1]; 137 | } 138 | 139 | void PinholeCamera::Project(const Eigen::Vector3f& camera_p, 140 | Eigen::Vector2f* image_p, float* d) const { 141 | (*image_p)[0] = 142 | focal_length_[0] / camera_p[2] * camera_p[0] + principal_point_[0]; 143 | (*image_p)[1] = 144 | focal_length_[1] / camera_p[2] * camera_p[1] + principal_point_[1]; 145 | *d = camera_p[2]; 146 | } 147 | 148 | void PinholeCamera::Unproject(const Eigen::Vector3f& image_p, 149 | Eigen::Vector3f* camera_p) const { 150 | (*camera_p)[0] = 151 | (image_p[0] - principal_point_[0]) * image_p[2] / focal_length_[0]; 152 | (*camera_p)[1] = 153 | (image_p[1] - principal_point_[1]) * image_p[2] / focal_length_[1]; 154 | (*camera_p)[2] = image_p[2]; 155 | } 156 | 157 | void PinholeCamera::Unproject(const Eigen::Vector2f& image_p, float d, 158 | Eigen::Vector3f* camera_p) const { 159 | (*camera_p)[0] = (image_p[0] - principal_point_[0]) * d / focal_length_[0]; 160 | (*camera_p)[1] = (image_p[1] - principal_point_[1]) * d / focal_length_[1]; 161 | (*camera_p)[2] = d; 162 | } 163 | 164 | void PinholeCamera::org_ray_c(float x, float y, Eigen::Vector3f* org) const { 165 | (void)x; 166 | (void)y; 167 | (*org)[0] = 0.0f; 168 | (*org)[1] = 0.0f; 169 | (*org)[2] = 0.0f; 170 | } 171 | 172 | void PinholeCamera::org_ray_w(float x, float y, Eigen::Vector3f* org) const { 173 | (void)x; 174 | (void)y; 175 | *org = c2w_.matrix().block<3, 1>(0, 3).cast(); 176 | } 177 | 178 | void PinholeCamera::ray_c(float x, float y, Eigen::Vector3f* dir) const { 179 | (*dir)[0] = (x - principal_point_[0]) / focal_length_[0]; 180 | (*dir)[1] = (y - principal_point_[1]) / focal_length_[1]; 181 | (*dir)[2] = 1.0f; 182 | dir->normalize(); 183 | } 184 | 185 | void PinholeCamera::ray_w(float x, float y, Eigen::Vector3f* dir) const { 186 | ray_c(x, y, dir); 187 | *dir = c2w_.matrix().block<3, 3>(0, 0).cast() * *dir; 188 | } 189 | 190 | OrthoCamera::OrthoCamera() : Camera() {} 191 | OrthoCamera::~OrthoCamera() {} 192 | OrthoCamera::OrthoCamera(int width, int height) : Camera(width, height) {} 193 | OrthoCamera::OrthoCamera(int width, int height, const Eigen::Affine3d& c2w) 194 | : Camera(width, height, c2w) {} 195 | 196 | void OrthoCamera::Project(const Eigen::Vector3f& camera_p, 197 | Eigen::Vector3f* image_p) const { 198 | *image_p = camera_p; 199 | } 200 | 201 | void OrthoCamera::Project(const Eigen::Vector3f& camera_p, 202 | Eigen::Vector2f* image_p) const { 203 | (*image_p)[0] = camera_p[0]; 204 | (*image_p)[1] = camera_p[1]; 205 | } 206 | 207 | void OrthoCamera::Project(const Eigen::Vector3f& camera_p, 208 | Eigen::Vector2f* image_p, float* d) const { 209 | (*image_p)[0] = camera_p[0]; 210 | (*image_p)[1] = camera_p[1]; 211 | *d = camera_p[2]; 212 | } 213 | 214 | void OrthoCamera::Unproject(const Eigen::Vector3f& image_p, 215 | Eigen::Vector3f* camera_p) const { 216 | *camera_p = image_p; 217 | } 218 | 219 | void OrthoCamera::Unproject(const Eigen::Vector2f& image_p, float d, 220 | Eigen::Vector3f* camera_p) const { 221 | (*camera_p)[0] = image_p[0]; 222 | (*camera_p)[1] = image_p[1]; 223 | (*camera_p)[2] = d; 224 | } 225 | 226 | void OrthoCamera::org_ray_c(float x, float y, Eigen::Vector3f* org) const { 227 | (*org)[0] = x - width_ / 2; 228 | (*org)[1] = y - height_ / 2; 229 | (*org)[2] = 0.0f; 230 | } 231 | 232 | void OrthoCamera::org_ray_w(float x, float y, Eigen::Vector3f* org) const { 233 | *org = c2w_.matrix().block<3, 1>(0, 3).cast(); 234 | 235 | Eigen::Vector3f x_direc = 236 | c2w_.matrix().block<3, 3>(0, 0).col(0).cast(); 237 | Eigen::Vector3f y_direc = 238 | c2w_.matrix().block<3, 3>(0, 0).col(1).cast(); 239 | 240 | Eigen::Vector3f offset_x = (x - width_ * 0.5f) * x_direc; 241 | Eigen::Vector3f offset_y = (y - height_ * 0.5f) * y_direc; 242 | 243 | *org += offset_x; 244 | *org += offset_y; 245 | } 246 | 247 | void OrthoCamera::ray_c(float x, float y, Eigen::Vector3f* dir) const { 248 | (void)x; 249 | (void)y; 250 | // parallell ray along with z axis 251 | (*dir)[0] = 0.0f; 252 | (*dir)[1] = 0.0f; 253 | (*dir)[2] = 1.0f; 254 | } 255 | 256 | void OrthoCamera::ray_w(float x, float y, Eigen::Vector3f* dir) const { 257 | (void)x; 258 | (void)y; 259 | // extract z direction of camera pose 260 | *dir = Eigen::Vector3f(c2w_.matrix().block<3, 3>(0, 0).cast().col(2)); 261 | } 262 | 263 | } // namespace vacancy 264 | -------------------------------------------------------------------------------- /src/vacancy/extract_voxel.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #include "vacancy/extract_voxel.h" 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace { 14 | 15 | void UpdateOnSurface(vacancy::VoxelGrid* voxel_grid) { 16 | // raycast like surface detection 17 | // search xyz axes to detect the voxel on sign change 18 | 19 | const Eigen::Vector3i& voxel_num = voxel_grid->voxel_num(); 20 | 21 | constexpr float e = std::numeric_limits::min(); 22 | 23 | // x 24 | for (int z = 0; z < voxel_num.z(); z++) { 25 | for (int y = 0; y < voxel_num.y(); y++) { 26 | for (int x = 1; x < voxel_num.x(); x++) { 27 | const vacancy::Voxel& prev_voxel = voxel_grid->get(x - 1, y, z); 28 | vacancy::Voxel* voxel = voxel_grid->get_ptr(x, y, z); 29 | if (voxel->update_num < 1 || prev_voxel.update_num < 1) { 30 | continue; 31 | } 32 | if (voxel->sdf * prev_voxel.sdf < 0) { 33 | voxel->on_surface = true; 34 | } 35 | if (std::abs(voxel->sdf) < e) { 36 | voxel->on_surface = true; 37 | } 38 | } 39 | } 40 | } 41 | 42 | // y 43 | for (int z = 0; z < voxel_num.z(); z++) { 44 | for (int x = 0; x < voxel_num.x(); x++) { 45 | for (int y = 1; y < voxel_num.y(); y++) { 46 | const vacancy::Voxel& prev_voxel = voxel_grid->get(x, y - 1, z); 47 | vacancy::Voxel* voxel = voxel_grid->get_ptr(x, y, z); 48 | if (voxel->update_num < 1 || prev_voxel.update_num < 1) { 49 | continue; 50 | } 51 | if (voxel->sdf * prev_voxel.sdf < 0) { 52 | voxel->on_surface = true; 53 | } 54 | if (std::abs(voxel->sdf) < e) { 55 | voxel->on_surface = true; 56 | } 57 | } 58 | } 59 | } 60 | 61 | // z 62 | for (int y = 0; y < voxel_num.y(); y++) { 63 | for (int x = 0; x < voxel_num.x(); x++) { 64 | for (int z = 1; z < voxel_num.z(); z++) { 65 | const vacancy::Voxel& prev_voxel = voxel_grid->get(x, y, z - 1); 66 | vacancy::Voxel* voxel = voxel_grid->get_ptr(x, y, z); 67 | if (voxel->update_num < 1 || prev_voxel.update_num < 1) { 68 | continue; 69 | } 70 | if (voxel->sdf * prev_voxel.sdf < 0) { 71 | voxel->on_surface = true; 72 | } 73 | if (std::abs(voxel->sdf) < e) { 74 | voxel->on_surface = true; 75 | } 76 | } 77 | } 78 | } 79 | } 80 | 81 | void UpdateOnSurfaceWithPseudo(vacancy::VoxelGrid* voxel_grid) { 82 | // raycast like surface detection 83 | // search xyz axes bidirectionally to detect the voxel whose sign changes from 84 | // + to - 85 | // this function adds pseudo surfaces, which are good for visualization 86 | 87 | const Eigen::Vector3i& voxel_num = voxel_grid->voxel_num(); 88 | 89 | // x 90 | for (int z = 0; z < voxel_num.z(); z++) { 91 | for (int y = 0; y < voxel_num.y(); y++) { 92 | int min_index{-1}; 93 | int max_index{-1}; 94 | // min to max 95 | bool found_min{false}; 96 | { 97 | float min_sdf = std::numeric_limits::max(); 98 | for (int x = 0; x < voxel_num.x(); x++) { 99 | const vacancy::Voxel& voxel = voxel_grid->get(x, y, z); 100 | 101 | if (voxel.update_num < 1) { 102 | continue; 103 | } 104 | 105 | if (voxel.sdf < min_sdf) { 106 | min_sdf = voxel.sdf; 107 | min_index = x; 108 | } 109 | if (min_sdf < 0) { 110 | found_min = true; 111 | break; 112 | } 113 | } 114 | } 115 | 116 | // max to min 117 | bool found_max{false}; 118 | { 119 | float min_sdf = std::numeric_limits::max(); 120 | for (int x = voxel_num.x() - 1; 0 <= x; x--) { 121 | const vacancy::Voxel& voxel = voxel_grid->get(x, y, z); 122 | 123 | if (voxel.update_num < 1) { 124 | continue; 125 | } 126 | 127 | if (voxel.sdf < min_sdf) { 128 | min_sdf = voxel.sdf; 129 | max_index = x; 130 | } 131 | if (min_sdf < 0) { 132 | found_max = true; 133 | break; 134 | } 135 | } 136 | } 137 | 138 | if (found_min) { 139 | voxel_grid->get_ptr(min_index, y, z)->on_surface = true; 140 | } 141 | if (found_max) { 142 | voxel_grid->get_ptr(max_index, y, z)->on_surface = true; 143 | } 144 | } 145 | } 146 | 147 | // y 148 | for (int z = 0; z < voxel_num.z(); z++) { 149 | for (int x = 0; x < voxel_num.x(); x++) { 150 | int min_index{-1}; 151 | int max_index{-1}; 152 | // min to max 153 | bool found_min{false}; 154 | { 155 | float min_sdf = std::numeric_limits::max(); 156 | for (int y = 0; y < voxel_num.y(); y++) { 157 | const vacancy::Voxel& voxel = voxel_grid->get(x, y, z); 158 | if (voxel.update_num < 1) { 159 | continue; 160 | } 161 | if (voxel.sdf < min_sdf) { 162 | min_sdf = voxel.sdf; 163 | min_index = y; 164 | } 165 | if (min_sdf < 0) { 166 | found_min = true; 167 | break; 168 | } 169 | } 170 | } 171 | 172 | // max to min 173 | bool found_max{false}; 174 | { 175 | float min_sdf = std::numeric_limits::max(); 176 | for (int y = voxel_num.y() - 1; 0 <= y; y--) { 177 | const vacancy::Voxel& voxel = voxel_grid->get(x, y, z); 178 | if (voxel.update_num < 1) { 179 | continue; 180 | } 181 | if (voxel.sdf < min_sdf) { 182 | min_sdf = voxel.sdf; 183 | max_index = y; 184 | } 185 | if (min_sdf < 0) { 186 | found_max = true; 187 | break; 188 | } 189 | } 190 | } 191 | 192 | if (found_min) { 193 | voxel_grid->get_ptr(x, min_index, z)->on_surface = true; 194 | } 195 | if (found_max) { 196 | voxel_grid->get_ptr(x, max_index, z)->on_surface = true; 197 | } 198 | } 199 | } 200 | 201 | // z 202 | for (int y = 0; y < voxel_num.y(); y++) { 203 | for (int x = 0; x < voxel_num.x(); x++) { 204 | int min_index{-1}; 205 | int max_index{-1}; 206 | // min to max 207 | bool found_min{false}; 208 | { 209 | float min_sdf = std::numeric_limits::max(); 210 | for (int z = 0; z < voxel_num.z(); z++) { 211 | const vacancy::Voxel& voxel = voxel_grid->get(x, y, z); 212 | if (voxel.update_num < 1) { 213 | continue; 214 | } 215 | if (voxel.sdf < min_sdf) { 216 | min_sdf = voxel.sdf; 217 | min_index = z; 218 | } 219 | if (min_sdf < 0) { 220 | found_min = true; 221 | break; 222 | } 223 | } 224 | } 225 | 226 | // max to min 227 | bool found_max{false}; 228 | { 229 | float min_sdf = std::numeric_limits::max(); 230 | for (int z = voxel_num.z() - 1; 0 <= z; z--) { 231 | const vacancy::Voxel& voxel = voxel_grid->get(x, y, z); 232 | if (voxel.update_num < 1) { 233 | continue; 234 | } 235 | if (voxel.sdf < min_sdf) { 236 | min_sdf = voxel.sdf; 237 | max_index = z; 238 | } 239 | if (min_sdf < 0) { 240 | found_max = true; 241 | break; 242 | } 243 | } 244 | } 245 | 246 | if (found_min) { 247 | voxel_grid->get_ptr(x, y, min_index)->on_surface = true; 248 | } 249 | if (found_max) { 250 | voxel_grid->get_ptr(x, y, max_index)->on_surface = true; 251 | } 252 | } 253 | } 254 | } 255 | } // namespace 256 | 257 | namespace vacancy { 258 | void ExtractVoxel(VoxelGrid* voxel_grid, Mesh* mesh, bool inside_empty) { 259 | const Eigen::Vector3i& voxel_num = voxel_grid->voxel_num(); 260 | 261 | mesh->Clear(); 262 | 263 | std::vector vertices; 264 | std::vector vertex_indices; 265 | 266 | std::shared_ptr cube = MakeCube(voxel_grid->resolution()); 267 | 268 | // update on_surface flag of voxels 269 | if (inside_empty) { 270 | voxel_grid->ResetOnSurface(); 271 | 272 | UpdateOnSurface(voxel_grid); 273 | } 274 | 275 | for (int z = 0; z < voxel_num.z(); z++) { 276 | for (int y = 0; y < voxel_num.y(); y++) { 277 | for (int x = 0; x < voxel_num.x(); x++) { 278 | const Voxel& voxel = voxel_grid->get(x, y, z); 279 | 280 | if (inside_empty) { 281 | if (!voxel.on_surface) { 282 | // if inside_empty is specified, skip non-surface voxels 283 | continue; 284 | } 285 | } else if (voxel.sdf > 0 || voxel.update_num < 1) { 286 | // otherwise, naively skip outside and non-updated voxels 287 | continue; 288 | } 289 | 290 | cube->Translate(voxel.pos); 291 | 292 | int vertex_index_offset = static_cast(vertices.size()); 293 | 294 | std::copy(cube->vertices().begin(), cube->vertices().end(), 295 | std::back_inserter(vertices)); 296 | 297 | std::vector fixed_vertex_indices = 298 | cube->vertex_indices(); 299 | 300 | std::for_each(fixed_vertex_indices.begin(), fixed_vertex_indices.end(), 301 | [&](Eigen::Vector3i& face) { 302 | face[0] += vertex_index_offset; 303 | face[1] += vertex_index_offset; 304 | face[2] += vertex_index_offset; 305 | }); 306 | 307 | std::copy(fixed_vertex_indices.begin(), fixed_vertex_indices.end(), 308 | std::back_inserter(vertex_indices)); 309 | 310 | cube->Translate(-voxel.pos); 311 | } 312 | } 313 | } 314 | 315 | mesh->set_vertices(vertices); 316 | mesh->set_vertex_indices(vertex_indices); 317 | } 318 | 319 | } // namespace vacancy 320 | -------------------------------------------------------------------------------- /src/vacancy/extract_voxel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #pragma once 7 | 8 | #include "vacancy/voxel_carver.h" 9 | 10 | namespace vacancy { 11 | 12 | void ExtractVoxel(VoxelGrid* voxel_grid, Mesh* mesh, bool inside_empty); 13 | 14 | } // namespace vacancy 15 | -------------------------------------------------------------------------------- /src/vacancy/image.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #include "vacancy/image.h" 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #ifdef VACANCY_USE_STB 14 | #ifdef _WIN32 15 | #pragma warning(push) 16 | #pragma warning(disable : 4100) 17 | #endif 18 | #include "stb/stb_image.h" 19 | #ifdef _WIN32 20 | #pragma warning(pop) 21 | #endif 22 | 23 | #ifdef _WIN32 24 | #pragma warning(push) 25 | #pragma warning(disable : 4996) 26 | #endif 27 | #include "stb/stb_image_write.h" 28 | #ifdef _WIN32 29 | #pragma warning(pop) 30 | #endif 31 | #endif 32 | 33 | namespace vacancy { 34 | 35 | void Depth2Gray(const Image1f& depth, Image1b* vis_depth, float min_d, 36 | float max_d) { 37 | assert(min_d < max_d); 38 | assert(vis_depth != nullptr); 39 | 40 | vis_depth->Init(depth.width(), depth.height()); 41 | 42 | float inv_denom = 1.0f / (max_d - min_d); 43 | for (int y = 0; y < vis_depth->height(); y++) { 44 | for (int x = 0; x < vis_depth->width(); x++) { 45 | auto d = depth.at(x, y, 0); 46 | 47 | float norm_color = (d - min_d) * inv_denom; 48 | norm_color = std::min(std::max(norm_color, 0.0f), 1.0f); 49 | 50 | vis_depth->at(x, y, 0) = static_cast(norm_color * 255); 51 | } 52 | } 53 | } 54 | 55 | void Normal2Color(const Image3f& normal, Image3b* vis_normal) { 56 | assert(vis_normal != nullptr); 57 | 58 | vis_normal->Init(normal.width(), normal.height()); 59 | 60 | // Followed https://en.wikipedia.org/wiki/Normal_mapping 61 | // X: -1 to +1 : Red: 0 to 255 62 | // Y: -1 to +1 : Green: 0 to 255 63 | // Z: 0 to -1 : Blue: 128 to 255 64 | for (int y = 0; y < vis_normal->height(); y++) { 65 | for (int x = 0; x < vis_normal->width(); x++) { 66 | vis_normal->at(x, y, 0) = static_cast( 67 | std::round((normal.at(x, y, 0) + 1.0) * 0.5 * 255)); 68 | vis_normal->at(x, y, 1) = static_cast( 69 | std::round((normal.at(x, y, 1) + 1.0) * 0.5 * 255)); 70 | vis_normal->at(x, y, 2) = 71 | static_cast(std::round(-normal.at(x, y, 2) * 127.0) + 128); 72 | } 73 | } 74 | } 75 | 76 | void FaceId2RandomColor(const Image1i& face_id, Image3b* vis_face_id) { 77 | assert(vis_face_id != nullptr); 78 | 79 | vis_face_id->Init(face_id.width(), face_id.height(), 0); 80 | 81 | std::unordered_map> id2color; 82 | 83 | for (int y = 0; y < vis_face_id->height(); y++) { 84 | for (int x = 0; x < vis_face_id->width(); x++) { 85 | int fid = face_id.at(x, y, 0); 86 | if (fid < 0) { 87 | continue; 88 | } 89 | 90 | std::array color; 91 | auto iter = id2color.find(fid); 92 | if (iter != id2color.end()) { 93 | color = iter->second; 94 | } else { 95 | std::mt19937 mt(fid); 96 | // stl distribution depends on environment while mt19937 is independent. 97 | // so simply mod mt19937 value for random color reproducing the same 98 | // color in different environment. 99 | color[0] = static_cast(mt() % 256); 100 | color[1] = static_cast(mt() % 256); 101 | color[2] = static_cast(mt() % 256); 102 | id2color[fid] = color; 103 | } 104 | 105 | vis_face_id->at(x, y, 0) = color[0]; 106 | vis_face_id->at(x, y, 1) = color[1]; 107 | vis_face_id->at(x, y, 2) = color[2]; 108 | } 109 | } 110 | } 111 | 112 | } // namespace vacancy 113 | -------------------------------------------------------------------------------- /src/vacancy/log.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #include "vacancy/log.h" 7 | 8 | #include 9 | #include 10 | 11 | // multi line macro with do {...} while (0) guard 12 | #define PRINT_MACRO \ 13 | do { \ 14 | va_list va; \ 15 | va_start(va, format); \ 16 | vprintf(format, va); \ 17 | va_end(va); \ 18 | } while (0) 19 | 20 | namespace { 21 | vacancy::LogLevel g_log_level_ = vacancy::LogLevel::kVerbose; 22 | } 23 | 24 | namespace vacancy { 25 | 26 | void set_log_level(LogLevel level) { g_log_level_ = level; } 27 | 28 | LogLevel get_log_level() { return g_log_level_; } 29 | 30 | void LOGD(const char *format, ...) { 31 | if (LogLevel::kDebug >= g_log_level_) { 32 | PRINT_MACRO; 33 | } 34 | } 35 | 36 | void LOGI(const char *format, ...) { 37 | if (LogLevel::kInfo >= g_log_level_) { 38 | PRINT_MACRO; 39 | } 40 | } 41 | 42 | void LOGW(const char *format, ...) { 43 | if (LogLevel::kWarning >= g_log_level_) { 44 | PRINT_MACRO; 45 | } 46 | } 47 | void LOGE(const char *format, ...) { 48 | if (LogLevel::kError >= g_log_level_) { 49 | PRINT_MACRO; 50 | } 51 | } 52 | 53 | } // namespace vacancy 54 | -------------------------------------------------------------------------------- /src/vacancy/marching_cubes.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | // original code is from 7 | // http://paulbourke.net/geometry/polygonise/ 8 | 9 | #include "vacancy/marching_cubes.h" 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "vacancy/marching_cubes_lut.h" 17 | #include "vacancy/timer.h" 18 | 19 | namespace { 20 | 21 | /* 22 | Linearly interpolate the position where an isosurface cuts 23 | an edge between two vertices, each with their own scalar value 24 | */ 25 | void VertexInterp(double isolevel, const Eigen::Vector3f &p1, 26 | const Eigen::Vector3f &p2, double valp1, double valp2, 27 | Eigen::Vector3f *p) { 28 | if (std::abs(isolevel - valp1) < 0.00001) { 29 | *p = p1; 30 | return; 31 | } 32 | if (std::abs(isolevel - valp2) < 0.00001) { 33 | *p = p2; 34 | return; 35 | } 36 | if (std::abs(valp1 - valp2) < 0.00001) { 37 | *p = p1; 38 | return; 39 | } 40 | double mu = (isolevel - valp1) / (valp2 - valp1); 41 | p->x() = static_cast(p1.x() + mu * (static_cast(p2.x()) - 42 | static_cast(p1.x()))); 43 | p->y() = static_cast(p1.y() + mu * (static_cast(p2.y()) - 44 | static_cast(p1.y()))); 45 | p->z() = static_cast(p1.z() + mu * (static_cast(p2.z()) - 46 | static_cast(p1.z()))); 47 | } 48 | 49 | void VertexInterp(double iso_level, const vacancy::Voxel &v1, 50 | const vacancy::Voxel &v2, Eigen::Vector3f *p, 51 | bool linear_interp) { 52 | if (linear_interp) { 53 | VertexInterp(iso_level, v1.pos, v2.pos, v1.sdf, v2.sdf, p); 54 | } else { 55 | *p = v1.pos; 56 | } 57 | } 58 | 59 | } // namespace 60 | 61 | namespace vacancy { 62 | 63 | void MarchingCubes(const VoxelGrid &voxel_grid, Mesh *mesh, double iso_level, 64 | bool linear_interp) { 65 | Timer<> timer; 66 | timer.Start(); 67 | 68 | mesh->Clear(); 69 | 70 | const Eigen::Vector3i &voxel_num = voxel_grid.voxel_num(); 71 | 72 | std::vector vertices; 73 | std::vector vertex_indices; 74 | 75 | // key: sorted a pair of unique voxel ids in the voxel grid 76 | // value: the corresponding interpolated vertex id in the unified mesh 77 | // todo: consider faster way... maybe unordered_map? 78 | std::map, int> voxelids2vertexid; 79 | 80 | const std::array &edge_table = 81 | vacancy::marching_cubes_lut::kEdgeTable; 82 | const std::array, 256> &tri_table = 83 | vacancy::marching_cubes_lut::kTriTable; 84 | 85 | for (int z = 1; z < voxel_num.z(); z++) { 86 | for (int y = 1; y < voxel_num.y(); y++) { 87 | for (int x = 1; x < voxel_num.x(); x++) { 88 | if (voxel_grid.get(x, y, z).update_num < 1) { 89 | continue; 90 | } 91 | 92 | std::array voxels; 93 | voxels[0] = &voxel_grid.get(x - 1, y - 1, z - 1); 94 | voxels[1] = &voxel_grid.get(x, y - 1, z - 1); 95 | voxels[2] = &voxel_grid.get(x, y, z - 1); 96 | voxels[3] = &voxel_grid.get(x - 1, y, z - 1); 97 | 98 | voxels[4] = &voxel_grid.get(x - 1, y - 1, z); 99 | voxels[5] = &voxel_grid.get(x, y - 1, z); 100 | voxels[6] = &voxel_grid.get(x, y, z); 101 | voxels[7] = &voxel_grid.get(x - 1, y, z); 102 | 103 | if (voxels[0]->sdf == InvalidSdf::kVal || 104 | voxels[1]->sdf == InvalidSdf::kVal || 105 | voxels[2]->sdf == InvalidSdf::kVal || 106 | voxels[3]->sdf == InvalidSdf::kVal || 107 | voxels[4]->sdf == InvalidSdf::kVal || 108 | voxels[5]->sdf == InvalidSdf::kVal || 109 | voxels[6]->sdf == InvalidSdf::kVal || 110 | voxels[7]->sdf == InvalidSdf::kVal) { 111 | continue; 112 | } 113 | 114 | int cube_index{0}; 115 | std::array vert_list; 116 | std::array, 12> voxelids_list; 117 | /* 118 | Determine the index into the edge table which 119 | tells us which vertices are inside of the surface 120 | */ 121 | if (voxels[0]->sdf < iso_level) cube_index |= 1; 122 | if (voxels[1]->sdf < iso_level) cube_index |= 2; 123 | if (voxels[2]->sdf < iso_level) cube_index |= 4; 124 | if (voxels[3]->sdf < iso_level) cube_index |= 8; 125 | if (voxels[4]->sdf < iso_level) cube_index |= 16; 126 | if (voxels[5]->sdf < iso_level) cube_index |= 32; 127 | if (voxels[6]->sdf < iso_level) cube_index |= 64; 128 | if (voxels[7]->sdf < iso_level) cube_index |= 128; 129 | 130 | /* Cube is entirely in/out of the surface */ 131 | if (edge_table[cube_index] == 0) { 132 | continue; 133 | } 134 | 135 | /* Find the vertices where the surface intersects the cube 136 | * And save a pair of voxel ids when the vertices occur 137 | */ 138 | if (edge_table[cube_index] & 1) { 139 | VertexInterp(iso_level, *voxels[0], *voxels[1], &vert_list[0], 140 | linear_interp); 141 | voxelids_list[0] = std::make_pair(voxels[0]->id, voxels[1]->id); 142 | } 143 | if (edge_table[cube_index] & 2) { 144 | VertexInterp(iso_level, *voxels[1], *voxels[2], &vert_list[1], 145 | linear_interp); 146 | voxelids_list[1] = std::make_pair(voxels[1]->id, voxels[2]->id); 147 | } 148 | if (edge_table[cube_index] & 4) { 149 | VertexInterp(iso_level, *voxels[2], *voxels[3], &vert_list[2], 150 | linear_interp); 151 | voxelids_list[2] = std::make_pair(voxels[3]->id, voxels[2]->id); 152 | } 153 | if (edge_table[cube_index] & 8) { 154 | VertexInterp(iso_level, *voxels[3], *voxels[0], &vert_list[3], 155 | linear_interp); 156 | voxelids_list[3] = std::make_pair(voxels[0]->id, voxels[3]->id); 157 | } 158 | if (edge_table[cube_index] & 16) { 159 | VertexInterp(iso_level, *voxels[4], *voxels[5], &vert_list[4], 160 | linear_interp); 161 | voxelids_list[4] = std::make_pair(voxels[4]->id, voxels[5]->id); 162 | } 163 | if (edge_table[cube_index] & 32) { 164 | VertexInterp(iso_level, *voxels[5], *voxels[6], &vert_list[5], 165 | linear_interp); 166 | voxelids_list[5] = std::make_pair(voxels[5]->id, voxels[6]->id); 167 | } 168 | if (edge_table[cube_index] & 64) { 169 | VertexInterp(iso_level, *voxels[6], *voxels[7], &vert_list[6], 170 | linear_interp); 171 | voxelids_list[6] = std::make_pair(voxels[7]->id, voxels[6]->id); 172 | } 173 | if (edge_table[cube_index] & 128) { 174 | VertexInterp(iso_level, *voxels[7], *voxels[4], &vert_list[7], 175 | linear_interp); 176 | voxelids_list[7] = std::make_pair(voxels[4]->id, voxels[7]->id); 177 | } 178 | if (edge_table[cube_index] & 256) { 179 | VertexInterp(iso_level, *voxels[0], *voxels[4], &vert_list[8], 180 | linear_interp); 181 | voxelids_list[8] = std::make_pair(voxels[0]->id, voxels[4]->id); 182 | } 183 | if (edge_table[cube_index] & 512) { 184 | VertexInterp(iso_level, *voxels[1], *voxels[5], &vert_list[9], 185 | linear_interp); 186 | voxelids_list[9] = std::make_pair(voxels[1]->id, voxels[5]->id); 187 | } 188 | if (edge_table[cube_index] & 1024) { 189 | VertexInterp(iso_level, *voxels[2], *voxels[6], &vert_list[10], 190 | linear_interp); 191 | voxelids_list[10] = std::make_pair(voxels[2]->id, voxels[6]->id); 192 | } 193 | if (edge_table[cube_index] & 2048) { 194 | VertexInterp(iso_level, *voxels[3], *voxels[7], &vert_list[11], 195 | linear_interp); 196 | voxelids_list[11] = std::make_pair(voxels[3]->id, voxels[7]->id); 197 | } 198 | 199 | for (int i = 0; tri_table[cube_index][i] != -1; i += 3) { 200 | Eigen::Vector3i face; 201 | 202 | for (int j = 0; j < 3; j++) { 203 | const std::pair &key = 204 | voxelids_list[tri_table[cube_index][i + (2 - j)]]; 205 | if (voxelids2vertexid.find(key) == voxelids2vertexid.end()) { 206 | // if a pair of voxel ids has not been added, the current vertex 207 | // is new. store the pair of voxel ids and new vertex position 208 | face[j] = static_cast(vertices.size()); 209 | vertices.push_back(vert_list[tri_table[cube_index][i + (2 - j)]]); 210 | voxelids2vertexid.insert(std::make_pair(key, face[j])); 211 | } else { 212 | // set existing vertex id if the pair of voxel ids has been 213 | // already added 214 | face[j] = voxelids2vertexid.at(key); 215 | } 216 | } 217 | vertex_indices.push_back(face); 218 | } 219 | } 220 | } 221 | } 222 | 223 | mesh->set_vertices(vertices); 224 | mesh->set_vertex_indices(vertex_indices); 225 | 226 | timer.End(); 227 | LOGI("MarchingCubes %02f\n", timer.elapsed_msec()); 228 | } 229 | 230 | } // namespace vacancy 231 | -------------------------------------------------------------------------------- /src/vacancy/marching_cubes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | // original code is from 7 | // http://paulbourke.net/geometry/polygonise/ 8 | 9 | #pragma once 10 | 11 | #include "vacancy/voxel_carver.h" 12 | 13 | namespace vacancy { 14 | 15 | void MarchingCubes(const VoxelGrid& voxel_grid, Mesh* mesh, 16 | double iso_level = 0.0, bool linear_interp = true); 17 | 18 | } // namespace vacancy 19 | -------------------------------------------------------------------------------- /src/vacancy/marching_cubes_lut.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | // original code is from 7 | // http://paulbourke.net/geometry/polygonise/ 8 | 9 | #include "vacancy/marching_cubes_lut.h" 10 | 11 | #include 12 | 13 | namespace vacancy { 14 | namespace marching_cubes_lut { 15 | const std::array kEdgeTable = { 16 | {0x0, 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c, 0x80c, 0x905, 17 | 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00, 0x190, 0x99, 0x393, 0x29a, 18 | 0x596, 0x49f, 0x795, 0x69c, 0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 19 | 0xf99, 0xe90, 0x230, 0x339, 0x33, 0x13a, 0x636, 0x73f, 0x435, 0x53c, 20 | 0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30, 0x3a0, 0x2a9, 21 | 0x1a3, 0xaa, 0x7a6, 0x6af, 0x5a5, 0x4ac, 0xbac, 0xaa5, 0x9af, 0x8a6, 22 | 0xfaa, 0xea3, 0xda9, 0xca0, 0x460, 0x569, 0x663, 0x76a, 0x66, 0x16f, 23 | 0x265, 0x36c, 0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60, 24 | 0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0xff, 0x3f5, 0x2fc, 0xdfc, 0xcf5, 25 | 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0, 0x650, 0x759, 0x453, 0x55a, 26 | 0x256, 0x35f, 0x55, 0x15c, 0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 27 | 0x859, 0x950, 0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0xcc, 28 | 0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0, 0x8c0, 0x9c9, 29 | 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc, 0xcc, 0x1c5, 0x2cf, 0x3c6, 30 | 0x4ca, 0x5c3, 0x6c9, 0x7c0, 0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 31 | 0xf55, 0xe5c, 0x15c, 0x55, 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650, 32 | 0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc, 0x2fc, 0x3f5, 33 | 0xff, 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0, 0xb60, 0xa69, 0x963, 0x86a, 34 | 0xf66, 0xe6f, 0xd65, 0xc6c, 0x36c, 0x265, 0x16f, 0x66, 0x76a, 0x663, 35 | 0x569, 0x460, 0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac, 36 | 0x4ac, 0x5a5, 0x6af, 0x7a6, 0xaa, 0x1a3, 0x2a9, 0x3a0, 0xd30, 0xc39, 37 | 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c, 0x53c, 0x435, 0x73f, 0x636, 38 | 0x13a, 0x33, 0x339, 0x230, 0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 39 | 0x895, 0x99c, 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x99, 0x190, 40 | 0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c, 0x70c, 0x605, 41 | 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x0}}; 42 | const std::array, 256> kTriTable = { 43 | {{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 44 | {0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 45 | {0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 46 | {1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 47 | {1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 48 | {0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 49 | {9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 50 | {2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1}, 51 | {3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 52 | {0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 53 | {1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 54 | {1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1}, 55 | {3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 56 | {0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1}, 57 | {3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1}, 58 | {9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 59 | {4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 60 | {4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 61 | {0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 62 | {4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1}, 63 | {1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 64 | {3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1}, 65 | {9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1}, 66 | {2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1}, 67 | {8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 68 | {11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1}, 69 | {9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1}, 70 | {4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1}, 71 | {3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1}, 72 | {1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1}, 73 | {4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1}, 74 | {4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1}, 75 | {9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 76 | {9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 77 | {0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 78 | {8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1}, 79 | {1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 80 | {3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1}, 81 | {5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1}, 82 | {2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1}, 83 | {9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 84 | {0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1}, 85 | {0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1}, 86 | {2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1}, 87 | {10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1}, 88 | {4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1}, 89 | {5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1}, 90 | {5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1}, 91 | {9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 92 | {9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1}, 93 | {0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1}, 94 | {1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 95 | {9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1}, 96 | {10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1}, 97 | {8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1}, 98 | {2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1}, 99 | {7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1}, 100 | {9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1}, 101 | {2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1}, 102 | {11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1}, 103 | {9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1}, 104 | {5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1}, 105 | {11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1}, 106 | {11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 107 | {10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 108 | {0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 109 | {9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 110 | {1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1}, 111 | {1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 112 | {1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1}, 113 | {9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1}, 114 | {5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1}, 115 | {2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 116 | {11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1}, 117 | {0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1}, 118 | {5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1}, 119 | {6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1}, 120 | {0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1}, 121 | {3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1}, 122 | {6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1}, 123 | {5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 124 | {4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1}, 125 | {1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1}, 126 | {10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1}, 127 | {6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1}, 128 | {1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1}, 129 | {8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1}, 130 | {7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1}, 131 | {3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1}, 132 | {5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1}, 133 | {0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1}, 134 | {9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1}, 135 | {8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1}, 136 | {5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1}, 137 | {0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1}, 138 | {6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1}, 139 | {10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 140 | {4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1}, 141 | {10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1}, 142 | {8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1}, 143 | {1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1}, 144 | {3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1}, 145 | {0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 146 | {8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1}, 147 | {10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1}, 148 | {0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1}, 149 | {3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1}, 150 | {6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1}, 151 | {9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1}, 152 | {8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1}, 153 | {3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1}, 154 | {6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 155 | {7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1}, 156 | {0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1}, 157 | {10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1}, 158 | {10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1}, 159 | {1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1}, 160 | {2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1}, 161 | {7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1}, 162 | {7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 163 | {2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1}, 164 | {2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1}, 165 | {1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1}, 166 | {11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1}, 167 | {8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1}, 168 | {0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 169 | {7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1}, 170 | {7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 171 | {7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 172 | {3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 173 | {0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 174 | {8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1}, 175 | {10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 176 | {1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1}, 177 | {2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1}, 178 | {6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1}, 179 | {7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 180 | {7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1}, 181 | {2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1}, 182 | {1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1}, 183 | {10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1}, 184 | {10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1}, 185 | {0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1}, 186 | {7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1}, 187 | {6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 188 | {3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1}, 189 | {8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1}, 190 | {9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1}, 191 | {6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1}, 192 | {1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1}, 193 | {4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1}, 194 | {10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1}, 195 | {8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1}, 196 | {0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 197 | {1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1}, 198 | {1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1}, 199 | {8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1}, 200 | {10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1}, 201 | {4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1}, 202 | {10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 203 | {4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 204 | {0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1}, 205 | {5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1}, 206 | {11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1}, 207 | {9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1}, 208 | {6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1}, 209 | {7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1}, 210 | {3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1}, 211 | {7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1}, 212 | {9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1}, 213 | {3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1}, 214 | {6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1}, 215 | {9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1}, 216 | {1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1}, 217 | {4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1}, 218 | {7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1}, 219 | {6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1}, 220 | {3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1}, 221 | {0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1}, 222 | {6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1}, 223 | {1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1}, 224 | {0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1}, 225 | {11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1}, 226 | {6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1}, 227 | {5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1}, 228 | {9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1}, 229 | {1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1}, 230 | {1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 231 | {1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1}, 232 | {10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1}, 233 | {0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 234 | {10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 235 | {11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 236 | {11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1}, 237 | {5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1}, 238 | {10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1}, 239 | {11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1}, 240 | {0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1}, 241 | {9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1}, 242 | {7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1}, 243 | {2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1}, 244 | {8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1}, 245 | {9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1}, 246 | {9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1}, 247 | {1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 248 | {0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1}, 249 | {9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1}, 250 | {9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 251 | {5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1}, 252 | {5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1}, 253 | {0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1}, 254 | {10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1}, 255 | {2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1}, 256 | {0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1}, 257 | {0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1}, 258 | {9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 259 | {2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1}, 260 | {5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1}, 261 | {3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1}, 262 | {5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1}, 263 | {8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1}, 264 | {0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 265 | {8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1}, 266 | {9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 267 | {4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1}, 268 | {0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1}, 269 | {1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1}, 270 | {3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1}, 271 | {4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1}, 272 | {9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1}, 273 | {11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1}, 274 | {11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1}, 275 | {2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1}, 276 | {9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1}, 277 | {3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1}, 278 | {1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 279 | {4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1}, 280 | {4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1}, 281 | {4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 282 | {4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 283 | {9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 284 | {3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1}, 285 | {0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1}, 286 | {3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 287 | {1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1}, 288 | {3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1}, 289 | {0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 290 | {3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 291 | {2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1}, 292 | {9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 293 | {2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1}, 294 | {1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 295 | {1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 296 | {0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 297 | {0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}, 298 | {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}}}; 299 | } // namespace marching_cubes_lut 300 | } // namespace vacancy 301 | -------------------------------------------------------------------------------- /src/vacancy/marching_cubes_lut.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | // original code is from 7 | // http://paulbourke.net/geometry/polygonise/ 8 | 9 | #pragma once 10 | 11 | #include 12 | 13 | namespace vacancy { 14 | namespace marching_cubes_lut { 15 | extern const std::array kEdgeTable; 16 | extern const std::array, 256> kTriTable; 17 | } // namespace marching_cubes_lut 18 | } // namespace vacancy 19 | -------------------------------------------------------------------------------- /src/vacancy/mesh.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #include "vacancy/mesh.h" 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #ifdef VACANCY_USE_TINYOBJLOADER 13 | #include "tinyobjloader/tiny_obj_loader.h" 14 | #endif 15 | 16 | namespace { 17 | template 18 | void CopyVec(const std::vector& src, std::vector* dst) { 19 | dst->clear(); 20 | std::copy(src.begin(), src.end(), std::back_inserter(*dst)); 21 | } 22 | 23 | std::vector Split(const std::string& s, char delim) { 24 | std::vector elems; 25 | std::stringstream ss(s); 26 | std::string item; 27 | while (std::getline(ss, item, delim)) { 28 | if (!item.empty()) { 29 | elems.push_back(item); 30 | } 31 | } 32 | return elems; 33 | } 34 | } // namespace 35 | 36 | namespace vacancy { 37 | 38 | Mesh::Mesh() {} 39 | Mesh::Mesh(const Mesh& src) { 40 | CopyVec(src.vertices_, &vertices_); 41 | CopyVec(src.vertex_colors_, &vertex_colors_); 42 | CopyVec(src.vertex_indices_, &vertex_indices_); 43 | 44 | CopyVec(src.normals_, &normals_); 45 | CopyVec(src.face_normals_, &face_normals_); 46 | CopyVec(src.normal_indices_, &normal_indices_); 47 | 48 | CopyVec(src.uv_, &uv_); 49 | CopyVec(src.uv_indices_, &uv_indices_); 50 | 51 | diffuse_texname_ = src.diffuse_texname_; 52 | diffuse_texpath_ = src.diffuse_texpath_; 53 | src.diffuse_tex_.CopyTo(&diffuse_tex_); 54 | stats_ = src.stats_; 55 | } 56 | Mesh::~Mesh() {} 57 | 58 | const std::vector& Mesh::vertices() const { return vertices_; } 59 | const std::vector& Mesh::vertex_colors() const { 60 | return vertex_colors_; 61 | } 62 | const std::vector& Mesh::vertex_indices() const { 63 | return vertex_indices_; 64 | } 65 | 66 | const std::vector& Mesh::normals() const { return normals_; } 67 | const std::vector& Mesh::face_normals() const { 68 | return face_normals_; 69 | } 70 | const std::vector& Mesh::normal_indices() const { 71 | return normal_indices_; 72 | } 73 | 74 | const std::vector& Mesh::uv() const { return uv_; } 75 | const std::vector& Mesh::uv_indices() const { 76 | return uv_indices_; 77 | } 78 | 79 | const MeshStats& Mesh::stats() const { return stats_; } 80 | 81 | const Image3b& Mesh::diffuse_tex() const { return diffuse_tex_; } 82 | 83 | void Mesh::CalcStats() { 84 | stats_.bb_min = Eigen::Vector3f(std::numeric_limits::max(), 85 | std::numeric_limits::max(), 86 | std::numeric_limits::max()); 87 | stats_.bb_max = Eigen::Vector3f(std::numeric_limits::lowest(), 88 | std::numeric_limits::lowest(), 89 | std::numeric_limits::lowest()); 90 | 91 | if (vertex_indices_.empty()) { 92 | return; 93 | } 94 | 95 | double sum[3] = {0.0, 0.0, 0.0}; // use double to avoid overflow 96 | for (const auto& v : vertices_) { 97 | for (int i = 0; i < 3; i++) { 98 | sum[i] += v[i]; 99 | 100 | if (v[i] < stats_.bb_min[i]) { 101 | stats_.bb_min[i] = v[i]; 102 | } 103 | 104 | if (stats_.bb_max[i] < v[i]) { 105 | stats_.bb_max[i] = v[i]; 106 | } 107 | } 108 | } 109 | 110 | for (int i = 0; i < 3; i++) { 111 | stats_.center[i] = static_cast(sum[i] / vertices_.size()); 112 | } 113 | } 114 | 115 | void Mesh::RemoveDuplicatedVertices() { 116 | std::vector vertex_id_table(vertices_.size(), -1); 117 | std::vector non_duplicated_vertices; 118 | non_duplicated_vertices.reserve(vertices_.size()); 119 | for (int i = 0; i < static_cast(vertices_.size() - 1); i++) { 120 | if (vertex_id_table[i] > 0) { 121 | continue; 122 | } 123 | const auto& v1 = vertices_[i]; 124 | vertex_id_table[i] = static_cast(non_duplicated_vertices.size()); 125 | for (int j = i + 1; j < static_cast(vertices_.size()); j++) { 126 | if (vertex_id_table[j] > 0) { 127 | continue; 128 | } 129 | const auto& v2 = vertices_[j]; 130 | if (std::abs(v1.x() - v2.x()) < std::numeric_limits::min() && 131 | std::abs(v1.y() - v2.y()) < std::numeric_limits::min() && 132 | std::abs(v1.z() - v2.z()) < std::numeric_limits::min()) { 133 | vertex_id_table[j] = vertex_id_table[i]; 134 | } 135 | } 136 | non_duplicated_vertices.push_back(v1); 137 | } 138 | 139 | set_vertices(non_duplicated_vertices); 140 | 141 | for (size_t i = 0; i < vertex_indices_.size(); i++) { 142 | for (int j = 0; j < 3; j++) { 143 | vertex_indices_[i][j] = vertex_id_table[vertex_indices_[i][j]]; 144 | } 145 | } 146 | } 147 | 148 | void Mesh::Rotate(const Eigen::Matrix3f& R) { 149 | for (auto& v : vertices_) { 150 | v = R * v; 151 | } 152 | for (auto& n : normals_) { 153 | n = R * n; 154 | } 155 | for (auto& fn : face_normals_) { 156 | fn = R * fn; 157 | } 158 | CalcStats(); 159 | } 160 | 161 | void Mesh::Translate(const Eigen::Vector3f& t) { 162 | for (auto& v : vertices_) { 163 | v = v + t; 164 | } 165 | CalcStats(); 166 | } 167 | 168 | void Mesh::Transform(const Eigen::Matrix3f& R, const Eigen::Vector3f& t) { 169 | Rotate(R); 170 | Translate(t); 171 | } 172 | 173 | void Mesh::Scale(float scale) { Scale(scale, scale, scale); } 174 | 175 | void Mesh::Scale(float x_scale, float y_scale, float z_scale) { 176 | for (auto& v : vertices_) { 177 | v[0] = v[0] * x_scale; 178 | v[1] = v[1] * y_scale; 179 | v[2] = v[2] * z_scale; 180 | } 181 | } 182 | 183 | void Mesh::Clear() { 184 | vertices_.clear(); 185 | vertex_colors_.clear(); 186 | vertex_indices_.clear(); // face 187 | 188 | normals_.clear(); 189 | normal_indices_.clear(); 190 | 191 | uv_.clear(); 192 | uv_indices_.clear(); 193 | 194 | diffuse_tex_.Clear(); 195 | } 196 | 197 | void Mesh::CalcNormal() { 198 | CalcFaceNormal(); 199 | 200 | normals_.clear(); 201 | normal_indices_.clear(); 202 | 203 | std::copy(vertex_indices_.begin(), vertex_indices_.end(), 204 | std::back_inserter(normal_indices_)); 205 | 206 | Eigen::Vector3f zero{0.0f, 0.0f, 0.0f}; 207 | normals_.resize(vertices_.size(), zero); 208 | 209 | std::vector add_count(vertices_.size(), 0); 210 | 211 | for (size_t i = 0; i < vertex_indices_.size(); i++) { 212 | const auto& face = vertex_indices_[i]; 213 | for (int j = 0; j < 3; j++) { 214 | int idx = face[j]; 215 | normals_[idx] += face_normals_[i]; 216 | add_count[idx]++; 217 | } 218 | } 219 | 220 | // get average normal 221 | // caution: this does not work for cube 222 | // https://answers.unity.com/questions/441722/splitting-up-verticies.html 223 | for (size_t i = 0; i < vertices_.size(); i++) { 224 | normals_[i] /= static_cast(add_count[i]); 225 | normals_[i].normalize(); 226 | } 227 | } 228 | 229 | void Mesh::CalcFaceNormal() { 230 | face_normals_.clear(); 231 | face_normals_.resize(vertex_indices_.size()); 232 | 233 | for (size_t i = 0; i < vertex_indices_.size(); i++) { 234 | const auto& f = vertex_indices_[i]; 235 | Eigen::Vector3f v1 = (vertices_[f[1]] - vertices_[f[0]]).normalized(); 236 | Eigen::Vector3f v2 = (vertices_[f[2]] - vertices_[f[0]]).normalized(); 237 | face_normals_[i] = v1.cross(v2).normalized(); 238 | } 239 | } 240 | 241 | bool Mesh::set_vertices(const std::vector& vertices) { 242 | if (vertices.size() > std::numeric_limits::max()) { 243 | LOGE("The number of vertices exceeds the maximum: %d\n", 244 | std::numeric_limits::max()); 245 | return false; 246 | } 247 | CopyVec(vertices, &vertices_); 248 | return true; 249 | } 250 | 251 | bool Mesh::set_vertex_colors( 252 | const std::vector& vertex_colors) { 253 | if (vertex_colors.size() > std::numeric_limits::max()) { 254 | LOGE("The number of vertices exceeds the maximum: %d\n", 255 | std::numeric_limits::max()); 256 | return false; 257 | } 258 | CopyVec(vertex_colors, &vertex_colors_); 259 | return true; 260 | } 261 | 262 | bool Mesh::set_vertex_indices( 263 | const std::vector& vertex_indices) { 264 | if (vertex_indices.size() > std::numeric_limits::max()) { 265 | LOGE("The number of faces exceeds the maximum: %d\n", 266 | std::numeric_limits::max()); 267 | return false; 268 | } 269 | CopyVec(vertex_indices, &vertex_indices_); 270 | return true; 271 | } 272 | 273 | bool Mesh::set_normals(const std::vector& normals) { 274 | if (normals.size() > std::numeric_limits::max()) { 275 | LOGE("The number of vertices exceeds the maximum: %d\n", 276 | std::numeric_limits::max()); 277 | return false; 278 | } 279 | CopyVec(normals, &normals_); 280 | return true; 281 | } 282 | 283 | bool Mesh::set_face_normals(const std::vector& face_normals) { 284 | if (face_normals.size() > std::numeric_limits::max()) { 285 | LOGE("The number of faces exceeds the maximum: %d\n", 286 | std::numeric_limits::max()); 287 | return false; 288 | } 289 | CopyVec(face_normals, &face_normals_); 290 | return true; 291 | } 292 | 293 | bool Mesh::set_normal_indices( 294 | const std::vector& normal_indices) { 295 | if (normal_indices.size() > std::numeric_limits::max()) { 296 | LOGE("The number of faces exceeds the maximum: %d\n", 297 | std::numeric_limits::max()); 298 | return false; 299 | } 300 | CopyVec(normal_indices, &normal_indices_); 301 | return true; 302 | } 303 | 304 | bool Mesh::set_uv(const std::vector& uv) { 305 | if (uv.size() > std::numeric_limits::max()) { 306 | LOGE("The number of vertices exceeds the maximum: %d\n", 307 | std::numeric_limits::max()); 308 | return false; 309 | } 310 | CopyVec(uv, &uv_); 311 | return true; 312 | } 313 | 314 | bool Mesh::set_uv_indices(const std::vector& uv_indices) { 315 | if (uv_indices.size() > std::numeric_limits::max()) { 316 | LOGE("The number of faces exceeds the maximum: %d\n", 317 | std::numeric_limits::max()); 318 | return false; 319 | } 320 | CopyVec(uv_indices, &uv_indices_); 321 | return true; 322 | } 323 | 324 | bool Mesh::set_diffuse_tex(const Image3b& diffuse_tex) { 325 | diffuse_tex.CopyTo(&diffuse_tex_); 326 | return true; 327 | } 328 | 329 | #ifdef VACANCY_USE_TINYOBJLOADER 330 | bool Mesh::LoadObj(const std::string& obj_path, const std::string& mtl_dir) { 331 | Clear(); 332 | 333 | std::vector shapes; 334 | std::vector materials; 335 | tinyobj::attrib_t attrib; 336 | std::string err_str, warn_str; 337 | bool ret = tinyobj::LoadObj(&attrib, &shapes, &materials, &warn_str, &err_str, 338 | obj_path.c_str(), mtl_dir.c_str()); 339 | 340 | if (!err_str.empty()) { // `err` may contain warning message. 341 | LOGE("%s\n", err_str.c_str()); 342 | } 343 | 344 | if (!ret) { 345 | return false; 346 | } 347 | 348 | if (materials.size() != 1) { 349 | LOGE("Doesn't support obj materials num %d. Must be 1\n", 350 | static_cast(materials.size())); 351 | return false; 352 | } 353 | 354 | size_t face_num = 0; 355 | for (size_t s = 0; s < shapes.size(); s++) { 356 | face_num += shapes[s].mesh.num_face_vertices.size(); 357 | } 358 | 359 | if (face_num > std::numeric_limits::max()) { 360 | LOGE("The number of faces exceeds the maximum: %d\n", 361 | std::numeric_limits::max()); 362 | return false; 363 | } 364 | 365 | vertex_indices_.resize(face_num); // face 366 | uv_indices_.resize(face_num); 367 | normal_indices_.resize(face_num); 368 | 369 | if (attrib.vertices.size() / 3 > std::numeric_limits::max() || 370 | attrib.normals.size() / 3 > std::numeric_limits::max() || 371 | attrib.texcoords.size() / 2 > std::numeric_limits::max() || 372 | attrib.colors.size() / 3 > std::numeric_limits::max()) { 373 | LOGE("The number of vertices exceeds the maximum: %d\n", 374 | std::numeric_limits::max()); 375 | return false; 376 | } 377 | 378 | vertices_.resize(attrib.vertices.size() / 3); 379 | normals_.resize(attrib.normals.size() / 3); 380 | uv_.resize(attrib.texcoords.size() / 2); 381 | vertex_colors_.resize(attrib.colors.size() / 3); 382 | 383 | size_t face_offset = 0; 384 | // Loop over shapes 385 | for (size_t s = 0; s < shapes.size(); s++) { 386 | // Loop over faces(polygon) 387 | size_t index_offset = 0; 388 | 389 | for (size_t f = 0; f < shapes[s].mesh.num_face_vertices.size(); f++) { 390 | int fv = shapes[s].mesh.num_face_vertices[f]; 391 | 392 | if (fv != 3) { 393 | LOGE("Doesn't support face num %d. Must be 3\n", fv); 394 | return false; 395 | } 396 | 397 | // Loop over vertices in the face. 398 | for (int v = 0; v < fv; v++) { 399 | // access to vertex 400 | tinyobj::index_t idx = shapes[s].mesh.indices[index_offset + v]; 401 | tinyobj::real_t vx = attrib.vertices[3 * idx.vertex_index + 0]; 402 | tinyobj::real_t vy = attrib.vertices[3 * idx.vertex_index + 1]; 403 | tinyobj::real_t vz = attrib.vertices[3 * idx.vertex_index + 2]; 404 | 405 | vertex_indices_[face_offset][v] = idx.vertex_index; 406 | 407 | vertices_[idx.vertex_index][0] = vx; 408 | vertices_[idx.vertex_index][1] = vy; 409 | vertices_[idx.vertex_index][2] = vz; 410 | 411 | if (!attrib.normals.empty()) { 412 | tinyobj::real_t nx = attrib.normals[3 * idx.normal_index + 0]; 413 | tinyobj::real_t ny = attrib.normals[3 * idx.normal_index + 1]; 414 | tinyobj::real_t nz = attrib.normals[3 * idx.normal_index + 2]; 415 | 416 | normal_indices_[face_offset][v] = idx.normal_index; 417 | normals_[idx.normal_index][0] = nx; 418 | normals_[idx.normal_index][1] = ny; 419 | normals_[idx.normal_index][2] = nz; 420 | } 421 | 422 | if (!attrib.texcoords.empty()) { 423 | tinyobj::real_t tx = attrib.texcoords[2 * idx.texcoord_index + 0]; 424 | tinyobj::real_t ty = attrib.texcoords[2 * idx.texcoord_index + 1]; 425 | 426 | uv_indices_[face_offset][v] = idx.texcoord_index; 427 | uv_[idx.texcoord_index][0] = tx; 428 | uv_[idx.texcoord_index][1] = ty; 429 | } 430 | // Optional: vertex colors 431 | if (!attrib.colors.empty()) { 432 | tinyobj::real_t red = attrib.colors[3 * idx.vertex_index + 0]; 433 | tinyobj::real_t green = attrib.colors[3 * idx.vertex_index + 1]; 434 | tinyobj::real_t blue = attrib.colors[3 * idx.vertex_index + 2]; 435 | 436 | vertex_colors_[idx.vertex_index][0] = red; 437 | vertex_colors_[idx.vertex_index][1] = green; 438 | vertex_colors_[idx.vertex_index][2] = blue; 439 | } 440 | } 441 | index_offset += fv; 442 | face_offset++; 443 | 444 | // per-face material 445 | shapes[s].mesh.material_ids[f]; 446 | } 447 | } 448 | 449 | CalcFaceNormal(); 450 | 451 | if (normals_.empty()) { 452 | CalcNormal(); 453 | } 454 | 455 | CalcStats(); 456 | 457 | diffuse_texname_ = materials[0].diffuse_texname; 458 | diffuse_texpath_ = mtl_dir + diffuse_texname_; 459 | 460 | std::ifstream ifs(diffuse_texpath_); 461 | if (ifs.is_open()) { 462 | #ifdef VACANCY_USE_STB 463 | ret = diffuse_tex_.Load(diffuse_texpath_); 464 | #else 465 | LOGW("define VACANCY_USE_STB to load diffuse texture.\n"); 466 | #endif 467 | } else { 468 | LOGW("diffuse texture doesn't exist %s\n", diffuse_texpath_.c_str()); 469 | } 470 | 471 | return ret; 472 | } 473 | #endif 474 | 475 | bool Mesh::LoadPly(const std::string& ply_path) { 476 | std::ifstream ifs(ply_path); 477 | std::string str; 478 | if (ifs.fail()) { 479 | LOGE("couldn't open ply: %s\n", ply_path.c_str()); 480 | return false; 481 | } 482 | 483 | getline(ifs, str); 484 | if (str != "ply") { 485 | LOGE("ply first line is wrong: %s\n", str.c_str()); 486 | return false; 487 | } 488 | getline(ifs, str); 489 | if (str.find("ascii") == std::string::npos) { 490 | LOGE("only ascii ply is supported: %s\n", str.c_str()); 491 | return false; 492 | } 493 | 494 | bool ret = false; 495 | std::int64_t vertex_num = 0; 496 | while (getline(ifs, str)) { 497 | if (str.find("element vertex") != std::string::npos) { 498 | std::vector splitted = Split(str, ' '); 499 | if (splitted.size() == 3) { 500 | vertex_num = std::atol(splitted[2].c_str()); 501 | ret = true; 502 | break; 503 | } 504 | } 505 | } 506 | if (!ret) { 507 | LOGE("couldn't find element vertex\n"); 508 | return false; 509 | } 510 | if (vertex_num > std::numeric_limits::max()) { 511 | LOGE("The number of vertices exceeds the maximum: %d\n", 512 | std::numeric_limits::max()); 513 | return false; 514 | } 515 | 516 | ret = false; 517 | std::int64_t face_num = 0; 518 | while (getline(ifs, str)) { 519 | if (str.find("element face") != std::string::npos) { 520 | std::vector splitted = Split(str, ' '); 521 | if (splitted.size() == 3) { 522 | face_num = std::atol(splitted[2].c_str()); 523 | ret = true; 524 | break; 525 | } 526 | } 527 | } 528 | if (!ret) { 529 | LOGE("couldn't find element face\n"); 530 | return false; 531 | } 532 | if (face_num > std::numeric_limits::max()) { 533 | LOGE("The number of faces exceeds the maximum: %d\n", 534 | std::numeric_limits::max()); 535 | return false; 536 | } 537 | 538 | while (getline(ifs, str)) { 539 | if (str.find("end_header") != std::string::npos) { 540 | break; 541 | } 542 | } 543 | 544 | vertices_.resize(vertex_num); 545 | int vertex_count = 0; 546 | while (getline(ifs, str)) { 547 | std::vector splitted = Split(str, ' '); 548 | vertices_[vertex_count][0] = 549 | static_cast(std::atof(splitted[0].c_str())); 550 | vertices_[vertex_count][1] = 551 | static_cast(std::atof(splitted[1].c_str())); 552 | vertices_[vertex_count][2] = 553 | static_cast(std::atof(splitted[2].c_str())); 554 | vertex_count++; 555 | if (vertex_count >= vertex_num) { 556 | break; 557 | } 558 | } 559 | 560 | vertex_indices_.resize(face_num); 561 | int face_count = 0; 562 | while (getline(ifs, str)) { 563 | std::vector splitted = Split(str, ' '); 564 | vertex_indices_[face_count][0] = std::atoi(splitted[1].c_str()); 565 | vertex_indices_[face_count][1] = std::atoi(splitted[2].c_str()); 566 | vertex_indices_[face_count][2] = std::atoi(splitted[3].c_str()); 567 | 568 | face_count++; 569 | if (face_count >= face_num) { 570 | break; 571 | } 572 | } 573 | 574 | ifs.close(); 575 | 576 | CalcNormal(); 577 | 578 | CalcStats(); 579 | 580 | return true; 581 | } 582 | 583 | bool Mesh::WritePly(const std::string& ply_path) const { 584 | std::ofstream ofs(ply_path); 585 | std::string str; 586 | if (ofs.fail()) { 587 | LOGE("couldn't open ply: %s\n", ply_path.c_str()); 588 | return false; 589 | } 590 | 591 | bool has_vertex_color = !vertex_colors_.empty(); 592 | if (has_vertex_color) { 593 | assert(vertices_.size() == vertex_colors_.size()); 594 | } 595 | 596 | ofs << "ply" << std::endl; 597 | ofs << "format ascii 1.0" << std::endl; 598 | ofs << "element vertex " + std::to_string(vertices_.size()) << std::endl; 599 | ofs << "property float x\n" 600 | "property float y\n" 601 | "property float z\n"; 602 | if (has_vertex_color) { 603 | ofs << "property uchar red\n" 604 | "property uchar green\n" 605 | "property uchar blue\n" 606 | "property uchar alpha\n"; 607 | } 608 | ofs << "element face " + std::to_string(vertex_indices_.size()) << std::endl; 609 | ofs << "property list uchar int vertex_indices" << std::endl; 610 | ofs << "end_header" << std::endl; 611 | 612 | for (size_t i = 0; i < vertices_.size(); i++) { 613 | ofs << vertices_[i][0] << " " << vertices_[i][1] << " " << vertices_[i][2] 614 | << " "; 615 | if (has_vertex_color) { 616 | ofs << static_cast(std::round(vertex_colors_[i][0])) << " " 617 | << static_cast(std::round(vertex_colors_[i][1])) << " " 618 | << static_cast(std::round(vertex_colors_[i][2])) << " 255 "; 619 | } 620 | ofs << std::endl; 621 | } 622 | 623 | for (size_t i = 0; i < vertex_indices_.size(); i++) { 624 | ofs << "3 " << vertex_indices_[i][0] << " " << vertex_indices_[i][1] << " " 625 | << vertex_indices_[i][2] << " " << std::endl; 626 | } 627 | 628 | ofs.close(); 629 | 630 | return true; 631 | } 632 | 633 | #ifdef VACANCY_USE_STB 634 | bool Mesh::WriteObj(const std::string& obj_dir, const std::string& obj_basename, 635 | const std::string& mtl_basename, 636 | const std::string& tex_basename) const { 637 | std::string mtl_name = mtl_basename + ".mtl"; 638 | if (mtl_basename.empty()) { 639 | mtl_name = obj_basename + ".mtl"; 640 | } 641 | std::string mtl_path = obj_dir + "/" + mtl_name; 642 | 643 | std::string tex_name = tex_basename + ".png"; 644 | if (tex_basename.empty()) { 645 | tex_name = obj_basename + ".png"; 646 | } 647 | std::string tex_path = obj_dir + "/" + tex_name; 648 | 649 | std::string obj_path = obj_dir + "/" + obj_basename + ".obj"; 650 | 651 | // write obj 652 | { 653 | std::ofstream ofs(obj_path); 654 | if (ofs.fail()) { 655 | LOGE("couldn't open obj path: %s\n", obj_path.c_str()); 656 | return false; 657 | } 658 | 659 | ofs << "mtllib ./" << mtl_name << std::endl << std::endl; 660 | 661 | // vertices 662 | for (const auto& v : vertices_) { 663 | ofs << "v " << v.x() << " " << v.y() << " " << v.z() << " 1.0" 664 | << std::endl; 665 | } 666 | 667 | // uv 668 | for (const auto& vt : uv_) { 669 | ofs << "vt " << vt.x() << " " << vt.y() << " 0" << std::endl; 670 | } 671 | 672 | // vertex normals 673 | for (const auto& vn : normals_) { 674 | ofs << "vn " << vn.x() << " " << vn.y() << " " << vn.z() << std::endl; 675 | } 676 | 677 | // indices 678 | bool write_uv_indices = !uv_indices_.empty(); 679 | bool write_normal_indices = !normal_indices_.empty(); 680 | for (size_t i = 0; i < vertex_indices_.size(); i++) { 681 | ofs << "f"; 682 | for (int j = 0; j < 3; j++) { 683 | ofs << " " << std::to_string(vertex_indices_[i][j] + 1); 684 | if (!write_uv_indices && !write_normal_indices) { 685 | continue; 686 | } 687 | ofs << "/"; 688 | if (write_uv_indices) { 689 | ofs << std::to_string(uv_indices_[i][j] + 1); 690 | } 691 | ofs << "/" << std::to_string(normal_indices_[i][j] + 1); 692 | } 693 | ofs << std::endl; 694 | } 695 | 696 | ofs.close(); 697 | } 698 | 699 | // write mtl 700 | { 701 | std::ofstream ofs(mtl_path); 702 | if (ofs.fail()) { 703 | LOGE("couldn't open mtl path: %s\n", mtl_path.c_str()); 704 | return false; 705 | } 706 | 707 | ofs << "property float x\n" 708 | "property float y\n" 709 | "property float z\n" 710 | "newmtl Textured\n" 711 | "Ka 1.000 1.000 1.000\n" 712 | "Kd 1.000 1.000 1.000\n" 713 | "Ks 0.000 0.000 0.000\n" 714 | "d 1.0\n" 715 | "illum 2\n"; 716 | ofs << "map_Kd " + tex_name << std::endl; 717 | 718 | ofs.close(); 719 | } 720 | 721 | // write texture 722 | diffuse_tex_.WritePng(tex_path); 723 | 724 | return true; 725 | } 726 | #endif 727 | 728 | std::shared_ptr MakeCube(const Eigen::Vector3f& length, 729 | const Eigen::Matrix3f& R, 730 | const Eigen::Vector3f& t) { 731 | std::shared_ptr cube(new Mesh); 732 | std::vector vertices(24); 733 | std::vector vertex_indices(12); 734 | std::vector vertex_colors(24); 735 | 736 | const float h_x = length.x() / 2; 737 | const float h_y = length.y() / 2; 738 | const float h_z = length.z() / 2; 739 | 740 | vertices[0] = Eigen::Vector3f(-h_x, h_y, -h_z); 741 | vertices[1] = Eigen::Vector3f(h_x, h_y, -h_z); 742 | vertices[2] = Eigen::Vector3f(h_x, h_y, h_z); 743 | vertices[3] = Eigen::Vector3f(-h_x, h_y, h_z); 744 | vertex_indices[0] = Eigen::Vector3i(0, 2, 1); 745 | vertex_indices[1] = Eigen::Vector3i(0, 3, 2); 746 | 747 | vertices[4] = Eigen::Vector3f(-h_x, -h_y, -h_z); 748 | vertices[5] = Eigen::Vector3f(h_x, -h_y, -h_z); 749 | vertices[6] = Eigen::Vector3f(h_x, -h_y, h_z); 750 | vertices[7] = Eigen::Vector3f(-h_x, -h_y, h_z); 751 | vertex_indices[2] = Eigen::Vector3i(4, 5, 6); 752 | vertex_indices[3] = Eigen::Vector3i(4, 6, 7); 753 | 754 | vertices[8] = vertices[1]; 755 | vertices[9] = vertices[2]; 756 | vertices[10] = vertices[6]; 757 | vertices[11] = vertices[5]; 758 | vertex_indices[4] = Eigen::Vector3i(8, 9, 10); 759 | vertex_indices[5] = Eigen::Vector3i(8, 10, 11); 760 | 761 | vertices[12] = vertices[0]; 762 | vertices[13] = vertices[3]; 763 | vertices[14] = vertices[7]; 764 | vertices[15] = vertices[4]; 765 | vertex_indices[6] = Eigen::Vector3i(12, 14, 13); 766 | vertex_indices[7] = Eigen::Vector3i(12, 15, 14); 767 | 768 | vertices[16] = vertices[0]; 769 | vertices[17] = vertices[1]; 770 | vertices[18] = vertices[5]; 771 | vertices[19] = vertices[4]; 772 | vertex_indices[8] = Eigen::Vector3i(16, 17, 18); 773 | vertex_indices[9] = Eigen::Vector3i(16, 18, 19); 774 | 775 | vertices[20] = vertices[3]; 776 | vertices[21] = vertices[2]; 777 | vertices[22] = vertices[6]; 778 | vertices[23] = vertices[7]; 779 | vertex_indices[10] = Eigen::Vector3i(20, 22, 21); 780 | vertex_indices[11] = Eigen::Vector3i(20, 23, 22); 781 | 782 | // set default color 783 | for (int i = 0; i < 24; i++) { 784 | vertex_colors[i][0] = (-vertices[i][0] + h_x) / length.x() * 255; 785 | vertex_colors[i][1] = (-vertices[i][1] + h_y) / length.y() * 255; 786 | vertex_colors[i][2] = (-vertices[i][2] + h_z) / length.z() * 255; 787 | } 788 | 789 | cube->set_vertices(vertices); 790 | cube->set_vertex_indices(vertex_indices); 791 | cube->set_vertex_colors(vertex_colors); 792 | 793 | cube->Transform(R, t); 794 | 795 | cube->CalcNormal(); 796 | 797 | return cube; 798 | } 799 | 800 | std::shared_ptr MakeCube(const Eigen::Vector3f& length) { 801 | const Eigen::Matrix3f R = Eigen::Matrix3f::Identity(); 802 | const Eigen::Vector3f t(0.0f, 0.0f, 0.0f); 803 | return MakeCube(length, R, t); 804 | } 805 | 806 | std::shared_ptr MakeCube(float length, const Eigen::Matrix3f& R, 807 | const Eigen::Vector3f& t) { 808 | Eigen::Vector3f length_xyz{length, length, length}; 809 | return MakeCube(length_xyz, R, t); 810 | } 811 | 812 | std::shared_ptr MakeCube(float length) { 813 | const Eigen::Matrix3f R = Eigen::Matrix3f::Identity(); 814 | const Eigen::Vector3f t(0.0f, 0.0f, 0.0f); 815 | return MakeCube(length, R, t); 816 | } 817 | 818 | void SetRandomVertexColor(std::shared_ptr mesh, int seed) { 819 | std::mt19937 mt(seed); 820 | std::uniform_int_distribution random_color(0, 255); 821 | 822 | std::vector vertex_colors(mesh->vertices().size()); 823 | for (auto& vc : vertex_colors) { 824 | vc[0] = static_cast(random_color(mt)); 825 | vc[1] = static_cast(random_color(mt)); 826 | vc[2] = static_cast(random_color(mt)); 827 | } 828 | 829 | mesh->set_vertex_colors(vertex_colors); 830 | } 831 | 832 | } // namespace vacancy 833 | -------------------------------------------------------------------------------- /src/vacancy/stb.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #ifdef VACANCY_USE_STB 7 | #ifdef _WIN32 8 | #pragma warning(push) 9 | #pragma warning(disable : 4100) 10 | #endif 11 | #define STB_IMAGE_IMPLEMENTATION 12 | #include "stb/stb_image.h" 13 | #ifdef _WIN32 14 | #pragma warning(pop) 15 | #endif 16 | 17 | #ifdef _WIN32 18 | #pragma warning(push) 19 | #pragma warning(disable : 4996) 20 | #endif 21 | #define STB_IMAGE_WRITE_IMPLEMENTATION 22 | #include "stb/stb_image_write.h" 23 | #ifdef _WIN32 24 | #pragma warning(pop) 25 | #endif 26 | #endif 27 | -------------------------------------------------------------------------------- /src/vacancy/timer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #pragma once 7 | #include //NOLINT 8 | #include 9 | #include 10 | 11 | namespace vacancy { 12 | 13 | template 14 | class Timer { 15 | std::chrono::system_clock::time_point start_t_, end_t_; 16 | T elapsed_msec_{-1}; 17 | size_t history_num_{30}; 18 | std::vector history_; 19 | 20 | public: 21 | Timer() {} 22 | ~Timer() {} 23 | explicit Timer(size_t history_num) : history_num_(history_num) {} 24 | 25 | std::chrono::system_clock::time_point start_t() const { return start_t_; } 26 | std::chrono::system_clock::time_point end_t() const { return end_t_; } 27 | 28 | void Start() { start_t_ = std::chrono::system_clock::now(); } 29 | void End() { 30 | end_t_ = std::chrono::system_clock::now(); 31 | elapsed_msec_ = static_cast( 32 | std::chrono::duration_cast(end_t_ - start_t_) 33 | .count() * 34 | 0.001); 35 | 36 | history_.push_back(elapsed_msec_); 37 | if (history_num_ < history_.size()) { 38 | history_.erase(history_.begin()); 39 | } 40 | } 41 | T elapsed_msec() const { return elapsed_msec_; } 42 | T average_msec() const { 43 | return static_cast(std::accumulate(history_.begin(), history_.end(), 0) / 44 | history_.size()); 45 | } 46 | }; 47 | 48 | } // namespace vacancy 49 | -------------------------------------------------------------------------------- /src/vacancy/voxel_carver.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2019, unclearness 3 | * All rights reserved. 4 | */ 5 | 6 | #include "vacancy/voxel_carver.h" 7 | 8 | #include 9 | 10 | #include "vacancy/extract_voxel.h" 11 | #include "vacancy/marching_cubes.h" 12 | #include "vacancy/timer.h" 13 | 14 | namespace { 15 | 16 | inline float SdfInterpolationNn(const Eigen::Vector2f& image_p, 17 | const vacancy::Image1f& sdf, 18 | const Eigen::Vector2i& roi_min, 19 | const Eigen::Vector2i& roi_max) { 20 | Eigen::Vector2i image_p_i(static_cast(std::round(image_p.x())), 21 | static_cast(std::round(image_p.y()))); 22 | 23 | // really need these? 24 | if (image_p_i.x() < roi_min.x()) { 25 | image_p_i.x() = roi_min.x(); 26 | } 27 | if (image_p_i.y() < roi_min.y()) { 28 | image_p_i.y() = roi_min.y(); 29 | } 30 | if (roi_max.x() < image_p_i.x()) { 31 | image_p_i.x() = roi_max.x(); 32 | } 33 | if (roi_max.y() < image_p_i.y()) { 34 | image_p_i.y() = roi_max.y(); 35 | } 36 | 37 | return sdf.at(image_p_i.x(), image_p_i.y(), 0); 38 | } 39 | 40 | inline float SdfInterpolationBiliner(const Eigen::Vector2f& image_p, 41 | const vacancy::Image1f& sdf, 42 | const Eigen::Vector2i& roi_min, 43 | const Eigen::Vector2i& roi_max) { 44 | std::array pos_min = {{0, 0}}; 45 | std::array pos_max = {{0, 0}}; 46 | pos_min[0] = static_cast(std::floor(image_p[0])); 47 | pos_min[1] = static_cast(std::floor(image_p[1])); 48 | pos_max[0] = pos_min[0] + 1; 49 | pos_max[1] = pos_min[1] + 1; 50 | 51 | // really need these? 52 | if (pos_min[0] < roi_min.x()) { 53 | pos_min[0] = roi_min.x(); 54 | } 55 | if (pos_min[1] < roi_min.y()) { 56 | pos_min[1] = roi_min.y(); 57 | } 58 | if (roi_max.x() < pos_max[0]) { 59 | pos_max[0] = roi_max.x(); 60 | } 61 | if (roi_max.y() < pos_max[1]) { 62 | pos_max[1] = roi_max.y(); 63 | } 64 | 65 | float local_u = image_p[0] - pos_min[0]; 66 | float local_v = image_p[1] - pos_min[1]; 67 | 68 | // bilinear interpolation of sdf 69 | float dist = 70 | (1.0f - local_u) * (1.0f - local_v) * sdf.at(pos_min[0], pos_min[1], 0) + 71 | local_u * (1.0f - local_v) * sdf.at(pos_max[0], pos_min[1], 0) + 72 | (1.0f - local_u) * local_v * sdf.at(pos_min[0], pos_max[1], 0) + 73 | local_u * local_v * sdf.at(pos_max[0], pos_max[1], 0); 74 | 75 | return dist; 76 | } 77 | 78 | inline void UpdateVoxelMax(vacancy::Voxel* voxel, 79 | const vacancy::VoxelUpdateOption& option, 80 | float sdf) { 81 | (void)option; 82 | if (sdf > voxel->sdf) { 83 | voxel->sdf = sdf; 84 | voxel->update_num++; 85 | } 86 | } 87 | 88 | inline void UpdateVoxelWeightedAverage(vacancy::Voxel* voxel, 89 | const vacancy::VoxelUpdateOption& option, 90 | float sdf) { 91 | const float& w = option.voxel_update_weight; 92 | const float inv_denom = 1.0f / (w * (voxel->update_num + 1)); 93 | voxel->sdf = (w * voxel->update_num * voxel->sdf + w * sdf) * inv_denom; 94 | voxel->update_num++; 95 | } 96 | } // namespace 97 | 98 | namespace vacancy { 99 | 100 | const float InvalidSdf::kVal = std::numeric_limits::lowest(); 101 | 102 | void DistanceTransformL1(const Image1b& mask, const Eigen::Vector2i& roi_min, 103 | const Eigen::Vector2i& roi_max, Image1f* dist) { 104 | dist->Init(mask.width(), mask.height(), 0.0f); 105 | 106 | // init inifinite inside mask 107 | for (int y = roi_min.y(); y <= roi_max.y(); y++) { 108 | for (int x = roi_min.x(); x <= roi_max.x(); x++) { 109 | if (mask.at(x, y, 0) != 255) { 110 | continue; 111 | } 112 | dist->at(x, y, 0) = std::numeric_limits::max(); 113 | } 114 | } 115 | 116 | // forward path 117 | for (int y = roi_min.y() + 1; y <= roi_max.y(); y++) { 118 | float up = dist->at(roi_min.x(), y - 1, 0); 119 | if (up < std::numeric_limits::max()) { 120 | dist->at(roi_min.x(), y, 0) = 121 | std::min(up + 1.0f, dist->at(roi_min.x(), y, 0)); 122 | } 123 | } 124 | for (int x = roi_min.x() + 1; x <= roi_max.x(); x++) { 125 | float left = dist->at(x - 1, roi_min.y(), 0); 126 | if (left < std::numeric_limits::max()) { 127 | dist->at(x, roi_min.y(), 0) = 128 | std::min(left + 1.0f, dist->at(x, roi_min.y(), 0)); 129 | } 130 | } 131 | for (int y = roi_min.y() + 1; y <= roi_max.y(); y++) { 132 | for (int x = roi_min.x() + 1; x <= roi_max.x(); x++) { 133 | float up = dist->at(x, y - 1, 0); 134 | float left = dist->at(x - 1, y, 0); 135 | float min_dist = std::min(up, left); 136 | if (min_dist < std::numeric_limits::max()) { 137 | dist->at(x, y, 0) = std::min(min_dist + 1.0f, dist->at(x, y, 0)); 138 | } 139 | } 140 | } 141 | 142 | // backward path 143 | for (int y = roi_max.y() - 1; roi_min.y() <= y; y--) { 144 | float down = dist->at(roi_max.x(), y + 1, 0); 145 | if (down < std::numeric_limits::max()) { 146 | dist->at(roi_max.x(), y, 0) = 147 | std::min(down + 1.0f, dist->at(roi_max.x(), y, 0)); 148 | } 149 | } 150 | for (int x = roi_max.x() - 1; roi_min.x() <= x; x--) { 151 | float right = dist->at(x + 1, roi_max.y(), 0); 152 | if (right < std::numeric_limits::max()) { 153 | dist->at(x, roi_max.y(), 0) = 154 | std::min(right + 1.0f, dist->at(x, roi_max.y(), 0)); 155 | } 156 | } 157 | for (int y = roi_max.y() - 1; roi_min.y() <= y; y--) { 158 | for (int x = roi_max.x() - 1; roi_min.x() <= x; x--) { 159 | float down = dist->at(x, y + 1, 0); 160 | float right = dist->at(x + 1, y, 0); 161 | float min_dist = std::min(down, right); 162 | if (min_dist < std::numeric_limits::max()) { 163 | dist->at(x, y, 0) = std::min(min_dist + 1.0f, dist->at(x, y, 0)); 164 | } 165 | } 166 | } 167 | } 168 | 169 | void MakeSignedDistanceField(const Image1b& mask, 170 | const Eigen::Vector2i& roi_min, 171 | const Eigen::Vector2i& roi_max, Image1f* dist, 172 | bool minmax_normalize, bool use_truncation, 173 | float truncation_band) { 174 | Image1f* negative_dist = dist; 175 | DistanceTransformL1(mask, roi_min, roi_max, negative_dist); 176 | for (int y = roi_min.y(); y <= roi_max.y(); y++) { 177 | for (int x = roi_min.x(); x <= roi_max.x(); x++) { 178 | if (negative_dist->at(x, y, 0) > 0) { 179 | negative_dist->at(x, y, 0) *= -1; 180 | } 181 | } 182 | } 183 | 184 | Image1b inv_mask(mask); 185 | for (int y = roi_min.y(); y <= roi_max.y(); y++) { 186 | for (int x = roi_min.x(); x <= roi_max.x(); x++) { 187 | if (inv_mask.at(x, y, 0) == 255) { 188 | inv_mask.at(x, y, 0) = 0; 189 | } else { 190 | inv_mask.at(x, y, 0) = 255; 191 | } 192 | } 193 | } 194 | 195 | Image1f positive_dist; 196 | DistanceTransformL1(inv_mask, roi_min, roi_max, &positive_dist); 197 | for (int y = roi_min.y(); y <= roi_max.y(); y++) { 198 | for (int x = roi_min.x(); x <= roi_max.x(); x++) { 199 | if (inv_mask.at(x, y, 0) == 255) { 200 | dist->at(x, y, 0) = positive_dist.at(x, y, 0); 201 | } 202 | } 203 | } 204 | 205 | if (minmax_normalize) { 206 | // Outside of roi is set to 0, so does not affect min/max 207 | float max_dist = 208 | *std::max_element(dist->data().begin(), dist->data().end()); 209 | float min_dist = 210 | *std::min_element(dist->data().begin(), dist->data().end()); 211 | float abs_max = std::max(std::abs(max_dist), std::abs(min_dist)); 212 | 213 | if (abs_max > std::numeric_limits::min()) { 214 | float norm_factor = 1.0f / abs_max; 215 | 216 | for (int y = roi_min.y(); y <= roi_max.y(); y++) { 217 | for (int x = roi_min.x(); x <= roi_max.x(); x++) { 218 | dist->at(x, y, 0) *= norm_factor; 219 | } 220 | } 221 | } 222 | } 223 | 224 | // truncation process same to KinectFusion 225 | if (use_truncation) { 226 | for (int y = roi_min.y(); y <= roi_max.y(); y++) { 227 | for (int x = roi_min.x(); x <= roi_max.x(); x++) { 228 | float& d = dist->at(x, y, 0); 229 | if (-truncation_band >= d) { 230 | d = InvalidSdf::kVal; 231 | } else { 232 | d = std::min(1.0f, d / truncation_band); 233 | } 234 | } 235 | } 236 | } 237 | } 238 | 239 | void SignedDistance2Color(const Image1f& sdf, Image3b* vis_sdf, 240 | float min_negative_d, float max_positive_d) { 241 | assert(min_negative_d < 0); 242 | assert(0 < max_positive_d); 243 | assert(vis_sdf != nullptr); 244 | 245 | vis_sdf->Init(sdf.width(), sdf.height()); 246 | 247 | for (int y = 0; y < vis_sdf->height(); y++) { 248 | for (int x = 0; x < vis_sdf->width(); x++) { 249 | auto d = sdf.at(x, y, 0); 250 | 251 | if (d > 0) { 252 | float norm_inv_dist = (max_positive_d - d) / max_positive_d; 253 | norm_inv_dist = std::min(std::max(norm_inv_dist, 0.0f), 1.0f); 254 | vis_sdf->at(x, y, 0) = static_cast(255); 255 | vis_sdf->at(x, y, 1) = static_cast(255 * norm_inv_dist); 256 | vis_sdf->at(x, y, 2) = static_cast(255 * norm_inv_dist); 257 | 258 | } else { 259 | float norm_inv_dist = (d - min_negative_d) / (-min_negative_d); 260 | norm_inv_dist = std::min(std::max(norm_inv_dist, 0.0f), 1.0f); 261 | vis_sdf->at(x, y, 0) = static_cast(255 * norm_inv_dist); 262 | vis_sdf->at(x, y, 1) = static_cast(255 * norm_inv_dist); 263 | vis_sdf->at(x, y, 2) = static_cast(255); 264 | } 265 | } 266 | } 267 | } 268 | 269 | Voxel::Voxel() {} 270 | Voxel::~Voxel() {} 271 | 272 | VoxelGrid::VoxelGrid() {} 273 | 274 | VoxelGrid::~VoxelGrid() {} 275 | 276 | bool VoxelGrid::Init(const Eigen::Vector3f& bb_max, 277 | const Eigen::Vector3f& bb_min, float resolution) { 278 | if (resolution < std::numeric_limits::min()) { 279 | LOGE("resolution must be positive %f\n", resolution); 280 | return false; 281 | } 282 | if (bb_max.x() <= bb_min.x() || bb_max.y() <= bb_min.y() || 283 | bb_max.z() <= bb_min.z()) { 284 | LOGE("input bounding box is invalid\n"); 285 | return false; 286 | } 287 | 288 | bb_max_ = bb_max; 289 | bb_min_ = bb_min; 290 | resolution_ = resolution; 291 | 292 | Eigen::Vector3f diff = bb_max_ - bb_min_; 293 | 294 | for (int i = 0; i < 3; i++) { 295 | voxel_num_[i] = static_cast(diff[i] / resolution_); 296 | } 297 | 298 | if (voxel_num_.x() * voxel_num_.y() * voxel_num_.z() > 299 | std::numeric_limits::max()) { 300 | LOGE("too many voxels\n"); 301 | return false; 302 | } 303 | 304 | xy_slice_num_ = voxel_num_[0] * voxel_num_[1]; 305 | 306 | voxels_.clear(); 307 | voxels_.resize(voxel_num_.x() * voxel_num_.y() * voxel_num_.z()); 308 | 309 | float offset = resolution_ * 0.5f; 310 | 311 | #if defined(_OPENMP) && defined(VACANCY_USE_OPENMP) 312 | #pragma omp parallel for schedule(dynamic, 1) 313 | #endif 314 | for (int z = 0; z < voxel_num_.z(); z++) { 315 | float z_pos = diff.z() * (static_cast(z) / 316 | static_cast(voxel_num_.z())) + 317 | bb_min_.z() + offset; 318 | 319 | for (int y = 0; y < voxel_num_.y(); y++) { 320 | float y_pos = diff.y() * (static_cast(y) / 321 | static_cast(voxel_num_.y())) + 322 | bb_min_.y() + offset; 323 | for (int x = 0; x < voxel_num_.x(); x++) { 324 | float x_pos = diff.x() * (static_cast(x) / 325 | static_cast(voxel_num_.x())) + 326 | bb_min_.x() + offset; 327 | 328 | Voxel* voxel = get_ptr(x, y, z); 329 | voxel->index.x() = x; 330 | voxel->index.y() = y; 331 | voxel->index.z() = z; 332 | 333 | voxel->id = z * xy_slice_num_ + (y * voxel_num_.x() + x); 334 | 335 | voxel->pos.x() = x_pos; 336 | voxel->pos.y() = y_pos; 337 | voxel->pos.z() = z_pos; 338 | 339 | voxel->sdf = InvalidSdf::kVal; 340 | } 341 | } 342 | } 343 | 344 | return true; 345 | } 346 | 347 | const Eigen::Vector3i& VoxelGrid::voxel_num() const { return voxel_num_; } 348 | 349 | const Voxel& VoxelGrid::get(int x, int y, int z) const { 350 | return voxels_[z * xy_slice_num_ + (y * voxel_num_.x() + x)]; 351 | } 352 | 353 | Voxel* VoxelGrid::get_ptr(int x, int y, int z) { 354 | return &voxels_[z * xy_slice_num_ + (y * voxel_num_.x() + x)]; 355 | } 356 | 357 | float VoxelGrid::resolution() const { return resolution_; } 358 | 359 | void VoxelGrid::ResetOnSurface() { 360 | for (Voxel& v : voxels_) { 361 | v.on_surface = false; 362 | } 363 | } 364 | 365 | bool VoxelGrid::initialized() const { return !voxels_.empty(); } 366 | 367 | VoxelCarver::VoxelCarver() {} 368 | 369 | VoxelCarver::~VoxelCarver() {} 370 | 371 | VoxelCarver::VoxelCarver(VoxelCarverOption option) { set_option(option); } 372 | 373 | void VoxelCarver::set_option(VoxelCarverOption option) { option_ = option; } 374 | 375 | bool VoxelCarver::Init() { 376 | if (option_.update_option.voxel_max_update_num < 1) { 377 | LOGE("voxel_max_update_num must be positive"); 378 | return false; 379 | } 380 | if (option_.update_option.voxel_update_weight < 381 | std::numeric_limits::min()) { 382 | LOGE("voxel_update_weight must be positive"); 383 | return false; 384 | } 385 | if (option_.update_option.truncation_band < 386 | std::numeric_limits::min()) { 387 | LOGE("truncation_band must be positive"); 388 | return false; 389 | } 390 | voxel_grid_ = std::make_unique(); 391 | return voxel_grid_->Init(option_.bb_max, option_.bb_min, option_.resolution); 392 | } 393 | 394 | bool VoxelCarver::Carve(const Camera& camera, const Image1b& silhouette, 395 | const Eigen::Vector2i& roi_min, 396 | const Eigen::Vector2i& roi_max, Image1f* sdf) { 397 | if (!voxel_grid_->initialized()) { 398 | LOGE("VoxelCarver::Carve voxel grid has not been initialized\n"); 399 | return false; 400 | } 401 | 402 | Timer<> timer; 403 | timer.Start(); 404 | // make signed distance field 405 | MakeSignedDistanceField(silhouette, roi_min, roi_max, sdf, 406 | option_.sdf_minmax_normalize, 407 | option_.update_option.use_truncation, 408 | option_.update_option.truncation_band); 409 | timer.End(); 410 | LOGI("VoxelCarver::Carve make SDF %02f\n", timer.elapsed_msec()); 411 | 412 | return Carve(camera, roi_min, roi_max, *sdf); 413 | } 414 | 415 | bool VoxelCarver::Carve(const Camera& camera, const Eigen::Vector2i& roi_min, 416 | const Eigen::Vector2i& roi_max, const Image1f& sdf) { 417 | Timer<> timer; 418 | std::function 420 | interpolate_sdf; 421 | if (option_.update_option.sdf_interp == SdfInterpolation::kNn) { 422 | interpolate_sdf = SdfInterpolationNn; 423 | } else if (option_.update_option.sdf_interp == SdfInterpolation::kBilinear) { 424 | interpolate_sdf = SdfInterpolationBiliner; 425 | } 426 | 427 | std::function update_voxel; 428 | if (option_.update_option.voxel_update == VoxelUpdate::kMax) { 429 | update_voxel = UpdateVoxelMax; 430 | } else if (option_.update_option.voxel_update == 431 | VoxelUpdate::kWeightedAverage) { 432 | update_voxel = UpdateVoxelWeightedAverage; 433 | } 434 | 435 | timer.Start(); 436 | const float max_sdf = *std::max_element(sdf.data().begin(), sdf.data().end()); 437 | const Eigen::Vector3i& voxel_num = voxel_grid_->voxel_num(); 438 | const Eigen::Affine3f& w2c = camera.w2c().cast(); 439 | #if defined(_OPENMP) && defined(VACANCY_USE_OPENMP) 440 | #pragma omp parallel for schedule(dynamic, 1) 441 | #endif 442 | for (int z = 0; z < voxel_num.z(); z++) { 443 | for (int y = 0; y < voxel_num.y(); y++) { 444 | for (int x = 0; x < voxel_num.x(); x++) { 445 | Voxel* voxel = voxel_grid_->get_ptr(x, y, z); 446 | 447 | if (voxel->outside || 448 | voxel->update_num > option_.update_option.voxel_max_update_num) { 449 | continue; 450 | } 451 | 452 | Eigen::Vector2f image_p_f; 453 | Eigen::Vector3f voxel_pos_c = w2c * voxel->pos; 454 | 455 | // skip if the voxel is in the back of the camera 456 | if (voxel_pos_c.z() < 0) { 457 | continue; 458 | } 459 | 460 | camera.Project(voxel_pos_c, &image_p_f); 461 | 462 | float dist = InvalidSdf::kVal; 463 | 464 | if (image_p_f.x() < roi_min.x() || image_p_f.y() < roi_min.y() || 465 | roi_max.x() < image_p_f.x() || roi_max.y() < image_p_f.y()) { 466 | if (option_.update_option.update_outside == 467 | UpdateOutsideImage::kNone) { 468 | continue; 469 | } else if (option_.update_option.update_outside == 470 | UpdateOutsideImage::kMax) { 471 | dist = max_sdf; 472 | } 473 | } else { 474 | dist = interpolate_sdf(image_p_f, sdf, roi_min, roi_max); 475 | } 476 | 477 | // skip if dist is truncated 478 | if (option_.update_option.use_truncation && dist < -1.0f) { 479 | continue; 480 | } 481 | 482 | if (voxel->update_num < 1) { 483 | voxel->sdf = dist; 484 | voxel->update_num++; 485 | continue; 486 | } 487 | 488 | update_voxel(voxel, option_.update_option, dist); 489 | } 490 | } 491 | } 492 | timer.End(); 493 | LOGI("VoxelCarver::Carve main loop %02f\n", timer.elapsed_msec()); 494 | 495 | return true; 496 | } 497 | 498 | bool VoxelCarver::Carve(const Camera& camera, const Image1b& silhouette) { 499 | Image1f sdf(camera.width(), camera.height()); 500 | return Carve(camera, silhouette, &sdf); 501 | } 502 | 503 | bool VoxelCarver::Carve(const Camera& camera, const Image1b& silhouette, 504 | Image1f* sdf) { 505 | Eigen::Vector2i roi_min{0, 0}; 506 | Eigen::Vector2i roi_max{silhouette.width() - 1, silhouette.height() - 1}; 507 | return Carve(camera, silhouette, roi_min, roi_max, sdf); 508 | } 509 | 510 | bool VoxelCarver::Carve(const Camera& camera, const Image1f& sdf) { 511 | Eigen::Vector2i roi_min{0, 0}; 512 | Eigen::Vector2i roi_max{sdf.width() - 1, sdf.height() - 1}; 513 | return Carve(camera, roi_min, roi_max, sdf); 514 | } 515 | 516 | bool VoxelCarver::Carve(const std::vector& cameras, 517 | const std::vector& silhouettes) { 518 | assert(cameras.size() == silhouettes.size()); 519 | 520 | for (size_t i = 0; i < cameras.size(); i++) { 521 | bool ret = Carve(cameras[i], silhouettes[i]); 522 | if (!ret) { 523 | return false; 524 | } 525 | } 526 | 527 | return true; 528 | } 529 | 530 | void VoxelCarver::ExtractVoxel(Mesh* mesh, bool inside_empty) { 531 | Timer<> timer; 532 | timer.Start(); 533 | 534 | vacancy::ExtractVoxel(voxel_grid_.get(), mesh, inside_empty); 535 | 536 | timer.End(); 537 | LOGI("VoxelCarver::ExtractVoxel %02f\n", timer.elapsed_msec()); 538 | } 539 | 540 | void VoxelCarver::ExtractIsoSurface(Mesh* mesh, double iso_level, 541 | bool linear_interp) { 542 | MarchingCubes(*voxel_grid_, mesh, iso_level, linear_interp); 543 | } 544 | 545 | } // namespace vacancy 546 | --------------------------------------------------------------------------------