├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── config ├── config_basket_LED.json ├── config_skorates.json └── config_tumrgbd.json ├── cpp ├── .gitignore ├── CMakeLists.txt ├── include │ ├── ConfigLoader.h │ ├── Timer.h │ ├── img_loader │ │ ├── ImageLoader.h │ │ ├── MultiviewLoader.h │ │ ├── SynthLoader.h │ │ ├── TumrgbdLoader.h │ │ └── img_loader.h │ ├── mat.h │ ├── normals │ │ └── NormalEstimator.h │ ├── ps_optimizer │ │ ├── Auxilary.h │ │ ├── LedOptimizer.cpp │ │ ├── LedOptimizer.h │ │ ├── LedOptimizerJa.cpp │ │ ├── Optimizer.cpp │ │ ├── Optimizer.h │ │ ├── OptimizerAux.cpp │ │ ├── OptimizerSettings.h │ │ ├── PsOptimizer.cpp │ │ ├── PsOptimizer.h │ │ ├── PsOptimizerJa.cpp │ │ └── SharpDetector.h │ └── sdf_tracker │ │ ├── RigidOptimizer.h │ │ ├── RigidPointOptimizer.cpp │ │ ├── RigidPointOptimizer.h │ │ ├── RigidPointOptimizerOmp.cpp │ │ ├── Sdf.h │ │ ├── Sdfvoxel.h │ │ ├── TrackingSettings.h │ │ ├── VolumetricGradSdf.cpp │ │ ├── VolumetricGradSdf.h │ │ ├── VoxelGrid.cpp │ │ └── VoxelGrid.h ├── third │ └── mesh │ │ ├── GradMarchingCubes.cpp │ │ ├── GradMarchingCubes.h │ │ ├── LayeredMarchingCubes.cpp │ │ ├── LayeredMarchingCubes.h │ │ ├── MarchingCubes.cpp │ │ ├── MarchingCubes.h │ │ ├── MarchingCubesNoColor.cpp │ │ └── MarchingCubesNoColor.h └── voxel_ps │ ├── CMakeLists.txt │ └── src │ └── main_ps.cpp ├── data └── sokrates-mvs │ ├── color000001.png │ ├── color000002.png │ ├── color000003.png │ ├── color000004.png │ ├── color000005.png │ ├── color000006.png │ ├── color000007.png │ ├── color000008.png │ ├── color000009.png │ ├── color000010.png │ ├── color000011.png │ ├── color000012.png │ ├── color000013.png │ ├── color000014.png │ ├── color000015.png │ ├── color000016.png │ ├── color000017.png │ ├── color000018.png │ ├── color000019.png │ ├── color000020.png │ ├── color000021.png │ ├── color000022.png │ ├── color000023.png │ ├── color000024.png │ ├── color000025.png │ ├── color000026.png │ ├── color000027.png │ ├── color000028.png │ ├── color000029.png │ ├── color000030.png │ ├── color000031.png │ ├── color000032.png │ ├── color000033.png │ ├── color000034.png │ ├── depth000001.png │ ├── depth000002.png │ ├── depth000003.png │ ├── depth000004.png │ ├── depth000005.png │ ├── depth000006.png │ ├── depth000007.png │ ├── depth000008.png │ ├── depth000009.png │ ├── depth000010.png │ ├── depth000011.png │ ├── depth000012.png │ ├── depth000013.png │ ├── depth000014.png │ ├── depth000015.png │ ├── depth000016.png │ ├── depth000017.png │ ├── depth000018.png │ ├── depth000019.png │ ├── depth000020.png │ ├── depth000021.png │ ├── depth000022.png │ ├── depth000023.png │ ├── depth000024.png │ ├── depth000025.png │ ├── depth000026.png │ ├── depth000027.png │ ├── depth000028.png │ ├── depth000029.png │ ├── depth000030.png │ ├── depth000031.png │ ├── depth000032.png │ ├── depth000033.png │ ├── depth000034.png │ ├── intrinsics.txt │ └── pose.txt ├── pipeline.png └── reconstruction.png /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | .autosave 3 | cpp/build/* 4 | cpp/voxel_ps/bin/* 5 | -------------------------------------------------------------------------------- /.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 | 14 | [submodule "cpp/third/json"] 15 | path = cpp/third/json 16 | url = https://github.com/nlohmann/json.git 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, Lu Sang 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 | ## High-Quality RGB-D Reconstruction via Multi-View Uncalibrated Photometric Stereo and Gradient-SDF 2 | 3 | WACV 2023 [paper](https://arxiv.org/abs/2210.12202) 4 | --- 5 | Enable detailed RGB-D data 3D reconstruction. Jointly estimate camera pose, geometry, albedo and environment lighting under natural light or point-light-source. 6 | 7 | ![method pipeline](pipeline.png) 8 | 9 | # clone repository 10 | ``` 11 | git clone https://github.com/Sangluisme/PSgradientSDF.git 12 | ``` 13 | please use 14 | ``` 15 | git submodule update --init --recursive 16 | ``` 17 | to pull our dependency. 18 | 19 | # code structure 20 | ``` 21 | cpp (basic code folder) 22 | |---include 23 | |---third 24 | |---voxel_ps 25 | | |---bin 26 | | |---src 27 | | |---CMakeLsits.txt 28 | |---CMakeLists.txt 29 | 30 | config (example config files) 31 | |---config.json 32 | |---... 33 | 34 | data (demo data folder) 35 | |---demo_data1 36 | |---demo_data2 37 | 38 | results (default results save path) 39 | |---... 40 | 41 | ``` 42 | # setup 43 | please create a `build` folder under the [cpp/](cpp/) folder path then do 44 | ``` 45 | cd cuild 46 | cmake .. 47 | make 48 | ``` 49 | it will build a binary file under the path [cpp/voxel_ps/bin/](cpp/voxel_ps/bin/). 50 | 51 | # usage 52 | ``` 53 | cd ./cpp/voxel_ps/bin/ 54 | ./voxelPS --config_file 55 | ``` 56 | 57 | # config file 58 | All the parameters you can control are listed in the example config files in the [config](config/) folder. 59 | Here is some explanation of the parameters. 60 | 61 | - **input**: data folder path 62 | - **output**: results saving path 63 | - pose filename: ground true pose or pre-calculated pose file name under input path (optional) 64 | - **datatype**: support `tum` (TUM_RGBD sequence), `synth`, `multiview` differences explained in next section 65 | - first: start frame number 66 | - last: end frame number 67 | - voxel size: voxel size in `m` (choose a larger one if the algorithm is too slow) 68 | - sharpness threshold: criteria of selecting the keyframe 69 | - **model type**: `SH1`, `SH2`(not recommend), `LED` (1st, 2nd spherical harmonics, point-light-source) 70 | - reg albedo: regularizer for albedo (set to 0) 71 | - reg norm: Eikonal regularizer 72 | - reg laplacian: regularizer for distance laplacian (set to 0) 73 | - lambda: loss function parameters 74 | - --light: bool if update light 75 | - --albedo: bool if update albedo 76 | - --distance: bool if update SDF distance 77 | - --pose: bool if update pose 78 | 79 | please note the **bold** parameters are required, others are optional. 80 | 81 | # data type 82 | **The main difference between each data type is the structure of the data folder** 83 | The general requirement is: 84 | - contain depth and RGB images, together with `intrinsics.txt` in the same folder. 85 | - If the pose is unknown, the images should be like a video sequence that allow camera tracking, otherwise initial camera pose file should be provided. 86 | 87 | `tum` -- TUM_RGBD (please refer to https://vision.in.tum.de/data/datasets/rgbd-dataset/download for detail) 88 | 89 | should have a structure 90 | ``` 91 | data 92 | |---depth (folder) 93 | | |---depth_timestamp1.png 94 | | |---depth_timestamp2.png 95 | | |---... 96 | | 97 | |---rgb (folder) 98 | | |---rgb_timestamp1.png 99 | | |---rgb_timestamp2.png 100 | | |---... 101 | | 102 | |---depth.txt 103 | |---rgb.txt 104 | |---associated.txt 105 | |---intrinsics.txt 106 | 107 | ``` 108 | `multiview` -- intrinsic3d data should have the structure (some multi view data has too large baseline, thus initial poses are needed) 109 | ``` 110 | data 111 | |---color0000001.png 112 | |---color0000002.png 113 | |---color0000003.png 114 | |---... 115 | |---depth0000001.png 116 | |---depth0000001.png 117 | |---depth0000001.png 118 | |---... 119 | |---intrinsics.txt 120 | |---pose.txt 121 | ``` 122 | `synth` -- the synthetic data or point-light-source data which is recorded using the set-up mentioned in the paper 123 | 124 | ``` 125 | data 126 | |---depth 127 | | |---001.png 128 | | |---002.png 129 | | |---003.png 130 | | |---... 131 | |---rgb 132 | | |---001.png 133 | | |---003.png 134 | | |---003.png 135 | | |---... 136 | |---intrinsics.txt 137 | ``` 138 | **To use your own data, just convert your data to either one of the structures and specify the corresponding data type in the `config.json` file**. 139 | 140 | # trouble shooting 141 | 142 | - **compile error of `Sophus`**: we use an older version of sophus, just commit back to the version shows in the git repository. 143 | - **too slow/out of memory**: disable `upsampling` in `config.json` or increase the voxel size. 144 | - **reconstruction size**: it is controlled by two factors, voxel size (per voxel, in meters) and voxel grid size, the default is 128x128x128. The actual reconstruction range will be voxel size times voxel grid size, e.g. 0.02*128 = 2.56 m (one edge). If you would like to use a bigger voxel grid size, change it here https://github.com/Sangluisme/PSgradientSDF/blob/164869288ffa2cca0162e79e262614d9309da57d/cpp/voxel_ps/src/main_ps.cpp#L123. 145 | - **voxel size**: changing voxel size will influence the reconstruction details since a smaller voxel size means each voxel only represents a smaller area. This will not affect the memory but a smaller voxel size means under the same voxel grid size, the reconstruction area will be small. 146 | - **voxel grid size**: currently is hard coded as 128x128x128. Please change here if you want https://github.com/Sangluisme/PSgradientSDF/blob/164869288ffa2cca0162e79e262614d9309da57d/cpp/voxel_ps/src/main_ps.cpp#L123. Larger voxel grid size will cause large memory consumptions, but also allows a larger reconstruction range. 147 | 148 | # citation 149 | ``` 150 | @inproceedings{sang2023high, 151 | author = {L Sang and B Haefner and X Zuo and D Cremers}, 152 | title = {High-Quality RGB-D Reconstruction via Multi-View Uncalibrated Photometric Stereo and Gradient-SDF}, 153 | booktitle = {IEEE Winter Conference on Applications of Computer Vision (WACV)}, 154 | month = {January}, 155 | address = {Hawaii, USA}, 156 | year = {2023}, 157 | eprint = {2210.12202}, 158 | eprinttype = {arXiv}, 159 | eprintclass = {cs.CV}, 160 | copyright = {Creative Commons Attribution Non Commercial No Derivatives 4.0 International}, 161 | keywords = {3d-reconstruction,rgb-d,photometry}, 162 | } 163 | ``` 164 | -------------------------------------------------------------------------------- /config/config_basket_LED.json: -------------------------------------------------------------------------------- 1 | { 2 | "input": "/storage/user/sang/datasets/LED_dataset/basket_LED/", 3 | "output": "/storage/user/sang/eccv_results/LED_dataset/basket_LED/", 4 | "datatype": "synth", 5 | "first": 400, 6 | "last": 550, 7 | "voxel size": 0.01, 8 | "truncation factor": 5, 9 | "zmin": 0.5, 10 | "zmax": 3.5, 11 | "sharpness threshold": 0.03, 12 | "model type": "LED", 13 | "loss function": "cauchy", 14 | "reg albedo": 0.0, 15 | "reg norm": 0.1, 16 | "reg laplacian": 5.0, 17 | "max iter": 100, 18 | "damping": 3.0, 19 | "converge threshold": 5e-3, 20 | "lambda": 0.2, 21 | "upsample":true, 22 | "--light": true, 23 | "--albedo": true, 24 | "--distance": true, 25 | "--pose": true 26 | } 27 | -------------------------------------------------------------------------------- /config/config_skorates.json: -------------------------------------------------------------------------------- 1 | { 2 | "input": "../data/skorates-mvs/", 3 | "output": "../results/", 4 | "pose filename": "pose.txt", 5 | "datatype": "synth", 6 | "first": 0, 7 | "last": 34, 8 | "voxel size": 0.004, 9 | "truncation factor": 5, 10 | "zmin": 0.5, 11 | "zmax": 3.5, 12 | "sharpness threshold": 0.00, 13 | "model type": "SH1", 14 | "loss function": "cauchy", 15 | "reg albedo": 0.0, 16 | "reg norm": 10.0, 17 | "reg laplacian": 0.0, 18 | "max iter": 100, 19 | "damping": 1.0, 20 | "converge threshold": 5e-3, 21 | "lambda": 0.2, 22 | "upsample":false, 23 | "--light": true, 24 | "--albedo": true, 25 | "--distance": true, 26 | "--pose": true 27 | } 28 | -------------------------------------------------------------------------------- /config/config_tumrgbd.json: -------------------------------------------------------------------------------- 1 | { 2 | "input": "please down load the sequence", 3 | "output": "../results/", 4 | "pose filename": " ", 5 | "datatype": "tum", 6 | "first": 50, 7 | "last": 250, 8 | "voxel size": 0.02, 9 | "truncation factor": 5, 10 | "zmin": 0.5, 11 | "zmax": 3.5, 12 | "sharpness threshold": 0.00, 13 | "model type": "SH1", 14 | "loss function": "cauchy", 15 | "reg albedo": 0.0, 16 | "reg norm": 10.0, 17 | "reg laplacian": 0.0, 18 | "max iter": 30, 19 | "damping": 1.0, 20 | "converge threshold": 5e-3, 21 | "lambda": 0.2, 22 | "upsample": false, 23 | "--light": true, 24 | "--albedo": true, 25 | "--distance": true, 26 | "--pose": true 27 | } 28 | -------------------------------------------------------------------------------- /cpp/.gitignore: -------------------------------------------------------------------------------- 1 | !build/ 2 | build/* 3 | build/ 4 | build/CMakeFiles/* 5 | */bin/ 6 | *.ply 7 | 8 | -------------------------------------------------------------------------------- /cpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 3.4) 2 | 3 | PROJECT( voxelPS ) 4 | SET(CMAKE_BUILD_TYPE RelWithDebInfo) 5 | 6 | # ROOTs 7 | SET(CERES_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/third/build-ceres-solver/") 8 | 9 | # path to CMakeModules 10 | SET(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") 11 | 12 | ########## INCLUDED LIBRARIES ########## 13 | 14 | # Eigen 15 | #SET(EIGEN_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third/eigen/include/eigen3") 16 | SET(EIGEN_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third/eigen/") 17 | # FIND_PACKAGE(Eigen REQUIRED) 18 | INCLUDE_DIRECTORIES( ${EIGEN_INCLUDE_DIR}) 19 | 20 | 21 | # OpenCV 22 | FIND_PACKAGE( OpenCV REQUIRED ) 23 | MESSAGE(STATUS "Found OpenCV headers in: ${OpenCV_INCLUDE_DIRS}") 24 | INCLUDE_DIRECTORIES( ${OpenCV_INCLUDE_DIRS} ) 25 | 26 | # Sophus 27 | SET(SOPHUS_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/third/Sophus/") 28 | 29 | # INCLUDE_DIRECTORIES( third/Sophus/ ) 30 | MESSAGE(STATUS "sophus dir in: ${SOPHUS_INCLUDE_DIR}") 31 | # FIND_PACKAGE(Sophus REQUIRED) 32 | INCLUDE_DIRECTORIES( ${SOPHUS_INCLUDE_DIR}) 33 | 34 | 35 | # CLI, command line parser 36 | INCLUDE_DIRECTORIES( third/CLI11/include/ ) 37 | 38 | # nlohmann json parser 39 | INCLUDE_DIRECTORIES(third/json/include/) 40 | 41 | ########## CUSTOM LIBRARIES ########## 42 | 43 | INCLUDE_DIRECTORIES( include/ 44 | third/ 45 | ) 46 | 47 | # marching cubes 48 | SET( LIB_NAME mc_lib ) 49 | ADD_LIBRARY( ${LIB_NAME} 50 | third/mesh/GradMarchingCubes.cpp 51 | third/mesh/MarchingCubes.cpp 52 | third/mesh/MarchingCubesNoColor.cpp 53 | ) 54 | TARGET_COMPILE_OPTIONS( ${LIB_NAME} PRIVATE -std=c++17 ) 55 | 56 | 57 | 58 | # SDF-Tracking stuff 59 | SET( LIB_NAME sdf_tracker_lib ) 60 | ADD_LIBRARY( ${LIB_NAME} 61 | include/sdf_tracker/VoxelGrid.cpp 62 | include/sdf_tracker/VolumetricGradSdf.cpp 63 | # include/sdf_tracker/VolumetricGradPixelSdf.cpp 64 | include/sdf_tracker/RigidPointOptimizer.cpp 65 | ) 66 | TARGET_LINK_LIBRARIES( ${LIB_NAME} 67 | ${OpenCV_LIBS} 68 | mc_lib 69 | # ${CERES_LIBRARIES} # only needed if RigidPointOptimizerCeres.cpp included 70 | ) 71 | TARGET_COMPILE_OPTIONS( ${LIB_NAME} PRIVATE -std=c++17 ) 72 | 73 | # Color Optimization 74 | SET( LIB_NAME ps_lib ) 75 | ADD_LIBRARY( ${LIB_NAME} 76 | include/ps_optimizer/Optimizer.cpp 77 | include/ps_optimizer/OptimizerAux.cpp 78 | include/ps_optimizer/PsOptimizer.cpp 79 | include/ps_optimizer/PsOptimizerJa.cpp 80 | include/ps_optimizer/LedOptimizer.cpp 81 | include/ps_optimizer/LedOptimizerJa.cpp 82 | ) 83 | TARGET_LINK_LIBRARIES( ${LIB_NAME} 84 | ${OpenCV_LIBS} 85 | mc_lib 86 | sdf_tracker_lib 87 | ) 88 | 89 | 90 | SET( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp" ) 91 | 92 | 93 | ########## DIRECTORIES ########## 94 | ADD_SUBDIRECTORY(voxel_ps) 95 | 96 | -------------------------------------------------------------------------------- /cpp/include/ConfigLoader.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : ConfigLoader.h 3 | // Author : Lu Sang 4 | // Date : 04/2022 5 | // License : GNU General Public License 6 | // Description : load config json file 7 | //============================================================================ 8 | 9 | #include "ps_optimizer/OptimizerSettings.h" 10 | #include "sdf_tracker/TrackingSettings.h" 11 | #include "img_loader/img_loader.h" 12 | #include 13 | 14 | using json = nlohmann::json; 15 | 16 | bool LoadConfig(const std::string& filepath, TrackingSettings** trac_settings, OptimizerSettings** opt_settings, ImageLoader** Loader, json& config) 17 | { 18 | std::ifstream file; 19 | file.open(filepath.c_str()); 20 | if(!file.is_open()){ 21 | std::cout << "can't load config file!" << std::endl; 22 | return false; 23 | } 24 | 25 | // read json file 26 | file >> config; 27 | 28 | //check necessary argument 29 | if(!config.contains("input") || !config.contains("output") || ! config.contains("datatype")){ 30 | std::cout << "missing necessary input arguments (input/out folder/datatype) in config file!" << std::endl; 31 | return false; 32 | } 33 | 34 | std::string input = config.at("input"); 35 | std::string output = config.at("output"); 36 | std::string datatype = config.at("datatype"); 37 | 38 | ImageLoader* loader; 39 | 40 | // parse datatype 41 | DataType DT; 42 | if (datatype == "tum") { 43 | DT = DataType::TUM_RGBD; 44 | loader = new TumrgbdLoader(input); 45 | } 46 | else if (datatype == "led" || datatype == "synth") { 47 | DT = DataType::SYNTH; 48 | loader = new SynthLoader(input); 49 | } 50 | else if (datatype == "intrinsic3d" || datatype == "multiview") { 51 | DT = DataType::MULTIVIEW; 52 | loader = new MultiviewLoader(input); 53 | } 54 | else { 55 | std::cerr << "Your specified dataset type is not supported (yet)." << std::endl; 56 | return false; 57 | } 58 | 59 | // consrtuct tracking settings 60 | TrackingSettings* trac_ = new TrackingSettings(input, output, DT); 61 | *Loader = loader; 62 | 63 | // check rest input 64 | if(config.contains("pose filename"))trac_->pose_file_ = config.at("pose filename"); 65 | if(config.contains("first")) trac_->first_ = config.at("first"); 66 | if(config.contains("last")) trac_->last_ = config.at("last"); 67 | if(config.contains("voxel size")) trac_->voxel_size_ = config.at("voxel size"); 68 | if(config.contains("truncation factor")) trac_->truncation_factor_ = config.at("truncation factor"); 69 | if(config.contains("sharpness threshold")) trac_->sharpness_threshold_ = config.at("sharpness threshold"); 70 | if(config.contains("zmin"))trac_->zmin_ = config.at("zmin"); 71 | if(config.contains("zmax"))trac_->zmax_ = config.at("zmax"); 72 | 73 | *trac_settings = trac_; 74 | 75 | // parse optimizer argument 76 | float reg_r, reg_n, reg_l; 77 | std::string loss_func, mtype; 78 | LossFunction loss_fun; 79 | 80 | // construct optimizer settings 81 | OptimizerSettings* opt_ = new OptimizerSettings(); 82 | 83 | if(config.contains("model type")){ 84 | mtype = config.at("model type"); 85 | 86 | // parse model type 87 | ModelType Model; 88 | if(mtype == "SH1"){ 89 | Model = ModelType::SH1; 90 | opt_->model = Model; 91 | opt_->order = 1; 92 | } 93 | else if(mtype == "SH2"){ 94 | Model = ModelType::SH2; 95 | opt_->model = Model; 96 | opt_->order = 2; 97 | } 98 | else if(mtype == "LED"){ 99 | Model = ModelType::LED; 100 | opt_->model = Model; 101 | } 102 | else 103 | { 104 | std::cerr << "Your specified model type is not supported (yet)." << std::endl; 105 | return false; 106 | } 107 | } 108 | 109 | // parse loss function 110 | if(config.contains("loss function")){ 111 | loss_func = config.at("loss function"); 112 | LossFunction Loss; 113 | if(loss_func == "cauchy"){ 114 | Loss = LossFunction::CAUCHY; 115 | opt_->loss = Loss; 116 | } 117 | else if(loss_func == "l2"){ 118 | Loss = LossFunction::L2; 119 | opt_->loss = Loss; 120 | } 121 | else if(loss_func == "huber"){ 122 | Loss = LossFunction::HUBER; 123 | opt_->loss = Loss; 124 | } 125 | else if(loss_func == "trunc_l2"){ 126 | Loss == LossFunction::TRUNC_L2; 127 | opt_->loss = Loss; 128 | } 129 | else if(loss_func == "tukey"){ 130 | Loss == LossFunction::TUKEY; 131 | opt_->loss = Loss; 132 | } 133 | else 134 | { 135 | std::cerr << "Your specified loss function type is not supported (yet)." << std::endl; 136 | return false; 137 | } 138 | 139 | } 140 | 141 | //check the rest settings 142 | if(config.contains("reg albedo"))opt_->reg_weight_rho = config.at("reg albedo"); 143 | if(config.contains("reg norm"))opt_->reg_weight_n =config.at("reg norm"); 144 | if(config.contains("reg laplacian")) opt_->reg_weight_l = config.at("reg laplacian"); 145 | if(config.contains("max iter")) opt_->max_it = config.at("max iter"); 146 | if(config.contains("damping")) opt_->damping = config.at("damping"); 147 | if(config.contains("converge threshold")) opt_->conv_threshold = config.at("converge threshold"); 148 | if(config.contains("upsample")) opt_->upsample = config.at("upsample"); 149 | 150 | 151 | if(config.contains("lambda")){ 152 | float lambda = config.at("lambda"); 153 | opt_->lambda = lambda; 154 | opt_->lambda_sq = lambda * lambda; 155 | } 156 | 157 | *opt_settings = opt_; 158 | 159 | // save this config file to results folder for reference 160 | // std::string save_path = (output + "config.json").c_str(); 161 | std::ofstream save_conf(output + "saved_config.json", std::fstream::out); 162 | if(!save_conf.is_open()){ 163 | std::cout << "could not save config file." << std::endl; 164 | } 165 | save_conf << std::setw(4) << config; 166 | 167 | 168 | return true; 169 | 170 | } -------------------------------------------------------------------------------- /cpp/include/Timer.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : Timer.h 3 | // Author : Christiane Sommer 4 | // Date : 07/2018 5 | // License : GNU General Public License 6 | // Description : simple class to time processes on the CPU 7 | //============================================================================ 8 | 9 | #ifndef TIMER_H_ 10 | #define TIMER_H_ 11 | 12 | // includes 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | class Timer { 19 | 20 | private: 21 | double start_time; 22 | double end_time; 23 | double elapsed; 24 | 25 | public: 26 | Timer() : start_time(0.), end_time(0.), elapsed(0.) {} 27 | ~Timer() {} 28 | 29 | void tic() { 30 | start_time = omp_get_wtime(); 31 | } 32 | 33 | double toc(std::string s = "Time elapsed") { 34 | if (start_time!=0) { 35 | end_time = omp_get_wtime(); 36 | elapsed = end_time-start_time; 37 | print_time(s); 38 | } 39 | else 40 | std::cout << "Timer was not started, no time could be measured." << std::endl; 41 | return elapsed; 42 | } 43 | 44 | void print_time(std::string s = "Time elapsed") { 45 | if (elapsed<1.) 46 | std::cout << "---------- " << s << ": " << 1000.*elapsed << "ms." << std::endl; 47 | else 48 | std::cout << "---------- " << s << ": " << elapsed << "s." << std::endl; 49 | } 50 | 51 | }; 52 | 53 | #endif // TIMER_H_ 54 | -------------------------------------------------------------------------------- /cpp/include/img_loader/ImageLoader.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : ImageLoader.h 3 | // Author : Lu sang, Christiane Sommer 4 | // Date : 10/2019, 06/2022 5 | // License : GNU General Public License 6 | // Description : class ImageLoader 7 | //============================================================================ 8 | 9 | #ifndef IMAGE_LOADER_H_ 10 | #define IMAGE_LOADER_H_ 11 | 12 | // includes 13 | #include 14 | #include 15 | #include 16 | // library includes 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | class ImageLoader { 23 | 24 | protected: 25 | 26 | Eigen::Matrix3f K_; 27 | 28 | const float unit_; // in meters 29 | const std::string path_; // path to directory containing data 30 | std::string timestamp_rgb_, timestamp_depth_; // current timestamp 31 | bool consecutive_numbers_ ; 32 | std::vector timestamps_depth_, timestamps_rgb_; 33 | 34 | 35 | public: 36 | 37 | 38 | ImageLoader() : 39 | unit_(1.), 40 | path_(""), 41 | timestamp_rgb_(""), 42 | timestamp_depth_(""), 43 | consecutive_numbers_(false) 44 | {} 45 | 46 | ImageLoader(float unit, bool consecutive_numbers) : 47 | unit_(unit), 48 | path_(""), 49 | timestamp_rgb_(""), 50 | timestamp_depth_(""), 51 | consecutive_numbers_(consecutive_numbers) 52 | {} 53 | 54 | ImageLoader(const std::string& path, bool consecutive_numbers) : 55 | unit_(1.), 56 | path_(path), 57 | timestamp_rgb_(""), 58 | timestamp_depth_(""), 59 | consecutive_numbers_(consecutive_numbers) 60 | {} 61 | 62 | ImageLoader(float unit, const std::string& path, bool consecutive_numbers) : 63 | unit_(unit), 64 | path_(path), 65 | timestamp_rgb_(""), 66 | timestamp_depth_(""), 67 | consecutive_numbers_(consecutive_numbers) 68 | {} 69 | 70 | virtual ~ImageLoader() {} 71 | 72 | Eigen::Matrix3f K() const { 73 | return K_; 74 | } 75 | 76 | std::string rgb_timestamp() { 77 | return timestamp_rgb_; 78 | } 79 | 80 | std::string depth_timestamp() { 81 | return timestamp_depth_; 82 | } 83 | 84 | 85 | std::string getDepthTimestamp(const int i){ 86 | return timestamps_depth_[i]; 87 | } 88 | 89 | std::string getRgbTimestamp(const int i){ 90 | return timestamps_rgb_[i]; 91 | } 92 | 93 | std::vector getDepthTimestamps(){ 94 | return timestamps_depth_; 95 | } 96 | 97 | std::vector getRgbTimestamps(){ 98 | return timestamps_rgb_; 99 | } 100 | 101 | void setDepthTimestamps(std::vector timestamps){ 102 | timestamps_depth_ = timestamps; 103 | } 104 | 105 | void setRgbTimestamps(std::vector timestamps){ 106 | timestamps_rgb_ = timestamps; 107 | } 108 | 109 | bool load_intrinsics(const std::string& filename = "intrinsics.txt") { 110 | 111 | if (filename.empty()) 112 | return false; 113 | 114 | std::ifstream infile(path_ + filename); 115 | if (!infile.is_open()) 116 | return false; 117 | 118 | // load intrinsic camera matrix 119 | float tmp = 0; 120 | for (size_t i=0; i<3; ++i) for (size_t j=0; j<3; ++j) { 121 | infile >> tmp; 122 | K_(i, j) = tmp; 123 | } 124 | 125 | infile.close(); 126 | 127 | return true; 128 | } 129 | 130 | bool load_depth(const std::string& filename, cv::Mat& depth) { 131 | 132 | if (filename.empty()) { 133 | std::cerr << "Error: missing filename" << std::endl; 134 | return false; 135 | } 136 | 137 | // fill/read 16 bit depth image 138 | cv::Mat depthIn = cv::imread(path_ + filename, cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR); 139 | if (depthIn.empty()) { 140 | std::cerr << "Error: empty depth image " << path_ + filename << std::endl; 141 | return false; 142 | } 143 | depthIn.convertTo(depth, CV_32FC1, unit_); 144 | 145 | return true; 146 | } 147 | 148 | bool load_gray(const std::string& filename, cv::Mat& gray){ 149 | if (filename.empty()) { 150 | std::cerr << "Error: missing filename" << std::endl; 151 | return false; 152 | } 153 | 154 | // load gray 155 | cv::Mat imgGray = cv::imread(filename, cv::IMREAD_GRAYSCALE); 156 | // convert gray to float 157 | imgGray.convertTo(gray, CV_32FC1, 1.0f / 255.0f); 158 | 159 | if(gray.empty()) { 160 | std::cerr << "Error: empty gray scale image " << path_ + filename << std::endl; 161 | return false; 162 | } 163 | return true; 164 | 165 | } 166 | 167 | bool load_color(const std::string& filename, cv::Mat& color) { 168 | 169 | if (filename.empty()) { 170 | std::cerr << "Error: missing filename" << std::endl; 171 | return false; 172 | } 173 | 174 | // load color 175 | cv::Mat imgColor = cv::imread(path_ + filename); 176 | 177 | if(imgColor.channels()==1) 178 | { 179 | cv::cvtColor(imgColor, imgColor, cv::COLOR_GRAY2BGR); 180 | } 181 | imgColor.convertTo(color, CV_32FC3, 1.0f / 255.0f); 182 | if (color.empty()) { 183 | std::cerr << "Error: empty color image " << path_ + filename << std::endl; 184 | return false; 185 | } 186 | 187 | return true; 188 | } 189 | 190 | bool load_albedo(const std::string& filename, cv::Mat& albedo) { 191 | 192 | if (filename.empty()) { 193 | std::cerr << "Error: missing filename" << std::endl; 194 | return false; 195 | } 196 | 197 | // load color 198 | cv::Mat imgColor = cv::imread(path_ + filename); 199 | if(imgColor.empty())return false; 200 | 201 | if(imgColor.channels()==1) 202 | { 203 | cv::cvtColor(imgColor, imgColor, cv::COLOR_GRAY2BGR); 204 | } 205 | 206 | imgColor.convertTo(albedo, CV_32FC3, 1.0f / 255.0f); 207 | 208 | if (albedo.empty()) { 209 | std::cerr << "Error: empty color image " << path_ + filename << std::endl; 210 | return false; 211 | } 212 | 213 | return true; 214 | } 215 | 216 | bool consecutive_numbers(){ 217 | return consecutive_numbers_; 218 | } 219 | 220 | virtual bool load_next(cv::Mat &color, cv::Mat &depth) = 0; 221 | 222 | 223 | virtual bool load_keyframe(cv::Mat &color, cv::Mat &depth, const int frame) { 224 | // only implement if consecutive_numbers_ == true 225 | return false; 226 | } 227 | 228 | bool load_pose(std::string& filename, std::vector >& poses) 229 | { 230 | 231 | std::ifstream file; 232 | file.open((path_ + filename).c_str()); 233 | if(!file.is_open()){ 234 | std::cout << "can't load poses!" << std::endl; 235 | return false; 236 | } 237 | std::string line; 238 | while (std::getline(file, line)){ 239 | float timestamp; 240 | Eigen::Vector3f translation; 241 | Eigen::Quaternionf q; 242 | std::stringstream s(line); 243 | s >> timestamp >> translation[0] >> translation[1] >> translation[2] >> q.x() >> q.y() >> q.z() >> q.w(); 244 | if (q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z() < 0.99) { 245 | std::cerr << "pose " << timestamp << " has invalid rotation" << std::endl; 246 | } 247 | Eigen::Matrix4f tmp = Eigen::Matrix4f::Identity(); 248 | tmp.topRightCorner(3,1) = translation; 249 | Eigen::Matrix3f rot = q.toRotationMatrix(); 250 | tmp.topLeftCorner(3,3) = rot; 251 | poses.push_back(tmp); 252 | } 253 | 254 | if (poses.size()==0) return false; 255 | 256 | return true; 257 | 258 | } 259 | 260 | 261 | virtual void reset() = 0; 262 | virtual void reset_counter() = 0; 263 | virtual bool load_reflectance(cv::Mat& albedo, cv::Mat& depth) = 0; 264 | 265 | }; 266 | 267 | #endif // IMAGE_LOADER_H_ 268 | -------------------------------------------------------------------------------- /cpp/include/img_loader/MultiviewLoader.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : MultiviewLoader.h 3 | // Author : Lu Sang 4 | // Date : 09/2021 5 | // License : GNU General Public License 6 | // Description : class imageLoader 7 | //============================================================================ 8 | 9 | #ifndef MULTIVIEW_LOADER_H_ 10 | #define MULTIVIEW_LOADER_H_ 11 | 12 | #include 13 | #include "ImageLoader.h" 14 | 15 | class MultiviewLoader : public ImageLoader { 16 | 17 | private: 18 | 19 | size_t counter; 20 | 21 | public: 22 | 23 | MultiviewLoader() : 24 | ImageLoader(1./1000, true), 25 | counter(1) 26 | {} 27 | 28 | MultiviewLoader(const std::string& path) : 29 | ImageLoader(1./1000, path, true), 30 | counter(1) 31 | {} 32 | 33 | ~MultiviewLoader() {} 34 | 35 | bool load_next(cv::Mat& color, cv::Mat& depth) { 36 | 37 | std::stringstream ss; 38 | ss << std::setfill('0') << std::setw(6) << counter; 39 | 40 | timestamp_rgb_ = ss.str(); 41 | timestamp_depth_ = timestamp_rgb_; 42 | 43 | const std::string filename = timestamp_rgb_ + ".png"; 44 | 45 | 46 | 47 | if (!load_depth("depth" + filename, depth)) 48 | return false; 49 | 50 | 51 | 52 | if (!load_color("color" + filename, color)) 53 | return false; 54 | 55 | ++counter; 56 | 57 | return true; 58 | } 59 | 60 | bool load_keyframe(cv::Mat& color, cv::Mat& depth, const int frame) { 61 | 62 | std::stringstream ss; 63 | ss << std::setfill('0') << std::setw(6) << frame+1; 64 | 65 | timestamp_rgb_ = ss.str(); 66 | timestamp_depth_ = timestamp_rgb_; 67 | 68 | const std::string filename = timestamp_rgb_ + ".png"; 69 | timestamps_depth_.push_back(timestamp_depth_); 70 | timestamps_rgb_.push_back(timestamp_rgb_); 71 | 72 | if (!load_depth("depth" + filename, depth)) 73 | return false; 74 | 75 | if (!load_color("color" + filename, color)) 76 | return false; 77 | 78 | std::cout<<"image:" << filename << " is loaded!" << std::endl; 79 | return true; 80 | } 81 | 82 | void reset_counter() 83 | { 84 | counter = 1; 85 | } 86 | 87 | void reset() 88 | { 89 | counter = 1; 90 | timestamps_depth_.clear(); 91 | timestamps_rgb_.clear(); 92 | } 93 | bool load_reflectance(cv::Mat& albedo, cv::Mat& depth){return false;} 94 | 95 | }; 96 | 97 | #endif // MULTIVIEW_LOADER_H_ 98 | -------------------------------------------------------------------------------- /cpp/include/img_loader/SynthLoader.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : SynthLoader.h 3 | // Author : Christiane Sommer, Lu Sang 4 | // Date : 03/2021 5 | // License : GNU General Public License 6 | // Description : class imageLoader 7 | //============================================================================ 8 | 9 | #ifndef SYNTH_LOADER_H_ 10 | #define SYNTH_LOADER_H_ 11 | 12 | #include 13 | #include "ImageLoader.h" 14 | 15 | class SynthLoader : public ImageLoader { 16 | 17 | private: 18 | 19 | size_t counter; 20 | 21 | public: 22 | 23 | SynthLoader() : 24 | ImageLoader(1./1000, true), 25 | counter(1) 26 | {} 27 | 28 | SynthLoader(const std::string& path) : 29 | ImageLoader(1./1000, path, true), 30 | counter(1) 31 | {} 32 | 33 | ~SynthLoader() {} 34 | 35 | bool load_next(cv::Mat& color, cv::Mat& depth) { 36 | 37 | std::stringstream ss; 38 | ss << std::setfill('0') << std::setw(3) << counter; 39 | 40 | timestamp_rgb_ = ss.str(); 41 | timestamp_depth_ = timestamp_rgb_; 42 | 43 | const std::string filename = timestamp_rgb_ + ".png"; 44 | 45 | 46 | if (!load_depth("depth/" + filename, depth)) 47 | return false; 48 | 49 | 50 | 51 | if (!load_color("rgb/" + filename, color)) 52 | return false; 53 | 54 | ++counter; 55 | 56 | return true; 57 | } 58 | 59 | bool load_reflectance(cv::Mat& albedo, cv::Mat& depth){ 60 | 61 | std::stringstream ss; 62 | ss << std::setfill('0') << std::setw(3) << counter; 63 | 64 | std::string filename = ss.str() + ".png"; 65 | if (!load_albedo("albedo/" + filename, albedo)) 66 | return false; 67 | 68 | if (!load_depth("depth/" + filename, depth)) 69 | return false; 70 | 71 | ++counter; 72 | 73 | return true; 74 | 75 | } 76 | 77 | bool load_keyframe(cv::Mat& color, cv::Mat& depth, const int frame) { 78 | 79 | std::stringstream ss; 80 | ss << std::setfill('0') << std::setw(3) << frame+1; 81 | 82 | timestamp_rgb_ = ss.str(); 83 | timestamp_depth_ = timestamp_rgb_; 84 | 85 | const std::string filename = timestamp_rgb_ + ".png"; 86 | timestamps_depth_.push_back(timestamp_depth_); 87 | timestamps_rgb_.push_back(timestamp_rgb_); 88 | 89 | if (!load_depth("depth/" + filename, depth)) 90 | return false; 91 | 92 | if (!load_color("albedo/" + filename, color)) 93 | return false; 94 | 95 | std::cout<<"image:" << filename << " is loaded!" << std::endl; 96 | return true; 97 | } 98 | 99 | void reset_counter() 100 | { 101 | counter = 1; 102 | } 103 | 104 | void reset() 105 | { 106 | counter = 1; 107 | timestamps_depth_.clear(); 108 | timestamps_rgb_.clear(); 109 | } 110 | 111 | }; 112 | 113 | #endif // SYNTH_LOADER_H_ 114 | -------------------------------------------------------------------------------- /cpp/include/img_loader/TumrgbdLoader.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : TumrgbdLoader.h 3 | // Author : Christiane Sommer 4 | // Date : 10/2019 5 | // License : GNU General Public License 6 | // Description : class TumrgbdLoader 7 | //============================================================================ 8 | 9 | #ifndef TUMRGBD_LOADER_H_ 10 | #define TUMRGBD_LOADER_H_ 11 | 12 | #include "ImageLoader.h" 13 | #include 14 | #include 15 | 16 | class TumrgbdLoader : public ImageLoader { 17 | 18 | private: 19 | 20 | std::ifstream depth_file_; 21 | std::ifstream color_file_; 22 | std::ifstream assoc_file_; 23 | 24 | void init() { 25 | depth_file_.open(path_ + "depth.txt"); 26 | color_file_.open(path_ + "rgb.txt"); 27 | assoc_file_.open(path_ + "associated.txt"); 28 | 29 | return; 30 | } 31 | 32 | public: 33 | 34 | TumrgbdLoader() : 35 | ImageLoader(1./5000, false) 36 | { 37 | init(); 38 | } 39 | 40 | TumrgbdLoader(const std::string& path) : 41 | ImageLoader(1./5000, path, false) 42 | { 43 | init(); 44 | } 45 | 46 | ~TumrgbdLoader() { 47 | depth_file_.close(); 48 | color_file_.close(); 49 | assoc_file_.close(); 50 | } 51 | 52 | // bool load_next(cv::Mat& color, cv::Mat& depth) { 53 | 54 | // std::string line, tmp, filename; 55 | 56 | // line = "#"; 57 | // while (line.at(0) == '#') { 58 | // if(!std::getline(depth_file_, line)) 59 | // return false; 60 | // } 61 | 62 | // std::istringstream dss(line); 63 | // dss >> timestamp_ >> filename; 64 | 65 | // if (!load_depth(filename, depth)) 66 | // return false; 67 | 68 | // line = "#"; 69 | // while (line.at(0) == '#') { 70 | // if(!std::getline(color_file_, line)) 71 | // return false; 72 | // } 73 | 74 | // std::istringstream css(line); 75 | // css >> tmp >> filename; 76 | 77 | // if (!load_color(filename, color)) 78 | // return false; 79 | 80 | // return true; 81 | // } 82 | 83 | bool load_next(cv::Mat& color, cv::Mat& depth) { 84 | 85 | std::string line, tmp, rgb_filename, depth_filename; 86 | 87 | line = "#"; 88 | while (line.at(0) == '#') { 89 | if(!std::getline(assoc_file_, line)) 90 | return false; 91 | } 92 | 93 | std::istringstream dss(line); 94 | dss >> timestamp_rgb_ >> rgb_filename >> timestamp_depth_ >> depth_filename; 95 | 96 | // std::cout << "load image " << timestamp_rgb_ << std::endl; 97 | 98 | timestamps_depth_.push_back(timestamp_depth_); 99 | timestamps_rgb_.push_back(timestamp_rgb_); 100 | 101 | if (!load_depth(depth_filename, depth)) 102 | return false; 103 | 104 | // line = "#"; 105 | // while (line.at(0) == '#') { 106 | // if(!std::getline(color_file_, line)) 107 | // return false; 108 | // } 109 | 110 | // std::istringstream css(line); 111 | // css >> tmp >> filename; 112 | 113 | if (!load_color(rgb_filename, color)) 114 | return false; 115 | 116 | // std::cout << "loaded image rgb: " << rgb_filename << "\t depth: " << depth_filename << std::endl; 117 | 118 | return true; 119 | } 120 | 121 | void reset() { 122 | depth_file_.close(); 123 | color_file_.close(); 124 | assoc_file_.close(); 125 | depth_file_.open(path_ + "depth.txt"); 126 | color_file_.open(path_ + "rgb.txt"); 127 | assoc_file_.open(path_ + "associated.txt"); 128 | timestamps_depth_.clear(); 129 | timestamps_rgb_.clear(); 130 | } 131 | 132 | void reset_counter(){}; 133 | bool load_reflectance(cv::Mat& albedo, cv::Mat& depth){return false;} 134 | 135 | }; 136 | 137 | #endif // TUMRGBD_LOADER_H_ 138 | -------------------------------------------------------------------------------- /cpp/include/img_loader/img_loader.h: -------------------------------------------------------------------------------- 1 | #ifndef IMG_LOADER_H_ 2 | #define IMG_LOADER_H_ 3 | 4 | // convenience header that includes all relevant image loaders 5 | 6 | #include "img_loader/TumrgbdLoader.h" 7 | #include "img_loader/SynthLoader.h" 8 | #include "img_loader/MultiviewLoader.h" 9 | // #include "img_loader/MvsnetLoader.h" 10 | 11 | #endif // IMG_LOADER_H_ -------------------------------------------------------------------------------- /cpp/include/mat.h: -------------------------------------------------------------------------------- 1 | #ifndef MAT_H 2 | #define MAT_H 3 | 4 | #ifndef WIN64 5 | #define EIGEN_DONT_ALIGN_STATICALLY 6 | #endif 7 | #include 8 | // #include 9 | #include 10 | 11 | typedef Eigen::Vector2f Vec2f; 12 | typedef Eigen::Vector3f Vec3f; 13 | typedef Eigen::Matrix3f Mat3f; 14 | typedef Eigen::Matrix4f Mat4f; 15 | 16 | typedef Eigen::Vector2d Vec2; 17 | typedef Eigen::Vector3d Vec3; 18 | typedef Eigen::Matrix3d Mat3; 19 | typedef Eigen::Matrix4d Mat4; 20 | 21 | typedef Eigen::Vector2i Vec2i; 22 | typedef Eigen::Vector3i Vec3i; 23 | typedef Eigen::Matrix Vec3b; 24 | 25 | typedef Sophus::SE3 SE3; 26 | typedef Sophus::SO3 SO3; 27 | 28 | typedef Eigen::Matrix Mat6f; 29 | typedef Eigen::Matrix Mat3x8f; 30 | typedef Eigen::Matrix Mat3x8i; 31 | typedef Eigen::Matrix Vec6f; 32 | typedef Eigen::Matrix Vec4f; 33 | typedef Eigen::Matrix Vec8f; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /cpp/include/normals/NormalEstimator.h: -------------------------------------------------------------------------------- 1 | // ============================================================================ 2 | // Name : NormalEstimator.h 3 | // Author : Christiane Sommer 4 | // Date : 11/2018 5 | // License : GNU General Public License 6 | // Description : NormalEstimation class 7 | // ============================================================================ 8 | 9 | #ifndef CV_NORMAL_ESTIMATOR_H_ 10 | #define CV_NORMAL_ESTIMATOR_H_ 11 | 12 | // libraries 13 | #include 14 | #include 15 | #include 16 | 17 | namespace cv { 18 | 19 | /* 20 | * NormalEstimator class using the FALS method from Badino et al. 21 | */ 22 | template 23 | class NormalEstimator { 24 | 25 | using Mat3T = Eigen::Matrix; 26 | 27 | // problem parameters 28 | int width_, height_; 29 | Mat3T K_; 30 | Size window_size_; 31 | // values needed for normal computation 32 | Mat_ x0_, y0_; 33 | Mat_ x0_n_sq_inv_, y0_n_sq_inv_, n_sq_inv_; 34 | Mat_ Q11_, Q12_, Q13_, Q22_, Q23_, Q33_; 35 | 36 | // similar to Matlab's meshgrid function 37 | template 38 | void pixelgrid(U shift_x, U shift_y, Mat_ &u, Mat_ &v) { 39 | 40 | std::vector row, col; 41 | for (int k = 0; k < width_; ++k) 42 | row.push_back(k - shift_x); 43 | for (int k = 0; k < height_; ++k) 44 | col.push_back(k - shift_y); 45 | Mat Row(row), Col(col); 46 | repeat(Row.reshape(1, 1), Col.total(), 1, u); 47 | repeat(Col.reshape(1, 1).t(), 1, Row.total(), v); 48 | 49 | } 50 | 51 | // compute values needed for normal estimation only once 52 | void cache() { 53 | 54 | Eigen::Matrix K; 55 | K = K_.template cast(); 56 | 57 | const double fx_inv = 1. / K(0, 0); 58 | const double fy_inv = 1. / K(1, 1); 59 | const double cx = K(0, 2); 60 | const double cy = K(1, 2); 61 | 62 | Mat_ x0, y0, x0_sq, y0_sq, x0_y0, n_sq; 63 | Mat_ x0_n_sq_inv, y0_n_sq_inv, n_sq_inv; 64 | Mat_ M11, M12, M13, M22, M23, M33, det, det_inv; 65 | Mat_ Q11, Q12, Q13, Q22, Q23, Q33; 66 | 67 | pixelgrid(cx, cy, x0, y0); 68 | 69 | x0 = fx_inv * x0; 70 | x0_sq = x0.mul(x0); 71 | y0 = fy_inv * y0; 72 | y0_sq = y0.mul(y0); 73 | x0_y0 = x0.mul(y0); 74 | 75 | n_sq = 1. + x0_sq + y0_sq; 76 | divide(1., n_sq, n_sq_inv); 77 | x0_n_sq_inv = x0.mul(n_sq_inv); 78 | y0_n_sq_inv = y0.mul(n_sq_inv); 79 | 80 | boxFilter(x0_sq.mul(n_sq_inv), M11, -1, window_size_, Point(-1, -1), false); 81 | boxFilter(x0_y0.mul(n_sq_inv), M12, -1, window_size_, Point(-1, -1), false); 82 | boxFilter(x0_n_sq_inv, M13, -1, window_size_, Point(-1, -1), false); 83 | boxFilter(y0_sq.mul(n_sq_inv), M22, -1, window_size_, Point(-1, -1), false); 84 | boxFilter(y0_n_sq_inv, M23, -1, window_size_, Point(-1, -1), false); 85 | boxFilter(n_sq_inv, M33, -1, window_size_, Point(-1, -1), false); 86 | 87 | det = M11.mul(M22.mul(M33)) + 2 * M12.mul(M23.mul(M13)) - 88 | (M13.mul(M13.mul(M22)) + M12.mul(M12.mul(M33)) + M23.mul(M23.mul(M11))); 89 | divide(1., det, det_inv); 90 | 91 | Q11 = det_inv.mul(M22.mul(M33) - M23.mul(M23)); 92 | Q12 = det_inv.mul(M13.mul(M23) - M12.mul(M33)); 93 | Q13 = det_inv.mul(M12.mul(M23) - M13.mul(M22)); 94 | Q22 = det_inv.mul(M11.mul(M33) - M13.mul(M13)); 95 | Q23 = det_inv.mul(M12.mul(M13) - M11.mul(M23)); 96 | Q33 = det_inv.mul(M11.mul(M22) - M12.mul(M12)); 97 | 98 | // TODO: write in more concise way!! 99 | if (std::is_same::value) { 100 | x0.convertTo(x0_, CV_32F); 101 | y0.convertTo(y0_, CV_32F); 102 | x0_n_sq_inv.convertTo(x0_n_sq_inv_, CV_32F); 103 | y0_n_sq_inv.convertTo(y0_n_sq_inv_, CV_32F); 104 | n_sq_inv.convertTo(n_sq_inv_, CV_32F); 105 | Q11.convertTo(Q11_, CV_32F); 106 | Q12.convertTo(Q12_, CV_32F); 107 | Q13.convertTo(Q13_, CV_32F); 108 | Q22.convertTo(Q22_, CV_32F); 109 | Q23.convertTo(Q23_, CV_32F); 110 | Q33.convertTo(Q33_, CV_32F); 111 | } 112 | else { 113 | x0_ = x0; 114 | y0_ = y0; 115 | x0_n_sq_inv_ = x0_n_sq_inv; 116 | y0_n_sq_inv_ = y0_n_sq_inv; 117 | n_sq_inv_ = n_sq_inv; 118 | Q11_ = Q11; 119 | Q12_ = Q12; 120 | Q13_ = Q13; 121 | Q22_ = Q22; 122 | Q23_ = Q23; 123 | Q33_ = Q33; 124 | } 125 | } 126 | 127 | public: 128 | 129 | NormalEstimator(int width, int height, Eigen::Matrix K, Size window_size) : 130 | width_(width), 131 | height_(height), 132 | K_(K.cast()), 133 | window_size_(window_size) 134 | { 135 | cache(); 136 | } 137 | 138 | NormalEstimator(int width, int height, Eigen::Matrix K, Size window_size) : 139 | width_(width), 140 | height_(height), 141 | K_(K.cast()), 142 | window_size_(window_size) 143 | { 144 | cache(); 145 | } 146 | 147 | ~NormalEstimator() {} 148 | 149 | // compute normals 150 | void compute(const Mat &depth, Mat &nx, Mat &ny, Mat &nz) const { 151 | 152 | // workaround to only divide by depth where it is non-zero 153 | // not needed for OpenCV versions <4 154 | Mat_ tmp; 155 | divide(1., depth, tmp); 156 | Mat z_inv = Mat::zeros(tmp.size(), tmp.type()); 157 | Mat mask = (depth != 0); 158 | std::cout << mask.type() << std::endl; 159 | tmp.copyTo(z_inv, mask); 160 | 161 | Mat_ b1, b2, b3, norm_n; 162 | 163 | boxFilter(x0_n_sq_inv_.mul(z_inv), b1, -1, window_size_, Point(-1, -1), false); 164 | boxFilter(y0_n_sq_inv_.mul(z_inv), b2, -1, window_size_, Point(-1, -1), false); 165 | boxFilter(n_sq_inv_.mul(z_inv), b3, -1, window_size_, Point(-1, -1), false); 166 | 167 | nx = b1.mul(Q11_) + b2.mul(Q12_) + b3.mul(Q13_); 168 | ny = b1.mul(Q12_) + b2.mul(Q22_) + b3.mul(Q23_); 169 | nz = b1.mul(Q13_) + b2.mul(Q23_) + b3.mul(Q33_); 170 | 171 | sqrt(nx.mul(nx) + ny.mul(ny) + nz.mul(nz), norm_n); 172 | 173 | divide(nx, norm_n, nx); 174 | divide(ny, norm_n, ny); 175 | divide(nz, norm_n, nz); 176 | } 177 | 178 | Mat *x0_ptr() { return &x0_; } 179 | 180 | Mat *y0_ptr() { return &y0_; } 181 | 182 | Mat *n_sq_inv_ptr() { return &n_sq_inv_; } 183 | 184 | }; 185 | 186 | } // namespace cv 187 | 188 | #endif // CV_NORMAL_ESTIMATOR_H_ 189 | -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/Auxilary.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name :Auxilary.h 3 | // Author : Lu Sang 4 | // Date : 09/2021 5 | // License : GNU General Public License 6 | // Description : implementation of some helper functions 7 | //============================================================================ 8 | 9 | #ifndef AUXILARY_H_ 10 | #define AUXILARY_H_ 11 | 12 | #include "mat.h" 13 | 14 | #include 15 | #include 16 | // #include 17 | #include 18 | 19 | //======================================= helper functions ============================================================================ 20 | 21 | //! check for NaN values inside Eigen object 22 | template 23 | bool checkNan(S vector) 24 | { 25 | return vector.array().isNaN().sum(); 26 | } 27 | 28 | //! skew-symmetric matrix of a 3D vector 29 | static inline Mat3f skew(Vec3f v) { 30 | Mat3f vx = Mat3f::Zero(); 31 | vx(0,1) = -v[2]; 32 | vx(0,2) = v[1]; 33 | vx(1,0) = v[2]; 34 | vx(1,2) = -v[0]; 35 | vx(2,0) = -v[1]; 36 | vx(2,1) = v[0]; 37 | return vx; 38 | } 39 | 40 | //! interpolation for RGB image 41 | static Vec3f interpolateImage(const float m, const float n, const cv::Mat& img) 42 | { 43 | int x = std::floor(m); 44 | int y = std::floor(n); 45 | cv::Vec3f tmp; 46 | if ((x+1) < img.rows && (y+1) < img.cols){ 47 | 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); 48 | } 49 | else if ((y+1) < img.cols && x >= img.rows){ 50 | tmp = (y+1.0-n)*img.at(x,y) + (n-y)*img.at(x,y+1); 51 | } 52 | else if ( y >= img.cols && (x+1) < img.rows){ 53 | tmp = (m-x)*img.at(x+1,y) + (x+1.0-m)*img.at(x,y); 54 | } 55 | else{ 56 | tmp = img.at(x,y); 57 | } 58 | 59 | Vec3f intensity(tmp[2],tmp[1],tmp[0]); // OpenCV stores image colors as BGR. 60 | return intensity; 61 | } 62 | 63 | //! gradient of image 64 | static Vec3f computeImageGradient(const float m, const float n, const cv::Mat& img, int direction) 65 | { 66 | float w00, w01, w10, w11; 67 | 68 | int x = std::floor(m); 69 | int y = std::floor(n); 70 | 71 | w01 = m - x; 72 | w11 = n - y; 73 | w00 = 1.0 - w01; 74 | w10 = 1.0 - w11; 75 | 76 | // compute gradient manually using finite differences 77 | 78 | cv::Vec3f v0, v1; 79 | 80 | if (direction == 0) 81 | { 82 | // x-direction 83 | if( (x+1)(x,y+1) - img.at(x,y); 86 | v1 = img.at(x+1,y+1) - img.at(x+1,y); 87 | 88 | return w00 * Vec3f(v0[2],v0[1],v0[0]) + w01 * Vec3f(v1[2],v1[1],v1[0]); 89 | } 90 | else if ( (x+1) >= img.rows){// && (y+1) < img.cols){ 91 | 92 | v0 = img.at(x,y+1) - img.at(x,y); 93 | 94 | return Vec3f(v0[2],v0[1],v0[0]); 95 | } 96 | else if ( (x+1) < img.rows && (y+1) >= img.cols){ 97 | 98 | v0 = -img.at(x,y-1) + img.at(x,y); 99 | v1 = -img.at(x+1,y-1) + img.at(x+1,y); 100 | 101 | return w00 * Vec3f(v0[2],v0[1],v0[0]) + w01 * Vec3f(v1[2],v1[1],v1[0]); 102 | } 103 | } 104 | else 105 | { 106 | // y-direction 107 | if ((x+1)(x+1,y) - img.at(x,y); 109 | v1 = img.at(x+1,y+1) - img.at(x,y+1); 110 | return w10 * Vec3f(v0[2],v0[1],v0[0]) + w11 * Vec3f(v1[2],v1[1],v1[0]); 111 | } 112 | 113 | else if ( (x+1) >= img.rows && (y+1) < img.cols){ 114 | v0 = -img.at(x-1,y) + img.at(x,y); 115 | v1 = -img.at(x-1,y+1) + img.at(x,y+1); 116 | return w10 * Vec3f(v0[2],v0[1],v0[0]) + w11 * Vec3f(v1[2],v1[1],v1[0]); 117 | } 118 | else if ( (y+1) >= img.cols){ // && (x+1) < img.rows){ 119 | v0 = img.at(x+1,y) - img.at(x,y); 120 | return Vec3f(v0[2],v0[1],v0[0]); 121 | } 122 | } 123 | } 124 | 125 | #endif //AUXILARY_H_ -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/LedOptimizer.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : LedOptimizer.cpp 3 | // Author : Lu Sang 4 | // Date : 09/2021 5 | // License : GNU General Public License 6 | // Description : implementation of class LedOptimizer 7 | //============================================================================ 8 | 9 | #include "LedOptimizer.h" 10 | #include "sdf_tracker/Sdfvoxel.h" 11 | #include "Timer.h" 12 | 13 | // ============================= class functions ===================================== 14 | 15 | LedOptimizer::LedOptimizer(VolumetricGradSdf* tSDF, 16 | const float voxel_size, 17 | const Mat3f& K, 18 | std::string save_path, 19 | OptimizerSettings* settings): 20 | Optimizer(tSDF, voxel_size, K, save_path, settings) 21 | { 22 | init(); 23 | } 24 | 25 | void LedOptimizer::init() 26 | { 27 | num_frames_ = frame_idx_.size(); 28 | num_voxels_ = tSDF_->grid_dim_[0]*tSDF_->grid_dim_[1]*tSDF_->grid_dim_[2]; 29 | 30 | select_vis(); 31 | // initialize light_r/g/b_ 32 | light_ = Vec3f::Ones(); 33 | computeLightIntensive(); 34 | std::cout << "the light intensity is roughly: " << light_.transpose() << std::endl; 35 | 36 | } 37 | 38 | 39 | //------------------ energy function-------------------------------------------- 40 | float LedOptimizer::getPSEnergy() 41 | { 42 | float E = 0; 43 | 44 | for(auto& lin_idx: surface_points_){ 45 | 46 | const SdfVoxel& v = tSDF_->tsdf_[lin_idx]; 47 | std::vector vis = tSDF_->vis_[lin_idx]; 48 | 49 | for(size_t frame_id = 0; frame_id < num_frames_; frame_id++){ 50 | if(!vis[frame_id]){ // vis for whole sequence, frame_idx_ for key frame 51 | continue; 52 | } 53 | 54 | Vec3i idx = tSDF_->line2idx(lin_idx); 55 | 56 | Mat3f R = getRotation(frame_id); 57 | Vec3f t = getTranslation(frame_id); 58 | cv::Mat img = getImage(frame_id); 59 | 60 | Vec3f intensity; 61 | if(!getIntensity(idx, v, R, t, img, intensity)){ 62 | continue; 63 | } 64 | 65 | E += computeLoss(intensity - renderedIntensity(v, idx, frame_id)); 66 | } 67 | 68 | } 69 | 70 | return E/static_cast(surface_points_.size()); 71 | } 72 | 73 | 74 | 75 | 76 | void LedOptimizer::computeLightIntensive() 77 | { 78 | Vec3f Intensity = Vec3f::Zero(); 79 | Vec3f RenderedIntensity = Vec3f::Zero(); 80 | 81 | for(auto& lin_idx: surface_points_){ 82 | 83 | const SdfVoxel& v = tSDF_->tsdf_[lin_idx]; 84 | std::vector vis = tSDF_->vis_[lin_idx]; 85 | 86 | for(size_t frame_id = 0; frame_id < num_frames_; frame_id++){ 87 | if(!vis[frame_id]){ // vis for whole sequence, frame_idx_ for key frame 88 | continue; 89 | } 90 | 91 | Vec3i idx = tSDF_->line2idx(lin_idx); 92 | 93 | Mat3f R = getRotation(frame_id); 94 | Vec3f t = getTranslation(frame_id); 95 | cv::Mat img = getImage(frame_id); 96 | 97 | Vec3f intensity; 98 | if(!getIntensity(idx, v, R, t, img, intensity)){ 99 | continue; 100 | } 101 | 102 | Intensity += intensity; 103 | RenderedIntensity += renderedIntensity(v, idx, frame_id); 104 | } 105 | 106 | } 107 | float light_r = Intensity[0] / RenderedIntensity[0]; 108 | float light_g = Intensity[1] / RenderedIntensity[1]; 109 | float light_b = Intensity[2] / RenderedIntensity[2]; 110 | light_ << light_r, light_g, light_b; 111 | 112 | } 113 | 114 | 115 | 116 | bool LedOptimizer::computeResidual(const SdfVoxel& v, const Vec3i& idx, const int frame_id, Vec3f& r) 117 | { 118 | Vec3f intensity; 119 | Mat3f R = getRotation(frame_id); 120 | Vec3f t = getTranslation(frame_id); 121 | cv::Mat img = getImage(frame_id); 122 | 123 | if(!getIntensity(idx, v, R , t, img, intensity)){ //if voxel is projected outside of the boundary, then skip 124 | return false; 125 | } 126 | 127 | r = intensity - renderedIntensity(v, idx, frame_id); 128 | return true; 129 | 130 | } 131 | 132 | 133 | //------------------------------- optimization function --------------------------------------------------------------------------------- 134 | void LedOptimizer::optimizeLightAll(float damping) 135 | { 136 | Eigen::SparseMatrix J_l = lightJacobian(); 137 | std::pair > pair = computeResidual(); 138 | Eigen::VectorXf r = pair.first; 139 | Eigen::SparseMatrix w = pair.second; 140 | 141 | Eigen::SparseMatrix H = J_l.transpose()*w*J_l; 142 | Eigen::VectorXf b = J_l.transpose()*w*r; 143 | 144 | if(damping!=0.0){ 145 | H.diagonal() += damping*H.diagonal(); 146 | } 147 | 148 | Eigen::ConjugateGradient> solver; 149 | 150 | solver.compute(H); 151 | if(solver.info() != Eigen::Success ){ 152 | std::cout << "decomposition failed! " << std::endl; 153 | } 154 | Eigen::VectorXf delta_r = solver.solve(b); 155 | if(solver.info()!=Eigen::Success) { 156 | std::cout << "solver failed: " << solver.info() << "\n"; 157 | } 158 | // delta_r /= 10.0; 159 | light_ -= delta_r; 160 | } 161 | 162 | void LedOptimizer::optimizeAlbedoAll(bool albedo_reg, float damping) 163 | { 164 | Eigen::SparseMatrix J_r = albedoJacobian(); 165 | std::pair > pair = computeResidual(); 166 | Eigen::VectorXf r = pair.first; 167 | Eigen::SparseMatrix w = pair.second; 168 | 169 | Eigen::SparseMatrix H = J_r.transpose()*w*J_r; 170 | Eigen::VectorXf b = J_r.transpose()*w*r; 171 | 172 | if(albedo_reg){ 173 | std::pair, Eigen::VectorXf> tmp = albedoRegJacobian(); 174 | Eigen::SparseMatrix Jr_r = tmp.first; 175 | Eigen::VectorXf r_r = tmp.second; 176 | H += settings_->reg_weight_rho*Jr_r.transpose()*Jr_r; 177 | b += settings_->reg_weight_rho*Jr_r.transpose()*r_r; 178 | } 179 | 180 | if(damping!=0.0){ 181 | H.diagonal() += damping*H.diagonal(); 182 | } 183 | 184 | Eigen::ConjugateGradient> solver; 185 | 186 | solver.compute(H); 187 | if(solver.info() != Eigen::Success ){ 188 | std::cout << "decomposition failed! " << std::endl; 189 | } 190 | Eigen::VectorXf delta_r = solver.solve(b); 191 | if(solver.info()!=Eigen::Success) { 192 | std::cout << "solver failed: " << solver.info() << "\n"; 193 | } 194 | // delta_r /= 10.0; 195 | updateAlbedo(delta_r); 196 | } 197 | 198 | void LedOptimizer::optimizeDistAll(bool normal_reg, bool laplacian_reg, float damping) 199 | { 200 | // bool normal_reg = false; 201 | 202 | Eigen::SparseMatrix J_d = distJacobian(); 203 | std::pair > tmp = computeResidual(); 204 | Eigen::VectorXf r = tmp.first; 205 | Eigen::SparseMatrix w = tmp.second; 206 | 207 | Eigen::SparseMatrix H = J_d.transpose()*w*J_d; 208 | Eigen::VectorXf b = J_d.transpose()*w*r; 209 | 210 | if(normal_reg){ 211 | std::pair, Eigen::VectorXf> pair = distRegJacobian(); 212 | Eigen::SparseMatrix Jr_d = pair.first; 213 | Eigen::VectorXf r_n = pair.second; 214 | H += settings_->reg_weight_n*(Jr_d.transpose()*Jr_d); 215 | b += settings_->reg_weight_n*(Jr_d.transpose()*r_n); 216 | } 217 | 218 | if(laplacian_reg){ 219 | std::pair, Eigen::VectorXf> pair = distLaplacianJacobian(); 220 | Eigen::SparseMatrix Jl_d = pair.first; 221 | Eigen::VectorXf r_n = pair.second; 222 | H += settings_->reg_weight_l*(Jl_d.transpose()*Jl_d); 223 | b += settings_->reg_weight_l*(Jl_d.transpose()*r_n); 224 | } 225 | 226 | if(damping!=0.0){ 227 | H.diagonal() += damping*H.diagonal(); 228 | } 229 | 230 | 231 | Eigen::ConjugateGradient> solver; 232 | 233 | solver.compute(H); 234 | if(solver.info() != Eigen::Success ){ 235 | std::cout << "decomposition failed! " << std::endl; 236 | } 237 | Eigen::VectorXf delta_d = solver.solve(b); 238 | if(solver.info()!=Eigen::Success) { 239 | std::cout << "solver failed: " << solver.info() << "\n"; 240 | } 241 | 242 | updateDist(delta_d, true); 243 | 244 | } 245 | 246 | void LedOptimizer::optimizePosesAll(float damping) 247 | { 248 | Eigen::SparseMatrix Jc = poseJacobian(); 249 | std::pair > pair = computeResidual(); 250 | Eigen::VectorXf r = pair.first; 251 | Eigen::SparseMatrix w = pair.second; 252 | Eigen::SparseMatrix H = Jc.transpose()*w*Jc; 253 | Eigen::VectorXf b = Jc.transpose()*w*r; 254 | 255 | if(damping!=0.0){ 256 | H.diagonal() += damping*H.diagonal(); 257 | } 258 | 259 | Eigen::ConjugateGradient> solver; 260 | 261 | solver.compute(H); 262 | if(solver.info() != Eigen::Success ){ 263 | std::cout << "decomposition failed! " << std::endl; 264 | } 265 | 266 | Eigen::VectorXf delta_xi = solver.solve(b); 267 | if(solver.info()!=Eigen::Success) { 268 | std::cout << "solver failed: " << solver.info() << "\n"; 269 | } 270 | 271 | if(solver.info()==Eigen::Success){ 272 | updatePose(delta_xi); 273 | } 274 | 275 | } 276 | 277 | 278 | // ---------------------------- optimization function ----------------------------------- 279 | bool LedOptimizer::alternatingOptimize(bool light, bool albedo, bool distance, bool pose) 280 | { 281 | 282 | bool normal_reg = false; 283 | bool albedo_reg = false; 284 | bool laplacian_reg = false; 285 | 286 | if(settings_->reg_weight_n != 0.0) normal_reg = true; 287 | if(settings_->reg_weight_rho != 0.0) albedo_reg = true; 288 | if(settings_->reg_weight_l != 0.0) laplacian_reg = true; 289 | 290 | // save iter detail 291 | std::ofstream file; 292 | file.open((save_path_ + "optimizer_doc.txt").c_str()); 293 | 294 | std::cout << "albation study settings: \n" << 295 | "light: " << light << "\n" << "albedo: " << albedo << "\n" << "distance: " << distance << "\n" << "pose: " << pose << std::endl; 296 | 297 | float damping = settings_->damping; 298 | 299 | file << "albation study settings: \t" << 300 | "light: " << light << "\t" << "albedo: " << albedo << "\t" << "distance: " << distance << "\t" << "pose: " << pose << "\n" << "num of key frame: " << num_frames_ << " \n total voxels: " << num_voxels_ << "\n"; 301 | 302 | Timer T; 303 | 304 | T.tic(); 305 | // getSurfaceVoxel(); // compute surface points. 306 | initAlbedo(); 307 | T.toc("initial albedo"); 308 | 309 | 310 | float E, E_total; 311 | float E_n = 0.0; 312 | float E_r = 0.0; 313 | float E_l = 0.0; 314 | E = getPSEnergy(); 315 | if(normal_reg) { 316 | E_n = getNormalEnergy(); 317 | settings_->reg_weight_n *= E / E_n; 318 | 319 | } 320 | 321 | if(albedo_reg) E_r = getAlbedoRegEnergy(); 322 | 323 | if(laplacian_reg){ 324 | E_l = getLaplacianEnergy(); 325 | settings_->reg_weight_l *= E / E_l; 326 | if(settings_->upsample)laplacian_reg=false; 327 | } 328 | 329 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 330 | 331 | float rel_diff; 332 | std::vector E_vec; 333 | 334 | E_vec.push_back(E_total); 335 | 336 | std::cout << "======================== start alternating optimization ================================= " << std::endl 337 | << "======> total key frame:\t " << num_frames_ << std::endl 338 | << "======> total voxels: \t" << num_voxels_ << std::endl 339 | << "======> inital PS energy:" << E << "\t normal reg energy: " << settings_->reg_weight_n*E_n << "\t laplacian reg energy: " << settings_->reg_weight_l*E_l << " \t rho reg energy: " << settings_->reg_weight_rho*E_r << std::endl 340 | << "================================================================================================= " << std::endl; 341 | 342 | int iter = 0; 343 | while(iter < settings_->max_it){ 344 | 345 | if(light){ 346 | T.tic(); 347 | optimizeLightAll(damping); 348 | T.toc("light optimiz"); 349 | 350 | std::cout << "the light intensity is: " << light_.transpose() << std::endl; 351 | 352 | E = getPSEnergy(); 353 | std::cout << "===> [" << iter << "]: after light optimization: "; 354 | 355 | file << "===> [" << iter << "]: after light optimization: \n"; 356 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 357 | 358 | 359 | } 360 | 361 | if(albedo){ 362 | T.tic(); 363 | optimizeAlbedoAll(albedo_reg, damping); 364 | T.toc("albedo optimize"); 365 | 366 | E = getPSEnergy(); 367 | if(albedo_reg) E_r = getAlbedoRegEnergy(); 368 | std::cout << "===> [" << iter << "]: after albedo optimization: "; 369 | 370 | file << "===> [" << iter << "]: after albedo optimization: \n"; 371 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 372 | 373 | } 374 | 375 | if(distance){ 376 | 377 | T.tic(); 378 | optimizeDistAll(normal_reg, laplacian_reg, damping); 379 | T.toc("dist optimize"); 380 | 381 | E = getPSEnergy(); 382 | if(normal_reg) E_n = getNormalEnergy(); 383 | if(laplacian_reg) E_l = getLaplacianEnergy(); 384 | 385 | std::cout << "===> [" << iter << "]: after distance optimization: "; 386 | 387 | file << "===> [" << iter << "]: after distance optimization: \n"; 388 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 389 | } 390 | 391 | if(pose){ 392 | T.tic(); 393 | optimizePosesAll(damping); 394 | T.toc("poses optimize"); 395 | 396 | E = getPSEnergy(); 397 | std::cout << "===> [" << iter << "]: after pose optimization: "; 398 | 399 | file << "===> [" << iter << "]: after pose optimization: \n"; 400 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 401 | 402 | 403 | } 404 | 405 | E_vec.push_back(E_total); 406 | 407 | rel_diff = abs(E_vec.end()[-2] - E_total)/E_vec.end()[-2]; 408 | std::cout << "===> [" << iter << "]: relative diff " << rel_diff << std::endl; 409 | file << "===> [" << iter << "]: relative diff " << rel_diff << "\n"; 410 | 411 | 412 | // save_pointcloud("after_iter_" + std::to_string(iter)); 413 | // extract_mesh("after_iter_" + std::to_string(iter)); 414 | 415 | 416 | if(rel_diff < settings_->conv_threshold) 417 | { 418 | std::cout << "===> [" << iter << "]: converged!" < [" << iter << "]: converged! \n"; 420 | save_pointcloud("final_refined"); 421 | extract_mesh("final_refined"); 422 | saveSDF("refined_sdf.sdf"); 423 | return true; 424 | } 425 | 426 | if(E_vec.end()[-2] < E_total ) 427 | { 428 | save_pointcloud("final_refined"); 429 | extract_mesh("final_refined"); 430 | saveSDF("refined_sdf.sdf"); 431 | std::cout << "===> [" << iter << "]: diverged!" <upsample){ 436 | 437 | // if upsampled, need laplacian to smooth a bit 438 | // maybe we can remove it 439 | if(settings_->reg_weight_l==0.0)settings_->reg_weight_l = 1.0; 440 | laplacian_reg = true; 441 | 442 | T.tic(); 443 | subsampling(); 444 | T.toc("upsample"); 445 | 446 | save_pointcloud("upsample_after_" + std::to_string(iter));//DEBUG 447 | extract_mesh("upsample_after_" + std::to_string(iter));//DEBUG 448 | 449 | file << "===> [" << iter << "]: after pose optimization: \n"; 450 | 451 | E_l = getLaplacianEnergy(); 452 | settings_->reg_weight_l *= E / E_l; 453 | laplacian_reg = true; 454 | 455 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 456 | 457 | E_vec.push_back(E_total); 458 | 459 | } 460 | 461 | if (iter == 15 && settings_->upsample){ 462 | settings_->reg_weight_l = 0.0; 463 | } 464 | 465 | 466 | ++iter; 467 | 468 | // save after each 3 iterations for debug 469 | if(iter%3 == 0){ 470 | savePoses("after_poses_opt_" + std::to_string(iter)); 471 | save_pointcloud("after_iter_" + std::to_string(iter)); 472 | extract_mesh("after_iter_" + std::to_string(iter)); 473 | } 474 | 475 | } 476 | 477 | return false; 478 | } -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/LedOptimizer.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : LedOptimizer.h 3 | // Author : Lu Sang 4 | // Date : 04/2022 5 | // License : GNU General Public License 6 | // Description : header of class LedOptimizer 7 | //============================================================================ 8 | 9 | #ifndef LED_OPTIMIZER_H_ 10 | #define LED_OPTIMIZER_H_ 11 | 12 | #include "mat.h" 13 | #include "Optimizer.h" 14 | 15 | 16 | #include 17 | #include 18 | // #include 19 | #include 20 | 21 | 22 | 23 | class LedOptimizer : public Optimizer { 24 | 25 | // private variables: 26 | Vec3f light_ = Vec3f::Ones(); 27 | 28 | // private method: 29 | // energy related 30 | virtual float getPSEnergy(); 31 | void computeLightIntensive(); 32 | Vec3f renderedIntensity(const SdfVoxel& v, const Vec3i& idx, const int frame_idx); 33 | bool computeResidual(const SdfVoxel& v, const Vec3i& idx, const int frame_id, Vec3f& r); 34 | 35 | //Jacobian function 36 | bool poseJacobian(const Vec3i& idx, const SdfVoxel& v, int frame_id, const Mat3f& R, const Vec3f& t, Eigen::Matrix& J_c); 37 | Vec3f rhoJacobian(const SdfVoxel& v, const Vec3i& idx, const int frame_id); 38 | Vec3f LightJacobian(const SdfVoxel& v, const Vec3i& idx, const int frame_id); 39 | bool distJacobian(const SdfVoxel& v, const Vec3i& idx, Mat3f& R, Vec3f& t, const int frame_id, std::vector& J_d); 40 | bool numerical_distJacobian(const SdfVoxel& v, const Vec3i& idx, Mat3f& R, Vec3f& t, const int frame_id, std::vector& J_d); 41 | //set jacobian matrix 42 | Eigen::SparseMatrix albedoJacobian(); 43 | Eigen::SparseMatrix lightJacobian(); 44 | Eigen::SparseMatrix poseJacobian(); 45 | Eigen::SparseMatrix distJacobian(); 46 | std::pair > computeResidual(); 47 | 48 | //optimization function 49 | void optimizeLightAll(float damping); 50 | void optimizeAlbedoAll(bool albedo_reg, float damping); 51 | void optimizeDistAll(bool normal_reg, bool laplacian_reg, float damping); 52 | void optimizePosesAll(float damping); 53 | 54 | public: 55 | 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 57 | 58 | LedOptimizer(VolumetricGradSdf* tSDF, 59 | const float voxel_size, 60 | const Mat3f& K, 61 | std::string save_path, 62 | OptimizerSettings* settings 63 | 64 | ); 65 | 66 | 67 | // optimize 68 | virtual void init(); 69 | virtual bool alternatingOptimize(bool light, bool albedo, bool distance, bool pose); 70 | 71 | 72 | }; 73 | 74 | #endif // LED_OPTIMIZER_H_ 75 | -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/LedOptimizerJa.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : LedOptimizerJa.cpp 3 | // Author : Lu Sang 4 | // Date : 09/2021 5 | // License : GNU General Public License 6 | // Description : implementation of class LedOptimizer for jacobian calculation 7 | //============================================================================ 8 | 9 | #include "LedOptimizer.h" 10 | #include "Auxilary.h" 11 | #include "sdf_tracker/Sdfvoxel.h" 12 | #include "Timer.h" 13 | 14 | // =========================== rendered image ========================== 15 | Vec3f LedOptimizer::renderedIntensity(const SdfVoxel& v, const Vec3i& idx, const int frame_idx) 16 | { 17 | Mat3f R = getRotation(frame_idx); 18 | Vec3f t = getTranslation(frame_idx); 19 | Vec3f point = R.transpose() * (tSDF_->voxel2world(idx) - v.dist * v.grad.normalized() - t); 20 | Vec3f n = computeDistGrad(v, idx).first.normalized(); 21 | // Vec3f n = v.grad.normalized(); 22 | float Irradiance = - n.transpose()* R * point; 23 | float light_dist = std::pow(point.norm(),3); 24 | Irradiance /= light_dist; 25 | 26 | Vec3f rendered(v.r*light_[0]*Irradiance, v.g*light_[1]*Irradiance, v.b*light_[2]*Irradiance); 27 | return rendered; 28 | 29 | } 30 | 31 | // ============================= class functions ===================================== 32 | bool LedOptimizer::poseJacobian(const Vec3i& idx, const SdfVoxel& v, int frame_id, const Mat3f& R, const Vec3f& t, Eigen::Matrix& J_c) 33 | { 34 | const float fx = K_(0,0); 35 | const float fy = K_(1,1); 36 | const float cx = K_(0,2); 37 | const float cy = K_(1,2); 38 | 39 | // get subvoxel 40 | cv::Mat img = getImage(frame_id); 41 | Vec3f point = R.transpose() * (tSDF_->voxel2world(idx) - v.dist * v.grad.normalized() - t); 42 | float m = fx * point[0]/point[2] + cx; 43 | float n = fy * point[1]/point[2] + cy; 44 | 45 | if (m<0 || m>=img.cols || n<0 || n>=img.rows) { 46 | return false; 47 | } 48 | 49 | Eigen::Matrix image_grad; 50 | image_grad.col(0) = computeImageGradient(n, m, img, 0); //3x2 51 | image_grad.col(1) = computeImageGradient(n, m, img, 1); 52 | 53 | const float z_inv = 1./point[2]; 54 | const float z_inv_sq = z_inv * z_inv; 55 | Eigen::Matrix pi_grad = Eigen::Matrix::Zero(); 56 | pi_grad(0,0) = fx * z_inv; 57 | pi_grad(0,2) = -fx * point[0] * z_inv_sq; 58 | pi_grad(1,1) = fy * z_inv; 59 | pi_grad(1,2) = -fy * point[1] * z_inv_sq; 60 | 61 | Mat3f image_pi_grad = image_grad * pi_grad; 62 | 63 | Vec3f normal = v.grad.normalized(); 64 | float l = std::pow(point.norm(),3); 65 | Mat3f LED_t_grad, LED_R_grad; 66 | LED_t_grad.row(0) = - (v.r * light_[0] / l) * normal.transpose(); 67 | LED_t_grad.row(1) = - (v.g * light_[1] / l) * normal.transpose(); 68 | LED_t_grad.row(2) = - (v.b * light_[2] / l)* normal.transpose(); 69 | 70 | float dl = std::pow(point.norm(),5); 71 | Vec3f dl_dR = (skew(point)*point).transpose(); 72 | 73 | LED_R_grad.row(0) = -3*v.r * light_[0] / dl * (normal.transpose()* R * point).value() *dl_dR.transpose(); 74 | LED_R_grad.row(1) = -3*v.g * light_[1] / dl * (normal.transpose()* R * point).value() *dl_dR.transpose(); 75 | LED_R_grad.row(2) = -3*v.b * light_[2]/ dl * (normal.transpose()* R * point).value() *dl_dR.transpose(); 76 | 77 | J_c.block<3, 3>(0, 0) = -image_pi_grad * R.transpose() + LED_t_grad; 78 | J_c.block<3, 3>(0, 3) = image_pi_grad * skew(point) + LED_R_grad; 79 | 80 | return true; 81 | } 82 | 83 | 84 | //! compute alebdo jacobian 85 | Vec3f LedOptimizer::rhoJacobian(const SdfVoxel& v, const Vec3i& idx, const int frame_id) 86 | { 87 | Mat3f R = getRotation(frame_id); 88 | Vec3f t = getTranslation(frame_id); 89 | Vec3f n = v.grad.normalized(); 90 | // Vec3f n = computeDistGrad(v, idx).first; 91 | Vec3f point = R.transpose() * (tSDF_->voxel2world(idx) - v.dist * n- t); 92 | 93 | float reflectence = n.transpose()* R * point; 94 | float light_dist = std::pow( point.norm(), 3); 95 | reflectence /=light_dist; 96 | // reflectence *= 10.; 97 | 98 | return reflectence*light_; 99 | } 100 | 101 | Vec3f LedOptimizer::LightJacobian(const SdfVoxel& v, const Vec3i& idx, const int frame_id) 102 | { 103 | Mat3f R = getRotation(frame_id); 104 | Vec3f t = getTranslation(frame_id); 105 | Vec3f n = v.grad.normalized(); 106 | // Vec3f n = computeDistGrad(v, idx).first; 107 | Vec3f point = R.transpose() * (tSDF_->voxel2world(idx) - v.dist * n- t); 108 | 109 | float reflectence = n.transpose()* R * point; 110 | float light_dist = std::pow( point.norm(), 3); 111 | reflectence /=light_dist; 112 | // reflectence *= 10.; 113 | 114 | return reflectence*Vec3f(v.r, v.g, v.b); 115 | } 116 | 117 | bool LedOptimizer::distJacobian(const SdfVoxel& v, const Vec3i& idx, Mat3f& R, Vec3f& t, const int frame_id, std::vector& J_d) 118 | { 119 | if(J_d.size()!=5){J_d.clear(); J_d.resize(5);} 120 | 121 | // dI(pi(x(v)))/dv 122 | const float fx = K_(0,0); 123 | const float fy = K_(1,1); 124 | const float cx = K_(0,2); 125 | const float cy = K_(1,2); 126 | 127 | cv::Mat img = getImage(frame_id); 128 | 129 | Vec3f point = R.transpose() * (tSDF_->voxel2world(idx) - v.dist * v.grad.normalized() - t); 130 | float m = fx * point[0]/point[2] + cx; 131 | float n = fy * point[1]/point[2] + cy; 132 | 133 | if (m<0 || m>=img.cols || n<0 || n>=img.rows) { 134 | return false; 135 | } 136 | 137 | Eigen::Matrix image_grad; 138 | image_grad.col(0) = computeImageGradient(n, m, img, 0); //3x2 139 | image_grad.col(1) = computeImageGradient(n, m, img, 1); 140 | 141 | const float z_inv = 1./point[2]; 142 | const float z_inv_sq = z_inv * z_inv; 143 | Eigen::Matrix pi_grad = Eigen::Matrix::Zero(); 144 | pi_grad(0,0) = fx * z_inv; 145 | pi_grad(0,2) = -fx * point[0] * z_inv_sq; 146 | pi_grad(1,1) = fy * z_inv; 147 | pi_grad(1,2) = -fy * point[1] * z_inv_sq; 148 | 149 | Mat3f image_pi_grad = image_grad * pi_grad; 150 | // dx/dv = g + d*dg/dv 151 | std::pair tmp = computeDistGrad(v, idx); 152 | Vec3f grad = tmp.first; 153 | 154 | bool lag = false; 155 | Vec3f dn_d0 = normalJacobian(grad, tmp.second, lag); 156 | 157 | Vec3f n_d1 = Vec3f::Zero(); 158 | n_d1[0] += tmp.second[0]; 159 | Vec3f dn_d1 = normalJacobian(grad, n_d1, lag); 160 | 161 | n_d1.setZero(); 162 | n_d1[1] += tmp.second[1]; 163 | Vec3f dn_d2 = normalJacobian(grad, n_d1, lag); 164 | 165 | n_d1.setZero(); 166 | n_d1[2] += tmp.second[2]; 167 | Vec3f dn_d3 = normalJacobian(grad, n_d1, lag); 168 | 169 | Vec3f dx_d = -v.grad.normalized() - v.dist * dn_d0; // grad = -n 170 | 171 | // dI_dd = dI_dpi * dpi_dx * dx_dd 172 | Vec3f dI = image_pi_grad * R.transpose() * dx_d; // 3 x 1 for 3 channels 173 | 174 | Vec3f dI1 = image_pi_grad * R.transpose() * (-v.dist * dn_d1); 175 | Vec3f dI2 = image_pi_grad * R.transpose() * (-v.dist * dn_d2); 176 | Vec3f dI3 = image_pi_grad * R.transpose() * (-v.dist * dn_d3); 177 | 178 | // M part 179 | float dm_d0 = (dn_d0.transpose() * R * point + grad.normalized().transpose() * dx_d ).value(); 180 | float dm_d1 = (dn_d1.transpose() * R * point + grad.normalized().transpose() * (-v.dist * dn_d1) ).value(); 181 | float dm_d2 = (dn_d2.transpose() * R * point + grad.normalized().transpose() * (-v.dist * dn_d2) ).value(); 182 | float dm_d3 = (dn_d3.transpose() * R * point + grad.normalized().transpose() * (-v.dist * dn_d3) ).value(); 183 | 184 | // attenuation radius 185 | float radius = std::pow(point.norm(), 3); 186 | 187 | 188 | // jacobian of radius part 189 | float dm_d0_2 = -3 * (point.transpose() * R.transpose() * dx_d).value() / std::pow(point.norm(), 5); 190 | float dm_d1_2 = -3 * (point.transpose() * R.transpose() * (-v.dist * dn_d1)).value() / std::pow(point.norm(), 5); 191 | float dm_d2_2 = -3 * (point.transpose() * R.transpose() * (-v.dist * dn_d2)).value() / std::pow(point.norm(), 5); 192 | float dm_d3_2 = -3 * (point.transpose() * R.transpose() * (-v.dist * dn_d3)).value() / std::pow(point.norm(), 5); 193 | 194 | Vec3f dR(v.r * light_[0], v.g * light_[1], v.b * light_[2]); 195 | 196 | 197 | dm_d0 = dm_d0 / radius + dm_d0_2 * (grad.normalized().transpose() * R *point).value(); 198 | dm_d1 = dm_d1 / radius + dm_d1_2 * (grad.normalized().transpose() * R *point).value(); 199 | dm_d2 = dm_d2 / radius + dm_d2_2 * (grad.normalized().transpose() * R *point).value(); 200 | dm_d3 = dm_d3 / radius + dm_d3_2 * (grad.normalized().transpose() * R *point).value(); 201 | 202 | 203 | Vec3f J_d0, J_d1, J_d2, J_d3; 204 | J_d0 = dI + dR*dm_d0; 205 | J_d1 = dI1 + dR*dm_d1; 206 | J_d2 = dI2 + dR*dm_d2; 207 | J_d3 = dI3 + dR*dm_d3; 208 | 209 | 210 | J_d[0] = (J_d0); 211 | J_d[1] = (J_d1); 212 | J_d[2] = (J_d2); 213 | J_d[3] = (J_d3); 214 | J_d[4] = (tmp.second); // push_back direction for compute col in distJacobian 215 | 216 | return true; 217 | 218 | } 219 | 220 | 221 | //----------------------numercial dist jacobian ---------------------------------- 222 | bool LedOptimizer::numerical_distJacobian(const SdfVoxel& v, const Vec3i& idx, Mat3f& R, Vec3f& t, const int frame_id, std::vector& J_d) 223 | { 224 | cv::Mat img = getImage(frame_id); 225 | Vec3f intensity; 226 | if(!getIntensity(idx, v, R, t, img, intensity)) 227 | return false; 228 | 229 | Vec3f f1 = intensity - renderedIntensity(v, idx, frame_id); 230 | SdfVoxel v2 = v; 231 | float step_size = voxel_size_*0.1; 232 | v2.dist += step_size; 233 | // Vec3f step = Vec3f::Random(); 234 | // v2.grad += step; 235 | Vec3f intensity2; 236 | 237 | if(!getIntensity(idx, v2, R, t, img, intensity2)) 238 | return false; 239 | 240 | Vec3f f2 = intensity - renderedIntensity(v2, idx, frame_id); 241 | 242 | J_d.clear(); 243 | J_d.push_back((f2 - f1)/ step_size); 244 | 245 | return true; 246 | } 247 | 248 | //---------------------------------- albedo related for using albedo regularizer ---------------------------------------------- 249 | //! compute albedo jacobian, should be diagonal matrix 250 | Eigen::SparseMatrix LedOptimizer::albedoJacobian() 251 | { 252 | 253 | std::vector > tripleVector; // to construct sparse matirx 254 | size_t total_num = surface_points_.size()*num_frames_*3; 255 | Eigen::SparseMatrix J_r(total_num, surface_points_.size()*3); 256 | 257 | int row; 258 | for(size_t frame_id = 0; frame_id < num_frames_; frame_id++){ 259 | row = 0; 260 | 261 | // DEBUG for saving normal image 262 | Mat3f R = getRotation(frame_id); 263 | Vec3f t = getTranslation(frame_id); 264 | cv::Mat img = getImage(frame_id); 265 | int h = img.rows; 266 | int w = img.cols; 267 | 268 | for(auto& lin_idx: surface_points_){ 269 | 270 | SdfVoxel& v = tSDF_->tsdf_[lin_idx]; 271 | Vec3i idx = tSDF_->line2idx(lin_idx); 272 | 273 | std::vector vis = tSDF_->vis_[lin_idx]; 274 | 275 | 276 | if(!vis[frame_id]){ //if the voxel is not visible in this frame, then skip the voxel for this frame 277 | row++; 278 | continue; 279 | } 280 | 281 | Vec3f Jr = rhoJacobian(v, idx, frame_id); 282 | // Vec3f Jr_vec = light_ * Jr; 283 | auto row_cur = 3*surface_points_.size()*frame_id + 3*row; 284 | 285 | for(size_t i = 0; i < 3; i++){ 286 | Eigen::Triplet Tri(row_cur+i, 3*row+i, Jr[i]); 287 | tripleVector.push_back(Tri); 288 | } 289 | 290 | row++; 291 | } 292 | } 293 | 294 | J_r.setFromTriplets(tripleVector.begin(), tripleVector.end()); 295 | return J_r; 296 | 297 | } 298 | 299 | Eigen::SparseMatrix LedOptimizer::lightJacobian() 300 | { 301 | 302 | std::vector > tripleVector; // to construct sparse matirx 303 | size_t total_num = surface_points_.size()*num_frames_*3; 304 | Eigen::SparseMatrix J_l(total_num, 3); 305 | 306 | int row; 307 | for(size_t frame_id = 0; frame_id < num_frames_; frame_id++){ 308 | row = 0; 309 | 310 | // DEBUG for saving normal image 311 | Mat3f R = getRotation(frame_id); 312 | Vec3f t = getTranslation(frame_id); 313 | cv::Mat img = getImage(frame_id); 314 | int h = img.rows; 315 | int w = img.cols; 316 | 317 | for(auto& lin_idx: surface_points_){ 318 | 319 | SdfVoxel& v = tSDF_->tsdf_[lin_idx]; 320 | Vec3i idx = tSDF_->line2idx(lin_idx); 321 | 322 | std::vector vis = tSDF_->vis_[lin_idx]; 323 | 324 | 325 | if(!vis[frame_id]){ //if the voxel is not visible in this frame, then skip the voxel for this frame 326 | row++; 327 | continue; 328 | } 329 | 330 | Vec3f Jl = LightJacobian(v, idx, frame_id); 331 | // Vec3f Jr_vec = light_ * Jr; 332 | auto row_cur = 3*surface_points_.size()*frame_id + 3*row; 333 | 334 | for(size_t i = 0; i < 3; i++){ 335 | Eigen::Triplet Tri(row_cur+i, i, Jl[i]); 336 | tripleVector.push_back(Tri); 337 | } 338 | 339 | row++; 340 | } 341 | } 342 | 343 | J_l.setFromTriplets(tripleVector.begin(), tripleVector.end()); 344 | return J_l; 345 | 346 | } 347 | 348 | 349 | //--------------------------------- pose jacobian for all frames ------------------------------------------------------------------- 350 | //! compute pose jacobian sparse matrix 351 | Eigen::SparseMatrix LedOptimizer::poseJacobian() 352 | { 353 | std::vector > tripleVector; // to construct sparse matirx 354 | size_t total_num = surface_points_.size()*num_frames_*3; 355 | 356 | Eigen::SparseMatrix J_c(total_num, num_frames_*6); 357 | 358 | int row; 359 | 360 | for(int frame = 0; frame < num_frames_; frame++){ 361 | row = 0; 362 | const Mat3f R = getRotation(frame); 363 | const Vec3f t = getTranslation(frame); 364 | 365 | for(auto& lin_idx: surface_points_){ 366 | 367 | const SdfVoxel& v = tSDF_->tsdf_[lin_idx]; 368 | Vec3i idx = tSDF_->line2idx(lin_idx); 369 | 370 | std::vector vis = tSDF_->vis_[lin_idx]; 371 | 372 | 373 | if(!vis[frame]){ //if the voxel is not visible in this frame, then skip the voxel for this frame 374 | row++; 375 | continue; 376 | } 377 | 378 | auto row_cur = 3*frame*surface_points_.size()+3*row; 379 | auto col_cur = frame*6; 380 | Eigen::Matrix J_c; 381 | 382 | if(!poseJacobian(idx, v, frame, R , t, J_c)){ 383 | row++; 384 | continue; 385 | } 386 | 387 | // load triplet for r_ijc 388 | for(size_t j = 0; j < 3; j++)for(size_t i = 0; i < 6; i++){ 389 | Eigen::Triplet Tri(row_cur+j, col_cur+i, J_c(j,i)); 390 | tripleVector.push_back(Tri); 391 | } 392 | 393 | row++; 394 | } 395 | } 396 | J_c.setFromTriplets(tripleVector.begin(), tripleVector.end()); 397 | if(total_num != 3*num_frames_*row){std::cout << "rows number not correct!" << std::endl;} 398 | return J_c; 399 | } 400 | 401 | 402 | //---------------------------------- distance related consider coupling of all voxels ---------------------------------------------- 403 | //! compute distance jacobian, consider all related voxels 404 | Eigen::SparseMatrix LedOptimizer::distJacobian() 405 | { 406 | 407 | std::vector > tripleVector; // to construct sparse matirx 408 | size_t total_num = surface_points_.size()*num_frames_*3; 409 | 410 | // auto row; // prepare some thing 411 | 412 | int row; 413 | for(size_t frame_id = 0; frame_id < num_frames_; frame_id++){ 414 | row = 0; 415 | for(auto& lin_idx: surface_points_){ 416 | 417 | SdfVoxel& v = tSDF_->tsdf_[lin_idx]; 418 | Vec3i idx = tSDF_->line2idx(lin_idx); 419 | 420 | std::vector vis = tSDF_->vis_[lin_idx]; 421 | 422 | 423 | if(!vis[frame_id]){ //if the voxel is not visible in this frame, then skip the voxel for this frame 424 | row++; 425 | continue; 426 | } 427 | 428 | Vec3f intensity, direction; 429 | std::vector J, J_n; 430 | Mat3f R = getRotation(frame_id); 431 | Vec3f t = getTranslation(frame_id); 432 | // cv::Mat img = getImage(frame_id); 433 | 434 | if(!distJacobian(v, idx, R, t, frame_id, J)){ //if voxel is projected outside of the boundary, then skip 435 | row++; 436 | continue; 437 | } 438 | 439 | 440 | direction = J[4]; // store the direction to caculate the col 441 | 442 | 443 | // Jacobain for d0 -- row: lin_idx, col: lin_idx (diagonal) 444 | auto it = std::find(surface_points_.begin(), surface_points_.end(), lin_idx); 445 | auto col = std::distance(surface_points_.begin(), it); 446 | 447 | auto row_cur = 3*frame_id*surface_points_.size() + 3*row; 448 | Eigen::Triplet Tri0(row_cur, col, J[0][0]); 449 | Eigen::Triplet Tri1(row_cur+1, col, J[0][1]); 450 | Eigen::Triplet Tri2(row_cur+2, col, J[0][2]); 451 | tripleVector.push_back(Tri0); 452 | tripleVector.push_back(Tri1); 453 | tripleVector.push_back(Tri2); 454 | 455 | 456 | // Jacobian for di -- rowL lin_idx, col: voxel i idx 457 | for(size_t i=0; i<3; i++){ 458 | Vec3i idx_ = idx; 459 | idx_[i] += (int)direction[i]; 460 | int lin_idx_ = tSDF_->idx2line(idx_); 461 | // std::cout <<"lin idx_ " << i << ": " << lin_idx_ < Tri0(row_cur, col, J[i+1][0]); 467 | Eigen::Triplet Tri1(row_cur+1, col, J[i+1][1]); 468 | Eigen::Triplet Tri2(row_cur+2, col, J[i+1][2]); 469 | tripleVector.push_back(Tri0); 470 | tripleVector.push_back(Tri1); 471 | tripleVector.push_back(Tri2); 472 | } 473 | } 474 | 475 | row++; 476 | } 477 | } 478 | 479 | Eigen::SparseMatrix J_d(total_num, surface_points_.size()); 480 | J_d.setFromTriplets(tripleVector.begin(), tripleVector.end()); 481 | // if(total_num != 3*num_frames_*row){std::cout << "rows number not correct!" << std::endl;} 482 | return J_d; 483 | } 484 | 485 | //-------------------------------------------------------------------- residual --------------------------------------------------------------------------------- 486 | //! compute the residual r_{ic}(v_j) = I_ic(v_j) - rho(v_j) 487 | // arrange order: image i voxel 1(r,g,b), 2(r,g,b), ... 488 | std::pair > LedOptimizer::computeResidual() 489 | { 490 | Eigen::VectorXf residual; 491 | Eigen::SparseMatrix weight; 492 | 493 | size_t total_num = surface_points_.size()*num_frames_*3; //3 is the r,g,b channels 494 | 495 | weight.resize(total_num,total_num); 496 | // weight.reserve(num_voxels_); 497 | residual.resize(total_num); 498 | 499 | residual.setZero(); 500 | int row; 501 | for(size_t frame_id = 0; frame_id < num_frames_; frame_id++){ 502 | row = 0; 503 | for(auto& lin_idx: surface_points_){ 504 | 505 | SdfVoxel& v = tSDF_->tsdf_[lin_idx]; 506 | Vec3i idx = tSDF_->line2idx(lin_idx); 507 | 508 | std::vector vis = tSDF_->vis_[lin_idx]; 509 | 510 | if(!vis[frame_id]){ //if the voxel is not visible in this frame, then skip the voxel for this frame 511 | row++; 512 | continue; 513 | } 514 | 515 | // weight.coeffRef(row, row) = 0; 516 | 517 | Vec3f intensity; 518 | Mat3f R = getRotation(frame_id); 519 | Vec3f t = getTranslation(frame_id); 520 | cv::Mat img = getImage(frame_id); 521 | 522 | if(!getIntensity(idx, v, R , t, img, intensity)){ //if voxel is projected outside of the boundary, then skip 523 | row++; 524 | continue; 525 | } 526 | 527 | Vec3f r = intensity - renderedIntensity(v, idx, frame_id); 528 | Vec3f w = computeWeight(r); 529 | auto row_cur = 3*frame_id*surface_points_.size()+3*row; 530 | weight.coeffRef(row_cur, row_cur) = w[0]; 531 | weight.coeffRef(row_cur+1, row_cur+1) = w[1]; 532 | weight.coeffRef(row_cur+2, row_cur+2) = w[2]; 533 | residual.segment(row_cur, 3) = r; 534 | 535 | row++; 536 | } 537 | } 538 | 539 | std::pair > tmp(residual, weight); 540 | return tmp; 541 | 542 | } 543 | -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/Optimizer.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : Optimizer.h 3 | // Author : Lu Sang 4 | // Date : 10/2021 5 | // License : GNU General Public License 6 | // Description : header of class Optimizer 7 | //============================================================================ 8 | 9 | #ifndef OPTIMIZER_H_ 10 | #define OPTIMIZER_H_ 11 | 12 | #include "mat.h" 13 | // #include "sdf_tracker/VolumetricGradPixelSdf.h" 14 | #include "sdf_tracker/VolumetricGradSdf.h" 15 | #include "OptimizerSettings.h" 16 | 17 | #include 18 | #include 19 | // #include 20 | #include 21 | 22 | 23 | 24 | class Optimizer { 25 | 26 | // variables: 27 | protected: 28 | size_t num_frames_; 29 | size_t num_voxels_; 30 | float voxel_size_; 31 | float voxel_size_inv_; 32 | Mat3f K_; // camera intrinsics 33 | std::string save_path_; 34 | 35 | VolumetricGradSdf* tSDF_; 36 | // std::vector> vis_; // visibility per voxel and frame 37 | 38 | std::vector frame_idx_; // indices of keyframes to be taken into account 39 | std::vector> images_; // vector of pointers to keyframes 40 | std::vector > poses_; // poses from tracking part 41 | std::vector key_stamps_; 42 | 43 | // std::vector > light_; // length 4 for first order and length 9 for second order 44 | 45 | std::vector surface_points_; 46 | // std::vector band_voxels_; 47 | 48 | 49 | OptimizerSettings* settings_; 50 | 51 | // pose related 52 | Mat3f getRotation(const int frame_id) 53 | { 54 | return poses_[frame_id].topLeftCorner(3,3); 55 | } 56 | 57 | Vec3f getTranslation(const int frame_id) 58 | { 59 | return poses_[frame_id].topRightCorner(3,1); 60 | } 61 | 62 | cv::Mat getImage(const int frame_id) const 63 | { 64 | return *(images_[frame_id]); 65 | } 66 | 67 | void getSurfaceVoxel(); 68 | 69 | 70 | //update functions 71 | void updateAlbedo(const int lin_idx, float delta_r, float delta_g, float delta_b); 72 | void updateAlbedo(Eigen::VectorXf& delta_r); 73 | void updateGrad(); 74 | void updateDist(Eigen::VectorXf& delta_d, bool updateGrad); 75 | void updatePose(const int frame_id, const Mat3f& R, const Vec3f& t, const Vec6f& xi); 76 | void updatePose(Eigen::VectorXf& delta_xi); 77 | 78 | //regularizer energy functions 79 | virtual float getPSEnergy() = 0; 80 | float getNormalEnergy(); 81 | float getAlbedoRegEnergy(); 82 | float getLaplacianEnergy(); 83 | 84 | // compute weight and loss 85 | Eigen::VectorXf computeWeight(Eigen::VectorXf r); 86 | float computeLoss(Eigen::VectorXf r); 87 | 88 | //useful functions 89 | bool getIntensity(const Vec3i& idx, const SdfVoxel& v, const Mat3f& R, const Vec3f& t, const cv::Mat& img, Vec3f& intensity); 90 | 91 | // regularizer related 92 | float computeDistLaplacian(const SdfVoxel& v, const Vec3i& idx); 93 | 94 | std::pair computeDistGrad(const SdfVoxel& v, const Vec3i& idx); 95 | void distRegJacobian(const SdfVoxel& v, const Vec3i& idx, std::vector& Jr_d); 96 | 97 | void albedoRegJacobian(const SdfVoxel& v, const Vec3i& idx, std::vector& Jr_r); 98 | std::pair computeAlbedoGrad(const SdfVoxel& v, const Vec3i& idx); 99 | 100 | Vec3f normalJacobian(const SdfVoxel& v, const Vec3i& idx); 101 | Vec3f normalJacobian(Vec3f& grad, Vec3f& direction, bool lag); 102 | 103 | 104 | std::pair, Eigen::VectorXf> distRegJacobian(); 105 | std::pair, Eigen::VectorXf> distLaplacianJacobian(); 106 | std::pair, Eigen::VectorXf> albedoRegJacobian(); 107 | 108 | 109 | 110 | bool ifValidDirection(const Vec3i& idx, const int direction, const int pos); 111 | 112 | //auxilary functions 113 | 114 | float getTotalEnergy(float E, float E_n, float E_l, float E_r, std::ofstream& file); 115 | 116 | void setPoses(Eigen::VectorXf& xi); 117 | Eigen::VectorXf stackPoses(); 118 | void getAlldsitance(Eigen::VectorXf& Distance); 119 | void setAlldsitance(Eigen::VectorXf& Distance); 120 | void getAllalbedo(Eigen::VectorXf& rho); 121 | void setAllalbedo(Eigen::VectorXf& rho); 122 | 123 | public: 124 | 125 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 126 | 127 | Optimizer(VolumetricGradSdf* tSDF, 128 | const float voxel_size, 129 | const Mat3f& K, 130 | std::string save_path, 131 | OptimizerSettings* settings 132 | 133 | ); 134 | 135 | 136 | 137 | void setImages(std::vector> images) 138 | { 139 | images_ = images; 140 | } 141 | 142 | void setPoses(std::vector >& pose) 143 | { 144 | poses_ = pose; 145 | } 146 | 147 | void setKeyframes( std::vector& keyframes) 148 | { 149 | frame_idx_ = keyframes; 150 | } 151 | void setKeytimestamps(std::vector& keystamps) 152 | { 153 | key_stamps_ = keystamps; 154 | } 155 | 156 | // void setSavePath(std::string output){ 157 | // save_path_ = output; 158 | // } 159 | 160 | //debug functions 161 | void check_tsdf(bool sample); 162 | void check_vis_map(bool sample); 163 | void check_albedo(bool sample); 164 | void compare_normal(bool sample); 165 | 166 | virtual void init() = 0; 167 | void select_vis(); 168 | void initAlbedo(); 169 | bool savePoses(std::string filename); 170 | bool extract_mesh(std::string filename); 171 | bool save_pointcloud(std::string filename); 172 | bool save_white_mesh(std::string filename); 173 | bool saveSDF(std::string filename); 174 | // bool save_info(); 175 | 176 | void subsampling(); 177 | 178 | virtual bool alternatingOptimize(bool light, bool albedo, bool distance, bool pose){ 179 | return false; 180 | } 181 | 182 | }; 183 | 184 | 185 | #endif // PS_OPTIMIZER_H_ 186 | -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/OptimizerSettings.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef OPTIMIZER_SETTINGS_ 3 | #define OPTIMIZER_SETTINGS_ 4 | 5 | #include 6 | 7 | // parse loss function 8 | enum LossFunction 9 | { 10 | L2 = 0, 11 | CAUCHY = 1, 12 | HUBER = 2, 13 | TUKEY = 3, 14 | TRUNC_L2 = 4 15 | }; 16 | 17 | // parse image formation model 18 | enum ModelType { 19 | SH1, 20 | SH2, 21 | LED 22 | }; 23 | 24 | struct OptimizerSettings { 25 | int max_it; // maximum number of iterations 26 | float conv_threshold; // threshold for convergence 27 | float damping; // LM damping term (often referred to as lambda) 28 | float lambda; // lambda for weight function (not for LM!) 29 | float lambda_sq; 30 | float reg_weight_rho; 31 | float reg_weight_n; 32 | float reg_weight_l; 33 | int order; 34 | bool upsample; 35 | ModelType model; 36 | LossFunction loss; // type of (robust) loss 37 | 38 | OptimizerSettings() : 39 | max_it(100), 40 | conv_threshold(1e-4), 41 | damping(1.0), 42 | lambda(0.5), 43 | lambda_sq(lambda * lambda), 44 | reg_weight_rho(0.0), 45 | reg_weight_n(0.0), 46 | reg_weight_l(0.0), 47 | order(1), 48 | upsample(false), 49 | model(SH1), 50 | loss(CAUCHY) 51 | {} 52 | 53 | OptimizerSettings(float reg_r, float reg_n, float reg_l) : 54 | max_it(100), 55 | conv_threshold(1e-4), 56 | damping(1.0), 57 | lambda(0.5), 58 | lambda_sq(lambda * lambda), 59 | reg_weight_rho(reg_r), 60 | reg_weight_n(reg_n), 61 | reg_weight_l(reg_l), 62 | order(1), 63 | upsample(false), 64 | model(SH1), 65 | loss(CAUCHY) 66 | {} 67 | 68 | OptimizerSettings(float reg_r, float reg_n, float reg_l, ModelType model_type, LossFunction loss_fun) : 69 | max_it(100), 70 | conv_threshold(1e-4), 71 | damping(1.0), 72 | lambda(0.5), 73 | lambda_sq(lambda * lambda), 74 | reg_weight_rho(reg_r), 75 | reg_weight_n(reg_n), 76 | reg_weight_l(reg_l), 77 | order(1), 78 | upsample(false), 79 | model(model_type), 80 | loss(loss_fun) 81 | {} 82 | }; 83 | #endif // OPTIMIZER_SETTINGS_ -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/PsOptimizer.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : PsOptimizer.cpp 3 | // Author : Lu Sang 4 | // Date : 09/2021 5 | // License : GNU General Public License 6 | // Description : implementation of class PsOptimizer 7 | //============================================================================ 8 | 9 | #include "PsOptimizer.h" 10 | #include "sdf_tracker/Sdfvoxel.h" 11 | #include "Timer.h" 12 | 13 | // ============================= class functions ===================================== 14 | 15 | PsOptimizer::PsOptimizer(VolumetricGradSdf* tSDF, 16 | const float voxel_size, 17 | const Mat3f& K, 18 | std::string save_path, 19 | OptimizerSettings* settings): 20 | Optimizer(tSDF, voxel_size, K, save_path, settings) 21 | { 22 | init(); 23 | } 24 | 25 | void PsOptimizer::init() 26 | { 27 | num_frames_ = frame_idx_.size(); 28 | num_voxels_ = tSDF_->grid_dim_[0]*tSDF_->grid_dim_[1]*tSDF_->grid_dim_[2]; 29 | // initialize lighting vectors for each frame 30 | Vec3f s(0.0,0.0,-1.0); 31 | 32 | for(size_t frame = 0; frame < num_frames_; frame++){ 33 | Mat3f R = getRotation(frame); 34 | Eigen::VectorXf l = SH(R*s, settings_->order); 35 | l[0] = 0.02; 36 | light_.push_back(l); 37 | } 38 | 39 | 40 | select_vis(); 41 | 42 | } 43 | 44 | 45 | //------------------ energy function-------------------------------------------- 46 | 47 | float PsOptimizer::getPSEnergy() 48 | { 49 | float E = 0; 50 | 51 | for(auto& lin_idx: surface_points_){ 52 | 53 | const SdfVoxel& v = tSDF_->tsdf_[lin_idx]; 54 | std::vector vis = tSDF_->vis_[lin_idx]; 55 | 56 | for(size_t frame_id = 0; frame_id < num_frames_; frame_id++){ 57 | if(!vis[frame_id]){ // vis for whole sequence, frame_idx_ for key frame 58 | continue; 59 | } 60 | 61 | Vec3i idx = tSDF_->line2idx(lin_idx); 62 | 63 | Mat3f R = getRotation(frame_id); 64 | Vec3f t = getTranslation(frame_id); 65 | cv::Mat img = getImage(frame_id); 66 | 67 | Vec3f intensity; 68 | if(!getIntensity(idx, v, R, t, img, intensity)){ 69 | continue; 70 | } 71 | 72 | E += computeLoss(intensity - renderedIntensity(v, idx, frame_id)); 73 | } 74 | 75 | } 76 | 77 | return E/static_cast(surface_points_.size()); 78 | } 79 | 80 | 81 | 82 | 83 | //------------------------------- optimization function --------------------------------------------------------------------------------- 84 | 85 | void PsOptimizer::optimizeAlbedoAll(bool albedo_reg, float damping) 86 | { 87 | Eigen::SparseMatrix J_r = albedoJacobian(); 88 | std::pair > pair = computeResidual(); 89 | Eigen::VectorXf r = pair.first; 90 | Eigen::SparseMatrix w = pair.second; 91 | 92 | Eigen::SparseMatrix H = J_r.transpose()*w*J_r; 93 | Eigen::VectorXf b = J_r.transpose()*w*r; 94 | 95 | if(albedo_reg){ 96 | std::pair, Eigen::VectorXf> tmp = albedoRegJacobian(); 97 | Eigen::SparseMatrix Jr_r = tmp.first; 98 | Eigen::VectorXf r_r = tmp.second; 99 | H += settings_->reg_weight_rho*Jr_r.transpose()*Jr_r; 100 | b += settings_->reg_weight_rho*Jr_r.transpose()*r_r; 101 | } 102 | 103 | if(damping!=0.0){ 104 | H.diagonal() += damping*H.diagonal(); 105 | } 106 | 107 | Eigen::ConjugateGradient> solver; 108 | 109 | solver.compute(H); 110 | if(solver.info() != Eigen::Success ){ 111 | std::cout << "decomposition failed! " << std::endl; 112 | } 113 | Eigen::VectorXf delta_r = solver.solve(b); 114 | if(solver.info()!=Eigen::Success) { 115 | std::cout << "solver failed: " << solver.info() << "\n"; 116 | } 117 | 118 | if(solver.info()==Eigen::Success){ 119 | updateAlbedo(delta_r); 120 | } 121 | } 122 | 123 | 124 | void PsOptimizer::optimizeDistAll(bool normal_reg, bool laplacian_reg, float damping) 125 | { 126 | // bool normal_reg = false; 127 | 128 | Eigen::SparseMatrix J_d = distJacobian(); 129 | std::pair > tmp = computeResidual(); 130 | Eigen::VectorXf r = tmp.first; 131 | Eigen::SparseMatrix w = tmp.second; 132 | 133 | Eigen::SparseMatrix H = J_d.transpose()*w*J_d; 134 | Eigen::VectorXf b = J_d.transpose()*w*r; 135 | 136 | if(normal_reg){ 137 | std::pair, Eigen::VectorXf> pair = distRegJacobian(); 138 | Eigen::SparseMatrix Jr_d = pair.first; 139 | Eigen::VectorXf r_n = pair.second; 140 | H += settings_->reg_weight_n*(Jr_d.transpose()*Jr_d); 141 | b += settings_->reg_weight_n*(Jr_d.transpose()*r_n); 142 | } 143 | 144 | if(laplacian_reg){ 145 | std::pair, Eigen::VectorXf> pair = distLaplacianJacobian(); 146 | Eigen::SparseMatrix Jl_d = pair.first; 147 | Eigen::VectorXf r_n = pair.second; 148 | H += settings_->reg_weight_l*(Jl_d.transpose()*Jl_d); 149 | b += settings_->reg_weight_l*(Jl_d.transpose()*r_n); 150 | } 151 | 152 | if(damping!=0.0){ 153 | H.diagonal() += damping*H.diagonal(); 154 | } 155 | 156 | 157 | Eigen::ConjugateGradient> solver; 158 | 159 | solver.compute(H); 160 | if(solver.info() != Eigen::Success ){ 161 | std::cout << "decomposition failed! " << std::endl; 162 | } 163 | Eigen::VectorXf delta_d = solver.solve(b); 164 | if(solver.info()!=Eigen::Success) { 165 | std::cout << "solver failed: " << solver.info() << "\n"; 166 | } 167 | 168 | if(solver.info()==Eigen::Success){ 169 | updateDist(delta_d, true); 170 | } 171 | 172 | } 173 | 174 | 175 | void PsOptimizer::optimizeLightAll() 176 | { 177 | Eigen::SparseMatrix Jl = lightJacobian(); 178 | std::pair > pair = computeResidual(); 179 | Eigen::VectorXf r = pair.first; 180 | Eigen::SparseMatrix w = pair.second; 181 | Eigen::SparseMatrix H = Jl.transpose()*w*Jl; 182 | Eigen::VectorXf b = Jl.transpose()*w*r; 183 | 184 | Eigen::ConjugateGradient> solver; 185 | 186 | solver.compute(H); 187 | if(solver.info() != Eigen::Success ){ 188 | std::cout << "decomposition failed! " << std::endl; 189 | } 190 | 191 | Eigen::VectorXf delta_l = solver.solve(b); 192 | if(solver.info()!=Eigen::Success) { 193 | std::cout << "solver failed: " << solver.info() << "\n"; 194 | } 195 | 196 | size_t basis; //SH 1 or SH 2 197 | if(settings_->order == 1)basis = 4; 198 | if(settings_->order == 2)basis = 9; 199 | for(size_t frame = 0; frame < num_frames_; frame++){ 200 | light_[frame] -= delta_l.segment(frame*basis, basis); 201 | } 202 | 203 | } 204 | 205 | 206 | //! optimize camera poses 207 | void PsOptimizer::optimizePosesAll(float damping) 208 | { 209 | Eigen::SparseMatrix Jc = poseJacobian(); 210 | std::pair > pair = computeResidual(); 211 | Eigen::VectorXf r = pair.first; 212 | Eigen::SparseMatrix w = pair.second; 213 | Eigen::SparseMatrix H = Jc.transpose()*w*Jc; 214 | Eigen::VectorXf b = Jc.transpose()*w*r; 215 | 216 | 217 | Eigen::ConjugateGradient> solver; 218 | if(damping!=0.0){ 219 | H.diagonal() += damping*H.diagonal(); 220 | } 221 | 222 | solver.compute(H); 223 | if(solver.info() != Eigen::Success ){ 224 | std::cout << "decomposition failed! " << std::endl; 225 | } 226 | 227 | Eigen::VectorXf delta_xi = solver.solve(b); 228 | if(solver.info()!=Eigen::Success) { 229 | std::cout << "solver failed: " << solver.info() << "\n"; 230 | } 231 | 232 | updatePose(delta_xi); 233 | 234 | } 235 | 236 | 237 | 238 | // ---------------------------- optimization function ----------------------------------- 239 | bool PsOptimizer::alternatingOptimize(bool light, bool albedo, bool distance, bool pose) 240 | { 241 | 242 | // save iter detail 243 | std::ofstream file; 244 | file.open((save_path_ + "optimizer_doc.txt").c_str()); 245 | 246 | bool normal_reg = false; 247 | bool albedo_reg = false; 248 | bool laplacian_reg = false; 249 | 250 | if(settings_->reg_weight_n != 0.0) normal_reg = true; 251 | if(settings_->reg_weight_rho != 0.0) albedo_reg = true; 252 | if(settings_->reg_weight_l != 0.0) laplacian_reg = true; 253 | 254 | std::cout << "albation study settings: \n" << 255 | "light: " << light << "\n" << "albedo: " << albedo << "\n" << "distance: " << distance << "\n" << "pose: " << pose << std::endl; 256 | 257 | float damping = settings_->damping; 258 | 259 | file << "albation study settings: \t" << 260 | "light: " << light << "\t" << "albedo: " << albedo << "\t" << "distance: " << distance << "\t" << "pose: " << pose << "\n" << "num of key frame: " << num_frames_ << " \n total voxels: " << num_voxels_ << "\n"; 261 | 262 | 263 | Timer T; 264 | 265 | T.tic(); 266 | // getSurfaceVoxel(); // compute surface points. 267 | initAlbedo(); 268 | T.toc("initial albedo"); 269 | 270 | float E, E_total; 271 | float E_n = 0.0; 272 | float E_r = 0.0; 273 | float E_l = 0.0; 274 | E = getPSEnergy(); 275 | if(normal_reg) { 276 | E_n = getNormalEnergy(); 277 | settings_->reg_weight_n *= E / E_n; //normalize the weight 278 | } 279 | if(albedo_reg) E_r = getAlbedoRegEnergy(); 280 | 281 | if(laplacian_reg){ 282 | E_l = getLaplacianEnergy(); 283 | settings_->reg_weight_l *= E / E_l; 284 | if(settings_->upsample)laplacian_reg=false; 285 | } 286 | 287 | 288 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 289 | 290 | float rel_diff; 291 | std::vector E_vec; 292 | 293 | E_vec.push_back(E_total); 294 | 295 | std::cout << "======================== start alternating optimization ================================= " << std::endl 296 | << "======> total key frame:\t " << num_frames_ << std::endl 297 | << "======> total voxels: \t" << num_voxels_ << std::endl 298 | << "======> inital PS energy:" << E << "\t normal reg energy: " << settings_->reg_weight_n*E_n << "\t laplacian reg energy: " << settings_->reg_weight_l*E_l << " \t rho reg energy: " << settings_->reg_weight_rho*E_r << std::endl 299 | << "================================================================================================= " << std::endl; 300 | 301 | int iter = 0; 302 | 303 | while(iter < settings_->max_it){ 304 | if(albedo){ 305 | 306 | T.tic(); 307 | optimizeAlbedoAll(albedo_reg, damping); 308 | // optimizeAlbedo(); 309 | T.toc("albedo optimize"); 310 | 311 | E = getPSEnergy(); 312 | if(albedo_reg) E_r = getAlbedoRegEnergy(); 313 | std::cout << "===> [" << iter << "]: after albedo optimization: "; 314 | 315 | file << "===> [" << iter << "]: after albedo optimization: \n"; 316 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 317 | } 318 | 319 | if(light){ 320 | T.tic(); 321 | optimizeLightAll(); 322 | T.toc("lights optimize"); 323 | E = getPSEnergy(); 324 | std::cout << "===> [" << iter << "]: after lights optimization: "; 325 | 326 | file << "===> [" << iter << "]: after light optimization: \n"; 327 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 328 | 329 | 330 | } 331 | 332 | if(distance){ 333 | 334 | T.tic(); 335 | optimizeDistAll(normal_reg, laplacian_reg, damping); 336 | T.toc("dist optimize"); 337 | 338 | E = getPSEnergy(); 339 | if(normal_reg) E_n = getNormalEnergy(); 340 | if(laplacian_reg) E_l = getLaplacianEnergy(); 341 | 342 | std::cout << "===> [" << iter << "]: after distance optimization: "; 343 | 344 | file << "===> [" << iter << "]: after distance optimization: \n"; 345 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 346 | 347 | } 348 | 349 | if(pose){ 350 | T.tic(); 351 | optimizePosesAll(damping); 352 | T.toc("poses optimize"); 353 | 354 | E = getPSEnergy(); 355 | std::cout << "===> [" << iter << "]: after pose optimization: "; 356 | 357 | file << "===> [" << iter << "]: after pose optimization: \n"; 358 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 359 | 360 | } 361 | 362 | E_vec.push_back(E_total); 363 | 364 | rel_diff = abs(E_vec.end()[-2] - E_total)/E_vec.end()[-2]; 365 | std::cout << "===> [" << iter << "]: relative diff " << rel_diff << std::endl; 366 | file << "===> [" << iter << "]: relative diff " << rel_diff << "\n"; 367 | 368 | if(rel_diff < settings_->conv_threshold) 369 | { 370 | std::cout << "===> [" << iter << "]: converged!" < [" << iter << "]: converged! \n"; 372 | save_pointcloud("final_refined"); 373 | extract_mesh("final_refined"); 374 | return true; 375 | } 376 | 377 | if(E_vec.end()[-2] < E_total ) 378 | { 379 | save_pointcloud("final_refined"); 380 | extract_mesh("final_refined"); 381 | std::cout << "===> [" << iter << "]: diverged!" < [" << iter << "]: diverged!\n"; 383 | return false; 384 | } 385 | 386 | if(iter == 5 && settings_->upsample){ 387 | 388 | // if upsampled, need laplacian to smooth a bit 389 | // maybe we can remove it 390 | if(settings_->reg_weight_l==0.0)settings_->reg_weight_l = 1.0; 391 | laplacian_reg = true; 392 | 393 | T.tic(); 394 | subsampling(); 395 | T.toc("upsample"); 396 | 397 | save_pointcloud("upsample_after_" + std::to_string(iter));//DEBUG 398 | extract_mesh("upsample_after_" + std::to_string(iter));//DEBUG 399 | 400 | file << "===> [" << iter << "]: after pose optimization: \n"; 401 | 402 | E_l = getLaplacianEnergy(); 403 | settings_->reg_weight_l *= E / E_l; 404 | 405 | E_total = getTotalEnergy(E, E_n, E_l, E_r, file); 406 | 407 | E_vec.push_back(E_total); 408 | 409 | } 410 | 411 | if (iter > 15 && settings_->upsample){ 412 | settings_->reg_weight_l = 0.0; 413 | } 414 | 415 | 416 | ++iter; 417 | 418 | // save after each 3 iterations for debug 419 | if(iter%3 == 0){ 420 | savePoses("after_poses_opt_" + std::to_string(iter)); 421 | save_pointcloud("after_iter_" + std::to_string(iter)); 422 | extract_mesh("after_iter_" + std::to_string(iter)); 423 | } 424 | 425 | } 426 | 427 | return false; 428 | } -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/PsOptimizer.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : PsOptimizer.h 3 | // Author : Lu Sang 4 | // Date : 09/2021 5 | // License : GNU General Public License 6 | // Description : header of class PsOptimizer 7 | //============================================================================ 8 | 9 | #ifndef PS_OPTIMIZER_H_ 10 | #define PS_OPTIMIZER_H_ 11 | 12 | #include "mat.h" 13 | #include "Optimizer.h" 14 | 15 | 16 | #include 17 | #include 18 | // #include 19 | #include 20 | 21 | 22 | 23 | class PsOptimizer : public Optimizer { 24 | 25 | 26 | 27 | std::vector light_; // length 4 for first order and length 9 for second order // for each image there is a light 28 | 29 | 30 | // private method: 31 | 32 | // image related 33 | Eigen::VectorXf SH(const Vec3f& n, const int order); 34 | Vec3f renderedIntensity(const SdfVoxel& v, const Vec3i& idx, const int frame_idx); 35 | bool computeResidual(const SdfVoxel& v, const Vec3i& idx, const int frame_id, Vec3f& r); 36 | 37 | //residual matrix 38 | std::pair > computeResidual(); 39 | 40 | // energy related 41 | virtual float getPSEnergy(); 42 | 43 | // jacobian related 44 | bool poseJacobian(const Vec3i& idx, const SdfVoxel& v, int frame_id, const Mat3f& R, const Vec3f& t, Eigen::Matrix& J_c); 45 | 46 | float rhoJacobian(const SdfVoxel& v, const int frame_id); 47 | float rhoJacobian(const SdfVoxel& v, const Vec3i& idx, const int frame_id); 48 | 49 | 50 | 51 | void lightJacobian(const SdfVoxel& v, const int frame_id, std::vector& J_l); 52 | void lightJacobian(const SdfVoxel& v, const Vec3i& idx, const int frame_id, std::vector& J_l); 53 | 54 | bool distJacobian(const SdfVoxel& v, const Vec3i& idx, Mat3f& R, Vec3f& t, const int frame_id, std::vector& J_d); 55 | bool numerical_distJacobian(const SdfVoxel& v, const Vec3i& idx, Mat3f& R, Vec3f& t, const int frame_id, std::vector& J_d); 56 | // bool distJacobian_numeric(SdfVoxel& v, const Vec3i& idx, Mat3f& R, Vec3f& t, const int frame_id, Vec3f& J_d); 57 | 58 | // jacobian matrix 59 | Eigen::SparseMatrix lightJacobian(); 60 | Eigen::SparseMatrix albedoJacobian(); 61 | Eigen::SparseMatrix distJacobian(); 62 | Eigen::SparseMatrix poseJacobian(); 63 | 64 | 65 | void optimizeAlbedoAll(bool albedo_reg, float damping); 66 | void optimizeLightAll(); 67 | void optimizeDistAll(bool normal_reg, bool laplacian_reg, float damping); 68 | void optimizePosesAll(float damping); 69 | 70 | public: 71 | 72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 73 | 74 | PsOptimizer(VolumetricGradSdf* tSDF, 75 | const float voxel_size, 76 | const Mat3f& K, 77 | std::string save_path, 78 | OptimizerSettings* settings 79 | 80 | ); 81 | 82 | 83 | // optimize 84 | virtual void init(); 85 | virtual bool alternatingOptimize(bool light, bool albedo, bool distance, bool pose); 86 | 87 | }; 88 | 89 | 90 | #endif // PS_OPTIMIZER_H_ -------------------------------------------------------------------------------- /cpp/include/ps_optimizer/SharpDetector.h: -------------------------------------------------------------------------------- 1 | #ifndef SHARP_DETECTOR_H_ 2 | #define SHARP_DETECTOR_H_ 3 | 4 | #include 5 | 6 | float modifiedLaplacian(const cv::Mat& src); 7 | float varianceOfLaplacian(const cv::Mat& src); 8 | float tenengrad(const cv::Mat& src, int ksize); 9 | float normalizedGraylevelVariance(const cv::Mat& src); 10 | 11 | 12 | bool sharpDetector(const cv::Mat& img, float threshold){ 13 | float measure = modifiedLaplacian(img); 14 | std::cout << "======> the sharpness measure is " << measure << "." << std::endl; 15 | if( measure < threshold) 16 | return false; 17 | return true; 18 | } 19 | 20 | 21 | // OpenCV port of 'LAPM' algorithm (Nayar89) 22 | float modifiedLaplacian(const cv::Mat& src) 23 | { 24 | cv::Mat M = (cv::Mat_(3, 1) << -1, 2, -1); 25 | cv::Mat G = cv::getGaussianKernel(3, -1, CV_32F); 26 | 27 | cv::Mat Lx; 28 | cv::sepFilter2D(src, Lx, CV_32F, M, G); 29 | 30 | cv::Mat Ly; 31 | cv::sepFilter2D(src, Ly, CV_32F, G, M); 32 | 33 | cv::Mat FM = cv::abs(Lx) + cv::abs(Ly); 34 | 35 | float focusMeasure = cv::mean(FM).val[0]; 36 | return focusMeasure; 37 | } 38 | 39 | // OpenCV port of 'LAPV' algorithm (Pech2000) 40 | float varianceOfLaplacian(const cv::Mat& src) 41 | { 42 | cv::Mat lap; 43 | cv::Laplacian(src, lap, CV_32F); 44 | 45 | cv::Scalar mu, sigma; 46 | cv::meanStdDev(lap, mu, sigma); 47 | 48 | float focusMeasure = sigma.val[0]*sigma.val[0]; 49 | return focusMeasure; 50 | } 51 | 52 | // OpenCV port of 'TENG' algorithm (Krotkov86) 53 | float tenengrad(const cv::Mat& src, int ksize) 54 | { 55 | cv::Mat Gx, Gy; 56 | cv::Sobel(src, Gx, CV_32F, 1, 0, ksize); 57 | cv::Sobel(src, Gy, CV_32F, 0, 1, ksize); 58 | 59 | cv::Mat FM = Gx.mul(Gx) + Gy.mul(Gy); 60 | 61 | float focusMeasure = cv::mean(FM).val[0]; 62 | return focusMeasure; 63 | } 64 | 65 | // OpenCV port of 'GLVN' algorithm (Santos97) 66 | float normalizedGraylevelVariance(const cv::Mat& src) 67 | { 68 | cv::Scalar mu, sigma; 69 | cv::meanStdDev(src, mu, sigma); 70 | 71 | float focusMeasure = (sigma.val[0]*sigma.val[0]) / mu.val[0]; 72 | return focusMeasure; 73 | } 74 | 75 | #endif // SHARP_DETECTOR_H_ 76 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/RigidOptimizer.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : RigidOptimizer.h 3 | // Author : Christiane Sommer 4 | // Date : 10/2018 5 | // License : GNU General Public License 6 | // Description : header file for class RigidOptimizer 7 | //============================================================================ 8 | 9 | #ifndef RIGID_OPTIMIZER_H_ 10 | #define RIGID_OPTIMIZER_H_ 11 | 12 | // includes 13 | #include 14 | #include 15 | #include "mat.h" 16 | //class includes 17 | #include "Sdf.h" 18 | 19 | /** 20 | * class declaration 21 | */ 22 | class RigidOptimizer { 23 | 24 | protected: 25 | 26 | // variables 27 | 28 | int num_iterations_; 29 | float conv_threshold_; 30 | float conv_threshold_sq_; 31 | float damping_; 32 | 33 | Sdf* tSDF_; 34 | 35 | SE3 pose_ = SE3(); 36 | 37 | public: 38 | 39 | // constructors / destructor 40 | 41 | RigidOptimizer(Sdf* tSDF) : 42 | num_iterations_(50), 43 | conv_threshold_(1e-3), 44 | conv_threshold_sq_(conv_threshold_ * conv_threshold_), 45 | damping_(1.), 46 | tSDF_(tSDF) 47 | {} 48 | 49 | RigidOptimizer(int num_iterations, float conv_threshold, float damping, Sdf* tSDF) : 50 | num_iterations_(num_iterations), 51 | conv_threshold_(conv_threshold), 52 | conv_threshold_sq_(conv_threshold_ * conv_threshold_), 53 | damping_(damping), 54 | tSDF_(tSDF) 55 | {} 56 | 57 | virtual ~RigidOptimizer() {} 58 | 59 | // member functions 60 | 61 | void set_num_iterations(int num_iterations) { 62 | num_iterations_ = num_iterations; 63 | } 64 | 65 | void set_conv_threshold(float conv_threshold) { 66 | conv_threshold_ = conv_threshold; 67 | conv_threshold_sq_ = conv_threshold_ * conv_threshold_; 68 | } 69 | 70 | void set_damping(float damping) { 71 | damping_ = damping; 72 | } 73 | 74 | void set_pose(SE3 pose) { pose_ = pose; } 75 | 76 | SE3 pose() { 77 | return pose_; 78 | } 79 | 80 | // virtual bool optimize() {} 81 | virtual bool optimize(const cv::Mat &depth, const Mat3f K) = 0; 82 | 83 | // virtual float energy(SE3 pose = SE3()) {} 84 | virtual float energy(const cv::Mat &depth, const Mat3f K, SE3 pose = SE3()) = 0; 85 | 86 | }; 87 | 88 | #endif // RIGID_OPTIMIZER_H_ 89 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/RigidPointOptimizer.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : RigidPointOptimizer.cpp 3 | // Author : Christiane Sommer 4 | // Date : 10/2018 5 | // License : GNU General Public License 6 | // Description : implementation of class RigidPointOptimizer 7 | //============================================================================ 8 | 9 | #include "RigidPointOptimizer.h" 10 | // #include 11 | 12 | bool RigidPointOptimizer::optimize_sampled(const cv::Mat &depth, const Mat3f K, size_t sampling) { 13 | 14 | const float z_min = tSDF_->z_min_, z_max = tSDF_->z_max_; 15 | const int w = depth.cols; 16 | const int h = depth.rows; 17 | const float fx = K(0,0), fy = K(1,1); 18 | const float cx = K(0,2), cy = K(1,2); 19 | const float fx_inv = 1.f / fx; 20 | const float fy_inv = 1.f / fy; 21 | const float* depth_ptr = (const float*)depth.data; 22 | 23 | // float damping = damping_; 24 | 25 | // float E_old = std::numeric_limits::max(); 26 | 27 | for (size_t k=0; k=z_max) continue; 42 | 43 | const float x0 = (float(x) - cx) * fx_inv; 44 | const float y0 = (float(y) - cy) * fy_inv; 45 | Vec3f point(x0*z, y0*z, z); 46 | point = R * point + t; 47 | 48 | float w0 = tSDF_->weights(point); 49 | if (w0>0) { 50 | Vec3f grad_curr; 51 | float phi0 = tSDF_->tsdf(point, &grad_curr); 52 | E += phi0 * phi0; 53 | Vec6f grad_xi; 54 | grad_xi << grad_curr, point.cross(grad_curr); 55 | g += phi0 * grad_xi; 56 | H += grad_xi * grad_xi.transpose(); 57 | ++counter; 58 | } 59 | 60 | } 61 | 62 | E = E/counter; 63 | if (counter == 0) 64 | return false; 65 | 66 | Vec6f xi = damping_ * H.llt().solve(g); // Gauss-Newton 67 | // std::cout << xi.transpose() << std::endl; 68 | if (xi.squaredNorm() < conv_threshold_sq_) { 69 | std::cout << "... Convergence after " << k << " iterations!" << std::endl; 70 | return true; 71 | } 72 | 73 | // update pose 74 | pose_ = SE3::exp(-xi) * pose_; 75 | // E_old = E; 76 | } 77 | 78 | return false; 79 | } 80 | 81 | float RigidPointOptimizer::energy(const cv::Mat &depth, const Mat3f K, SE3 pose) { 82 | 83 | Mat3f R = pose.rotationMatrix(); 84 | Vec3f t = pose.translation(); 85 | 86 | float E = 0; // cost 87 | 88 | size_t counter = 0; 89 | 90 | float z_min = 0., z_max = 1./0.; 91 | int w = depth.cols; 92 | int h = depth.rows; 93 | float fx = K(0,0); 94 | float fy = K(1,1); 95 | float cx = K(0,2); 96 | float cy = K(1,2); 97 | float fx_inv = 1.f / fx; 98 | float fy_inv = 1.f / fy; 99 | const float* depth_ptr = (const float*)depth.data; 100 | 101 | int sampling = 1; 102 | 103 | for (int y=0; yweights(point); 114 | if (w0>0) { 115 | float phi0 = tSDF_->tsdf(point); 116 | E += phi0*phi0; 117 | ++counter; 118 | } 119 | 120 | } 121 | 122 | return .5*E; 123 | 124 | } 125 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/RigidPointOptimizer.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : RigidPointOptimizer.h 3 | // Author : Christiane Sommer 4 | // Date : 11/2018 5 | // License : GNU General Public License 6 | // Description : header file for class RigidPointOptimizer 7 | //============================================================================ 8 | 9 | #ifndef RIGID_POINT_OPTIMIZER_H_ 10 | #define RIGID_POINT_OPTIMIZER_H_ 11 | 12 | // includes 13 | #include 14 | //class includes 15 | #include "RigidOptimizer.h" 16 | 17 | /** 18 | * class declaration 19 | */ 20 | class RigidPointOptimizer : public RigidOptimizer { 21 | 22 | public: 23 | 24 | // constructors / destructor 25 | 26 | RigidPointOptimizer(Sdf* tSDF) : 27 | RigidOptimizer(tSDF) 28 | {} 29 | 30 | RigidPointOptimizer(int num_iterations, float conv_threshold, float damping, Sdf* tSDF) : 31 | RigidOptimizer(num_iterations, conv_threshold, damping, tSDF) 32 | {} 33 | 34 | ~RigidPointOptimizer() {} 35 | 36 | bool optimize_sampled(const cv::Mat &depth, const Mat3f K, size_t sampling); //, size_t num_iterations = num_iterations_); 37 | 38 | // member functions 39 | 40 | bool optimize(const cv::Mat &depth, const Mat3f K) { 41 | // tSDF_->increase_counter(); 42 | return optimize_sampled(depth, K, 1); 43 | } 44 | 45 | float energy(const cv::Mat &depth, const Mat3f K, SE3 pose = SE3()); 46 | 47 | }; 48 | 49 | #endif // RIGID_POINT_OPTIMIZER_H_ 50 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/RigidPointOptimizerOmp.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : RigidPointOptimizerOmp.cpp 3 | // Author : Christiane Sommer 4 | // Date : 01/2020 5 | // License : GNU General Public License 6 | // Description : implementation of class RigidPointOptimizer, with OpenMP 7 | //============================================================================ 8 | 9 | #include "RigidPointOptimizer.h" 10 | #include 11 | 12 | #pragma omp declare reduction (vecsum : Eigen::Vector : omp_out += omp_in) initializer(omp_priv = Eigen::Vector::Zero()) 13 | #pragma omp declare reduction (matsum : Eigen::Matrix : omp_out += omp_in) initializer(omp_priv = Eigen::Matrix::Zero()) 14 | 15 | bool RigidPointOptimizer::optimize_sampled(const cv::Mat &depth, const Mat3f K, size_t sampling) { 16 | 17 | const float z_min = 0., z_max = 3.5; 18 | const int w = depth.cols; 19 | const int h = depth.rows; 20 | const float fx = K(0,0), fy = K(1,1); 21 | const float cx = K(0,2), cy = K(1,2); 22 | const float fx_inv = 1.f / fx; 23 | const float fy_inv = 1.f / fy; 24 | const float* depth_ptr = (const float*)depth.data; 25 | 26 | for (size_t k=0; k=z_max) continue; 43 | 44 | const float x0 = (float(x) - cx) * fx_inv; 45 | const float y0 = (float(y) - cy) * fy_inv; 46 | Vec3f point(x0*z, y0*z, z); 47 | point = R * point + t; 48 | 49 | float w0 = tSDF_->weights(point); 50 | if (w0>0) { 51 | Vec3f grad_curr; 52 | float phi0 = tSDF_->tsdf(point, &grad_curr); 53 | E += phi0 * phi0; 54 | Vec6f grad_xi; 55 | grad_xi << grad_curr, point.cross(grad_curr); 56 | g += phi0 * grad_xi; 57 | H += grad_xi * grad_xi.transpose(); 58 | ++counter; 59 | } 60 | } 61 | 62 | Vec6f xi = Vec6f::Zero(); 63 | if (counter == 0) 64 | return false; 65 | 66 | xi = damping_ * H.llt().solve(g); // Gauss-Newton 67 | 68 | if (xi.squaredNorm() < conv_threshold_sq_) { 69 | std::cout << "... Convergence after " << k << " iterations!" << std::endl; 70 | return true; 71 | } 72 | 73 | // update pose 74 | pose_ = SE3::exp(-xi) * pose_; 75 | } 76 | 77 | return false; 78 | } 79 | 80 | float RigidPointOptimizer::energy(const cv::Mat &depth, const Mat3f K, SE3 pose) { 81 | 82 | Mat3f R = pose.rotationMatrix(); 83 | Vec3f t = pose.translation(); 84 | 85 | float E = 0; // cost 86 | 87 | size_t counter = 0; 88 | 89 | float z_min = 0., z_max = 1./0.; 90 | int w = depth.cols; 91 | int h = depth.rows; 92 | float fx = K(0,0); 93 | float fy = K(1,1); 94 | float cx = K(0,2); 95 | float cy = K(1,2); 96 | float fx_inv = 1.f / fx; 97 | float fy_inv = 1.f / fy; 98 | const float* depth_ptr = (const float*)depth.data; 99 | 100 | int sampling = 1; 101 | 102 | for (int y=0; yweights(point); 113 | if (w0>0) { 114 | float phi0 = tSDF_->tsdf(point); 115 | E += phi0*phi0; 116 | ++counter; 117 | } 118 | 119 | } 120 | 121 | return .5*E; 122 | 123 | } 124 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/Sdf.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : Sdf.h 3 | // Author : Christiane Sommer 4 | // Date : 09/2019 5 | // License : GNU General Public License 6 | // Description : header file for class Sdf 7 | //============================================================================ 8 | 9 | #ifndef SDF_H_ 10 | #define SDF_H_ 11 | 12 | // includes 13 | #include 14 | 15 | namespace cv { 16 | template 17 | class NormalEstimator; 18 | // class NormalSetup; 19 | } 20 | 21 | /** 22 | * class declaration 23 | */ 24 | class Sdf { 25 | 26 | protected: 27 | 28 | // friends 29 | 30 | friend class RigidPointOptimizer; 31 | friend class RigidSdfOptimizer; 32 | 33 | // variables 34 | 35 | float T_; // truncation distance in meters 36 | float inv_T_; 37 | size_t counter_; 38 | 39 | float z_max_ = 10.0; 40 | float z_min_ = 0.5; 41 | 42 | // methods 43 | 44 | float truncate(float sdf) const { 45 | return std::max(-T_, std::min(T_, sdf)); 46 | } 47 | 48 | float weight(float sdf) const { 49 | float w = 0.f; 50 | float epsilon = 0.5*T_; 51 | 52 | // if (sdf<=0.) { 53 | // w = 1.f; 54 | // } 55 | // else if (sdf<=T_) { 56 | // w = 1.f - sdf*inv_T_; 57 | // } 58 | if (sdf>= 0.) { 59 | w = 1.f; 60 | } 61 | else if (sdf>= -T_) { 62 | w = 1.f + sdf*inv_T_; 63 | } 64 | 65 | return w; 66 | } 67 | 68 | 69 | 70 | 71 | // void init(); 72 | 73 | public: 74 | 75 | // constructors / destructor 76 | 77 | Sdf() : 78 | T_(0.05), 79 | inv_T_(1./T_), 80 | counter_(0) 81 | {} 82 | 83 | Sdf(float T) : 84 | T_(T), 85 | inv_T_(1./T_), 86 | counter_(0) 87 | {} 88 | 89 | virtual ~Sdf() {} 90 | 91 | // methods 92 | 93 | virtual float tsdf(Vec3f point, Vec3f* grad_ptr = nullptr) const = 0; 94 | 95 | virtual float weights(Vec3f point) const = 0; 96 | 97 | virtual void update(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst = nullptr) = 0; 98 | 99 | 100 | virtual void setup(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, cv::NormalEstimator* NEst = nullptr) { 101 | update(color, depth, K, SE3(), NEst); 102 | } 103 | 104 | 105 | void set_zmin(float z_min) { 106 | z_min_ = z_min; 107 | } 108 | 109 | void set_zmax(float z_max) { 110 | z_max_ = z_max; 111 | } 112 | 113 | void increase_counter() { 114 | ++counter_; 115 | } 116 | 117 | // visualization / debugging 118 | 119 | virtual bool extract_mesh(std::string filename = "mesh.ply") { 120 | return false; 121 | } 122 | 123 | virtual bool extract_pc(std::string filename = "cloud.ply") { 124 | return false; 125 | } 126 | 127 | virtual bool saveSDF(std::string filename = " sdf.sdf") { 128 | return false; 129 | } 130 | 131 | 132 | virtual bool save_normal(const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst, std::string filename){ 133 | return false; 134 | } 135 | 136 | 137 | virtual void write() {} 138 | 139 | virtual void check_vis_map() {} ; 140 | 141 | }; 142 | 143 | #endif // SDF_H_ 144 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/Sdfvoxel.h: -------------------------------------------------------------------------------- 1 | #ifndef SDFVOXEL_H_ 2 | #define SDFVOXEL_H_ 3 | 4 | #include "mat.h" 5 | 6 | struct SdfVoxel { 7 | float dist = 0; 8 | Vec3f grad = Vec3f::Zero(); 9 | float weight = 0; 10 | float r = 1.0; 11 | float g = 1.0; 12 | float b = 1.0; 13 | }; 14 | 15 | #endif //SDFVOXEL_H -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/TrackingSettings.h: -------------------------------------------------------------------------------- 1 | #ifndef TRACKING_SETTINGS_ 2 | #define TRACKING_SETTINGS_ 3 | 4 | #include 5 | #include 6 | 7 | // parse dataset type 8 | enum class DataType { 9 | TUM_RGBD, 10 | SYNTH, 11 | MULTIVIEW 12 | }; 13 | 14 | struct TrackingSettings { 15 | std::string input_; 16 | std::string output_; 17 | std::string pose_file_; 18 | DataType datatype_; 19 | size_t first_; 20 | size_t last_; 21 | float voxel_size_; 22 | float truncation_factor_; 23 | float zmin_; 24 | float zmax_; 25 | float sharpness_threshold_; 26 | TrackingSettings(std::string& input, std::string& output, DataType datatype): 27 | input_(input), 28 | output_(output), 29 | pose_file_("pose.txt"), 30 | datatype_(datatype), 31 | first_(0), 32 | last_(std::numeric_limits::max()), 33 | voxel_size_(0.02), 34 | truncation_factor_(5), 35 | zmin_(0.5), 36 | zmax_(3.5), 37 | sharpness_threshold_(0.5) 38 | {} 39 | 40 | }; 41 | 42 | #endif // TRACKING_SETTINGS_ -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/VolumetricGradSdf.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : VolumetricGradSdf.cpp 3 | // Author : Christiane Sommer 4 | // Modified : Lu Sang 5 | // Date : 09/2021, origin on 10/2018 6 | // License : GNU General Public License 7 | // Description : implementation of class VolumetricGradSdf 8 | //============================================================================ 9 | 10 | #include "VolumetricGradSdf.h" 11 | #include "normals/NormalEstimator.h" 12 | #include "mesh/MarchingCubes.h" 13 | 14 | void VolumetricGradSdf::init() { 15 | 16 | SdfVoxel v; 17 | v.dist = T_; 18 | v.grad = Vec3f::Zero(); 19 | v.weight = 0; 20 | v.r = 0.0; 21 | v.g = 0.0; 22 | v.b = 0.0; 23 | 24 | // allcoate grids 25 | tsdf_ = new SdfVoxel[num_voxels_]; 26 | // initialize grids 27 | std::fill_n(tsdf_, num_voxels_, v); 28 | // initialize visibility map 29 | // vis_.resize(num_voxels_); 30 | 31 | std::vector vis; 32 | vis_ = new std::vector[num_voxels_]; 33 | std::fill_n(vis_, num_voxels_, vis); 34 | 35 | std::cout << "Number of voxels: " << num_voxels_ << std::endl 36 | << "Memory used: " << num_voxels_ * 5. * sizeof(float) / (1024.*1024.) << "MB" << std::endl; 37 | 38 | } 39 | 40 | VolumetricGradSdf::~VolumetricGradSdf() { 41 | 42 | // free grid memory 43 | delete[] tsdf_; 44 | delete[] vis_; 45 | // overwrite pointer 46 | tsdf_ = nullptr; 47 | vis_ = nullptr; 48 | 49 | } 50 | 51 | void VolumetricGradSdf::update(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst) { 52 | 53 | const float fx = K(0,0), fy = K(1,1); 54 | const float cx = K(0,2), cy = K(1,2); 55 | 56 | const float z_min = 0., z_max = 1./0.; 57 | 58 | // size_t lin_index = 0; 59 | 60 | cv::Mat nx, ny, nz, med_depth; 61 | cv::medianBlur(depth, med_depth, 5); // median filtering 62 | 63 | if (!NEst) { // TODO: implement on-the-go normal estimation? 64 | std::cerr << "No normal estimation possible - cannot update SDF volume!" << std::endl; 65 | return; 66 | } 67 | 68 | NEst->compute(depth, nx, ny, nz); 69 | cv::Mat* n_sq_inv_ptr = NEst->n_sq_inv_ptr(); 70 | 71 | const Mat3f R = pose.rotationMatrix(); 72 | const Mat3f Rt = R.transpose(); 73 | const Vec3f t = pose.translation(); 74 | 75 | size_t vis_size; 76 | 77 | 78 | for (size_t k=0; k(cx + fx * point[0] / point[2] + 0.5); // +0.5 is needed for proper rounding 88 | const int m = static_cast(cy + fy * point[1] / point[2] + 0.5); 89 | 90 | 91 | if ((n<0) || (n>=depth.cols) || (m<0) || (m>=depth.rows)) 92 | continue; 93 | 94 | const float z = depth.at(m, n); // no/nn interpolation 95 | const cv::Vec3f z_color = color.at(m, n); 96 | 97 | if (z <= z_min_ || z >= z_max_ )//|| z != med_depth.at(m,n)) 98 | continue; 99 | 100 | // const float sdf = point[2] - z; // point-to-point, negative outside, positive inside 101 | const float sdf = z - point[2]; //negative inside, positive outside 102 | const float w = weight(sdf); 103 | if (w == 0) 104 | continue; 105 | 106 | const float nx_i = nx.at(m, n); 107 | const float ny_i = ny.at(m, n); 108 | const float nz_i = nz.at(m, n); 109 | 110 | Vec3f normal(nx_i, ny_i, nz_i); 111 | if (normal.squaredNorm() < .1) // invalid normal 112 | continue; 113 | 114 | Vec3f xy_hom = (1. / point[2]) * point; 115 | if (normal.dot(xy_hom) * normal.dot(xy_hom) * n_sq_inv_ptr->at(m, n) < .25 * .25) // normal direction too far from viewing ray direction (>75.5°) 116 | continue; 117 | 118 | SdfVoxel& v = tsdf_[lin_index]; 119 | std::vector& vis = vis_[lin_index]; 120 | 121 | v.weight += w; 122 | v.dist += (truncate(sdf) - v.dist) * w / v.weight; 123 | v.grad -= w * R * Vec3f(nx_i, ny_i, nz_i); // normals are inward-pointing! 124 | 125 | v.r += (z_color[2] - v.r) * w / v.weight; 126 | v.g += (z_color[1] - v.g) * w / v.weight; 127 | v.b += (z_color[0] - v.b) * w / v.weight; 128 | 129 | vis.resize(counter_); 130 | vis.push_back(true); 131 | 132 | vis_size = vis.size(); 133 | 134 | } 135 | // ++lin_index; 136 | std::cout << " --------------------------------------------------current counter " << counter_ << std::endl; 137 | std::cout << " --------------------------------------------------current vis size " << vis_size<* NEst, std::string filename) 141 | { 142 | //DEBUG 143 | // std::ofstream normal_file; 144 | // normal_file.open((filename + ".txt").c_str()); 145 | 146 | // if(!normal_file.is_open()){ 147 | // std::cout << " can't save normal file!" << std::endl; 148 | // return false; 149 | // } 150 | 151 | cv::Mat output(cv::Size(depth.cols, depth.rows), CV_32FC3, cv::Scalar(.0, .0, .0)); 152 | 153 | 154 | const float fx = K(0,0), fy = K(1,1); 155 | const float cx = K(0,2), cy = K(1,2); 156 | 157 | const float z_min = 0., z_max = 1./0.; 158 | 159 | // size_t lin_index = 0; 160 | 161 | cv::Mat nx, ny, nz, med_depth; 162 | cv::medianBlur(depth, med_depth, 5); // median filtering 163 | 164 | if (!NEst) { // TODO: implement on-the-go normal estimation? 165 | std::cerr << "No normal estimation possible - cannot update SDF volume!" << std::endl; 166 | return false; 167 | } 168 | 169 | NEst->compute(depth, nx, ny, nz); 170 | cv::Mat* n_sq_inv_ptr = NEst->n_sq_inv_ptr(); 171 | 172 | const Mat3f R = pose.rotationMatrix(); 173 | const Mat3f Rt = R.transpose(); 174 | const Vec3f t = pose.translation(); 175 | 176 | 177 | for (size_t k=0; k(cx + fx * point[0] / point[2] + 0.5); // +0.5 is needed for proper rounding 187 | const int m = static_cast(cy + fy * point[1] / point[2] + 0.5); 188 | 189 | 190 | if ((n<0) || (n>=depth.cols) || (m<0) || (m>=depth.rows)) 191 | continue; 192 | 193 | const float z = depth.at(m, n); // no/nn interpolation 194 | 195 | if (z <= z_min_ || z >= z_max_ )//|| z != med_depth.at(m,n)) 196 | continue; 197 | 198 | // const float sdf = point[2] - z; // point-to-point, negative outside, positive inside 199 | const float sdf = z - point[2]; //negative inside, positive outside 200 | const float w = weight(sdf); 201 | if (w == 0) 202 | continue; 203 | 204 | const float nx_i = nx.at(m, n); 205 | const float ny_i = ny.at(m, n); 206 | const float nz_i = nz.at(m, n); 207 | 208 | // Vec3f N(nx_i, ny_i, nz_i); 209 | // N = N.normalized(); 210 | // // N = R.transpose()*N; 211 | 212 | cv::Vec3f& color = output.at(m, n); 213 | color[0] = (-nx_i + 1.0f)/2.0f*255.0f; 214 | color[1] = (-ny_i + 1.0f)/2.0f*255.0f; 215 | color[2] = (-nz_i + 1.0f)/2.0f*255.0f; 216 | // color[0] = (-N[0] + 1.0f)/2.0f*255.0f; 217 | // color[1] = (-N[1] + 1.0f)/2.0f*255.0f; 218 | // color[2] = (-N[2] + 1.0f)/2.0f*255.0f; 219 | 220 | // normal_file << n << " " << m << " " << -N[0] << " " << -N[1] << " " << -N[2] << "\n"; 221 | } 222 | 223 | // normal_file.close(); 224 | cv::Mat image; 225 | output.convertTo(image, CV_8UC3); 226 | 227 | cv::imwrite(filename + "normal_map.png", image); 228 | std::cout << "==== DEBUG: normal map for frame saved!" << std::endl; 229 | 230 | return true; 231 | 232 | } 233 | 234 | bool VolumetricGradSdf::extract_mesh(std::string filename) { 235 | 236 | const int pos_inf = std::numeric_limits::max(); 237 | const int neg_inf = std::numeric_limits::min(); 238 | int xmin, xmax, ymin, ymax, zmin, zmax; 239 | xmin = pos_inf; 240 | xmax = neg_inf; 241 | ymin = pos_inf; 242 | ymax = neg_inf; 243 | zmin = pos_inf; 244 | zmax = neg_inf; 245 | 246 | size_t lin_index = 0; 247 | for (int k=0; k std::sqrt(3)*voxel_size_) { 255 | ++lin_index; 256 | continue; 257 | } 258 | 259 | if (i < xmin) xmin = i; 260 | if (i > xmax) xmax = i; 261 | if (j < ymin) ymin = j; 262 | if (j > ymax) ymax = j; 263 | if (k < zmin) zmin = k; 264 | if (k > zmax) zmax = k; 265 | ++lin_index; 266 | } 267 | std::cout << "Size limits:" << std::endl 268 | << xmin << "\t" << xmax << std::endl 269 | << ymin << "\t" << ymax << std::endl 270 | << zmin << "\t" << zmax << std::endl; 271 | 272 | // create input that can handle MarchingCubes class 273 | const Vec3i dim(xmax - xmin + 1, ymax - ymin + 1, zmax - zmin + 1); 274 | const size_t num_voxels = dim[0] * dim[1] * dim[2]; 275 | 276 | float* dist = new float[num_voxels]; 277 | float* weights = new float[num_voxels]; 278 | unsigned char* red = new unsigned char[num_voxels]; 279 | unsigned char* green = new unsigned char[num_voxels]; 280 | unsigned char* blue = new unsigned char[num_voxels]; 281 | 282 | // MarchingCubes mc; 283 | // mc.set_resolution(dim[0], dim[1], dim[2]); 284 | // mc.init_all(); 285 | 286 | size_t pos = 0; 287 | for (int k=zmin; k<=zmax; ++k) for (int j=ymin; j<=ymax; ++j) for (int i=xmin; i<=xmax; ++i) { 288 | Vec3i idx(i,j,k); 289 | size_t lin_idx = idx2line(idx); 290 | dist[pos] = -tsdf_[lin_idx].dist; 291 | weights[pos] = tsdf_[lin_idx].weight; 292 | red[pos] = int(255*tsdf_[lin_idx].r); 293 | green[pos] = int(255*tsdf_[lin_idx].g); 294 | blue[pos] = int(255*tsdf_[lin_idx].b); 295 | pos++; 296 | 297 | // mc.set_data(tsdf_[lin_idx].dist, i ,j ,k); 298 | } 299 | 300 | // mc.run(); 301 | // mc.clean_temps(); 302 | 303 | // mc.writePLY(filename.c_str()); 304 | 305 | // mc.clean_all(); 306 | // call marching cubes 307 | 308 | MarchingCubes mc(dim, voxel_size_ * dim.template cast(), -voxel_size_ * Vec3f(xmin, ymin, zmin)); 309 | mc.computeIsoSurface(dist, weights, red, green, blue); 310 | bool success = mc.savePly(filename); 311 | 312 | // delete temporary arrays 313 | delete[] dist; 314 | delete[] weights; 315 | 316 | return success; 317 | 318 | } 319 | 320 | bool VolumetricGradSdf::extract_pc(std::string filename) { 321 | 322 | std::vector > points_normals_colors; 323 | 324 | size_t lin_index = 0; 325 | 326 | for (size_t k=0; k vis = vis_[lin_index]; 330 | auto sum = std::count(vis.begin(), vis.end(), true); 331 | 332 | if ((std::fabs(v.dist) > std::sqrt(3.0)*voxel_size_) || (sum < 1)) { 333 | ++lin_index; 334 | continue; 335 | } 336 | 337 | Vec3f g = v.grad.normalized(); 338 | Vec3f d = v.dist * g; 339 | float voxel_size_2 = .5 * voxel_size_; 340 | // if (std::fabs(d[0]) < voxel_size_2 && std::fabs(d[1]) < voxel_size_2 && std::fabs(d[2]) < voxel_size_2) { 341 | Eigen::Matrix pnc; 342 | pnc.segment<3>(0) = vox2float(i,j,k) - d; 343 | pnc.segment<3>(3) = g; 344 | pnc.segment<3>(6) = Vec3f(v.r, v.g, v.b); 345 | points_normals_colors.push_back(pnc); 346 | // } 347 | ++lin_index; 348 | } 349 | 350 | std::ofstream plyFile; 351 | plyFile.open(filename.c_str()); 352 | if (!plyFile.is_open()) 353 | return false; 354 | 355 | plyFile << "ply" << std::endl; 356 | plyFile << "format ascii 1.0" << std::endl; 357 | plyFile << "element vertex " << points_normals_colors.size() << std::endl; 358 | plyFile << "property float x" << std::endl; 359 | plyFile << "property float y" << std::endl; 360 | plyFile << "property float z" << std::endl; 361 | plyFile << "property float nx" << std::endl; 362 | plyFile << "property float ny" << std::endl; 363 | plyFile << "property float nz" << std::endl; 364 | plyFile << "property uchar red" << std::endl; 365 | plyFile << "property uchar green" << std::endl; 366 | plyFile << "property uchar blue" << std::endl; 367 | plyFile << "end_header" << std::endl; 368 | 369 | for (const Eigen::Matrix& p : points_normals_colors) { 370 | 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; 371 | } 372 | 373 | plyFile.close(); 374 | std::cout << "total " << points_normals_colors.size() << "surface points are written." << std::endl; 375 | return true; 376 | } 377 | 378 | // this code is to save sdf and compare with the SDFGen generated GT sdf from mesh obj 379 | bool VolumetricGradSdf::saveSDF(std::string filename){ 380 | const int pos_inf = std::numeric_limits::max(); 381 | const int neg_inf = std::numeric_limits::min(); 382 | int xmin, xmax, ymin, ymax, zmin, zmax; 383 | xmin = pos_inf; 384 | xmax = neg_inf; 385 | ymin = pos_inf; 386 | ymax = neg_inf; 387 | zmin = pos_inf; 388 | zmax = neg_inf; 389 | 390 | size_t lin_index = 0; 391 | for (int k=0; k std::sqrt(3)*voxel_size_) { 395 | ++lin_index; 396 | continue; 397 | } 398 | 399 | if (i < xmin) xmin = i; 400 | if (i > xmax) xmax = i; 401 | if (j < ymin) ymin = j; 402 | if (j > ymax) ymax = j; 403 | if (k < zmin) zmin = k; 404 | if (k > zmax) zmax = k; 405 | ++lin_index; 406 | } 407 | // std::cout << "Size limits:" << std::endl 408 | // << xmin << "\t" << xmax << std::endl 409 | // << ymin << "\t" << ymax << std::endl 410 | // << zmin << "\t" << zmax << std::endl; 411 | 412 | // create input that can handle MarchingCubes class 413 | const Vec3i dim(xmax - xmin + 1, ymax - ymin + 1, zmax - zmin + 1); 414 | const size_t num_voxels = dim[0] * dim[1] * dim[2]; 415 | 416 | std::ofstream sdfFile; 417 | sdfFile.open(filename.c_str()); 418 | if (!sdfFile.is_open()) 419 | return false; 420 | 421 | sdfFile << dim[0] << " " << dim[1] << " " << dim[2] << "\n"; 422 | //compute bounding box 423 | Vec3f origin = - voxel_size_*Vec3i(xmin, ymin, zmin).cast(); 424 | 425 | Vec3f bottom = Vec3i(xmin, ymin, zmin).cast()*voxel_size_; 426 | Vec3f up = Vec3i(xmax, ymax, zmax).cast()*voxel_size_ ; 427 | 428 | std::cout << "Bounding box size: (" << bottom.transpose() << ") to (" << up.transpose() << ") with dimensions " << dim.transpose() << std::endl; 429 | 430 | sdfFile << bottom[0] << " " << bottom[1] << " " << bottom[2] << "\n"; 431 | sdfFile << voxel_size_ << "\n"; 432 | 433 | for (int k=zmin; k<=zmax; ++k) for (int j=ymin; j<=ymax; ++j) for (int i=xmin; i<=xmax; ++i) { 434 | Vec3i idx(i,j,k); 435 | size_t lin_idx = idx2line(idx); 436 | sdfFile << -tsdf_[lin_idx].dist << "\n"; 437 | } 438 | 439 | sdfFile.close(); 440 | return true; 441 | 442 | } 443 | 444 | 445 | void VolumetricGradSdf::check_vis_map() 446 | { 447 | int count = 0; 448 | for (size_t lin_idx = 0; lin_idx < num_voxels_; lin_idx++){ 449 | std::vector& vis = vis_[lin_idx]; 450 | SdfVoxel& v = tsdf_[lin_idx]; 451 | if(std::fabs(v.dist) < std::sqrt(3.)*voxel_size_){ 452 | 453 | if(count%100 == 0){ 454 | std::cout << "voxel " << lin_idx << ":\t" << std::endl; 455 | for(size_t frame = 0; frame < vis.size(); frame++){ 456 | std::cout << " frame " << frame << " vis: " << vis[frame] <<" "; 457 | } 458 | std::cout << std::endl; 459 | 460 | } 461 | count++; 462 | } 463 | } 464 | 465 | } 466 | 467 | 468 | // calculate the subsampled indx and grad, 469 | Vec8f VolumetricGradSdf::subsample(const SdfVoxel& v) 470 | { 471 | Vec8f d; 472 | Vec3f grad = v.grad.normalized(); 473 | float dist = v.dist; 474 | 475 | const float voxel_size_4 = 0.25 * voxel_size_; 476 | d[0] = dist + voxel_size_4 * (-grad[0] - grad[1] - grad[2]); 477 | d[1] = dist + voxel_size_4 * ( grad[0] - grad[1] - grad[2]); 478 | d[2] = dist + voxel_size_4 * (-grad[0] + grad[1] - grad[2]); 479 | d[3] = dist + voxel_size_4 * ( grad[0] + grad[1] - grad[2]); 480 | d[4] = dist + voxel_size_4 * (-grad[0] - grad[1] + grad[2]); 481 | d[5] = dist + voxel_size_4 * ( grad[0] - grad[1] + grad[2]); 482 | d[6] = dist + voxel_size_4 * (-grad[0] + grad[1] + grad[2]); 483 | d[7] = dist + voxel_size_4 * ( grad[0] + grad[1] + grad[2]); 484 | 485 | // d[0] = dist + voxel_size_4 * (-grad[0] - grad[1] - grad[2] - 1); 486 | // d[1] = dist + voxel_size_4 * ( grad[0] - grad[1] - grad[2] - 1); 487 | // d[2] = dist + voxel_size_4 * (-grad[0] + grad[1] - grad[2] - 1); 488 | // d[3] = dist + voxel_size_4 * ( grad[0] + grad[1] - grad[2] - 1); 489 | // d[4] = dist + voxel_size_4 * (-grad[0] - grad[1] + grad[2] - 1); 490 | // d[5] = dist + voxel_size_4 * ( grad[0] - grad[1] + grad[2] - 1); 491 | // d[6] = dist + voxel_size_4 * (-grad[0] + grad[1] + grad[2] - 1); 492 | // d[7] = dist + voxel_size_4 * ( grad[0] + grad[1] + grad[2] - 1); 493 | return d; 494 | } -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/VolumetricGradSdf.h: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : VolumetricGradSdf.h 3 | // Author : Christiane Sommer 4 | // Date : 09/2019 5 | // License : GNU General Public License 6 | // Description : header file for class VolumetricGradSdf 7 | //============================================================================ 8 | 9 | #ifndef VOLUMETRIC_GRAD_SDF_H_ 10 | #define VOLUMETRIC_GRAD_SDF_H_ 11 | 12 | // includes 13 | #include 14 | //#include 15 | // class includes 16 | #include "VoxelGrid.h" 17 | #include "Sdfvoxel.h" 18 | #include "Sdf.h" 19 | 20 | /** 21 | * class declaration 22 | */ 23 | class VolumetricGradSdf : public VoxelGrid, public Sdf { 24 | 25 | // friend class 26 | friend class Optimizer; 27 | friend class PsOptimizer; 28 | friend class GsOptimizer; 29 | friend class LedOptimizer; 30 | friend class ImgRender; 31 | 32 | // struct SdfVoxel { 33 | // float dist = 0; 34 | // Vec3f grad = Vec3f::Zero(); 35 | // float weight = 0; 36 | // }; 37 | 38 | // variables 39 | 40 | SdfVoxel* tsdf_; 41 | // std::vector>* vis_; 42 | std::vector* vis_; 43 | 44 | void init(); 45 | void subsampling(); 46 | 47 | public: 48 | 49 | // constructors / destructor 50 | 51 | VolumetricGradSdf() : 52 | VoxelGrid(), 53 | Sdf() 54 | { 55 | init(); 56 | } 57 | 58 | VolumetricGradSdf(Vec3i grid_dim, float voxel_size, Vec3f shift) : 59 | VoxelGrid(grid_dim, voxel_size, shift), 60 | Sdf() 61 | { 62 | init(); 63 | } 64 | 65 | VolumetricGradSdf(Vec3i grid_dim, float voxel_size, Vec3f shift, float T) : 66 | VoxelGrid(grid_dim, voxel_size, shift), 67 | Sdf(T) 68 | { 69 | init(); 70 | } 71 | 72 | ~VolumetricGradSdf(); 73 | 74 | // methods 75 | 76 | virtual float tsdf(Vec3f point, Vec3f* grad_ptr) const { 77 | int I = nearest_index(point); 78 | if (I < 0) { 79 | if (grad_ptr) 80 | (*grad_ptr) = Vec3f::Zero(); 81 | return T_; 82 | } 83 | const SdfVoxel& v = tsdf_[I]; 84 | if (grad_ptr) 85 | (*grad_ptr) = v.grad.normalized(); 86 | return v.dist + v.grad.normalized().dot(voxel2world(world2voxel(point)) - point); 87 | } 88 | 89 | virtual float weights(Vec3f point) const { 90 | int I = nearest_index(point); 91 | if (I < 0) return 0; 92 | return tsdf_[I].weight; 93 | } 94 | 95 | virtual void update(const cv::Mat &color, const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst); 96 | 97 | SdfVoxel getVoxel(Vec3i& idx){ 98 | return tsdf_[idx2line(idx)]; 99 | } 100 | 101 | SdfVoxel getVoxel(const int i, const int j, const int k){ 102 | Vec3i idx(i,j,k); 103 | return tsdf_[idx2line(idx)]; 104 | } 105 | 106 | // visualization / debugging 107 | 108 | virtual bool extract_mesh(std::string filename); 109 | 110 | virtual bool extract_pc(std::string filename); 111 | 112 | virtual bool saveSDF(std::string filename); 113 | 114 | virtual void check_vis_map(); 115 | 116 | virtual bool save_normal(const cv::Mat &depth, const Mat3f K, const SE3 &pose, cv::NormalEstimator* NEst, std::string filename); 117 | 118 | Vec8f subsample(const SdfVoxel& v); 119 | 120 | }; 121 | 122 | #endif // VOLUMETRIC_GRAD_SDF_H_ 123 | -------------------------------------------------------------------------------- /cpp/include/sdf_tracker/VoxelGrid.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================ 2 | // Name : VoxelGrid.cpp 3 | // Author : Christiane Sommer 4 | // Date : 10/2018 5 | // License : GNU General Public License 6 | // Description : implementation of class VoxelGrid 7 | //============================================================================ 8 | 9 | #include "VoxelGrid.h" 10 | 11 | float VoxelGrid::interp3(const float* grid, Vec3f point, float extrap) const { 12 | 13 | Vec3f float_idx = world2voxelf(point); 14 | const float i = float_idx[0], j = float_idx[1], k = float_idx[2]; 15 | 16 | // remove out of volume cases 17 | if (i<=0 || j<=0 || k<=0 || i>=(grid_dim_[0]-1) || j>=(grid_dim_[1]-1) || k>=(grid_dim_[2]-1)) 18 | return extrap; 19 | 20 | const int im = static_cast(i); 21 | const int jm = static_cast(j); 22 | const int km = static_cast(k); 23 | const float dx = i-im; 24 | const float dy = j-jm; 25 | const float dz = k-km; 26 | 27 | int I = im + jm * grid_dim_[0] + km * grid_dim_[0] * grid_dim_[1]; // linear index i-dx, j-dy, k-dz 28 | const float V_imjmkm = grid[I]; 29 | I += 1; // i+1-dx , j-dy , k-dz 30 | const float V_ipjmkm = grid[I]; 31 | I += grid_dim_[0]; // i+1-dx , j+1-dy , k-dz 32 | const float V_ipjpkm = grid[I]; 33 | I -= 1; // i-dx , j+1-dy , k-dz 34 | const float V_imjpkm = grid[I]; 35 | I += grid_dim_[0]*grid_dim_[1]; // i-dx , j+1-dy , k+1-dz 36 | const float V_imjpkp = grid[I]; 37 | I -= grid_dim_[0]; // i-dx , j-dy , k+1-dz 38 | const float V_imjmkp = grid[I]; 39 | I += 1; // i+1-dx , j-dy , k+1-dz 40 | const float V_ipjmkp = grid[I]; 41 | I += grid_dim_[0]; // i+1-dx , j+1-dy , k+1-dz 42 | const float V_ipjpkp = grid[I]; 43 | 44 | // interpolate in z-direction 45 | const float V_imjm = (1-dz) * V_imjmkm + dz * V_imjmkp; 46 | const float V_imjp = (1-dz) * V_imjpkm + dz * V_imjpkp; 47 | const float V_ipjm = (1-dz) * V_ipjmkm + dz * V_ipjmkp; 48 | const float V_ipjp = (1-dz) * V_ipjpkm + dz * V_ipjpkp; 49 | // next level: y-direction 50 | const float V_im = (1-dy) * V_imjm + dy * V_imjp; 51 | const float V_ip = (1-dy) * V_ipjm + dy * V_ipjp; 52 | // final interpolated value: x-direction 53 | return (1-dx) * V_im + dx * V_ip; 54 | 55 | } 56 | 57 | int VoxelGrid::nearest_index(Vec3f point) const { 58 | 59 | Vec3f float_idx = world2voxelf(point); 60 | const float i = float_idx[0], j = float_idx[1], k = float_idx[2]; 61 | 62 | // remove out of volume cases 63 | if (i<=0 || j<=0 || k<=0 || i>=(grid_dim_[0]-1) || j>=(grid_dim_[1]-1) || k>=(grid_dim_[2]-1)) 64 | return -1; 65 | 66 | const int im = static_cast(i + 0.5); 67 | const int jm = static_cast(j + 0.5); 68 | const int km = static_cast(k + 0.5); 69 | 70 | return im + jm * grid_dim_[0] + km * grid_dim_[0] * grid_dim_[1]; // linear index of closest voxel 71 | 72 | } 73 | 74 | // TODO: re-implement slice functions in a more efficient way! 75 | 76 | void VoxelGrid::slice_x(float* grid, size_t i, cv::Mat &slice) { 77 | 78 | slice = cv::Mat(grid_dim_[1], grid_dim_[2], CV_32FC1); 79 | std::ofstream outfile("../../../results/tmp_xslice.dat"); 80 | for (size_t k=0; k(k, j) = val; 83 | outfile << val << "\t"; 84 | 85 | } 86 | outfile.close(); 87 | 88 | } 89 | 90 | 91 | void VoxelGrid::slice_y(float* grid, size_t j, cv::Mat &slice) { 92 | 93 | slice = cv::Mat(grid_dim_[2], grid_dim_[0], CV_32FC1); 94 | std::ofstream outfile("../../../results/tmp_yslice.dat"); 95 | for (size_t i=0; i(i, k) = val; 98 | outfile << val << "\t"; 99 | } 100 | outfile.close(); 101 | 102 | } 103 | 104 | void VoxelGrid::slice_z(float* grid, size_t k, cv::Mat &slice) { 105 | 106 | slice = cv::Mat(grid_dim_[0], grid_dim_[1], CV_32FC1); 107 | std::ofstream outfile("../../../results/tmp_zslice.dat"); 108 | for (size_t j=0; j(j, i) = val; 111 | outfile << val << "\t"; 112 | } 113 | outfile.close(); 114 | 115 | } 116 | 117 | void VoxelGrid::write(float* grid, std::string file) { 118 | 119 | std::ofstream outfile("../../../results/" + file); 120 | for (int I=0; I 15 | #include 16 | #include "mat.h" 17 | // library includes 18 | // #include 19 | #include 20 | 21 | /** 22 | * class declaration 23 | */ 24 | class VoxelGrid { 25 | 26 | protected: 27 | 28 | // variables 29 | 30 | Vec3i grid_dim_; // voxel grid resolution 31 | size_t num_voxels_; 32 | float voxel_size_; // (linear) size of one voxel in m 33 | Vec3f shift_; // position of voxel grid center in camera coordinates 34 | Vec3f origin_; // voxel grid origin, i.e. position of voxel[0] 35 | 36 | // methods 37 | 38 | Vec3f voxel2world(Vec3i index) const { 39 | return origin_ + voxel_size_ * index.cast(); 40 | } 41 | 42 | Vec3f voxel2world(int i, int j, int k) const { 43 | return origin_ + voxel_size_ * Vec3f(i, j, k); 44 | } 45 | 46 | Vec3f vox2float(int i, int j, int k) 47 | { 48 | return voxel_size_ * Vec3f(i, j, k); 49 | } 50 | 51 | Vec3f vox2float(const Vec3i idx) const { 52 | return voxel_size_ * idx.cast(); 53 | } 54 | 55 | Vec3f world2voxelf(Vec3f point) const { 56 | return (point - origin_) / voxel_size_; 57 | } 58 | 59 | Vec3f world2voxelf(float x, float y, float z) const { 60 | return (Vec3f(x, y, z) - origin_) / voxel_size_; 61 | } 62 | 63 | Vec3i world2voxel(Vec3f point) const { 64 | Vec3f tmp = world2voxelf(point) + Vec3f(0.5, 0.5, 0.5); 65 | return tmp.cast(); // round 66 | } 67 | 68 | Vec3i world2voxel(float x, float y, float z) const { 69 | Vec3f tmp = world2voxelf(x, y, z) + Vec3f(0.5, 0.5, 0.5); 70 | return tmp.cast(); // round 71 | } 72 | 73 | float interp3(const float* grid, Vec3f point, float extrap = 1./0.) const; 74 | 75 | float interp3(const float* grid, float x, float y, float z, float extrap = 1./0.) const { 76 | return interp3(grid, Vec3f(x, y, z), extrap); 77 | } 78 | 79 | int idx2line(const Vec3i idx) const{ 80 | return idx[0] + idx[1]*grid_dim_[0] + idx[2]*grid_dim_[0]*grid_dim_[1]; 81 | } 82 | 83 | int idx2line(const int i, const int j, const int k) const{ 84 | Vec3i idx(i,j,k); 85 | return idx[0] + idx[1]*grid_dim_[0] + idx[2]*grid_dim_[0]*grid_dim_[1]; 86 | } 87 | 88 | Vec3i line2idx(const int lin_idx){ 89 | int rest = lin_idx; 90 | int k = rest/(grid_dim_[0]*grid_dim_[1]); 91 | rest -= k*grid_dim_[0]*grid_dim_[1]; 92 | int j = rest/grid_dim_[0]; 93 | rest -= j*grid_dim_[0]; 94 | int i = rest; 95 | Eigen::Vector3i tmp(i,j,k); 96 | return tmp; 97 | } 98 | 99 | int nearest_index(Vec3f point) const; 100 | 101 | int nearest_index(float x, float y, float z) const { 102 | return nearest_index(Vec3f(x, y, z)); 103 | } 104 | 105 | float eval(float* grid, size_t i, size_t j, size_t k) { 106 | size_t lin_index = i + j * grid_dim_[0] + k * grid_dim_[0] * grid_dim_[1]; 107 | return grid[lin_index]; 108 | } 109 | 110 | void slice_x(float* grid, size_t i, cv::Mat &slice); 111 | void slice_y(float* grid, size_t j, cv::Mat &slice); 112 | void slice_z(float* grid, size_t k, cv::Mat &slice); 113 | void write(float* grid, std::string file = "tmp_vol.dat"); 114 | 115 | public: 116 | 117 | // constructors / destructor 118 | 119 | VoxelGrid() : 120 | grid_dim_(Vec3i(64, 64, 64)), 121 | voxel_size_(0.005), 122 | shift_(Vec3f(0.,0.,1.)), 123 | origin_(shift_ - 0.5*voxel_size_*grid_dim_.cast()), 124 | num_voxels_(grid_dim_[0]*grid_dim_[1]*grid_dim_[2]) 125 | {} 126 | 127 | VoxelGrid(Vec3i grid_dim, float voxel_size, Vec3f shift) : 128 | grid_dim_(grid_dim), 129 | voxel_size_(voxel_size), 130 | shift_(shift), 131 | origin_(shift_ - 0.5*voxel_size*grid_dim_.cast()), 132 | num_voxels_(grid_dim_[0]*grid_dim_[1]*grid_dim_[2]) 133 | {} 134 | 135 | void set_voxel_size(float voxel_size){ 136 | voxel_size_ = voxel_size; 137 | } 138 | 139 | void set_grid_dim(Vec3i grid_dim){ 140 | grid_dim_ = grid_dim; 141 | } 142 | 143 | void grid_subsample() 144 | { 145 | voxel_size_ *= 0.5; 146 | grid_dim_ = 2*grid_dim_; 147 | origin_ = shift_ - 0.5*voxel_size_*grid_dim_.cast() - 0.5*voxel_size_*Vec3f::Ones(); 148 | num_voxels_ = grid_dim_[0]*grid_dim_[1]*grid_dim_[2]; 149 | } 150 | 151 | virtual ~VoxelGrid() {} 152 | 153 | }; 154 | 155 | #endif // VOXEL_GRID_H_ 156 | -------------------------------------------------------------------------------- /cpp/third/mesh/GradMarchingCubes.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAD_MARCHING_CUBES_H 2 | #define GRAD_MARCHING_CUBES_H 3 | 4 | #include 5 | 6 | #include "mat.h" 7 | 8 | #include 9 | 10 | class GradMarchingCubes 11 | { 12 | public: 13 | GradMarchingCubes(const Vec3i &dimensions, const Vec3f &size, const Vec3f &origin); 14 | 15 | ~GradMarchingCubes(); 16 | 17 | bool computeIsoSurface(const float* tsdf, const float* weights, const unsigned char* red, const unsigned char* green, const unsigned char* blue, float isoValue = 0.0f); 18 | bool computeIsoSurface(const float* tsdf, const float* weights, const Vec3f* grads, const unsigned char* red, const unsigned char* green, const unsigned char* blue, float isoValue = 0.0f); 19 | 20 | bool savePly(const std::string &filename) const; 21 | 22 | protected: 23 | 24 | static int edgeTable[256]; 25 | 26 | static int triTable[256][16]; 27 | 28 | inline int computeLutIndex(int i, int j, int k, float isoValue); 29 | 30 | Vec3f interpolate(float tsdf0, float tsdf1, const Vec3f &val0, const Vec3f &val1, float isoValue); 31 | 32 | Vec3f getVertex(int i1, int j1, int k1, int i2, int j2, int k2, float isoValue); 33 | Vec3f getVertex(int i1, int j1, int k1, float isoValue); 34 | 35 | Vec3b getColor(int x1, int y1, int z1, int x2, int y2, int z2, float isoValue); 36 | Vec3b getColor(int x1, int y1, int z1, float isoValue); 37 | 38 | void computeTriangles(int cubeIndex, const Vec3f edgePoints[12], const Vec3b edgeColors[12]); 39 | 40 | inline unsigned int addVertex(const Vec3f &v, const Vec3b &c); 41 | 42 | Vec3f voxelToWorld(int i, int j, int k) const; 43 | 44 | std::vector vertices_; 45 | std::vector colors_; 46 | std::vector faces_; 47 | Vec3i dim_; 48 | Vec3f size_; 49 | Vec3f voxelSize_; 50 | Vec3f origin_; 51 | 52 | const float* tsdf_; 53 | const float* weights_; 54 | const Vec3f* grads_; 55 | const unsigned char* red_; 56 | const unsigned char* green_; 57 | const unsigned char* blue_; 58 | }; 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /cpp/third/mesh/LayeredMarchingCubes.h: -------------------------------------------------------------------------------- 1 | #ifndef LAYERED_MARCHING_CUBES_H 2 | #define LAYERED_MARCHING_CUBES_H 3 | 4 | #include 5 | 6 | #include "mat.h" 7 | 8 | #include 9 | 10 | #include "sdf_voxel/SdfVoxel.h" 11 | 12 | class LayeredMarchingCubes 13 | { 14 | public: 15 | 16 | using voxel_phmap = phmap::parallel_node_hash_map, 18 | phmap::priv::hash_default_eq, 19 | Eigen::aligned_allocator>>; 20 | 21 | // LayeredMarchingCubes(const Vec3i &dimensions, const Vec3f &size); 22 | 23 | LayeredMarchingCubes(const Vec3f &voxelSize); 24 | 25 | ~LayeredMarchingCubes(); 26 | 27 | bool computeIsoSurface(const voxel_phmap* sdf_map, float isoValue = 0.0f); 28 | 29 | bool savePly(const std::string &filename) const; 30 | 31 | protected: 32 | 33 | static int edgeTable[256]; 34 | 35 | static int triTable[256][16]; 36 | 37 | inline int computeLutIndex(int i, int j, int k, float isoValue); 38 | 39 | void copyLayer(int z, const voxel_phmap* sdf_map); 40 | 41 | inline void zeroWeights(int x, int y, int z); 42 | 43 | inline void copyCube(int x, int y, int z, const float w, 44 | const Eigen::Matrix& d, 45 | const Eigen::Matrix& r, 46 | const Eigen::Matrix& g, 47 | const Eigen::Matrix& b 48 | ); 49 | 50 | inline void setVoxel(const size_t off, const size_t corner, const float w, 51 | const Eigen::Matrix& d, 52 | const Eigen::Matrix& r, 53 | const Eigen::Matrix& g, 54 | const Eigen::Matrix& b 55 | ); 56 | 57 | Vec3f interpolate(float tsdf0, float tsdf1, const Vec3f &val0, const Vec3f &val1, float isoValue); 58 | 59 | Vec3f getVertex(int i1, int j1, int k1, int i2, int j2, int k2, float isoValue); 60 | 61 | Vec3b getColor(int x1, int y1, int z1, int x2, int y2, int z2, float isoValue); 62 | 63 | void computeTriangles(int cubeIndex, const Vec3f edgePoints[12], const Vec3b edgeColors[12]); 64 | 65 | inline unsigned int addVertex(const Vec3f &v, const Vec3b &c); 66 | 67 | Vec3f voxelToWorld(int i, int j, int k) const; 68 | 69 | std::vector vertices_; 70 | std::vector colors_; 71 | std::vector faces_; 72 | Vec3i dim_; 73 | Vec3f size_; 74 | Vec3f voxelSize_; 75 | Vec3f origin_; 76 | Vec3i min_; 77 | 78 | // layers 79 | float* tsdf_; 80 | float* weights_; 81 | unsigned char* red_; 82 | unsigned char* green_; 83 | unsigned char* blue_; 84 | }; 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /cpp/third/mesh/MarchingCubes.h: -------------------------------------------------------------------------------- 1 | #ifndef MARCHING_CUBES_H 2 | #define MARCHING_CUBES_H 3 | 4 | #include 5 | 6 | #include "mat.h" 7 | 8 | #include 9 | 10 | class MarchingCubes 11 | { 12 | public: 13 | MarchingCubes(const Vec3i &dimensions, const Vec3f &size, const Vec3f &origin); 14 | 15 | ~MarchingCubes(); 16 | 17 | bool computeIsoSurface(const float* tsdf, const float* weights, const unsigned char* red, const unsigned char* green, const unsigned char* blue, float isoValue = 0.0f); 18 | 19 | bool savePly(const std::string &filename) const; 20 | 21 | protected: 22 | 23 | static int edgeTable[256]; 24 | 25 | static int triTable[256][16]; 26 | 27 | inline int computeLutIndex(int i, int j, int k, float isoValue); 28 | 29 | Vec3f interpolate(float tsdf0, float tsdf1, const Vec3f &val0, const Vec3f &val1, float isoValue); 30 | 31 | Vec3f getVertex(int i1, int j1, int k1, int i2, int j2, int k2, float isoValue); 32 | 33 | Vec3b getColor(int x1, int y1, int z1, int x2, int y2, int z2, float isoValue); 34 | 35 | void computeTriangles(int cubeIndex, const Vec3f edgePoints[12], const Vec3b edgeColors[12]); 36 | 37 | inline unsigned int addVertex(const Vec3f &v, const Vec3b &c); 38 | 39 | Vec3f voxelToWorld(int i, int j, int k) const; 40 | 41 | std::vector vertices_; 42 | std::vector colors_; 43 | std::vector faces_; 44 | Vec3i dim_; 45 | Vec3f size_; 46 | Vec3f voxelSize_; 47 | Vec3f origin_; 48 | 49 | const float* tsdf_; 50 | const float* weights_; 51 | const unsigned char* red_; 52 | const unsigned char* green_; 53 | const unsigned char* blue_; 54 | }; 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /cpp/third/mesh/MarchingCubesNoColor.h: -------------------------------------------------------------------------------- 1 | #ifndef MARCHING_CUBES_NO_COLOR_H 2 | #define MARCHING_CUBES_NO_COLOR_H 3 | 4 | #include 5 | 6 | #include "mat.h" 7 | 8 | #include 9 | 10 | class MarchingCubesNoColor 11 | { 12 | public: 13 | MarchingCubesNoColor(const Vec3i &dimensions, const Vec3f &size, const Vec3f &origin); 14 | 15 | ~MarchingCubesNoColor(); 16 | 17 | bool computeIsoSurface(const float* tsdf, const float* weights, float isoValue = 0.0f); 18 | 19 | bool savePly(const std::string &filename) const; 20 | 21 | protected: 22 | 23 | static int edgeTable[256]; 24 | 25 | static int triTable[256][16]; 26 | 27 | inline int computeLutIndex(int i, int j, int k, float isoValue); 28 | 29 | Vec3f interpolate(float tsdf0, float tsdf1, const Vec3f &val0, const Vec3f &val1, float isoValue); 30 | 31 | Vec3f getVertex(int i1, int j1, int k1, int i2, int j2, int k2, float isoValue); 32 | 33 | Vec3b getColor(int x1, int y1, int z1, int x2, int y2, int z2, float isoValue); 34 | 35 | void computeTriangles(int cubeIndex, const Vec3f edgePoints[12]); 36 | 37 | inline unsigned int addVertex(const Vec3f &v); 38 | 39 | Vec3f voxelToWorld(int i, int j, int k) const; 40 | 41 | std::vector vertices_; 42 | std::vector faces_; 43 | Vec3i dim_; 44 | Vec3f size_; 45 | Vec3f voxelSize_; 46 | Vec3f origin_; 47 | 48 | const float* tsdf_; 49 | const float* weights_; 50 | }; 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /cpp/voxel_ps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 3.4) 2 | 3 | SET(EXECUTABLE_OUTPUT_PATH ../../voxel_ps/bin) 4 | SET(SUBPROJECT_NAME voxelPS) 5 | # volumetric tracking - different options 6 | 7 | ADD_EXECUTABLE( ${SUBPROJECT_NAME} 8 | src/main_ps.cpp 9 | ) 10 | 11 | TARGET_LINK_LIBRARIES( ${SUBPROJECT_NAME} 12 | sdf_tracker_lib 13 | ps_lib 14 | ${OpenCV_LIBS} 15 | ) 16 | 17 | TARGET_COMPILE_OPTIONS( ${SUBPROJECT_NAME} PRIVATE -std=c++17 ) 18 | 19 | -------------------------------------------------------------------------------- /cpp/voxel_ps/src/main_ps.cpp: -------------------------------------------------------------------------------- 1 | // ============================================================================ 2 | // Name : main.cpp 3 | // Author : L.Sang, with many parts taken from C. Sommer 4 | // Date : 09/2021 5 | // License : GNU General Public License 6 | // Description : 3D reconstruction using RGB-D data 7 | // ============================================================================ 8 | 9 | // standard includes 10 | #include 11 | #include 12 | #include 13 | #include 14 | // library includes 15 | #include 16 | #include 17 | #include 18 | #include 19 | // class includes 20 | #include "Timer.h" 21 | #include "normals/NormalEstimator.h" 22 | #include "ps_optimizer/OptimizerSettings.h" 23 | #include "sdf_tracker/TrackingSettings.h" 24 | #include "ConfigLoader.h" 25 | #include "sdf_tracker/VolumetricGradSdf.h" 26 | #include "sdf_tracker/RigidPointOptimizer.h" 27 | #include "img_loader/img_loader.h" 28 | #include "ps_optimizer/PsOptimizer.h" 29 | #include "ps_optimizer/LedOptimizer.h" 30 | #include "ps_optimizer/SharpDetector.h" 31 | // own includes 32 | #include "mat.h" 33 | 34 | // Vec3f compute_centroid(const Mat3f &K, const cv::Mat &depth); 35 | Vec3f compute_centroid(const Mat3f &K, const cv::Mat &depth, const Mat4f& Trans); 36 | void sampleKeyFrame(std::vector& key_frames, std::vector& key_stamps, std::vector>& key_images, std::vector>& key_poses, int max_num); 37 | 38 | /** 39 | * main function 40 | */ 41 | int main(int argc, char *argv[]) { 42 | 43 | Timer T; 44 | 45 | // Default input sequence in folder 46 | std::string configfile = ""; 47 | 48 | // bool REDIRECT = false; 49 | bool light = false; 50 | bool albedo = false; 51 | bool distance = false; 52 | bool pose = false; 53 | 54 | CLI::App app{"Volumetric Tracking Example Code"}; 55 | app.add_option("--config_file", configfile, "folder of input sequence"); 56 | 57 | // parse input arguments 58 | try { 59 | app.parse(argc, argv); 60 | } catch (const CLI::ParseError& e) { 61 | return app.exit(e); 62 | } 63 | 64 | // load config json file 65 | std::cout << "load the config file from: " << configfile << std::endl; 66 | 67 | OptimizerSettings* opt_set_; 68 | TrackingSettings* trac_set_; 69 | ImageLoader* loader; 70 | json config; 71 | 72 | if(!LoadConfig(configfile, &trac_set_, &opt_set_, &loader, config)){ 73 | std::cout << "fail to load the config file!" << std::endl; 74 | return 1; 75 | } 76 | 77 | 78 | // save config file to results for reference 79 | if(config.contains("--light")) light = config.at("--light"); 80 | if(config.contains("--albedo")) albedo = config.at("--albedo"); 81 | if(config.contains("--distance")) distance = config.at("--distance"); 82 | if(config.contains("--pose")) pose = config.at("--pose"); 83 | 84 | 85 | // Trunction distance for the tSDF 86 | const float truncation = trac_set_->truncation_factor_ * trac_set_->voxel_size_; 87 | float voxel_size = trac_set_->voxel_size_; 88 | std::string input = trac_set_->input_; 89 | std::string output = trac_set_->output_; 90 | 91 | 92 | // Load camera intrinsics 93 | if (!loader->load_intrinsics("intrinsics.txt")) { 94 | std::cerr << "No intrinsics file found in " << input << "!" << std::endl; 95 | return 1; 96 | } 97 | const Mat3f K = loader->K(); 98 | std::cout << "K: " << std::endl << K << std::endl; 99 | 100 | // load one frame to determine the image size 101 | cv::Mat color, depth; 102 | if (!loader->load_next(color, depth)) { 103 | std::cerr << " -> Frame could not be loaded!" << std::endl; 104 | return 1; 105 | } 106 | 107 | if (color.rows!=depth.rows || color.cols != depth.cols){ 108 | std::cerr << "-> depth image and color image sizes don't match."<< std::endl; 109 | return 1; 110 | } 111 | loader->reset_counter(); // reset the loader to start from the first frame again. 112 | 113 | // create normal estimator 114 | T.tic(); 115 | cv::NormalEstimator* NEst = new cv::NormalEstimator(color.cols, color.rows, K, cv::Size(2*5+1, 2*5+1)); 116 | T.toc("Init normal estimation"); 117 | 118 | std::stringstream stream; 119 | stream << std::fixed << std::setprecision(3) << voxel_size; 120 | std::string voxel_size_str = stream.str(); 121 | 122 | // Prepare tSDF volume 123 | size_t gridW = 128, gridH = 128, gridD = 128; 124 | Vec3i grid_dim(gridW, gridH, gridD); 125 | 126 | Sdf* tSDF; 127 | RigidPointOptimizer* pOpt; 128 | Optimizer* vOpt; 129 | 130 | std::ofstream pose_file(trac_set_->output_ + "tracking_poses.txt"); 131 | std::vector> poses; 132 | 133 | std::vector keyframes; // key frame index 134 | keyframes.push_back(0); 135 | // vector of sampled poses and frames 136 | std::vector key_stamps; 137 | std::vector> key_images; 138 | std::vector> key_poses; 139 | key_poses.push_back(Mat4f::Identity()); 140 | 141 | 142 | // Frames to be processed 143 | // cv::Mat color, depth; 144 | int dist_to_last_keyframe = 0; //help to void to far distance for key frame selection 145 | bool GT_pose = false; // do we have GT poses? 146 | if(!loader->load_pose(trac_set_->pose_file_, poses)){ 147 | std::cout << "GT poses is not avalible!" << std::endl; 148 | poses.push_back(Mat4f::Identity()); 149 | } 150 | else{ 151 | std::cout << poses.size() << " GT poses are loaded." << std::endl; 152 | GT_pose = true; 153 | } 154 | 155 | 156 | // Proceed until first frame 157 | for (size_t i = 0; i < trac_set_->first_; ++i) { 158 | loader->load_next(color, depth); 159 | } 160 | 161 | // Actual scanning loop 162 | for (size_t i = trac_set_->first_; i <= trac_set_->last_; ++i) { 163 | std::cout << "Working on frame: " << i << std::endl; 164 | 165 | // Load data 166 | T.tic(); 167 | if (!loader->load_next(color, depth)) { 168 | std::cerr << " -> Frame " << i << " could not be loaded!" << std::endl; 169 | T.toc("Load data"); 170 | break; 171 | } 172 | T.toc("Load data"); 173 | 174 | // Get initial volume pose from centroid of first depth map 175 | if (i == trac_set_->first_) { 176 | 177 | // Initial pose for volume by computing centroid of first depth/vertex map 178 | Vec3f centroid = compute_centroid(K, depth, poses[0]); 179 | // std::cout << centroid << std::endl; 180 | 181 | // create SDF data 182 | T.tic(); 183 | tSDF = new VolumetricGradSdf(grid_dim, voxel_size, centroid, truncation); 184 | T.toc("Create Sdf"); 185 | tSDF->set_zmin(trac_set_->zmin_); 186 | tSDF->set_zmax(trac_set_->zmax_); 187 | 188 | T.tic(); 189 | pOpt = new RigidPointOptimizer(tSDF); 190 | T.toc("Create RigidOptimizer"); 191 | 192 | T.tic(); 193 | switch (opt_set_->model) { 194 | case ModelType::SH1 : 195 | case ModelType::SH2 : 196 | vOpt = new PsOptimizer(static_cast(tSDF), voxel_size, K, output, opt_set_); 197 | break; 198 | case ModelType::LED : 199 | vOpt = new LedOptimizer(static_cast(tSDF), voxel_size, K, output, opt_set_); 200 | break; 201 | 202 | } 203 | T.toc("Create PhotometricOptimizer"); 204 | 205 | // Initialize tSDF 206 | T.tic(); 207 | tSDF->update(color, depth, K, SE3(poses[0]), NEst); 208 | T.toc("Integrate depth data into Sdf"); 209 | // Initialize optimizer 210 | 211 | key_stamps.push_back(loader->rgb_timestamp()); 212 | cv::Mat new_color; 213 | color.copyTo(new_color); 214 | key_images.push_back(std::make_shared(new_color)); 215 | } 216 | else if(GT_pose){ 217 | T.tic(); 218 | tSDF->increase_counter(); 219 | tSDF->update(color, depth, K, SE3(poses[i]), NEst); 220 | T.toc("Integrate depth data into Sdf"); 221 | // select key frame if the frame is sharp enough or dist to last key frame is too large 222 | if (sharpDetector(color, trac_set_->sharpness_threshold_) || dist_to_last_keyframe > 5){ 223 | dist_to_last_keyframe = 0; 224 | keyframes.push_back(i-trac_set_->first_); 225 | key_stamps.push_back(loader->rgb_timestamp()); 226 | key_poses.push_back(poses[i]); 227 | cv::Mat new_color; 228 | color.copyTo(new_color); 229 | key_images.push_back(std::make_shared(new_color)); 230 | 231 | } 232 | else{ 233 | ++dist_to_last_keyframe; 234 | } 235 | } 236 | else{ 237 | T.tic(); 238 | tSDF->increase_counter(); 239 | bool conv = pOpt->optimize(depth, K); 240 | T.toc("Point optimization"); 241 | if(conv){ 242 | T.tic(); 243 | tSDF->update(color, depth, K, pOpt->pose(), NEst); 244 | T.toc("Integrate depth data into Sdf"); 245 | // select key frame if the frame is sharp enough or dist to last key frame is too large 246 | if (sharpDetector(color, trac_set_->sharpness_threshold_) || dist_to_last_keyframe > 5){ 247 | dist_to_last_keyframe = 0; 248 | keyframes.push_back(i-trac_set_->first_); 249 | key_stamps.push_back(loader->rgb_timestamp()); 250 | key_poses.push_back(pOpt->pose().matrix()); 251 | cv::Mat new_color; 252 | color.copyTo(new_color); 253 | key_images.push_back(std::make_shared(new_color)); 254 | 255 | } 256 | else{ 257 | ++dist_to_last_keyframe; 258 | } 259 | } 260 | 261 | } 262 | // write timestamp + pose in tx ty tz qx qy qz qw format 263 | Mat4f p; 264 | if(GT_pose) p = poses[i]; 265 | else p = (pOpt->pose()).matrix(); 266 | std::cout << "Current pose:" << std::endl 267 | << p << std::endl; 268 | 269 | Vec3f t(p.topRightCorner(3,1)); 270 | Mat3f R = p.topLeftCorner(3,3); 271 | Eigen::Quaternion q(R); 272 | 273 | pose_file << loader->depth_timestamp() << " " 274 | << t[0] << " " << t[1] << " " << t[2] << " " 275 | << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << "\n"; 276 | 277 | } 278 | 279 | pose_file.close(); 280 | 281 | // extract mesh and write to file 282 | T.tic(); 283 | if (!tSDF->extract_mesh(output + "init_mesh.ply")) { 284 | std::string filename = output + "init_mesh.ply"; 285 | std::cerr << "Could not save mesh to " << filename << "!" << std::endl; 286 | } 287 | T.toc("Save mesh to disk"); 288 | 289 | // extract point cloud and write to file 290 | T.tic(); 291 | if (!tSDF->extract_pc(output + "init_pointcloud.ply")) { 292 | std::string filename = output + "init_pointcloud.ply"; 293 | std::cerr << "Could not save point cloud to " << filename << "!" << std::endl; 294 | } 295 | T.toc("Save point cloud to disk"); 296 | 297 | // extract point cloud and write to file 298 | T.tic(); 299 | if (!tSDF->saveSDF(output + "init_sdf.sdf")) { 300 | std::string filename = output + "init_sdf.sdf"; 301 | std::cerr << "Could not save sdf to " << filename << "!" << std::endl; 302 | } 303 | T.toc("Save point cloud to disk"); 304 | // tSDF->check_vis_map(); 305 | 306 | //=================================================== finished tracking ============================================================= 307 | std::cout << " selected key frame: " << std::endl; 308 | for(size_t i = 0; i < key_images.size(); i++){ 309 | std::cout << keyframes[i] << " "; 310 | } 311 | std::cout << std::endl; 312 | if(keyframes.size()>40){ 313 | sampleKeyFrame(keyframes, key_stamps, key_images, key_poses, 40); 314 | } 315 | std::cout << " selected key frame after sampling: " << std::endl; 316 | for(size_t i = 0; i < key_images.size(); i++){ 317 | std::cout << keyframes[i] << " "; 318 | } 319 | std::cout << std::endl; 320 | 321 | 322 | 323 | vOpt->setImages(key_images); 324 | vOpt->setKeyframes(keyframes); 325 | vOpt->setKeytimestamps(key_stamps); 326 | vOpt->setPoses(key_poses); 327 | 328 | 329 | vOpt->init(); 330 | vOpt->alternatingOptimize(light, albedo, distance, pose); 331 | 332 | // run python script 333 | 334 | // tidy up 335 | delete tSDF; 336 | delete pOpt; 337 | delete loader; 338 | delete vOpt; 339 | 340 | cv::destroyAllWindows(); 341 | 342 | return 0; 343 | } 344 | 345 | // centroid computation to initialize volume at first frame 346 | Vec3f compute_centroid(const Mat3f &K, const cv::Mat &depth, const Mat4f& Trans) { 347 | 348 | Vec3f centroid(0., 0., 0.); 349 | int counter = 0; 350 | Mat3f R = Trans.topLeftCorner(3,3); 351 | Vec3f t = Trans.topRightCorner(3,1); 352 | 353 | int w = depth.cols; 354 | int h = depth.rows; 355 | float fx = K(0,0); 356 | float fy = K(1,1); 357 | float cx = K(0,2); 358 | float cy = K(1,2); 359 | float fx_inv = 1.f / fx; 360 | float fy_inv = 1.f / fy; 361 | const float* depth_ptr = (const float*)depth.data; 362 | 363 | for (int y=0; y0.0) { 366 | float x0 = (float(x) - cx) * fx_inv; 367 | float y0 = (float(y) - cy) * fy_inv; 368 | centroid += R*Vec3f(x0 * z, y0 * z, z) + t; 369 | ++counter; 370 | } 371 | } 372 | 373 | 374 | return centroid / float(counter); 375 | } 376 | 377 | void normalize_GT_poses(std::vector>& poses, const int first_frame) 378 | { 379 | std::vector> normalized_poses; 380 | Mat4f base_pose = poses[first_frame]; 381 | for(size_t i = first_frame; i& key_frames, std::vector& key_stamps, std::vector>& key_images, std::vector>& key_poses, int max_num){ 393 | if (key_frames.size() < max_num ){ 394 | return; 395 | } 396 | // int step = int(std::round(static_cast(valid_frames.size())/static_cast(max_num))); 397 | max_num -= 1; 398 | float step = static_cast(key_frames.size()) / static_cast(max_num); 399 | std::vector frames; 400 | std::vector stamps; 401 | std::vector> images; 402 | std::vector> poses; 403 | float idx = 0; 404 | for(int count = 0; count < max_num; count++){ 405 | int i = static_cast(idx); 406 | frames.push_back(key_frames[i]); 407 | stamps.push_back(key_stamps[i]); 408 | images.push_back(key_images[i]); 409 | poses.push_back(key_poses[i]); 410 | idx+=step; 411 | } 412 | frames.push_back(key_frames.back()); //we need the last frame for resize the visibility vector 413 | stamps.push_back(key_stamps.back()); 414 | images.push_back(key_images.back()); 415 | poses.push_back(key_poses.back()); 416 | 417 | key_frames = frames; 418 | key_stamps = stamps; 419 | key_poses = poses; 420 | key_images = images; 421 | } -------------------------------------------------------------------------------- /data/sokrates-mvs/color000001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000001.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000002.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000003.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000003.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000004.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000004.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000005.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000005.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000006.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000006.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000007.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000007.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000008.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000008.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000009.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000009.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000010.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000010.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000011.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000011.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000012.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000012.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000013.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000013.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000014.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000014.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000015.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000015.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000016.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000016.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000017.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000017.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000018.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000018.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000019.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000020.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000020.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000021.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000021.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000022.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000022.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000023.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000023.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000024.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000024.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000025.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000025.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000026.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000026.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000027.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000027.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000028.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000028.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000029.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000029.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000030.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000030.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000031.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000031.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000032.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000032.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000033.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000033.png -------------------------------------------------------------------------------- /data/sokrates-mvs/color000034.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/color000034.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000001.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000002.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000003.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000003.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000004.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000004.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000005.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000005.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000006.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000006.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000007.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000007.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000008.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000008.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000009.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000009.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000010.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000010.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000011.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000011.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000012.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000012.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000013.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000013.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000014.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000014.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000015.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000015.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000016.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000016.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000017.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000017.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000018.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000018.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000019.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000019.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000020.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000020.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000021.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000021.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000022.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000022.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000023.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000023.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000024.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000024.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000025.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000025.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000026.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000026.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000027.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000027.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000028.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000028.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000029.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000029.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000030.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000030.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000031.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000031.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000032.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000032.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000033.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000033.png -------------------------------------------------------------------------------- /data/sokrates-mvs/depth000034.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/data/sokrates-mvs/depth000034.png -------------------------------------------------------------------------------- /data/sokrates-mvs/intrinsics.txt: -------------------------------------------------------------------------------- 1 | 4071.93 0 570.058 2 | 0 4071.93 849.925 3 | 0 0 1 4 | 0 0 1 5 | -------------------------------------------------------------------------------- /data/sokrates-mvs/pose.txt: -------------------------------------------------------------------------------- 1 | 001 -0.299636 -0.753338 -0.138828 -0.160607 0.079351 0.022417 0.983568 2 | 002 -0.030122 -0.767338 -0.092911 -0.174575 -0.036727 -0.031660 0.983449 3 | 003 0.459497 -0.818704 0.077008 -0.171628 -0.257501 -0.133887 0.941441 4 | 004 0.823991 -0.924332 0.441979 -0.152418 -0.468294 -0.231478 0.838980 5 | 005 0.960418 -1.067580 0.968759 -0.121221 -0.654076 -0.311930 0.678373 6 | 006 0.809417 -1.190530 1.440540 -0.092161 -0.785769 -0.373865 0.484043 7 | 007 0.476145 -1.286410 1.814110 -0.050567 -0.870200 -0.409698 0.268964 8 | 008 -0.063595 -1.314170 1.940900 -0.005090 -0.905720 -0.423539 0.016122 9 | 009 -0.633672 -1.283550 1.844850 -0.038465 0.878749 0.405884 0.248152 10 | 010 -1.128010 -1.126930 1.287430 -0.095986 0.745020 0.342881 0.564061 11 | 011 -0.961250 -0.858697 0.301370 -0.155318 0.401808 0.179388 0.884447 12 | 012 -0.612007 -0.790440 0.019686 -0.170780 0.219227 0.091312 0.956261 13 | 013 -0.095174 -0.105108 -0.106506 0.088191 -0.004082 -0.012109 0.996022 14 | 014 0.335297 -0.119647 -0.078616 0.086914 -0.170795 -0.041209 0.980600 15 | 015 0.812999 -0.220675 0.265763 0.081652 -0.392360 -0.077684 0.912881 16 | 016 1.139660 -0.400127 0.928039 0.062630 -0.651425 -0.122844 0.746078 17 | 017 1.073290 -0.585849 1.631100 0.042426 -0.830723 -0.150408 0.534301 18 | 018 0.616211 -0.711441 2.121890 0.021507 -0.942001 -0.167844 0.289828 19 | 019 0.017584 -0.773685 2.365980 0.004424 -0.983821 -0.172953 0.046526 20 | 020 -0.807619 -0.711189 2.172520 0.028322 0.945951 0.163385 0.278712 21 | 021 -1.283270 -0.541418 1.566920 0.052225 0.824174 0.140823 0.546057 22 | 022 -1.330620 -0.293261 0.657091 0.075789 0.568544 0.092401 0.813926 23 | 023 -0.760807 -0.139474 0.058024 0.088437 0.263402 0.036297 0.959938 24 | 024 -0.305970 -0.101274 -0.108587 0.088538 0.082452 0.002465 0.992651 25 | 025 -0.258922 0.238137 0.154587 0.267479 0.077736 -0.009926 0.960372 26 | 026 0.244322 0.220922 0.187755 0.263611 -0.153614 -0.009615 0.952271 27 | 027 0.732279 0.122946 0.519576 0.241415 -0.429738 -0.004203 0.870073 28 | 028 0.959078 -0.000586 0.981335 0.202003 -0.635271 -0.002579 0.745398 29 | 029 0.915075 -0.198448 1.737780 0.124645 -0.864624 -0.005603 0.486680 30 | 030 0.191572 -0.362090 2.381440 0.031563 -0.992301 -0.018647 0.118297 31 | 031 -0.708030 -0.332393 2.316350 0.058138 0.970512 0.024589 0.232642 32 | 032 -1.374710 -0.094316 1.468450 0.150986 0.783752 0.012926 0.602303 33 | 033 -1.242150 0.169659 0.497074 0.206967 0.476696 0.007051 0.854328 34 | 034 -0.643685 0.284807 0.041212 0.227268 0.217282 -0.002646 0.949279 35 | -------------------------------------------------------------------------------- /pipeline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/pipeline.png -------------------------------------------------------------------------------- /reconstruction.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sangluisme/PSgradientSDF/d6921528ed68e830d06b99a1e39ae43ce5e0506c/reconstruction.png --------------------------------------------------------------------------------