├── .dockerignore ├── .gitignore ├── .gitmodules ├── Dockerfile ├── LICENSE ├── README.md ├── cpp ├── .gitignore ├── CMakeLists.txt ├── depth_scanning │ ├── CMakeLists.txt │ └── src │ │ └── main_scan_3d.cpp ├── include │ ├── Timer.h │ ├── hash_map.h │ ├── img_loader │ │ ├── ImageLoader.h │ │ ├── Printed3dLoader.h │ │ ├── RedwoodLoader.h │ │ ├── SynthLoader.h │ │ ├── TumrgbdLoader.h │ │ └── img_loader.h │ ├── mat.h │ ├── mesh │ │ ├── HrLayeredMarchingCubes.cpp │ │ ├── HrLayeredMarchingCubes.h │ │ ├── LayeredMarchingCubesNoColor.cpp │ │ └── LayeredMarchingCubesNoColor.h │ ├── normals │ │ └── NormalEstimator.h │ ├── ps_optimizer │ │ ├── ColorUpsampler.cpp │ │ ├── ColorUpsampler.h │ │ ├── PhotometricOptimizer.cpp │ │ ├── PhotometricOptimizer.h │ │ ├── SharpDetector.h │ │ └── loss.h │ ├── sdf_tracker │ │ ├── MapGradPixelSdf.cpp │ │ ├── MapGradPixelSdf.h │ │ ├── MapGradPixelSdfOmp.cpp │ │ ├── MapPixelSdf.cpp │ │ ├── MapPixelSdf.h │ │ ├── MapPixelSdfOmp.cpp │ │ ├── RigidOptimizer.h │ │ ├── RigidPointOptimizer.cpp │ │ ├── RigidPointOptimizer.h │ │ ├── RigidPointOptimizerOmp.cpp │ │ └── Sdf.h │ └── sdf_voxel │ │ └── SdfVoxel.h └── photometric_opt │ ├── CMakeLists.txt │ └── src │ └── main_photo_ba.cpp └── matlab ├── GradientAnalysisSpheres.m ├── RenderSpheres.m ├── add_kinect_noise.m ├── phi_statistics.m └── poses.txt /.dockerignore: -------------------------------------------------------------------------------- 1 | build/* 2 | .git -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | .autosave 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cpp/third/CLI11"] 2 | path = cpp/third/CLI11 3 | url = https://github.com/CLIUtils/CLI11.git 4 | [submodule "cpp/third/Sophus"] 5 | path = cpp/third/Sophus 6 | url = https://github.com/strasdat/Sophus.git 7 | [submodule "cpp/third/eigen"] 8 | path = cpp/third/eigen 9 | url = https://gitlab.com/libeigen/eigen.git 10 | [submodule "cpp/third/parallel_hashmap"] 11 | path = cpp/third/parallel_hashmap 12 | url = https://github.com/greg7mdp/parallel-hashmap.git 13 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM opencvcourses/opencv-docker 2 | 3 | COPY . /usr/src 4 | 5 | WORKDIR /usr/src 6 | 7 | RUN apt-get update 8 | RUN apt-get install libomp-dev -y 9 | RUN apt-get update 10 | RUN apt-get install libboost-dev -y 11 | 12 | RUN rm -rf cpp/build/* 13 | 14 | CMD [ "bash" ] -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, C. Sommer, L. Sang, D. Schubert, and D. Cremers. 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 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. 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 | 3. 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 | # Gradient-SDF 2 | 3 | This repository contains the code accompanying the paper 4 | 5 | **Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction** 6 | 7 | by Christiane Sommer*, Lu Sang* (equal contribution), David Schubert, and Daniel Cremers, accepted for publication at CVPR 2022. A preprint version can be found on [arXiv](https://arxiv.org/pdf/2111.13652v1.pdf). 8 | 9 | The **C++ code** in [cpp/](cpp/) contains two experiments: 10 | - 3D scanning: our Gradient-SDF representation is used to do depth camera tracking and mapping. 11 | - Photometric bundle adjustment: our Gradient-SDF representation is used to do bundle adjustment to further improve camera poses and SDF distance. 12 | 13 | The **Matlab code** in [matlab/](matlab/) allows to analyze SDF gradients for synthetic renderings of a set of random spheres. 14 | 15 | ## Instructions - C++ code 16 | 17 | ### Dependencies 18 | - You need to have **OpenCV 4** installed on your computer. 19 | - For other packages, see [`cpp/include/third/`](cpp/include/third). All these additional dependencies are included as submodules and will be automatically added when the repository is cloned with the `--recurse-submodules` flag. Only headers will be used from these. 20 | 21 | We also provide a Dockerfile to create a configuration in which OpenCV 4 is already installed: 22 | ``` 23 | docker build . -t gradient_sdf 24 | docker run -it gradient_sdf 25 | ``` 26 | Note that for this to work, you need to have all test data copied to the repository directory. 27 | You can then continue inside the command line of the Docker container. 28 | 29 | ### Compile 30 | 1. Go to `cpp` folder and run `mkdir build` 31 | 2. Go to `cpp/build/` and run 32 | ``` 33 | cmake .. 34 | make -j 35 | ``` 36 | Two binary files will be generated in `cpp/depth_scanning/bin/` and `cpp/photometric_opt/bin`. 37 | 38 | ### Run 39 | 40 | We provide data loaders for different dataset formats. After downloading a sequence of one of the datasets mentioned below, all you need to add it a file `intrinsics.txt` to the sequence folder, containing the camera intrinsics in format 41 | ``` 42 | fx 0 cx 43 | 0 fy cy 44 | 0 0 1 45 | ``` 46 | - **3D scanning**: to obtain runtimes comparable to those stated in the paper, make sure to activate the OMP versions of the `.cpp` files in `CMakeLists.txt` (ll. 54-56). 47 | The executable file `cpp/depth_scanning/bin/Scan3D` takes at least 3 input arguments. 48 | - `--input`: folder containing rgb and depth images, as well as *intrinsic.txt* 49 | - `--output`: path the save output files 50 | - `--data-type`: supported datasets format *[tum_rgbd_benchmark](https://vision.in.tum.de/data/datasets/rgbd-dataset)*, *[redwood](http://redwood-data.org/3dscan/)*, *[printed3D](http://campar.in.tum.de/personal/slavcheva/3d-printed-dataset/index.html)* 51 | 52 | The scan type is an optional argument specifying if you want to run our proposed Gradient-SDF code or our baseline implementation of a direct SDF tracker with a voxel hash map to store the SDF. 53 | - `--scan-type`: Gradient-SDF (`grad-sdf`, default) or standard SDF (`base-sdf`), both use a hash table to store the SDF voxels. 54 | 55 | For other input argument you can use `./Scan3D -h` to check. 56 | 57 | Example command line: 58 | ``` 59 | cd cpp/depth_scanning/bin 60 | ./Scan3D --input /dataset_path/ --output /savepath/ --scan-type voxel-gp --data-type tum 61 | ``` 62 | 63 | - **Photometric bundle adjustment**: The excutable file `cpp/photometric_opt/bin/PhotoBA` takes at least 3 input arguments. 64 | Note that the non-OMP versions need to be activated in `CMakeLists.txt` (ll. 50-52) for this to work. 65 | - `--input`: folder containing rgb and depth images, as well as `intrinsics.txt` 66 | - `--output`: path the save output files 67 | - `--data-type`: supported datasets format *[tum_rgbd_benchmark](https://vision.in.tum.de/data/datasets/rgbd-dataset)*, *[redwood](http://redwood-data.org/3dscan/)*, *[printed3D](http://campar.in.tum.de/personal/slavcheva/3d-printed-dataset/index.html)* 68 | 69 | For other input argument you can use `./PhotoBA -h` to check. 70 | 71 | Example command line: 72 | ``` 73 | cd cpp/photometric_opt/bin 74 | ./PhotoBA --input /dataset_path/ --output /savepath/ --data-type tum 75 | ``` 76 | 77 | ## Instructions - Matlab code 78 | 79 | ### Data generation 80 | 81 | Adapt the `out_path` variable in `RenderSpheres.m` and then run the script to generate the synthetic sphere data. A new random set of five spheres will be generated for each run. This can be disabled by commenting ll.10-18 and uncommenting l.21. 82 | 83 | ### SDF generation 84 | 85 | To generate the SDF and the stored Gradient-SDF vectors, you need to run the C++ code with `--input ` and `--trunc 10`. 86 | 87 | ### Gradient analysis 88 | 89 | Adapt the `path` variable in `GradientAnalysisSpheres.m` to your `--output` path from the SDF generation and adapt the `sz`, `dmin`, and `dmax` variables accordingly (see file). Then run the script to generate the plot according to Figure 3 in the paper. In addition to Gradient-SDF and central difference vectors, by default, also forward and backward finite difference curves will be plotted. As they are a lot worse than the central differences and clutter the plot, we decided to exclude them in the paper. This can be achieved by commenting the respective lines (see file). 90 | 91 | ## License and Publication 92 | 93 | Our code is released under the BSD-3 license, for more details please see the license file. Also note the different licenses of the submodules in the folder [`cpp/third/`](cpp/third/). 94 | 95 | Please cite our paper when using the code in a scientific project. You can copy-paste the following BibTex entry: 96 | 97 | ``` 98 | @inproceedings{sommersang2022, 99 | title = {Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction}, 100 | author = {Sommer, Christiane and Sang, Lu and Schubert, David and Cremers, Daniel}, 101 | booktitle = {IEEE/CVF International Conference on Computer Vision and Pattern Recognition (CVPR)}, 102 | year = {2022} 103 | } 104 | ``` -------------------------------------------------------------------------------- /cpp/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | results/ 3 | */bin/ 4 | *.sdf 5 | *.vis 6 | *.ply 7 | -------------------------------------------------------------------------------- /cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 3.4) 2 | 3 | SET(PROJECT_NAME Gradient-Sdf) 4 | SET(CMAKE_BUILD_TYPE RelWithDebInfo) 5 | 6 | ########## INCLUDED LIBRARIES ########## 7 | 8 | # Eigen 9 | SET(EIGEN_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third/eigen/") 10 | # FIND_PACKAGE( Eigen REQUIRED ) 11 | MESSAGE(STATUS "Found Eigen headers in: ${EIGEN_INCLUDE_DIR}") 12 | INCLUDE_DIRECTORIES( ${EIGEN_INCLUDE_DIR} ) 13 | 14 | # OpenCV 15 | FIND_PACKAGE( OpenCV REQUIRED ) 16 | MESSAGE(STATUS "Found OpenCV headers in: ${OpenCV_INCLUDE_DIRS}") 17 | INCLUDE_DIRECTORIES( ${OpenCV_INCLUDE_DIRS} ) 18 | 19 | # Sophus 20 | INCLUDE_DIRECTORIES( third/Sophus/ ) 21 | 22 | # CLI, command line parser 23 | INCLUDE_DIRECTORIES( third/CLI11/include/ ) 24 | 25 | # Parallel Hashmap 26 | INCLUDE_DIRECTORIES( third/parallel_hashmap/ ) 27 | 28 | 29 | ########## CUSTOM LIBRARIES ########## 30 | 31 | INCLUDE_DIRECTORIES( include/ 32 | third/ 33 | ) 34 | 35 | # marching cubes 36 | SET( LIB_NAME mc_lib ) 37 | ADD_LIBRARY( ${LIB_NAME} 38 | include/mesh/HrLayeredMarchingCubes.cpp 39 | include/mesh/LayeredMarchingCubesNoColor.cpp 40 | ) 41 | TARGET_COMPILE_OPTIONS( ${LIB_NAME} PRIVATE -std=c++17 ) 42 | 43 | 44 | 45 | # SDF-Tracking stuff 46 | SET( LIB_NAME sdf_tracker_lib ) 47 | ADD_LIBRARY( ${LIB_NAME} 48 | ## uncomment following lines to run non-OMP version 49 | include/sdf_tracker/MapGradPixelSdf.cpp 50 | include/sdf_tracker/MapPixelSdf.cpp 51 | include/sdf_tracker/RigidPointOptimizer.cpp 52 | ## uncomment following lines to run OMP version 53 | # include/sdf_tracker/MapGradPixelSdfOmp.cpp 54 | # include/sdf_tracker/MapPixelSdfOmp.cpp 55 | # include/sdf_tracker/RigidPointOptimizerOmp.cpp 56 | ) 57 | 58 | TARGET_COMPILE_OPTIONS( ${LIB_NAME} PRIVATE -std=c++17 ) 59 | 60 | SET( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp" ) 61 | 62 | 63 | # Color Optimization 64 | SET( LIB_NAME color_lib ) 65 | ADD_LIBRARY( ${LIB_NAME} 66 | include/ps_optimizer/PhotometricOptimizer.cpp 67 | include/ps_optimizer/ColorUpsampler.cpp 68 | ) 69 | 70 | 71 | ########## DIRECTORIES ########## 72 | 73 | ADD_SUBDIRECTORY(depth_scanning) # direct SDF tracking 74 | ADD_SUBDIRECTORY(photometric_opt) # photometric bundle adjustment -------------------------------------------------------------------------------- /cpp/depth_scanning/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 3.2) 2 | 3 | SET(EXECUTABLE_OUTPUT_PATH ../../depth_scanning/bin) 4 | SET(SUBPROJECT_NAME Scan3D) 5 | 6 | ADD_EXECUTABLE( ${SUBPROJECT_NAME} 7 | src/main_scan_3d.cpp 8 | ) 9 | 10 | TARGET_LINK_LIBRARIES( ${SUBPROJECT_NAME} 11 | sdf_tracker_lib 12 | ${OpenCV_LIBS} 13 | mc_lib 14 | ) -------------------------------------------------------------------------------- /cpp/depth_scanning/src/main_scan_3d.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | // standard includes 39 | #include 40 | #include 41 | #include 42 | // library includes 43 | #include 44 | #include 45 | #include 46 | #include 47 | // class includes 48 | #include "Timer.h" 49 | #include "normals/NormalEstimator.h" 50 | #include "sdf_tracker/MapGradPixelSdf.h" 51 | #include "sdf_tracker/MapPixelSdf.h" 52 | #include "sdf_tracker/RigidPointOptimizer.h" 53 | #include "img_loader/img_loader.h" 54 | 55 | // own includes 56 | #include "mat.h" 57 | 58 | /** 59 | * main function 60 | */ 61 | 62 | int main(int argc, char *argv[]) { 63 | 64 | Timer T; 65 | 66 | // Default input sequence in folder 67 | 68 | std::string input = ""; 69 | std::string output = "../results/"; 70 | std::string pose_file_name = "pose.txt"; 71 | std::string stype = "map-gp"; 72 | std::string dtype = ""; 73 | size_t first = 0; 74 | size_t last = std::numeric_limits::max(); 75 | float voxel_size = 0.01; // Voxel size in m 76 | float z_max = 3.5; // maximal depth to take into account in m 77 | float truncation_factor = 5; // truncation in voxels 78 | bool save_sdf = false; // specify if final SDF (+gradients) shall be saved 79 | 80 | CLI::App app{"Hash Table-Based 3D Scanning"}; 81 | app.add_option("--input", input, "folder of input sequence"); 82 | app.add_option("--results", output, "folder to store results in"); 83 | app.add_option("--pose-file", pose_file_name, "pose file name in input folder"); 84 | app.add_option("--first", first, "number of first frame to be processed (default: 0)"); 85 | app.add_option("--last", last, "number of last frame (default: all)"); 86 | app.add_option("--scan-type", stype, "type of scanner used (default: grad-sdf)"); 87 | app.add_option("--data-type", dtype, "type of dataset"); 88 | app.add_option("--voxel-size", voxel_size, "voxel size in meters (default: 0.01)"); 89 | app.add_option("--trunc", truncation_factor, "truncation in multiples of voxels"); 90 | app.add_flag("--save-sdf", save_sdf, "flag: save sdf (+gradient) to txt file"); 91 | 92 | // parse input arguments 93 | try { 94 | app.parse(argc, argv); 95 | } catch (const CLI::ParseError& e) { 96 | return app.exit(e); 97 | } 98 | 99 | // parse scan type 100 | enum class ScanType { 101 | GRAD_SDF, 102 | BASE 103 | }; 104 | ScanType ST; 105 | if (stype == "grad-sdf") { 106 | ST = ScanType::GRAD_SDF; 107 | } 108 | else if (stype == "base-sdf") { 109 | ST = ScanType::BASE; 110 | } 111 | else { 112 | std::cerr << "Your specified scan type is not supported (yet)." << std::endl; 113 | return 1; 114 | } 115 | 116 | // parse dataset type 117 | enum class DataType { 118 | TUM_RGBD, 119 | SYNTH, 120 | PRINTED_3D, 121 | REDWOOD, 122 | }; 123 | DataType DT; 124 | if (dtype == "tum") { 125 | DT = DataType::TUM_RGBD; 126 | } 127 | else if (dtype == "synth") { 128 | DT = DataType::SYNTH; 129 | } 130 | else if (dtype == "printed") { 131 | DT = DataType::PRINTED_3D; 132 | } 133 | else if (dtype == "rw" || dtype == "redwood") { 134 | DT = DataType::REDWOOD; 135 | } 136 | else { 137 | std::cerr << "Your specified dataset type is not supported (yet)." << std::endl; 138 | return 1; 139 | } 140 | 141 | // create image loader 142 | ImageLoader* loader; 143 | switch (DT) { 144 | case DataType::TUM_RGBD : 145 | loader = new TumrgbdLoader(input); 146 | break; 147 | case DataType::SYNTH : 148 | loader = new SynthLoader(input); 149 | break; 150 | case DataType::PRINTED_3D : 151 | loader = new Printed3dLoader(input); 152 | break; 153 | case DataType::REDWOOD : 154 | loader = new RedwoodLoader(input); 155 | break; 156 | default: 157 | std::cerr << "Specified dataset type not recognized, return" << std::endl; 158 | return 1; 159 | } 160 | 161 | // Load camera intrinsics 162 | if (!loader->load_intrinsics("intrinsics.txt")) { 163 | std::cerr << "No intrinsics file found in " << input << "!" << std::endl; 164 | return 1; 165 | } 166 | const Mat3f K = loader->K(); 167 | std::cout << "K: " << std::endl << K << std::endl; 168 | 169 | // load GT poses if they are available 170 | std::vector> poses; 171 | bool GT_pose = false; 172 | if (!loader->load_pose(input + pose_file_name, poses)){ 173 | std::cerr << "No GT poses are avaible!" << std::endl; 174 | } 175 | else{ 176 | std::cout << poses.size() << " GT poses are loaded!" << std::endl; 177 | GT_pose = true; 178 | } 179 | 180 | // create normal estimator 181 | T.tic(); 182 | cv::NormalEstimator* NEst; 183 | NEst = new cv::NormalEstimator(640, 480, K, cv::Size(2*5+1, 2*5+1)); 184 | T.toc("Init normal estimation"); 185 | 186 | // float voxel_size = 0.02; // Voxel size in m 187 | std::stringstream stream; 188 | stream << std::fixed << std::setprecision(3) << voxel_size; 189 | std::string voxel_size_str = stream.str(); 190 | // Trunction distance for the tSDF 191 | const float truncation = truncation_factor * voxel_size; 192 | 193 | Sdf* tSDF; 194 | RigidPointOptimizer* pOpt; 195 | 196 | std::string filename = output + "_poses.txt"; 197 | std::ofstream pose_file(filename); 198 | 199 | // Frames to be processed 200 | cv::Mat color, depth; 201 | 202 | // Proceed until first frame 203 | for (size_t i = 0; i < first; ++i) { 204 | loader->load_next(color, depth); 205 | } 206 | 207 | // Actual scanning loop 208 | for (size_t i = first; i <= last; ++i) { 209 | std::cout << "Working on frame: " << i << std::endl; 210 | 211 | // Load data 212 | T.tic(); 213 | if (!loader->load_next(color, depth)) { 214 | std::cerr << " -> Frame " << i << " could not be loaded!" << std::endl; 215 | T.toc("Load data"); 216 | break; 217 | } 218 | T.toc("Load data"); 219 | 220 | if (i == first) { 221 | 222 | Mat4f Trans = Mat4f::Identity(); 223 | if(GT_pose)Trans = poses[0]; 224 | 225 | // create SDF data 226 | T.tic(); 227 | switch (ST) { 228 | case ScanType::GRAD_SDF : 229 | tSDF = new MapGradPixelSdf(voxel_size, truncation); 230 | break; 231 | case ScanType::BASE : 232 | tSDF = new MapPixelSdf(voxel_size, truncation); 233 | break; 234 | default: 235 | std::cerr << "Specified scan type not recognized, return" << std::endl; 236 | return 1; 237 | } 238 | T.toc("Create Sdf"); 239 | 240 | // Initialize tSDF 241 | T.tic(); 242 | if(GT_pose){tSDF->update(color, depth, K, SE3(poses[0]), NEst);} 243 | else{tSDF->setup(color, depth, K, NEst);} 244 | T.toc("Integrate depth data into Sdf"); 245 | // Initialize optimizer 246 | T.tic(); 247 | pOpt = new RigidPointOptimizer(tSDF); 248 | T.toc("Create RigidOptimizer"); 249 | } 250 | else if(GT_pose){ 251 | T.tic(); 252 | tSDF->update(color, depth, K, SE3(poses[i]), NEst); 253 | T.toc("Integrate depth data into Sdf"); 254 | } 255 | else { 256 | // Perform optimization 257 | T.tic(); 258 | bool conv = pOpt->optimize(depth, K); 259 | T.toc("Point optimization"); 260 | // Integrate data into model 261 | if (conv) { 262 | T.tic(); 263 | tSDF->update(color, depth, K, pOpt->pose(), NEst); 264 | T.toc("Integrate depth data into Sdf"); 265 | } 266 | } 267 | // write timestamp + pose in tx ty tz qx qy qz qw format 268 | Mat4f p; 269 | if(GT_pose){p = poses[i];} 270 | else{p = pOpt->pose().matrix(); } 271 | std::cout << "Current pose:" << std::endl 272 | << p << std::endl; 273 | 274 | Vec3f t(p.topRightCorner(3,1)); 275 | Mat3f R = p.topLeftCorner(3,3); 276 | Eigen::Quaternion q(R); 277 | 278 | pose_file << loader->depth_timestamp() << " " 279 | << t[0] << " " << t[1] << " " << t[2] << " " 280 | << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << "\n"; 281 | } 282 | 283 | pose_file.close(); 284 | 285 | std::string file_prefix = "gradient_sdf"; 286 | 287 | // extract mesh and write to file 288 | T.tic(); 289 | filename = output + file_prefix + "_mesh_final.ply"; 290 | if (!tSDF->extract_mesh(filename)) { 291 | std::cerr << "Could not save mesh to " << filename << "!" << std::endl; 292 | } 293 | T.toc("Save mesh to disk"); 294 | 295 | // extract point cloud and write to file 296 | T.tic(); 297 | filename = output + file_prefix + "_cloud_final.ply"; 298 | if (!tSDF->extract_pc(filename)) { 299 | std::cerr << "Could not save point cloud to " << filename << "!" << std::endl; 300 | } 301 | T.toc("Save point cloud to disk"); 302 | 303 | // write sdf to file to run gradient analysis 304 | if (save_sdf) { 305 | T.tic(); 306 | filename = output + file_prefix; 307 | if(!tSDF->save_sdf(filename)){ 308 | std::cerr << "could not save voxel grid info file " << filename << "!" << std::endl; 309 | } 310 | T.toc("Save sdf txt files to disk"); 311 | } 312 | 313 | // tidy up 314 | delete tSDF; 315 | delete pOpt; 316 | delete loader; 317 | 318 | return 0; 319 | } 320 | -------------------------------------------------------------------------------- /cpp/include/Timer.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef TIMER_H_ 39 | #define TIMER_H_ 40 | 41 | // includes 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | class Timer { 48 | 49 | private: 50 | double start_time; 51 | double end_time; 52 | double elapsed; 53 | 54 | public: 55 | Timer() : start_time(0.), end_time(0.), elapsed(0.) {} 56 | ~Timer() {} 57 | 58 | void tic() { 59 | start_time = omp_get_wtime(); 60 | } 61 | 62 | double toc(std::string s = "Time elapsed") { 63 | if (start_time!=0) { 64 | end_time = omp_get_wtime(); 65 | elapsed = end_time-start_time; 66 | print_time(s); 67 | } 68 | else 69 | std::cout << "Timer was not started, no time could be measured." << std::endl; 70 | return elapsed; 71 | } 72 | 73 | void print_time(std::string s = "Time elapsed") { 74 | if (elapsed<1.) 75 | std::cout << "---------- " << s << ": " << 1000.*elapsed << "ms." << std::endl; 76 | else 77 | std::cout << "---------- " << s << ": " << elapsed << "s." << std::endl; 78 | } 79 | 80 | }; 81 | 82 | #endif // TIMER_H_ 83 | -------------------------------------------------------------------------------- /cpp/include/hash_map.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef HASH_MAP_H_ 39 | #define HASH_MAP_H_ 40 | 41 | #include "parallel_hashmap/phmap.h" 42 | #include "mat.h" 43 | 44 | namespace std { 45 | // inject specialization of std::hash for Vec3i into namespace std 46 | // ---------------------------------------------------------------- 47 | template <> struct hash { 48 | std::size_t operator()(Vec3i const& p) const { 49 | return phmap::HashState().combine(p[0], p[1], p[2]); 50 | } 51 | }; 52 | } // namespace std 53 | 54 | 55 | #endif //HASH_MAP_H_ -------------------------------------------------------------------------------- /cpp/include/img_loader/ImageLoader.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef IMAGE_LOADER_H_ 39 | #define IMAGE_LOADER_H_ 40 | 41 | // includes 42 | #include 43 | #include 44 | #include 45 | // library includes 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | class ImageLoader { 52 | 53 | protected: 54 | 55 | Eigen::Matrix3f K_; 56 | 57 | const float unit_; // in meters 58 | const std::string path_; // path to directory containing data 59 | std::string timestamp_rgb_, timestamp_depth_; // current timestamp 60 | bool consecutive_numbers_ ; 61 | std::vector timestamps_depth_, timestamps_rgb_; 62 | 63 | 64 | public: 65 | 66 | 67 | ImageLoader() : 68 | unit_(1.), 69 | path_(""), 70 | timestamp_rgb_(""), 71 | timestamp_depth_(""), 72 | consecutive_numbers_(false) 73 | {} 74 | 75 | ImageLoader(float unit, bool consecutive_numbers) : 76 | unit_(unit), 77 | path_(""), 78 | timestamp_rgb_(""), 79 | timestamp_depth_(""), 80 | consecutive_numbers_(consecutive_numbers) 81 | {} 82 | 83 | ImageLoader(const std::string& path, bool consecutive_numbers) : 84 | unit_(1.), 85 | path_(path), 86 | timestamp_rgb_(""), 87 | timestamp_depth_(""), 88 | consecutive_numbers_(consecutive_numbers) 89 | {} 90 | 91 | ImageLoader(float unit, const std::string& path, bool consecutive_numbers) : 92 | unit_(unit), 93 | path_(path), 94 | timestamp_rgb_(""), 95 | timestamp_depth_(""), 96 | consecutive_numbers_(consecutive_numbers) 97 | {} 98 | 99 | virtual ~ImageLoader() {} 100 | 101 | Eigen::Matrix3f K() const { 102 | return K_; 103 | } 104 | 105 | std::string rgb_timestamp() { 106 | return timestamp_rgb_; 107 | } 108 | 109 | std::string depth_timestamp() { 110 | return timestamp_depth_; 111 | } 112 | 113 | 114 | std::string getDepthTimestamp(const int i){ 115 | return timestamps_depth_[i]; 116 | } 117 | 118 | std::string getRgbTimestamp(const int i){ 119 | return timestamps_rgb_[i]; 120 | } 121 | 122 | std::vector getDepthTimestamps(){ 123 | return timestamps_depth_; 124 | } 125 | 126 | std::vector getRgbTimestamps(){ 127 | return timestamps_rgb_; 128 | } 129 | 130 | void setDepthTimestamps(std::vector timestamps){ 131 | timestamps_depth_ = timestamps; 132 | } 133 | 134 | void setRgbTimestamps(std::vector timestamps){ 135 | timestamps_rgb_ = timestamps; 136 | } 137 | 138 | bool load_intrinsics(const std::string& filename = "intrinsics.txt") { 139 | 140 | if (filename.empty()) 141 | return false; 142 | 143 | std::ifstream infile(path_ + filename); 144 | if (!infile.is_open()) 145 | return false; 146 | 147 | // load intrinsic camera matrix 148 | float tmp = 0; 149 | for (size_t i=0; i<3; ++i) for (size_t j=0; j<3; ++j) { 150 | infile >> tmp; 151 | K_(i, j) = tmp; 152 | } 153 | 154 | infile.close(); 155 | 156 | return true; 157 | } 158 | 159 | bool load_depth(const std::string& filename, cv::Mat& depth) { 160 | 161 | if (filename.empty()) { 162 | std::cerr << "Error: missing filename" << std::endl; 163 | return false; 164 | } 165 | 166 | // fill/read 16 bit depth image 167 | cv::Mat depthIn = cv::imread(path_ + filename, cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR); 168 | if (depthIn.empty()) { 169 | std::cerr << "Error: empty depth image " << path_ + filename << std::endl; 170 | return false; 171 | } 172 | depthIn.convertTo(depth, CV_32FC1, unit_); 173 | 174 | return true; 175 | } 176 | 177 | bool load_gray(const std::string& filename, cv::Mat& gray){ 178 | if (filename.empty()) { 179 | std::cerr << "Error: missing filename" << std::endl; 180 | return false; 181 | } 182 | 183 | // load gray 184 | cv::Mat imgGray = cv::imread(filename, cv::IMREAD_GRAYSCALE); 185 | // convert gray to float 186 | imgGray.convertTo(gray, CV_32FC1, 1.0f / 255.0f); 187 | 188 | if(gray.empty()) { 189 | std::cerr << "Error: empty gray scale image " << path_ + filename << std::endl; 190 | return false; 191 | } 192 | return true; 193 | 194 | } 195 | 196 | bool load_color(const std::string& filename, cv::Mat& color) { 197 | 198 | if (filename.empty()) { 199 | std::cerr << "Error: missing filename" << std::endl; 200 | return false; 201 | } 202 | 203 | // load color 204 | cv::Mat imgColor = cv::imread(path_ + filename); 205 | 206 | if(imgColor.channels()==1) 207 | { 208 | cv::cvtColor(imgColor, imgColor, cv::COLOR_GRAY2BGR); 209 | } 210 | imgColor.convertTo(color, CV_32FC1, 1.0f / 255.0f); 211 | if (color.empty()) { 212 | std::cerr << "Error: empty color image " << path_ + filename << std::endl; 213 | return false; 214 | } 215 | 216 | return true; 217 | } 218 | 219 | bool consecutive_numbers(){ 220 | return consecutive_numbers_; 221 | } 222 | 223 | virtual bool load_next(cv::Mat &color, cv::Mat &depth) = 0; 224 | 225 | 226 | virtual bool load_keyframe(cv::Mat &color, cv::Mat &depth, const int frame) { 227 | // only implement if consecutive_numbers_ == true 228 | return false; 229 | } 230 | 231 | bool load_pose(std::string filename, std::vector >& poses) 232 | { 233 | 234 | std::ifstream file; 235 | file.open(filename.c_str()); 236 | if(!file.is_open()){ 237 | std::cout << "can't load poses!" << std::endl; 238 | return false; 239 | } 240 | std::string line; 241 | while (std::getline(file, line)){ 242 | float timestamp; 243 | Eigen::Vector3f translation; 244 | Eigen::Quaternionf q; 245 | std::stringstream s(line); 246 | s >> timestamp >> translation[0] >> translation[1] >> translation[2] >> q.x() >> q.y() >> q.z() >> q.w(); 247 | if (q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z() < 0.99) { 248 | std::cerr << "pose " << timestamp << " has invalid rotation" << std::endl; 249 | } 250 | Eigen::Matrix4f tmp = Eigen::Matrix4f::Identity(); 251 | tmp.topRightCorner(3,1) = translation; 252 | Eigen::Matrix3f rot = q.toRotationMatrix(); 253 | tmp.topLeftCorner(3,3) = rot; 254 | poses.push_back(tmp); 255 | } 256 | 257 | return true; 258 | 259 | } 260 | 261 | virtual void reset() = 0; 262 | 263 | }; 264 | 265 | #endif // IMAGE_LOADER_H_ 266 | -------------------------------------------------------------------------------- /cpp/include/img_loader/Printed3dLoader.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef PRINTED3D_LOADER_H_ 39 | #define PRINTED3D_LOADER_H_ 40 | 41 | #include 42 | #include "ImageLoader.h" 43 | 44 | class Printed3dLoader : public ImageLoader { 45 | 46 | private: 47 | 48 | size_t counter; 49 | 50 | public: 51 | 52 | Printed3dLoader() : 53 | ImageLoader(1./1000, true), 54 | counter(0) 55 | {} 56 | 57 | Printed3dLoader(const std::string& path) : 58 | ImageLoader(1./1000, path, true), 59 | counter(0) 60 | {} 61 | 62 | ~Printed3dLoader() {} 63 | 64 | bool load_next(cv::Mat& color, cv::Mat& depth) { 65 | 66 | std::stringstream ss; 67 | ss << std::setfill('0') << std::setw(6) << counter; 68 | 69 | timestamp_rgb_ = ss.str(); 70 | timestamp_depth_ = timestamp_rgb_; 71 | 72 | const std::string filename = timestamp_rgb_ + ".png"; 73 | 74 | if (!load_depth("depth_" + filename, depth)) 75 | return false; 76 | 77 | if (!load_color("color_" + filename, color)) 78 | return false; 79 | 80 | ++counter; 81 | 82 | return true; 83 | } 84 | bool load_keyframe(cv::Mat &color, cv::Mat &depth, const int frame) 85 | { 86 | 87 | std::stringstream ss; 88 | ss << std::setfill('0') << std::setw(6) << frame; 89 | 90 | timestamp_rgb_ = ss.str(); 91 | timestamp_depth_ = timestamp_rgb_; 92 | 93 | const std::string filename = timestamp_rgb_ + ".png"; 94 | timestamps_depth_.push_back(timestamp_depth_); 95 | timestamps_rgb_.push_back(timestamp_rgb_); 96 | 97 | if (!load_depth("depth_" + filename, depth)) 98 | return false; 99 | 100 | if (!load_color("color_" + filename, color)) 101 | return false; 102 | 103 | 104 | return true; 105 | } 106 | 107 | void reset() 108 | { 109 | counter = 0; 110 | timestamps_depth_.clear(); 111 | timestamps_rgb_.clear(); 112 | } 113 | 114 | }; 115 | 116 | #endif // PRINTED3D_LOADER_H_ 117 | -------------------------------------------------------------------------------- /cpp/include/img_loader/RedwoodLoader.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef REDWOOD_LOADER_H_ 39 | #define REDWOOD_LOADER_H_ 40 | 41 | #include "ImageLoader.h" 42 | #include 43 | #include 44 | #include 45 | 46 | class RedwoodLoader : public ImageLoader { 47 | 48 | private: 49 | 50 | DIR *depth_dir_; 51 | dirent *depth_ent_; 52 | DIR *rgb_dir_; 53 | dirent *rgb_ent_; 54 | std::vector depth_filenames_, rgb_filenames_; 55 | size_t counter; 56 | 57 | void init() { 58 | depth_dir_ = opendir((path_ + "depth/").c_str()); 59 | rgb_dir_ = opendir((path_ + "rgb/").c_str()); 60 | readdir(depth_dir_); 61 | readdir(depth_dir_); 62 | readdir(rgb_dir_); 63 | readdir(rgb_dir_); 64 | load_file_name(); 65 | return; 66 | } 67 | 68 | public: 69 | 70 | RedwoodLoader() : 71 | ImageLoader(1./1000, true), 72 | counter(0) 73 | { 74 | init(); 75 | } 76 | 77 | RedwoodLoader(const std::string& path) : 78 | ImageLoader(1./1000, path, true), 79 | counter(0) 80 | { 81 | init(); 82 | } 83 | 84 | ~RedwoodLoader() 85 | { 86 | closedir(depth_dir_); 87 | closedir(rgb_dir_); 88 | } 89 | 90 | void load_file_name(){ 91 | 92 | while ((depth_ent_ = readdir(depth_dir_))!= NULL && (rgb_ent_ = readdir(rgb_dir_)) != NULL){ 93 | std::string depthfiles, rgbfiles; 94 | depthfiles = depth_ent_->d_name; 95 | rgbfiles = rgb_ent_->d_name; 96 | 97 | if(depthfiles.substr(depthfiles.length()-3)=="png") 98 | depth_filenames_.push_back(depthfiles); 99 | if(rgbfiles.substr(rgbfiles.length()-3)=="jpg") 100 | rgb_filenames_.push_back(rgbfiles); 101 | } 102 | 103 | std::sort(depth_filenames_.begin(), depth_filenames_.end()); 104 | std::sort(rgb_filenames_.begin(),rgb_filenames_.end()); 105 | 106 | } 107 | 108 | bool load_next(cv::Mat& color, cv::Mat& depth) { 109 | 110 | std::string filename; 111 | 112 | // if (!(depth_ent_ = readdir(depth_dir_)) || !(rgb_ent_ = readdir(rgb_dir_))) 113 | // return false; 114 | 115 | filename = depth_filenames_[counter]; 116 | timestamp_depth_ = filename; //TODO get rid of suffix 117 | timestamps_depth_.push_back(filename); 118 | 119 | std::cout << "------load depth " << filename << ". " << std::endl; 120 | 121 | if (!load_depth("depth/" + filename, depth)) 122 | return false; 123 | 124 | filename = rgb_filenames_[counter]; 125 | timestamps_rgb_.push_back(filename); 126 | 127 | std::cout << "------load rgb " << filename << ". " << std::endl; 128 | 129 | if (!load_color("rgb/" + filename, color)) 130 | return false; 131 | 132 | ++counter; 133 | return true; 134 | } 135 | 136 | void reset() { 137 | init(); 138 | timestamps_depth_.clear(); 139 | timestamps_rgb_.clear(); 140 | counter = 0; 141 | } 142 | 143 | }; 144 | 145 | #endif // REDWOOD_LOADER_H_ 146 | -------------------------------------------------------------------------------- /cpp/include/img_loader/SynthLoader.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef SYNTH_LOADER_H_ 39 | #define SYNTH_LOADER_H_ 40 | 41 | #include 42 | #include "ImageLoader.h" 43 | 44 | class SynthLoader : public ImageLoader { 45 | 46 | private: 47 | 48 | size_t counter; 49 | 50 | public: 51 | 52 | SynthLoader() : 53 | ImageLoader(1./1000, true), 54 | counter(1) 55 | {} 56 | 57 | SynthLoader(const std::string& path) : 58 | ImageLoader(1./1000, path, true), 59 | counter(1) 60 | {} 61 | 62 | ~SynthLoader() {} 63 | 64 | bool load_next(cv::Mat& color, cv::Mat& depth) { 65 | 66 | std::stringstream ss; 67 | ss << std::setfill('0') << std::setw(3) << counter; 68 | 69 | timestamp_rgb_ = ss.str(); 70 | timestamp_depth_ = timestamp_rgb_; 71 | 72 | const std::string filename = timestamp_rgb_ + ".png"; 73 | 74 | if (!load_depth("depth/" + filename, depth)) 75 | return false; 76 | 77 | if (!load_color("rgb/" + filename, color)) 78 | return false; 79 | 80 | ++counter; 81 | 82 | return true; 83 | } 84 | 85 | bool load_keyframe(cv::Mat& color, cv::Mat& depth, const int frame) { 86 | 87 | std::stringstream ss; 88 | ss << std::setfill('0') << std::setw(3) << frame+1; 89 | 90 | timestamp_rgb_ = ss.str(); 91 | timestamp_depth_ = timestamp_rgb_; 92 | 93 | const std::string filename = timestamp_rgb_ + ".png"; 94 | timestamps_depth_.push_back(timestamp_depth_); 95 | timestamps_rgb_.push_back(timestamp_rgb_); 96 | 97 | if (!load_depth("depth/" + filename, depth)) 98 | return false; 99 | 100 | if (!load_color("albedo/" + filename, color)) 101 | return false; 102 | 103 | std::cout<<"image:" << filename << " is loaded!" << std::endl; 104 | return true; 105 | } 106 | 107 | void reset() 108 | { 109 | counter = 1; 110 | timestamps_depth_.clear(); 111 | timestamps_rgb_.clear(); 112 | } 113 | 114 | }; 115 | 116 | #endif // SYNTH_LOADER_H_ 117 | -------------------------------------------------------------------------------- /cpp/include/img_loader/TumrgbdLoader.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | #ifndef TUMRGBD_LOADER_H_ 38 | #define TUMRGBD_LOADER_H_ 39 | 40 | #include "ImageLoader.h" 41 | #include 42 | #include 43 | 44 | class TumrgbdLoader : public ImageLoader { 45 | 46 | private: 47 | 48 | std::ifstream depth_file_; 49 | std::ifstream color_file_; 50 | std::ifstream assoc_file_; 51 | 52 | void init() { 53 | depth_file_.open(path_ + "depth.txt"); 54 | color_file_.open(path_ + "rgb.txt"); 55 | assoc_file_.open(path_ + "associated.txt"); 56 | return; 57 | } 58 | 59 | public: 60 | 61 | TumrgbdLoader() : 62 | ImageLoader(1./5000, false) 63 | { 64 | init(); 65 | } 66 | 67 | TumrgbdLoader(const std::string& path) : 68 | ImageLoader(1./5000, path, false) 69 | { 70 | init(); 71 | } 72 | 73 | ~TumrgbdLoader() { 74 | depth_file_.close(); 75 | color_file_.close(); 76 | assoc_file_.close(); 77 | } 78 | 79 | 80 | bool load_next(cv::Mat& color, cv::Mat& depth) { 81 | 82 | std::string line, tmp, rgb_filename, depth_filename; 83 | 84 | line = "#"; 85 | while (line.at(0) == '#') { 86 | if(!std::getline(assoc_file_, line)) 87 | return false; 88 | } 89 | 90 | std::istringstream dss(line); 91 | dss >> timestamp_rgb_ >> rgb_filename >> timestamp_depth_ >> depth_filename; 92 | 93 | std::cout << "load image " << timestamp_rgb_ << std::endl; 94 | 95 | timestamps_depth_.push_back(timestamp_depth_); 96 | timestamps_rgb_.push_back(timestamp_rgb_); 97 | 98 | if (!load_depth(depth_filename, depth)) 99 | return false; 100 | 101 | if (!load_color(rgb_filename, color)) 102 | return false; 103 | return true; 104 | } 105 | 106 | void reset() { 107 | depth_file_.close(); 108 | color_file_.close(); 109 | assoc_file_.close(); 110 | depth_file_.open(path_ + "depth.txt"); 111 | color_file_.open(path_ + "rgb.txt"); 112 | assoc_file_.open(path_ + "associated.txt"); 113 | timestamps_depth_.clear(); 114 | timestamps_rgb_.clear(); 115 | } 116 | 117 | }; 118 | 119 | #endif // TUMRGBD_LOADER_H_ 120 | -------------------------------------------------------------------------------- /cpp/include/img_loader/img_loader.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef IMG_LOADER_H_ 39 | #define IMG_LOADER_H_ 40 | 41 | // convenience header that includes all relevant image loaders 42 | 43 | #include "img_loader/TumrgbdLoader.h" 44 | #include "img_loader/Printed3dLoader.h" 45 | #include "img_loader/SynthLoader.h" 46 | #include "img_loader/RedwoodLoader.h" 47 | 48 | #endif // IMG_LOADER_H_ -------------------------------------------------------------------------------- /cpp/include/mat.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef MAT_H 39 | #define MAT_H 40 | 41 | #ifndef WIN64 42 | #define EIGEN_DONT_ALIGN_STATICALLY 43 | #endif 44 | #include 45 | #include 46 | 47 | typedef Eigen::Vector2f Vec2f; 48 | typedef Eigen::Vector3f Vec3f; 49 | typedef Eigen::Vector4f Vec4f; 50 | typedef Eigen::Matrix3f Mat3f; 51 | typedef Eigen::Matrix4f Mat4f; 52 | 53 | typedef Eigen::Vector2d Vec2; 54 | typedef Eigen::Vector3d Vec3; 55 | typedef Eigen::Matrix3d Mat3; 56 | typedef Eigen::Matrix4d Mat4; 57 | 58 | typedef Eigen::Vector2i Vec2i; 59 | typedef Eigen::Vector3i Vec3i; 60 | typedef Eigen::Matrix Vec3b; 61 | 62 | typedef Sophus::SE3 SE3; 63 | typedef Sophus::SO3 SO3; 64 | 65 | typedef Eigen::Matrix Mat6f; 66 | typedef Eigen::Matrix Vec6f; 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /cpp/include/mesh/HrLayeredMarchingCubes.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef HR_LAYERED_MARCHING_CUBES_H 39 | #define HR_LAYERED_MARCHING_CUBES_H 40 | 41 | #include 42 | 43 | #include "mat.h" 44 | 45 | #include 46 | 47 | #include "sdf_voxel/SdfVoxel.h" 48 | 49 | // class of high resolution layered marching cubes with colors. 50 | class HrLayeredMarchingCubes 51 | { 52 | public: 53 | 54 | using voxel_phmap = phmap::parallel_node_hash_map, 56 | phmap::priv::hash_default_eq, 57 | Eigen::aligned_allocator>>; 58 | 59 | // HrLayeredMarchingCubes(const Vec3i &dimensions, const Vec3f &size); 60 | 61 | HrLayeredMarchingCubes(const Vec3f &voxelSize); 62 | 63 | ~HrLayeredMarchingCubes(); 64 | 65 | bool computeIsoSurface(const voxel_phmap* sdf_map, float isoValue = 0.0f); 66 | 67 | bool savePly(const std::string &filename) const; 68 | 69 | protected: 70 | 71 | static int edgeTable[256]; 72 | 73 | static int triTable[256][16]; 74 | 75 | inline int computeLutIndex(int i, int j, int k, float isoValue); 76 | 77 | void copyLayer(int z, const voxel_phmap* sdf_map); 78 | 79 | inline void zeroWeights(int x, int y, int z); 80 | 81 | inline void copyCube(int x, int y, int z, const float w, 82 | const Eigen::Matrix& d, 83 | const Eigen::Matrix& r, 84 | const Eigen::Matrix& g, 85 | const Eigen::Matrix& b 86 | ); 87 | 88 | inline void setVoxel(const size_t off, const size_t corner, const float w, 89 | const Eigen::Matrix& d, 90 | const Eigen::Matrix& r, 91 | const Eigen::Matrix& g, 92 | const Eigen::Matrix& b 93 | ); 94 | 95 | Vec3f interpolate(float tsdf0, float tsdf1, const Vec3f &val0, const Vec3f &val1, float isoValue); 96 | 97 | Vec3f getVertex(int i1, int j1, int k1, int i2, int j2, int k2, float isoValue); 98 | 99 | Vec3b getColor(int x1, int y1, int z1, int x2, int y2, int z2, float isoValue); 100 | 101 | void computeTriangles(int cubeIndex, const Vec3f edgePoints[12], const Vec3b edgeColors[12]); 102 | 103 | inline unsigned int addVertex(const Vec3f &v, const Vec3b &c); 104 | 105 | Vec3f voxelToWorld(int i, int j, int k) const; 106 | 107 | std::vector vertices_; 108 | std::vector colors_; 109 | std::vector faces_; 110 | Vec3i dim_; 111 | Vec3f size_; 112 | Vec3f voxelSize_; 113 | Vec3f origin_; 114 | Vec3i min_; 115 | 116 | // layers 117 | float* tsdf_; 118 | float* weights_; 119 | unsigned char* red_; 120 | unsigned char* green_; 121 | unsigned char* blue_; 122 | }; 123 | 124 | #endif 125 | -------------------------------------------------------------------------------- /cpp/include/mesh/LayeredMarchingCubesNoColor.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef LAYERED_MARCHING_CUBES_NO_COLOR_H 39 | #define LAYERED_MARCHING_CUBES_NO_COLOR_H 40 | 41 | #include 42 | 43 | #include "mat.h" 44 | 45 | #include 46 | 47 | #include "sdf_voxel/SdfVoxel.h" 48 | 49 | 50 | // class of layered marching cubes without color. 51 | class LayeredMarchingCubesNoColor 52 | { 53 | public: 54 | 55 | using voxel_phmap = phmap::parallel_node_hash_map, 57 | phmap::priv::hash_default_eq, 58 | Eigen::aligned_allocator>>; 59 | 60 | // LayeredMarchingCubesNoColor(const Vec3i &dimensions, const Vec3f &size); 61 | 62 | LayeredMarchingCubesNoColor(const Vec3f &voxelSize); 63 | 64 | ~LayeredMarchingCubesNoColor(); 65 | 66 | bool computeIsoSurface(const voxel_phmap* sdf_map, float isoValue = 0.0f); 67 | 68 | bool savePly(const std::string &filename) const; 69 | 70 | protected: 71 | 72 | static int edgeTable[256]; 73 | 74 | static int triTable[256][16]; 75 | 76 | inline int computeLutIndex(int i, int j, int k, float isoValue); 77 | 78 | void copyLayer(int z, const voxel_phmap* sdf_map); 79 | 80 | Vec3f interpolate(float tsdf0, float tsdf1, const Vec3f &val0, const Vec3f &val1, float isoValue); 81 | 82 | Vec3f getVertex(int i1, int j1, int k1, int i2, int j2, int k2, float isoValue); 83 | 84 | void computeTriangles(int cubeIndex, const Vec3f edgePoints[12]); 85 | 86 | inline unsigned int addVertex(const Vec3f &v); 87 | 88 | Vec3f voxelToWorld(int i, int j, int k) const; 89 | 90 | size_t areaXY_ = 0; 91 | std::vector vertices_; 92 | std::vector faces_; 93 | Vec3i dim_; 94 | Vec3f size_; 95 | Vec3f voxelSize_; 96 | Vec3f origin_; 97 | Vec3i min_; 98 | 99 | // layers 100 | float* tsdf_; 101 | float* weights_; 102 | }; 103 | 104 | #endif 105 | -------------------------------------------------------------------------------- /cpp/include/normals/NormalEstimator.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef CV_NORMAL_ESTIMATOR_H_ 39 | #define CV_NORMAL_ESTIMATOR_H_ 40 | 41 | // libraries 42 | #include 43 | #include 44 | #include 45 | 46 | namespace cv { 47 | 48 | /* 49 | * NormalEstimator class using the FALS method from Badino et al. 50 | */ 51 | template 52 | class NormalEstimator { 53 | 54 | using Mat3T = Eigen::Matrix; 55 | 56 | // problem parameters 57 | int width_, height_; 58 | Mat3T K_; 59 | Size window_size_; 60 | // values needed for normal computation 61 | Mat_ x0_, y0_; 62 | Mat_ x0_n_sq_inv_, y0_n_sq_inv_, n_sq_inv_; 63 | Mat_ Q11_, Q12_, Q13_, Q22_, Q23_, Q33_; 64 | 65 | // similar to Matlab's meshgrid function 66 | template 67 | void pixelgrid(U shift_x, U shift_y, Mat_ &u, Mat_ &v) { 68 | 69 | std::vector row, col; 70 | for (int k = 0; k < width_; ++k) 71 | row.push_back(k - shift_x); 72 | for (int k = 0; k < height_; ++k) 73 | col.push_back(k - shift_y); 74 | Mat Row(row), Col(col); 75 | repeat(Row.reshape(1, 1), Col.total(), 1, u); 76 | repeat(Col.reshape(1, 1).t(), 1, Row.total(), v); 77 | 78 | } 79 | 80 | // compute values needed for normal estimation only once 81 | void cache() { 82 | 83 | Eigen::Matrix K; 84 | K = K_.template cast(); 85 | 86 | const double fx_inv = 1. / K(0, 0); 87 | const double fy_inv = 1. / K(1, 1); 88 | const double cx = K(0, 2); 89 | const double cy = K(1, 2); 90 | 91 | Mat_ x0, y0, x0_sq, y0_sq, x0_y0, n_sq; 92 | Mat_ x0_n_sq_inv, y0_n_sq_inv, n_sq_inv; 93 | Mat_ M11, M12, M13, M22, M23, M33, det, det_inv; 94 | Mat_ Q11, Q12, Q13, Q22, Q23, Q33; 95 | 96 | pixelgrid(cx, cy, x0, y0); 97 | 98 | x0 = fx_inv * x0; 99 | x0_sq = x0.mul(x0); 100 | y0 = fy_inv * y0; 101 | y0_sq = y0.mul(y0); 102 | x0_y0 = x0.mul(y0); 103 | 104 | n_sq = 1. + x0_sq + y0_sq; 105 | divide(1., n_sq, n_sq_inv); 106 | x0_n_sq_inv = x0.mul(n_sq_inv); 107 | y0_n_sq_inv = y0.mul(n_sq_inv); 108 | 109 | boxFilter(x0_sq.mul(n_sq_inv), M11, -1, window_size_, Point(-1, -1), false); 110 | boxFilter(x0_y0.mul(n_sq_inv), M12, -1, window_size_, Point(-1, -1), false); 111 | boxFilter(x0_n_sq_inv, M13, -1, window_size_, Point(-1, -1), false); 112 | boxFilter(y0_sq.mul(n_sq_inv), M22, -1, window_size_, Point(-1, -1), false); 113 | boxFilter(y0_n_sq_inv, M23, -1, window_size_, Point(-1, -1), false); 114 | boxFilter(n_sq_inv, M33, -1, window_size_, Point(-1, -1), false); 115 | 116 | det = M11.mul(M22.mul(M33)) + 2 * M12.mul(M23.mul(M13)) - 117 | (M13.mul(M13.mul(M22)) + M12.mul(M12.mul(M33)) + M23.mul(M23.mul(M11))); 118 | divide(1., det, det_inv); 119 | 120 | Q11 = det_inv.mul(M22.mul(M33) - M23.mul(M23)); 121 | Q12 = det_inv.mul(M13.mul(M23) - M12.mul(M33)); 122 | Q13 = det_inv.mul(M12.mul(M23) - M13.mul(M22)); 123 | Q22 = det_inv.mul(M11.mul(M33) - M13.mul(M13)); 124 | Q23 = det_inv.mul(M12.mul(M13) - M11.mul(M23)); 125 | Q33 = det_inv.mul(M11.mul(M22) - M12.mul(M12)); 126 | 127 | // TODO: write in more concise way!! 128 | if (std::is_same::value) { 129 | x0.convertTo(x0_, CV_32F); 130 | y0.convertTo(y0_, CV_32F); 131 | x0_n_sq_inv.convertTo(x0_n_sq_inv_, CV_32F); 132 | y0_n_sq_inv.convertTo(y0_n_sq_inv_, CV_32F); 133 | n_sq_inv.convertTo(n_sq_inv_, CV_32F); 134 | Q11.convertTo(Q11_, CV_32F); 135 | Q12.convertTo(Q12_, CV_32F); 136 | Q13.convertTo(Q13_, CV_32F); 137 | Q22.convertTo(Q22_, CV_32F); 138 | Q23.convertTo(Q23_, CV_32F); 139 | Q33.convertTo(Q33_, CV_32F); 140 | } 141 | else { 142 | x0_ = x0; 143 | y0_ = y0; 144 | x0_n_sq_inv_ = x0_n_sq_inv; 145 | y0_n_sq_inv_ = y0_n_sq_inv; 146 | n_sq_inv_ = n_sq_inv; 147 | Q11_ = Q11; 148 | Q12_ = Q12; 149 | Q13_ = Q13; 150 | Q22_ = Q22; 151 | Q23_ = Q23; 152 | Q33_ = Q33; 153 | } 154 | } 155 | 156 | public: 157 | 158 | NormalEstimator(int width, int height, Eigen::Matrix K, Size window_size) : 159 | width_(width), 160 | height_(height), 161 | K_(K.cast()), 162 | window_size_(window_size) 163 | { 164 | cache(); 165 | } 166 | 167 | NormalEstimator(int width, int height, Eigen::Matrix K, Size window_size) : 168 | width_(width), 169 | height_(height), 170 | K_(K.cast()), 171 | window_size_(window_size) 172 | { 173 | cache(); 174 | } 175 | 176 | ~NormalEstimator() {} 177 | 178 | // compute normals 179 | void compute(const Mat &depth, Mat &nx, Mat &ny, Mat &nz) const { 180 | 181 | // workaround to only divide by depth where it is non-zero 182 | // not needed for OpenCV versions <4 183 | Mat_ tmp; 184 | divide(1., depth, tmp); 185 | Mat z_inv = Mat::zeros(tmp.size(), tmp.type()); 186 | Mat mask = (depth != 0); 187 | tmp.copyTo(z_inv, mask); 188 | 189 | Mat_ b1, b2, b3, norm_n; 190 | 191 | boxFilter(x0_n_sq_inv_.mul(z_inv), b1, -1, window_size_, Point(-1, -1), false); 192 | boxFilter(y0_n_sq_inv_.mul(z_inv), b2, -1, window_size_, Point(-1, -1), false); 193 | boxFilter(n_sq_inv_.mul(z_inv), b3, -1, window_size_, Point(-1, -1), false); 194 | 195 | nx = b1.mul(Q11_) + b2.mul(Q12_) + b3.mul(Q13_); 196 | ny = b1.mul(Q12_) + b2.mul(Q22_) + b3.mul(Q23_); 197 | nz = b1.mul(Q13_) + b2.mul(Q23_) + b3.mul(Q33_); 198 | 199 | sqrt(nx.mul(nx) + ny.mul(ny) + nz.mul(nz), norm_n); 200 | 201 | divide(nx, norm_n, nx); 202 | divide(ny, norm_n, ny); 203 | divide(nz, norm_n, nz); 204 | } 205 | 206 | Mat *x0_ptr() { return &x0_; } 207 | 208 | Mat *y0_ptr() { return &y0_; } 209 | 210 | Mat *n_sq_inv_ptr() { return &n_sq_inv_; } 211 | 212 | }; 213 | 214 | } // namespace cv 215 | 216 | #endif // CV_NORMAL_ESTIMATOR_H_ 217 | -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/ColorUpsampler.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #include "ColorUpsampler.h" 39 | #include "mesh/HrLayeredMarchingCubes.h" 40 | 41 | #include 42 | // #include 43 | 44 | using Vec8f = Vec8f; 45 | using Arr8f = Eigen::Array; 46 | 47 | // ========== non-class functions ========== 48 | 49 | //! from pose matrix to 6-DoF 50 | Eigen::Matrix MatrixTo7DoF(Mat4f& pose){ 51 | Eigen::Matrix xi; 52 | Vec3f t(pose.topRightCorner(3,1)); 53 | Eigen::Quaternion q(pose.topLeftCorner<3,3>()); 54 | xi << t[0], t[1], t[2], q.x(), q.y(), q.z(), q.w(); 55 | return xi; 56 | } 57 | 58 | template 59 | static void printVec(std::vector vec){ 60 | for (const auto& i: vec) 61 | std::cout << i << " "; 62 | } 63 | 64 | 65 | //! check for NaN values inside Eigen object 66 | template 67 | bool checkNan(S vector) 68 | { 69 | return vector.array().isNaN().sum(); 70 | } 71 | 72 | //! interpolation for RGB image 73 | static Vec3f interpolateImage(const float m, const float n, const cv::Mat& img) 74 | { 75 | int x = std::floor(m); 76 | int y = std::floor(n); 77 | cv::Vec3f tmp; 78 | if ((x+1) < img.rows && (y+1) < img.cols){ 79 | tmp = (y+1.0-n)*(m-x)*img.at(x+1,y) + (y+1.0-n)*(x+1.0-m)*img.at(x,y) + (n-y)*(m-x)*img.at(x+1,y+1) + (n-y)*(x+1.0-m)*img.at(x,y+1); 80 | } 81 | else if ((y+1) < img.cols && x >= img.rows){ 82 | tmp = (y+1.0-n)*img.at(x,y) + (n-y)*img.at(x,y+1); 83 | } 84 | else if ( y >= img.cols && (x+1) < img.rows){ 85 | tmp = (m-x)*img.at(x+1,y) + (x+1.0-m)*img.at(x,y); 86 | } 87 | else{ 88 | tmp = img.at(x,y); 89 | } 90 | 91 | Vec3f intensity(tmp[2],tmp[1],tmp[0]); // OpenCV stores image colors as BGR. 92 | return intensity; 93 | } 94 | 95 | 96 | //! corners of cube [-1, 1]^3 97 | Eigen::Matrix centeredCubeCorners() 98 | { 99 | Eigen::Matrix corners; 100 | 101 | corners.col(0) << -1.0, -1.0, -1.0; 102 | corners.col(1) << 1.0, -1.0, -1.0; 103 | corners.col(2) << -1.0, 1.0, -1.0; 104 | corners.col(3) << 1.0, 1.0, -1.0; 105 | corners.col(4) << -1.0, -1.0, 1.0; 106 | corners.col(5) << 1.0, -1.0, 1.0; 107 | corners.col(6) << -1.0, 1.0, 1.0; 108 | corners.col(7) << 1.0, 1.0, 1.0; 109 | 110 | return corners; 111 | } 112 | 113 | 114 | // ========== initialization ========== 115 | 116 | //! constructor 117 | ColorUpsampler::ColorUpsampler(const SdfLrMap& sdf_lr, 118 | phmap::parallel_flat_hash_map>& vis_map, 119 | std::vector>& images, 120 | std::vector>& poses, 121 | std::vector& frame_idx, 122 | const float voxel_size, 123 | const Mat3f& K) : 124 | images_(images), 125 | poses_(poses), 126 | frame_idx_(frame_idx), 127 | voxel_size_(voxel_size), 128 | voxel_size_inv_(1.f / voxel_size), 129 | K_(K) 130 | { 131 | num_frames_ = frame_idx_.size(); // does not work inside colon initializer 132 | init(sdf_lr, vis_map); 133 | // selectPoses(); 134 | } 135 | 136 | //! init 137 | void ColorUpsampler::init(const SdfLrMap& sdf_lr, phmap::parallel_flat_hash_map>& vis_map) 138 | { 139 | indices_.clear(); 140 | vis_.clear(); 141 | sdf_.clear(); 142 | // convert low-res voxels to high-res voxels 143 | const float voxel_diameter = std::sqrt(3.) * voxel_size_; 144 | for (const auto& voxel: sdf_lr) { 145 | if (std::fabs(voxel.second.dist) < voxel_diameter) { // only accept voxels close to surface 146 | sdf_.emplace(voxel.first, SdfVoxelHr(voxel.second, voxel_size_)); 147 | } 148 | } 149 | 150 | num_voxels_ = sdf_.size(); 151 | indices_.resize(num_voxels_); 152 | vis_.resize(num_voxels_); 153 | 154 | // store indices and visibility vectors in correct order (that of high-res voxel map) 155 | int count = 0; 156 | for (const auto& voxel: sdf_) { 157 | indices_[count] = voxel.first; 158 | vis_[count] = vis_map[voxel.first]; 159 | vis_[count].resize(frame_idx_.back()+1); 160 | ++count; 161 | } 162 | } 163 | 164 | 165 | // ========== helper functions for Jacobians ========== 166 | 167 | //! get I_i(R_iv+t) RGB/gray value for 8 subvoxels 168 | bool ColorUpsampler::getIntensity(const int frame, const Vec3i& idx, const Mat3f& R, const Vec3f& t, Mat3x8f& intensity) 169 | { 170 | const float fx = K_(0,0); 171 | const float fy = K_(1,1); 172 | const float cx = K_(0,2); 173 | const float cy = K_(1,2); 174 | 175 | SdfVoxelHr voxel(getSdf(idx)); 176 | 177 | 178 | Mat3x8f point = R.transpose()*(getSubvoxelFloat(idx) - voxel.grad * voxel.d.transpose() - t.replicate(1,8)); 179 | 180 | auto m = fx * point.row(0).array()/point.row(2).array() + cx; 181 | auto n = fy * point.row(1).array()/point.row(2).array() + cy; 182 | 183 | //DEBUG 184 | if (m.hasNaN()|| n.hasNaN()){ 185 | std::cout << "invalid pixel coordinate at voxel "<< idx.transpose() << " at frame " << frame << std::endl; 186 | return false; 187 | } 188 | 189 | const cv::Mat& img = getFrame(frame); 190 | 191 | // Mat3x8f intensity; 192 | for (size_t i = 0; i < 8; i++){ 193 | if (m(i)<0 || m(i)>=img.cols || n(i)<0 || n(i)>=img.rows) { 194 | 195 | return false; 196 | } 197 | else { 198 | intensity.col(i) = interpolateImage(n(i), m(i), img); 199 | } 200 | } 201 | 202 | return true; 203 | } 204 | 205 | 206 | 207 | //! coordinates of subvoxel centers 208 | Mat3x8f ColorUpsampler::getSubvoxelFloat(const Vec3i& voxel_in) 209 | { 210 | Vec3f voxel_float = voxel_in.cast(); 211 | return voxel_size_ * (.25 * centeredCubeCorners() + voxel_float.replicate<1,8>()); 212 | } 213 | 214 | // ========== set values for individual voxels ========== 215 | 216 | //! set albedo 217 | void ColorUpsampler::setAlbedo(const Vec3i& idx, const Vec8f& r, const Vec8f& g, const Vec8f& b) 218 | { 219 | Vec8f rr, gg, bb; 220 | rr = r; 221 | rr = rr.cwiseMax(0.0); 222 | rr = rr.cwiseMin(1.0); 223 | 224 | gg = g; 225 | gg = gg.cwiseMax(0.0); 226 | gg = gg.cwiseMin(1.0); 227 | 228 | bb = b; 229 | bb = bb.cwiseMax(0.0); 230 | bb = bb.cwiseMin(1.0); 231 | 232 | sdf_.at(idx).r = rr; 233 | sdf_.at(idx).g = gg; 234 | sdf_.at(idx).b = bb; 235 | 236 | } 237 | 238 | 239 | //! extract mesh from SDF to debug geometry 240 | bool ColorUpsampler::extractMesh(std::string filename) 241 | { 242 | HrLayeredMarchingCubes lmc(Vec3f(voxel_size_, voxel_size_, voxel_size_)); 243 | lmc.computeIsoSurface(&sdf_); 244 | bool success = lmc.savePly(filename + ".ply"); 245 | if (success) 246 | std::cout << "Mesh " << filename << ".ply successfully saved." << std::endl; 247 | 248 | return success; 249 | } 250 | 251 | bool ColorUpsampler::extractCloud(std::string filename) 252 | { 253 | 254 | const float voxel_size_4 = .25 * voxel_size_; 255 | filename += ".ply"; 256 | 257 | int voxel_id = 0; 258 | std::vector> points_normals_colors; 259 | for (const auto& el : sdf_) { 260 | 261 | std::vector vis = vis_[voxel_id]; 262 | bool visible = false; 263 | for (size_t i = 0; i < num_frames_; i++){ 264 | if (vis[frame_idx_[i]]){ 265 | visible = true; 266 | break; 267 | } 268 | } 269 | 270 | if(!visible){ 271 | voxel_id++; 272 | continue; 273 | } 274 | 275 | const SdfVoxelHr& v = el.second; 276 | if (v.weight < 5){ 277 | voxel_id++; 278 | continue; 279 | } 280 | 281 | Mat3x8f voxel_normal = -v.grad.normalized().replicate(1,8); 282 | Mat3x8f voxel_float = getSubvoxelFloat(el.first); 283 | Mat3x8f distances = voxel_normal * v.d.asDiagonal(); 284 | for (int i=0; i<8; ++i) 285 | { 286 | Vec3f d = distances.col(i); 287 | if (std::fabs(d[0]) < voxel_size_4 && std::fabs(d[1]) < voxel_size_4 && std::fabs(d[2]) < voxel_size_4 && !std::isnan(v.r[i]) && !std::isnan(v.g[i]) && !std::isnan(v.b[i])) 288 | { 289 | Eigen::Matrix pnc; 290 | pnc.segment<3>(0) = voxel_float.col(i) + d; 291 | pnc.segment<3>(3) = voxel_normal.col(i); 292 | pnc.segment<3>(6) = Vec3f(v.r[i], v.g[i], v.b[i]); 293 | points_normals_colors.push_back(pnc); 294 | } 295 | } 296 | voxel_id++; 297 | } 298 | 299 | std::ofstream plyFile; 300 | plyFile.open(filename.c_str()); 301 | if (!plyFile.is_open()) 302 | return false; 303 | 304 | plyFile << "ply" << std::endl; 305 | plyFile << "format ascii 1.0" << std::endl; 306 | plyFile << "element vertex " << points_normals_colors.size() << std::endl; 307 | plyFile << "property float x" << std::endl; 308 | plyFile << "property float y" << std::endl; 309 | plyFile << "property float z" << std::endl; 310 | plyFile << "property float nx" << std::endl; 311 | plyFile << "property float ny" << std::endl; 312 | plyFile << "property float nz" << std::endl; 313 | plyFile << "property uchar red" << std::endl; 314 | plyFile << "property uchar green" << std::endl; 315 | plyFile << "property uchar blue" << std::endl; 316 | plyFile << "end_header" << std::endl; 317 | 318 | for (const Eigen::Matrix& p : points_normals_colors) 319 | { 320 | plyFile << p[0] << " " << p[1] << " " << p[2] << " " << p[3] << " " << p[4] << " " << p[5] << " " << int(255 * p[6]) << " " << int(255 * p[7]) << " " << int(255 * p[8]) << std::endl; 321 | } 322 | 323 | plyFile.close(); 324 | 325 | std::cout << "Cloud " << filename << " successfully saved." << std::endl; 326 | return true; 327 | } 328 | 329 | 330 | // ------------------- solvers ----------------------------------------------------------------------------------------------------------------- 331 | 332 | 333 | //! compute albedo from ambient light assumption, i.e. average over all observations 334 | void ColorUpsampler::computeColor() 335 | { 336 | // size_t num_tot_frames = getVis(0).size(); 337 | 338 | size_t count = 0; // counter (same for all three color channels) 339 | Vec8f br, bb, bg; 340 | int voxel_id = 0; 341 | 342 | for (const auto& idx : indices_) { 343 | 344 | br.setZero(); 345 | bb.setZero(); 346 | bg.setZero(); 347 | 348 | std::vector vis = getVis(voxel_id); 349 | SdfVoxelHr voxel(getSdf(idx)); 350 | 351 | for(size_t i = 0; i < num_frames_; i++){ 352 | Mat3f R = getRotation(i); 353 | Vec3f t = getTranslation(i); 354 | 355 | if(vis[frame_idx_[i]]) { 356 | // r(v) = \sum_i (I(pi(Rv+t)) - pho(v)()) 357 | Mat3x8f intensity; 358 | if(!getIntensity(i, idx, R, t, intensity)){ 359 | continue; 360 | } 361 | br += intensity.row(0); 362 | bg += intensity.row(1); 363 | bb += intensity.row(2); 364 | ++count; 365 | } 366 | } 367 | 368 | Vec8f r,g,b; 369 | float inv_count = 1.f / float(count); 370 | r = inv_count * br; 371 | g = inv_count * bg; 372 | b = inv_count * bb; 373 | setAlbedo(idx, r, g, b); // cut-off happens here 374 | count = 0; 375 | voxel_id++; 376 | } 377 | } 378 | 379 | -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/ColorUpsampler.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef COLOR_UPSAMPLER_H_ 39 | #define COLOR_UPSAMPLER_H_ 40 | 41 | #include "mat.h" 42 | #include "sdf_voxel/SdfVoxel.h" 43 | 44 | #include 45 | #include 46 | #include "loss.h" 47 | 48 | using Mat3x8f = Eigen::Matrix; 49 | using Mat8f = Eigen::Matrix; 50 | 51 | class ColorUpsampler { 52 | 53 | // ========== members ========== 54 | 55 | // ..... problem properties ..... 56 | 57 | size_t num_frames_; 58 | size_t num_voxels_; 59 | const float voxel_size_; 60 | const float voxel_size_inv_; 61 | Mat3f K_; // camera intrinsics 62 | 63 | std::vector frame_idx_; // indices of keyframes to be taken into account 64 | std::vector indices_; // 3D integer indices of voxels, stored in vector 65 | std::vector> vis_; // visibility per voxel and frame 66 | 67 | std::vector> images_; // vector of pointers to keyframes 68 | 69 | // ..... variables to be optimized ..... 70 | 71 | std::vector > poses_; 72 | 73 | SdfHrMap sdf_; // SDF map 74 | 75 | // ========== private member functions ========== 76 | 77 | // ..... initialization internals ..... 78 | 79 | void init(const SdfLrMap& sdf_lr, phmap::parallel_flat_hash_map>& vis_map); 80 | 81 | bool getIntensity(const int frame, const Vec3i& voxel, const Mat3f& R, const Vec3f& t, Mat3x8f& intensity); 82 | Mat3x8f getSubvoxelFloat(const Vec3i& voxel_in); 83 | 84 | void setAlbedo(const Vec3i& idx, const Vec8f& r, const Vec8f& g, const Vec8f& b); 85 | 86 | public: 87 | 88 | // ========== constructors ========== 89 | 90 | ColorUpsampler(const SdfLrMap& sdf_lr, 91 | phmap::parallel_flat_hash_map>& vis_map, 92 | std::vector>& images, 93 | std::vector>& poses, 94 | std::vector& frame_idx, 95 | const float voxel_size, 96 | const Mat3f& K); 97 | 98 | 99 | // ========== public member functions ========== 100 | 101 | // ..... inline get functions ..... 102 | 103 | size_t getFrameNumber() const 104 | { 105 | return num_frames_; 106 | } 107 | 108 | size_t getVoxelNumber() const 109 | { 110 | return num_voxels_; 111 | } 112 | 113 | Eigen::Matrix3f getRotation(int frame_id) const 114 | { 115 | return poses_[frame_id].topLeftCorner(3,3); 116 | } 117 | 118 | Vec3f getTranslation(int frame_id) const 119 | { 120 | return poses_[frame_id].topRightCorner(3,1); 121 | } 122 | 123 | cv::Mat getFrame(const int frame) const 124 | { 125 | return *(images_[frame]); 126 | } 127 | 128 | std::vector getVis(int voxel_id) const 129 | { 130 | return vis_[voxel_id]; 131 | } 132 | 133 | SdfVoxelHr getSdf(const Vec3i& voxel) const 134 | { 135 | SdfVoxelHr v(sdf_.at(voxel)); 136 | return v; 137 | } 138 | 139 | 140 | void computeColor(); 141 | bool extractMesh(std::string filename); 142 | bool extractCloud(std::string filename); 143 | 144 | }; 145 | 146 | #endif // COLOR_UPSAMPLER_H_ -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/PhotometricOptimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef PHOTOMETRIC_OPTIMIZER_H_ 39 | #define PHOTOMETRIC_OPTIMIZER_H_ 40 | 41 | #include "mat.h" 42 | #include "sdf_voxel/SdfVoxel.h" 43 | #include "sdf_tracker/MapGradPixelSdf.h" 44 | 45 | #include 46 | #include 47 | #include "loss.h" 48 | 49 | 50 | struct OptSettings { 51 | int max_it; // maximum number of iterations 52 | float conv_threshold; // threshold for convergence 53 | float damping; // LM damping term (often referred to as lambda) 54 | float lambda; // lambda for weight function (not for LM!) 55 | float lambda_sq; 56 | float reg_weight; 57 | LossFunction loss; // type of (robust) loss 58 | OptSettings() : 59 | max_it(25), 60 | conv_threshold(1e-4), 61 | damping(1.0), 62 | lambda(0.5), 63 | lambda_sq(lambda * lambda), 64 | reg_weight(10.0), 65 | loss(LossFunction::CAUCHY) 66 | {} 67 | }; 68 | 69 | class PhotometricOptimizer { 70 | 71 | // ========== members ========== 72 | 73 | // ..... problem properties ..... 74 | 75 | size_t num_voxels_; 76 | const float voxel_size_; 77 | const float voxel_size_inv_; 78 | Mat3f K_; // camera intrinsics 79 | std::string save_path_; 80 | 81 | 82 | OptSettings settings_; 83 | 84 | // ..... variables to be optimized ..... 85 | 86 | MapGradPixelSdf* tSDF_; 87 | std::vector frame_idx_; // indices of keyframes to be taken into account 88 | std::vector> images_; // vector of pointers to keyframes 89 | std::vector > poses_; // poses from tracking part 90 | std::vector key_stamps_; 91 | 92 | // ========== private member functions ========== 93 | 94 | // ..... initialization internals ..... 95 | 96 | void init(const SdfLrMap& sdf_lr, phmap::parallel_flat_hash_map>& vis_map); 97 | 98 | // ..... pose related ....... 99 | Mat3f getRotation(const int frame_id) 100 | { 101 | return poses_[frame_id].topLeftCorner(3,3); 102 | } 103 | 104 | Vec3f getTranslation(const int frame_id) 105 | { 106 | return poses_[frame_id].topRightCorner(3,1); 107 | } 108 | 109 | cv::Mat getImage(const int frame_id) const 110 | { 111 | return *(images_[frame_id]); 112 | } 113 | 114 | // ..... Jacobian computations ..... 115 | 116 | // distance Jacobian 117 | bool computeJdOneFrame(const Vec3i& idx, const SdfVoxel& voxel, const Mat3f& R, const Vec3f& t, const cv::Mat& img, Vec3f& Jd); 118 | // camera pose Jacobian 119 | bool computeJc(const Vec3i& idx, const SdfVoxel& voxel, const cv::Mat& img, const Mat3f& R, const Vec3f& t, Eigen::Matrix& Jc); 120 | 121 | // ..... helper functions for Jacobians ..... 122 | 123 | bool getIntensity(const cv::Mat& img, const Vec3i& voxel, const Mat3f& R, const Vec3f& t, Vec3f& intensity); 124 | 125 | // ..... set values for individual voxels ..... 126 | 127 | void updateDist(const Vec3i& idx, const float delta_d); 128 | 129 | public: 130 | 131 | // ========== constructors ========== 132 | 133 | PhotometricOptimizer(MapGradPixelSdf* tSDF, 134 | const float voxel_size, 135 | const Mat3f& K, 136 | std::string save_path, 137 | OptSettings settings = OptSettings()); 138 | 139 | 140 | // ========== public member functions ========== 141 | 142 | // .... initialization functions ..... 143 | void setImages(std::vector> images) 144 | { 145 | images_ = images; 146 | } 147 | 148 | void setPoses(std::vector >& pose) 149 | { 150 | poses_ = pose; 151 | } 152 | 153 | void setKeyframes( std::vector& keyframes) 154 | { 155 | frame_idx_ = keyframes; 156 | } 157 | void setKeytimestamps(std::vector& keystamps) 158 | { 159 | key_stamps_ = keystamps; 160 | } 161 | 162 | // ..... inline get functions ..... 163 | 164 | size_t getVoxelNumber() const 165 | { 166 | return num_voxels_; 167 | } 168 | 169 | SdfVoxel getSdf(const Vec3i& voxel) const 170 | { 171 | SdfVoxel v(tSDF_->tsdf_.at(voxel)); 172 | return v; 173 | } 174 | 175 | // ..... optimization and energy computation ..... 176 | 177 | float getEnergy(); 178 | void solveDist(float damping = 1.0); 179 | void solvePose(float damping = 1.0); 180 | void solvePoseFull(float damping = 1.0); 181 | 182 | bool optimize(); 183 | 184 | // ..... debugging ..... 185 | 186 | bool savePoses(std::string filename); 187 | }; 188 | 189 | #endif // PHOTOMETRIC_OPTIMIZER_H_ -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/SharpDetector.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef SHARP_DETECTOR_H_ 39 | #define SHARP_DETECTOR_H_ 40 | 41 | #include 42 | 43 | float modifiedLaplacian(const cv::Mat& src); 44 | 45 | bool sharpDetector(const cv::Mat& img, float threshold){ 46 | float measure = modifiedLaplacian(img); 47 | std::cout << "the sharpness measure is " << measure << "." << std::endl; 48 | if( measure < threshold) 49 | return false; 50 | return true; 51 | } 52 | 53 | 54 | // OpenCV port of 'LAPM' algorithm (Nayar89) 55 | float modifiedLaplacian(const cv::Mat& src) 56 | { 57 | cv::Mat M = (cv::Mat_(3, 1) << -1, 2, -1); 58 | cv::Mat G = cv::getGaussianKernel(3, -1, CV_32F); 59 | 60 | cv::Mat Lx; 61 | cv::sepFilter2D(src, Lx, CV_32F, M, G); 62 | 63 | cv::Mat Ly; 64 | cv::sepFilter2D(src, Ly, CV_32F, G, M); 65 | 66 | cv::Mat FM = cv::abs(Lx) + cv::abs(Ly); 67 | 68 | float focusMeasure = cv::mean(FM).val[0]; 69 | return focusMeasure; 70 | } 71 | 72 | #endif // SHARP_DETECTOR_H_ -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/loss.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef LOSS_H_ 39 | #define LOSS_H_ 40 | 41 | enum LossFunction 42 | { 43 | L2 = 0, 44 | CAUCHY = 1, 45 | HUBER = 2, 46 | TUKEY = 3, 47 | TRUNC_L2 = 4 48 | }; 49 | 50 | #endif //LOSS_H_ -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/MapGradPixelSdf.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #include "MapGradPixelSdf.h" 39 | #include "normals/NormalEstimator.h" 40 | #include "mesh/LayeredMarchingCubesNoColor.h" 41 | #include 42 | 43 | void MapGradPixelSdf::update(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst) { 44 | 45 | const float fx = K(0,0), fy = K(1,1); 46 | const float cx = K(0,2), cy = K(1,2); 47 | 48 | const float z_min = z_min_, z_max = z_max_; 49 | 50 | size_t lin_index = 0; 51 | 52 | cv::Mat nx, ny, nz, med_depth; 53 | cv::medianBlur(depth, med_depth, 5); // median filtering 54 | 55 | if (!NEst) { 56 | std::cerr << "No normal estimation possible - cannot update SDF volume!" << std::endl; 57 | return; 58 | } 59 | 60 | NEst->compute(depth, nx, ny, nz); 61 | 62 | const Mat3f R = pose.rotationMatrix(); 63 | const Mat3f Rt = pose.rotationMatrix().transpose(); 64 | const Vec3f t = pose.translation(); 65 | 66 | cv::Mat* x0_ptr = NEst->x0_ptr(); 67 | cv::Mat* y0_ptr = NEst->y0_ptr(); 68 | cv::Mat* n_sq_inv_ptr = NEst->n_sq_inv_ptr(); 69 | const float* x_hom_ptr = (const float*)x0_ptr->data; 70 | const float* y_hom_ptr = (const float*)y0_ptr->data; 71 | const float* hom_inv_ptr = (const float*)n_sq_inv_ptr->data; 72 | const float* z_ptr = (const float*)depth.data; 73 | const float* zm_ptr = (const float*)med_depth.data; 74 | 75 | const float* nx_ptr = (const float*)nx.data; 76 | const float* ny_ptr = (const float*)ny.data; 77 | const float* nz_ptr = (const float*)nz.data; 78 | 79 | const int factor = std::floor(T_ / voxel_size_); 80 | 81 | for (size_t m=0; m < depth.rows; ++m) for (size_t n=0; n < depth.cols; ++n) { 82 | 83 | const size_t idx = m * depth.cols + n; 84 | 85 | const float z = z_ptr[idx]; 86 | 87 | if (z <= z_min || z >= z_max ) // z out of range or unreliable z 88 | continue; 89 | 90 | const Vec3f xy_hom(x_hom_ptr[idx], y_hom_ptr[idx], 1.); 91 | const Vec3f R_xy_hom(R * xy_hom); 92 | const Vec3f normal(nx_ptr[idx], ny_ptr[idx], nz_ptr[idx]); 93 | const Vec3f Rn(R * normal); 94 | 95 | if (normal.squaredNorm() < .1) // invalid normal 96 | continue; 97 | 98 | if (normal.dot(xy_hom) * normal.dot(xy_hom) * hom_inv_ptr[idx] < .25) // normal direction too far from viewing ray direction (>72.5°) 99 | continue; 100 | 101 | for (float k = -factor; k <= factor; ++k) { // loop along ray 102 | 103 | Vec3f point = (z + k*voxel_size_) * R_xy_hom + t; // convert point into Sdf coordinate system 104 | const Vec3i vi = float2vox(point); 105 | point = Rt * (vox2float(vi) - t); 106 | const float sdf = point[2] - z; 107 | const float w = weight(sdf); 108 | if (w>0) { 109 | SdfVoxel& v = tsdf_[vi]; 110 | v.weight += w; 111 | v.dist += (truncate(sdf) - v.dist) * w / v.weight; 112 | v.grad += w * Rn; // normals are inward-pointing! 113 | std::vector& vis = vis_[vi]; 114 | vis.resize(counter_); 115 | vis.push_back(true); 116 | } 117 | } 118 | } 119 | 120 | increase_counter(); 121 | std::cout << "Current frame counter: " << counter_ << std::endl; // DEBUG 122 | } 123 | 124 | bool MapGradPixelSdf::extract_mesh(std::string filename) { 125 | 126 | // compute dimensions (and, from that, size) 127 | const int pos_inf = std::numeric_limits::max(); 128 | const int neg_inf = std::numeric_limits::min(); 129 | int xmin, xmax, ymin, ymax, zmin, zmax; 130 | xmin = pos_inf; 131 | xmax = neg_inf; 132 | ymin = pos_inf; 133 | ymax = neg_inf; 134 | zmin = pos_inf; 135 | zmax = neg_inf; 136 | for (auto v : tsdf_) { 137 | if (v.first[0] < xmin) xmin = v.first[0]; 138 | if (v.first[0] > xmax) xmax = v.first[0]; 139 | if (v.first[1] < ymin) ymin = v.first[1]; 140 | if (v.first[1] > ymax) ymax = v.first[1]; 141 | if (v.first[2] < zmin) zmin = v.first[2]; 142 | if (v.first[2] > zmax) zmax = v.first[2]; 143 | } 144 | 145 | // create input that can handle MarchingCubes class 146 | const Vec3i dim(xmax - xmin + 1, ymax - ymin + 1, zmax - zmin + 1); 147 | const size_t num_voxels = dim[0] * dim[1] * dim[2]; 148 | float* dist = new float[num_voxels]; 149 | float* weights = new float[num_voxels]; 150 | size_t lin_index = 0; 151 | for (int k=zmin; k<=zmax; ++k) for (int j=ymin; j<=ymax; ++j) for (int i=xmin; i<=xmax; ++i) { 152 | Vec3i idx(i, j, k); 153 | auto pair = tsdf_.find(idx); 154 | if (pair != tsdf_.end()) { 155 | dist[lin_index] = pair->second.dist; 156 | weights[lin_index] = pair->second.weight; 157 | } 158 | else { 159 | dist[lin_index] = T_; 160 | weights[lin_index] = 0; 161 | } 162 | ++lin_index; 163 | } 164 | 165 | // call marching cubes 166 | LayeredMarchingCubesNoColor lmc(Vec3f(voxel_size_, voxel_size_, voxel_size_)); 167 | lmc.computeIsoSurface(&tsdf_); 168 | bool success = lmc.savePly(filename); 169 | 170 | // delete temporary arrays 171 | delete[] dist; 172 | delete[] weights; 173 | 174 | return success; 175 | } 176 | 177 | bool MapGradPixelSdf::extract_pc(std::string filename) { 178 | 179 | const float voxel_size_2 = .5 * voxel_size_; 180 | 181 | std::vector points_normals; 182 | for (const auto& el : tsdf_) { 183 | const SdfVoxel& v = el.second; 184 | if (v.weight < 5) 185 | continue; 186 | Vec3f g = 1.2*v.grad.normalized(); 187 | Vec3f d = v.dist * g; 188 | if (std::fabs(d[0]) < voxel_size_2 && std::fabs(d[1]) < voxel_size_2 && std::fabs(d[2]) < voxel_size_2) 189 | { 190 | Vec6f pn; 191 | pn.segment<3>(0) = vox2float(el.first) - d; 192 | pn.segment<3>(3) = -g; 193 | points_normals.push_back(pn); 194 | } 195 | } 196 | 197 | std::ofstream plyFile; 198 | plyFile.open(filename.c_str()); 199 | if (!plyFile.is_open()) 200 | return false; 201 | 202 | plyFile << "ply" << std::endl; 203 | plyFile << "format ascii 1.0" << std::endl; 204 | plyFile << "element vertex " << points_normals.size() << std::endl; 205 | plyFile << "property float x" << std::endl; 206 | plyFile << "property float y" << std::endl; 207 | plyFile << "property float z" << std::endl; 208 | plyFile << "property float nx" << std::endl; 209 | plyFile << "property float ny" << std::endl; 210 | plyFile << "property float nz" << std::endl; 211 | plyFile << "end_header" << std::endl; 212 | 213 | for (const Vec6f& p : points_normals) { 214 | plyFile << p[0] << " " << p[1] << " " << p[2] << " " << p[3] << " " << p[4] << " " << p[5] << std::endl; 215 | } 216 | 217 | plyFile.close(); 218 | 219 | return true; 220 | } 221 | 222 | bool MapGradPixelSdf::save_sdf(std::string filename) 223 | { 224 | // compute dimensions (and, from that, size) 225 | const int pos_inf = std::numeric_limits::max(); 226 | const int neg_inf = std::numeric_limits::min(); 227 | int xmin, xmax, ymin, ymax, zmin, zmax; 228 | xmin = pos_inf; 229 | xmax = neg_inf; 230 | ymin = pos_inf; 231 | ymax = neg_inf; 232 | zmin = pos_inf; 233 | zmax = neg_inf; 234 | for (auto v : tsdf_) { 235 | if (v.first[0] < xmin) xmin = v.first[0]; 236 | if (v.first[0] > xmax) xmax = v.first[0]; 237 | if (v.first[1] < ymin) ymin = v.first[1]; 238 | if (v.first[1] > ymax) ymax = v.first[1]; 239 | if (v.first[2] < zmin) zmin = v.first[2]; 240 | if (v.first[2] > zmax) zmax = v.first[2]; 241 | } 242 | 243 | 244 | // create input that can handle MarchingCubes class 245 | const Vec3i dim(xmax - xmin + 1, ymax - ymin + 1, zmax - zmin + 1); 246 | const size_t num_voxels = dim[0] * dim[1] * dim[2]; 247 | 248 | // --------------save grid info ---------------------------- 249 | std::ofstream grid_file; 250 | grid_file.open((filename + "_grid_info.txt").c_str()); 251 | if (!grid_file.is_open()){ 252 | std::cerr << "couldn't save grid_info file!" << std::endl; 253 | return false; 254 | } 255 | grid_file << "voxel size: " << voxel_size_ << std::endl 256 | << "voxel dim: " << dim[0] << " " << dim[1] << " " << dim[2] << std::endl 257 | << "voxel min: " << xmin << " " << ymin << " " << zmin << std::endl 258 | << "voxel max: " << xmax << " " << ymax << " " << zmax << std::endl; 259 | grid_file.close(); 260 | 261 | // ---------------- save sdf dist ------------------------------- 262 | std::ofstream file; 263 | std::ofstream weight_file; 264 | file.open((filename + "_sdf_d.txt").c_str()); 265 | weight_file.open((filename + "_sdf_weight.txt").c_str()); 266 | 267 | // -------------- save sdf grad ------------ 268 | std::ofstream sdf_n0, sdf_n1, sdf_n2; 269 | sdf_n0.open((filename + "_sdf_n0.txt").c_str()); 270 | sdf_n1.open((filename + "_sdf_n1.txt").c_str()); 271 | sdf_n2.open((filename + "_sdf_n2.txt").c_str()); 272 | 273 | if (!file.is_open() || !weight_file.is_open() || !sdf_n0.is_open() || !sdf_n1.is_open() || !sdf_n2.is_open()){ 274 | std::cerr << "couldn't save sdf or sdf weight file!" << std::endl; 275 | return false; 276 | } 277 | 278 | for(const auto& pair : tsdf_){ 279 | Vec3i idx = pair.first; 280 | const SdfVoxel& v = pair.second; 281 | int lin_idx = dim[0]*dim[1]*(idx[2]-zmin) + dim[0]*(idx[1]-ymin) + idx[0]-xmin; 282 | file << lin_idx << " " << v.dist << "\n"; 283 | weight_file << lin_idx << " " << v.weight << "\n"; 284 | sdf_n0 << lin_idx << " " << v.grad[0] << "\n"; 285 | sdf_n1 << lin_idx << " " << v.grad[1] << "\n"; 286 | sdf_n2 << lin_idx << " " << v.grad[2] << "\n"; 287 | 288 | } 289 | 290 | file.close(); 291 | weight_file.close(); 292 | sdf_n0.close(); 293 | sdf_n1.close(); 294 | sdf_n2.close(); 295 | return true; 296 | } 297 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/MapGradPixelSdf.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef MAP_GRAD_PIXEL_SDF_H_ 39 | #define MAP_GRAD_PIXEL_SDF_H_ 40 | 41 | // includes 42 | #include 43 | #include "mat.h" 44 | #include 45 | // class includes 46 | #include "sdf_voxel/SdfVoxel.h" 47 | #include "Sdf.h" 48 | #include "hash_map.h" 49 | 50 | /** 51 | * class declaration 52 | */ 53 | class MapGradPixelSdf : public Sdf { 54 | 55 | // friends 56 | 57 | friend class PhotometricOptimizer; 58 | 59 | // variables 60 | 61 | const float voxel_size_; 62 | const float voxel_size_inv_; 63 | 64 | // phmap::parallel_flat_hash_map tsdf_; 65 | phmap::parallel_node_hash_map, 67 | phmap::priv::hash_default_eq, 68 | Eigen::aligned_allocator>> tsdf_; 69 | 70 | phmap::parallel_flat_hash_map> vis_; 71 | 72 | // methods 73 | 74 | Vec3i float2vox(Vec3f point) const { 75 | Vec3f pv = voxel_size_inv_ * point; 76 | return Vec3i(std::round(pv[0]), std::round(pv[1]), std::round(pv[2])); 77 | } 78 | 79 | Vec3f vox2float(Vec3i idx) const { 80 | return voxel_size_ * idx.cast(); 81 | } 82 | 83 | public: 84 | 85 | // constructors / destructor 86 | 87 | MapGradPixelSdf() : 88 | Sdf(), 89 | voxel_size_(0.02), 90 | voxel_size_inv_(1./voxel_size_) 91 | {} 92 | 93 | MapGradPixelSdf(float voxel_size) : 94 | Sdf(), 95 | voxel_size_(voxel_size), 96 | voxel_size_inv_(1./voxel_size_) 97 | {} 98 | 99 | MapGradPixelSdf(float voxel_size, float T) : 100 | Sdf(T), 101 | voxel_size_(voxel_size), 102 | voxel_size_inv_(1./voxel_size_) 103 | {} 104 | 105 | ~MapGradPixelSdf() {} 106 | 107 | // methods 108 | 109 | virtual float tsdf(Vec3f point, Vec3f* grad_ptr) const { 110 | const Vec3i idx = float2vox(point); 111 | const SdfVoxel& v = tsdf_.at(idx); // at performs bound checking, which is not necessary, but otherwise tsdf_ cannot be const 112 | if (grad_ptr) 113 | (*grad_ptr) = 1.2*v.grad.normalized(); // factor 1.2 corrects for SDF scaling due to projectiveness (heuristic) 114 | return v.dist + 1.2*v.grad.normalized().dot(vox2float(idx) - point); 115 | } 116 | 117 | virtual float weights(Vec3f point) const { 118 | const Vec3i idx = float2vox(point); 119 | auto pair = tsdf_.find(idx); 120 | if (pair != tsdf_.end()){ 121 | // std::cout << idx << std::endl; 122 | return pair->second.weight; 123 | } 124 | return 0; 125 | } 126 | 127 | SdfVoxel getSdf(Vec3i idx) { 128 | return tsdf_.at(idx); 129 | } 130 | 131 | virtual void update(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst); 132 | 133 | phmap::parallel_node_hash_map, 135 | phmap::priv::hash_default_eq, 136 | Eigen::aligned_allocator>> get_tsdf() const { 137 | return tsdf_; 138 | } 139 | 140 | phmap::parallel_flat_hash_map>& get_vis() { 141 | return vis_; 142 | } 143 | 144 | // visualization / debugging 145 | 146 | virtual bool extract_mesh(std::string filename); 147 | 148 | virtual bool extract_pc(std::string filename); 149 | 150 | virtual bool save_sdf(std::string filename); 151 | 152 | }; 153 | 154 | #endif // MAP_GRAD_PIXEL_SDF_H_ 155 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/MapGradPixelSdfOmp.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #include "MapGradPixelSdf.h" 39 | #include "normals/NormalEstimator.h" 40 | #include "mesh/LayeredMarchingCubesNoColor.h" 41 | #include 42 | #include 43 | 44 | void MapGradPixelSdf::update(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst) { 45 | 46 | const float fx = K(0,0), fy = K(1,1); 47 | const float cx = K(0,2), cy = K(1,2); 48 | 49 | const float z_min = 0.5, z_max = 3.5; 50 | 51 | cv::Mat nx, ny, nz, med_depth; 52 | cv::medianBlur(depth, med_depth, 5); // median filtering 53 | 54 | if (!NEst) { 55 | std::cerr << "No normal estimation possible - cannot update SDF volume!" << std::endl; 56 | return; 57 | } 58 | 59 | NEst->compute(depth, nx, ny, nz); 60 | 61 | const Mat3f R = pose.rotationMatrix(); 62 | const Mat3f Rt = pose.rotationMatrix().transpose(); 63 | const Vec3f t = pose.translation(); 64 | 65 | cv::Mat* x0_ptr = NEst->x0_ptr(); 66 | cv::Mat* y0_ptr = NEst->y0_ptr(); 67 | cv::Mat* n_sq_inv_ptr = NEst->n_sq_inv_ptr(); 68 | const float* x_hom_ptr = (const float*)x0_ptr->data; 69 | const float* y_hom_ptr = (const float*)y0_ptr->data; 70 | const float* hom_inv_ptr = (const float*)n_sq_inv_ptr->data; 71 | const float* z_ptr = (const float*)depth.data; 72 | const float* zm_ptr = (const float*)med_depth.data; 73 | 74 | const float* nx_ptr = (const float*)nx.data; 75 | const float* ny_ptr = (const float*)ny.data; 76 | const float* nz_ptr = (const float*)nz.data; 77 | 78 | const int factor = std::floor(T_ / voxel_size_); 79 | 80 | size_t counter = 0; 81 | 82 | # pragma omp parallel for reduction(+: counter) 83 | for (size_t m=0; m < depth.rows; ++m) for (size_t n=0; n < depth.cols; ++n) { 84 | 85 | const size_t idx = m * depth.cols + n; 86 | 87 | const float z = z_ptr[idx]; 88 | 89 | if (z <= z_min || z >= z_max) // z out of range or unreliable z 90 | continue; 91 | 92 | const Vec3f xy_hom(x_hom_ptr[idx], y_hom_ptr[idx], 1.); 93 | const Vec3f R_xy_hom(R * xy_hom); 94 | const Vec3f normal(nx_ptr[idx], ny_ptr[idx], nz_ptr[idx]); 95 | const Vec3f Rn(R * normal); 96 | 97 | if (normal.squaredNorm() < .1) // invalid normal 98 | continue; 99 | 100 | if (normal.dot(xy_hom) * normal.dot(xy_hom) * hom_inv_ptr[idx] < .25) // normal direction too far from viewing ray direction (>72.5°) 101 | continue; 102 | 103 | for (float k = -factor; k <= factor; ++k) { // loop along ray 104 | 105 | Vec3f point = (z + k*voxel_size_) * R_xy_hom + t; // convert point into Sdf coordinate system 106 | const Vec3i vi = float2vox(point); 107 | point = Rt * (vox2float(vi) - t); 108 | const float sdf = point[2] - z; 109 | 110 | const float w = weight(sdf); 111 | if (w>0) { 112 | #pragma omp critical 113 | { 114 | SdfVoxel& v = tsdf_[vi]; 115 | v.weight += w; 116 | v.dist += (truncate(sdf) - v.dist) * w / v.weight; 117 | v.grad += w * Rn; // normals are inward-pointing! 118 | std::vector& vis = vis_[vi]; 119 | vis.resize(counter_); 120 | vis.push_back(true); 121 | } 122 | } 123 | } 124 | 125 | ++counter; 126 | 127 | } 128 | increase_counter(); 129 | std::cout << counter << " points integrated into tSDF volume!" << std::endl; 130 | } 131 | 132 | bool MapGradPixelSdf::extract_mesh(std::string filename) { 133 | 134 | // compute dimensions (and, from that, size) 135 | const int pos_inf = std::numeric_limits::max(); 136 | const int neg_inf = std::numeric_limits::min(); 137 | int xmin, xmax, ymin, ymax, zmin, zmax; 138 | xmin = pos_inf; 139 | xmax = neg_inf; 140 | ymin = pos_inf; 141 | ymax = neg_inf; 142 | zmin = pos_inf; 143 | zmax = neg_inf; 144 | for (auto v : tsdf_) { 145 | if (v.first[0] < xmin) xmin = v.first[0]; 146 | if (v.first[0] > xmax) xmax = v.first[0]; 147 | if (v.first[1] < ymin) ymin = v.first[1]; 148 | if (v.first[1] > ymax) ymax = v.first[1]; 149 | if (v.first[2] < zmin) zmin = v.first[2]; 150 | if (v.first[2] > zmax) zmax = v.first[2]; 151 | } 152 | 153 | 154 | // create input that can handle MarchingCubes class 155 | const Vec3i dim(xmax - xmin + 1, ymax - ymin + 1, zmax - zmin + 1); 156 | const size_t num_voxels = dim[0] * dim[1] * dim[2]; 157 | float* dist = new float[num_voxels]; 158 | float* weights = new float[num_voxels]; 159 | size_t lin_index = 0; 160 | for (int k=zmin; k<=zmax; ++k) for (int j=ymin; j<=ymax; ++j) for (int i=xmin; i<=xmax; ++i) { 161 | Vec3i idx(i, j, k); 162 | auto pair = tsdf_.find(idx); 163 | if (pair != tsdf_.end()) { 164 | dist[lin_index] = pair->second.dist; 165 | weights[lin_index] = pair->second.weight; 166 | } 167 | else { 168 | dist[lin_index] = T_; 169 | weights[lin_index] = 0; 170 | } 171 | ++lin_index; 172 | } 173 | 174 | // call marching cubes 175 | LayeredMarchingCubesNoColor lmc(Vec3f(voxel_size_, voxel_size_, voxel_size_)); 176 | lmc.computeIsoSurface(&tsdf_); 177 | bool success = lmc.savePly(filename); 178 | 179 | // delete temporary arrays 180 | delete[] dist; 181 | delete[] weights; 182 | 183 | return success; 184 | } 185 | 186 | bool MapGradPixelSdf::extract_pc(std::string filename) { 187 | 188 | std::vector points_normals; 189 | for (const auto& el : tsdf_) { 190 | const SdfVoxel& v = el.second; 191 | if (v.weight < 5) 192 | continue; 193 | Vec3f g = 1.2*v.grad.normalized(); //add a correction factor for gradient 194 | Vec3f d = v.dist * g; 195 | float voxel_size_2 = .5 * voxel_size_; 196 | if (std::fabs(d[0]) < voxel_size_2 && std::fabs(d[1]) < voxel_size_2 && std::fabs(d[2]) < voxel_size_2) 197 | { 198 | Vec6f pn; 199 | pn.segment<3>(0) = vox2float(el.first) - d; 200 | pn.segment<3>(3) = -g; 201 | points_normals.push_back(pn); 202 | } 203 | } 204 | 205 | std::ofstream plyFile; 206 | plyFile.open(filename.c_str()); 207 | if (!plyFile.is_open()) 208 | return false; 209 | 210 | plyFile << "ply" << std::endl; 211 | plyFile << "format ascii 1.0" << std::endl; 212 | plyFile << "element vertex " << points_normals.size() << std::endl; 213 | plyFile << "property float x" << std::endl; 214 | plyFile << "property float y" << std::endl; 215 | plyFile << "property float z" << std::endl; 216 | plyFile << "property float nx" << std::endl; 217 | plyFile << "property float ny" << std::endl; 218 | plyFile << "property float nz" << std::endl; 219 | plyFile << "end_header" << std::endl; 220 | 221 | for (const Vec6f& p : points_normals) { 222 | plyFile << p[0] << " " << p[1] << " " << p[2] << " " << p[3] << " " << p[4] << " " << p[5] << std::endl; 223 | } 224 | 225 | plyFile.close(); 226 | 227 | return true; 228 | } 229 | 230 | bool MapGradPixelSdf::save_sdf(std::string filename) 231 | { 232 | // compute dimensions (and, from that, size) 233 | const int pos_inf = std::numeric_limits::max(); 234 | const int neg_inf = std::numeric_limits::min(); 235 | int xmin, xmax, ymin, ymax, zmin, zmax; 236 | xmin = pos_inf; 237 | xmax = neg_inf; 238 | ymin = pos_inf; 239 | ymax = neg_inf; 240 | zmin = pos_inf; 241 | zmax = neg_inf; 242 | for (auto v : tsdf_) { 243 | if (v.first[0] < xmin) xmin = v.first[0]; 244 | if (v.first[0] > xmax) xmax = v.first[0]; 245 | if (v.first[1] < ymin) ymin = v.first[1]; 246 | if (v.first[1] > ymax) ymax = v.first[1]; 247 | if (v.first[2] < zmin) zmin = v.first[2]; 248 | if (v.first[2] > zmax) zmax = v.first[2]; 249 | } 250 | 251 | // create input that can handle MarchingCubes class 252 | const Vec3i dim(xmax - xmin + 1, ymax - ymin + 1, zmax - zmin + 1); 253 | const size_t num_voxels = dim[0] * dim[1] * dim[2]; 254 | 255 | // --------------save grid info ---------------------------- 256 | std::ofstream grid_file; 257 | grid_file.open((filename + "_grid_info.txt").c_str()); 258 | if (!grid_file.is_open()){ 259 | std::cerr << "couldn't save grid_info file!" << std::endl; 260 | return false; 261 | } 262 | grid_file << "voxel size: " << voxel_size_ << std::endl 263 | << "voxel dim: " << dim[0] << " " << dim[1] << " " << dim[2] << std::endl 264 | << "voxel min: " << xmin << " " << ymin << " " << zmin << std::endl 265 | << "voxel max: " << xmax << " " << ymax << " " << zmax << std::endl; 266 | grid_file.close(); 267 | 268 | // ---------------- save sdf dist ------------------------------- 269 | std::ofstream file; 270 | std::ofstream weight_file; 271 | file.open((filename + "_sdf_d.txt").c_str()); 272 | weight_file.open((filename + "_sdf_weight.txt").c_str()); 273 | 274 | // -------------- save sdf grad ------------ 275 | std::ofstream sdf_n0, sdf_n1, sdf_n2; 276 | sdf_n0.open((filename + "_sdf_n0.txt").c_str()); 277 | sdf_n1.open((filename + "_sdf_n1.txt").c_str()); 278 | sdf_n2.open((filename + "_sdf_n2.txt").c_str()); 279 | 280 | if (!file.is_open() || !weight_file.is_open() || !sdf_n0.is_open() || !sdf_n1.is_open() || !sdf_n2.is_open()){ 281 | std::cerr << "couldn't save sdf or sdf weight file!" << std::endl; 282 | return false; 283 | } 284 | 285 | for(const auto& pair : tsdf_){ 286 | Vec3i idx = pair.first; 287 | const SdfVoxel& v = pair.second; 288 | int lin_idx = dim[0]*dim[1]*(idx[2]-zmin) + dim[0]*(idx[1]-ymin) + idx[0]-xmin; 289 | file << lin_idx << " " << v.dist << "\n"; 290 | weight_file << lin_idx << " " << v.weight << "\n"; 291 | sdf_n0 << lin_idx << " " << v.grad[0] << "\n"; 292 | sdf_n1 << lin_idx << " " << v.grad[1] << "\n"; 293 | sdf_n2 << lin_idx << " " << v.grad[2] << "\n"; 294 | 295 | } 296 | 297 | file.close(); 298 | weight_file.close(); 299 | sdf_n0.close(); 300 | sdf_n1.close(); 301 | sdf_n2.close(); 302 | return true; 303 | } 304 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/MapPixelSdf.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #include "MapPixelSdf.h" 39 | #include "normals/NormalEstimator.h" 40 | #include "mesh/LayeredMarchingCubesNoColor.h" 41 | #include 42 | 43 | float MapPixelSdf::interp3(Vec3f point, float extrap, Vec3f& grad) const{ 44 | Vec3f pv = voxel_size_inv_ * point; 45 | const float i = pv[0], j= pv[1], k = pv[2]; 46 | 47 | // remove out of volume cases 48 | const int im = std::floor(i); 49 | const int jm = std::floor(j); 50 | const int km = std::floor(k); 51 | 52 | 53 | const float dx = i-im; 54 | const float dy = j-jm; 55 | const float dz = k-km; 56 | 57 | // get 8 corner 58 | Eigen::Matrix d = extrap*Eigen::Matrix::Ones(); 59 | std::vector corner; 60 | for(size_t k0 = 0; k0 < 2; k0++)for(size_t j0 = 0; j0 < 2; j0++)for(size_t i0 = 0; i0 < 2; i0++){ 61 | Vec3i idx(im+i0, jm+j0 , km+k0); 62 | auto pair = tsdf_.find(idx); 63 | if (pair != tsdf_.end()){ 64 | d[i0 + j0*2 + k0*2*2] = pair->second.dist; 65 | corner.push_back(true); 66 | } 67 | else 68 | corner.push_back(false); 69 | } 70 | 71 | if (std::all_of(corner.begin(), corner.end(), [](bool v) { return !v; })) // if all false 72 | return extrap; 73 | 74 | if (std::all_of(corner.begin(), corner.end(), [](bool v) { return v; })){ //if all true 75 | // std::cout << "8 corner d: " << d.transpose() << std::endl; 76 | // interpolate in x direction 77 | const float d01 = (1-dx)*d[0] + dx*d[1]; 78 | const float d23 = (1-dx)*d[2] + dx*d[3]; 79 | const float d45 = (1-dx)*d[4] + dx*d[5]; 80 | const float d67 = (1-dx)*d[6] + dx*d[7]; 81 | 82 | // interpolate in y direction 83 | const float d02 = (1-dy)*d[0] + dy*d[2]; 84 | const float d13 = (1-dy)*d[1] + dy*d[3]; 85 | const float d46 = (1-dy)*d[4] + dy*d[6]; 86 | const float d57 = (1-dy)*d[5] + dy*d[7]; 87 | 88 | // interpolate in z direction 89 | const float d04 = (1-dz)*d[0] + dz*d[4]; 90 | const float d15 = (1-dz)*d[1] + dz*d[5]; 91 | const float d26 = (1-dz)*d[2] + dz*d[6]; 92 | const float d37 = (1-dz)*d[3] + dz*d[7]; 93 | 94 | // calculate gradient 95 | grad[0] = voxel_size_inv_*((1-dz)*d13 + dz*d57 - (1-dz)*d02 - dz*d46); 96 | grad[1] = voxel_size_inv_*((1-dz)*d23 + dz*d67 - (1-dz)*d01 - dz*d45); 97 | grad[2] = voxel_size_inv_*((1-dy)*d45 + dy*d67 - (1-dy)*d01 - dy*d23); 98 | 99 | 100 | // interpolate in y direction 101 | const float dy0 = (1-dy)*d01 + dy*d23; 102 | const float dy1 = (1-dy)*d45 + dy*d67; 103 | 104 | //interpolate in z direction 105 | return (1-dz)*dy0 + dz*dy1; 106 | } 107 | 108 | // if not all 8 exists 109 | return 0.0; 110 | 111 | } 112 | 113 | 114 | void MapPixelSdf::update(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst) { 115 | 116 | const float fx = K(0,0), fy = K(1,1); 117 | const float cx = K(0,2), cy = K(1,2); 118 | 119 | cv::Mat nx, ny, nz, med_depth; 120 | cv::medianBlur(depth, med_depth, 5); // median filtering 121 | 122 | if (!NEst) { 123 | std::cerr << "No normal estimation possible - cannot update SDF volume!" << std::endl; 124 | return; 125 | } 126 | 127 | NEst->compute(depth, nx, ny, nz); 128 | 129 | const Mat3f R = pose.rotationMatrix(); 130 | const Mat3f Rt = pose.rotationMatrix().transpose(); 131 | const Vec3f t = pose.translation(); 132 | 133 | cv::Mat* x0_ptr = NEst->x0_ptr(); 134 | cv::Mat* y0_ptr = NEst->y0_ptr(); 135 | cv::Mat* n_sq_inv_ptr = NEst->n_sq_inv_ptr(); 136 | const float* x_hom_ptr = (const float*)x0_ptr->data; 137 | const float* y_hom_ptr = (const float*)y0_ptr->data; 138 | const float* hom_inv_ptr = (const float*)n_sq_inv_ptr->data; 139 | const float* z_ptr = (const float*)depth.data; 140 | const float* zm_ptr = (const float*)med_depth.data; 141 | 142 | const float* nx_ptr = (const float*)nx.data; 143 | const float* ny_ptr = (const float*)ny.data; 144 | const float* nz_ptr = (const float*)nz.data; 145 | 146 | const int factor = std::floor(T_ / voxel_size_); 147 | 148 | size_t counter = 0; 149 | 150 | for (size_t m=0; m < depth.rows; ++m) for (size_t n=0; n < depth.cols; ++n) { 151 | 152 | const size_t idx = m * depth.cols + n; 153 | 154 | const float z = z_ptr[idx]; 155 | 156 | if (z <= z_min_ || z >= z_max_ ) // z out of range or unreliable z 157 | continue; 158 | 159 | const Vec3f xy_hom(x_hom_ptr[idx], y_hom_ptr[idx], 1.); 160 | const Vec3f R_xy_hom(z * R * xy_hom + t); 161 | const Vec3f normal(nx_ptr[idx], ny_ptr[idx], nz_ptr[idx]); 162 | 163 | if (normal.squaredNorm() < .1) // invalid normal 164 | continue; 165 | 166 | if (normal.dot(xy_hom) * normal.dot(xy_hom) * hom_inv_ptr[idx] < .25) // normal direction too far from viewing ray direction (>75.5°) 167 | continue; 168 | 169 | for (float k = -factor; k <= factor; ++k) { // loop along ray 170 | 171 | Vec3f point = (z + k*voxel_size_) * R_xy_hom + t; // convert point into Sdf coordinate system 172 | const Vec3i vi = float2vox(point); 173 | point = Rt * (vox2float(vi) - t); 174 | const float sdf = point[2] - z; 175 | 176 | const float w = weight(sdf); 177 | if (w>0) { 178 | SdfVoxel& v = tsdf_[vi]; 179 | v.weight += w; 180 | v.dist += (truncate(sdf) - v.dist) * w / v.weight; 181 | } 182 | } 183 | 184 | ++counter; 185 | 186 | } 187 | 188 | std::cout << counter << " points integrated into tSDF volume!" << std::endl; 189 | } 190 | 191 | bool MapPixelSdf::extract_mesh(std::string filename) { 192 | 193 | // compute dimensions (and, from that, size) 194 | const int pos_inf = std::numeric_limits::max(); 195 | const int neg_inf = std::numeric_limits::min(); 196 | int xmin, xmax, ymin, ymax, zmin, zmax; 197 | xmin = pos_inf; 198 | xmax = neg_inf; 199 | ymin = pos_inf; 200 | ymax = neg_inf; 201 | zmin = pos_inf; 202 | zmax = neg_inf; 203 | for (auto v : tsdf_) { 204 | if (v.first[0] < xmin) xmin = v.first[0]; 205 | if (v.first[0] > xmax) xmax = v.first[0]; 206 | if (v.first[1] < ymin) ymin = v.first[1]; 207 | if (v.first[1] > ymax) ymax = v.first[1]; 208 | if (v.first[2] < zmin) zmin = v.first[2]; 209 | if (v.first[2] > zmax) zmax = v.first[2]; 210 | } 211 | 212 | 213 | // create input that can handle MarchingCubes class 214 | const Vec3i dim(xmax - xmin + 1, ymax - ymin + 1, zmax - zmin + 1); 215 | const size_t num_voxels = dim[0] * dim[1] * dim[2]; 216 | float* dist = new float[num_voxels]; 217 | float* weights = new float[num_voxels]; 218 | size_t lin_index = 0; 219 | for (int k=zmin; k<=zmax; ++k) for (int j=ymin; j<=ymax; ++j) for (int i=xmin; i<=xmax; ++i) { 220 | Vec3i idx(i, j, k); 221 | auto pair = tsdf_.find(idx); 222 | if (pair != tsdf_.end()) { 223 | dist[lin_index] = pair->second.dist; 224 | weights[lin_index] = pair->second.weight; 225 | } 226 | else { 227 | dist[lin_index] = T_; 228 | weights[lin_index] = 0; 229 | } 230 | ++lin_index; 231 | } 232 | 233 | // call marching cubes 234 | LayeredMarchingCubesNoColor lmc(Vec3f(voxel_size_, voxel_size_, voxel_size_)); 235 | lmc.computeIsoSurface(&tsdf_); 236 | bool success = lmc.savePly(filename); 237 | 238 | // delete temporary arrays 239 | delete[] dist; 240 | delete[] weights; 241 | 242 | return success; 243 | } 244 | 245 | bool MapPixelSdf::extract_pc(std::string filename) { 246 | 247 | const float voxel_size_2 = .5 * voxel_size_; 248 | 249 | std::vector points; 250 | for (const auto& el : tsdf_) { 251 | const SdfVoxel& v = el.second; 252 | if (v.weight < 5) 253 | continue; 254 | if(std::fabs(v.dist) < std::sqrt(3)*voxel_size_) 255 | { 256 | Vec3f pn; 257 | pn.segment<3>(0) = vox2float(el.first); 258 | points.push_back(pn); 259 | } 260 | } 261 | 262 | std::ofstream plyFile; 263 | plyFile.open(filename.c_str()); 264 | if (!plyFile.is_open()) 265 | return false; 266 | 267 | plyFile << "ply" << std::endl; 268 | plyFile << "format ascii 1.0" << std::endl; 269 | plyFile << "element vertex " << points.size() << std::endl; 270 | plyFile << "property float x" << std::endl; 271 | plyFile << "property float y" << std::endl; 272 | plyFile << "property float z" << std::endl; 273 | 274 | plyFile << "end_header" << std::endl; 275 | 276 | for (const Vec3f& p : points) { 277 | plyFile << p[0] << " " << p[1] << " " << p[2] << std::endl; 278 | } 279 | 280 | plyFile.close(); 281 | 282 | return true; 283 | } 284 | 285 | bool MapPixelSdf::save_sdf(std::string filename) 286 | { 287 | // compute dimensions (and, from that, size) 288 | const int pos_inf = std::numeric_limits::max(); 289 | const int neg_inf = std::numeric_limits::min(); 290 | int xmin, xmax, ymin, ymax, zmin, zmax; 291 | xmin = pos_inf; 292 | xmax = neg_inf; 293 | ymin = pos_inf; 294 | ymax = neg_inf; 295 | zmin = pos_inf; 296 | zmax = neg_inf; 297 | for (auto v : tsdf_) { 298 | if (v.first[0] < xmin) xmin = v.first[0]; 299 | if (v.first[0] > xmax) xmax = v.first[0]; 300 | if (v.first[1] < ymin) ymin = v.first[1]; 301 | if (v.first[1] > ymax) ymax = v.first[1]; 302 | if (v.first[2] < zmin) zmin = v.first[2]; 303 | if (v.first[2] > zmax) zmax = v.first[2]; 304 | } 305 | 306 | 307 | // create input that can handle MarchingCubes class 308 | const Vec3i dim(xmax - xmin + 1, ymax - ymin + 1, zmax - zmin + 1); 309 | const size_t num_voxels = dim[0] * dim[1] * dim[2]; 310 | 311 | // --------------save grid info ---------------------------- 312 | std::ofstream grid_file; 313 | grid_file.open((filename + "_grid_info.txt").c_str()); 314 | if (!grid_file.is_open()){ 315 | std::cerr << "couldn't save grid_info file!" << std::endl; 316 | return false; 317 | } 318 | grid_file << "voxel size: " << voxel_size_ << std::endl 319 | << "voxel dim: " << dim[0] << " " << dim[1] << " " << dim[2] << std::endl 320 | << "voxel min: " << xmin << " " << ymin << " " << zmin << std::endl 321 | << "voxel max: " << xmax << " " << ymax << " " << zmax << std::endl; 322 | grid_file.close(); 323 | 324 | // ---------------- save sdf dist ------------------------------- 325 | std::ofstream file; 326 | std::ofstream weight_file; 327 | file.open((filename + "_sdf_d.txt").c_str()); 328 | weight_file.open((filename + "_sdf_weight.txt").c_str()); 329 | 330 | 331 | if (!file.is_open() || !weight_file.is_open()){ 332 | std::cerr << "couldn't save sdf or sdf weight file!" << std::endl; 333 | return false; 334 | } 335 | 336 | for(const auto& pair : tsdf_){ 337 | Vec3i idx = pair.first; 338 | const SdfVoxel& v = pair.second; 339 | int lin_idx = dim[0]*dim[1]*(idx[2]-zmin) + dim[0]*(idx[1]-ymin) + idx[0]-xmin; 340 | file << lin_idx << " " << v.dist << "\n"; 341 | weight_file << lin_idx << " " << v.weight << "\n"; 342 | } 343 | 344 | file.close(); 345 | weight_file.close(); 346 | return true; 347 | } 348 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/MapPixelSdf.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef MAP_PIXEL_SDF_H_ 39 | #define MAP_PIXEL_SDF_H_ 40 | 41 | // includes 42 | #include 43 | #include "mat.h" 44 | // #include 45 | #include 46 | // class includes 47 | #include "Sdf.h" 48 | #include "hash_map.h" 49 | #include "sdf_voxel/SdfVoxel.h" 50 | 51 | /** 52 | * class declaration 53 | */ 54 | class MapPixelSdf : public Sdf { 55 | 56 | // variables 57 | 58 | const float voxel_size_; 59 | const float voxel_size_inv_; 60 | 61 | phmap::parallel_node_hash_map, 63 | phmap::priv::hash_default_eq, 64 | Eigen::aligned_allocator>> tsdf_; 65 | 66 | // methods 67 | 68 | // world point to voxel idx 69 | Vec3i float2vox(Vec3f point) const { 70 | Vec3f pv = voxel_size_inv_ * point; 71 | return Vec3i(std::round(pv[0]), std::round(pv[1]), std::round(pv[2])); 72 | } 73 | 74 | Vec3f vox2float(Vec3i idx) const { 75 | return voxel_size_ * idx.cast(); 76 | } 77 | 78 | float interp3(Vec3f point, float extrap, Vec3f& grad) const; 79 | 80 | 81 | public: 82 | 83 | // constructors / destructor 84 | 85 | MapPixelSdf() : 86 | Sdf(), 87 | voxel_size_(0.02), 88 | voxel_size_inv_(1./voxel_size_) 89 | {} 90 | 91 | MapPixelSdf(float voxel_size) : 92 | Sdf(), 93 | voxel_size_(voxel_size), 94 | voxel_size_inv_(1./voxel_size_) 95 | {} 96 | 97 | MapPixelSdf(float voxel_size, float T) : 98 | Sdf(T), 99 | voxel_size_(voxel_size), 100 | voxel_size_inv_(1./voxel_size_) 101 | {} 102 | 103 | ~MapPixelSdf() {} 104 | 105 | // methods 106 | 107 | virtual float tsdf(Vec3f point, Vec3f* grad_ptr) const { 108 | Vec3f grad = Vec3f::Zero(); 109 | float phi0 = interp3(point, -T_, grad); 110 | if (grad_ptr) { 111 | (*grad_ptr)[0] = grad[0]; 112 | (*grad_ptr)[1] = grad[1]; 113 | (*grad_ptr)[2] = grad[2]; 114 | } 115 | return phi0; 116 | } 117 | 118 | virtual float weights(Vec3f point) const { 119 | float w = 0; 120 | Vec3f pv = voxel_size_inv_ * point; 121 | 122 | // remove out of volume cases 123 | const int im = std::floor(pv[0]); 124 | const int jm = std::floor(pv[1]); 125 | const int km = std::floor(pv[2]); 126 | 127 | bool A = tsdf_.find(Vec3i(im,jm,km)) != tsdf_.end(); 128 | bool B = tsdf_.find(Vec3i(im,jm,km+1)) != tsdf_.end(); 129 | bool C = tsdf_.find(Vec3i(im,jm+1,km)) != tsdf_.end(); 130 | bool D = tsdf_.find(Vec3i(im,jm+1,km+1)) != tsdf_.end(); 131 | bool E = tsdf_.find(Vec3i(im+1,jm,km)) != tsdf_.end(); 132 | bool F = tsdf_.find(Vec3i(im+1,jm,km+1)) != tsdf_.end(); 133 | bool G = tsdf_.find(Vec3i(im+1,jm+1,km)) != tsdf_.end(); 134 | bool H = tsdf_.find(Vec3i(im+1,jm+1,km+1)) != tsdf_.end(); 135 | 136 | if (A && B && C && D && E && F && G && H) { 137 | const Vec3i idx = float2vox(point); 138 | auto pair = tsdf_.find(idx); 139 | w = pair->second.weight; 140 | } 141 | return w; 142 | 143 | } 144 | 145 | virtual void update(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst); 146 | 147 | // visualization / debugging 148 | 149 | virtual bool extract_mesh(std::string filename); 150 | 151 | virtual bool extract_pc(std::string filename); 152 | 153 | virtual bool save_sdf(std::string filename); 154 | 155 | }; 156 | 157 | #endif // MAP_PIXEL_SDF_H_ 158 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/MapPixelSdfOmp.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #include "MapPixelSdf.h" 39 | #include "normals/NormalEstimator.h" 40 | #include "mesh/LayeredMarchingCubesNoColor.h" 41 | #include 42 | #include 43 | 44 | float MapPixelSdf::interp3(Vec3f point, float extrap, Vec3f& grad) const{ 45 | Vec3f pv = voxel_size_inv_ * point; 46 | const float i = pv[0], j= pv[1], k = pv[2]; 47 | 48 | // remove out of volume cases 49 | const int im = std::floor(i); 50 | const int jm = std::floor(j); 51 | const int km = std::floor(k); 52 | 53 | 54 | const float dx = i-im; 55 | const float dy = j-jm; 56 | const float dz = k-km; 57 | 58 | // get 8 corner 59 | 60 | Eigen::Matrix d = extrap*Eigen::Matrix::Ones(); 61 | std::vector corner; 62 | for(size_t k0 = 0; k0 < 2; k0++)for(size_t j0 = 0; j0 < 2; j0++)for(size_t i0 = 0; i0 < 2; i0++){ 63 | Vec3i idx(im+i0, jm+j0 , km+k0); 64 | auto pair = tsdf_.find(idx); 65 | if (pair != tsdf_.end()){ 66 | d[i0 + j0*2 + k0*2*2] = pair->second.dist; 67 | corner.push_back(true); 68 | } 69 | else 70 | corner.push_back(false); 71 | } 72 | 73 | if (std::all_of(corner.begin(), corner.end(), [](bool v) { return !v; })) // if all false 74 | return extrap; 75 | 76 | if (std::all_of(corner.begin(), corner.end(), [](bool v) { return v; })){ //if all true 77 | // std::cout << "8 corner d: " << d.transpose() << std::endl; 78 | // interpolate in x direction 79 | const float d01 = (1-dx)*d[0] + dx*d[1]; 80 | const float d23 = (1-dx)*d[2] + dx*d[3]; 81 | const float d45 = (1-dx)*d[4] + dx*d[5]; 82 | const float d67 = (1-dx)*d[6] + dx*d[7]; 83 | 84 | // interpolate in y direction 85 | const float d02 = (1-dy)*d[0] + dy*d[2]; 86 | const float d13 = (1-dy)*d[1] + dy*d[3]; 87 | const float d46 = (1-dy)*d[4] + dy*d[6]; 88 | const float d57 = (1-dy)*d[5] + dy*d[7]; 89 | 90 | // interpolate in z direction 91 | const float d04 = (1-dz)*d[0] + dz*d[4]; 92 | const float d15 = (1-dz)*d[1] + dz*d[5]; 93 | const float d26 = (1-dz)*d[2] + dz*d[6]; 94 | const float d37 = (1-dz)*d[3] + dz*d[7]; 95 | 96 | // calculate gradient 97 | grad[0] = voxel_size_inv_*((1-dz)*d13 + dz*d57 - (1-dz)*d02 - dz*d46); 98 | grad[1] = voxel_size_inv_*((1-dz)*d23 + dz*d67 - (1-dz)*d01 - dz*d45); 99 | grad[2] = voxel_size_inv_*((1-dy)*d45 + dy*d67 - (1-dy)*d01 - dy*d23); 100 | 101 | 102 | // interpolate in y direction 103 | const float dy0 = (1-dy)*d01 + dy*d23; 104 | const float dy1 = (1-dy)*d45 + dy*d67; 105 | 106 | //interpolate in z direction 107 | return (1-dz)*dy0 + dz*dy1; 108 | } 109 | 110 | // if not all 8 exists 111 | return 0.0; 112 | 113 | } 114 | 115 | 116 | void MapPixelSdf::update(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst) { 117 | 118 | const float fx = K(0,0), fy = K(1,1); 119 | const float cx = K(0,2), cy = K(1,2); 120 | 121 | cv::Mat nx, ny, nz, med_depth; 122 | cv::medianBlur(depth, med_depth, 5); // median filtering 123 | 124 | if (!NEst) { 125 | std::cerr << "No normal estimation possible - cannot update SDF volume!" << std::endl; 126 | return; 127 | } 128 | 129 | NEst->compute(depth, nx, ny, nz); 130 | 131 | const Mat3f R = pose.rotationMatrix(); 132 | const Mat3f Rt = pose.rotationMatrix().transpose(); 133 | const Vec3f t = pose.translation(); 134 | 135 | cv::Mat* x0_ptr = NEst->x0_ptr(); 136 | cv::Mat* y0_ptr = NEst->y0_ptr(); 137 | cv::Mat* n_sq_inv_ptr = NEst->n_sq_inv_ptr(); 138 | const float* x_hom_ptr = (const float*)x0_ptr->data; 139 | const float* y_hom_ptr = (const float*)y0_ptr->data; 140 | const float* hom_inv_ptr = (const float*)n_sq_inv_ptr->data; 141 | const float* z_ptr = (const float*)depth.data; 142 | const float* zm_ptr = (const float*)med_depth.data; 143 | 144 | const float* nx_ptr = (const float*)nx.data; 145 | const float* ny_ptr = (const float*)ny.data; 146 | const float* nz_ptr = (const float*)nz.data; 147 | 148 | const int factor = std::floor(T_ / voxel_size_); 149 | 150 | size_t counter = 0; 151 | 152 | # pragma omp parallel for reduction(+: counter) 153 | for (size_t m=0; m < depth.rows; ++m) for (size_t n=0; n < depth.cols; ++n) { 154 | 155 | const size_t idx = m * depth.cols + n; 156 | 157 | const float z = z_ptr[idx]; 158 | 159 | if (z <= z_min_ || z >= z_max_ ) // z out of range or unreliable z 160 | continue; 161 | 162 | const Vec3f xy_hom(x_hom_ptr[idx], y_hom_ptr[idx], 1.); 163 | const Vec3f R_xy_hom(R * xy_hom); 164 | const Vec3f normal(nx_ptr[idx], ny_ptr[idx], nz_ptr[idx]); 165 | 166 | if (normal.squaredNorm() < .1) // invalid normal 167 | continue; 168 | 169 | if (normal.dot(xy_hom) * normal.dot(xy_hom) * hom_inv_ptr[idx] < .25) // normal direction too far from viewing ray direction (>75.5°) 170 | continue; 171 | 172 | for (float k = -factor; k <= factor; ++k) { // loop along ray 173 | 174 | Vec3f point = (z + k*voxel_size_) * R_xy_hom + t; // convert point into Sdf coordinate system 175 | const Vec3i vi = float2vox(point); 176 | point = Rt * (vox2float(vi) - t); 177 | const float sdf = point[2] - z; 178 | 179 | const float w = weight(sdf); 180 | if (w>0) { 181 | #pragma omp critical 182 | { 183 | SdfVoxel& v = tsdf_[vi]; 184 | v.weight += w; 185 | v.dist += (truncate(sdf) - v.dist) * w / v.weight; 186 | } 187 | } 188 | } 189 | 190 | ++counter; 191 | 192 | } 193 | 194 | std::cout << counter << " points integrated into tSDF volume!" << std::endl; 195 | } 196 | 197 | bool MapPixelSdf::extract_mesh(std::string filename) { 198 | 199 | // compute dimensions (and, from that, size) 200 | const int pos_inf = std::numeric_limits::max(); 201 | const int neg_inf = std::numeric_limits::min(); 202 | int xmin, xmax, ymin, ymax, zmin, zmax; 203 | xmin = pos_inf; 204 | xmax = neg_inf; 205 | ymin = pos_inf; 206 | ymax = neg_inf; 207 | zmin = pos_inf; 208 | zmax = neg_inf; 209 | for (auto v : tsdf_) { 210 | if (v.first[0] < xmin) xmin = v.first[0]; 211 | if (v.first[0] > xmax) xmax = v.first[0]; 212 | if (v.first[1] < ymin) ymin = v.first[1]; 213 | if (v.first[1] > ymax) ymax = v.first[1]; 214 | if (v.first[2] < zmin) zmin = v.first[2]; 215 | if (v.first[2] > zmax) zmax = v.first[2]; 216 | } 217 | 218 | // create input that can handle MarchingCubes class 219 | const Vec3i dim(xmax - xmin + 1, ymax - ymin + 1, zmax - zmin + 1); 220 | const size_t num_voxels = dim[0] * dim[1] * dim[2]; 221 | float* dist = new float[num_voxels]; 222 | float* weights = new float[num_voxels]; 223 | size_t lin_index = 0; 224 | for (int k=zmin; k<=zmax; ++k) for (int j=ymin; j<=ymax; ++j) for (int i=xmin; i<=xmax; ++i) { 225 | Vec3i idx(i, j, k); 226 | auto pair = tsdf_.find(idx); 227 | if (pair != tsdf_.end()) { 228 | dist[lin_index] = pair->second.dist; 229 | weights[lin_index] = pair->second.weight; 230 | } 231 | else { 232 | dist[lin_index] = T_; 233 | weights[lin_index] = 0; 234 | } 235 | ++lin_index; 236 | } 237 | 238 | // call marching cubes 239 | LayeredMarchingCubesNoColor lmc(Vec3f(voxel_size_, voxel_size_, voxel_size_)); 240 | lmc.computeIsoSurface(&tsdf_); 241 | bool success = lmc.savePly(filename); 242 | 243 | // delete temporary arrays 244 | delete[] dist; 245 | delete[] weights; 246 | 247 | return success; 248 | } 249 | 250 | bool MapPixelSdf::extract_pc(std::string filename) { 251 | 252 | const float voxel_size_2 = .5 * voxel_size_; 253 | 254 | std::vector points; 255 | for (const auto& el : tsdf_) { 256 | const SdfVoxel& v = el.second; 257 | if (v.weight < 5) 258 | continue; 259 | 260 | if(std::fabs(v.dist) < std::sqrt(3)*voxel_size_) 261 | { 262 | Vec3f pn; 263 | pn.segment<3>(0) = vox2float(el.first); 264 | points.push_back(pn); 265 | } 266 | } 267 | 268 | std::ofstream plyFile; 269 | plyFile.open(filename.c_str()); 270 | if (!plyFile.is_open()) 271 | return false; 272 | 273 | plyFile << "ply" << std::endl; 274 | plyFile << "format ascii 1.0" << std::endl; 275 | plyFile << "element vertex " << points.size() << std::endl; 276 | plyFile << "property float x" << std::endl; 277 | plyFile << "property float y" << std::endl; 278 | plyFile << "property float z" << std::endl; 279 | plyFile << "end_header" << std::endl; 280 | 281 | for (const Vec3f& p : points) { 282 | plyFile << p[0] << " " << p[1] << " " << p[2] << std::endl; 283 | } 284 | 285 | plyFile.close(); 286 | 287 | return true; 288 | } 289 | 290 | bool MapPixelSdf::save_sdf(std::string filename) 291 | { 292 | // compute dimensions (and, from that, size) 293 | const int pos_inf = std::numeric_limits::max(); 294 | const int neg_inf = std::numeric_limits::min(); 295 | int xmin, xmax, ymin, ymax, zmin, zmax; 296 | xmin = pos_inf; 297 | xmax = neg_inf; 298 | ymin = pos_inf; 299 | ymax = neg_inf; 300 | zmin = pos_inf; 301 | zmax = neg_inf; 302 | for (auto v : tsdf_) { 303 | if (v.first[0] < xmin) xmin = v.first[0]; 304 | if (v.first[0] > xmax) xmax = v.first[0]; 305 | if (v.first[1] < ymin) ymin = v.first[1]; 306 | if (v.first[1] > ymax) ymax = v.first[1]; 307 | if (v.first[2] < zmin) zmin = v.first[2]; 308 | if (v.first[2] > zmax) zmax = v.first[2]; 309 | } 310 | 311 | 312 | // create input that can handle MarchingCubes class 313 | const Vec3i dim(xmax - xmin + 1, ymax - ymin + 1, zmax - zmin + 1); 314 | const size_t num_voxels = dim[0] * dim[1] * dim[2]; 315 | 316 | // --------------save grid info ---------------------------- 317 | std::ofstream grid_file; 318 | grid_file.open((filename + "_grid_info.txt").c_str()); 319 | if (!grid_file.is_open()){ 320 | std::cerr << "couldn't save grid_info file!" << std::endl; 321 | return false; 322 | } 323 | grid_file << "voxel size: " << voxel_size_ << std::endl 324 | << "voxel dim: " << dim[0] << " " << dim[1] << " " << dim[2] << std::endl 325 | << "voxel min: " << xmin << " " << ymin << " " << zmin << std::endl 326 | << "voxel max: " << xmax << " " << ymax << " " << zmax << std::endl; 327 | grid_file.close(); 328 | 329 | // ---------------- save sdf dist ------------------------------- 330 | std::ofstream file; 331 | std::ofstream weight_file; 332 | file.open((filename + "_sdf_d.txt").c_str()); 333 | weight_file.open((filename + "_sdf_weight.txt").c_str()); 334 | 335 | 336 | if (!file.is_open() || !weight_file.is_open()){ 337 | std::cerr << "couldn't save sdf or sdf weight file!" << std::endl; 338 | return false; 339 | } 340 | 341 | for(const auto& pair : tsdf_){ 342 | Vec3i idx = pair.first; 343 | const SdfVoxel& v = pair.second; 344 | int lin_idx = dim[0]*dim[1]*(idx[2]-zmin) + dim[0]*(idx[1]-ymin) + idx[0]-xmin; 345 | file << lin_idx << " " << v.dist << "\n"; 346 | weight_file << lin_idx << " " << v.weight << "\n"; 347 | } 348 | 349 | file.close(); 350 | weight_file.close(); 351 | return true; 352 | } -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/RigidOptimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef RIGID_OPTIMIZER_H_ 39 | #define RIGID_OPTIMIZER_H_ 40 | 41 | // includes 42 | #include 43 | #include 44 | #include "mat.h" 45 | //class includes 46 | #include "Sdf.h" 47 | 48 | /** 49 | * class declaration 50 | */ 51 | class RigidOptimizer { 52 | 53 | protected: 54 | 55 | // variables 56 | 57 | int num_iterations_; 58 | float conv_threshold_; 59 | float conv_threshold_sq_; 60 | float damping_; 61 | 62 | Sdf* tSDF_; 63 | 64 | SE3 pose_ = SE3(); 65 | 66 | public: 67 | 68 | // constructors / destructor 69 | 70 | RigidOptimizer(Sdf* tSDF) : 71 | num_iterations_(25), 72 | conv_threshold_(1e-3), 73 | conv_threshold_sq_(conv_threshold_ * conv_threshold_), 74 | damping_(1.), 75 | tSDF_(tSDF) 76 | {} 77 | 78 | RigidOptimizer(int num_iterations, float conv_threshold, float damping, Sdf* tSDF) : 79 | num_iterations_(num_iterations), 80 | conv_threshold_(conv_threshold), 81 | conv_threshold_sq_(conv_threshold_ * conv_threshold_), 82 | damping_(damping), 83 | tSDF_(tSDF) 84 | {} 85 | 86 | virtual ~RigidOptimizer() {} 87 | 88 | // member functions 89 | 90 | void set_num_iterations(int num_iterations) { 91 | num_iterations_ = num_iterations; 92 | } 93 | 94 | void set_conv_threshold(float conv_threshold) { 95 | conv_threshold_ = conv_threshold; 96 | conv_threshold_sq_ = conv_threshold_ * conv_threshold_; 97 | } 98 | 99 | void set_damping(float damping) { 100 | damping_ = damping; 101 | } 102 | 103 | void set_pose(SE3 pose) { pose_ = pose; } 104 | 105 | SE3 pose() { 106 | return pose_; 107 | } 108 | 109 | // virtual bool optimize() {} 110 | virtual bool optimize(const cv::Mat &depth, const Mat3f K) = 0; 111 | 112 | }; 113 | 114 | #endif // RIGID_OPTIMIZER_H_ 115 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/RigidPointOptimizer.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #include "RigidPointOptimizer.h" 39 | 40 | bool RigidPointOptimizer::optimize_sampled(const cv::Mat &depth, const Mat3f K, size_t sampling) { 41 | 42 | const float z_min = tSDF_->z_min_, z_max = tSDF_->z_max_; 43 | const int w = depth.cols; 44 | const int h = depth.rows; 45 | const float fx = K(0,0), fy = K(1,1); 46 | const float cx = K(0,2), cy = K(1,2); 47 | const float fx_inv = 1.f / fx; 48 | const float fy_inv = 1.f / fy; 49 | const float* depth_ptr = (const float*)depth.data; 50 | 51 | for (size_t k=0; k=z_max) continue; 66 | 67 | const float x0 = (float(x) - cx) * fx_inv; 68 | const float y0 = (float(y) - cy) * fy_inv; 69 | Vec3f point(x0*z, y0*z, z); 70 | point = R * point + t; 71 | 72 | float w0 = tSDF_->weights(point); 73 | if (w0>0) { 74 | Vec3f grad_curr; 75 | float phi0 = tSDF_->tsdf(point, &grad_curr); 76 | E += phi0 * phi0; 77 | Vec6f grad_xi; 78 | grad_xi << grad_curr, point.cross(grad_curr); 79 | g += phi0 * grad_xi; 80 | H += grad_xi * grad_xi.transpose(); 81 | ++counter; 82 | } 83 | 84 | } 85 | 86 | Vec6f xi = damping_ * H.llt().solve(g); // Gauss-Newton 87 | 88 | if (xi.squaredNorm() < conv_threshold_sq_) { 89 | std::cout << "... Convergence after " << k << " iterations!" << std::endl; 90 | return true; 91 | } 92 | 93 | // update pose 94 | if(!xi.array().isNaN().any()) 95 | pose_ = SE3::exp(-xi) * pose_; 96 | } 97 | 98 | return false; 99 | } -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/RigidPointOptimizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef RIGID_POINT_OPTIMIZER_H_ 39 | #define RIGID_POINT_OPTIMIZER_H_ 40 | 41 | // includes 42 | #include 43 | //class includes 44 | #include "RigidOptimizer.h" 45 | 46 | /** 47 | * class declaration 48 | */ 49 | class RigidPointOptimizer : public RigidOptimizer { 50 | 51 | public: 52 | 53 | // constructors / destructor 54 | 55 | RigidPointOptimizer(Sdf* tSDF) : 56 | RigidOptimizer(tSDF) 57 | {} 58 | 59 | RigidPointOptimizer(int num_iterations, float conv_threshold, float damping, Sdf* tSDF) : 60 | RigidOptimizer(num_iterations, conv_threshold, damping, tSDF) 61 | {} 62 | 63 | ~RigidPointOptimizer() {} 64 | 65 | bool optimize_sampled(const cv::Mat &depth, const Mat3f K, size_t sampling); 66 | 67 | // member functions 68 | 69 | bool optimize(const cv::Mat &depth, const Mat3f K) { 70 | // tSDF_->increase_counter(); 71 | return optimize_sampled(depth, K, 1); 72 | } 73 | 74 | }; 75 | 76 | #endif // RIGID_POINT_OPTIMIZER_H_ -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/RigidPointOptimizerOmp.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #include "RigidPointOptimizer.h" 39 | #include 40 | 41 | #pragma omp declare reduction (vecsum : Eigen::Vector : omp_out += omp_in) initializer(omp_priv = Eigen::Vector::Zero()) 42 | #pragma omp declare reduction (matsum : Eigen::Matrix : omp_out += omp_in) initializer(omp_priv = Eigen::Matrix::Zero()) 43 | 44 | bool RigidPointOptimizer::optimize_sampled(const cv::Mat &depth, const Mat3f K, size_t sampling) { 45 | 46 | const float z_min = tSDF_->z_min_, z_max = tSDF_->z_max_; 47 | const int w = depth.cols; 48 | const int h = depth.rows; 49 | const float fx = K(0,0), fy = K(1,1); 50 | const float cx = K(0,2), cy = K(1,2); 51 | const float fx_inv = 1.f / fx; 52 | const float fy_inv = 1.f / fy; 53 | const float* depth_ptr = (const float*)depth.data; 54 | 55 | std::pair pair; 56 | 57 | for (size_t k=0; k=z_max) continue; 74 | 75 | const float x0 = (float(x) - cx) * fx_inv; 76 | const float y0 = (float(y) - cy) * fy_inv; 77 | Vec3f point(x0*z, y0*z, z); 78 | point = R * point + t; 79 | 80 | float w0 = tSDF_->weights(point); 81 | if (w0>0) { 82 | Vec3f grad_curr; 83 | float phi0 = tSDF_->tsdf(point, &grad_curr); 84 | E += phi0 * phi0; 85 | Vec6f grad_xi; 86 | grad_xi << grad_curr, point.cross(grad_curr); 87 | g += phi0 * grad_xi; 88 | H += grad_xi * grad_xi.transpose(); 89 | ++counter; 90 | } 91 | } 92 | 93 | Vec6f xi = damping_ * H.llt().solve(g); // Gauss-Newton 94 | 95 | if (xi.squaredNorm() < conv_threshold_sq_) { 96 | std::cout << "... Convergence after " << k << " iterations!" << std::endl; 97 | return true; 98 | } 99 | 100 | // update pose 101 | if(!xi.array().isNaN().any()) 102 | pose_ = SE3::exp(-xi) * pose_; 103 | } 104 | 105 | return false; 106 | } -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/Sdf.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef SDF_H_ 39 | #define SDF_H_ 40 | 41 | // includes 42 | #include 43 | 44 | namespace cv { 45 | template 46 | class NormalEstimator; 47 | } 48 | 49 | /** 50 | * class declaration 51 | */ 52 | class Sdf { 53 | 54 | protected: 55 | 56 | // friends 57 | 58 | friend class RigidPointOptimizer; 59 | friend class RigidSdfOptimizer; 60 | 61 | // variables 62 | 63 | float T_; // truncation distance in meters 64 | float inv_T_; 65 | size_t counter_; // frame counter 66 | 67 | float z_min_ = 0.5; 68 | float z_max_ = 3.5; 69 | 70 | // methods 71 | 72 | float truncate(float sdf) const { 73 | return std::max(-T_, std::min(T_, sdf)); 74 | } 75 | 76 | float weight(float sdf) const { 77 | float w = 0.f; 78 | if (sdf<=0.) { 79 | w = 1.f; 80 | } 81 | else if (sdf<=T_) { 82 | w = 1.f - sdf*inv_T_; 83 | } 84 | return w; 85 | } 86 | 87 | void increase_counter() { 88 | ++counter_; 89 | } 90 | 91 | // void init(); 92 | 93 | public: 94 | 95 | // constructors / destructor 96 | 97 | Sdf() : 98 | T_(0.05), 99 | inv_T_(1./T_), 100 | counter_(0) 101 | {} 102 | 103 | Sdf(float T) : 104 | T_(T), 105 | inv_T_(1./T_), 106 | counter_(0) 107 | {} 108 | 109 | virtual ~Sdf() {} 110 | 111 | // methods 112 | 113 | virtual float tsdf(Vec3f point, Vec3f* grad_ptr = nullptr) const = 0; 114 | 115 | virtual float weights(Vec3f point) const = 0; 116 | 117 | virtual void update(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst = nullptr) = 0; 118 | 119 | virtual void setup(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, cv::NormalEstimator* NEst = nullptr) { 120 | update(color, depth, K, SE3(), NEst); 121 | } 122 | 123 | void set_zmin(float z_min) { 124 | z_min_ = z_min; 125 | } 126 | 127 | void set_zmax(float z_max) { 128 | z_max_ = z_max; 129 | } 130 | 131 | 132 | 133 | // visualization / debugging 134 | 135 | virtual bool extract_mesh(std::string filename = "mesh.ply") { 136 | return false; 137 | } 138 | 139 | virtual bool extract_pc(std::string filename = "cloud.ply") { 140 | return false; 141 | } 142 | 143 | virtual bool save_sdf(std::string filename ="sdf.txt"){ 144 | return false; 145 | } 146 | 147 | // virtual void write() {} 148 | 149 | }; 150 | 151 | #endif // SDF_H_ 152 | -------------------------------------------------------------------------------- /cpp/include/sdf_voxel/SdfVoxel.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the code accompanying the paper 5 | Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 6 | by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 7 | 8 | Copyright (c) 2021, Christiane Sommer and Lu Sang. 9 | All rights reserved. 10 | 11 | Redistribution and use in source and binary forms, with or without 12 | modification, are permitted provided that the following conditions are met: 13 | 14 | * Redistributions of source code must retain the above copyright notice, this 15 | list of conditions and the following disclaimer. 16 | 17 | * Redistributions in binary form must reproduce the above copyright notice, 18 | this list of conditions and the following disclaimer in the documentation 19 | and/or other materials provided with the distribution. 20 | 21 | * Neither the name of the copyright holder nor the names of its 22 | contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 29 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 30 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 31 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 33 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 34 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | 38 | #ifndef SDF_VOXEL_H_ 39 | #define SDF_VOXEL_H_ 40 | 41 | #include 42 | #include 43 | #include "hash_map.h" 44 | 45 | struct SdfVoxel { 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | float dist; 49 | Eigen::Vector3f grad; 50 | float weight; 51 | 52 | SdfVoxel() : 53 | dist(0.), 54 | grad(Eigen::Vector3f::Zero()), 55 | weight(0.) 56 | {} 57 | }; 58 | 59 | using Vec8f = Eigen::Matrix; 60 | 61 | struct SdfVoxelHr { 62 | 63 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 64 | Vec8f d = Vec8f::Zero(); 65 | Vec8f r = Vec8f::Zero(); 66 | Vec8f g = Vec8f::Zero(); 67 | Vec8f b = Vec8f::Zero(); 68 | 69 | const float dist; 70 | const float weight; 71 | const Eigen::Vector3f grad; 72 | 73 | SdfVoxelHr() : 74 | dist(0.), 75 | grad(Eigen::Vector3f::Zero()), 76 | weight(0.), 77 | d(Vec8f::Zero()), 78 | r(Vec8f::Zero()), 79 | g(Vec8f::Zero()), 80 | b(Vec8f::Zero()) 81 | {} 82 | 83 | SdfVoxelHr(const SdfVoxel &voxel, const float voxel_size) : 84 | dist(voxel.dist), 85 | grad(voxel.grad.normalized()), 86 | weight(voxel.weight), 87 | r(Vec8f::Zero()), 88 | g(Vec8f::Zero()), 89 | b(Vec8f::Zero()) 90 | { 91 | const float voxel_size_4 = 0.25 * voxel_size; 92 | d[0] = dist + voxel_size_4 * (-grad[0] - grad[1] - grad[2]); 93 | d[1] = dist + voxel_size_4 * ( grad[0] - grad[1] - grad[2]); 94 | d[2] = dist + voxel_size_4 * (-grad[0] + grad[1] - grad[2]); 95 | d[3] = dist + voxel_size_4 * ( grad[0] + grad[1] - grad[2]); 96 | d[4] = dist + voxel_size_4 * (-grad[0] - grad[1] + grad[2]); 97 | d[5] = dist + voxel_size_4 * ( grad[0] - grad[1] + grad[2]); 98 | d[6] = dist + voxel_size_4 * (-grad[0] + grad[1] + grad[2]); 99 | d[7] = dist + voxel_size_4 * ( grad[0] + grad[1] + grad[2]); 100 | } 101 | 102 | SdfVoxelHr(const SdfVoxelHr &voxel) : 103 | dist(voxel.dist), 104 | grad(voxel.grad), 105 | weight(voxel.weight), 106 | d(voxel.d), 107 | r(voxel.r), 108 | g(voxel.g), 109 | b(voxel.b) 110 | {} 111 | 112 | }; 113 | 114 | using Vec3i = Eigen::Vector3i; 115 | using SdfLrMap = phmap::parallel_node_hash_map, phmap::priv::hash_default_eq, Eigen::aligned_allocator>>; 116 | using SdfHrMap = phmap::parallel_node_hash_map, phmap::priv::hash_default_eq, Eigen::aligned_allocator>>; 117 | 118 | #endif // SDF_VOXEL_H_ 119 | 120 | -------------------------------------------------------------------------------- /cpp/photometric_opt/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.2) 2 | 3 | SET(EXECUTABLE_OUTPUT_PATH ../../photometric_opt/bin) 4 | SET(SUBPROJECT_NAME PhotoBA) 5 | 6 | ADD_EXECUTABLE( ${SUBPROJECT_NAME} 7 | src/main_photo_ba.cpp 8 | ) 9 | 10 | TARGET_LINK_LIBRARIES( ${SUBPROJECT_NAME} 11 | sdf_tracker_lib 12 | ${OpenCV_LIBS} 13 | color_lib 14 | mc_lib 15 | ) -------------------------------------------------------------------------------- /matlab/GradientAnalysisSpheres.m: -------------------------------------------------------------------------------- 1 | % BSD 3-Clause License 2 | % 3 | % This file is part of the code accompanying the paper 4 | % Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 5 | % by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 6 | % 7 | % Copyright (c) 2021, Christiane Sommer and Lu Sang. 8 | % All rights reserved. 9 | % 10 | % Redistribution and use in source and binary forms, with or without 11 | % modification, are permitted provided that the following conditions are met: 12 | % 13 | % * Redistributions of source code must retain the above copyright notice, this 14 | % list of conditions and the following disclaimer. 15 | % 16 | % * Redistributions in binary form must reproduce the above copyright notice, 17 | % this list of conditions and the following disclaimer in the documentation 18 | % and/or other materials provided with the distribution. 19 | % 20 | % * Neither the name of the copyright holder nor the names of its 21 | % contributors may be used to endorse or promote products derived from 22 | % this software without specific prior written permission. 23 | % 24 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | % AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | % IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | % FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | % DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | % SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | % CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | % OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | % OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | %% 36 | 37 | ml_blue = [0 0.4470 0.7410]; 38 | ml_yellow = [0.9290 0.6940 0.1250]; 39 | ml_green = [0.4660 0.6740 0.1880]; 40 | ml_red = [0.6350 0.0780 0.1840]; 41 | 42 | %% read in per-voxel SDF and stored Gradient-SDF gradient vectors 43 | 44 | path = './'; % modify accordingly 45 | file_prefix = 'gradient_sdf_sdf_'; 46 | 47 | % adapt the following values to those from grid_info.txt 48 | sz = [162 176 123]; % voxel dim 49 | dmin = [-77 -92 -66]; % voxel min 50 | dmax = [84 83 56]; % voxel max 51 | vs = 0.01; 52 | vs_inv = 1/vs; 53 | T = 10; 54 | 55 | D = vs * T * ones(sz); % init SDF volume 56 | tmp = load([path file_prefix 'd.txt']); 57 | D(tmp(:,1)+1) = tmp(:,2); 58 | 59 | Gx1 = zeros(sz); 60 | tmp = load([path file_prefix 'n0.txt']); 61 | Gx1(tmp(:,1)+1) = tmp(:,2); 62 | 63 | Gy1 = zeros(sz); 64 | tmp = load([path file_prefix 'n1.txt']); 65 | Gy1(tmp(:,1)+1) = tmp(:,2); 66 | 67 | Gz1 = zeros(sz); 68 | tmp = load([path file_prefix 'n2.txt']); 69 | Gz1(tmp(:,1)+1) = tmp(:,2); 70 | 71 | % normalize stored Gradient-SDF vectors 72 | inv_normG1 = 1 ./ sqrt(Gx1.^2 + Gy1.^2 + Gz1.^2); 73 | Gx1 = Gx1 .* inv_normG1; 74 | Gy1 = Gy1 .* inv_normG1; 75 | Gz1 = Gz1 .* inv_normG1; 76 | 77 | clear tmp inv_normG1 path file_prefix 78 | 79 | %% compute gradients with finite differences 80 | 81 | [Gy2, Gx2, Gz2] = gradient(D, vs); 82 | 83 | % normalize computed gradients 84 | inv_normG2 = 1 ./ sqrt(Gx2.^2 + Gy2.^2 + Gz2.^2); 85 | Gx2 = Gx2 .* inv_normG2; 86 | Gy2 = Gy2 .* inv_normG2; 87 | Gz2 = Gz2 .* inv_normG2; 88 | 89 | clear inv_normG2 90 | 91 | %% compute GT gradients 92 | 93 | % load spheres in format [cx cy cz R] 94 | load('spheres.mat') 95 | 96 | [y,x,z] = meshgrid((dmin(2):dmax(2))*vs, (dmin(1):dmax(1))*vs, (dmin(3):dmax(3))*vs) ; 97 | xyz = [x(:), y(:), z(:)]; 98 | % clear x y z 99 | D_GT = spheres(:,4)' - pdist2(xyz, spheres(:,1:3)); % signed distances 100 | [D_GT, idx] = max(D_GT, [], 2); 101 | D_GT = reshape(D_GT, sz); 102 | 103 | gx = reshape(xyz(:,1) - spheres(idx, 1), sz); 104 | gy = reshape(xyz(:,2) - spheres(idx, 2), sz); 105 | gz = reshape(xyz(:,3) - spheres(idx, 3), sz); 106 | 107 | % normalize GT gradients 108 | inv_norm_g = 1 ./ sqrt(gx.^2 + gy.^2 + gz.^2); 109 | gx = gx .* inv_norm_g; 110 | gy = gy .* inv_norm_g; 111 | gz = gz .* inv_norm_g; 112 | 113 | clear inv_norm_g xyz idx 114 | 115 | %% compute gradients with forward finite differences 116 | 117 | Gx3 = vs_inv * (D(2:end, :, :) - D(1:end-1, :, :)); 118 | Gx3 = cat(1, Gx3, zeros(1, sz(2), sz(3))); 119 | 120 | Gy3 = vs_inv * (D(:, 2:end, :) - D(:, 1:end-1, :)); 121 | Gy3 = cat(2, Gy3, zeros(sz(1), 1, sz(3))); 122 | 123 | Gz3 = vs_inv * (D(:, :, 2:end) - D(:, :, 1:end-1)); 124 | Gz3 = cat(3, Gz3, zeros(sz(1), sz(2), 1)); 125 | 126 | % normalize computed gradients 127 | inv_normG3 = 1 ./ sqrt(Gx3.^2 + Gy3.^2 + Gz3.^2); 128 | Gx3 = Gx3 .* inv_normG3; 129 | Gy3 = Gy3 .* inv_normG3; 130 | Gz3 = Gz3 .* inv_normG3; 131 | 132 | clear inv_normG3 133 | 134 | %% compute gradients with backward finite differences 135 | 136 | Gx4 = vs_inv * (D(2:end, :, :) - D(1:end-1, :, :)); 137 | Gx4 = cat(1, zeros(1, sz(2), sz(3)), Gx4); 138 | 139 | Gy4 = vs_inv * (D(:, 2:end, :) - D(:, 1:end-1, :)); 140 | Gy4 = cat(2, zeros(sz(1), 1, sz(3)), Gy4); 141 | 142 | Gz4 = vs_inv * (D(:, :, 2:end) - D(:, :, 1:end-1)); 143 | Gz4 = cat(3, zeros(sz(1), sz(2), 1), Gz4); 144 | 145 | % normalize computed gradients 146 | inv_normG4 = 1 ./ sqrt(Gx4.^2 + Gy4.^2 + Gz4.^2); 147 | Gx4 = Gx4 .* inv_normG4; 148 | Gy4 = Gy4 .* inv_normG4; 149 | Gz4 = Gz4 .* inv_normG4; 150 | 151 | clear inv_normG4 152 | 153 | %% general preparation 154 | 155 | d = 0.001:.001:(vs*T); % x-axis of plot 156 | 157 | ax_limits = [0 T 0 10]; 158 | 159 | %% analyze and visualize our gradients and finite differences vs GT 160 | 161 | % our Gradient-SDF gradients 162 | cos_phi = Gx1.*gx + Gy1.*gy + Gz1.*gz; 163 | phi = acosd(min(abs(cos_phi),1)); 164 | % statistical measures: mean, median, RMSE, 95th percentile 165 | stats1 = phi_statistics(d, D, phi); 166 | 167 | % central finite differences 168 | cos_phi = Gx2.*gx + Gy2.*gy + Gz2.*gz; 169 | phi = acosd(min(abs(cos_phi),1)); 170 | % statistical measures: mean, median, RMSE, 95th percentile 171 | stats2 = phi_statistics(d, D, phi); 172 | 173 | % forward finite differences 174 | cos_phi = Gx3.*gx + Gy3.*gy + Gz3.*gz; 175 | phi = acosd(min(abs(cos_phi),1)); 176 | % statistical measures: mean, median, RMSE, 95th percentile 177 | stats3 = phi_statistics(d, D, phi); 178 | 179 | % backward finite differences 180 | cos_phi = Gx4.*gx + Gy4.*gy + Gz4.*gz; 181 | phi = acosd(min(abs(cos_phi),1)); 182 | % statistical measures: mean, median, RMSE, 95th percentile 183 | stats4 = phi_statistics(d, D, phi); 184 | 185 | clear phi cos_phi 186 | 187 | %% actually visualize 188 | 189 | figure 190 | 191 | hold on 192 | % plot actual evaluation data 193 | plot(NaN, 'w-'); % for legend entry 194 | plot(d / vs, stats1(:,1), '-', 'LineWidth', 1, 'Color', ml_red) % mean 195 | plot(d / vs, stats1(:,2), '-', 'LineWidth', 1, 'Color', ml_blue) % median 196 | plot(d / vs, stats1(:,4), '-', 'LineWidth', 1, 'Color', ml_green) % 95th percentile 197 | plot(NaN, 'w-'); % for legend entry 198 | plot(d / vs, stats2(:,1), '--', 'LineWidth', 1, 'Color', ml_red) % mean 199 | plot(d / vs, stats2(:,2), '--', 'LineWidth', 1, 'Color', ml_blue) % median 200 | plot(d / vs, stats2(:,4), '--', 'LineWidth', 1, 'Color', ml_green) % 95th percentile 201 | % comment following four lines to exclude forward differences as in paper 202 | plot(NaN, 'w-'); % for legend entry 203 | plot(d / vs, stats3(:,1), ':', 'LineWidth', 1, 'Color', ml_red) % mean 204 | plot(d / vs, stats3(:,2), ':', 'LineWidth', 1, 'Color', ml_blue) % median 205 | plot(d / vs, stats3(:,4), ':', 'LineWidth', 1, 'Color', ml_green) % 95th percentile 206 | % comment following four lines to exclude backward differences as in paper 207 | plot(NaN, 'w-'); % for legend entry 208 | plot(d / vs, stats4(:,1), '-.', 'LineWidth', 1, 'Color', ml_red) % mean 209 | plot(d / vs, stats4(:,2), '-.', 'LineWidth', 1, 'Color', ml_blue) % median 210 | plot(d / vs, stats4(:,4), '-.', 'LineWidth', 1, 'Color', ml_green) % 95th percentile 211 | hold off 212 | grid on 213 | 214 | axis(ax_limits) 215 | 216 | legend('Gradient-SDF', ' mean', ' median', ' 95%', ... 217 | 'Central differences', ' mean', ' median', ' 95%', ... 218 | 'Forward differences', ' mean', ' median', ' 95%', ... % comment this line to exclude forward differences 219 | 'Backward differences', ' mean', ' median', ' 95%', ... % comment this line to exclude backward differences 220 | 'Location', 'eastoutside') 221 | 222 | %% cleanup 223 | 224 | clear d ax_limits -------------------------------------------------------------------------------- /matlab/RenderSpheres.m: -------------------------------------------------------------------------------- 1 | % BSD 3-Clause License 2 | % 3 | % This file is part of the code accompanying the paper 4 | % Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 5 | % by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 6 | % 7 | % Copyright (c) 2021, Christiane Sommer and Lu Sang. 8 | % All rights reserved. 9 | % 10 | % Redistribution and use in source and binary forms, with or without 11 | % modification, are permitted provided that the following conditions are met: 12 | % 13 | % * Redistributions of source code must retain the above copyright notice, this 14 | % list of conditions and the following disclaimer. 15 | % 16 | % * Redistributions in binary form must reproduce the above copyright notice, 17 | % this list of conditions and the following disclaimer in the documentation 18 | % and/or other materials provided with the distribution. 19 | % 20 | % * Neither the name of the copyright holder nor the names of its 21 | % contributors may be used to endorse or promote products derived from 22 | % this software without specific prior written permission. 23 | % 24 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | % AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | % IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | % FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | % DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | % SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | % CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | % OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | % OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | 36 | %% setup 37 | 38 | % intrinsics matrix - mimic Kinect 39 | K = [525 0 319.5; 0 525 239.5; 0 0 1]; 40 | 41 | % spheres in format [cx cy cz R] 42 | 43 | % create five random non-intersecting spheres 44 | % comment following code block to load spheres from file instead (if one exists already) 45 | % parameters are chosen to make sure depth values are in Kinect range 46 | spheres = [rand(1, 3)-0.5 0.0625+0.4375*rand(1)]; 47 | while size(spheres,1)<5 48 | c = rand(1, 3) - 0.5; 49 | r = 0.0625 + 0.4375*rand(1); 50 | if all(sqrt(sum((spheres(:,1:3)-c).^2,2)) > (spheres(:,4)+r)) 51 | spheres = [spheres; c r]; 52 | end 53 | end 54 | save('spheres.mat', 'spheres') 55 | 56 | % uncomment next line to load spheres from file 57 | % load('spheres.mat') 58 | 59 | % load poses from file (spiral shape) in TUM RGB-D format 60 | poses = load('poses.txt'); 61 | t_vec = poses(:,2:4); 62 | q_vec = poses(:,[8 5:7]); 63 | clear poses 64 | 65 | % pixel meshgrid 66 | [u,v] = meshgrid(0:639, 0:479); 67 | coeff_u = (u - K(1,3)) / K(1,1); 68 | coeff_v = (v - K(2,3)) / K(2,2); 69 | A = coeff_u.^2 + coeff_v.^2 + 1; 70 | 71 | % output path, should contain subfolders depth/ and rgb/ 72 | out_path = './'; % modify accordingly 73 | % create subfolders if they do not exist 74 | if ~exist([out_path 'depth/'], 'dir') 75 | mkdir([out_path 'depth/']); 76 | end 77 | if ~exist([out_path 'rgb/'], 'dir') 78 | mkdir([out_path 'rgb/']); 79 | end 80 | 81 | color = [0 0.4470 0.7410; 82 | 0.8500 0.3250 0.0980; 83 | 0.9290 0.6940 0.1250; 84 | 0.4940 0.1840 0.5560; 85 | 0.4660 0.6740 0.1880]; 86 | 87 | %% rendering 88 | 89 | for k=1:length(t_vec) 90 | 91 | I = inf(480, 640); % depth image, 1 channel 92 | I_color = zeros(480, 640, 3); % color image, 3 channels 93 | 94 | t = t_vec(k,:); 95 | Q = q_vec(k,:); 96 | 97 | R = quat2rotm(Q); 98 | 99 | c = (spheres(:,1:3) - t) * R; 100 | c_sq_r = c(:,1).^2 + c(:,2).^2 + c(:,3).^2 - spheres(:,4).^2; 101 | 102 | for s=1:size(spheres,1) 103 | I_tmp = zeros(480, 640); 104 | % A, B, and C are coefficients of A*z^2 + B*z + C = 0 105 | B = coeff_u * c(s,1) + coeff_v * c(s,2) + c(s,3); 106 | B = -2 * B; 107 | C = c_sq_r(s); 108 | mask = B.^2 < 4 * A * C; 109 | I_tmp(mask) = inf; 110 | I_tmp(~mask) = .5 * (- B(~mask) - sqrt(B(~mask).^2 - 4 * A(~mask) * C)) ./ A(~mask); 111 | 112 | color_mask = (I >= I_tmp)& ~mask; 113 | 114 | for ch=1:3 115 | color_tmp = I_color(:,:,ch); 116 | color_tmp(color_mask) = color(s,ch); 117 | I_color(:,:,ch) = color_tmp; 118 | end 119 | 120 | I = min(I, I_tmp); 121 | end 122 | 123 | I(isinf(I)) = 0; 124 | I = add_kinect_noise(I); 125 | 126 | % visualize current renderings 127 | subplot(121) 128 | imagesc(I) 129 | axis image 130 | subplot(122) 131 | image(I_color) 132 | axis image 133 | 134 | % write to file 135 | imwrite(uint16(1000 * I), [out_path 'depth/' num2str(k,'%03d') '.png']) 136 | imwrite(I_color, [out_path 'rgb/' num2str(k,'%03d') '.png']) 137 | 138 | pause(.2) 139 | end -------------------------------------------------------------------------------- /matlab/add_kinect_noise.m: -------------------------------------------------------------------------------- 1 | function z = add_kinect_noise(z, mask) 2 | % ADD_KINECT_NOISE adds a Kinect-like noise pattern to a synthetic depth image 3 | % 4 | % z = ADD_KINECT_NOISE(z) 5 | % z = ADD_KINECT_NOISE(z, mask) 6 | % 7 | % Input: 8 | % - z: synthetic depth image 9 | % - mask (optional): values to which noise shall be added, defaul: all >0 10 | % 11 | % Output: 12 | % - z: noisy depth image, same size as original one 13 | % 14 | % Noise model follows Khoshelham et al. 2012 15 | 16 | % BSD 3-Clause License 17 | % 18 | % This file is part of the code accompanying the paper 19 | % Gradient-SDF: A Semi-Implicit Surface Representation for 3D Reconstruction 20 | % by Christiane Sommer*, Lu Sang*, David Schubert, and Daniel Cremers (* denotes equal contribution). 21 | % 22 | % Copyright (c) 2021, Christiane Sommer and Lu Sang. 23 | % All rights reserved. 24 | % 25 | % Redistribution and use in source and binary forms, with or without 26 | % modification, are permitted provided that the following conditions are met: 27 | % 28 | % * Redistributions of source code must retain the above copyright notice, this 29 | % list of conditions and the following disclaimer. 30 | % 31 | % * Redistributions in binary form must reproduce the above copyright notice, 32 | % this list of conditions and the following disclaimer in the documentation 33 | % and/or other materials provided with the distribution. 34 | % 35 | % * Neither the name of the copyright holder nor the names of its 36 | % contributors may be used to endorse or promote products derived from 37 | % this software without specific prior written permission. 38 | % 39 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 40 | % AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 41 | % IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 42 | % DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 43 | % FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 44 | % DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 45 | % SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 46 | % CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 47 | % OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 48 | % OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 49 | 50 | if nargin<2 51 | mask = z>0; 52 | end 53 | 54 | sz = size(z); 55 | 56 | % Compute normalized disparity d 57 | % Eq. (5) and experiments: 58 | % in cm: z^-1 = -2.85e-5 * d + 0.03 59 | % in m: z^-1 = -2.85e-3 * d + 3 60 | % d = (3 - z^-1) / 2.85e-3 61 | d = zeros(sz); 62 | d(mask) = (3 - 1./z(mask)) / 2.85e-3; 63 | 64 | % Add noise to normalized disparity 65 | % sigma_d = 0.5 pixels (p. 1450) 66 | tmp = 0.5 * randn(sz); 67 | d(mask) = d(mask) + tmp(mask); 68 | 69 | % Add quantization to normalized disparity 70 | d = round(d); 71 | 72 | % Go back to z^-1 and z 73 | z_inv = -2.85e-3 * d + 3; 74 | z(mask) = 1 ./ z_inv(mask); -------------------------------------------------------------------------------- /matlab/phi_statistics.m: -------------------------------------------------------------------------------- 1 | function [stats, hist2d] = phi_statistics(d, D, phi, max_phi) 2 | % PHI_STATISTICS computes statistical measures on angular deviation 3 | % 4 | % stats = PHI_STATISTICS(d, D, phi) 5 | % stats = PHI_STATISTICS(d, D, phi, max_phi) 6 | % [stats, hist2d] = PHI_STATISTICS(d, D, phi) 7 | % [stats, hist2d] = PHI_STATISTICS(d, D, phi, max_phi) 8 | % 9 | % Input: 10 | % - d: vector of distances for which the statistics shall be computed 11 | % - D: 3D SDF volume 12 | % - phi: 3D volume of deviation angles (in degrees) 13 | % - max_phi (optional): cut-off value for phi in histogram (default: 2°) 14 | % 15 | % Output: 16 | % - stats: array of size length(d) x 4, with the four columns 17 | % 1: mean of all phi with abs(D) 1 66 | hist2d = zeros(length(bin_edges)-1, size(d,1)); 67 | end 68 | 69 | for k=1:length(d) 70 | idx = abs(D(:)) 1 75 | hist2d(:,k) = histcounts(phi(idx), bin_edges, 'Normalization', 'probability'); 76 | end 77 | end -------------------------------------------------------------------------------- /matlab/poses.txt: -------------------------------------------------------------------------------- 1 | 001 0.127500 -2.003500 -0.125000 0.022285 -0.706756 -0.706756 0.022285 2 | 002 0.250500 -1.998500 -0.120787 0.043938 -0.705740 -0.705740 0.043938 3 | 003 0.369750 -1.986250 -0.116573 0.064982 -0.704115 -0.704115 0.064982 4 | 004 0.492000 -1.965250 -0.112360 0.086567 -0.701788 -0.701788 0.086567 5 | 005 0.621500 -1.928750 -0.108146 0.109844 -0.698523 -0.698523 0.109844 6 | 006 0.759500 -1.872000 -0.103932 0.135394 -0.694023 -0.694023 0.135394 7 | 007 0.907500 -1.788500 -0.099719 0.164613 -0.687679 -0.687679 0.164613 8 | 008 1.062500 -1.674000 -0.095505 0.197386 -0.678998 -0.678998 0.197386 9 | 009 1.214500 -1.535500 -0.091292 0.232180 -0.667902 -0.667902 0.232180 10 | 010 1.340000 -1.413750 -0.087079 0.261798 -0.656858 -0.656858 0.261798 11 | 011 1.405750 -1.399500 -0.082865 0.271290 -0.652994 -0.652994 0.271290 12 | 012 1.447250 -1.439250 -0.078652 0.271522 -0.652898 -0.652898 0.271522 13 | 013 1.534000 -1.350750 -0.074438 0.291036 -0.644436 -0.644436 0.291036 14 | 014 1.635500 -1.178250 -0.070225 0.322352 -0.629356 -0.629356 0.322352 15 | 015 1.721500 -0.994750 -0.066011 0.353550 -0.612375 -0.612375 0.353550 16 | 016 1.790000 -0.825500 -0.061797 0.381118 -0.595608 -0.595608 0.381118 17 | 017 1.845000 -0.676750 -0.057584 0.404972 -0.579653 -0.579653 0.404972 18 | 018 1.890750 -0.544750 -0.053371 0.425157 -0.565014 -0.565014 0.425157 19 | 019 1.929000 -0.422500 -0.049157 0.443293 -0.550900 -0.550900 0.443293 20 | 020 1.959750 -0.304000 -0.044943 0.460154 -0.536897 -0.536897 0.460154 21 | 021 1.982000 -0.187000 -0.040731 0.475931 -0.522963 -0.522963 0.475931 22 | 022 1.997750 -0.061250 -0.036517 0.492193 -0.507687 -0.507687 0.492193 23 | 023 2.003500 0.064500 -0.032303 0.507941 -0.491931 -0.491931 0.507941 24 | 024 2.000250 0.188750 -0.028090 0.522963 -0.475931 -0.475931 0.522963 25 | 025 1.993750 0.310250 -0.023876 0.537124 -0.459888 -0.459888 0.537124 26 | 026 1.977250 0.432000 -0.019663 0.550684 -0.443562 -0.443562 0.550684 27 | 027 1.949000 0.556250 -0.015449 0.564342 -0.426049 -0.426049 0.564342 28 | 028 1.904000 0.687500 -0.011236 0.578752 -0.406258 -0.406258 0.578752 29 | 029 1.835000 0.830000 -0.007023 0.594153 -0.383383 -0.383383 0.594153 30 | 030 1.735750 0.983750 -0.002809 0.610944 -0.356015 -0.356015 0.610944 31 | 031 1.606500 1.140250 0.001404 0.628322 -0.324363 -0.324363 0.628322 32 | 032 1.466500 1.283250 0.005618 0.643818 -0.292400 -0.292400 0.643818 33 | 033 1.388500 1.381250 0.009831 0.652898 -0.271522 -0.271522 0.652898 34 | 034 1.428750 1.422750 0.014045 0.652994 -0.271290 -0.271290 0.652994 35 | 035 1.411000 1.486000 0.018259 0.656672 -0.262262 -0.262262 0.656672 36 | 036 1.268250 1.585750 0.022472 0.667228 -0.234109 -0.234109 0.667228 37 | 037 1.086000 1.680750 0.026686 0.678198 -0.200118 -0.200118 0.678198 38 | 038 0.909000 1.758250 0.030899 0.687085 -0.167075 -0.167075 0.687085 39 | 039 0.750250 1.819750 0.035112 0.693630 -0.137394 -0.137394 0.693630 40 | 040 0.609500 1.869250 0.039325 0.698348 -0.110948 -0.110948 0.698348 41 | 041 0.482000 1.910000 0.043540 0.701704 -0.087247 -0.087247 0.701704 42 | 042 0.363500 1.944250 0.047753 0.704083 -0.065329 -0.065329 0.704083 43 | 043 0.245000 1.972750 0.051966 0.705762 -0.043588 -0.043588 0.705762 44 | 044 0.123750 1.991500 0.056180 0.706767 -0.021933 -0.021933 0.706767 45 | 045 0.000000 2.000000 0.060394 0.707107 0.000000 0.000000 0.707107 46 | 046 -0.127500 2.003500 0.064607 0.706756 0.022285 0.022285 0.706756 47 | 047 -0.250500 1.998500 0.068820 0.705740 0.043938 0.043938 0.705740 48 | 048 -0.369750 1.986250 0.073034 0.704115 0.064982 0.064982 0.704115 49 | 049 -0.492000 1.965250 0.077247 0.701788 0.086567 0.086567 0.701788 50 | 050 -0.621500 1.928750 0.081461 0.698523 0.109844 0.109844 0.698523 51 | 051 -0.759500 1.872000 0.085674 0.694023 0.135394 0.135394 0.694023 52 | 052 -0.907500 1.788500 0.089888 0.687679 0.164613 0.164613 0.687679 53 | 053 -1.062500 1.674000 0.094101 0.678998 0.197386 0.197386 0.678998 54 | 054 -1.214500 1.535500 0.098314 0.667902 0.232180 0.232180 0.667902 55 | 055 -1.339750 1.413750 0.102528 0.656858 0.261798 0.261798 0.656858 56 | 056 -1.405750 1.399500 0.106742 0.652994 0.271290 0.271290 0.652994 57 | 057 -1.447250 1.439250 0.110955 0.652898 0.271522 0.271522 0.652898 58 | 058 -1.534000 1.350750 0.115168 0.644436 0.291036 0.291036 0.644436 59 | 059 -1.635500 1.178250 0.119382 0.629422 0.322224 0.322224 0.629422 60 | 060 -1.721500 0.994750 0.123595 0.612375 0.353550 0.353550 0.612375 61 | 061 -1.790000 0.825500 0.127809 0.595608 0.381118 0.381118 0.595608 62 | 062 -1.845000 0.676750 0.132022 0.579653 0.404972 0.404972 0.579653 63 | 063 -1.890750 0.544750 0.136236 0.565014 0.425157 0.425157 0.565014 64 | 064 -1.929000 0.422500 0.140450 0.550900 0.443293 0.443293 0.550900 65 | 065 -1.959750 0.304000 0.144663 0.536897 0.460154 0.460154 0.536897 66 | 066 -1.982000 0.187000 0.148876 0.522963 0.475931 0.475931 0.522963 67 | 067 -1.997750 0.061250 0.153090 0.507687 0.492193 0.492193 0.507687 68 | 068 -2.003500 -0.064500 0.157304 0.491931 0.507941 0.507941 0.491931 69 | 069 -2.000250 -0.188750 0.161517 0.475931 0.522963 0.522963 0.475931 70 | 070 -1.993750 -0.310250 0.165730 0.459888 0.537124 0.537124 0.459888 71 | 071 -1.977250 -0.432000 0.169944 0.443562 0.550684 0.550684 0.443562 72 | 072 -1.949000 -0.556250 0.174157 0.426049 0.564342 0.564342 0.426049 73 | 073 -1.904000 -0.687500 0.178370 0.406258 0.578752 0.578752 0.406258 74 | 074 -1.835000 -0.830000 0.182585 0.383383 0.594153 0.594153 0.383383 75 | 075 -1.735750 -0.983750 0.186798 0.356015 0.610944 0.610944 0.356015 76 | 076 -1.606500 -1.140250 0.191011 0.324363 0.628322 0.628322 0.324363 77 | 077 -1.466500 -1.283250 0.195225 0.292400 0.643818 0.643818 0.292400 78 | 078 -1.388500 -1.381250 0.199438 0.271522 0.652898 0.652898 0.271522 79 | 079 -1.428750 -1.422750 0.203652 0.271290 0.652994 0.652994 0.271290 80 | 080 -1.411000 -1.486000 0.207866 0.262262 0.656672 0.656672 0.262262 81 | 081 -1.268250 -1.585750 0.212079 0.234109 0.667228 0.667228 0.234109 82 | 082 -1.086000 -1.680750 0.216292 0.200118 0.678198 0.678198 0.200118 83 | 083 -0.909000 -1.758250 0.220505 0.167075 0.687085 0.687085 0.167075 84 | 084 -0.750250 -1.819750 0.224719 0.137394 0.693630 0.693630 0.137394 85 | 085 -0.609500 -1.869250 0.228933 0.110948 0.698348 0.698348 0.110948 86 | 086 -0.482000 -1.910000 0.233146 0.087247 0.701704 0.701704 0.087247 87 | 087 -0.363500 -1.944250 0.237360 0.065329 0.704083 0.704083 0.065329 88 | 088 -0.245000 -1.972750 0.241573 0.043588 0.705762 0.705762 0.043588 89 | 089 -0.123750 -1.991500 0.245786 0.021933 0.706767 0.706767 0.021933 90 | 090 0.000000 -2.000000 0.250000 0.000000 0.707107 0.707107 0.000000 91 | --------------------------------------------------------------------------------