├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE.md ├── README.md ├── Report.md ├── apps ├── CMakeLists.txt ├── demo.cpp └── dynamicfusion_kinect.cpp ├── build.sh ├── cmake ├── Modules │ └── FindOpenNI.cmake ├── Targets.cmake └── Utils.cmake ├── download_data.sh ├── kfusion ├── CMakeLists.txt ├── include │ ├── io │ │ └── capture.hpp │ ├── kfusion │ │ ├── cuda │ │ │ ├── device_array.hpp │ │ │ ├── device_memory.hpp │ │ │ ├── imgproc.hpp │ │ │ ├── kernel_containers.hpp │ │ │ ├── projective_icp.hpp │ │ │ └── tsdf_volume.hpp │ │ ├── exports.hpp │ │ ├── kinfu.hpp │ │ ├── optimisation.hpp │ │ ├── types.hpp │ │ ├── warp_field.hpp │ │ └── warp_field_optimiser.hpp │ ├── nanoflann │ │ └── nanoflann.hpp │ └── opt │ │ ├── CUDATimer.h │ │ ├── CombinedSolver.h │ │ ├── OpenMesh.h │ │ ├── Resource.h │ │ ├── mLibInclude.h │ │ ├── main.h │ │ └── resource.h ├── solvers │ └── dynamicfusion.t └── src │ ├── capture.cpp │ ├── cuda │ ├── device.hpp │ ├── imgproc.cu │ ├── proj_icp.cu │ ├── temp_utils.hpp │ ├── texture_binder.hpp │ └── tsdf_volume.cu │ ├── device_memory.cpp │ ├── imgproc.cpp │ ├── internal.hpp │ ├── kinfu.cpp │ ├── mLibSource.cpp │ ├── precomp.cpp │ ├── precomp.hpp │ ├── projective_icp.cpp │ ├── safe_call.hpp │ ├── tsdf_volume.cpp │ ├── utils │ ├── dual_quaternion.hpp │ ├── knn_point_cloud.hpp │ ├── macro_utils.hpp │ └── quaternion.hpp │ ├── warp_field.cpp │ └── warp_field_optimiser.cpp └── tests ├── CMakeLists.txt ├── ceres_warp_test.cpp ├── nanoflann_test.cpp ├── utils ├── CMakeLists.txt ├── test_dual_quaternion.cc └── test_quaternion.cc └── warp_test.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | output/ 3 | .idea/ 4 | cmake-build-debug/ 5 | cmake-build-release/ 6 | data/ 7 | html/ 8 | latex/ 9 | Doxyfile 10 | deps/terra -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "deps/Opt"] 2 | path = deps/Opt 3 | url = https://github.com/niessner/Opt 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.0) 2 | 3 | # ---[ Configurations types 4 | set(CMAKE_CONFIGURATION_TYPES "Debug;Release" CACHE STRING "Possible configurations" FORCE) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DOM_STATIC_BUILD -std=c++11 -DFORCE_INLINERS -D_MWAITXINTRIN_H_INCLUDED -Wno-write-strings") 6 | set(CMAKE_CXX_STANDARD 11) 7 | if (DEFINED CMAKE_BUILD_TYPE) 8 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${CMAKE_CONFIGURATION_TYPES}) 9 | endif() 10 | if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile) 11 | find_package(Doxygen) 12 | if(DOXYGEN_FOUND) 13 | 14 | set(DOXYGEN_INPUT ${SOURCE_FILES}) 15 | set(DOXYGEN_OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) 16 | 17 | add_custom_command( 18 | OUTPUT ${DOXYGEN_OUTPUT} 19 | COMMAND ${CMAKE_COMMAND} -E echo_append "Building API Documentation..." 20 | COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_INPUT} 21 | COMMAND ${CMAKE_COMMAND} -E echo "Done." 22 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 23 | DEPENDS ${DOXYGEN_INPUT} 24 | ) 25 | 26 | add_custom_target(apidoc ALL DEPENDS ${DOXYGEN_OUTPUT}) 27 | 28 | add_custom_target(apidoc_forced 29 | COMMAND ${CMAKE_COMMAND} -E echo_append "Building API Documentation..." 30 | COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_INPUT} 31 | COMMAND ${CMAKE_COMMAND} -E echo "Done." 32 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} 33 | ) 34 | 35 | endif(DOXYGEN_FOUND) 36 | endif() 37 | 38 | # ---[ Solution name 39 | project(kfusion C CXX) 40 | 41 | # ---[ utility 42 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake/Modules/") 43 | include(cmake/Utils.cmake) 44 | include(cmake/Targets.cmake) 45 | 46 | # ---[ find dependencies 47 | find_package(OpenCV REQUIRED COMPONENTS core viz highgui calib3d) 48 | find_package(CUDA REQUIRED) 49 | find_package(OpenNI) 50 | find_package(Ceres REQUIRED) 51 | include_directories(${OpenCV_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ${CUDA_INCLUDE_DIRS} ${OPENNI_INCLUDE_DIR} "kfusion/src/utils" "kfusion/include/nanoflann") 52 | if(OPENNI_FOUND) 53 | message("FOUND OPENNI AT: ${OPENNI_INCLUDE_DIR}") 54 | endif() 55 | # ---[ misc settings 56 | if(USE_PROJECT_FOLDERS) 57 | set_property(GLOBAL PROPERTY USE_FOLDERS ON) 58 | set_property(GLOBAL PROPERTY PREDEFINED_TARGETS_FOLDER "CMakeTargets") 59 | endif() 60 | 61 | # ---[ cuda settings 62 | set(HAVE_CUDA 1) 63 | list(APPEND CUDA_NVCC_FLAGS "-gencode;arch=compute_30,code=sm_30;-gencode;arch=compute_35,code=sm_35;-gencode;arch=compute_50,code=sm_50;") 64 | if(CUDA_VERSION GREATER 7.5) 65 | list(APPEND CUDA_NVCC_FLAGS "-gencode;arch=compute_61,code=sm_61") 66 | endif() 67 | if(UNIX OR APPLE) 68 | list(APPEND CUDA_NVCC_FLAGS "-Xcompiler;-fPIC;") 69 | endif() 70 | 71 | warnings_disable(CMAKE_CXX_FLAGS /wd4985) 72 | set(OPT_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/deps/Opt/examples/external/mLib/include ${CMAKE_CURRENT_SOURCE_DIR}/deps/Opt/examples/shared/ ${CMAKE_CURRENT_SOURCE_DIR}/deps/Opt/API/release/include) 73 | set(OPT_LIBRARIES ${CMAKE_CURRENT_SOURCE_DIR}/deps/Opt/API/release/lib/libOptDev.a) 74 | set(TERRA_LIBRARIES ${CMAKE_CURRENT_SOURCE_DIR}/deps/terra/lib/libterra.a) 75 | 76 | add_subdirectory(kfusion) 77 | add_subdirectory(apps) 78 | 79 | if(BUILD_TESTS) 80 | find_package(GTest REQUIRED) 81 | if( GTEST_FOUND ) 82 | message( "Found Gtest at ${GTEST_ROOT}") 83 | message( "GTest Libs: ${GTEST_BOTH_LIBRARIES}") 84 | message( "GTest Include: ${GTEST_INCLUDE_DIRS}") 85 | include_directories(${GTEST_INCLUDE_DIRS}) 86 | add_subdirectory(tests) 87 | endif() 88 | endif() 89 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | Copyright (c) 2012, Anatoly Baksheev 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the {organization} nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | DynamicFusion 2 | ============ 3 | Implementation of [Newcombe et al. 2015 DynamicFusion paper](http://grail.cs.washington.edu/projects/dynamicfusion/papers/DynamicFusion.pdf). 4 | 5 | #### This project is no longer maintained. Dependencies like CUDA and Opt are likely to be severely outdated. 6 | 7 | The code is based on this [KinectFusion implemenation](https://github.com/Nerei/kinfu_remake) 8 | 9 | ## Building instructions: 10 | 11 | ### Ubuntu 16.04 12 | Clone dynamicfusion and dependencies. 13 | ``` 14 | git clone https://github.com/mihaibujanca/dynamicfusion --recursive 15 | ``` 16 | 17 | Install NVIDIA drivers. 18 | - Enable NVidia drivers (Search / Additional Drivers) selecting: 19 | "Using NVIDIA binary driver - version 375.66 from nvidia-375 (proprietary, tested)" 20 | "Using processor microcode firmware for Intel CPUs from intel-microcode (proprietary)" 21 | - Restart pc to complete installation 22 | 23 | Alternatively a good tutorial with some common issues covered can be found [here]( 24 | https://askubuntu.com/a/61433/167689). 25 | 26 | For fresh installs (this assumes you cloned your project in your home directory!): 27 | ``` 28 | chmod +x build.sh 29 | ./build.sh 30 | ``` 31 | 32 | If you are not on a fresh install, check `build.sh` for building instructions and dependencies. 33 | 34 | If you want to build the tests as well, set `-DBUILD_TESTS=ON`.\ 35 | To save frames showing the reconstruction progress, pass `-DSAVE_RECONSTRUCTION_FRAMES=ON`. The frames will be saved in `/output` 36 | 37 | To build documentation, go to the project root directory and execute 38 | ``` 39 | doxygen -g 40 | doxygen Doxyfile 41 | ``` 42 | 43 | 44 | ### Running 45 | ``` 46 | ./download_data 47 | ./build/bin/dynamicfusion data/umbrella 48 | ``` 49 | 50 | ### Windows 51 | Dependencies: 52 | * CUDA 5.0 or higher 53 | * OpenCV 2.4.8 or higher (modules opencv_core, opencv_highgui, opencv_calib3d, opencv_imgproc, opencv_viz). Make sure that WITH_VTK flag is enabled in CMake during OpenCV configuration. 54 | * Boost (libraries system, filesystem and program options. Only used in the demo. Tested with [1.64.0](http://www.boost.org/users/history/version_1_64_0.html)) 55 | * Ceres solver (Tested with version [1.13.0](http://ceres-solver.org/ceres-solver-1.13.0.tar.gz)) 56 | 57 | Implicit dependency (needed by opencv_viz): 58 | * VTK 5.8.0 or higher 59 | * SuiteSparse, BLAS and LAPACK for ceres 60 | Optional dependencies: 61 | * GTest for testing 62 | * Doxygen for documentation 63 | * OpenNI v1.5.4 for getting input straight from a kinect device. 64 | 65 | [Install NVIDIA drivers](https://www.geforce.com/drivers) and [CUDA](https://developer.nvidia.com/cuda-downloads) 66 | * [Install LAPACK](http://icl.cs.utk.edu/lapack-for-windows/lapack/). 67 | * [Install VTK](http://www.vtk.org/download/) (download and build from source) 68 | * [Install OpenCV](http://docs.opencv.org/3.2.0/d3/d52/tutorial_windows_install.html). 69 | * [Install Boost](http://www.boost.org/users/download/) 70 | 71 | 72 | Optionals: 73 | * [Doxygen](http://www.stack.nl/~dimitri/doxygen/download.html) 74 | * [GTest](https://github.com/google/googletest) 75 | * [OpenNI]( http://pointclouds.org/downloads/windows.html) 76 | 77 | [Download the dataset](http://lgdv.cs.fau.de/uploads/publications/data/innmann2016deform/umbrella_data.zip).\ 78 | Create a `data` folder inside the project root directory. \ 79 | Unzip the archive into `data` and remove any files that are not .png. \ 80 | Inside `data`, create directories `color` and `depth`, and move color and depth frames to their corresponding folders. 81 | 82 | To use with .oni captures or straight from a kinect device, use `./build/bin/dynamicfusion_kinect ` or `./build/bin/dynamicfusion_kinect ` 83 | 84 | --- 85 | Note: currently, the frame rate is too low (10s / frame) to be able to cope with live inputs, so it is advisable that you capture your input first. 86 | 87 | ## References 88 | [DynamicFusion project page](http://grail.cs.washington.edu/projects/dynamicfusion/) 89 | 90 | ``` 91 | @InProceedings{Newcombe_2015_CVPR, 92 | author = {Newcombe, Richard A. and Fox, Dieter and Seitz, Steven M.}, 93 | title = {DynamicFusion: Reconstruction and Tracking of Non-Rigid Scenes in Real-Time}, 94 | booktitle = {The IEEE Conference on Computer Vision and Pattern Recognition (CVPR)}, 95 | month = {June}, 96 | year = {2015} 97 | } 98 | ``` 99 | 100 | The example dataset is taken from the [VolumeDeform project](http://lgdv.cs.fau.de/publications/publication/Pub.2016.tech.IMMD.IMMD9.volume_6/). 101 | ``` 102 | @inbook{innmann2016volume, 103 | author = "Innmann, Matthias and Zollh{\"o}fer, Michael and Nie{\ss}ner, Matthias and Theobalt, Christian 104 | and Stamminger, Marc", 105 | editor = "Leibe, Bastian and Matas, Jiri and Sebe, Nicu and Welling, Max", 106 | title = "VolumeDeform: Real-Time Volumetric Non-rigid Reconstruction", 107 | bookTitle = "Computer Vision -- ECCV 2016: 14th European Conference, Amsterdam, The Netherlands, 108 | October 11-14, 2016, Proceedings, Part VIII", 109 | year = "2016", 110 | publisher = "Springer International Publishing", 111 | address = "Cham", 112 | pages = "362--379", 113 | isbn = "978-3-319-46484-8", 114 | doi = "10.1007/978-3-319-46484-8_22", 115 | url = "http://dx.doi.org/10.1007/978-3-319-46484-8_22" 116 | } 117 | ``` 118 | -------------------------------------------------------------------------------- /Report.md: -------------------------------------------------------------------------------- 1 | # Project Report 2 | 3 | ##### [Project repository](https://github.com/mihaibujanca/dynamicfusion) | [Commits list](https://github.com/search?utf8=%E2%9C%93&q=repo%3Amihaibujanca%2Fdynamicfusion+committer%3Amihaibujanca+author-date%3A%3A%3C2017-08-30&type=Commits) | [PR to OpenCV]() 4 | 5 | ### Brief 6 | The aim of this project was to implement [DynamicFusion](http://grail.cs.washington.edu/projects/dynamicfusion/papers/DynamicFusion.pdf), published in 2015 by Newcombe et al., which introduces an RGBD SLAM system capable of reconstructing 3D models of non-rigidly deforming scenes. 7 | The method represents the scene as a canonical model and a warp field that transforms the model at each frame, in order 8 | to distinguish between already observed data and data that needs to be added to the model. 9 | The system employs KinectFusion [Newcombe et al., 2011] as a 3D reconstruction engine, with an altered pipeline: 10 | 11 | * Get a new frame 12 | * Estimate warp field parameters (align warp field with frame) 13 | * Apply warp on the canonical model to transform it into the frame 14 | * Perform data association 15 | * Update geometry with the new data 16 | * Add new nodes to the warp field if needed 17 | 18 | ### Work structure 19 | The implementation is based off [an available implementation of KinectFusion](https://github.com/Nerei/kinfu_remake) 20 | 21 | My work required the implementation of four main components, plus helper classes and putting everything in a pipeline: 22 | ##### 1. Implementation of the warp field structure, along with the warp function (and functions depending on it) - implemented 23 | 24 | Includes the warp function that transforms vertices from the canonical model (the 3d reconstruction of the model) into the live frame, 25 | the quaternion and dual quaternion classes and other related functionality. 26 | ##### 2. Implementation of the surface fusion algorithm, to add new data to the model - implemented 27 | 28 | Includes functionality related to data association (classifying incoming vertices into new data and old data), and updating geometry of the model 29 | 30 | ##### 3. Estimation of the warp field parameters - partially implemented 31 | 32 | This is one of the most important components and involves estimating the state of the warp field (adapting the warp field to match incoming frames). 33 | This step involves an optimisation routine consisting of a data term and a regularisation term. 34 | There is a significant amount of code for estimating the data term and updating the 35 | warp field with the new estimation, but no code for the regularisation term. 36 | 37 | ##### 4. Extending the warp field - stubbed out functionality 38 | This step involves extending the warp field to cover the canonical model geometry. The warp field holds a 39 | subsampled version of the canonical model. As the canonical model grows with new data being fused in, the warp 40 | field needs to grow as well, to support it. 41 | 42 | The code for this step should be reasonably easy to write and is not expected to take long. 43 | 44 | ### Next steps 45 | 46 | * Finalise implementation of the data term in warp field estimation. After this is done, 47 | the code should be able to reconstruct deforming scenes reasonably well, but is expected to break 48 | when there are major changes between frames, or large portions of perviously occluded objects come into view rapidly 49 | * Implement the regularisation term estimation routine 50 | * Estimate the total energy using the data term and the regularisation term 51 | * Improve speed of the code by rewriting CPU code for the GPU 52 | * Better testing coverage 53 | * Improve documentation 54 | * Restructure the code into interfaces for incoming frames, pose estimation etc, to be able to work with 55 | devices other than kinect (e.g. Google Project Tango, that provides pose estimation from the IMU) 56 | * Add code to color the models 57 | * Export the reconstructions to .ply or .obj files. 58 | 59 | 60 | --- 61 | I would like to thank my mentors Zhe Zhang and Reza Amayeh for all their support throughout GSoC. I have 62 | certainly learned a lot through this and have enjoyed working on the project. -------------------------------------------------------------------------------- /apps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${CMAKE_SOURCE_DIR}/kfusion/include) 2 | 3 | #Adding CombinedSolver because the IDE provides code insight. Not actually needed. 4 | add_executable(dynamicfusion demo.cpp ${CMAKE_SOURCE_DIR}/kfusion/include/opt/CombinedSolver.h) 5 | target_link_libraries(dynamicfusion ${OpenCV_LIBS} 6 | ${OPT_LIBRARIES} 7 | ${TERRA_LIBRARIES} 8 | ${CUDA_CUDART_LIBRARY} 9 | kfusion 10 | ) 11 | 12 | if(SAVE_RECONSTRUCTION_FRAMES) 13 | file(MAKE_DIRECTORY ${CMAKE_SOURCE_DIR}/output/) 14 | target_compile_definitions(dynamicfusion PUBLIC OUTPUT_PATH=${CMAKE_SOURCE_DIR}/output/) 15 | endif() 16 | 17 | set_target_properties(dynamicfusion PROPERTIES 18 | DEBUG_POSTFIX "d" 19 | ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" 20 | RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") 21 | 22 | 23 | if(OPENNI_FOUND) 24 | add_executable(dynamicfusion_kinect dynamicfusion_kinect.cpp) 25 | target_link_libraries(dynamicfusion_kinect ${OPENNI_LIBRARY} ${OpenCV_LIBS} kfusion) 26 | 27 | set_target_properties(dynamicfusion_kinect PROPERTIES 28 | DEBUG_POSTFIX "d" 29 | ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" 30 | RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") 31 | install(TARGETS dynamicfusion_kinect RUNTIME DESTINATION bin COMPONENT main) 32 | endif() 33 | 34 | install(TARGETS dynamicfusion RUNTIME DESTINATION bin COMPONENT main) 35 | -------------------------------------------------------------------------------- /apps/demo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | using namespace kfusion; 7 | 8 | struct DynamicFusionApp 9 | { 10 | static void KeyboardCallback(const cv::viz::KeyboardEvent& event, void* pthis) 11 | { 12 | DynamicFusionApp& kinfu = *static_cast(pthis); 13 | 14 | if(event.action != cv::viz::KeyboardEvent::KEY_DOWN) 15 | return; 16 | 17 | if(event.code == 't' || event.code == 'T') 18 | kinfu.show_warp(*kinfu.kinfu_); 19 | 20 | if(event.code == 'i' || event.code == 'I') 21 | kinfu.interactive_mode_ = !kinfu.interactive_mode_; 22 | } 23 | 24 | DynamicFusionApp(std::string dir) : exit_ (false), interactive_mode_(false), pause_(false), directory(true), dir_name(dir) 25 | { 26 | KinFuParams params = KinFuParams::default_params_dynamicfusion(); 27 | kinfu_ = KinFu::Ptr( new KinFu(params) ); 28 | 29 | 30 | cv::viz::WCube cube(cv::Vec3d::all(0), cv::Vec3d(params.volume_size), true, cv::viz::Color::apricot()); 31 | viz.showWidget("cube", cube, params.volume_pose); 32 | viz.showWidget("coor", cv::viz::WCoordinateSystem(0.1)); 33 | viz.registerKeyboardCallback(KeyboardCallback, this); 34 | 35 | } 36 | static void show_depth(const cv::Mat& depth) 37 | { 38 | cv::Mat display; 39 | depth.convertTo(display, CV_8U, 255.0/4000); 40 | cv::imshow("Depth", display); 41 | cvWaitKey(10); 42 | } 43 | 44 | void show_raycasted(KinFu& kinfu, int i) 45 | { 46 | const int mode = 3; 47 | if (interactive_mode_) 48 | kinfu.renderImage(view_device_, viz.getViewerPose(), mode); 49 | else 50 | kinfu.renderImage(view_device_, mode); 51 | 52 | view_host_.create(view_device_.rows(), view_device_.cols(), CV_8UC4); 53 | view_device_.download(view_host_.ptr(), view_host_.step); 54 | 55 | #ifdef OUTPUT_PATH 56 | std::string path = TOSTRING(OUTPUT_PATH) + std::to_string(i) + ".jpg"; 57 | cv::imwrite(path, view_host_); 58 | #endif 59 | 60 | cv::imshow("Scene", view_host_); 61 | cvWaitKey(100); 62 | 63 | } 64 | 65 | void show_warp(KinFu &kinfu) 66 | { 67 | cv::Mat warp_host = kinfu.getWarp().getNodesAsMat(); 68 | viz.showWidget("warp_field", cv::viz::WCloud(warp_host)); 69 | } 70 | 71 | bool execute() 72 | { 73 | KinFu& dynamic_fusion = *kinfu_; 74 | cv::Mat depth, image; 75 | double time_ms = 0; 76 | bool has_image = false; 77 | std::vector depths; // store paths, 78 | std::vector images; // store paths, 79 | 80 | cv::glob(dir_name + "/depth", depths); 81 | cv::glob(dir_name + "/color", images); 82 | 83 | std::sort(depths.begin(), depths.end()); 84 | std::sort(images.begin(), images.end()); 85 | 86 | for (int i = 0; i < depths.size() && !exit_ && !viz.wasStopped(); i++) { 87 | image = cv::imread(images[i], CV_LOAD_IMAGE_COLOR); 88 | depth = cv::imread(depths[i], CV_LOAD_IMAGE_ANYDEPTH); 89 | depth_device_.upload(depth.data, depth.step, depth.rows, depth.cols); 90 | 91 | // { 92 | // SampledScopeTime fps(time_ms); 93 | // (void) fps; 94 | has_image = dynamic_fusion(depth_device_); 95 | // } 96 | 97 | if (has_image) 98 | show_raycasted(dynamic_fusion, i); 99 | 100 | show_depth(depth); 101 | cv::imshow("Image", image); 102 | 103 | if (!interactive_mode_) { 104 | viz.setViewerPose(dynamic_fusion.getCameraPose()); 105 | } 106 | 107 | int key = cv::waitKey(pause_ ? 0 : 3); 108 | show_warp(dynamic_fusion); 109 | switch (key) { 110 | case 't': 111 | case 'T' : 112 | show_warp(dynamic_fusion); 113 | break; 114 | case 'i': 115 | case 'I' : 116 | interactive_mode_ = !interactive_mode_; 117 | break; 118 | case 27: 119 | exit_ = true; 120 | break; 121 | case 32: 122 | pause_ = !pause_; 123 | break; 124 | } 125 | viz.spinOnce(3, true); 126 | //exit_ = exit_ || i > 100; 127 | } 128 | return true; 129 | } 130 | 131 | bool pause_ /*= false*/; 132 | bool exit_, interactive_mode_, directory; 133 | std::string dir_name; 134 | KinFu::Ptr kinfu_; 135 | cv::viz::Viz3d viz; 136 | 137 | cv::Mat view_host_; 138 | cuda::Image view_device_; 139 | cuda::Depth depth_device_; 140 | 141 | 142 | }; 143 | 144 | int main (int argc, char* argv[]) 145 | { 146 | assert(argc == 2 && "Usage: ./dynamicfusion "); 147 | DynamicFusionApp *app; 148 | app = new DynamicFusionApp(argv[1]); 149 | 150 | // executing 151 | try { app->execute (); } 152 | catch (const std::bad_alloc& /*e*/) { std::cout << "Bad alloc" << std::endl; } 153 | catch (const std::exception& /*e*/) { std::cout << "Exception" << std::endl; } 154 | 155 | delete app; 156 | return 0; 157 | } 158 | -------------------------------------------------------------------------------- /apps/dynamicfusion_kinect.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | using namespace kfusion; 9 | 10 | struct KinFuApp 11 | { 12 | static void KeyboardCallback(const cv::viz::KeyboardEvent& event, void* pthis) 13 | { 14 | KinFuApp& kinfu = *static_cast(pthis); 15 | 16 | if(event.action != cv::viz::KeyboardEvent::KEY_DOWN) 17 | return; 18 | 19 | if(event.code == 't' || event.code == 'T') 20 | kinfu.take_cloud(*kinfu.kinfu_); 21 | 22 | if(event.code == 'i' || event.code == 'I') 23 | kinfu.interactive_mode_ = !kinfu.interactive_mode_; 24 | } 25 | 26 | KinFuApp(OpenNISource& source) : exit_ (false), interactive_mode_(false), capture_ (source), pause_(false) 27 | { 28 | KinFuParams params = KinFuParams::default_params(); 29 | kinfu_ = KinFu::Ptr( new KinFu(params) ); 30 | 31 | capture_.setRegistration(true); 32 | 33 | cv::viz::WCube cube(cv::Vec3d::all(0), cv::Vec3d(params.volume_size), true, cv::viz::Color::apricot()); 34 | viz.showWidget("cube", cube, params.volume_pose); 35 | viz.showWidget("coor", cv::viz::WCoordinateSystem(0.1)); 36 | viz.registerKeyboardCallback(KeyboardCallback, this); 37 | } 38 | 39 | static void show_depth(const cv::Mat& depth) 40 | { 41 | cv::Mat display; 42 | //cv::normalize(depth, display, 0, 255, cv::NORM_MINMAX, CV_8U); 43 | depth.convertTo(display, CV_8U, 255.0/4000); 44 | cv::imshow("Depth", display); 45 | } 46 | 47 | void show_raycasted(KinFu& kinfu) 48 | { 49 | const int mode = 3; 50 | if (interactive_mode_) 51 | kinfu.renderImage(view_device_, viz.getViewerPose(), mode); 52 | else 53 | kinfu.renderImage(view_device_, mode); 54 | 55 | view_host_.create(view_device_.rows(), view_device_.cols(), CV_8UC4); 56 | view_device_.download(view_host_.ptr(), view_host_.step); 57 | cv::imshow("Scene", view_host_); 58 | } 59 | 60 | void take_cloud(KinFu& kinfu) 61 | { 62 | cv::Mat normal_host = kinfu.tsdf().get_normal_host(); 63 | cv::Mat warp_host = kinfu.getWarp().getNodesAsMat(); 64 | 65 | viz1.showWidget("warp_field", cv::viz::WCloud(warp_host)); 66 | } 67 | 68 | bool execute() 69 | { 70 | KinFu& kinfu = *kinfu_; 71 | cv::Mat depth, image; 72 | double time_ms = 0; 73 | bool has_image = false; 74 | for (int i = 0; !exit_ && !viz.wasStopped(); ++i) 75 | { 76 | bool has_frame = capture_.grab(depth, image); 77 | if (!has_frame) 78 | return std::cout << "Can't grab" << std::endl, false; 79 | depth_device_.upload(depth.data, depth.step, depth.rows, depth.cols); 80 | 81 | has_image = kinfu(depth_device_); 82 | 83 | if (has_image) 84 | show_raycasted(kinfu); 85 | 86 | show_depth(depth); 87 | cv::imshow("Image", image); 88 | 89 | if (!interactive_mode_) 90 | { 91 | viz.setViewerPose(kinfu.getCameraPose()); 92 | viz1.setViewerPose(kinfu.getCameraPose()); 93 | } 94 | 95 | int key = cv::waitKey(pause_ ? 0 : 3); 96 | take_cloud(kinfu); 97 | switch(key) 98 | { 99 | case 't': case 'T' : take_cloud(kinfu); break; 100 | case 'i': case 'I' : interactive_mode_ = !interactive_mode_; break; 101 | case 27: exit_ = true; break; 102 | case 32: pause_ = !pause_; break; 103 | } 104 | 105 | //exit_ = exit_ || i > 100; 106 | viz.spinOnce(3, true); 107 | viz1.spinOnce(3, true); 108 | } 109 | return true; 110 | } 111 | 112 | bool pause_ /*= false*/; 113 | bool exit_, interactive_mode_; 114 | KinFu::Ptr kinfu_; 115 | cv::viz::Viz3d viz; 116 | cv::viz::Viz3d viz1; 117 | 118 | cv::Mat view_host_; 119 | cuda::Image view_device_; 120 | cuda::Depth depth_device_; 121 | OpenNISource& capture_; 122 | 123 | }; 124 | 125 | 126 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 127 | 128 | int main (int argc, char* argv[]) 129 | { 130 | KinFuApp *app; 131 | 132 | OpenNISource capture; 133 | capture.open(argv[1]); 134 | app = new KinFuApp(capture); 135 | 136 | // executing 137 | try { app->execute (); } 138 | catch (const std::bad_alloc& /*e*/) { std::cout << "Bad alloc" << std::endl; } 139 | catch (const std::exception& /*e*/) { std::cout << "Exception" << std::endl; } 140 | 141 | delete app; 142 | return 0; 143 | } 144 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | sudo apt-get update 4 | # install cuda toolkit and nvidia-prime 5 | sudo apt-get install nvidia-cuda-dev nvidia-cuda-toolkit nvidia-nsight nvidia-prime 6 | # install git, cmake, SuiteSparse, Lapack, BLAS etc 7 | sudo apt-get install cmake libvtk5-dev libsuitesparse-dev liblapack-dev libblas-dev libgtk2.0-dev pkg-config libopenni-dev libusb-1.0-0-dev wget zip clang 8 | 9 | cd .. 10 | 11 | # Build gflags 12 | git clone https://github.com/gflags/gflags.git 13 | cd gflags 14 | mkdir -p build/ && cd build 15 | cmake .. && make 16 | cd ../../ 17 | 18 | # Build glog 19 | git clone https://github.com/google/glog.git 20 | cd glog 21 | mkdir -p build/ && cd build/ 22 | cmake .. && make 23 | cd ../../ 24 | 25 | # Install Eigen 3.3.4 26 | wget http://bitbucket.org/eigen/eigen/get/3.3.4.tar.gz 27 | tar -xf 3.3.4.tar.gz 28 | cd eigen-eigen-5a0156e40feb 29 | mkdir -p build && cd build 30 | cmake .. 31 | sudo make install 32 | cd ../../ 33 | 34 | # Build Ceres 35 | git clone https://ceres-solver.googlesource.com/ceres-solver 36 | cd ceres-solver 37 | mkdir -p build/ && cd build/ 38 | cmake .. 39 | make -j4 40 | sudo make install 41 | cd ../../ 42 | 43 | # Build OpenCV 2.4.13 44 | git clone https://github.com/opencv/opencv 45 | cd opencv/ 46 | git checkout 2.4.13.3 47 | mkdir -p build && cd build 48 | cmake -DWITH_VTK=ON -DBUILD_opencv_calib3d=ON -DBUILD_opencv_imgproc=ON -DWITH_CUDA=OFF .. 49 | make -j4 50 | sudo make install 51 | cd ../../ 52 | 53 | # Build Boost 54 | wget https://dl.bintray.com/boostorg/release/1.64.0/source/boost_1_64_0.tar.gz 55 | tar -xf boost_1_64_0.tar.gz 56 | cd boost_1_64_0 57 | sudo ./bootstrap.sh 58 | ./b2 59 | cd .. 60 | 61 | # Build DynamicFusion 62 | 63 | cd dynamicfusion/deps 64 | wget https://github.com/zdevito/terra/releases/download/release-2016-03-25/terra-Linux-x86_64-332a506.zip 65 | unzip terra-Linux-x86_64-332a506.zip 66 | rm terra-Linux-x86_64-332a506.zip 67 | mv terra-Linux-x86_64-332a506 terra 68 | 69 | # Build Opt 70 | # Change line 71 | # FLAGS += -O3 -g -std=c++11 -I$(SRC) -I$(SRC)/cutil/inc -I../../API/release/include -I$(TERRAHOME)/include -I$(CUDAHOME)/include -I../external/mLib/include -I../external -I../external/OpenMesh/include 72 | # with 73 | # FLAGS += -D_MWAITXINTRIN_H_INCLUDED -D_FORCE_INLINES -D__STRICT_ANSI__ -O3 -g -std=c++11 -I$(SRC) -I$(SRC)/cutil/inc -I../../API/release/include -I$(TERRAHOME)/include -I$(CUDAHOME)/include -I../external/mLib/include -I../external -I../external/OpenMesh/include 74 | cd Opt/API/ 75 | make -j4 76 | cd ../../../ 77 | 78 | mkdir -p build && cd build 79 | cmake -DOpenCV_DIR=~/opencv/build -DBOOST_ROOT=~/boost_1_64_0/ -DOPENNI_INCLUDE_DIR=/usr/include/ni -DOpenCV_FOUND=TRUE .. 80 | make -j4 81 | cd .. -------------------------------------------------------------------------------- /cmake/Modules/FindOpenNI.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Find OpenNI 3 | # 4 | # This sets the following variables: 5 | # OPENNI_FOUND - True if OPENNI was found. 6 | # OPENNI_INCLUDE_DIRS - Directories containing the OPENNI include files. 7 | # OPENNI_LIBRARIES - Libraries needed to use OPENNI. 8 | # OPENNI_DEFINITIONS - Compiler flags for OPENNI. 9 | # 10 | # For libusb-1.0, add USB_10_ROOT if not found 11 | 12 | find_package(PkgConfig QUIET) 13 | 14 | # Find LibUSB 15 | if(NOT WIN32) 16 | pkg_check_modules(PC_USB_10 libusb-1.0) 17 | find_path(USB_10_INCLUDE_DIR libusb-1.0/libusb.h 18 | HINTS ${PC_USB_10_INCLUDEDIR} ${PC_USB_10_INCLUDE_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" 19 | PATH_SUFFIXES libusb-1.0) 20 | 21 | find_library(USB_10_LIBRARY 22 | NAMES usb-1.0 23 | HINTS ${PC_USB_10_LIBDIR} ${PC_USB_10_LIBRARY_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" 24 | PATH_SUFFIXES lib) 25 | 26 | include(FindPackageHandleStandardArgs) 27 | find_package_handle_standard_args(USB_10 DEFAULT_MSG USB_10_LIBRARY USB_10_INCLUDE_DIR) 28 | 29 | if(NOT USB_10_FOUND) 30 | message(STATUS "OpenNI disabled because libusb-1.0 not found.") 31 | return() 32 | else() 33 | include_directories(SYSTEM ${USB_10_INCLUDE_DIR}) 34 | endif() 35 | endif(NOT WIN32) 36 | 37 | if(${CMAKE_VERSION} VERSION_LESS 2.8.2) 38 | pkg_check_modules(PC_OPENNI openni-dev) 39 | else() 40 | pkg_check_modules(PC_OPENNI QUIET openni-dev) 41 | endif() 42 | 43 | set(OPENNI_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER}) 44 | 45 | set(OPENNI_SUFFIX) 46 | if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) 47 | set(OPENNI_SUFFIX 64) 48 | endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) 49 | 50 | #add a hint so that it can find it without the pkg-config 51 | find_path(OPENNI_INCLUDE_DIR XnStatus.h 52 | HINTS ${PC_OPENNI_INCLUDEDIR} ${PC_OPENNI_INCLUDE_DIRS} /usr/include/openni /usr/include/ni "${OPENNI_ROOT}" "$ENV{OPENNI_ROOT}" 53 | PATHS "$ENV{OPEN_NI_INSTALL_PATH${OPENNI_SUFFIX}}/Include" 54 | PATH_SUFFIXES openni include Include) 55 | #add a hint so that it can find it without the pkg-config 56 | find_library(OPENNI_LIBRARY 57 | NAMES OpenNI${OPENNI_SUFFIX} 58 | HINTS ${PC_OPENNI_LIBDIR} ${PC_OPENNI_LIBRARY_DIRS} /usr/lib "${OPENNI_ROOT}" "$ENV{OPENNI_ROOT}" 59 | PATHS "$ENV{OPEN_NI_LIB${OPENNI_SUFFIX}}" 60 | PATH_SUFFIXES lib Lib Lib64) 61 | 62 | if(CMAKE_SYSTEM_NAME STREQUAL "Darwin") 63 | set(OPENNI_LIBRARIES ${OPENNI_LIBRARY} ${LIBUSB_1_LIBRARIES}) 64 | else() 65 | set(OPENNI_LIBRARIES ${OPENNI_LIBRARY}) 66 | endif() 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args(OpenNI DEFAULT_MSG OPENNI_LIBRARY OPENNI_INCLUDE_DIR) 70 | 71 | mark_as_advanced(OPENNI_LIBRARY OPENNI_INCLUDE_DIR) 72 | 73 | if(OPENNI_FOUND) 74 | # Add the include directories 75 | set(OPENNI_INCLUDE_DIRS ${OPENNI_INCLUDE_DIR}) 76 | message(STATUS "OpenNI found (include: ${OPENNI_INCLUDE_DIRS}, lib: ${OPENNI_LIBRARY})") 77 | endif(OPENNI_FOUND) 78 | 79 | -------------------------------------------------------------------------------- /cmake/Targets.cmake: -------------------------------------------------------------------------------- 1 | ################################################################################################ 2 | # short command to setup source group 3 | function(kf_source_group group) 4 | cmake_parse_arguments(VW_SOURCE_GROUP "" "" "GLOB" ${ARGN}) 5 | file(GLOB srcs ${VW_SOURCE_GROUP_GLOB}) 6 | #list(LENGTH ${srcs} ___size) 7 | #if (___size GREATER 0) 8 | source_group(${group} FILES ${srcs}) 9 | #endif() 10 | endfunction() 11 | 12 | 13 | ################################################################################################ 14 | # short command getting sources from standard directores 15 | macro(pickup_std_sources) 16 | kf_source_group("Include" GLOB "include/${module_name}/*.h*") 17 | kf_source_group("Include\\cuda" GLOB "include/${module_name}/cuda/*.h*") 18 | kf_source_group("Source" GLOB "src/*.cpp" "src/*.h*") 19 | kf_source_group("Source\\utils" GLOB "src/utils/*.cpp" "src/utils/*.h*") 20 | kf_source_group("Source\\cuda" GLOB "src/cuda/*.c*" "src/cuda/*.h*") 21 | FILE(GLOB_RECURSE sources include/${module_name}/*.h* src/*.cpp src/*.h* src/cuda/*.h* src/cuda/*.c*) 22 | endmacro() 23 | 24 | 25 | ################################################################################################ 26 | # short command for declaring includes from other modules 27 | macro(declare_deps_includes) 28 | foreach(__arg ${ARGN}) 29 | get_filename_component(___path "${CMAKE_SOURCE_DIR}/modules/${__arg}/include" ABSOLUTE) 30 | if (EXISTS ${___path}) 31 | include_directories(${___path}) 32 | endif() 33 | endforeach() 34 | 35 | unset(___path) 36 | unset(__arg) 37 | endmacro() 38 | 39 | 40 | ################################################################################################ 41 | # short command for setting defeault target properties 42 | function(default_properties target) 43 | set_target_properties(${target} PROPERTIES 44 | DEBUG_POSTFIX "d" 45 | ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" 46 | RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") 47 | 48 | if (NOT ${target} MATCHES "^test_") 49 | install(TARGETS ${the_target} RUNTIME DESTINATION ".") 50 | endif() 51 | endfunction() 52 | 53 | function(test_props target) 54 | #os_project_label(${target} "[test]") 55 | if(USE_PROJECT_FOLDERS) 56 | set_target_properties(${target} PROPERTIES FOLDER "Tests") 57 | endif() 58 | endfunction() 59 | 60 | function(app_props target) 61 | #os_project_label(${target} "[app]") 62 | if(USE_PROJECT_FOLDERS) 63 | set_target_properties(${target} PROPERTIES FOLDER "Apps") 64 | endif() 65 | endfunction() 66 | 67 | 68 | ################################################################################################ 69 | # short command for setting defeault target properties 70 | function(default_properties target) 71 | set_target_properties(${target} PROPERTIES 72 | DEBUG_POSTFIX "d" 73 | ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" 74 | RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") 75 | 76 | if (NOT ${target} MATCHES "^test_") 77 | install(TARGETS ${the_target} RUNTIME DESTINATION ".") 78 | endif() 79 | endfunction() 80 | 81 | 82 | ################################################################################################ 83 | # short command for adding library module 84 | macro(add_module_library name) 85 | set(module_name ${name}) 86 | pickup_std_sources() 87 | include_directories(include src src/cuda src/utils) 88 | 89 | set(__has_cuda OFF) 90 | check_cuda(__has_cuda) 91 | 92 | set(__lib_type STATIC) 93 | if (${ARGV1} MATCHES "SHARED|STATIC") 94 | set(__lib_type ${ARGV1}) 95 | endif() 96 | 97 | if (__has_cuda) 98 | cuda_add_library(${module_name} ${__lib_type} ${sources}) 99 | else() 100 | add_library(${module_name} ${__lib_type} ${sources}) 101 | endif() 102 | 103 | if(MSVC) 104 | set_target_properties(${module_name} PROPERTIES DEFINE_SYMBOL KFUSION_API_EXPORTS) 105 | else() 106 | add_definitions(-DKFUSION_API_EXPORTS) 107 | endif() 108 | 109 | default_properties(${module_name}) 110 | 111 | if(USE_PROJECT_FOLDERS) 112 | set_target_properties(${module_name} PROPERTIES FOLDER "Libraries") 113 | endif() 114 | 115 | set_target_properties(${module_name} PROPERTIES INSTALL_NAME_DIR lib) 116 | 117 | install(TARGETS ${module_name} 118 | RUNTIME DESTINATION bin COMPONENT main 119 | LIBRARY DESTINATION lib COMPONENT main 120 | ARCHIVE DESTINATION lib COMPONENT main) 121 | 122 | install(DIRECTORY include/ DESTINATION include/ FILES_MATCHING PATTERN "*.h*") 123 | endmacro() 124 | 125 | ################################################################################################ 126 | # short command for adding application module 127 | macro(add_application target sources) 128 | add_executable(${target} ${sources}) 129 | default_properties(${target}) 130 | app_props(${target}) 131 | endmacro() 132 | 133 | 134 | ################################################################################################ 135 | # short command for adding test target 136 | macro(add_test the_target) 137 | add_executable(${the_target} ${ARGN}) 138 | default_properties(${the_target}) 139 | test_props(${the_target}) 140 | endmacro() 141 | -------------------------------------------------------------------------------- /cmake/Utils.cmake: -------------------------------------------------------------------------------- 1 | ################################################################################################ 2 | # short alias for CMkae loggign 3 | function(dmsg) 4 | message(STATUS "<<${ARGN}") 5 | endfunction() 6 | 7 | ################################################################################################ 8 | # Command checking if current module has cuda souces to compile 9 | macro(check_cuda var) 10 | file(GLOB cuda src/cuda/*.cu) 11 | list(LENGTH cuda ___size) 12 | if (HAVE_CUDA AND ___size GREATER 0) 13 | set(${var} ON) 14 | else() 15 | set(${var} OFF) 16 | endif() 17 | unset(___size) 18 | unset(cuda) 19 | endmacro() 20 | 21 | ################################################################################################ 22 | # short command for adding library module 23 | macro(warnings_disable) 24 | if(NOT ENABLE_NOISY_WARNINGS) 25 | set(_flag_vars "") 26 | set(_msvc_warnings "") 27 | set(_gxx_warnings "") 28 | foreach(arg ${ARGN}) 29 | if(arg MATCHES "^CMAKE_") 30 | list(APPEND _flag_vars ${arg}) 31 | elseif(arg MATCHES "^/wd") 32 | list(APPEND _msvc_warnings ${arg}) 33 | elseif(arg MATCHES "^-W") 34 | list(APPEND _gxx_warnings ${arg}) 35 | endif() 36 | endforeach() 37 | if(MSVC AND _msvc_warnings AND _flag_vars) 38 | foreach(var ${_flag_vars}) 39 | foreach(warning ${_msvc_warnings}) 40 | set(${var} "${${var}} ${warning}") 41 | endforeach() 42 | endforeach() 43 | elseif(CV_COMPILER_IS_GNU AND _gxx_warnings AND _flag_vars) 44 | foreach(var ${_flag_vars}) 45 | foreach(warning ${_gxx_warnings}) 46 | if(NOT warning MATCHES "^-Wno-") 47 | string(REPLACE "${warning}" "" ${var} "${${var}}") 48 | string(REPLACE "-W" "-Wno-" warning "${warning}") 49 | endif() 50 | ocv_check_flag_support(${var} "${warning}" _varname) 51 | if(${_varname}) 52 | set(${var} "${${var}} ${warning}") 53 | endif() 54 | endforeach() 55 | endforeach() 56 | endif() 57 | unset(_flag_vars) 58 | unset(_msvc_warnings) 59 | unset(_gxx_warnings) 60 | endif(NOT ENABLE_NOISY_WARNINGS) 61 | endmacro() 62 | 63 | ################################################################################################ 64 | # Command for asstion options with some preconditions 65 | macro(kf_option variable description value) 66 | set(__value ${value}) 67 | set(__condition "") 68 | set(__varname "__value") 69 | foreach(arg ${ARGN}) 70 | if(arg STREQUAL "IF" OR arg STREQUAL "if") 71 | set(__varname "__condition") 72 | else() 73 | list(APPEND ${__varname} ${arg}) 74 | endif() 75 | endforeach() 76 | unset(__varname) 77 | if("${__condition}" STREQUAL "") 78 | set(__condition 2 GREATER 1) 79 | endif() 80 | 81 | if(${__condition}) 82 | if("${__value}" MATCHES ";") 83 | if(${__value}) 84 | option(${variable} "${description}" ON) 85 | else() 86 | option(${variable} "${description}" OFF) 87 | endif() 88 | elseif(DEFINED ${__value}) 89 | if(${__value}) 90 | option(${variable} "${description}" ON) 91 | else() 92 | option(${variable} "${description}" OFF) 93 | endif() 94 | else() 95 | option(${variable} "${description}" ${__value}) 96 | endif() 97 | else() 98 | unset(${variable} CACHE) 99 | endif() 100 | unset(__condition) 101 | unset(__value) 102 | endmacro() 103 | -------------------------------------------------------------------------------- /download_data.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | wget http://lgdv.cs.fau.de/uploads/publications/data/innmann2016deform/umbrella_data.zip 4 | mkdir -p data/umbrella/depth 5 | mkdir -p data/umbrella/color 6 | 7 | mv umbrella_data.zip data/umbrella 8 | cd data/umbrella 9 | unzip umbrella_data.zip 10 | rm *.txt 11 | mv *color*.png color/ 12 | mv *depth*.png depth/ 13 | rm umbrella_data.zip 14 | 15 | -------------------------------------------------------------------------------- /kfusion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "--ftz=true;--prec-div=false;--prec-sqrt=false") 2 | add_module_library(kfusion) 3 | if(OPENNI_FOUND) 4 | target_compile_definitions(kfusion PRIVATE OPENNI_FOUND=1) 5 | endif() 6 | 7 | message("CUDA_LIBRARIES:" ${CUDA_LIBRARIES}) 8 | message("CUDA_CUDART_LIBRARY:" ${CUDA_CUDART_LIBRARY}) 9 | message("CUDA_CUDA_LIBRARY:" ${CUDA_CUDA_LIBRARY}) 10 | find_package(Threads REQUIRED) 11 | 12 | target_link_libraries(kfusion 13 | ${OPENMESH_LIBRARIES} 14 | ${OPT_LIBRARIES} 15 | ${TERRA_LIBRARIES} 16 | Threads::Threads 17 | ${OpenCV_LIBS} 18 | ${OPENNI_LIBRARY} 19 | ${CERES_LIBRARIES}) 20 | 21 | target_include_directories(kfusion PUBLIC ${OPT_INCLUDE_DIRS}) 22 | target_compile_definitions(kfusion PUBLIC SOLVER_PATH=${CMAKE_SOURCE_DIR}/kfusion/solvers/) 23 | -------------------------------------------------------------------------------- /kfusion/include/io/capture.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace kfusion 8 | { 9 | class KF_EXPORTS OpenNISource 10 | { 11 | public: 12 | typedef kfusion::PixelRGB RGB24; 13 | 14 | enum { PROP_OPENNI_REGISTRATION_ON = 104 }; 15 | 16 | OpenNISource(); 17 | OpenNISource(int device); 18 | OpenNISource(const std::string& oni_filename, bool repeat = false); 19 | 20 | void open(int device); 21 | void open(const std::string& oni_filename, bool repeat = false); 22 | void release(); 23 | 24 | ~OpenNISource(); 25 | 26 | bool grab(cv::Mat &depth, cv::Mat &image); 27 | 28 | //parameters taken from camera/oni 29 | int shadow_value, no_sample_value; 30 | float depth_focal_length_VGA; 31 | float baseline; // mm 32 | double pixelSize; // mm 33 | unsigned short max_depth; // mm 34 | 35 | bool setRegistration (bool value = false); 36 | private: 37 | struct Impl; 38 | cv::Ptr impl_; 39 | void getParams (); 40 | 41 | }; 42 | } 43 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/cuda/device_memory.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace kfusion 7 | { 8 | namespace cuda 9 | { 10 | /** \brief Error handler. All GPU functions from this subsystem call the function to report an error. For internal use only */ 11 | KF_EXPORTS void error(const char *error_string, const char *file, const int line, const char *func = ""); 12 | 13 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 14 | /** \brief @b DeviceMemory class 15 | * 16 | * \note This is a BLOB container class with reference counting for GPU memory. 17 | * 18 | * \author Anatoly Baksheev 19 | */ 20 | 21 | class KF_EXPORTS DeviceMemory 22 | { 23 | public: 24 | /** \brief Empty constructor. */ 25 | DeviceMemory(); 26 | 27 | /** \brief Destructor. */ 28 | ~DeviceMemory(); 29 | 30 | /** \brief Allocates internal buffer in GPU memory 31 | * \param sizeBytes_arg: amount of memory to allocate 32 | * */ 33 | DeviceMemory(size_t sizeBytes_arg); 34 | 35 | /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case. 36 | * \param ptr_arg: pointer to buffer 37 | * \param sizeBytes_arg: buffer size 38 | * */ 39 | DeviceMemory(void *ptr_arg, size_t sizeBytes_arg); 40 | 41 | /** \brief Copy constructor. Just increments reference counter. */ 42 | DeviceMemory(const DeviceMemory& other_arg); 43 | 44 | /** \brief Assigment operator. Just increments reference counter. */ 45 | DeviceMemory& operator=(const DeviceMemory& other_arg); 46 | 47 | /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with new size. If new and old sizes are equal it does nothing. 48 | * \param sizeBytes_arg: buffer size 49 | * */ 50 | void create(size_t sizeBytes_arg); 51 | 52 | /** \brief Decrements reference counter and releases internal buffer if needed. */ 53 | void release(); 54 | 55 | /** \brief Performs data copying. If destination size differs it will be reallocated. 56 | * \param other_arg: destination container 57 | * */ 58 | void copyTo(DeviceMemory& other) const; 59 | 60 | /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough. 61 | * \param host_ptr_arg: pointer to buffer to upload 62 | * \param sizeBytes_arg: buffer size 63 | * */ 64 | void upload(const void *host_ptr_arg, size_t sizeBytes_arg); 65 | 66 | /** \brief Downloads data from internal buffer to CPU memory 67 | * \param host_ptr_arg: pointer to buffer to download 68 | * */ 69 | void download(void *host_ptr_arg) const; 70 | 71 | /** \brief Performs swap of data pointed with another device memory. 72 | * \param other: device memory to swap with 73 | * */ 74 | void swap(DeviceMemory& other_arg); 75 | 76 | /** \brief Returns pointer for internal buffer in GPU memory. */ 77 | template T* ptr(); 78 | 79 | /** \brief Returns constant pointer for internal buffer in GPU memory. */ 80 | template const T* ptr() const; 81 | 82 | /** \brief Conversion to PtrSz for passing to kernel functions. */ 83 | template operator PtrSz() const; 84 | 85 | /** \brief Returns true if unallocated otherwise false. */ 86 | bool empty() const; 87 | 88 | size_t sizeBytes() const; 89 | 90 | private: 91 | /** \brief Device pointer. */ 92 | void *data_; 93 | 94 | /** \brief Allocated size in bytes. */ 95 | size_t sizeBytes_; 96 | 97 | /** \brief Pointer to reference counter in CPU memory. */ 98 | int* refcount_; 99 | }; 100 | 101 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 102 | /** \brief @b DeviceMemory2D class 103 | * 104 | * \note This is a BLOB container class with reference counting for pitched GPU memory. 105 | * 106 | * \author Anatoly Baksheev 107 | */ 108 | 109 | class KF_EXPORTS DeviceMemory2D 110 | { 111 | public: 112 | /** \brief Empty constructor. */ 113 | DeviceMemory2D(); 114 | 115 | /** \brief Destructor. */ 116 | ~DeviceMemory2D(); 117 | 118 | /** \brief Allocates internal buffer in GPU memory 119 | * \param rows_arg: number of rows to allocate 120 | * \param colsBytes_arg: width of the buffer in bytes 121 | * */ 122 | DeviceMemory2D(int rows_arg, int colsBytes_arg); 123 | 124 | 125 | /** \brief Initializes with user allocated buffer. Reference counting is disabled in this case. 126 | * \param rows_arg: number of rows 127 | * \param colsBytes_arg: width of the buffer in bytes 128 | * \param data_arg: pointer to buffer 129 | * \param stepBytes_arg: stride between two consecutive rows in bytes 130 | * */ 131 | DeviceMemory2D(int rows_arg, int colsBytes_arg, void *data_arg, size_t step_arg); 132 | 133 | /** \brief Copy constructor. Just increments reference counter. */ 134 | DeviceMemory2D(const DeviceMemory2D& other_arg); 135 | 136 | /** \brief Assigment operator. Just increments reference counter. */ 137 | DeviceMemory2D& operator=(const DeviceMemory2D& other_arg); 138 | 139 | /** \brief Allocates internal buffer in GPU memory. If internal buffer was created before the function recreates it with new size. If new and old sizes are equal it does nothing. 140 | * \param ptr_arg: number of rows to allocate 141 | * \param sizeBytes_arg: width of the buffer in bytes 142 | * */ 143 | void create(int rows_arg, int colsBytes_arg); 144 | 145 | /** \brief Decrements reference counter and releases internal buffer if needed. */ 146 | void release(); 147 | 148 | /** \brief Performs data copying. If destination size differs it will be reallocated. 149 | * \param other_arg: destination container 150 | * */ 151 | void copyTo(DeviceMemory2D& other) const; 152 | 153 | /** \brief Uploads data to internal buffer in GPU memory. It calls create() inside to ensure that intenal buffer size is enough. 154 | * \param host_ptr_arg: pointer to host buffer to upload 155 | * \param host_step_arg: stride between two consecutive rows in bytes for host buffer 156 | * \param rows_arg: number of rows to upload 157 | * \param sizeBytes_arg: width of host buffer in bytes 158 | * */ 159 | void upload(const void *host_ptr_arg, size_t host_step_arg, int rows_arg, int colsBytes_arg); 160 | 161 | /** \brief Downloads data from internal buffer to CPU memory. User is resposible for correct host buffer size. 162 | * \param host_ptr_arg: pointer to host buffer to download 163 | * \param host_step_arg: stride between two consecutive rows in bytes for host buffer 164 | * */ 165 | void download(void *host_ptr_arg, size_t host_step_arg) const; 166 | 167 | /** \brief Performs swap of data pointed with another device memory. 168 | * \param other: device memory to swap with 169 | * */ 170 | void swap(DeviceMemory2D& other_arg); 171 | 172 | /** \brief Returns pointer to given row in internal buffer. 173 | * \param y_arg: row index 174 | * */ 175 | template T* ptr(int y_arg = 0); 176 | 177 | /** \brief Returns constant pointer to given row in internal buffer. 178 | * \param y_arg: row index 179 | * */ 180 | template const T* ptr(int y_arg = 0) const; 181 | 182 | /** \brief Conversion to PtrStep for passing to kernel functions. */ 183 | template operator PtrStep() const; 184 | 185 | /** \brief Conversion to PtrStepSz for passing to kernel functions. */ 186 | template operator PtrStepSz() const; 187 | 188 | /** \brief Returns true if unallocated otherwise false. */ 189 | bool empty() const; 190 | 191 | /** \brief Returns number of bytes in each row. */ 192 | int colsBytes() const; 193 | 194 | /** \brief Returns number of rows. */ 195 | int rows() const; 196 | 197 | /** \brief Returns stride between two consecutive rows in bytes for internal buffer. Step is stored always and everywhere in bytes!!! */ 198 | size_t step() const; 199 | private: 200 | /** \brief Device pointer. */ 201 | void *data_; 202 | 203 | /** \brief Stride between two consecutive rows in bytes for internal buffer. Step is stored always and everywhere in bytes!!! */ 204 | size_t step_; 205 | 206 | /** \brief Width of the buffer in bytes. */ 207 | int colsBytes_; 208 | 209 | /** \brief Number of rows. */ 210 | int rows_; 211 | 212 | /** \brief Pointer to reference counter in CPU memory. */ 213 | int* refcount_; 214 | }; 215 | } 216 | 217 | namespace device 218 | { 219 | using kfusion::cuda::DeviceMemory; 220 | using kfusion::cuda::DeviceMemory2D; 221 | } 222 | } 223 | 224 | ///////////////////// Inline implementations of DeviceMemory //////////////////////////////////////////// 225 | 226 | template inline T* kfusion::cuda::DeviceMemory::ptr() { return ( T*)data_; } 227 | template inline const T* kfusion::cuda::DeviceMemory::ptr() const { return (const T*)data_; } 228 | 229 | template inline kfusion::cuda::DeviceMemory::operator kfusion::cuda::PtrSz() const 230 | { 231 | PtrSz result; 232 | result.data = (U*)ptr(); 233 | result.size = sizeBytes_/sizeof(U); 234 | return result; 235 | } 236 | 237 | ///////////////////// Inline implementations of DeviceMemory2D //////////////////////////////////////////// 238 | 239 | template T* kfusion::cuda::DeviceMemory2D::ptr(int y_arg) { return ( T*)(( char*)data_ + y_arg * step_); } 240 | template const T* kfusion::cuda::DeviceMemory2D::ptr(int y_arg) const { return (const T*)((const char*)data_ + y_arg * step_); } 241 | 242 | template kfusion::cuda::DeviceMemory2D::operator kfusion::cuda::PtrStep() const 243 | { 244 | PtrStep result; 245 | result.data = (U*)ptr(); 246 | result.step = step_; 247 | return result; 248 | } 249 | 250 | template kfusion::cuda::DeviceMemory2D::operator kfusion::cuda::PtrStepSz() const 251 | { 252 | PtrStepSz result; 253 | result.data = (U*)ptr(); 254 | result.step = step_; 255 | result.cols = colsBytes_/sizeof(U); 256 | result.rows = rows_; 257 | return result; 258 | } 259 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/cuda/imgproc.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace kfusion 6 | { 7 | namespace cuda 8 | { 9 | KF_EXPORTS void depthBilateralFilter(const Depth& in, Depth& out, int ksz, float sigma_spatial, float sigma_depth); 10 | 11 | KF_EXPORTS void depthTruncation(Depth& depth, float threshold); 12 | 13 | KF_EXPORTS void depthBuildPyramid(const Depth& depth, Depth& pyramid, float sigma_depth); 14 | 15 | KF_EXPORTS void computeNormalsAndMaskDepth(const Intr& intr, Depth& depth, Normals& normals); 16 | 17 | KF_EXPORTS void computePointNormals(const Intr& intr, const Depth& depth, Cloud& points, Normals& normals); 18 | 19 | KF_EXPORTS void computeDists(const Depth& depth, Dists& dists, const Intr& intr); 20 | 21 | KF_EXPORTS void cloudToDepth(const Cloud& cloud, Depth& depth); 22 | 23 | KF_EXPORTS void resizeDepthNormals(const Depth& depth, const Normals& normals, Depth& depth_out, Normals& normals_out); 24 | 25 | KF_EXPORTS void resizePointsNormals(const Cloud& points, const Normals& normals, Cloud& points_out, Normals& normals_out); 26 | 27 | KF_EXPORTS void waitAllDefaultStream(); 28 | 29 | KF_EXPORTS void renderTangentColors(const Normals& normals, Image& image); 30 | 31 | KF_EXPORTS void renderImage(const Depth& depth, const Normals& normals, const Intr& intr, const Vec3f& light_pose, Image& image); 32 | 33 | KF_EXPORTS void renderImage(const Cloud& points, const Normals& normals, const Intr& intr, const Vec3f& light_pose, Image& image); 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/cuda/kernel_containers.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined(__CUDACC__) 4 | #define __kf_hdevice__ __host__ __device__ __forceinline__ 5 | #define __kf_device__ __device__ __forceinline__ 6 | #else 7 | #define __kf_hdevice__ 8 | #define __kf_device__ 9 | #endif 10 | 11 | #include 12 | 13 | namespace kfusion 14 | { 15 | namespace cuda 16 | { 17 | template struct DevPtr 18 | { 19 | typedef T elem_type; 20 | const static size_t elem_size = sizeof(elem_type); 21 | 22 | T* data; 23 | 24 | __kf_hdevice__ DevPtr() : data(0) {} 25 | __kf_hdevice__ DevPtr(T* data_arg) : data(data_arg) {} 26 | 27 | __kf_hdevice__ size_t elemSize() const { return elem_size; } 28 | __kf_hdevice__ operator T*() { return data; } 29 | __kf_hdevice__ operator const T*() const { return data; } 30 | }; 31 | 32 | template struct PtrSz : public DevPtr 33 | { 34 | __kf_hdevice__ PtrSz() : size(0) {} 35 | __kf_hdevice__ PtrSz(T* data_arg, size_t size_arg) : DevPtr(data_arg), size(size_arg) {} 36 | 37 | size_t size; 38 | }; 39 | 40 | template struct PtrStep : public DevPtr 41 | { 42 | __kf_hdevice__ PtrStep() : step(0) {} 43 | __kf_hdevice__ PtrStep(T* data_arg, size_t step_arg) : DevPtr(data_arg), step(step_arg) {} 44 | 45 | /** \brief stride between two consecutive rows in bytes. Step is stored always and everywhere in bytes!!! */ 46 | size_t step; 47 | 48 | __kf_hdevice__ T* ptr(int y = 0) { return ( T*)( ( char*)DevPtr::data + y * step); } 49 | __kf_hdevice__ const T* ptr(int y = 0) const { return (const T*)( (const char*)DevPtr::data + y * step); } 50 | 51 | __kf_hdevice__ T& operator()(int y, int x) { return ptr(y)[x]; } 52 | __kf_hdevice__ const T& operator()(int y, int x) const { return ptr(y)[x]; } 53 | }; 54 | 55 | template struct PtrStepSz : public PtrStep 56 | { 57 | __kf_hdevice__ PtrStepSz() : cols(0), rows(0) {} 58 | __kf_hdevice__ PtrStepSz(int rows_arg, int cols_arg, T* data_arg, size_t step_arg) 59 | : PtrStep(data_arg, step_arg), cols(cols_arg), rows(rows_arg) {} 60 | 61 | int cols; 62 | int rows; 63 | }; 64 | } 65 | 66 | namespace device 67 | { 68 | using kfusion::cuda::PtrSz; 69 | using kfusion::cuda::PtrStep; 70 | using kfusion::cuda::PtrStepSz; 71 | } 72 | } 73 | 74 | namespace kf = kfusion; 75 | 76 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/cuda/projective_icp.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace kfusion 6 | { 7 | namespace cuda 8 | { 9 | class ProjectiveICP 10 | { 11 | public: 12 | enum { MAX_PYRAMID_LEVELS = 4 }; 13 | 14 | typedef std::vector DepthPyr; 15 | typedef std::vector PointsPyr; 16 | typedef std::vector NormalsPyr; 17 | 18 | ProjectiveICP(); 19 | virtual ~ProjectiveICP(); 20 | 21 | float getDistThreshold() const; 22 | void setDistThreshold(float distance); 23 | 24 | float getAngleThreshold() const; 25 | void setAngleThreshold(float angle); 26 | 27 | void setIterationsNum(const std::vector& iters); 28 | int getUsedLevelsNum() const; 29 | 30 | virtual bool estimateTransform(Affine3f& affine, const Intr& intr, const Frame& curr, const Frame& prev); 31 | 32 | /** The function takes masked depth, i.e. it assumes for performance reasons that 33 | * "if depth(y,x) is not zero, then normals(y,x) surely is not qnan" */ 34 | virtual bool estimateTransform(Affine3f& affine, const Intr& intr, const DepthPyr& dcurr, const NormalsPyr ncurr, const DepthPyr dprev, const NormalsPyr nprev); 35 | virtual bool estimateTransform(Affine3f& affine, const Intr& intr, const PointsPyr& vcurr, const NormalsPyr ncurr, const PointsPyr vprev, const NormalsPyr nprev); 36 | 37 | //static Vec3f rodrigues2(const Mat3f& matrix); 38 | private: 39 | std::vector iters_; 40 | float angle_thres_; 41 | float dist_thres_; 42 | DeviceArray2D buffer_; 43 | 44 | struct StreamHelper; 45 | cv::Ptr shelp_; 46 | }; 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/cuda/tsdf_volume.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace kfusion 7 | { 8 | class WarpField; 9 | namespace cuda 10 | { 11 | class KF_EXPORTS TsdfVolume 12 | { 13 | public: 14 | TsdfVolume(const cv::Vec3i& dims); 15 | virtual ~TsdfVolume(); 16 | 17 | void create(const Vec3i& dims); 18 | 19 | Vec3i getDims() const; 20 | Vec3f getVoxelSize() const; 21 | 22 | const CudaData data() const; 23 | CudaData data(); 24 | 25 | cv::Mat get_cloud_host() const; 26 | cv::Mat get_normal_host() const; 27 | 28 | cv::Mat* get_cloud_host_ptr() const; 29 | cv::Mat* get_normal_host_ptr() const; 30 | 31 | Vec3f getSize() const; 32 | void setSize(const Vec3f& size); 33 | 34 | float getTruncDist() const; 35 | void setTruncDist(float distance); 36 | 37 | int getMaxWeight() const; 38 | void setMaxWeight(int weight); 39 | 40 | Affine3f getPose() const; 41 | void setPose(const Affine3f& pose); 42 | 43 | float getRaycastStepFactor() const; 44 | void setRaycastStepFactor(float factor); 45 | 46 | float getGradientDeltaFactor() const; 47 | void setGradientDeltaFactor(float factor); 48 | 49 | Vec3i getGridOrigin() const; 50 | void setGridOrigin(const Vec3i& origin); 51 | 52 | std::vector psdf(const std::vector& warped, Dists& depth_img, const Intr& intr); 53 | // float psdf(const std::vector& warped, Dists& dists, const Intr& intr); 54 | float weighting(const std::vector& dist_sqr, int k) const; 55 | void surface_fusion(const WarpField& warp_field, 56 | std::vector warped, 57 | std::vector canonical, 58 | cuda::Depth &depth, 59 | const Affine3f& camera_pose, 60 | const Intr& intr); 61 | 62 | virtual void clear(); 63 | virtual void applyAffine(const Affine3f& affine); 64 | virtual void integrate(const Dists& dists, const Affine3f& camera_pose, const Intr& intr); 65 | virtual void raycast(const Affine3f& camera_pose, const Intr& intr, Depth& depth, Normals& normals); 66 | virtual void raycast(const Affine3f& camera_pose, const Intr& intr, Cloud& points, Normals& normals); 67 | 68 | void swap(CudaData& data); 69 | 70 | DeviceArray fetchCloud(DeviceArray& cloud_buffer) const; 71 | void fetchNormals(const DeviceArray& cloud, DeviceArray& normals) const; 72 | void compute_points(); 73 | void compute_normals(); 74 | 75 | 76 | private: 77 | CudaData data_; 78 | // need to make this cv::Ptr 79 | cuda::DeviceArray *cloud_buffer_; 80 | cuda::DeviceArray *cloud_; 81 | cuda::DeviceArray *normal_buffer_; 82 | cv::Mat *cloud_host_; 83 | cv::Mat *normal_host_; 84 | 85 | float trunc_dist_; 86 | float max_weight_; 87 | Vec3i dims_; 88 | Vec3f size_; 89 | Affine3f pose_; 90 | float gradient_delta_factor_; 91 | float raycast_step_factor_; 92 | // TODO: remember to add entry when adding a new node 93 | struct Entry 94 | { 95 | float tsdf_value; 96 | float tsdf_weight; 97 | }; 98 | 99 | std::vector tsdf_entries_; 100 | }; 101 | } 102 | } 103 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/exports.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if (defined WIN32 || defined _WIN32 || defined WINCE) && defined KFUSION_API_EXPORTS 4 | #define KF_EXPORTS __declspec(dllexport) 5 | #else 6 | #define KF_EXPORTS 7 | #endif 8 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/kinfu.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "warp_field_optimiser.hpp" 12 | 13 | namespace kfusion 14 | { 15 | struct KF_EXPORTS KinFuParams 16 | { 17 | static KinFuParams default_params(); 18 | static KinFuParams default_params_dynamicfusion(); 19 | 20 | int cols; //pixels 21 | int rows; //pixels 22 | 23 | Intr intr; //Camera parameters 24 | 25 | Vec3i volume_dims; //number of voxels 26 | Vec3f volume_size; //meters 27 | Affine3f volume_pose; //meters, inital pose 28 | 29 | float bilateral_sigma_depth; //meters 30 | float bilateral_sigma_spatial; //pixels 31 | int bilateral_kernel_size; //pixels 32 | 33 | float icp_truncate_depth_dist; //meters 34 | float icp_dist_thres; //meters 35 | float icp_angle_thres; //radians 36 | std::vector icp_iter_num; //iterations for level index 0,1,..,3 37 | 38 | float tsdf_min_camera_movement; //meters, integrate only if exceedes 39 | float tsdf_trunc_dist; //meters; 40 | int tsdf_max_weight; //frames 41 | 42 | float raycast_step_factor; // in voxel sizes 43 | float gradient_delta_factor; // in voxel sizes 44 | 45 | Vec3f light_pose; //meters 46 | 47 | }; 48 | 49 | class KF_EXPORTS KinFu 50 | { 51 | public: 52 | typedef cv::Ptr Ptr; 53 | 54 | KinFu(const KinFuParams& params); 55 | 56 | const KinFuParams& params() const; 57 | KinFuParams& params(); 58 | 59 | const cuda::TsdfVolume& tsdf() const; 60 | cuda::TsdfVolume& tsdf(); 61 | 62 | const cuda::ProjectiveICP& icp() const; 63 | cuda::ProjectiveICP& icp(); 64 | 65 | const WarpField& getWarp() const; 66 | WarpField& getWarp(); 67 | 68 | void reset(); 69 | 70 | bool operator()(const cuda::Depth& depth, const cuda::Image& image = cuda::Image()); 71 | 72 | void renderImage(cuda::Image& image, int flags = 0); 73 | void dynamicfusion(cuda::Depth& depth, cuda::Cloud live_frame, cuda::Normals current_normals); 74 | void renderImage(cuda::Image& image, const Affine3f& pose, int flags = 0); 75 | 76 | Affine3f getCameraPose (int time = -1) const; 77 | private: 78 | void allocate_buffers(); 79 | 80 | int frame_counter_; 81 | KinFuParams params_; 82 | 83 | std::vector poses_; 84 | 85 | cuda::Dists dists_; 86 | cuda::Frame curr_, prev_, first_; 87 | 88 | cuda::Cloud points_; 89 | cuda::Normals normals_; 90 | cuda::Depth depths_; 91 | 92 | cv::Ptr volume_; 93 | cv::Ptr icp_; 94 | cv::Ptr warp_; 95 | std::vector, utils::DualQuaternion>> edges_; 96 | cv::Ptr optimiser_; 97 | }; 98 | } 99 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/optimisation.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KFUSION_OPTIMISATION_H 2 | #define KFUSION_OPTIMISATION_H 3 | #include "ceres/ceres.h" 4 | #include "ceres/rotation.h" 5 | #include 6 | 7 | struct DynamicFusionDataEnergy 8 | { 9 | DynamicFusionDataEnergy(const cv::Vec3f& live_vertex, 10 | const cv::Vec3f& live_normal, 11 | const cv::Vec3f& canonical_vertex, 12 | const cv::Vec3f& canonical_normal, 13 | kfusion::WarpField *warpField, 14 | const float weights[KNN_NEIGHBOURS], 15 | const unsigned long knn_indices[KNN_NEIGHBOURS]) 16 | : live_vertex_(live_vertex), 17 | live_normal_(live_normal), 18 | canonical_vertex_(canonical_vertex), 19 | canonical_normal_(canonical_normal), 20 | warpField_(warpField) 21 | { 22 | weights_ = new float[KNN_NEIGHBOURS]; 23 | knn_indices_ = new unsigned long[KNN_NEIGHBOURS]; 24 | for(int i = 0; i < KNN_NEIGHBOURS; i++) 25 | { 26 | weights_[i] = weights[i]; 27 | knn_indices_[i] = knn_indices[i]; 28 | } 29 | } 30 | ~DynamicFusionDataEnergy() 31 | { 32 | delete[] weights_; 33 | delete[] knn_indices_; 34 | } 35 | template 36 | bool operator()(T const * const * epsilon_, T* residuals) const 37 | { 38 | auto nodes = warpField_->getNodes(); 39 | 40 | T total_translation[3] = {T(0), T(0), T(0)}; 41 | float total_translation_float[3] = {0, 0, 0}; 42 | 43 | for(int i = 0; i < KNN_NEIGHBOURS; i++) 44 | { 45 | auto quat = nodes->at(knn_indices_[i]).transform; 46 | cv::Vec3f vert; 47 | quat.getTranslation(vert); 48 | 49 | T eps_t[3] = {epsilon_[i][3], epsilon_[i][4], epsilon_[i][5]}; 50 | 51 | float temp[3]; 52 | quat.getTranslation(temp[0], temp[1], temp[2]); 53 | 54 | // total_translation[0] += (T(temp[0]) + eps_t[0]); 55 | // total_translation[1] += (T(temp[1]) + eps_t[1]); 56 | // total_translation[2] += (T(temp[2]) + eps_t[2]); 57 | // 58 | total_translation[0] += (T(temp[0]) + eps_t[0]) * T(weights_[i]); 59 | total_translation[1] += (T(temp[1]) + eps_t[1]) * T(weights_[i]); 60 | total_translation[2] += (T(temp[2]) + eps_t[2]) * T(weights_[i]); 61 | 62 | } 63 | 64 | residuals[0] = T(live_vertex_[0] - canonical_vertex_[0]) - total_translation[0]; 65 | residuals[1] = T(live_vertex_[1] - canonical_vertex_[1]) - total_translation[1]; 66 | residuals[2] = T(live_vertex_[2] - canonical_vertex_[2]) - total_translation[2]; 67 | 68 | return true; 69 | } 70 | 71 | /** 72 | * Tukey loss function as described in http://web.as.uky.edu/statistics/users/pbreheny/764-F11/notes/12-1.pdf 73 | * \param x 74 | * \param c 75 | * \return 76 | * 77 | * \note 78 | * The value c = 4.685 is usually used for this loss function, and 79 | * it provides an asymptotic efficiency 95% that of linear 80 | * regression for the normal distribution 81 | * 82 | * In the paper, a value of 0.01 is suggested for c 83 | */ 84 | template 85 | T tukeyPenalty(T x, T c = T(0.01)) const 86 | { 87 | return ceres::abs(x) <= c ? x * ceres::pow((T(1.0) - (x * x) / (c * c)), 2) : T(0.0); 88 | } 89 | 90 | // Factory to hide the construction of the CostFunction object from 91 | // the client code. 92 | // TODO: this will only have one residual at the end, remember to change 93 | static ceres::CostFunction* Create(const cv::Vec3f& live_vertex, 94 | const cv::Vec3f& live_normal, 95 | const cv::Vec3f& canonical_vertex, 96 | const cv::Vec3f& canonical_normal, 97 | kfusion::WarpField* warpField, 98 | const float weights[KNN_NEIGHBOURS], 99 | const unsigned long ret_index[KNN_NEIGHBOURS]) 100 | { 101 | auto cost_function = new ceres::DynamicAutoDiffCostFunction( 102 | new DynamicFusionDataEnergy(live_vertex, 103 | live_normal, 104 | canonical_vertex, 105 | canonical_normal, 106 | warpField, 107 | weights, 108 | ret_index)); 109 | for(int i=0; i < KNN_NEIGHBOURS; i++) 110 | cost_function->AddParameterBlock(6); 111 | cost_function->SetNumResiduals(3); 112 | return cost_function; 113 | } 114 | const cv::Vec3f live_vertex_; 115 | const cv::Vec3f live_normal_; 116 | const cv::Vec3f canonical_vertex_; 117 | const cv::Vec3f canonical_normal_; 118 | 119 | float *weights_; 120 | unsigned long *knn_indices_; 121 | 122 | kfusion::WarpField *warpField_; 123 | }; 124 | 125 | struct DynamicFusionRegEnergy 126 | { 127 | DynamicFusionRegEnergy(){}; 128 | ~DynamicFusionRegEnergy(){}; 129 | template 130 | bool operator()(T const * const * epsilon_, T* residuals) const 131 | { 132 | return true; 133 | } 134 | 135 | /** 136 | * Huber penalty function, implemented as described in https://en.wikipedia.org/wiki/Huber_loss 137 | * In the paper, a value of 0.0001 is suggested for delta. 138 | * \param a 139 | * \param delta 140 | * \return 141 | */ 142 | template 143 | T huberPenalty(T a, T delta = 0.0001) const 144 | { 145 | return ceres::abs(a) <= delta ? a * a / 2 : delta * ceres::abs(a) - delta * delta / 2; 146 | } 147 | 148 | static ceres::CostFunction* Create() 149 | { 150 | auto cost_function = new ceres::DynamicAutoDiffCostFunction( 151 | new DynamicFusionRegEnergy()); 152 | for(int i=0; i < KNN_NEIGHBOURS; i++) 153 | cost_function->AddParameterBlock(6); 154 | cost_function->SetNumResiduals(3); 155 | return cost_function; 156 | } 157 | }; 158 | 159 | class WarpProblem { 160 | public: 161 | explicit WarpProblem(kfusion::WarpField *warp) : warpField_(warp) 162 | { 163 | parameters_ = new double[warpField_->getNodes()->size() * 6]; 164 | for(int i = 0; i < warp->getNodes()->size() * 6; i+=6) 165 | { 166 | auto transform = warp->getNodes()->at(i / 6).transform; 167 | 168 | float x,y,z; 169 | 170 | transform.getTranslation(x,y,z); 171 | parameters_[i] = x; 172 | parameters_[i+1] = y; 173 | parameters_[i+2] = z; 174 | 175 | transform.getRotation().getRodrigues(x,y,z); 176 | parameters_[i+3] = x; 177 | parameters_[i+4] = y; 178 | parameters_[i+5] = z; 179 | } 180 | }; 181 | 182 | ~WarpProblem() { 183 | delete parameters_; 184 | } 185 | std::vector mutable_epsilon(const unsigned long *index_list) const 186 | { 187 | std::vector mutable_epsilon_(KNN_NEIGHBOURS); 188 | for(int i = 0; i < KNN_NEIGHBOURS; i++) 189 | mutable_epsilon_[i] = &(parameters_[index_list[i] * 6]); // Blocks of 6 190 | return mutable_epsilon_; 191 | } 192 | 193 | std::vector mutable_epsilon(const std::vector& index_list) const 194 | { 195 | std::vector mutable_epsilon_(KNN_NEIGHBOURS); 196 | for(int i = 0; i < KNN_NEIGHBOURS; i++) 197 | mutable_epsilon_[i] = &(parameters_[index_list[i] * 6]); // Blocks of 6 198 | return mutable_epsilon_; 199 | } 200 | double *mutable_params() 201 | { 202 | return parameters_; 203 | } 204 | 205 | const double *params() const 206 | { 207 | return parameters_; 208 | } 209 | 210 | 211 | void updateWarp() 212 | { 213 | for(int i = 0; i < warpField_->getNodes()->size() * 6; i+=6) 214 | { 215 | warpField_->getNodes()->at(i / 6).transform.encodeRotation(parameters_[i],parameters_[i+1],parameters_[i+2]); 216 | warpField_->getNodes()->at(i / 6).transform.encodeTranslation(parameters_[i+3],parameters_[i+4],parameters_[i+5]); 217 | } 218 | } 219 | 220 | 221 | private: 222 | double *parameters_; 223 | kfusion::WarpField *warpField_; 224 | }; 225 | 226 | #endif //KFUSION_OPTIMISATION_H 227 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/types.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | struct CUevent_st; 10 | 11 | namespace kfusion 12 | { 13 | typedef cv::Matx33f Mat3f; 14 | typedef cv::Matx44f Mat4f; 15 | typedef cv::Vec3f Vec3f; 16 | typedef cv::Vec4f Vec4f; 17 | typedef cv::Vec3i Vec3i; 18 | typedef cv::Affine3f Affine3f; 19 | 20 | struct KF_EXPORTS Intr 21 | { 22 | float fx, fy, cx, cy; 23 | 24 | Intr (); 25 | Intr (float fx, float fy, float cx, float cy); 26 | Intr operator()(int level_index) const; 27 | }; 28 | 29 | KF_EXPORTS std::ostream& operator << (std::ostream& os, const Intr& intr); 30 | 31 | struct Point 32 | { 33 | union 34 | { 35 | float data[4]; 36 | struct { float x, y, z; }; 37 | }; 38 | }; 39 | 40 | typedef Point Normal; 41 | 42 | struct RGB 43 | { 44 | union 45 | { 46 | struct { unsigned char b, g, r; }; 47 | int bgra; 48 | }; 49 | }; 50 | 51 | struct PixelRGB 52 | { 53 | unsigned char r, g, b; 54 | }; 55 | 56 | namespace cuda 57 | { 58 | typedef cuda::DeviceMemory CudaData; 59 | typedef cuda::DeviceArray2D Depth; 60 | typedef cuda::DeviceArray2D Dists; 61 | typedef cuda::DeviceArray2D Image; 62 | typedef cuda::DeviceArray2D Normals; 63 | typedef cuda::DeviceArray2D Cloud; 64 | 65 | struct Frame 66 | { 67 | bool use_points; 68 | 69 | std::vector depth_pyr; 70 | std::vector points_pyr; 71 | std::vector normals_pyr; 72 | }; 73 | } 74 | 75 | inline float deg2rad (float alpha) { return alpha * 0.017453293f; } 76 | 77 | struct KF_EXPORTS ScopeTime 78 | { 79 | const char* name; 80 | double start; 81 | ScopeTime(const char *name); 82 | ~ScopeTime(); 83 | }; 84 | 85 | struct KF_EXPORTS SampledScopeTime 86 | { 87 | public: 88 | enum { EACH = 33 }; 89 | SampledScopeTime(double& time_ms); 90 | ~SampledScopeTime(); 91 | private: 92 | double getTime(); 93 | SampledScopeTime(const SampledScopeTime&); 94 | SampledScopeTime& operator=(const SampledScopeTime&); 95 | 96 | double& time_ms_; 97 | double start; 98 | }; 99 | 100 | } 101 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/warp_field.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KFUSION_WARP_FIELD_HPP 2 | #define KFUSION_WARP_FIELD_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #define KNN_NEIGHBOURS 8 11 | namespace kfusion 12 | { 13 | typedef nanoflann::KDTreeSingleIndexAdaptor< 14 | nanoflann::L2_Simple_Adaptor, 15 | utils::PointCloud, 16 | 3 /* dim */ 17 | > kd_tree_t; 18 | 19 | 20 | /*! 21 | * \struct node 22 | * \brief A node of the warp field 23 | * \details The state of the warp field Wt at time t is defined by the values of a set of n 24 | * deformation nodes Nt_warp = {dg_v, dg_w, dg_se3}_t. Here, this is represented as follows 25 | * 26 | * \var node::vertex 27 | * Position of the vertex in space. This will be used when computing KNN for warping points. 28 | * 29 | * \var node::transform 30 | * Transformation for each vertex to warp it into the live frame, equivalent to dg_se in the paper. 31 | * 32 | * \var node::weight 33 | * Equivalent to dg_w 34 | */ 35 | struct deformation_node 36 | { 37 | Vec3f vertex; 38 | kfusion::utils::DualQuaternion transform; 39 | float weight = 0; 40 | }; 41 | class WarpField 42 | { 43 | public: 44 | WarpField(); 45 | ~WarpField(); 46 | 47 | void init(const cv::Mat& first_frame); 48 | void init(const std::vector& first_frame); 49 | void energy(const cuda::Cloud &frame, 50 | const cuda::Normals &normals, 51 | const Affine3f &pose, 52 | const cuda::TsdfVolume &tsdfVolume, 53 | const std::vector, 54 | kfusion::utils::DualQuaternion>> &edges 55 | ); 56 | 57 | void energy_data(const std::vector &canonical_vertices, 58 | const std::vector &canonical_normals, 59 | const std::vector &live_vertices, 60 | const std::vector &live_normals); 61 | void energy_reg(const std::vector, 62 | kfusion::utils::DualQuaternion>> &edges); 63 | 64 | 65 | void warp(std::vector& points, std::vector& normals) const; 66 | 67 | utils::DualQuaternion DQB(const Vec3f& vertex) const; 68 | 69 | void getWeightsAndUpdateKNN(const Vec3f& vertex, float weights[KNN_NEIGHBOURS]) const; 70 | 71 | float weighting(float squared_dist, float weight) const; 72 | void KNN(Vec3f point) const; 73 | 74 | void clear(); 75 | 76 | const std::vector* getNodes() const; 77 | std::vector* getNodes(); 78 | const cv::Mat getNodesAsMat() const; 79 | void setWarpToLive(const Affine3f &pose); 80 | std::vector* getDistSquared() const; 81 | std::vector* getRetIndex() const; 82 | void buildKDTree(); 83 | 84 | private: 85 | std::vector* nodes_; 86 | kd_tree_t* index_; 87 | Affine3f warp_to_live_; 88 | }; 89 | } 90 | #endif //KFUSION_WARP_FIELD_HPP 91 | -------------------------------------------------------------------------------- /kfusion/include/kfusion/warp_field_optimiser.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KFUSION_WARP_FIELD_OPTIMISER_H 2 | #define KFUSION_WARP_FIELD_OPTIMISER_H 3 | 4 | #include 5 | #include "warp_field.hpp" 6 | 7 | namespace kfusion{ 8 | class WarpFieldOptimiser 9 | { 10 | public: 11 | WarpFieldOptimiser(WarpField *warp, CombinedSolver *solver); 12 | WarpFieldOptimiser(WarpField *warp, CombinedSolverParameters params); 13 | ~WarpFieldOptimiser(){}; 14 | void optimiseWarpData(const std::vector &canonical_vertices, 15 | const std::vector &canonical_normals, 16 | const std::vector &live_vertices, 17 | const std::vector &live_normals); 18 | private: 19 | WarpField *warp_; 20 | CombinedSolver *solver_; 21 | }; 22 | } 23 | #endif //KFUSION_WARP_FIELD_OPTIMISER_H 24 | -------------------------------------------------------------------------------- /kfusion/include/opt/CUDATimer.h: -------------------------------------------------------------------------------- 1 | #ifndef CUDATimer_h 2 | #define CUDATimer_h 3 | #include 4 | #include 5 | #include 6 | struct TimingInfo { 7 | cudaEvent_t startEvent; 8 | cudaEvent_t endEvent; 9 | float duration; 10 | std::string eventName; 11 | }; 12 | 13 | /** Copied wholesale from mLib, so nvcc doesn't choke. */ 14 | template 15 | int findFirstIndex(const std::vector &collection, const T &value) 16 | { 17 | int index = 0; 18 | for (const auto &element : collection) 19 | { 20 | if (element == value) 21 | return index; 22 | index++; 23 | } 24 | return -1; 25 | } 26 | 27 | struct CUDATimer { 28 | std::vector timingEvents; 29 | int currentIteration; 30 | 31 | CUDATimer() : currentIteration(0) { 32 | TimingInfo timingInfo; 33 | cudaEventCreate(&timingInfo.startEvent); 34 | cudaEventCreate(&timingInfo.endEvent); 35 | cudaEventRecord(timingInfo.startEvent); 36 | timingInfo.eventName = "overall"; 37 | timingEvents.push_back(timingInfo); 38 | } 39 | void nextIteration() { 40 | ++currentIteration; 41 | } 42 | 43 | void reset() { 44 | currentIteration = 0; 45 | timingEvents.clear(); 46 | } 47 | 48 | void startEvent(const std::string& name) { 49 | TimingInfo timingInfo; 50 | cudaEventCreate(&timingInfo.startEvent); 51 | cudaEventCreate(&timingInfo.endEvent); 52 | cudaEventRecord(timingInfo.startEvent); 53 | timingInfo.eventName = name; 54 | timingEvents.push_back(timingInfo); 55 | } 56 | 57 | void endEvent() { 58 | TimingInfo& timingInfo = timingEvents[timingEvents.size() - 1]; 59 | cudaEventRecord(timingInfo.endEvent, 0); 60 | } 61 | 62 | void evaluate() { 63 | cudaEventRecord(timingEvents[0].endEvent); 64 | std::vector aggregateTimingNames; 65 | std::vector aggregateTimes; 66 | std::vector aggregateCounts; 67 | for (int i = 0; i < timingEvents.size(); ++i) { 68 | TimingInfo& eventInfo = timingEvents[i]; 69 | cudaEventSynchronize(eventInfo.endEvent); 70 | cudaEventElapsedTime(&eventInfo.duration, eventInfo.startEvent, eventInfo.endEvent); 71 | int index = findFirstIndex(aggregateTimingNames, eventInfo.eventName); 72 | if (index < 0) { 73 | aggregateTimingNames.push_back(eventInfo.eventName); 74 | aggregateTimes.push_back(eventInfo.duration); 75 | aggregateCounts.push_back(1); 76 | } else { 77 | aggregateTimes[index] = aggregateTimes[index] + eventInfo.duration; 78 | aggregateCounts[index] = aggregateCounts[index] + 1; 79 | } 80 | } 81 | printf("------------------------------------------------------------\n"); 82 | printf(" Kernel | Count | Total | Average \n"); 83 | printf("--------------------------+----------+-----------+----------\n"); 84 | for (int i = 0; i < aggregateTimingNames.size(); ++i) { 85 | printf("--------------------------+----------+-----------+----------\n"); 86 | printf(" %-24s | %4d | %8.3fms| %7.4fms\n", aggregateTimingNames[i].c_str(), aggregateCounts[i], aggregateTimes[i], aggregateTimes[i] / aggregateCounts[i]); 87 | } 88 | printf("------------------------------------------------------------\n"); 89 | } 90 | }; 91 | 92 | #endif -------------------------------------------------------------------------------- /kfusion/include/opt/CombinedSolver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "mLibInclude.h" 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | class CombinedSolver : public CombinedSolverBase 16 | { 17 | 18 | public: 19 | CombinedSolver(kfusion::WarpField *warpField, CombinedSolverParameters params) 20 | { 21 | m_combinedSolverParameters = params; 22 | m_warp = warpField; 23 | } 24 | 25 | void initializeProblemInstance(const std::vector &canonical_vertices, 26 | const std::vector &canonical_normals, 27 | const std::vector &live_vertices, 28 | const std::vector &live_normals) 29 | { 30 | m_canonicalVerticesOpenCV = canonical_vertices; 31 | m_canonicalNormalsOpenCV = canonical_normals; 32 | m_liveVerticesOpenCV = live_vertices; 33 | m_liveNormalsOpenCV = live_normals; 34 | 35 | 36 | unsigned int D = m_warp->getNodes()->size(); 37 | unsigned int N = canonical_vertices.size(); 38 | 39 | m_dims = { D, N }; 40 | 41 | m_rotationDeform = createEmptyOptImage({D}, OptImage::Type::FLOAT, 3, OptImage::GPU, true); 42 | m_translationDeform = createEmptyOptImage({D}, OptImage::Type::FLOAT, 3, OptImage::GPU, true); 43 | 44 | m_canonicalVerticesOpt = createEmptyOptImage({N}, OptImage::Type::FLOAT, 3, OptImage::GPU, true); 45 | m_liveVerticesOpt = createEmptyOptImage({N}, OptImage::Type::FLOAT, 3, OptImage::GPU, true); 46 | 47 | m_canonicalNormalsOpt = createEmptyOptImage({N}, OptImage::Type::FLOAT, 3, OptImage::GPU, true); 48 | m_liveNormalsOpt = createEmptyOptImage({N}, OptImage::Type::FLOAT, 3, OptImage::GPU, true); 49 | 50 | m_weights = createEmptyOptImage({N}, OptImage::Type::FLOAT, KNN_NEIGHBOURS, OptImage::GPU, true); 51 | 52 | resetGPUMemory(); 53 | initializeConnectivity(m_canonicalVerticesOpenCV); 54 | 55 | #ifdef SOLVER_PATH 56 | if(m_solverInfo.size() == 0) 57 | { 58 | std::string solver_file = std::string(TOSTRING(SOLVER_PATH)) + "dynamicfusion.t"; 59 | addOptSolvers(m_dims, solver_file); 60 | } 61 | #else 62 | std::cerr<<"Please define a path for your solvers."< canonical_vertices) 67 | { 68 | unsigned int N = (unsigned int) canonical_vertices.size(); 69 | 70 | std::vector > graph_vector(KNN_NEIGHBOURS + 1, vector(N)); 71 | // std::vector weights(N * KNN_NEIGHBOURS); 72 | std::vector weights(N); 73 | //FIXME: KNN doesn't need to be recomputed every time. 74 | for(int count = 0; count < canonical_vertices.size(); count++) 75 | { 76 | graph_vector[0].push_back(count); 77 | m_warp->getWeightsAndUpdateKNN(canonical_vertices[count], weights[count]); 78 | for(int i = 1; i < graph_vector.size(); i++) 79 | graph_vector[i].push_back((int)m_warp->getRetIndex()->at(i-1)); 80 | } 81 | m_weights->update(weights); 82 | m_data_graph = std::make_shared(graph_vector); 83 | 84 | } 85 | 86 | virtual void combinedSolveInit() override 87 | { 88 | m_functionTolerance = 1e-6f; 89 | m_paramTolerance = 1e-5f; 90 | 91 | m_problemParams.set("RotationDeform", m_rotationDeform); 92 | m_problemParams.set("TranslationDeform", m_translationDeform); 93 | 94 | m_problemParams.set("CanonicalVertices", m_canonicalVerticesOpt); 95 | m_problemParams.set("LiveVertices", m_liveVerticesOpt); 96 | 97 | m_problemParams.set("CanonicalNormals", m_canonicalNormalsOpt); 98 | m_problemParams.set("LiveNormals", m_liveNormalsOpt); 99 | 100 | m_problemParams.set("Weights", m_weights); 101 | 102 | m_problemParams.set("DataG", m_data_graph); 103 | // m_problemParams.set("RegG", m_reg_graph); 104 | 105 | m_solverParams.set("nIterations", &m_combinedSolverParameters.nonLinearIter); 106 | m_solverParams.set("lIterations", &m_combinedSolverParameters.linearIter); 107 | m_solverParams.set("function_tolerance", &m_functionTolerance); 108 | // m_solverParams.set("max_trust_region_radius", &m_trust_region_radius); 109 | // m_solverParams.set("q_tolerance", &m_paramTolerance); 110 | } 111 | 112 | virtual void preSingleSolve() override { 113 | // resetGPUMemory(); 114 | } 115 | virtual void postSingleSolve() override { 116 | copyResultToCPUFromFloat3(); 117 | } 118 | 119 | virtual void preNonlinearSolve(int) override {} 120 | 121 | virtual void postNonlinearSolve(int) override {} 122 | 123 | virtual void combinedSolveFinalize() override { 124 | reportFinalCosts("Robust Mesh Deformation", m_combinedSolverParameters, getCost("Opt(GN)"), getCost("Opt(LM)"), nan("")); 125 | } 126 | 127 | void resetGPUMemory() 128 | { 129 | uint N = (uint)m_canonicalVerticesOpenCV.size(); 130 | std::vector h_canonical_vertices(N); 131 | std::vector h_canonical_normals(N); 132 | std::vector h_live_vertices(N); 133 | std::vector h_live_normals(N); 134 | 135 | for(int i = 0; i < N; i++) 136 | { 137 | // FIXME: this code could look better 138 | if(std::isnan(m_canonicalVerticesOpenCV[i][0]) || 139 | std::isnan(m_canonicalVerticesOpenCV[i][1]) || 140 | std::isnan(m_canonicalVerticesOpenCV[i][2])) continue; 141 | 142 | if(std::isnan(m_canonicalNormalsOpenCV[i][0]) || 143 | std::isnan(m_canonicalNormalsOpenCV[i][1]) || 144 | std::isnan(m_canonicalNormalsOpenCV[i][2])) continue; 145 | 146 | if(std::isnan(m_liveVerticesOpenCV[i][0]) || 147 | std::isnan(m_liveVerticesOpenCV[i][1]) || 148 | std::isnan(m_liveVerticesOpenCV[i][2])) continue; 149 | 150 | if(std::isnan(m_liveNormalsOpenCV[i][0]) || 151 | std::isnan(m_liveNormalsOpenCV[i][1]) || 152 | std::isnan(m_liveNormalsOpenCV[i][2])) continue; 153 | 154 | 155 | h_canonical_vertices[i] = make_float3(m_canonicalVerticesOpenCV[i][0], m_canonicalVerticesOpenCV[i][1], m_canonicalVerticesOpenCV[i][2]); 156 | h_canonical_normals[i] = make_float3(m_canonicalNormalsOpenCV[i][0], m_canonicalNormalsOpenCV[i][1], m_canonicalNormalsOpenCV[i][2]); 157 | h_live_vertices[i] = make_float3(m_liveVerticesOpenCV[i][0], m_liveVerticesOpenCV[i][1], m_liveVerticesOpenCV[i][2]); 158 | h_live_normals[i] = make_float3(m_liveNormalsOpenCV[i][0], m_liveNormalsOpenCV[i][1], m_liveNormalsOpenCV[i][2]); 159 | } 160 | m_canonicalVerticesOpt->update(h_canonical_vertices); 161 | m_canonicalNormalsOpt->update(h_canonical_normals); 162 | m_liveVerticesOpt->update(h_live_vertices); 163 | m_liveNormalsOpt->update(h_live_normals); 164 | 165 | uint D = (uint)m_warp->getNodes()->size(); 166 | std::vector h_translation(D); 167 | std::vector h_rotation(D); 168 | 169 | for(int i = 0; i < m_warp->getNodes()->size(); i++) 170 | { 171 | float x,y,z; 172 | auto t = m_warp->getNodes()->at(i).transform; 173 | t.getTranslation(x,y,z); 174 | h_translation[i] = make_float3(x,y,z); 175 | 176 | t.getRotation().getRodrigues(x,y,z); 177 | h_rotation[i] = make_float3(x,y,z); 178 | } 179 | 180 | m_rotationDeform->update(h_rotation); 181 | m_translationDeform->update(h_translation); 182 | } 183 | 184 | std::vector result() 185 | { 186 | return m_resultVertices; 187 | } 188 | 189 | void copyResultToCPUFromFloat3() 190 | { 191 | unsigned int N = (unsigned int)m_warp->getNodes()->size(); 192 | std::vector h_translation(N); 193 | m_translationDeform->copyTo(h_translation); 194 | 195 | for (unsigned int i = 0; i < N; i++) 196 | m_warp->getNodes()->at(i).transform.encodeTranslation(h_translation[i].x, h_translation[i].y, h_translation[i].z); 197 | } 198 | 199 | private: 200 | 201 | kfusion::WarpField *m_warp; 202 | 203 | ml::Timer m_timer; 204 | 205 | // Current index in solve 206 | std::vector m_dims; 207 | 208 | std::shared_ptr m_rotationDeform; 209 | std::shared_ptr m_translationDeform; 210 | 211 | std::shared_ptr m_canonicalVerticesOpt; 212 | std::shared_ptr m_liveVerticesOpt; 213 | std::shared_ptr m_canonicalNormalsOpt; 214 | std::shared_ptr m_liveNormalsOpt; 215 | std::shared_ptr m_weights; 216 | std::shared_ptr m_reg_graph; 217 | std::shared_ptr m_data_graph; 218 | 219 | std::vector m_canonicalVerticesOpenCV; 220 | std::vector m_canonicalNormalsOpenCV; 221 | std::vector m_liveVerticesOpenCV; 222 | std::vector m_liveNormalsOpenCV; 223 | std::vector m_resultVertices; 224 | 225 | 226 | float m_functionTolerance; 227 | float m_paramTolerance; 228 | float m_trust_region_radius; 229 | }; 230 | 231 | -------------------------------------------------------------------------------- /kfusion/include/opt/OpenMesh.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Includes OpenMesh 4 | #ifndef NOMINMAX 5 | #define NOMINMAX 6 | #endif 7 | #define _USE_MATH_DEFINES 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace OpenMesh; 15 | 16 | struct Traits : DefaultTraits 17 | { 18 | typedef OpenMesh::Vec3f Point; 19 | typedef OpenMesh::Vec3f Normal; 20 | typedef OpenMesh::Vec3uc Color; 21 | typedef float Scalar; 22 | 23 | VertexTraits 24 | { 25 | public: 26 | 27 | VertexT() : m_constrained(false) 28 | { 29 | } 30 | 31 | bool m_constrained; 32 | }; 33 | 34 | VertexAttributes(Attributes::Status| Attributes::Normal | Attributes::Color); 35 | FaceAttributes(Attributes::Status); 36 | EdgeAttributes(Attributes::Status); 37 | }; 38 | 39 | typedef TriMesh_ArrayKernelT SimpleMesh; 40 | -------------------------------------------------------------------------------- /kfusion/include/opt/Resource.h: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------------ 2 | // 3 | // Copyright (c) Microsoft Corporation. All rights reserved. 4 | // 5 | //------------------------------------------------------------------------------ 6 | 7 | //{{NO_DEPENDENCIES}} 8 | // Microsoft Visual C++ generated include file. 9 | // Used by DepthWithColor-D3D.rc 10 | // 11 | 12 | #define IDS_APP_TITLE 103 13 | 14 | #define IDI_APP 107 15 | #define IDC_STATIC -1 16 | // Next default values for new objects 17 | // 18 | #ifdef APSTUDIO_INVOKED 19 | #ifndef APSTUDIO_READONLY_SYMBOLS 20 | 21 | #define _APS_NO_MFC 130 22 | #define _APS_NEXT_RESOURCE_VALUE 129 23 | #define _APS_NEXT_COMMAND_VALUE 32771 24 | #define _APS_NEXT_CONTROL_VALUE 1000 25 | #define _APS_NEXT_SYMED_VALUE 110 26 | #endif 27 | #endif 28 | -------------------------------------------------------------------------------- /kfusion/include/opt/mLibInclude.h: -------------------------------------------------------------------------------- 1 | // 2 | // mLib config options 3 | // 4 | 5 | #ifdef _DEBUG 6 | #define MLIB_ERROR_CHECK 7 | #define MLIB_BOUNDS_CHECK 8 | //#define _IDL0 9 | #endif // _DEBUG 10 | 11 | // 12 | // mLib includes 13 | // 14 | 15 | #include 16 | #include 17 | 18 | 19 | using namespace ml; 20 | -------------------------------------------------------------------------------- /kfusion/include/opt/main.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Resource.h" 4 | #include "mLibInclude.h" 5 | 6 | #ifndef SAFE_DELETE 7 | #define SAFE_DELETE(p) { if (p) { delete (p); (p)=NULL; } } 8 | #endif 9 | #ifndef SAFE_DELETE_ARRAY 10 | #define SAFE_DELETE_ARRAY(p) { if (p) { delete[] (p); (p)=NULL; } } 11 | #endif 12 | -------------------------------------------------------------------------------- /kfusion/include/opt/resource.h: -------------------------------------------------------------------------------- 1 | //------------------------------------------------------------------------------ 2 | // 3 | // Copyright (c) Microsoft Corporation. All rights reserved. 4 | // 5 | //------------------------------------------------------------------------------ 6 | 7 | //{{NO_DEPENDENCIES}} 8 | // Microsoft Visual C++ generated include file. 9 | // Used by DepthWithColor-D3D.rc 10 | // 11 | 12 | #define IDS_APP_TITLE 103 13 | 14 | #define IDI_APP 107 15 | #define IDC_STATIC -1 16 | // Next default values for new objects 17 | // 18 | #ifdef APSTUDIO_INVOKED 19 | #ifndef APSTUDIO_READONLY_SYMBOLS 20 | 21 | #define _APS_NO_MFC 130 22 | #define _APS_NEXT_RESOURCE_VALUE 129 23 | #define _APS_NEXT_COMMAND_VALUE 32771 24 | #define _APS_NEXT_CONTROL_VALUE 1000 25 | #define _APS_NEXT_SYMED_VALUE 110 26 | #endif 27 | #endif 28 | -------------------------------------------------------------------------------- /kfusion/solvers/dynamicfusion.t: -------------------------------------------------------------------------------- 1 | local D,N = Dim("D",0), Dim("N",1) 2 | 3 | local RotationDeform = Unknown("RotationDeform", opt_float3,{D},0) 4 | local TranslationDeform = Unknown("TranslationDeform", opt_float3,{D}, 1) 5 | 6 | local CanonicalVertices = Array("CanonicalVertices", opt_float3,{N}, 2) 7 | local LiveVertices = Array("LiveVertices", opt_float3,{N},3) 8 | 9 | local CanonicalNormals = Array("CanonicalNormals", opt_float3,{N}, 4) 10 | local LiveNormals = Array("LiveNormals", opt_float3,{N},5) 11 | 12 | local Weights = Array("Weights", opt_float8, {N}, 6) 13 | 14 | local G = Graph("DataG", 7, 15 | "v", {N}, 8, 16 | "n0", {D}, 9, 17 | "n1", {D}, 10, 18 | "n2", {D}, 11, 19 | "n3", {D}, 12, 20 | "n4", {D}, 13, 21 | "n5", {D}, 14, 22 | "n6", {D}, 15, 23 | "n7", {D}, 16) 24 | 25 | 26 | local weightedTranslation = 0 27 | 28 | nodes = {0,1,2,3,4,5,6,7} 29 | 30 | for _,i in ipairs(nodes) do 31 | weightedTranslation = weightedTranslation + Weights(G.v)(i) * TranslationDeform(G["n"..i]) 32 | end 33 | 34 | function huberPenalty(a, delta) -- delta should be 0.00001 35 | if lesseq(abs(a),delta) then 36 | return a * a / 2 37 | else 38 | return delta * abs(a) - delta * delta / 2 39 | end 40 | end 41 | 42 | 43 | function tukeyPenalty(x, c) -- c = 0.01 44 | if lesseq(abs(x), c) then 45 | return x * pow(1.0 - (x * x) / (c * c), 2) 46 | else 47 | return 0 48 | end 49 | end 50 | 51 | --Energy(tukeyPenalty(LiveVertices(G.v) - CanonicalVertices(G.v) - weightedTranslation[G.v], 0.01)) 52 | Energy(LiveVertices(G.v) - CanonicalVertices(G.v) - weightedTranslation) 53 | -------------------------------------------------------------------------------- /kfusion/src/capture.cpp: -------------------------------------------------------------------------------- 1 | #ifdef OPENNI_FOUND 2 | #pragma warning (disable :4996) 3 | #undef _CRT_SECURE_NO_DEPRECATE 4 | #include "XnCppWrapper.h" 5 | #include 6 | 7 | using namespace std; 8 | using namespace xn; 9 | 10 | //const std::string XMLConfig = 11 | //"" 12 | // "" 13 | // "" 14 | // "" 15 | // "" 16 | // "" 17 | // "" 18 | // "" 19 | // "" 20 | // "" 21 | // "" 22 | // "" 23 | // "" 24 | // "" 25 | // "" 26 | // "" 27 | // "" 28 | // "" 29 | // " " 30 | // "" 31 | // "" 32 | // "" 33 | // "" 34 | // "" 35 | // "" 36 | // "" 37 | //""; 38 | 39 | #define REPORT_ERROR(msg) kfusion::cuda::error ((msg), __FILE__, __LINE__) 40 | 41 | struct kfusion::OpenNISource::Impl 42 | { 43 | Context context; 44 | ScriptNode scriptNode; 45 | DepthGenerator depth; 46 | ImageGenerator image; 47 | ProductionNode node; 48 | DepthMetaData depthMD; 49 | ImageMetaData imageMD; 50 | XnChar strError[1024]; 51 | Player player_; 52 | 53 | bool has_depth; 54 | bool has_image; 55 | }; 56 | 57 | kfusion::OpenNISource::OpenNISource() : depth_focal_length_VGA (0.f), baseline (0.f), 58 | shadow_value (0), no_sample_value (0), pixelSize (0.0), max_depth (0) {} 59 | 60 | kfusion::OpenNISource::OpenNISource(int device) {open (device); } 61 | kfusion::OpenNISource::OpenNISource(const string& filename, bool repeat /*= false*/) {open (filename, repeat); } 62 | kfusion::OpenNISource::~OpenNISource() { release (); } 63 | 64 | void kfusion::OpenNISource::open (int device) 65 | { 66 | impl_ = cv::Ptr( new Impl () ); 67 | 68 | XnMapOutputMode mode; 69 | mode.nXRes = XN_VGA_X_RES; 70 | mode.nYRes = XN_VGA_Y_RES; 71 | mode.nFPS = 30; 72 | 73 | XnStatus rc; 74 | rc = impl_->context.Init (); 75 | if (rc != XN_STATUS_OK) 76 | { 77 | sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc)); 78 | REPORT_ERROR (impl_->strError); 79 | } 80 | 81 | xn::NodeInfoList devicesList; 82 | rc = impl_->context.EnumerateProductionTrees ( XN_NODE_TYPE_DEVICE, NULL, devicesList, 0 ); 83 | if (rc != XN_STATUS_OK) 84 | { 85 | sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc)); 86 | REPORT_ERROR (impl_->strError); 87 | } 88 | 89 | xn::NodeInfoList::Iterator it = devicesList.Begin (); 90 | for (int i = 0; i < device; ++i) 91 | it++; 92 | 93 | NodeInfo node = *it; 94 | rc = impl_->context.CreateProductionTree ( node, impl_->node ); 95 | if (rc != XN_STATUS_OK) 96 | { 97 | sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc)); 98 | REPORT_ERROR (impl_->strError); 99 | } 100 | 101 | XnLicense license; 102 | const char* vendor = "PrimeSense"; 103 | const char* key = "0KOIk2JeIBYClPWVnMoRKn5cdY4="; 104 | sprintf ("%s, %s", license.strKey, key); 105 | sprintf ("%s, %s", license.strVendor, vendor); 106 | 107 | rc = impl_->context.AddLicense (license); 108 | if (rc != XN_STATUS_OK) 109 | { 110 | sprintf (impl_->strError, "licence failed: %s\n", xnGetStatusString (rc)); 111 | REPORT_ERROR (impl_->strError); 112 | } 113 | 114 | rc = impl_->depth.Create (impl_->context); 115 | if (rc != XN_STATUS_OK) 116 | { 117 | sprintf (impl_->strError, "Depth generator failed: %s\n", xnGetStatusString (rc)); 118 | REPORT_ERROR (impl_->strError); 119 | } 120 | //rc = impl_->depth.SetIntProperty("HoleFilter", 1); 121 | rc = impl_->depth.SetMapOutputMode (mode); 122 | impl_->has_depth = true; 123 | 124 | rc = impl_->image.Create (impl_->context); 125 | if (rc != XN_STATUS_OK) 126 | { 127 | printf ("Image generator creation failed: %s\n", xnGetStatusString (rc)); 128 | impl_->has_image = false; 129 | } 130 | else 131 | { 132 | impl_->has_image = true; 133 | rc = impl_->image.SetMapOutputMode (mode); 134 | } 135 | 136 | getParams (); 137 | 138 | rc = impl_->context.StartGeneratingAll (); 139 | if (rc != XN_STATUS_OK) 140 | { 141 | sprintf (impl_->strError, "Start failed: %s\n", xnGetStatusString (rc)); 142 | REPORT_ERROR (impl_->strError); 143 | } 144 | } 145 | 146 | void kfusion::OpenNISource::open(const std::string& filename, bool repeat /*= false*/) 147 | { 148 | impl_ = cv::Ptr ( new Impl () ); 149 | 150 | XnStatus rc; 151 | 152 | rc = impl_->context.Init (); 153 | if (rc != XN_STATUS_OK) 154 | { 155 | sprintf (impl_->strError, "Init failed: %s\n", xnGetStatusString (rc)); 156 | REPORT_ERROR (impl_->strError); 157 | } 158 | 159 | //rc = impl_->context.OpenFileRecording (filename.c_str (), impl_->node); 160 | rc = impl_->context.OpenFileRecording (filename.c_str (), impl_->player_); 161 | if (rc != XN_STATUS_OK) 162 | { 163 | sprintf (impl_->strError, "Open failed: %s\n", xnGetStatusString (rc)); 164 | REPORT_ERROR (impl_->strError); 165 | } 166 | 167 | impl_->player_.SetRepeat(repeat); 168 | 169 | rc = impl_->context.FindExistingNode (XN_NODE_TYPE_DEPTH, impl_->depth); 170 | impl_->has_depth = (rc == XN_STATUS_OK); 171 | 172 | rc = impl_->context.FindExistingNode (XN_NODE_TYPE_IMAGE, impl_->image); 173 | impl_->has_image = (rc == XN_STATUS_OK); 174 | 175 | if (!impl_->has_depth) 176 | REPORT_ERROR ("No depth nodes. Check your configuration"); 177 | 178 | if (impl_->has_depth) 179 | impl_->depth.GetMetaData (impl_->depthMD); 180 | 181 | if (impl_->has_image) 182 | impl_->image.GetMetaData (impl_->imageMD); 183 | 184 | // RGB is the only image format supported. 185 | if (impl_->imageMD.PixelFormat () != XN_PIXEL_FORMAT_RGB24) 186 | REPORT_ERROR ("Image format must be RGB24\n"); 187 | 188 | getParams (); 189 | } 190 | 191 | void kfusion::OpenNISource::release () 192 | { 193 | if (impl_) 194 | { 195 | impl_->context.StopGeneratingAll (); 196 | impl_->context.Release (); 197 | } 198 | 199 | impl_.release();; 200 | depth_focal_length_VGA = 0; 201 | baseline = 0.f; 202 | shadow_value = 0; 203 | no_sample_value = 0; 204 | pixelSize = 0.0; 205 | } 206 | 207 | bool kfusion::OpenNISource::grab(cv::Mat& depth, cv::Mat& image) 208 | { 209 | XnStatus rc = XN_STATUS_OK; 210 | 211 | rc = impl_->context.WaitAndUpdateAll (); 212 | if (rc != XN_STATUS_OK) 213 | return printf ("Read failed: %s\n", xnGetStatusString (rc)), false; 214 | 215 | if (impl_->has_depth) 216 | { 217 | impl_->depth.GetMetaData (impl_->depthMD); 218 | const XnDepthPixel* pDepth = impl_->depthMD.Data (); 219 | int x = impl_->depthMD.FullXRes (); 220 | int y = impl_->depthMD.FullYRes (); 221 | cv::Mat(y, x, CV_16U, (void*)pDepth).copyTo(depth); 222 | } 223 | else 224 | { 225 | depth.release(); 226 | printf ("no depth\n"); 227 | } 228 | 229 | if (impl_->has_image) 230 | { 231 | impl_->image.GetMetaData (impl_->imageMD); 232 | const XnRGB24Pixel* pImage = impl_->imageMD.RGB24Data (); 233 | int x = impl_->imageMD.FullXRes (); 234 | int y = impl_->imageMD.FullYRes (); 235 | image.create(y, x, CV_8UC3); 236 | 237 | cv::Vec3b *dptr = image.ptr(); 238 | for(size_t i = 0; i < image.total(); ++i) 239 | dptr[i] = cv::Vec3b(pImage[i].nBlue, pImage[i].nGreen, pImage[i].nRed); 240 | } 241 | else 242 | { 243 | image.release(); 244 | printf ("no image\n"); 245 | } 246 | 247 | return impl_->has_image || impl_->has_depth; 248 | } 249 | 250 | void kfusion::OpenNISource::getParams () 251 | { 252 | XnStatus rc = XN_STATUS_OK; 253 | 254 | max_depth = impl_->depth.GetDeviceMaxDepth (); 255 | 256 | rc = impl_->depth.GetRealProperty ( "ZPPS", pixelSize ); // in mm 257 | if (rc != XN_STATUS_OK) 258 | { 259 | sprintf (impl_->strError, "ZPPS failed: %s\n", xnGetStatusString (rc)); 260 | REPORT_ERROR (impl_->strError); 261 | } 262 | 263 | XnUInt64 depth_focal_length_SXGA_mm; //in mm 264 | rc = impl_->depth.GetIntProperty ("ZPD", depth_focal_length_SXGA_mm); 265 | if (rc != XN_STATUS_OK) 266 | { 267 | sprintf (impl_->strError, "ZPD failed: %s\n", xnGetStatusString (rc)); 268 | REPORT_ERROR (impl_->strError); 269 | } 270 | 271 | XnDouble baseline_local; 272 | rc = impl_->depth.GetRealProperty ("LDDIS", baseline_local); 273 | if (rc != XN_STATUS_OK) 274 | { 275 | sprintf (impl_->strError, "ZPD failed: %s\n", xnGetStatusString (rc)); 276 | REPORT_ERROR (impl_->strError); 277 | } 278 | 279 | XnUInt64 shadow_value_local; 280 | rc = impl_->depth.GetIntProperty ("ShadowValue", shadow_value_local); 281 | if (rc != XN_STATUS_OK) 282 | { 283 | sprintf (impl_->strError, "ShadowValue failed: %s\n", xnGetStatusString (rc)); 284 | // REPORT_ERROR (impl_->strError); 285 | } 286 | shadow_value = (int)shadow_value_local; 287 | 288 | XnUInt64 no_sample_value_local; 289 | rc = impl_->depth.GetIntProperty ("NoSampleValue", no_sample_value_local); 290 | if (rc != XN_STATUS_OK) 291 | { 292 | sprintf (impl_->strError, "NoSampleValue failed: %s\n", xnGetStatusString (rc)); 293 | // REPORT_ERROR (impl_->strError); 294 | } 295 | no_sample_value = (int)no_sample_value_local; 296 | 297 | 298 | // baseline from cm -> mm 299 | baseline = (float)(baseline_local * 10); 300 | 301 | //focal length from mm -> pixels (valid for 1280x1024) 302 | float depth_focal_length_SXGA = static_cast(depth_focal_length_SXGA_mm / pixelSize); 303 | depth_focal_length_VGA = depth_focal_length_SXGA / 2; 304 | } 305 | 306 | bool kfusion::OpenNISource::setRegistration (bool value) 307 | { 308 | XnStatus rc = XN_STATUS_OK; 309 | 310 | if (value) 311 | { 312 | if (!impl_->has_image) 313 | return false; 314 | 315 | if (impl_->depth.GetAlternativeViewPointCap ().IsViewPointAs (impl_->image) ) 316 | return true; 317 | 318 | if (!impl_->depth.GetAlternativeViewPointCap ().IsViewPointSupported (impl_->image) ) 319 | { 320 | printf ("SetRegistration failed: Unsupported viewpoint.\n"); 321 | return false; 322 | } 323 | 324 | rc = impl_->depth.GetAlternativeViewPointCap ().SetViewPoint (impl_->image); 325 | if (rc != XN_STATUS_OK) 326 | printf ("SetRegistration failed: %s\n", xnGetStatusString (rc)); 327 | 328 | } 329 | else // "off" 330 | { 331 | rc = impl_->depth.GetAlternativeViewPointCap ().ResetViewPoint (); 332 | if (rc != XN_STATUS_OK) 333 | printf ("SetRegistration failed: %s\n", xnGetStatusString (rc)); 334 | } 335 | 336 | getParams (); 337 | return rc == XN_STATUS_OK; 338 | } 339 | #endif -------------------------------------------------------------------------------- /kfusion/src/cuda/device.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "internal.hpp" 4 | #include "temp_utils.hpp" 5 | 6 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 7 | /// TsdfVolume 8 | 9 | //__kf_device__ 10 | //kfusion::device::TsdfVolume::TsdfVolume(elem_type* _data, int3 _dims, float3 _voxel_size, float _trunc_dist, int _max_weight) 11 | // : data(_data), dims(_dims), voxel_size(_voxel_size), trunc_dist(_trunc_dist), max_weight(_max_weight) {} 12 | 13 | //__kf_device__ 14 | //kfusion::device::TsdfVolume::TsdfVolume(const TsdfVolume& other) 15 | // : data(other.data), dims(other.dims), voxel_size(other.voxel_size), trunc_dist(other.trunc_dist), max_weight(other.max_weight) {} 16 | 17 | __kf_device__ kfusion::device::TsdfVolume::elem_type* kfusion::device::TsdfVolume::operator()(int x, int y, int z) 18 | { return data + x + y*dims.x + z*dims.y*dims.x; } 19 | 20 | __kf_device__ const kfusion::device::TsdfVolume::elem_type* kfusion::device::TsdfVolume::operator() (int x, int y, int z) const 21 | { return data + x + y*dims.x + z*dims.y*dims.x; } 22 | 23 | __kf_device__ kfusion::device::TsdfVolume::elem_type* kfusion::device::TsdfVolume::beg(int x, int y) const 24 | { return data + x + dims.x * y; } 25 | 26 | __kf_device__ kfusion::device::TsdfVolume::elem_type* kfusion::device::TsdfVolume::zstep(elem_type *const ptr) const 27 | { return ptr + dims.x * dims.y; } 28 | 29 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 30 | /// Projector 31 | 32 | __kf_device__ float2 kfusion::device::Projector::operator()(const float3& p) const 33 | { 34 | float2 coo; 35 | coo.x = __fmaf_rn(f.x, __fdividef(p.x, p.z), c.x); 36 | coo.y = __fmaf_rn(f.y, __fdividef(p.y, p.z), c.y); 37 | return coo; 38 | } 39 | 40 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 41 | /// Reprojector 42 | 43 | __kf_device__ float3 kfusion::device::Reprojector::operator()(int u, int v, float z) const 44 | { 45 | float x = z * (u - c.x) * finv.x; 46 | float y = z * (v - c.y) * finv.y; 47 | return make_float3(x, y, z); 48 | } 49 | 50 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 51 | /// packing/unpacking tsdf volume element 52 | 53 | __kf_device__ ushort2 kfusion::device::pack_tsdf (float tsdf, int weight) 54 | { return make_ushort2 (__float2half_rn (tsdf), weight); } 55 | 56 | __kf_device__ float kfusion::device::unpack_tsdf(ushort2 value, int& weight) 57 | { 58 | weight = value.y; 59 | return __half2float (value.x); 60 | } 61 | __kf_device__ float kfusion::device::unpack_tsdf (ushort2 value) { return __half2float (value.x); } 62 | 63 | 64 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 65 | /// Utility 66 | 67 | namespace kfusion 68 | { 69 | namespace device 70 | { 71 | __kf_device__ Vec3f operator*(const Mat3f& m, const Vec3f& v) 72 | { return make_float3(dot(m.data[0], v), dot (m.data[1], v), dot (m.data[2], v)); } 73 | 74 | __kf_device__ Vec3f operator*(const Aff3f& a, const Vec3f& v) { return a.R * v + a.t; } 75 | 76 | __kf_device__ Vec3f tr(const float4& v) { return make_float3(v.x, v.y, v.z); } 77 | 78 | struct plus 79 | { 80 | __kf_device__ float operator () (float l, float r) const { return l + r; } 81 | __kf_device__ double operator () (double l, double r) const { return l + r; } 82 | }; 83 | 84 | struct gmem 85 | { 86 | template __kf_device__ static T LdCs(T *ptr); 87 | template __kf_device__ static void StCs(const T& val, T *ptr); 88 | }; 89 | 90 | template<> __kf_device__ ushort2 gmem::LdCs(ushort2* ptr); 91 | template<> __kf_device__ void gmem::StCs(const ushort2& val, ushort2* ptr); 92 | } 93 | } 94 | 95 | 96 | #if defined __CUDA_ARCH__ && __CUDA_ARCH__ >= 200 97 | 98 | #if defined(_WIN64) || defined(__LP64__) 99 | #define _ASM_PTR_ "l" 100 | #else 101 | #define _ASM_PTR_ "r" 102 | #endif 103 | 104 | template<> __kf_device__ ushort2 kfusion::device::gmem::LdCs(ushort2* ptr) 105 | { 106 | ushort2 val; 107 | asm("ld.global.cs.v2.u16 {%0, %1}, [%2];" : "=h"(reinterpret_cast(val.x)), "=h"(reinterpret_cast(val.y)) : _ASM_PTR_(ptr)); 108 | return val; 109 | } 110 | 111 | template<> __kf_device__ void kfusion::device::gmem::StCs(const ushort2& val, ushort2* ptr) 112 | { 113 | short cx = val.x, cy = val.y; 114 | asm("st.global.cs.v2.u16 [%0], {%1, %2};" : : _ASM_PTR_(ptr), "h"(reinterpret_cast(cx)), "h"(reinterpret_cast(cy))); 115 | } 116 | #undef _ASM_PTR_ 117 | 118 | #else 119 | template<> __kf_device__ ushort2 kfusion::device::gmem::LdCs(ushort2* ptr) { return *ptr; } 120 | template<> __kf_device__ void kfusion::device::gmem::StCs(const ushort2& val, ushort2* ptr) { *ptr = val; } 121 | #endif 122 | 123 | 124 | 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /kfusion/src/cuda/texture_binder.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace kfusion 7 | { 8 | namespace cuda 9 | { 10 | class TextureBinder 11 | { 12 | public: 13 | template 14 | TextureBinder(const DeviceArray2D& arr, const struct texture& tex) : texref(&tex) 15 | { 16 | cudaChannelFormatDesc desc = cudaCreateChannelDesc(); 17 | cudaSafeCall( cudaBindTexture2D(0, tex, arr.ptr(), desc, arr.cols(), arr.rows(), arr.step()) ); 18 | } 19 | 20 | template 21 | TextureBinder(const DeviceArray& arr, const struct texture &tex) : texref(&tex) 22 | { 23 | cudaChannelFormatDesc desc = cudaCreateChannelDesc(); 24 | cudaSafeCall( cudaBindTexture(0, tex, arr.ptr(), desc, arr.sizeBytes()) ); 25 | } 26 | 27 | template 28 | TextureBinder(const PtrStepSz& arr, const struct texture& tex) : texref(&tex) 29 | { 30 | cudaChannelFormatDesc desc = cudaCreateChannelDesc(); 31 | cudaSafeCall( cudaBindTexture2D(0, tex, arr.data, desc, arr.cols, arr.rows, arr.step) ); 32 | } 33 | 34 | template 35 | TextureBinder(const A& arr, const struct texture& tex, const cudaChannelFormatDesc& desc) : texref(&tex) 36 | { 37 | cudaSafeCall( cudaBindTexture2D(0, tex, arr.data, desc, arr.cols, arr.rows, arr.step) ); 38 | } 39 | 40 | template 41 | TextureBinder(const PtrSz& arr, const struct texture &tex) : texref(&tex) 42 | { 43 | cudaChannelFormatDesc desc = cudaCreateChannelDesc(); 44 | cudaSafeCall( cudaBindTexture(0, tex, arr.data, desc, arr.size * arr.elemSize()) ); 45 | } 46 | 47 | ~TextureBinder() 48 | { 49 | cudaSafeCall( cudaUnbindTexture(texref) ); 50 | } 51 | private: 52 | const struct textureReference *texref; 53 | }; 54 | } 55 | 56 | namespace device 57 | { 58 | using kfusion::cuda::TextureBinder; 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /kfusion/src/device_memory.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "safe_call.hpp" 3 | #include 4 | #include 5 | #include 6 | 7 | void kfusion::cuda::error(const char *error_string, const char *file, const int line, const char *func) 8 | { 9 | std::cout << "KinFu2 error: " << error_string << "\t" << file << ":" << line << std::endl; 10 | exit(0); 11 | } 12 | 13 | ////////////////////////// XADD /////////////////////////////// 14 | 15 | #ifdef __GNUC__ 16 | 17 | #if __GNUC__*10 + __GNUC_MINOR__ >= 42 18 | 19 | #if !defined WIN32 && (defined __i486__ || defined __i586__ || defined __i686__ || defined __MMX__ || defined __SSE__ || defined __ppc__) 20 | #define CV_XADD __sync_fetch_and_add 21 | #else 22 | #include 23 | #define CV_XADD __gnu_cxx::__exchange_and_add 24 | #endif 25 | #else 26 | #include 27 | #if __GNUC__*10 + __GNUC_MINOR__ >= 34 28 | #define CV_XADD __gnu_cxx::__exchange_and_add 29 | #else 30 | #define CV_XADD __exchange_and_add 31 | #endif 32 | #endif 33 | 34 | #elif defined WIN32 || defined _WIN32 35 | #include 36 | #define CV_XADD(addr,delta) _InterlockedExchangeAdd((long volatile*)(addr), (delta)) 37 | #else 38 | 39 | template static inline _Tp CV_XADD(_Tp* addr, _Tp delta) 40 | { int tmp = *addr; *addr += delta; return tmp; } 41 | 42 | #endif 43 | 44 | //////////////////////// DeviceArray ///////////////////////////// 45 | 46 | kfusion::cuda::DeviceMemory::DeviceMemory() : data_(0), sizeBytes_(0), refcount_(0) {} 47 | kfusion::cuda::DeviceMemory::DeviceMemory(void *ptr_arg, size_t sizeBytes_arg) : data_(ptr_arg), sizeBytes_(sizeBytes_arg), refcount_(0){} 48 | kfusion::cuda::DeviceMemory::DeviceMemory(size_t sizeBtes_arg) : data_(0), sizeBytes_(0), refcount_(0) { create(sizeBtes_arg); } 49 | kfusion::cuda::DeviceMemory::~DeviceMemory() { release(); } 50 | 51 | kfusion::cuda::DeviceMemory::DeviceMemory(const DeviceMemory& other_arg) 52 | : data_(other_arg.data_), sizeBytes_(other_arg.sizeBytes_), refcount_(other_arg.refcount_) 53 | { 54 | if( refcount_ ) 55 | CV_XADD(refcount_, 1); 56 | } 57 | 58 | kfusion::cuda::DeviceMemory& kfusion::cuda::DeviceMemory::operator = (const kfusion::cuda::DeviceMemory& other_arg) 59 | { 60 | if( this != &other_arg ) 61 | { 62 | if( other_arg.refcount_ ) 63 | CV_XADD(other_arg.refcount_, 1); 64 | release(); 65 | 66 | data_ = other_arg.data_; 67 | sizeBytes_ = other_arg.sizeBytes_; 68 | refcount_ = other_arg.refcount_; 69 | } 70 | return *this; 71 | } 72 | 73 | void kfusion::cuda::DeviceMemory::create(size_t sizeBytes_arg) 74 | { 75 | if (sizeBytes_arg == sizeBytes_) 76 | return; 77 | 78 | if( sizeBytes_arg > 0) 79 | { 80 | if( data_ ) 81 | release(); 82 | 83 | sizeBytes_ = sizeBytes_arg; 84 | 85 | cudaSafeCall( cudaMalloc(&data_, sizeBytes_) ); 86 | 87 | //refcount_ = (int*)cv::fastMalloc(sizeof(*refcount_)); 88 | refcount_ = new int; 89 | *refcount_ = 1; 90 | } 91 | } 92 | 93 | void kfusion::cuda::DeviceMemory::copyTo(DeviceMemory& other) const 94 | { 95 | if (empty()) 96 | other.release(); 97 | else 98 | { 99 | other.create(sizeBytes_); 100 | cudaSafeCall( cudaMemcpy(other.data_, data_, sizeBytes_, cudaMemcpyDeviceToDevice) ); 101 | } 102 | } 103 | 104 | void kfusion::cuda::DeviceMemory::release() 105 | { 106 | if( refcount_ && CV_XADD(refcount_, -1) == 1 ) 107 | { 108 | //cv::fastFree(refcount); 109 | delete refcount_; 110 | cudaSafeCall( cudaFree(data_) ); 111 | } 112 | data_ = 0; 113 | sizeBytes_ = 0; 114 | refcount_ = 0; 115 | } 116 | 117 | void kfusion::cuda::DeviceMemory::upload(const void *host_ptr_arg, size_t sizeBytes_arg) 118 | { 119 | create(sizeBytes_arg); 120 | cudaSafeCall( cudaMemcpy(data_, host_ptr_arg, sizeBytes_, cudaMemcpyHostToDevice) ); 121 | } 122 | 123 | void kfusion::cuda::DeviceMemory::download(void *host_ptr_arg) const 124 | { 125 | cudaSafeCall( cudaMemcpy(host_ptr_arg, data_, sizeBytes_, cudaMemcpyDeviceToHost) ); 126 | } 127 | 128 | void kfusion::cuda::DeviceMemory::swap(DeviceMemory& other_arg) 129 | { 130 | std::swap(data_, other_arg.data_); 131 | std::swap(sizeBytes_, other_arg.sizeBytes_); 132 | std::swap(refcount_, other_arg.refcount_); 133 | } 134 | 135 | bool kfusion::cuda::DeviceMemory::empty() const { return !data_; } 136 | size_t kfusion::cuda::DeviceMemory::sizeBytes() const { return sizeBytes_; } 137 | 138 | 139 | //////////////////////// DeviceArray2D ///////////////////////////// 140 | 141 | kfusion::cuda::DeviceMemory2D::DeviceMemory2D() : data_(0), step_(0), colsBytes_(0), rows_(0), refcount_(0) {} 142 | 143 | kfusion::cuda::DeviceMemory2D::DeviceMemory2D(int rows_arg, int colsBytes_arg) 144 | : data_(0), step_(0), colsBytes_(0), rows_(0), refcount_(0) 145 | { 146 | create(rows_arg, colsBytes_arg); 147 | } 148 | 149 | kfusion::cuda::DeviceMemory2D::DeviceMemory2D(int rows_arg, int colsBytes_arg, void *data_arg, size_t step_arg) 150 | : data_(data_arg), step_(step_arg), colsBytes_(colsBytes_arg), rows_(rows_arg), refcount_(0) {} 151 | 152 | kfusion::cuda::DeviceMemory2D::~DeviceMemory2D() { release(); } 153 | 154 | 155 | kfusion::cuda::DeviceMemory2D::DeviceMemory2D(const DeviceMemory2D& other_arg) : 156 | data_(other_arg.data_), step_(other_arg.step_), colsBytes_(other_arg.colsBytes_), rows_(other_arg.rows_), refcount_(other_arg.refcount_) 157 | { 158 | if( refcount_ ) 159 | CV_XADD(refcount_, 1); 160 | } 161 | 162 | kfusion::cuda::DeviceMemory2D& kfusion::cuda::DeviceMemory2D::operator = (const kfusion::cuda::DeviceMemory2D& other_arg) 163 | { 164 | if( this != &other_arg ) 165 | { 166 | if( other_arg.refcount_ ) 167 | CV_XADD(other_arg.refcount_, 1); 168 | release(); 169 | 170 | colsBytes_ = other_arg.colsBytes_; 171 | rows_ = other_arg.rows_; 172 | data_ = other_arg.data_; 173 | step_ = other_arg.step_; 174 | 175 | refcount_ = other_arg.refcount_; 176 | } 177 | return *this; 178 | } 179 | 180 | void kfusion::cuda::DeviceMemory2D::create(int rows_arg, int colsBytes_arg) 181 | { 182 | if (colsBytes_ == colsBytes_arg && rows_ == rows_arg) 183 | return; 184 | 185 | if( rows_arg > 0 && colsBytes_arg > 0) 186 | { 187 | if( data_ ) 188 | release(); 189 | 190 | colsBytes_ = colsBytes_arg; 191 | rows_ = rows_arg; 192 | 193 | cudaSafeCall( cudaMallocPitch( (void**)&data_, &step_, colsBytes_, rows_) ); 194 | 195 | //refcount = (int*)cv::fastMalloc(sizeof(*refcount)); 196 | refcount_ = new int; 197 | *refcount_ = 1; 198 | } 199 | } 200 | 201 | void kfusion::cuda::DeviceMemory2D::release() 202 | { 203 | if( refcount_ && CV_XADD(refcount_, -1) == 1 ) 204 | { 205 | //cv::fastFree(refcount); 206 | delete refcount_; 207 | cudaSafeCall( cudaFree(data_) ); 208 | } 209 | 210 | colsBytes_ = 0; 211 | rows_ = 0; 212 | data_ = 0; 213 | step_ = 0; 214 | refcount_ = 0; 215 | } 216 | 217 | void kfusion::cuda::DeviceMemory2D::copyTo(DeviceMemory2D& other) const 218 | { 219 | if (empty()) 220 | other.release(); 221 | else 222 | { 223 | other.create(rows_, colsBytes_); 224 | cudaSafeCall( cudaMemcpy2D(other.data_, other.step_, data_, step_, colsBytes_, rows_, cudaMemcpyDeviceToDevice) ); 225 | } 226 | } 227 | 228 | void kfusion::cuda::DeviceMemory2D::upload(const void *host_ptr_arg, size_t host_step_arg, int rows_arg, int colsBytes_arg) 229 | { 230 | create(rows_arg, colsBytes_arg); 231 | cudaSafeCall( cudaMemcpy2D(data_, step_, host_ptr_arg, host_step_arg, colsBytes_, rows_, cudaMemcpyHostToDevice) ); 232 | } 233 | 234 | void kfusion::cuda::DeviceMemory2D::download(void *host_ptr_arg, size_t host_step_arg) const 235 | { 236 | cudaSafeCall( cudaMemcpy2D(host_ptr_arg, host_step_arg, data_, step_, colsBytes_, rows_, cudaMemcpyDeviceToHost) ); 237 | } 238 | 239 | void kfusion::cuda::DeviceMemory2D::swap(DeviceMemory2D& other_arg) 240 | { 241 | std::swap(data_, other_arg.data_); 242 | std::swap(step_, other_arg.step_); 243 | 244 | std::swap(colsBytes_, other_arg.colsBytes_); 245 | std::swap(rows_, other_arg.rows_); 246 | std::swap(refcount_, other_arg.refcount_); 247 | } 248 | 249 | bool kfusion::cuda::DeviceMemory2D::empty() const { return !data_; } 250 | int kfusion::cuda::DeviceMemory2D::colsBytes() const { return colsBytes_; } 251 | int kfusion::cuda::DeviceMemory2D::rows() const { return rows_; } 252 | size_t kfusion::cuda::DeviceMemory2D::step() const { return step_; } 253 | -------------------------------------------------------------------------------- /kfusion/src/imgproc.cpp: -------------------------------------------------------------------------------- 1 | #include "precomp.hpp" 2 | /** 3 | * 4 | * \param in 5 | * \param out 6 | * \param kernel_size 7 | * \param sigma_spatial 8 | * \param sigma_depth 9 | */ 10 | void kfusion::cuda::depthBilateralFilter(const Depth& in, Depth& out, int kernel_size, float sigma_spatial, float sigma_depth) 11 | { 12 | out.create(in.rows(), in.cols()); 13 | device::bilateralFilter(in, out, kernel_size, sigma_spatial, sigma_depth); 14 | } 15 | 16 | /** 17 | * 18 | * \param depth 19 | * \param threshold 20 | */ 21 | void kfusion::cuda::depthTruncation(Depth& depth, float threshold) 22 | { 23 | device::truncateDepth(depth, threshold); 24 | } 25 | 26 | /** 27 | * 28 | * \param depth 29 | * \param pyramid 30 | * \param sigma_depth 31 | */ 32 | void kfusion::cuda::depthBuildPyramid(const Depth& depth, Depth& pyramid, float sigma_depth) 33 | { 34 | pyramid.create(depth.rows () / 2, depth.cols () / 2); 35 | device::depthPyr(depth, pyramid, sigma_depth); 36 | } 37 | 38 | /** 39 | * 40 | */ 41 | void kfusion::cuda::waitAllDefaultStream() 42 | { 43 | cudaSafeCall(cudaDeviceSynchronize()); 44 | } 45 | 46 | /** 47 | * 48 | * \param intr 49 | * \param depth 50 | * \param normals 51 | */ 52 | void kfusion::cuda::computeNormalsAndMaskDepth(const Intr& intr, Depth& depth, Normals& normals) 53 | { 54 | normals.create(depth.rows(), depth.cols()); 55 | 56 | device::Reprojector reproj(intr.fx, intr.fy, intr.cx, intr.cy); 57 | 58 | device::Normals& n = (device::Normals&)normals; 59 | device::computeNormalsAndMaskDepth(reproj, depth, n); 60 | } 61 | 62 | /** 63 | * 64 | * \param intr 65 | * \param depth 66 | * \param points 67 | * \param normals 68 | */ 69 | void kfusion::cuda::computePointNormals(const Intr& intr, const Depth& depth, Cloud& points, Normals& normals) 70 | { 71 | points.create(depth.rows(), depth.cols()); 72 | normals.create(depth.rows(), depth.cols()); 73 | 74 | device::Reprojector reproj(intr.fx, intr.fy, intr.cx, intr.cy); 75 | 76 | device::Points& p = (device::Points&)points; 77 | device::Normals& n = (device::Normals&)normals; 78 | device::computePointNormals(reproj, depth, p, n); 79 | } 80 | 81 | /** 82 | * 83 | * \param depth 84 | * \param dists 85 | * \param intr 86 | */ 87 | void kfusion::cuda::computeDists(const Depth& depth, Dists& dists, const Intr& intr) 88 | { 89 | dists.create(depth.rows(), depth.cols()); 90 | device::compute_dists(depth, dists, make_float2(intr.fx, intr.fy), make_float2(intr.cx, intr.cy)); 91 | } 92 | 93 | /** 94 | * 95 | * \param cloud 96 | * \param depth 97 | */ 98 | void kfusion::cuda::cloudToDepth(const Cloud& cloud, Depth& depth) 99 | { 100 | depth.create(cloud.rows(), cloud.cols()); 101 | device::Points points = (device::Points&)cloud; 102 | device::cloud_to_depth(points, depth); 103 | } 104 | 105 | /** 106 | * 107 | * \param depth 108 | * \param normals 109 | * \param depth_out 110 | * \param normals_out 111 | */ 112 | void kfusion::cuda::resizeDepthNormals(const Depth& depth, const Normals& normals, Depth& depth_out, Normals& normals_out) 113 | { 114 | depth_out.create(depth.rows() / 2, depth.cols() / 2); 115 | normals_out.create(normals.rows() / 2, normals.cols() / 2); 116 | 117 | device::Normals& nsrc = (device::Normals&)normals; 118 | device::Normals& ndst = (device::Normals&)normals_out; 119 | 120 | device::resizeDepthNormals(depth, nsrc, depth_out, ndst); 121 | } 122 | 123 | /** 124 | * 125 | * \param points 126 | * \param normals 127 | * \param points_out 128 | * \param normals_out 129 | */ 130 | void kfusion::cuda::resizePointsNormals(const Cloud& points, const Normals& normals, Cloud& points_out, Normals& normals_out) 131 | { 132 | points_out.create(points.rows() / 2, points.cols() / 2); 133 | normals_out.create(normals.rows() / 2, normals.cols() / 2); 134 | 135 | device::Points& pi = (device::Points&)points; 136 | device::Normals& ni= (device::Normals&)normals; 137 | 138 | device::Points& po = (device::Points&)points_out; 139 | device::Normals& no = (device::Normals&)normals_out; 140 | 141 | device::resizePointsNormals(pi, ni, po, no); 142 | } 143 | 144 | /** 145 | * 146 | * \param depth 147 | * \param normals 148 | * \param intr 149 | * \param light_pose 150 | * \param image 151 | */ 152 | void kfusion::cuda::renderImage(const Depth& depth, const Normals& normals, const Intr& intr, const Vec3f& light_pose, Image& image) 153 | { 154 | image.create(depth.rows(), depth.cols()); 155 | 156 | const device::Depth& d = (const device::Depth&)depth; 157 | const device::Normals& n = (const device::Normals&)normals; 158 | device::Reprojector reproj(intr.fx, intr.fy, intr.cx, intr.cy); 159 | device::Vec3f light = device_cast(light_pose); 160 | 161 | device::Image& i = (device::Image&)image; 162 | device::renderImage(d, n, reproj, light, i); 163 | waitAllDefaultStream(); 164 | } 165 | 166 | /** 167 | * 168 | * \param points 169 | * \param normals 170 | * \param intr 171 | * \param light_pose 172 | * \param image 173 | */ 174 | void kfusion::cuda::renderImage(const Cloud& points, const Normals& normals, const Intr& intr, const Vec3f& light_pose, Image& image) 175 | { 176 | image.create(points.rows(), points.cols()); 177 | 178 | const device::Points& p = (const device::Points&)points; 179 | const device::Normals& n = (const device::Normals&)normals; 180 | device::Reprojector reproj(intr.fx, intr.fy, intr.cx, intr.cy); 181 | device::Vec3f light = device_cast(light_pose); 182 | 183 | device::Image& i = (device::Image&)image; 184 | device::renderImage(p, n, reproj, light, i); 185 | waitAllDefaultStream(); 186 | } 187 | 188 | /** 189 | * \brief 190 | * \param normals 191 | * \param image 192 | */ 193 | void kfusion::cuda::renderTangentColors(const Normals& normals, Image& image) 194 | { 195 | image.create(normals.rows(), normals.cols()); 196 | const device::Normals& n = (const device::Normals&)normals; 197 | device::Image& i = (device::Image&)image; 198 | 199 | device::renderTangentColors(n, i); 200 | waitAllDefaultStream(); 201 | } 202 | -------------------------------------------------------------------------------- /kfusion/src/internal.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "safe_call.hpp" 5 | #include 6 | //#define USE_DEPTH 7 | 8 | namespace kfusion 9 | { 10 | namespace device 11 | { 12 | typedef float4 Normal; 13 | typedef float4 Point; 14 | 15 | typedef unsigned short ushort; 16 | typedef unsigned char uchar; 17 | 18 | typedef PtrStepSz Dists; 19 | typedef DeviceArray2D Depth; 20 | typedef DeviceArray2D Normals; 21 | typedef DeviceArray2D Points; 22 | typedef DeviceArray2D Image; 23 | 24 | typedef int3 Vec3i; 25 | typedef float3 Vec3f; 26 | struct Mat3f { float3 data[3]; }; 27 | struct Aff3f { Mat3f R; Vec3f t; }; 28 | 29 | struct TsdfVolume 30 | { 31 | public: 32 | typedef ushort2 elem_type; 33 | 34 | elem_type *const data; 35 | const int3 dims; 36 | const float3 voxel_size; 37 | const float trunc_dist; 38 | const int max_weight; 39 | 40 | TsdfVolume(elem_type* data, int3 dims, float3 voxel_size, float trunc_dist, int max_weight); 41 | //TsdfVolume(const TsdfVolume&); 42 | 43 | __kf_device__ elem_type* operator()(int x, int y, int z); 44 | __kf_device__ const elem_type* operator() (int x, int y, int z) const ; 45 | __kf_device__ elem_type* beg(int x, int y) const; 46 | __kf_device__ elem_type* zstep(elem_type *const ptr) const; 47 | private: 48 | TsdfVolume& operator=(const TsdfVolume&); 49 | }; 50 | 51 | struct Projector 52 | { 53 | float2 f, c; 54 | Projector(){} 55 | Projector(float fx, float fy, float cx, float cy); 56 | __kf_device__ float2 operator()(const float3& p) const; 57 | }; 58 | 59 | struct Reprojector 60 | { 61 | Reprojector() {} 62 | Reprojector(float fx, float fy, float cx, float cy); 63 | float2 finv, c; 64 | __kf_device__ float3 operator()(int x, int y, float z) const; 65 | }; 66 | 67 | struct ComputeIcpHelper 68 | { 69 | struct Policy; 70 | struct PageLockHelper 71 | { 72 | float* data; 73 | PageLockHelper(); 74 | ~PageLockHelper(); 75 | }; 76 | 77 | float min_cosine; 78 | float dist2_thres; 79 | 80 | Aff3f aff; 81 | 82 | float rows, cols; 83 | float2 f, c, finv; 84 | 85 | PtrStep dcurr; 86 | PtrStep ncurr; 87 | PtrStep vcurr; 88 | 89 | ComputeIcpHelper(float dist_thres, float angle_thres); 90 | void setLevelIntr(int level_index, float fx, float fy, float cx, float cy); 91 | 92 | void operator()(const Depth& dprev, const Normals& nprev, DeviceArray2D& buffer, float* data, cudaStream_t stream); 93 | void operator()(const Points& vprev, const Normals& nprev, DeviceArray2D& buffer, float* data, cudaStream_t stream); 94 | 95 | static void allocate_buffer(DeviceArray2D& buffer, int partials_count = -1); 96 | 97 | //private: 98 | __kf_device__ int find_coresp(int x, int y, float3& n, float3& d, float3& s) const; 99 | __kf_device__ void partial_reduce(const float row[7], PtrStep& partial_buffer) const; 100 | __kf_device__ float2 proj(const float3& p) const; 101 | __kf_device__ float3 reproj(float x, float y, float z) const; 102 | }; 103 | 104 | //tsdf volume functions 105 | void clear_volume(TsdfVolume volume); 106 | void integrate(const Dists& depth, TsdfVolume& volume, const Aff3f& aff, const Projector& proj); 107 | void project(const Dists& depth, Points& vertices, const Projector& proj); 108 | void project_and_remove(PtrStepSz& dists, Points &vertices, const Projector &proj); 109 | void project_and_remove(const PtrStepSz& dists, Points &vertices, const Projector &proj); 110 | 111 | void raycast(const TsdfVolume& volume, const Aff3f& aff, const Mat3f& Rinv, 112 | const Reprojector& reproj, Depth& depth, Normals& normals, float step_factor, float delta_factor); 113 | 114 | void raycast(const TsdfVolume& volume, const Aff3f& aff, const Mat3f& Rinv, 115 | const Reprojector& reproj, Points& points, Normals& normals, float step_factor, float delta_factor); 116 | 117 | __kf_device__ ushort2 pack_tsdf(float tsdf, int weight); 118 | __kf_device__ float unpack_tsdf(ushort2 value, int& weight); 119 | __kf_device__ float unpack_tsdf(ushort2 value); 120 | 121 | 122 | //image proc functions 123 | void compute_dists(const Depth& depth, Dists dists, float2 f, float2 c); 124 | void cloud_to_depth(const Points& cloud, Depth depth); 125 | 126 | void truncateDepth(Depth& depth, float max_dist /*meters*/); 127 | void bilateralFilter(const Depth& src, Depth& dst, int kernel_size, float sigma_spatial, float sigma_depth); 128 | void depthPyr(const Depth& source, Depth& pyramid, float sigma_depth); 129 | 130 | void resizeDepthNormals(const Depth& depth, const Normals& normals, Depth& depth_out, Normals& normals_out); 131 | void resizePointsNormals(const Points& points, const Normals& normals, Points& points_out, Normals& normals_out); 132 | 133 | void computeNormalsAndMaskDepth(const Reprojector& reproj, Depth& depth, Normals& normals); 134 | void computePointNormals(const Reprojector& reproj, const Depth& depth, Points& points, Normals& normals); 135 | 136 | void renderImage(const Depth& depth, const Normals& normals, const Reprojector& reproj, const Vec3f& light_pose, Image& image); 137 | void renderImage(const Points& points, const Normals& normals, const Reprojector& reproj, const Vec3f& light_pose, Image& image); 138 | void renderTangentColors(const Normals& normals, Image& image); 139 | 140 | 141 | //exctraction functionality 142 | size_t extractCloud(const TsdfVolume& volume, const Aff3f& aff, PtrSz output); 143 | void extractNormals(const TsdfVolume& volume, const PtrSz& points, const Aff3f& aff, const Mat3f& Rinv, float gradient_delta_factor, float4* output); 144 | 145 | struct float8 { float x, y, z, w, c1, c2, c3, c4; }; 146 | struct float12 { float x, y, z, w, normal_x, normal_y, normal_z, n4, c1, c2, c3, c4; }; 147 | void mergePointNormal(const DeviceArray& cloud, const DeviceArray& normals, const DeviceArray& output); 148 | } 149 | } 150 | -------------------------------------------------------------------------------- /kfusion/src/mLibSource.cpp: -------------------------------------------------------------------------------- 1 | #include "opt/mLibInclude.h" 2 | 3 | #include "mLibCore.cpp" 4 | #include "mLibLodePNG.cpp" 5 | -------------------------------------------------------------------------------- /kfusion/src/precomp.cpp: -------------------------------------------------------------------------------- 1 | #include "precomp.hpp" 2 | #include "internal.hpp" 3 | 4 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 5 | /// Kinfu/types implementation 6 | 7 | kfusion::Intr::Intr () {} 8 | kfusion::Intr::Intr (float fx_, float fy_, float cx_, float cy_) : fx(fx_), fy(fy_), cx(cx_), cy(cy_) {} 9 | 10 | kfusion::Intr kfusion::Intr::operator()(int level_index) const 11 | { 12 | int div = 1 << level_index; 13 | return (Intr (fx / div, fy / div, cx / div, cy / div)); 14 | } 15 | 16 | std::ostream& operator << (std::ostream& os, const kfusion::Intr& intr) 17 | { 18 | return os << "([f = " << intr.fx << ", " << intr.fy << "] [cp = " << intr.cx << ", " << intr.cy << "])"; 19 | } 20 | 21 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 22 | /// TsdfVolume host implementation 23 | 24 | kfusion::device::TsdfVolume::TsdfVolume(elem_type* _data, int3 _dims, float3 _voxel_size, float _trunc_dist, int _max_weight) 25 | : data(_data), dims(_dims), voxel_size(_voxel_size), trunc_dist(_trunc_dist), max_weight(_max_weight) {} 26 | 27 | //kfusion::device::TsdfVolume::elem_type* kfusionl::device::TsdfVolume::operator()(int x, int y, int z) 28 | //{ return data + x + y*dims.x + z*dims.y*dims.x; } 29 | // 30 | //const kfusion::device::TsdfVolume::elem_type* kfusionl::device::TsdfVolume::operator() (int x, int y, int z) const 31 | //{ return data + x + y*dims.x + z*dims.y*dims.x; } 32 | // 33 | //kfusion::device::TsdfVolume::elem_type* kfusionl::device::TsdfVolume::beg(int x, int y) const 34 | //{ return data + x + dims.x * y; } 35 | // 36 | //kfusion::device::TsdfVolume::elem_type* kfusionl::device::TsdfVolume::zstep(elem_type *const ptr) const 37 | //{ return data + dims.x * dims.y; } 38 | 39 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 40 | /// Projector host implementation 41 | 42 | kfusion::device::Projector::Projector(float fx, float fy, float cx, float cy) : f(make_float2(fx, fy)), c(make_float2(cx, cy)) {} 43 | 44 | //float2 kfusion::device::Projector::operator()(const float3& p) const 45 | //{ 46 | // float2 coo; 47 | // coo.x = p.x * f.x / p.z + c.x; 48 | // coo.y = p.y * f.y / p.z + c.y; 49 | // return coo; 50 | //} 51 | 52 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 53 | /// Reprojector host implementation 54 | 55 | kfusion::device::Reprojector::Reprojector(float fx, float fy, float cx, float cy) : finv(make_float2(1.f/fx, 1.f/fy)), c(make_float2(cx, cy)) {} 56 | 57 | //float3 kfusion::device::Reprojector::operator()(int u, int v, float z) const 58 | //{ 59 | // float x = z * (u - c.x) * finv.x; 60 | // float y = z * (v - c.y) * finv.y; 61 | // return make_float3(x, y, z); 62 | //} 63 | 64 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 65 | /// Host implementation of packing/unpacking tsdf volume element 66 | 67 | //ushort2 kfusion::device::pack_tsdf(float tsdf, int weight) { throw "Not implemented"; return ushort2(); } 68 | //float kfusion::device::unpack_tsdf(ushort2 value, int& weight) { throw "Not implemented"; return 0; } 69 | //float kfusion::device::unpack_tsdf(ushort2 value) { throw "Not implemented"; return 0; } 70 | 71 | -------------------------------------------------------------------------------- /kfusion/src/precomp.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "internal.hpp" 9 | #include 10 | #include "vector_functions.h" 11 | 12 | namespace kfusion 13 | { 14 | template 15 | inline D device_cast(const S& source) 16 | { 17 | return *reinterpret_cast(source.val); 18 | } 19 | 20 | template<> 21 | inline device::Aff3f device_cast(const Affine3f& source) 22 | { 23 | device::Aff3f aff; 24 | Mat3f R = source.rotation(); 25 | Vec3f t = source.translation(); 26 | aff.R = device_cast(R); 27 | aff.t = device_cast(t); 28 | return aff; 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /kfusion/src/projective_icp.cpp: -------------------------------------------------------------------------------- 1 | #include "precomp.hpp" 2 | 3 | 4 | using namespace kfusion; 5 | using namespace std; 6 | using namespace kfusion::cuda; 7 | 8 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 9 | /// ComputeIcpHelper 10 | 11 | kfusion::device::ComputeIcpHelper::ComputeIcpHelper(float dist_thres, float angle_thres) 12 | { 13 | min_cosine = cos(angle_thres); 14 | dist2_thres = dist_thres * dist_thres; 15 | } 16 | 17 | void kfusion::device::ComputeIcpHelper::setLevelIntr(int level_index, float fx, float fy, float cx, float cy) 18 | { 19 | int div = 1 << level_index; 20 | f = make_float2(fx/div, fy/div); 21 | c = make_float2(cx/div, cy/div); 22 | finv = make_float2(1.f/f.x, 1.f/f.y); 23 | } 24 | 25 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 26 | /// ProjectiveICP::StreamHelper 27 | 28 | struct kfusion::cuda::ProjectiveICP::StreamHelper 29 | { 30 | typedef device::ComputeIcpHelper::PageLockHelper PageLockHelper; 31 | typedef cv::Matx66f Mat6f; 32 | typedef cv::Vec6f Vec6f; 33 | 34 | cudaStream_t stream; 35 | PageLockHelper locked_buffer; 36 | 37 | StreamHelper() { cudaSafeCall( cudaStreamCreate(&stream) ); } 38 | ~StreamHelper() { cudaSafeCall( cudaStreamDestroy(stream) ); } 39 | 40 | operator float*() { return locked_buffer.data; } 41 | operator cudaStream_t() { return stream; } 42 | 43 | Mat6f get(Vec6f& b) 44 | { 45 | cudaSafeCall( cudaStreamSynchronize(stream) ); 46 | 47 | Mat6f A; 48 | float *data_A = A.val; 49 | float *data_b = b.val; 50 | 51 | int shift = 0; 52 | for (int i = 0; i < 6; ++i) //rows 53 | for (int j = i; j < 7; ++j) // cols + b 54 | { 55 | float value = locked_buffer.data[shift++]; 56 | if (j == 6) // vector b 57 | data_b[i] = value; 58 | else 59 | data_A[j * 6 + i] = data_A[i * 6 + j] = value; 60 | } 61 | return A; 62 | } 63 | }; 64 | 65 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////// 66 | /// ProjectiveICP 67 | 68 | kfusion::cuda::ProjectiveICP::ProjectiveICP() : angle_thres_(deg2rad(20.f)), dist_thres_(0.1f) 69 | { 70 | const int iters[] = {10, 5, 4, 0}; 71 | std::vector vector_iters(iters, iters + 4); 72 | setIterationsNum(vector_iters); 73 | device::ComputeIcpHelper::allocate_buffer(buffer_); 74 | 75 | shelp_ = cv::Ptr(new StreamHelper()); 76 | } 77 | 78 | kfusion::cuda::ProjectiveICP::~ProjectiveICP() {} 79 | 80 | float kfusion::cuda::ProjectiveICP::getDistThreshold() const 81 | { return dist_thres_; } 82 | 83 | void kfusion::cuda::ProjectiveICP::setDistThreshold(float distance) 84 | { dist_thres_ = distance; } 85 | 86 | float kfusion::cuda::ProjectiveICP::getAngleThreshold() const 87 | { return angle_thres_; } 88 | 89 | void kfusion::cuda::ProjectiveICP::setAngleThreshold(float angle) 90 | { angle_thres_ = angle; } 91 | 92 | void kfusion::cuda::ProjectiveICP::setIterationsNum(const std::vector& iters) 93 | { 94 | if (iters.size() >= MAX_PYRAMID_LEVELS) 95 | iters_.assign(iters.begin(), iters.begin() + MAX_PYRAMID_LEVELS); 96 | else 97 | { 98 | iters_ = vector(MAX_PYRAMID_LEVELS, 0); 99 | copy(iters.begin(), iters.end(),iters_.begin()); 100 | } 101 | } 102 | 103 | int kfusion::cuda::ProjectiveICP::getUsedLevelsNum() const 104 | { 105 | int i = MAX_PYRAMID_LEVELS - 1; 106 | for(; i >= 0 && !iters_[i]; --i); 107 | return i + 1; 108 | } 109 | 110 | bool kfusion::cuda::ProjectiveICP::estimateTransform(Affine3f& /*affine*/, const Intr& /*intr*/, const Frame& /*curr*/, const Frame& /*prev*/) 111 | { 112 | // bool has_depth = !curr.depth_pyr.empty() && !prev.depth_pyr.empty(); 113 | // bool has_points = !curr.points_pyr.empty() && !prev.points_pyr.empty(); 114 | 115 | // if (has_depth) 116 | // return estimateTransform(affine, intr, curr.depth_pyr, curr.normals_pyr, prev.depth_pyr, prev.normals_pyr); 117 | // else if(has_points) 118 | // return estimateTransform(affine, intr, curr.points_pyr, curr.normals_pyr, prev.points_pyr, prev.normals_pyr); 119 | // else 120 | // CV_Assert(!"Wrong parameters passed to estimateTransform"); 121 | CV_Assert(!"Not implemented"); 122 | return false; 123 | } 124 | 125 | bool kfusion::cuda::ProjectiveICP::estimateTransform(Affine3f& affine, const Intr& intr, const DepthPyr& dcurr, const NormalsPyr ncurr, const DepthPyr dprev, const NormalsPyr nprev) 126 | { 127 | const int LEVELS = getUsedLevelsNum(); 128 | StreamHelper& sh = *shelp_; 129 | 130 | device::ComputeIcpHelper helper(dist_thres_, angle_thres_); 131 | affine = Affine3f::Identity(); 132 | 133 | for(int level_index = LEVELS - 1; level_index >= 0; --level_index) 134 | { 135 | const device::Normals& n = (const device::Normals& )nprev[level_index]; 136 | 137 | helper.rows = (float)n.rows(); 138 | helper.cols = (float)n.cols(); 139 | helper.setLevelIntr(level_index, intr.fx, intr.fy, intr.cx, intr.cy); 140 | helper.dcurr = dcurr[level_index]; 141 | helper.ncurr = ncurr[level_index]; 142 | 143 | for(int iter = 0; iter < iters_[level_index]; ++iter) 144 | { 145 | helper.aff = device_cast(affine); 146 | helper(dprev[level_index], n, buffer_, sh, sh); 147 | 148 | StreamHelper::Vec6f b; 149 | StreamHelper::Mat6f A = sh.get(b); 150 | 151 | //checking nullspace 152 | double det = cv::determinant(A); 153 | 154 | if (fabs (det) < 1e-15 || cv::viz::isNan(det)) 155 | { 156 | if (cv::viz::isNan(det)) cout << "qnan" << endl; 157 | return false; 158 | } 159 | 160 | StreamHelper::Vec6f r; 161 | cv::solve(A, b, r, cv::DECOMP_SVD); 162 | Affine3f Tinc(Vec3f(r.val), Vec3f(r.val+3)); 163 | affine = Tinc * affine; 164 | } 165 | } 166 | return true; 167 | } 168 | 169 | bool kfusion::cuda::ProjectiveICP::estimateTransform(Affine3f& affine, const Intr& intr, const PointsPyr& vcurr, const NormalsPyr ncurr, const PointsPyr vprev, const NormalsPyr nprev) 170 | { 171 | const int LEVELS = getUsedLevelsNum(); 172 | StreamHelper& sh = *shelp_; 173 | 174 | device::ComputeIcpHelper helper(dist_thres_, angle_thres_); 175 | affine = Affine3f::Identity(); 176 | 177 | for(int level_index = LEVELS - 1; level_index >= 0; --level_index) 178 | { 179 | const device::Normals& n = (const device::Normals& )nprev[level_index]; 180 | const device::Points& v = (const device::Points& )vprev[level_index]; 181 | 182 | helper.rows = (float)n.rows(); 183 | helper.cols = (float)n.cols(); 184 | helper.setLevelIntr(level_index, intr.fx, intr.fy, intr.cx, intr.cy); 185 | helper.vcurr = vcurr[level_index]; 186 | helper.ncurr = ncurr[level_index]; 187 | 188 | for(int iter = 0; iter < iters_[level_index]; ++iter) 189 | { 190 | helper.aff = device_cast(affine); 191 | helper(v, n, buffer_, sh, sh); 192 | 193 | StreamHelper::Vec6f b; 194 | StreamHelper::Mat6f A = sh.get(b); 195 | 196 | //checking nullspace 197 | double det = cv::determinant(A); 198 | 199 | if (fabs (det) < 1e-15 || cv::viz::isNan (det)) 200 | { 201 | if (cv::viz::isNan (det)) cout << "qnan" << endl; 202 | return false; 203 | } 204 | 205 | StreamHelper::Vec6f r; 206 | cv::solve(A, b, r, cv::DECOMP_SVD); 207 | 208 | Affine3f Tinc(Vec3f(r.val), Vec3f(r.val+3)); 209 | affine = Tinc * affine; 210 | } 211 | } 212 | return true; 213 | } 214 | 215 | -------------------------------------------------------------------------------- /kfusion/src/safe_call.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "cuda_runtime_api.h" 4 | 5 | namespace kfusion 6 | { 7 | namespace cuda 8 | { 9 | void error(const char *error_string, const char *file, const int line, const char *func); 10 | } 11 | } 12 | 13 | #if defined(__GNUC__) 14 | #define cudaSafeCall(expr) kfusion::cuda::___cudaSafeCall(expr, __FILE__, __LINE__, __func__) 15 | #else /* defined(__CUDACC__) || defined(__MSVC__) */ 16 | #define cudaSafeCall(expr) kfusion::cuda::___cudaSafeCall(expr, __FILE__, __LINE__) 17 | #endif 18 | 19 | namespace kfusion 20 | { 21 | namespace cuda 22 | { 23 | static inline void ___cudaSafeCall(cudaError_t err, const char *file, const int line, const char *func = "") 24 | { 25 | if (cudaSuccess != err) 26 | error(cudaGetErrorString(err), file, line, func); 27 | } 28 | 29 | static inline int divUp(int total, int grain) { return (total + grain - 1) / grain; } 30 | } 31 | 32 | namespace device 33 | { 34 | using kfusion::cuda::divUp; 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /kfusion/src/tsdf_volume.cpp: -------------------------------------------------------------------------------- 1 | #include "precomp.hpp" 2 | #include 3 | using namespace kfusion; 4 | using namespace kfusion::cuda; 5 | 6 | 7 | kfusion::cuda::TsdfVolume::TsdfVolume(const Vec3i& dims) : data_(), 8 | trunc_dist_(0.03f), 9 | max_weight_(128), 10 | dims_(dims), 11 | size_(Vec3f::all(3.f)), 12 | pose_(Affine3f::Identity()), 13 | gradient_delta_factor_(0.75f), 14 | raycast_step_factor_(0.75f) 15 | { 16 | create(dims_); 17 | } 18 | 19 | kfusion::cuda::TsdfVolume::~TsdfVolume() 20 | { 21 | delete cloud_host_; 22 | delete cloud_buffer_; 23 | delete cloud_; 24 | delete normal_host_; 25 | delete normal_buffer_; 26 | } 27 | 28 | /** 29 | * \brief 30 | * \param dims 31 | */ 32 | void kfusion::cuda::TsdfVolume::create(const Vec3i& dims) 33 | { 34 | dims_ = dims; 35 | int voxels_number = dims_[0] * dims_[1] * dims_[2]; 36 | data_.create(voxels_number * sizeof(int)); 37 | setTruncDist(trunc_dist_); 38 | clear(); 39 | } 40 | 41 | /** 42 | * \brief 43 | * \return 44 | */ 45 | Vec3i kfusion::cuda::TsdfVolume::getDims() const 46 | { 47 | return dims_; 48 | } 49 | 50 | /** 51 | * \brief 52 | * \return 53 | */ 54 | Vec3f kfusion::cuda::TsdfVolume::getVoxelSize() const 55 | { 56 | return Vec3f(size_[0] / dims_[0], size_[1] / dims_[1], size_[2] / dims_[2]); 57 | } 58 | 59 | const CudaData kfusion::cuda::TsdfVolume::data() const { return data_; } 60 | CudaData kfusion::cuda::TsdfVolume::data() { return data_; } 61 | Vec3f kfusion::cuda::TsdfVolume::getSize() const { return size_; } 62 | 63 | void kfusion::cuda::TsdfVolume::setSize(const Vec3f& size) 64 | { size_ = size; setTruncDist(trunc_dist_); } 65 | 66 | float kfusion::cuda::TsdfVolume::getTruncDist() const { return trunc_dist_; } 67 | 68 | void kfusion::cuda::TsdfVolume::setTruncDist(float distance) 69 | { 70 | Vec3f vsz = getVoxelSize(); 71 | float max_coeff = std::max(std::max(vsz[0], vsz[1]), vsz[2]); 72 | trunc_dist_ = std::max (distance, 2.1f * max_coeff); 73 | } 74 | cv::Mat kfusion::cuda::TsdfVolume::get_cloud_host() const {return *cloud_host_;}; 75 | cv::Mat kfusion::cuda::TsdfVolume::get_normal_host() const {return *normal_host_;}; 76 | cv::Mat* kfusion::cuda::TsdfVolume::get_cloud_host_ptr() const {return cloud_host_;}; 77 | cv::Mat* kfusion::cuda::TsdfVolume::get_normal_host_ptr() const {return normal_host_;}; 78 | 79 | int kfusion::cuda::TsdfVolume::getMaxWeight() const { return max_weight_; } 80 | void kfusion::cuda::TsdfVolume::setMaxWeight(int weight) { max_weight_ = weight; } 81 | Affine3f kfusion::cuda::TsdfVolume::getPose() const { return pose_; } 82 | void kfusion::cuda::TsdfVolume::setPose(const Affine3f& pose) { pose_ = pose; } 83 | float kfusion::cuda::TsdfVolume::getRaycastStepFactor() const { return raycast_step_factor_; } 84 | void kfusion::cuda::TsdfVolume::setRaycastStepFactor(float factor) { raycast_step_factor_ = factor; } 85 | float kfusion::cuda::TsdfVolume::getGradientDeltaFactor() const { return gradient_delta_factor_; } 86 | void kfusion::cuda::TsdfVolume::setGradientDeltaFactor(float factor) { gradient_delta_factor_ = factor; } 87 | void kfusion::cuda::TsdfVolume::swap(CudaData& data) { data_.swap(data); } 88 | void kfusion::cuda::TsdfVolume::applyAffine(const Affine3f& affine) { pose_ = affine * pose_; } 89 | void kfusion::cuda::TsdfVolume::clear() 90 | { 91 | cloud_buffer_ = new cuda::DeviceArray(); 92 | cloud_ = new cuda::DeviceArray(); 93 | normal_buffer_ = new cuda::DeviceArray(); 94 | cloud_host_ = new cv::Mat(); 95 | normal_host_ = new cv::Mat(); 96 | 97 | device::Vec3i dims = device_cast(dims_); 98 | device::Vec3f vsz = device_cast(getVoxelSize()); 99 | 100 | device::TsdfVolume volume(data_.ptr(), dims, vsz, trunc_dist_, max_weight_); 101 | device::clear_volume(volume); 102 | } 103 | 104 | /** 105 | * \brief 106 | * \param dists 107 | * \param camera_pose 108 | * \param intr 109 | */ 110 | void kfusion::cuda::TsdfVolume::integrate(const Dists& dists, const Affine3f& camera_pose, const Intr& intr) 111 | { 112 | Affine3f vol2cam = camera_pose.inv() * pose_; 113 | 114 | device::Projector proj(intr.fx, intr.fy, intr.cx, intr.cy); 115 | 116 | device::Vec3i dims = device_cast(dims_); 117 | device::Vec3f vsz = device_cast(getVoxelSize()); 118 | device::Aff3f aff = device_cast(vol2cam); 119 | 120 | device::TsdfVolume volume(data_.ptr(), dims, vsz, trunc_dist_, max_weight_); 121 | device::integrate(dists, volume, aff, proj); 122 | } 123 | 124 | /** 125 | * \brief 126 | * \param camera_pose 127 | * \param intr 128 | * \param depth 129 | * \param normals 130 | */ 131 | void kfusion::cuda::TsdfVolume::raycast(const Affine3f& camera_pose, const Intr& intr, Depth& depth, Normals& normals) 132 | { 133 | DeviceArray2D& n = (DeviceArray2D&)normals; 134 | 135 | Affine3f cam2vol = pose_.inv() * camera_pose; 136 | 137 | device::Aff3f aff = device_cast(cam2vol); 138 | device::Mat3f Rinv = device_cast(cam2vol.rotation().inv(cv::DECOMP_SVD)); 139 | 140 | device::Reprojector reproj(intr.fx, intr.fy, intr.cx, intr.cy); 141 | 142 | device::Vec3i dims = device_cast(dims_); 143 | device::Vec3f vsz = device_cast(getVoxelSize()); 144 | 145 | device::TsdfVolume volume(data_.ptr(), dims, vsz, trunc_dist_, max_weight_); 146 | device::raycast(volume, aff, Rinv, reproj, depth, n, raycast_step_factor_, gradient_delta_factor_); 147 | 148 | } 149 | 150 | /** 151 | * \brief 152 | * \param camera_pose 153 | * \param intr 154 | * \param points 155 | * \param normals 156 | */ 157 | void kfusion::cuda::TsdfVolume::raycast(const Affine3f& camera_pose, const Intr& intr, Cloud& points, Normals& normals) 158 | { 159 | device::Normals& n = (device::Normals&)normals; 160 | device::Points& p = (device::Points&)points; 161 | 162 | Affine3f cam2vol = pose_.inv() * camera_pose; 163 | 164 | device::Aff3f aff = device_cast(cam2vol); 165 | device::Mat3f Rinv = device_cast(cam2vol.rotation().inv(cv::DECOMP_SVD)); 166 | 167 | device::Reprojector reproj(intr.fx, intr.fy, intr.cx, intr.cy); 168 | 169 | device::Vec3i dims = device_cast(dims_); 170 | device::Vec3f vsz = device_cast(getVoxelSize()); 171 | 172 | device::TsdfVolume volume(data_.ptr(), dims, vsz, trunc_dist_, max_weight_); 173 | device::raycast(volume, aff, Rinv, reproj, p, n, raycast_step_factor_, gradient_delta_factor_); 174 | } 175 | 176 | /** 177 | * \brief 178 | * \param cloud_buffer 179 | * \return 180 | */ 181 | DeviceArray kfusion::cuda::TsdfVolume::fetchCloud(DeviceArray& cloud_buffer) const 182 | { 183 | // enum { DEFAULT_CLOUD_BUFFER_SIZE = 10 * 1000 * 1000 }; 184 | enum { DEFAULT_CLOUD_BUFFER_SIZE = 256 * 256 * 256 }; 185 | 186 | if (cloud_buffer.empty ()) 187 | cloud_buffer.create (DEFAULT_CLOUD_BUFFER_SIZE); 188 | 189 | DeviceArray& b = (DeviceArray&)cloud_buffer; 190 | 191 | device::Vec3i dims = device_cast(dims_); 192 | device::Vec3f vsz = device_cast(getVoxelSize()); 193 | device::Aff3f aff = device_cast(pose_); 194 | 195 | device::TsdfVolume volume((ushort2*)data_.ptr(), dims, vsz, trunc_dist_, max_weight_); 196 | size_t size = extractCloud(volume, aff, b); 197 | 198 | return DeviceArray((Point*)cloud_buffer.ptr(), size); 199 | } 200 | 201 | /** 202 | * 203 | * @param cloud 204 | * @param normals 205 | */ 206 | void kfusion::cuda::TsdfVolume::fetchNormals(const DeviceArray& cloud, DeviceArray& normals) const 207 | { 208 | normals.create(cloud.size()); 209 | DeviceArray& c = (DeviceArray&)cloud; 210 | 211 | device::Vec3i dims = device_cast(dims_); 212 | device::Vec3f vsz = device_cast(getVoxelSize()); 213 | device::Aff3f aff = device_cast(pose_); 214 | device::Mat3f Rinv = device_cast(pose_.rotation().inv(cv::DECOMP_SVD)); 215 | 216 | device::TsdfVolume volume((ushort2*)data_.ptr(), dims, vsz, trunc_dist_, max_weight_); 217 | device::extractNormals(volume, c, aff, Rinv, gradient_delta_factor_, (float4*)normals.ptr()); 218 | } 219 | 220 | 221 | /** 222 | * \brief 223 | * \param warp_field 224 | * \param depth_img 225 | * \param camera_pose 226 | * \param intr 227 | */ 228 | void kfusion::cuda::TsdfVolume::surface_fusion(const WarpField& warp_field, 229 | std::vector warped, 230 | std::vector canonical, 231 | cuda::Depth& depth, 232 | const Affine3f& camera_pose, 233 | const Intr& intr) 234 | { 235 | std::vector ro = psdf(warped, depth, intr); 236 | 237 | cuda::Dists dists; 238 | cuda::computeDists(depth, dists, intr); 239 | integrate(dists, camera_pose, intr); 240 | 241 | for(size_t i = 0; i < ro.size(); i++) 242 | { 243 | if(ro[i] > -trunc_dist_) 244 | { 245 | warp_field.KNN(canonical[i]); 246 | float weight = weighting(*(warp_field.getDistSquared()), KNN_NEIGHBOURS); 247 | float coeff = std::min(ro[i], trunc_dist_); 248 | 249 | // tsdf_entries[i].tsdf_value = tsdf_entries[i].tsdf_value * tsdf_entries[i].tsdf_weight + coeff * weight; 250 | // tsdf_entries[i].tsdf_value = tsdf_entries[i].tsdf_weight + weight; 251 | // 252 | // tsdf_entries[i].tsdf_weight = std::min(tsdf_entries[i].tsdf_weight + weight, W_MAX); 253 | } 254 | } 255 | } 256 | 257 | /** 258 | * \fn TSDF::psdf (Mat3f K, Depth& depth, Vec3f voxel_center) 259 | * \brief return a quaternion that is the spherical linear interpolation between q1 and q2 260 | * where percentage (from 0 to 1) defines the amount of interpolation 261 | * \param K: camera matrix 262 | * \param depth: a depth frame 263 | * \param voxel_center 264 | * 265 | */ 266 | std::vector kfusion::cuda::TsdfVolume::psdf(const std::vector& warped, 267 | Dists& dists, 268 | const Intr& intr) 269 | { 270 | device::Projector proj(intr.fx, intr.fy, intr.cx, intr.cy); 271 | std::vector> point_type(warped.size()); 272 | for(int i = 0; i < warped.size(); i++) 273 | { 274 | point_type[i].x = warped[i][0]; 275 | point_type[i].y = warped[i][1]; 276 | point_type[i].z = warped[i][2]; 277 | point_type[i].w = 0.f; 278 | } 279 | device::Points points; 280 | points.upload(point_type, dists.cols()); 281 | device::project_and_remove(dists, points, proj); 282 | int size; 283 | points.download(point_type, size); 284 | Mat3f K = Mat3f(intr.fx, 0, intr.cx, 285 | 0, intr.fy, intr.cy, 286 | 0, 0, 1).inv(); 287 | 288 | std::vector distances(warped.size()); 289 | for(int i = 0; i < warped.size(); i++) 290 | distances[i] = (K * Vec3f(point_type[i].x, point_type[i].y, point_type[i].z))[2] - warped[i][2]; 291 | return distances; 292 | } 293 | 294 | /** 295 | * \brief 296 | * \param dist_sqr 297 | * \param k 298 | * \return 299 | */ 300 | float kfusion::cuda::TsdfVolume::weighting(const std::vector& dist_sqr, int k) const 301 | { 302 | float distances = 0; 303 | for(auto distance : dist_sqr) 304 | distances += sqrt(distance); 305 | return distances / k; 306 | } 307 | /** 308 | * \brief 309 | * \param dist_sqr 310 | * \param k 311 | * \return 312 | */ 313 | void kfusion::cuda::TsdfVolume::compute_points() 314 | { 315 | *cloud_ = fetchCloud(*cloud_buffer_); 316 | *cloud_host_ = cv::Mat(1, (int)cloud_->size(), CV_32FC4); 317 | cloud_->download(cloud_host_->ptr()); 318 | } 319 | 320 | void kfusion::cuda::TsdfVolume::compute_normals() 321 | { 322 | fetchNormals(*cloud_, *normal_buffer_); 323 | *normal_host_ = cv::Mat(1, (int)cloud_->size(), CV_32FC4); 324 | normal_buffer_->download(normal_host_->ptr()); 325 | } 326 | -------------------------------------------------------------------------------- /kfusion/src/utils/dual_quaternion.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DYNAMIC_FUSION_DUAL_QUATERNION_HPP 2 | #define DYNAMIC_FUSION_DUAL_QUATERNION_HPP 3 | #include 4 | #include 5 | 6 | //Adapted from https://github.com/Poofjunior/QPose 7 | /** 8 | * \brief a dual quaternion class for encoding transformations. 9 | * \details transformations are stored as first a translation; then a 10 | * rotation. It is possible to switch the order. See this paper: 11 | * https://www.thinkmind.org/download.php?articleid=intsys_v6_n12_2013_5 12 | */ 13 | namespace kfusion { 14 | namespace utils { 15 | static float epsilon() 16 | { 17 | return 1e-6; 18 | } 19 | template 20 | class DualQuaternion { 21 | public: 22 | /** 23 | * \brief default constructor. 24 | */ 25 | DualQuaternion() 26 | { 27 | rotation_ = Quaternion(); 28 | translation_ = Quaternion(); 29 | }; 30 | ~DualQuaternion(){}; 31 | 32 | /** 33 | * \brief constructor that takes cartesian coordinates and Euler angles as 34 | * arguments. 35 | */ 36 | // FIXME: always use Rodrigues angles, not Euler 37 | DualQuaternion(T x, T y, T z, T roll, T pitch, T yaw) 38 | { 39 | // convert here. 40 | rotation_.w_ = cos(roll / 2) * cos(pitch / 2) * cos(yaw / 2) + 41 | sin(roll / 2) * sin(pitch / 2) * sin(yaw / 2); 42 | rotation_.x_ = sin(roll / 2) * cos(pitch / 2) * cos(yaw / 2) - 43 | cos(roll / 2) * sin(pitch / 2) * sin(yaw / 2); 44 | rotation_.y_ = cos(roll / 2) * sin(pitch / 2) * cos(yaw / 2) + 45 | sin(roll / 2) * cos(pitch / 2) * sin(yaw / 2); 46 | rotation_.z_ = cos(roll / 2) * cos(pitch / 2) * sin(yaw / 2) - 47 | sin(roll / 2) * sin(pitch / 2) * cos(yaw / 2); 48 | 49 | translation_ = 0.5 * Quaternion(0, x, y, z) * rotation_; 50 | } 51 | 52 | /** 53 | * \brief constructor that takes two quaternions as arguments. 54 | * \details The rotation 55 | * quaternion has the conventional encoding for a rotation as a 56 | * quaternion. The translation quaternion is a quaternion with 57 | * cartesian coordinates encoded as (0, x, y, z) 58 | */ 59 | DualQuaternion(Quaternion translation, Quaternion rotation) 60 | { 61 | rotation_ = rotation; 62 | translation_ = 0.5 * translation * rotation; 63 | } 64 | 65 | /** 66 | * \brief store a rotation 67 | * \param angle is in radians 68 | */ 69 | void encodeRotation(T angle, T x, T y, T z) 70 | { 71 | rotation_.encodeRotation(angle, x, y, z); 72 | } 73 | /** 74 | * \brief store a rotation 75 | * \param angle is in radians 76 | */ 77 | void encodeRotation(T x, T y, T z) 78 | { 79 | rotation_.encodeRotation(sqrt(x*x+y*y+z*z), x, y, z); 80 | } 81 | 82 | void encodeTranslation(T x, T y, T z) 83 | { 84 | translation_ = 0.5 * Quaternion(0, x, y, z) * rotation_; 85 | } 86 | 87 | /// handle accumulating error. 88 | void normalize() 89 | { 90 | T x, y, z; 91 | getTranslation(x, y, z); 92 | 93 | rotation_.normalize(); 94 | 95 | encodeTranslation(x, y, z); 96 | } 97 | 98 | /** 99 | * \brief a reference-based method for acquiring the latest 100 | * translation data. 101 | */ 102 | void getTranslation(T &x, T &y, T &z) const 103 | { 104 | Quaternion result = getTranslation(); 105 | /// note: inverse of a quaternion is the same as the conjugate. 106 | x = result.x_; 107 | y = result.y_; 108 | z = result.z_; 109 | } 110 | 111 | /** 112 | * \brief a reference-based method for acquiring the latest 113 | * translation data. 114 | */ 115 | void getTranslation(Vec3f& vec3f) const 116 | { 117 | getTranslation(vec3f[0], vec3f[1], vec3f[2]); 118 | } 119 | 120 | Quaternion getTranslation() const 121 | { 122 | auto rot = rotation_; 123 | rot.normalize(); 124 | return 2 * translation_ * rot.conjugate(); 125 | } 126 | 127 | 128 | /** 129 | * \brief a reference-based method for acquiring the latest rotation data. 130 | */ 131 | void getEuler(T &roll, T &pitch, T &yaw) 132 | { 133 | // FIXME: breaks for some value around PI. 134 | roll = getRoll(); 135 | pitch = getPitch(); 136 | yaw = getYaw(); 137 | } 138 | 139 | Quaternion getRotation() const 140 | { 141 | return rotation_; 142 | } 143 | 144 | DualQuaternion operator+(const DualQuaternion &other) 145 | { 146 | DualQuaternion result; 147 | result.rotation_ = rotation_ + other.rotation_; 148 | result.translation_ = translation_ + other.translation_; 149 | return result; 150 | } 151 | 152 | DualQuaternion operator-(const DualQuaternion &other) 153 | { 154 | DualQuaternion result; 155 | result.rotation_ = rotation_ - other.rotation_; 156 | result.translation_ = translation_ - other.translation_; 157 | return result; 158 | } 159 | 160 | DualQuaternion operator*(const DualQuaternion &other) 161 | { 162 | DualQuaternion result; 163 | result.rotation_ = rotation_ * other.rotation_; 164 | // result.translation_ = (rotation_ * other.translation_) + (translation_ * other.rotation_); 165 | result.translation_ = translation_ + other.translation_; 166 | return result; 167 | } 168 | 169 | DualQuaternion operator/(const std::pair divisor) 170 | { 171 | DualQuaternion result; 172 | result.rotation_ = 1 / divisor.first * rotation_; 173 | result.translation_ = 1 / divisor.second * translation_; 174 | return result; 175 | } 176 | 177 | /// (left) Scalar Multiplication 178 | /** 179 | * \fn template friend Quaternion operator*(const U scalar, 180 | * \brief implements scalar multiplication for arbitrary scalar types 181 | */ 182 | template 183 | friend DualQuaternion operator*(const U scalar, const DualQuaternion &q) 184 | { 185 | DualQuaternion result; 186 | result.rotation_ = scalar * q.rotation_; 187 | result.translation_ = scalar * q.translation_; 188 | return result; 189 | } 190 | 191 | DualQuaternion conjugate() 192 | { 193 | DualQuaternion result; 194 | result.rotation_ = rotation_.conjugate(); 195 | result.translation_ = translation_.conjugate(); 196 | return result; 197 | } 198 | 199 | inline DualQuaternion identity() 200 | { 201 | return DualQuaternion(Quaternion(0, 0, 0, 0),Quaternion(0, 1, 0, 0)); 202 | } 203 | 204 | void transform(Vec3f& point) // TODO: this should be a lot more generic 205 | { 206 | Vec3f translation; 207 | getTranslation(translation); 208 | rotation_.rotate(point); 209 | point += translation; 210 | } 211 | 212 | void from_twist(const float &r0, const float &r1, const float &r2, 213 | const float &x, const float &y, const float &z) 214 | { 215 | float norm = sqrt(r0*r0 + r1 * r1 + r2 * r2); 216 | Quaternion rotation; 217 | if (norm > epsilon()) 218 | { 219 | float cosNorm = cos(norm); 220 | float sign = (cosNorm > 0.f) - (cosNorm < 0.f); 221 | cosNorm *= sign; 222 | float sinNorm_norm = sign * sin(norm) / norm; 223 | rotation = Quaternion(cosNorm, r0 * sinNorm_norm, r1 * sinNorm_norm, r2 * sinNorm_norm); 224 | } 225 | else 226 | rotation = Quaternion(); 227 | 228 | *this = DualQuaternion(Quaternion(0, x, y, z), rotation); 229 | } 230 | 231 | std::pair magnitude() 232 | { 233 | DualQuaternion result = (*this) * (*this).conjugate(); 234 | return std::make_pair(result.rotation_.w_, result.translation_.w_); 235 | } 236 | 237 | private: 238 | Quaternion rotation_; 239 | Quaternion translation_; 240 | 241 | T position_[3] = {}; /// default initialize vector to zeros. 242 | 243 | T rotAxis_[3] = {}; /// default initialize vector to zeros. 244 | T rotAngle_; 245 | 246 | 247 | T getRoll() 248 | { 249 | // TODO: test this! 250 | return atan2(2*((rotation_.w_ * rotation_.x_) + (rotation_.y_ * rotation_.z_)), 251 | (1 - 2*((rotation_.x_*rotation_.x_) + (rotation_.y_*rotation_.y_)))); 252 | } 253 | 254 | T getPitch() 255 | { 256 | // TODO: test this! 257 | return asin(2*(rotation_.w_ * rotation_.y_ - rotation_.z_ * rotation_.x_)); 258 | } 259 | 260 | T getYaw() 261 | { 262 | // TODO: test this! 263 | return atan2(2*((rotation_.w_ * rotation_.z_) + (rotation_.x_ * rotation_.y_)), 264 | (1 - 2*((rotation_.y_*rotation_.y_) + (rotation_.z_*rotation_.z_)))); 265 | } 266 | }; 267 | 268 | template 269 | std::ostream &operator<<(std::ostream &os, const DualQuaternion &q) 270 | { 271 | os << "[" << q.getRotation() << ", " << q.getTranslation()<< ", " << "]" << std::endl; 272 | return os; 273 | } 274 | } 275 | } 276 | #endif //DYNAMIC_FUSION_DUAL_QUATERNION_HPP -------------------------------------------------------------------------------- /kfusion/src/utils/knn_point_cloud.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KFUSION_KNN_POINfloat_CLOUD_HPP 2 | #define KFUSION_KNN_POINfloat_CLOUD_HPP 3 | 4 | #include 5 | namespace kfusion 6 | { 7 | namespace utils{ 8 | 9 | struct PointCloud 10 | { 11 | std::vector pts; 12 | 13 | // Must return the number of data points 14 | inline size_t kdtree_get_point_count() const { return pts.size(); } 15 | 16 | // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: 17 | inline float kdtree_distance(const Vec3f p1, const size_t idx_p2,size_t /*size*/) const 18 | { 19 | const float d0=p1[0] - pts[idx_p2][0]; 20 | const float d1=p1[1] - pts[idx_p2][1]; 21 | const float d2=p1[2] - pts[idx_p2][2]; 22 | return d0*d0 + d1*d1 + d2*d2; 23 | } 24 | 25 | // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: 26 | inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t /*size*/) const 27 | { 28 | const float d0=p1[0]-pts[idx_p2][0]; 29 | const float d1=p1[1]-pts[idx_p2][1]; 30 | const float d2=p1[2]-pts[idx_p2][2]; 31 | return d0*d0+d1*d1+d2*d2; 32 | } 33 | 34 | // Returns the dim'th component of the idx'th point in the class: 35 | // Since this is inlined and the "dim" argument is typically an immediate value, the 36 | // "if/else's" are actually solved at compile time. 37 | inline float kdtree_get_pt(const size_t idx, int dim) const 38 | { 39 | if (dim==0) return pts[idx][0]; 40 | else if (dim==1) return pts[idx][1]; 41 | else return pts[idx][2]; 42 | } 43 | 44 | // Optional bounding-box computation: return false to default to a standard bbox computation loop. 45 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 46 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 47 | template 48 | bool kdtree_get_bbox(BBOX& /* bb */) const { return false; } 49 | 50 | }; 51 | 52 | } 53 | } 54 | #endif //KFUSION_KNN_POINfloat_CLOUD_HPP 55 | -------------------------------------------------------------------------------- /kfusion/src/utils/macro_utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KFUSION_MACRO_UTILS_HPP 2 | #define KFUSION_MACRO_UTILS_HPP 3 | 4 | #define STRINGIFY(x) #x 5 | #define TOSTRING(x) STRINGIFY(x) 6 | 7 | #endif //KFUSION_MACRO_UTILS_H 8 | -------------------------------------------------------------------------------- /kfusion/src/utils/quaternion.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DYNAMIC_FUSION_QUATERNION_HPP 2 | #define DYNAMIC_FUSION_QUATERNION_HPP 3 | #pragma once 4 | #include 5 | #include 6 | //Adapted from https://github.com/Poofjunior/QPose 7 | namespace kfusion{ 8 | namespace utils{ 9 | 10 | 11 | /** 12 | * \class Quaternion 13 | * \brief a templated quaternion class that also enables quick storage and 14 | * retrieval of rotations encoded as a vector3 and angle. 15 | * \details All angles are in radians. 16 | * \warning This template is intended to be instantiated with a floating point 17 | * data type. 18 | */ 19 | template class Quaternion 20 | { 21 | public: 22 | Quaternion() : w_(1), x_(0), y_(0), z_(0) 23 | {} 24 | 25 | Quaternion(T w, T x, T y, T z) : w_(w), x_(x), y_(y), z_(z) 26 | {} 27 | 28 | /** 29 | * Encodes rotation from a normal 30 | * @param normal 31 | */ 32 | Quaternion(const Vec3f& normal) 33 | { 34 | Vec3f a(1, 0, 0); 35 | Vec3f b(0, 1, 0); 36 | 37 | Vec3f t0 = normal.cross(a); 38 | 39 | if (t0.dot(t0) < 0.001f) 40 | t0 = normal.cross(b); 41 | t0 = cv::normalize(t0); 42 | 43 | Vec3f t1 = normal.cross(t0); 44 | t1 = cv::normalize(t1); 45 | 46 | cv::Mat3f matrix; 47 | matrix.push_back(t0); 48 | matrix.push_back(t1); 49 | matrix.push_back(normal); 50 | w_ = sqrt(1.0 + matrix.at(0,0) + matrix.at(1,1) + matrix.at(2,2)) / 2.0; 51 | // FIXME: this breaks when w_ = 0; 52 | x_ = (matrix.at(2,1) - matrix.at(1,2)) / (w_ * 4); 53 | y_ = (matrix.at(0,2) - matrix.at(2,0)) / (w_ * 4); 54 | z_ = (matrix.at(1,0) - matrix.at(2,1)) / (w_ * 4); 55 | if(norm() > 0) 56 | normalize(); 57 | } 58 | 59 | ~Quaternion(){} 60 | 61 | 62 | /** 63 | * Quaternion Rotation Properties for straightforward usage of quaternions 64 | * to store rotations. 65 | */ 66 | 67 | /** 68 | * \fn void encodeRotation( T theta, T x, T y, T z) 69 | * \brief Store a normalized rotation in the quaternion encoded as a rotation 70 | * of theta about the vector (x,y,z). 71 | */ 72 | void encodeRotation(T theta, T x, T y, T z) 73 | { 74 | auto sin_half = sin(theta / 2); 75 | w_ = cos(theta / 2); 76 | x_ = x * sin_half; 77 | y_ = y * sin_half; 78 | z_ = z * sin_half; 79 | normalize(); 80 | } 81 | 82 | /** 83 | * \fn void encodeRotation( T theta, T x, T y, T z) 84 | * \brief Store a normalized rotation in the quaternion encoded as a rotation 85 | * of theta about the vector (x,y,z). 86 | */ 87 | void getRodrigues(T& x, T& y, T& z) 88 | { 89 | if(w_ == 1) 90 | { 91 | x = y = z = 0; 92 | return; 93 | } 94 | T half_theta = acos(w_); 95 | T k = sin(half_theta) * tan(half_theta); 96 | x = x_ / k; 97 | y = y_ / k; 98 | z = z_ / k; 99 | } 100 | 101 | 102 | /** 103 | * \fn void rotate( T& x, T& y, T& z) 104 | * \brief rotate a vector3 (x,y,z) by the angle theta about the axis 105 | * (U_x, U_y, U_z) stored in the quaternion. 106 | */ 107 | void rotate(T& x, T& y, T& z) 108 | { 109 | Quaternion q = (*this); 110 | Quaternion qStar = (*this).conjugate(); 111 | Quaternion rotatedVal = q * Quaternion(0, x, y, z) * qStar; 112 | 113 | x = rotatedVal.x_; 114 | y = rotatedVal.y_; 115 | z = rotatedVal.z_; 116 | } 117 | 118 | /** 119 | /** 120 | * \fn void rotate( T& x, T& y, T& z) 121 | * \brief rotate a vector3 (x,y,z) by the angle theta about the axis 122 | * (U_x, U_y, U_z) stored in the quaternion. 123 | */ 124 | void rotate(Vec3f& v) const 125 | { 126 | auto rot= *this; 127 | rot.normalize(); 128 | Vec3f q_vec(rot.x_, rot.y_, rot.z_); 129 | v += (q_vec*2.f).cross( q_vec.cross(v) + v*rot.w_ ); 130 | } 131 | 132 | /** 133 | * Quaternion Mathematical Properties 134 | * implemented below 135 | **/ 136 | 137 | Quaternion operator+(const Quaternion& other) 138 | { 139 | return Quaternion( (w_ + other.w_), 140 | (x_ + other.x_), 141 | (y_ + other.y_), 142 | (z_ + other.z_)); 143 | } 144 | void operator+=(const Quaternion& other) 145 | { 146 | *this = *this + other; 147 | } 148 | 149 | Quaternion operator-(const Quaternion& other) 150 | { 151 | return Quaternion((w_ - other.w_), 152 | (x_ - other.x_), 153 | (y_ - other.y_), 154 | (z_ - other.z_)); 155 | } 156 | 157 | Quaternion operator-() 158 | { 159 | return Quaternion(-w_, -x_, -y_, -z_); 160 | } 161 | 162 | bool operator==(const Quaternion& other) const 163 | { 164 | return (w_ == other.w_) && (x_ == other.x_) && (y_ == other.y_) && (z_ == other.z_); 165 | } 166 | 167 | /** 168 | * \fn template friend Quaternion operator*(const U scalar, 169 | * const Quaternion& q) 170 | * \brief implements scalar multiplication for arbitrary scalar types. 171 | */ 172 | template friend Quaternion operator*(const U scalar, const Quaternion& other) 173 | { 174 | return Quaternion((scalar * other.w_), 175 | (scalar * other.x_), 176 | (scalar * other.y_), 177 | (scalar * other.z_)); 178 | } 179 | 180 | template friend Quaternion operator/(const Quaternion& q, const U scalar) 181 | { 182 | return (1 / scalar) * q; 183 | } 184 | 185 | /// Quaternion Product 186 | Quaternion operator*(const Quaternion& other) 187 | { 188 | return Quaternion( 189 | ((w_*other.w_) - (x_*other.x_) - (y_*other.y_) - (z_*other.z_)), 190 | ((w_*other.x_) + (x_*other.w_) + (y_*other.z_) - (z_*other.y_)), 191 | ((w_*other.y_) - (x_*other.z_) + (y_*other.w_) + (z_*other.x_)), 192 | ((w_*other.z_) + (x_*other.y_) - (y_*other.x_) + (z_*other.w_)) 193 | ); 194 | } 195 | 196 | /** 197 | * \fn static T dotProduct(Quaternion q1, Quaternion q2) 198 | * \brief returns the dot product of two quaternions. 199 | */ 200 | T dotProduct(Quaternion other) 201 | { 202 | return 0.5 * ((conjugate() * other) + (*this) * other.conjugate()).w_; 203 | } 204 | 205 | /// Conjugate 206 | Quaternion conjugate() const 207 | { 208 | return Quaternion(w_, -x_, -y_, -z_); 209 | } 210 | 211 | T norm() 212 | { 213 | return sqrt((w_ * w_) + (x_ * x_) + (y_ * y_) + (z_ * z_)); 214 | } 215 | 216 | /** 217 | * \fn void normalize() 218 | * \brief normalizes the quaternion to magnitude 1 219 | */ 220 | void normalize() 221 | { 222 | // should never happen unless the Quaternion wasn't initialized 223 | // correctly. 224 | assert( !((w_ == 0) && (x_ == 0) && (y_ == 0) && (z_ == 0))); 225 | T theNorm = norm(); 226 | assert(theNorm > 0); 227 | (*this) = (1.0/theNorm) * (*this); 228 | } 229 | 230 | /** 231 | * \fn template friend std::ostream& operator << 232 | * (std::ostream& os, const Quaternion& q); 233 | * \brief a templated friend function for printing quaternions. 234 | */ 235 | template friend std::ostream& operator << (std::ostream& os, const Quaternion& q) 236 | { 237 | os << "(" << q.w_ << ", " << q.x_ << ", " << q.y_ << ", " << q.z_ << ")"; 238 | return os; 239 | } 240 | 241 | T w_; 242 | T x_; 243 | T y_; 244 | T z_; 245 | }; 246 | } 247 | } 248 | #endif // DYNAMIC_FUSION_QUATERNION_HPP -------------------------------------------------------------------------------- /kfusion/src/warp_field.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "kfusion/warp_field.hpp" 5 | #include "internal.hpp" 6 | #include "precomp.hpp" 7 | #include 8 | #include 9 | 10 | using namespace kfusion; 11 | std::vector> neighbours; //THIS SHOULD BE SOMEWHERE ELSE BUT TOO SLOW TO REINITIALISE 12 | utils::PointCloud cloud; 13 | nanoflann::KNNResultSet *resultSet_; 14 | std::vector out_dist_sqr_; 15 | std::vector ret_index_; 16 | 17 | WarpField::WarpField() 18 | { 19 | nodes_ = new std::vector(); 20 | index_ = new kd_tree_t(3, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10)); 21 | ret_index_ = std::vector(KNN_NEIGHBOURS); 22 | out_dist_sqr_ = std::vector(KNN_NEIGHBOURS); 23 | resultSet_ = new nanoflann::KNNResultSet(KNN_NEIGHBOURS); 24 | resultSet_->init(&ret_index_[0], &out_dist_sqr_[0]); 25 | neighbours = std::vector>(KNN_NEIGHBOURS); 26 | warp_to_live_ = cv::Affine3f(); 27 | } 28 | 29 | WarpField::~WarpField() 30 | { 31 | delete nodes_; 32 | delete resultSet_; 33 | delete index_; 34 | } 35 | 36 | /** 37 | * 38 | * @param first_frame 39 | * @param normals 40 | */ 41 | void WarpField::init(const cv::Mat& first_frame) 42 | { 43 | nodes_->resize(first_frame.cols * first_frame.rows); 44 | auto voxel_size = kfusion::KinFuParams::default_params_dynamicfusion().volume_size[0] / 45 | kfusion::KinFuParams::default_params_dynamicfusion().volume_dims[0]; 46 | 47 | // FIXME:: this is a test, remove later 48 | voxel_size = 1; 49 | int step = 50; 50 | for(size_t i = 0; i < first_frame.rows; i+=step) 51 | for(size_t j = 0; j < first_frame.cols; j+=step) 52 | { 53 | auto point = first_frame.at(i,j); 54 | if(!std::isnan(point.x)) 55 | { 56 | nodes_->at(i*first_frame.cols+j).transform = utils::DualQuaternion(); 57 | nodes_->at(i*first_frame.cols+j).vertex = Vec3f(point.x,point.y,point.z); 58 | nodes_->at(i*first_frame.cols+j).weight = 3 * voxel_size; 59 | } 60 | } 61 | buildKDTree(); 62 | } 63 | 64 | /** 65 | * 66 | * @param first_frame 67 | * @param normals 68 | */ 69 | void WarpField::init(const std::vector& first_frame) 70 | { 71 | nodes_->resize(first_frame.size()); 72 | auto voxel_size = kfusion::KinFuParams::default_params_dynamicfusion().volume_size[0] / 73 | kfusion::KinFuParams::default_params_dynamicfusion().volume_dims[0]; 74 | 75 | // FIXME: this is a test, remove 76 | voxel_size = 1; 77 | for (size_t i = 0; i < first_frame.size(); i++) 78 | { 79 | auto point = first_frame[i]; 80 | if (!std::isnan(point[0])) 81 | { 82 | nodes_->at(i).transform = utils::DualQuaternion(); 83 | nodes_->at(i).vertex = point; 84 | nodes_->at(i).weight = 3 * voxel_size; 85 | } 86 | } 87 | buildKDTree(); 88 | } 89 | 90 | /** 91 | * \brief 92 | * \param frame 93 | * \param normals 94 | * \param pose 95 | * \param tsdfVolume 96 | * \param edges 97 | */ 98 | void WarpField::energy(const cuda::Cloud &frame, 99 | const cuda::Normals &normals, 100 | const Affine3f &pose, 101 | const cuda::TsdfVolume &tsdfVolume, 102 | const std::vector, utils::DualQuaternion>> &edges 103 | ) 104 | { 105 | assert(normals.cols()==frame.cols()); 106 | assert(normals.rows()==frame.rows()); 107 | } 108 | 109 | /** 110 | * 111 | * @param canonical_vertices 112 | * @param canonical_normals 113 | * @param live_vertices 114 | * @param live_normals 115 | * @return 116 | */ 117 | void WarpField::energy_data(const std::vector &canonical_vertices, 118 | const std::vector &canonical_normals, 119 | const std::vector &live_vertices, 120 | const std::vector &live_normals 121 | ) 122 | { 123 | ceres::Problem problem; 124 | float weights[KNN_NEIGHBOURS]; 125 | unsigned long indices[KNN_NEIGHBOURS]; 126 | 127 | WarpProblem warpProblem(this); 128 | for(int i = 0; i < live_vertices.size(); i++) 129 | { 130 | if(std::isnan(canonical_vertices[i][0]) || 131 | std::isnan(canonical_vertices[i][1]) || 132 | std::isnan(canonical_vertices[i][2]) || 133 | std::isnan(live_vertices[i][0]) || 134 | std::isnan(live_vertices[i][1]) || 135 | std::isnan(live_vertices[i][2])) 136 | continue; 137 | getWeightsAndUpdateKNN(canonical_vertices[i], weights); 138 | 139 | // FIXME: could just pass ret_index 140 | for(int j = 0; j < KNN_NEIGHBOURS; j++) 141 | indices[j] = ret_index_[j]; 142 | 143 | ceres::CostFunction* cost_function = DynamicFusionDataEnergy::Create(live_vertices[i], 144 | live_normals[i], 145 | canonical_vertices[i], 146 | canonical_normals[i], 147 | this, 148 | weights, 149 | indices); 150 | problem.AddResidualBlock(cost_function, NULL /* squared loss */, warpProblem.mutable_epsilon(indices)); 151 | 152 | } 153 | ceres::Solver::Options options; 154 | // options.minimizer_type = ceres::TRUST_REGION; 155 | options.linear_solver_type = ceres::SPARSE_SCHUR; 156 | options.minimizer_progress_to_stdout = true; 157 | options.num_linear_solver_threads = 8; 158 | options.num_threads = 8; 159 | ceres::Solver::Summary summary; 160 | ceres::Solve(options, &problem, &summary); 161 | std::cout << summary.FullReport() << std::endl; 162 | warpProblem.updateWarp(); 163 | } 164 | /** 165 | * \brief 166 | * \param edges 167 | */ 168 | void WarpField::energy_reg(const std::vector, 169 | kfusion::utils::DualQuaternion>> &edges) 170 | { 171 | 172 | } 173 | 174 | 175 | /** 176 | * 177 | * @param points 178 | * @param normals 179 | */ 180 | void WarpField::warp(std::vector& points, std::vector& normals) const 181 | { 182 | int i = 0; 183 | for (auto& point : points) 184 | { 185 | if(std::isnan(point[0]) || std::isnan(normals[i][0])) 186 | continue; 187 | utils::DualQuaternion dqb = DQB(point); 188 | dqb.transform(point); 189 | point = warp_to_live_ * point; 190 | 191 | dqb.transform(normals[i]); 192 | normals[i] = warp_to_live_ * normals[i]; 193 | i++; 194 | } 195 | } 196 | 197 | /** 198 | * \brief 199 | * \param vertex 200 | * \param weight 201 | * \return 202 | */ 203 | utils::DualQuaternion WarpField::DQB(const Vec3f& vertex) const 204 | { 205 | float weights[KNN_NEIGHBOURS]; 206 | getWeightsAndUpdateKNN(vertex, weights); 207 | utils::Quaternion translation_sum(0,0,0,0); 208 | utils::Quaternion rotation_sum(0,0,0,0); 209 | for (size_t i = 0; i < KNN_NEIGHBOURS; i++) 210 | { 211 | translation_sum += weights[i] * nodes_->at(ret_index_[i]).transform.getTranslation(); 212 | rotation_sum += weights[i] * nodes_->at(ret_index_[i]).transform.getRotation(); 213 | } 214 | rotation_sum.normalize(); 215 | auto res = utils::DualQuaternion(translation_sum, rotation_sum); 216 | return res; 217 | } 218 | 219 | /** 220 | * \brief 221 | * \param vertex 222 | * \param weight 223 | * \return 224 | */ 225 | void WarpField::getWeightsAndUpdateKNN(const Vec3f& vertex, float weights[KNN_NEIGHBOURS]) const 226 | { 227 | KNN(vertex); 228 | for (size_t i = 0; i < KNN_NEIGHBOURS; i++) 229 | weights[i] = weighting(out_dist_sqr_[i], nodes_->at(ret_index_[i]).weight); 230 | } 231 | 232 | /** 233 | * \brief 234 | * \param squared_dist 235 | * \param weight 236 | * \return 237 | */ 238 | float WarpField::weighting(float squared_dist, float weight) const 239 | { 240 | return (float) exp(-squared_dist / (2 * weight * weight)); 241 | } 242 | 243 | /** 244 | * \brief 245 | * \return 246 | */ 247 | void WarpField::KNN(Vec3f point) const 248 | { 249 | resultSet_->init(&ret_index_[0], &out_dist_sqr_[0]); 250 | index_->findNeighbors(*resultSet_, point.val, nanoflann::SearchParams(10)); 251 | } 252 | 253 | /** 254 | * \brief 255 | * \return 256 | */ 257 | const std::vector* WarpField::getNodes() const 258 | { 259 | return nodes_; 260 | } 261 | 262 | /** 263 | * \brief 264 | * \return 265 | */ 266 | std::vector* WarpField::getNodes() 267 | { 268 | return nodes_; 269 | } 270 | 271 | /** 272 | * \brief 273 | * \return 274 | */ 275 | void WarpField::buildKDTree() 276 | { 277 | // Build kd-tree with current warp nodes. 278 | cloud.pts.resize(nodes_->size()); 279 | for(size_t i = 0; i < nodes_->size(); i++) 280 | cloud.pts[i] = nodes_->at(i).vertex; 281 | index_->buildIndex(); 282 | } 283 | 284 | const cv::Mat WarpField::getNodesAsMat() const 285 | { 286 | cv::Mat matrix(1, nodes_->size(), CV_32FC3); 287 | for(int i = 0; i < nodes_->size(); i++) 288 | { 289 | nodes_->at(i).transform.getTranslation(matrix.at(i)); 290 | matrix.at(i) += nodes_->at(i).vertex; 291 | } 292 | return matrix; 293 | } 294 | 295 | /** 296 | * \brief 297 | */ 298 | void WarpField::clear() 299 | { 300 | 301 | } 302 | void WarpField::setWarpToLive(const Affine3f &pose) 303 | { 304 | warp_to_live_ = pose; 305 | } 306 | 307 | std::vector* WarpField::getDistSquared() const 308 | { 309 | return &out_dist_sqr_; 310 | } 311 | std::vector* WarpField::getRetIndex() const 312 | { 313 | return &ret_index_; 314 | } -------------------------------------------------------------------------------- /kfusion/src/warp_field_optimiser.cpp: -------------------------------------------------------------------------------- 1 | #include "kfusion/warp_field_optimiser.hpp" 2 | kfusion::WarpFieldOptimiser::WarpFieldOptimiser(WarpField *warp, CombinedSolver *solver) : solver_(solver), warp_(warp){} 3 | kfusion::WarpFieldOptimiser::WarpFieldOptimiser(WarpField *warp, CombinedSolverParameters params) : warp_(warp) 4 | { 5 | solver_ = new CombinedSolver(warp, params); 6 | } 7 | void kfusion::WarpFieldOptimiser::optimiseWarpData(const std::vector &canonical_vertices, 8 | const std::vector &canonical_normals, 9 | const std::vector &live_vertices, 10 | const std::vector &live_normals) 11 | { 12 | solver_->initializeProblemInstance(canonical_vertices, 13 | canonical_normals, 14 | live_vertices, 15 | live_normals); 16 | solver_->solveAll(); 17 | } -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(ceres_tests) 2 | add_subdirectory(utils) 3 | 4 | include_directories( 5 | ceres_test PUBLIC 6 | ${CMAKE_SOURCE_DIR}/kfusion/include 7 | ) 8 | 9 | add_test(ceres_test ceres_warp_test.cpp) 10 | target_link_libraries( 11 | ceres_test 12 | ${CERES_LIBRARIES} 13 | ${GTEST_BOTH_LIBRARIES} 14 | ${CUDA_CUDART_LIBRARY} 15 | kfusion 16 | ) 17 | target_include_directories(ceres_test PUBLIC ${CERES_INCLUDE_DIRS}) 18 | 19 | 20 | add_test(nanoflann_test nanoflann_test.cpp) 21 | 22 | add_test(warp_test warp_test.cpp) 23 | target_link_libraries( 24 | warp_test 25 | ${OpenCV_LIBS} 26 | ${OPT_LIBRARIES} 27 | ${TERRA_LIBRARIES} 28 | ${CUDA_CUDART_LIBRARY} 29 | ${GTEST_BOTH_LIBRARIES} 30 | kfusion 31 | ) -------------------------------------------------------------------------------- /tests/ceres_warp_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "ceres/ceres.h" 5 | 6 | TEST(CERES_WARP_TEST, EnergyDataSingleVertexTest) 7 | { 8 | const float max_error = 1e-3; 9 | 10 | kfusion::WarpField warp_field; 11 | std::vector warp_init; 12 | 13 | warp_init.emplace_back(cv::Vec3f(1,1,1)); 14 | warp_init.emplace_back(cv::Vec3f(1,1,-1)); 15 | warp_init.emplace_back(cv::Vec3f(1,-1,1)); 16 | warp_init.emplace_back(cv::Vec3f(1,-1,-1)); 17 | warp_init.emplace_back(cv::Vec3f(-1,1,1)); 18 | warp_init.emplace_back(cv::Vec3f(-1,1,-1)); 19 | warp_init.emplace_back(cv::Vec3f(-1,-1,1)); 20 | warp_init.emplace_back(cv::Vec3f(-1,-1,-1)); 21 | warp_field.init(warp_init); 22 | 23 | std::vector source_vertices; 24 | source_vertices.emplace_back(cv::Vec3f(0,0,0)); 25 | 26 | std::vector canonical_normals; 27 | canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 28 | 29 | std::vector target_vertices; 30 | target_vertices.emplace_back(cv::Vec3f(0.05,0.05,0.05)); 31 | 32 | std::vector target_normals; 33 | target_normals.emplace_back(cv::Vec3f(0,0,1)); 34 | 35 | warp_field.energy_data(source_vertices, canonical_normals, target_vertices, target_normals); 36 | warp_field.warp(source_vertices, canonical_normals); 37 | 38 | 39 | for(int i = 0; i < warp_field.getNodes()->size(); i++) 40 | { 41 | auto t = warp_field.getNodes()->at(i).transform.getTranslation(); 42 | std::cout<< t < warp_init; 60 | 61 | warp_init.emplace_back(cv::Vec3f(1,1,1)); 62 | warp_init.emplace_back(cv::Vec3f(1,1,-1)); 63 | warp_init.emplace_back(cv::Vec3f(1,-1,1)); 64 | warp_init.emplace_back(cv::Vec3f(1,-1,-1)); 65 | warp_init.emplace_back(cv::Vec3f(-1,1,1)); 66 | warp_init.emplace_back(cv::Vec3f(-1,1,-1)); 67 | warp_init.emplace_back(cv::Vec3f(-1,-1,1)); 68 | warp_init.emplace_back(cv::Vec3f(-1,-1,-1)); 69 | warp_field.init(warp_init); 70 | 71 | std::vector source_vertices; 72 | // source_vertices.emplace_back(cv::Vec3f(-3,-3,-3)); 73 | // source_vertices.emplace_back(cv::Vec3f(-2,-2,-2)); 74 | // source_vertices.emplace_back(cv::Vec3f(0,0,0)); 75 | source_vertices.emplace_back(cv::Vec3f(2,2,2)); 76 | source_vertices.emplace_back(cv::Vec3f(3,3,3)); 77 | 78 | std::vector canonical_normals; 79 | // canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 80 | // canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 81 | // canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 82 | canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 83 | canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 84 | 85 | std::vector target_vertices; 86 | // target_vertices.emplace_back(cv::Vec3f(-2.95f,-2.95f,-2.95f)); 87 | // target_vertices.emplace_back(cv::Vec3f(-1.95f,-1.95f,-1.95f)); 88 | // target_vertices.emplace_back(cv::Vec3f(0.05,0.05,0.05)); 89 | target_vertices.emplace_back(cv::Vec3f(2.05,2.05,2.05)); 90 | target_vertices.emplace_back(cv::Vec3f(3.05,3.05,3.05)); 91 | 92 | std::vector target_normals; 93 | // target_normals.emplace_back(cv::Vec3f(0,0,1)); 94 | // target_normals.emplace_back(cv::Vec3f(0,0,1)); 95 | // target_normals.emplace_back(cv::Vec3f(0,0,1)); 96 | target_normals.emplace_back(cv::Vec3f(0,0,1)); 97 | target_normals.emplace_back(cv::Vec3f(0,0,1)); 98 | 99 | warp_field.energy_data(source_vertices, canonical_normals, target_vertices, target_normals); 100 | warp_field.warp(source_vertices, canonical_normals); 101 | 102 | auto sum = kfusion::utils::Quaternion(); 103 | for(int i = 0; i < warp_field.getNodes()->size(); i++) 104 | { 105 | auto t = warp_field.getNodes()->at(i).transform.getTranslation(); 106 | sum = sum + t; 107 | std::cout<< t < warp_init; 126 | 127 | warp_init.emplace_back(cv::Vec3f(1,1,1)); 128 | warp_init.emplace_back(cv::Vec3f(1,1,-1)); 129 | warp_init.emplace_back(cv::Vec3f(1,-1,1)); 130 | warp_init.emplace_back(cv::Vec3f(1,-1,-1)); 131 | warp_init.emplace_back(cv::Vec3f(-1,1,1)); 132 | warp_init.emplace_back(cv::Vec3f(-1,1,-1)); 133 | warp_init.emplace_back(cv::Vec3f(-1,-1,1)); 134 | warp_init.emplace_back(cv::Vec3f(-1,-1,-1)); 135 | warp_field.init(warp_init); 136 | 137 | std::vector source_vertices; 138 | source_vertices.emplace_back(cv::Vec3f(-3,-3,-3)); 139 | source_vertices.emplace_back(cv::Vec3f(-2,-2,-2)); 140 | source_vertices.emplace_back(cv::Vec3f(0,0,0)); 141 | source_vertices.emplace_back(cv::Vec3f(2,2,2)); 142 | source_vertices.emplace_back(cv::Vec3f(3,3,3)); 143 | 144 | std::vector canonical_normals; 145 | canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 146 | canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 147 | canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 148 | canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 149 | canonical_normals.emplace_back(cv::Vec3f(0,0,1)); 150 | 151 | std::vector target_vertices; 152 | target_vertices.emplace_back(cv::Vec3f(-2.95f,-2.95f,-2.95f)); 153 | target_vertices.emplace_back(cv::Vec3f(-1.95f,-1.95f,-1.95f)); 154 | target_vertices.emplace_back(cv::Vec3f(0.05,0.05,0.05)); 155 | target_vertices.emplace_back(cv::Vec3f(2.05,2.05,2.05)); 156 | target_vertices.emplace_back(cv::Vec3f(3.05,3.05,3.05)); 157 | 158 | std::vector target_normals; 159 | target_normals.emplace_back(cv::Vec3f(0,0,1)); 160 | target_normals.emplace_back(cv::Vec3f(0,0,1)); 161 | target_normals.emplace_back(cv::Vec3f(0,0,1)); 162 | target_normals.emplace_back(cv::Vec3f(0,0,1)); 163 | target_normals.emplace_back(cv::Vec3f(0,0,1)); 164 | 165 | 166 | std::vector initial_source_vertices(source_vertices); 167 | std::vector initial_source_normals(canonical_normals); 168 | 169 | warp_field.energy_data(source_vertices, canonical_normals, target_vertices, target_normals); 170 | warp_field.warp(source_vertices, canonical_normals); 171 | 172 | for(size_t i = 0; i < source_vertices.size(); i++) 173 | { 174 | ASSERT_NEAR(source_vertices[i][0], target_vertices[i][0], max_error); 175 | ASSERT_NEAR(source_vertices[i][1], target_vertices[i][1], max_error); 176 | ASSERT_NEAR(source_vertices[i][2], target_vertices[i][2], max_error); 177 | } 178 | 179 | auto sum = kfusion::utils::Quaternion(); 180 | for(int i = 0; i < warp_field.getNodes()->size(); i++) 181 | { 182 | auto t = warp_field.getNodes()->at(i).transform.getTranslation(); 183 | sum = sum + t; 184 | std::cout<< t <size(); i++) 197 | { 198 | auto t = warp_field.getNodes()->at(i).transform.getTranslation(); 199 | sum = sum - t; 200 | std::cout<< t < 2 | #include 3 | #include "../kfusion/src/utils/knn_point_cloud.hpp" 4 | #define KNN_NEIGHBOURS 8 5 | typedef nanoflann::KDTreeSingleIndexAdaptor< 6 | nanoflann::L2_Simple_Adaptor, 7 | kfusion::utils::PointCloud, 8 | 3 /* dim */ 9 | > kd_tree_t; 10 | 11 | int main(int argc, char** argv) { 12 | 13 | kd_tree_t* index_; 14 | nanoflann::KNNResultSet *resultSet_; 15 | std::vector out_dist_sqr_(KNN_NEIGHBOURS); 16 | std::vector ret_index_(KNN_NEIGHBOURS); 17 | kfusion::utils::PointCloud cloud; 18 | 19 | index_ = new kd_tree_t(3, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10)); 20 | resultSet_ = new nanoflann::KNNResultSet(KNN_NEIGHBOURS); 21 | resultSet_->init(&ret_index_[0], &out_dist_sqr_[0]); 22 | 23 | std::vector warp_init; 24 | warp_init.emplace_back(cv::Vec3f(1,1,1)); 25 | warp_init.emplace_back(cv::Vec3f(1,1,-1)); 26 | warp_init.emplace_back(cv::Vec3f(1,-1,1)); 27 | warp_init.emplace_back(cv::Vec3f(1,-1,-1)); 28 | warp_init.emplace_back(cv::Vec3f(-1,1,1)); 29 | warp_init.emplace_back(cv::Vec3f(-1,1,-1)); 30 | warp_init.emplace_back(cv::Vec3f(-1,-1,1)); 31 | warp_init.emplace_back(cv::Vec3f(-1,-1,-1)); 32 | cloud.pts = warp_init; 33 | 34 | std::vector canonical_vertices; 35 | canonical_vertices.emplace_back(cv::Vec3f(-1,-1,-1)); 36 | canonical_vertices.emplace_back(cv::Vec3f(0,0,0)); 37 | canonical_vertices.emplace_back(cv::Vec3f(1,1,1)); 38 | canonical_vertices.emplace_back(cv::Vec3f(2,2,2)); 39 | canonical_vertices.emplace_back(cv::Vec3f(3,3,3)); 40 | 41 | index_->buildIndex(); 42 | 43 | for(auto v : canonical_vertices) 44 | { 45 | index_->findNeighbors(*resultSet_, v.val, nanoflann::SearchParams(10)); 46 | for(int i = 0; i < KNN_NEIGHBOURS; i++) 47 | std::cout< 2 | #include 3 | 4 | 5 | using namespace kfusion::utils; 6 | TEST(DualQuaternionTest, DualQuaternionConstructor) 7 | { 8 | DualQuaternion dualQuaternion(1, 2, 3, 1, 2, 3); 9 | Quaternion translation = dualQuaternion.getTranslation(); 10 | Quaternion rotation = dualQuaternion.getRotation(); 11 | 12 | EXPECT_NEAR(translation.w_, 0, 0.001); 13 | EXPECT_NEAR(translation.x_, 1, 0.1); 14 | EXPECT_NEAR(translation.y_, 2, 0.1); 15 | EXPECT_NEAR(translation.z_, 3, 0.1); 16 | 17 | EXPECT_NEAR(rotation.w_, 0.435953, 0.01); 18 | EXPECT_NEAR(rotation.x_, -0.718287, 0.01); 19 | EXPECT_NEAR(rotation.y_, 0.310622, 0.01); 20 | EXPECT_NEAR(rotation.z_, 0.454649, 0.01); 21 | } 22 | 23 | TEST(DualQuaternionTest, isAssociative) 24 | { 25 | DualQuaternion dualQuaternion(1, 2, 3, 1, 2, 3); 26 | DualQuaternion dualQuaternion1(3, 4, 5, 3, 4, 5); 27 | dualQuaternion.encodeRotation(1,2,3); 28 | dualQuaternion1.encodeRotation(3,4,5); 29 | cv::Vec3f test11(1,0,0); 30 | cv::Vec3f test12(1,0,0); 31 | cv::Vec3f test2(1,0,0); 32 | auto cumul = dualQuaternion1 + dualQuaternion; 33 | cumul.normalize(); 34 | dualQuaternion.transform(test11); 35 | dualQuaternion1.transform(test12); 36 | cumul.transform(test2); 37 | auto result = test11 + test12; 38 | 39 | EXPECT_NE(test2, result); 40 | } 41 | TEST(DualQuaternionTest, canSplitOperations) 42 | { 43 | DualQuaternion dualQuaternion(1, 2, 3, 1, 2, 3); 44 | DualQuaternion dualQuaternion1(3, 4, 5, 3, 4, 5); 45 | dualQuaternion.encodeRotation(1,2,3); 46 | dualQuaternion1.encodeRotation(3,4,5); 47 | cv::Vec3f test1(1,0,0); 48 | cv::Vec3f test2(1,0,0); 49 | cv::Vec3f t1, t2; 50 | 51 | auto cumul = dualQuaternion1 + dualQuaternion; 52 | auto quat1 = dualQuaternion.getRotation() + dualQuaternion1.getRotation(); 53 | dualQuaternion.getTranslation(t1); 54 | dualQuaternion1.getTranslation(t2); 55 | auto trans = t1 + t2; 56 | 57 | 58 | cumul.normalize(); 59 | cumul.transform(test1); 60 | quat1.rotate(test2); 61 | auto result = test2 + trans; 62 | EXPECT_NE(test1, result); 63 | } -------------------------------------------------------------------------------- /tests/utils/test_quaternion.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | using namespace kfusion::utils; 6 | TEST(QuaternionTest, encodeRotation) 7 | { 8 | Quaternion quaternion; 9 | quaternion.encodeRotation(M_PI_4, 0, 0, 1); 10 | 11 | ASSERT_FLOAT_EQ(0.9238795, quaternion.w_); 12 | ASSERT_FLOAT_EQ(0, quaternion.x_); 13 | ASSERT_FLOAT_EQ(0, quaternion.y_); 14 | ASSERT_FLOAT_EQ(0.38268346, quaternion.z_); 15 | } 16 | 17 | TEST(QuaternionTest, rotate) 18 | { 19 | Quaternion quaternion(0,0,1,1); 20 | float vector[] = {0,0,1}; 21 | quaternion.rotate(vector[0], vector[1], vector[2]); 22 | EXPECT_EQ(vector[0], 0); 23 | EXPECT_EQ(vector[1], 2); 24 | EXPECT_EQ(vector[2], 0); 25 | } 26 | 27 | TEST(QuaternionTest, quat_product) 28 | { 29 | Quaternion quaternion(1,1,2,2); 30 | Quaternion quaternion1(0,0,1,1); 31 | Quaternion product = quaternion * quaternion1; 32 | EXPECT_EQ(product.w_, -4); 33 | EXPECT_EQ(product.x_, 0); 34 | EXPECT_EQ(product.y_, 0); 35 | EXPECT_EQ(product.z_, 2); 36 | } 37 | 38 | TEST(QuaternionTest, dotProduct) 39 | { 40 | Quaternion quaternion(1,1,2,2); 41 | Quaternion quaternion1(0,0,1,1); 42 | EXPECT_EQ(quaternion.dotProduct(quaternion1), 4); 43 | } 44 | 45 | TEST(QuaternionTest, normalize) 46 | { 47 | Quaternion quaternion(10,10,10,10); 48 | quaternion.normalize(); 49 | EXPECT_EQ(quaternion, Quaternion(0.5, 0.5, 0.5, 0.5)); 50 | } 51 | 52 | 53 | TEST(QuaternionTest, rodrigues) 54 | { 55 | Quaternion quaternion; 56 | quaternion.encodeRotation(3,1,1,1); 57 | float x, y, z; 58 | quaternion.getRodrigues(x,y,z); 59 | std::cout< quaternion(normal); 69 | 70 | std::cout<<"Quaternion:" << quaternion; 71 | } 72 | --------------------------------------------------------------------------------