├── .dockerignore ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── Dockerfile.ubuntu ├── LICENSE ├── README.md ├── cmake ├── FindGlog.cmake ├── FindJsoncpp.cmake ├── FindTensorFlow.cmake └── Findgflags.cmake ├── data ├── calib │ ├── flea.yml │ └── grasshopper.yml ├── flags │ ├── alg_odom.flags │ ├── alg_refine.flags │ ├── common.flags │ ├── dataset_odom.flags │ ├── live_odom.flags │ └── live_refine.flags ├── sequences.json ├── testimg │ ├── 0.jpg │ ├── 0.png │ ├── 1047.jpg │ ├── 1047.png │ ├── 1052.jpg │ ├── 1052.png │ ├── 25.jpg │ └── 25.png └── voc │ └── small_voc.yml.gz ├── docs ├── logo3.png └── ui_example.png ├── scripts ├── docker │ ├── build_docker.sh │ └── run_docker.sh ├── download_network.bash ├── download_scannet_python3.patch ├── run_scannet.bash └── sens_reader │ ├── LICENSE │ ├── README.md │ ├── SensorData.py │ ├── SensorData.pyc │ └── reader.py ├── sources ├── CMakeLists.txt ├── common │ ├── CMakeLists.txt │ ├── algorithm │ │ ├── camera_pyramid.h │ │ ├── dense_sfm.h │ │ ├── lucas_kanade_se3.h │ │ ├── m_estimators.h │ │ ├── nearest_psd.h │ │ ├── pinhole_camera.h │ │ ├── pinhole_camera_impl.h │ │ └── warping.h │ ├── display_utils.cpp │ ├── display_utils.h │ ├── image_sequences.cpp │ ├── image_sequences.h │ ├── indexed_map.h │ ├── interp.cpp │ ├── interp.h │ ├── intrinsics.h │ ├── logutils.cpp │ ├── logutils.h │ ├── timing.cpp │ ├── timing.h │ └── tum_io.h ├── core │ ├── CMakeLists.txt │ ├── deepfactors.cpp │ ├── deepfactors.h │ ├── deepfactors_options.cpp │ ├── deepfactors_options.h │ ├── features │ │ ├── feature_detection.h │ │ ├── matching.cpp │ │ └── matching.h │ ├── gtsam │ │ ├── depth_prior_factor.cpp │ │ ├── depth_prior_factor.h │ │ ├── factor_graph.h │ │ ├── gtsam_traits.h │ │ ├── gtsam_utils.h │ │ ├── photometric_factor.cpp │ │ ├── photometric_factor.h │ │ ├── reprojection_factor.cpp │ │ ├── reprojection_factor.h │ │ ├── sparse_geometric_factor.cpp │ │ ├── sparse_geometric_factor.h │ │ ├── uniform_sampler.cpp │ │ └── uniform_sampler.h │ ├── mapping │ │ ├── df_work.cpp │ │ ├── df_work.h │ │ ├── frame.h │ │ ├── keyframe.h │ │ ├── keyframe_map.h │ │ ├── mapper.cpp │ │ ├── mapper.h │ │ ├── work.cpp │ │ ├── work.h │ │ ├── work_manager.cpp │ │ └── work_manager.h │ ├── network │ │ ├── decoder_network.cpp │ │ ├── decoder_network.h │ │ └── tfwrap.h │ └── system │ │ ├── camera_tracker.cpp │ │ ├── camera_tracker.h │ │ ├── fbrisk.cpp │ │ ├── fbrisk.h │ │ ├── loop_detector.cpp │ │ └── loop_detector.h ├── cuda │ ├── CMakeLists.txt │ ├── cu_depthaligner.cpp │ ├── cu_depthaligner.h │ ├── cu_image_proc.cpp │ ├── cu_image_proc.h │ ├── cu_se3aligner.cpp │ ├── cu_se3aligner.h │ ├── cu_sfmaligner.cpp │ ├── cu_sfmaligner.h │ ├── cuda_context.cpp │ ├── cuda_context.h │ ├── device_info.h │ ├── kernel_utils.h │ ├── launch_utils.h │ ├── reduction_items.h │ └── synced_pyramid.h ├── demo │ ├── CMakeLists.txt │ ├── live_demo.cpp │ ├── live_demo.h │ └── main.cpp ├── drivers │ ├── CMakeLists.txt │ ├── camera │ │ ├── live_interface.h │ │ ├── openni_interface.cpp │ │ ├── openni_interface.h │ │ ├── pointgrey_interface.cpp │ │ └── pointgrey_interface.h │ ├── camera_interface.h │ ├── camera_interface_factory.cpp │ ├── camera_interface_factory.h │ └── dataset │ │ ├── dataset_interface.h │ │ ├── file_interface.cpp │ │ ├── file_interface.h │ │ ├── icl_interface.cpp │ │ ├── icl_interface.h │ │ ├── scannet_interface.cpp │ │ ├── scannet_interface.h │ │ ├── tum_interface.cpp │ │ └── tum_interface.h ├── gui │ ├── CMakeLists.txt │ ├── keyframe_renderer.cpp │ ├── keyframe_renderer.h │ ├── shaders │ │ ├── drawkf.geom │ │ ├── empty.vert │ │ └── phong.frag │ ├── visualizer.cpp │ └── visualizer.h └── tools │ ├── CMakeLists.txt │ ├── decode_image.cpp │ ├── kernel_benchmark.cpp │ ├── result_viewer.cpp │ ├── test_matching.cpp │ ├── voc_builder.cpp │ └── voc_test.cpp ├── tests ├── CMakeLists.txt ├── main.cpp ├── random_machine.h ├── testing_utils.h ├── ut_cuda_utils.cpp ├── ut_decoder.cpp ├── ut_nearestpsd.cpp ├── ut_pinhole_camera.cpp ├── ut_sampling.cpp ├── ut_se3aligner.cpp ├── ut_sfmaligner.cpp └── ut_warping.cpp └── thirdparty ├── CMakeLists.txt ├── makedeps.sh └── patch ├── brisk-missing-functional.patch └── brisk-opencv4.patch /.dockerignore: -------------------------------------------------------------------------------- 1 | # hidden files and dirs 2 | .gitignore 3 | .git/ 4 | .idea/ 5 | .vscode/ 6 | *.user 7 | 8 | *.zip 9 | 10 | # build directory 11 | build 12 | thirdparty/build 13 | thirdparty/install 14 | 15 | isam_graph* 16 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # IDE projects 2 | *.kdev4 3 | *.user 4 | *.idea 5 | 6 | __pycache__ 7 | 8 | # networks 9 | *.cfg 10 | *.pb 11 | 12 | # third party 13 | *.tar.gz 14 | thirdparty/brisk* 15 | thirdparty/bazel* 16 | thirdparty/install 17 | 18 | # generated files 19 | build 20 | results 21 | live_results 22 | *.dot 23 | download 24 | 25 | scripts/download-scannet.py 26 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "thirdparty/Sophus"] 2 | path = thirdparty/Sophus 3 | url = https://github.com/strasdat/Sophus 4 | [submodule "thirdparty/camera_drivers"] 5 | path = thirdparty/camera_drivers 6 | url = https://github.com/lukier/camera_drivers 7 | [submodule "thirdparty/DBoW2"] 8 | path = thirdparty/DBoW2 9 | url = https://github.com/dorian3d/DBoW2 10 | [submodule "thirdparty/vision_core"] 11 | path = thirdparty/vision_core 12 | url = https://github.com/jczarnowski/vision_core 13 | [submodule "thirdparty/gtsam"] 14 | path = thirdparty/gtsam 15 | url = https://github.com/borglab/gtsam 16 | [submodule "tests/googletest"] 17 | path = tests/googletest 18 | url = https://github.com/google/googletest 19 | [submodule "thirdparty/Pangolin"] 20 | path = thirdparty/Pangolin 21 | url = https://github.com/jczarnowski/Pangolin 22 | [submodule "thirdparty/opengv"] 23 | path = thirdparty/opengv 24 | url = https://github.com/laurentkneip/opengv 25 | [submodule "thirdparty/eigen"] 26 | path = thirdparty/eigen 27 | url = https://gitlab.com/libeigen/eigen 28 | -------------------------------------------------------------------------------- /Dockerfile.ubuntu: -------------------------------------------------------------------------------- 1 | FROM nvidia/cuda:10.0-cudnn7-devel 2 | ARG DEBIAN_FRONTEND=noninteractive 3 | 4 | # Add library path 5 | ENV LD_LIBRARY_PATH ./thirdparty/install/lib:${LD_LIBRARY_PATH} 6 | 7 | # Add nvidia driver settings 8 | ENV NVIDIA_VISIBLE_DEVICES ${NVIDIA_VISIBLE_DEVICES:-all} 9 | ENV NVIDIA_DRIVER_CAPABILITIES ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics 10 | 11 | # Install system dependencies 12 | RUN set -x && apt-get update -y && apt-get install -y \ 13 | cmake \ 14 | libboost-all-dev \ 15 | libglew-dev \ 16 | libgoogle-glog-dev \ 17 | libjsoncpp-dev \ 18 | libopencv-dev \ 19 | libopenni2-dev \ 20 | unzip \ 21 | wget \ 22 | && rm -rf /var/lib/apt/lists/* 23 | 24 | # Download TensorFlow C API 25 | ARG TFAPI_URL=https://storage.googleapis.com/tensorflow/libtensorflow/libtensorflow-gpu-linux-x86_64-1.15.0.tar.gz 26 | RUN wget -qO - ${TFAPI_URL} | tar -C /usr/local -xvz 27 | 28 | # Build DeepFactors 29 | COPY . /DeepFactors 30 | WORKDIR /DeepFactors 31 | 32 | # Download pretrained weight 33 | RUN set -x && bash ./scripts/download_network.bash 34 | 35 | # build deps 36 | RUN set -x && ./thirdparty/makedeps.sh --threads 2 37 | 38 | # build slam 39 | RUN set -x && \ 40 | mkdir -p build && \ 41 | cd build && \ 42 | cmake \ 43 | -DDF_BUILD_TOOLS=ON \ 44 | -DDF_BUILD_TESTS=ON \ 45 | -DDF_BUILD_DEMO=ON \ 46 | -DDF_CUDA_ARCH="6.1" \ 47 | .. && \ 48 | make -j1 49 | 50 | WORKDIR /DeepFactors 51 | ENTRYPOINT ["/bin/bash"] 52 | -------------------------------------------------------------------------------- /cmake/FindGlog.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Glog 2 | # 3 | # The following variables are optionally searched for defaults 4 | # GLOG_ROOT_DIR: Base directory where all GLOG components are found 5 | # 6 | # The following are set after configuration is done: 7 | # GLOG_FOUND 8 | # GLOG_INCLUDE_DIRS 9 | # GLOG_LIBRARIES 10 | # GLOG_LIBRARYRARY_DIRS 11 | 12 | include(FindPackageHandleStandardArgs) 13 | 14 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 15 | 16 | if(WIN32) 17 | find_path(GLOG_INCLUDE_DIR glog/logging.h 18 | PATHS ${GLOG_ROOT_DIR}/src/windows) 19 | else() 20 | find_path(GLOG_INCLUDE_DIR glog/logging.h 21 | PATHS ${GLOG_ROOT_DIR}) 22 | endif() 23 | 24 | if(MSVC) 25 | find_library(GLOG_LIBRARY_RELEASE libglog_static 26 | PATHS ${GLOG_ROOT_DIR} 27 | PATH_SUFFIXES Release) 28 | 29 | find_library(GLOG_LIBRARY_DEBUG libglog_static 30 | PATHS ${GLOG_ROOT_DIR} 31 | PATH_SUFFIXES Debug) 32 | 33 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 34 | else() 35 | find_library(GLOG_LIBRARY glog 36 | PATHS ${GLOG_ROOT_DIR} 37 | PATH_SUFFIXES lib lib64) 38 | endif() 39 | 40 | find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) 41 | 42 | if(GLOG_FOUND) 43 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 44 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 45 | 46 | add_library(glog SHARED IMPORTED) 47 | set_property(TARGET glog PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${GLOG_INCLUDE_DIRS}) 48 | set_property(TARGET glog PROPERTY IMPORTED_LOCATION ${GLOG_LIBRARIES}) 49 | 50 | message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") 51 | mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG 52 | GLOG_LIBRARY GLOG_INCLUDE_DIR) 53 | endif() 54 | -------------------------------------------------------------------------------- /cmake/FindJsoncpp.cmake: -------------------------------------------------------------------------------- 1 | # Find jsoncpp 2 | # 3 | # Find the jsoncpp includes and library 4 | # source: https://github.com/cinemast/libjson-rpc-cpp/blob/master/cmake/FindJsoncpp.cmake 5 | 6 | # This module defines 7 | # JSONCPP_INCLUDE_DIR, where to find header, etc. 8 | # JSONCPP_LIBRARY, the libraries needed to use jsoncpp. 9 | # Jsoncpp_FOUND, If false, do not try to use jsoncpp. 10 | # JSONCPP_INCLUDE_PREFIX, include prefix for jsoncpp. 11 | # jsoncpp imported target 12 | # 13 | 14 | # only look in default directories 15 | find_path( 16 | JSONCPP_INCLUDE_DIR 17 | NAMES json/json.h 18 | PATH_SUFFIXES jsoncpp 19 | DOC "jsoncpp include dir" 20 | ) 21 | 22 | find_library( 23 | JSONCPP_LIBRARY 24 | NAMES jsoncpp 25 | DOC "jsoncpp library" 26 | ) 27 | 28 | add_library(jsoncpp UNKNOWN IMPORTED) 29 | set_target_properties( 30 | jsoncpp 31 | PROPERTIES 32 | IMPORTED_LOCATION "${JSONCPP_LIBRARY}" 33 | INTERFACE_INCLUDE_DIRECTORIES "${JSONCPP_INCLUDE_DIR}" 34 | ) 35 | 36 | # find JSONCPP_INCLUDE_PREFIX 37 | find_path( 38 | JSONCPP_INCLUDE_PREFIX 39 | NAMES json.h 40 | PATH_SUFFIXES jsoncpp/json json 41 | ) 42 | 43 | if (${JSONCPP_INCLUDE_PREFIX} MATCHES "jsoncpp") 44 | set(JSONCPP_INCLUDE_PREFIX "jsoncpp/json") 45 | else() 46 | set(JSONCPP_INCLUDE_PREFIX "json") 47 | endif() 48 | 49 | # handle the QUIETLY and REQUIRED arguments and set JSONCPP_FOUND to TRUE 50 | # if all listed variables are TRUE, hide their existence from configuration view 51 | include(FindPackageHandleStandardArgs) 52 | find_package_handle_standard_args(Jsoncpp DEFAULT_MSG JSONCPP_INCLUDE_DIR JSONCPP_LIBRARY) 53 | mark_as_advanced (JSONCPP_INCLUDE_DIR JSONCPP_LIBRARY) 54 | -------------------------------------------------------------------------------- /cmake/FindTensorFlow.cmake: -------------------------------------------------------------------------------- 1 | # Find TensorFlow C API 2 | # Author: Jan Czarnowski (jancz@tuta.io) 3 | 4 | # find the main header 5 | find_path( 6 | TENSORFLOW_INCLUDE_DIR 7 | tensorflow/c/c_api.h 8 | HINTS 9 | /usr/include/tensorflow 10 | /usr/local/include/tensorflow 11 | ) 12 | 13 | # find both libraries 14 | find_library(TENSORFLOW_LIBRARY libtensorflow.so) 15 | find_library(TENSORFLOW_FRAMEWORK_LIBRARY libtensorflow_framework.so) 16 | 17 | include(FindPackageHandleStandardArgs) 18 | find_package_handle_standard_args(TensorFlow DEFAULT_MSG TENSORFLOW_INCLUDE_DIR TENSORFLOW_LIBRARY TENSORFLOW_FRAMEWORK_LIBRARY) 19 | 20 | # create an imported target 21 | if(TensorFlow_FOUND) 22 | message(STATUS "Found TensorFlow (include: ${TENSORFLOW_INCLUDE_DIR}, library: ${TENSORFLOW_LIBRARY})") 23 | 24 | # provide commonly used aliases 25 | set(TENSORFLOW_INCLUDE_DIRS ${TENSORFLOW_INCLUDE_DIR}) 26 | set(TENSORFLOW_LIBRARIES ${TENSORFLOW_LIBRARY} ${TENSORFLOW_FRAMEWORK_LIBRARY}) 27 | 28 | # libtensorflow 29 | add_library(TensorFlowBase SHARED IMPORTED) 30 | set_property(TARGET TensorFlowBase PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${TENSORFLOW_INCLUDE_DIRS}) 31 | set_property(TARGET TensorFlowBase PROPERTY IMPORTED_LOCATION ${TENSORFLOW_LIBRARY}) 32 | 33 | # libtensorflow_framework 34 | add_library(TensorFlowFramework SHARED IMPORTED) 35 | set_property(TARGET TensorFlowFramework PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${TENSORFLOW_INCLUDE_DIRS}) 36 | set_property(TARGET TensorFlowFramework PROPERTY IMPORTED_LOCATION ${TENSORFLOW_FRAMEWORK_LIBRARY}) 37 | 38 | # aggregate the above in a single target 39 | add_library(TensorFlow INTERFACE IMPORTED) 40 | set_property(TARGET TensorFlow PROPERTY 41 | INTERFACE_LINK_LIBRARIES TensorFlowBase TensorFlowFramework) 42 | endif() 43 | -------------------------------------------------------------------------------- /cmake/Findgflags.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find GFLAGS 2 | # 3 | # The following variables are optionally searched for defaults 4 | # GFLAGS_ROOT_DIR: Base directory where all GFLAGS components are found 5 | # 6 | # The following are set after configuration is done: 7 | # GFLAGS_FOUND 8 | # GFLAGS_INCLUDE_DIRS 9 | # GFLAGS_LIBRARIES 10 | # GFLAGS_LIBRARYRARY_DIRS 11 | 12 | include(FindPackageHandleStandardArgs) 13 | 14 | set(GFLAGS_ROOT_DIR "" CACHE PATH "Folder contains Gflags") 15 | 16 | # We are testing only a couple of files in the include directories 17 | if(WIN32) 18 | find_path(GFLAGS_INCLUDE_DIR gflags/gflags.h 19 | PATHS ${GFLAGS_ROOT_DIR}/src/windows) 20 | else() 21 | find_path(GFLAGS_INCLUDE_DIR gflags/gflags.h 22 | PATHS ${GFLAGS_ROOT_DIR}) 23 | endif() 24 | 25 | if(MSVC) 26 | find_library(GFLAGS_LIBRARY_RELEASE 27 | NAMES libgflags 28 | PATHS ${GFLAGS_ROOT_DIR} 29 | PATH_SUFFIXES Release) 30 | 31 | find_library(GFLAGS_LIBRARY_DEBUG 32 | NAMES libgflags-debug 33 | PATHS ${GFLAGS_ROOT_DIR} 34 | PATH_SUFFIXES Debug) 35 | 36 | set(GFLAGS_LIBRARY optimized ${GFLAGS_LIBRARY_RELEASE} debug ${GFLAGS_LIBRARY_DEBUG}) 37 | else() 38 | find_library(GFLAGS_LIBRARY gflags) 39 | endif() 40 | 41 | find_package_handle_standard_args(GFlags DEFAULT_MSG GFLAGS_INCLUDE_DIR GFLAGS_LIBRARY) 42 | 43 | 44 | if(GFLAGS_FOUND) 45 | set(GFLAGS_INCLUDE_DIRS ${GFLAGS_INCLUDE_DIR}) 46 | set(GFLAGS_LIBRARIES ${GFLAGS_LIBRARY}) 47 | 48 | add_library(gflags SHARED IMPORTED) 49 | set_property(TARGET gflags PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${GFLAGS_INCLUDE_DIRS}) 50 | set_property(TARGET gflags PROPERTY IMPORTED_LOCATION ${GFLAGS_LIBRARY}) 51 | 52 | message(STATUS "Found gflags (include: ${GFLAGS_INCLUDE_DIR}, library: ${GFLAGS_LIBRARY})") 53 | mark_as_advanced(GFLAGS_LIBRARY_DEBUG GFLAGS_LIBRARY_RELEASE 54 | GFLAGS_LIBRARY GFLAGS_INCLUDE_DIR GFLAGS_ROOT_DIR) 55 | endif() 56 | -------------------------------------------------------------------------------- /data/calib/flea.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | image_width: 800 3 | image_height: 600 4 | camera_matrix: !!opencv-matrix 5 | rows: 3 6 | cols: 3 7 | dt: d 8 | data: [5.5623791642817105e+02, 0, 3.8277192675339887e+02, 0, 9 | 5.5642894922117569e+02, 2.9620530814427343e+02, 0, 0, 1] 10 | -------------------------------------------------------------------------------- /data/calib/grasshopper.yml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | image_width: 1224 3 | image_height: 1024 4 | camera_matrix: !!opencv-matrix 5 | rows: 3 6 | cols: 3 7 | dt: d 8 | data: [7.2997875567260223e+02, 0.0000000000000000e+00, 6.1446116227873779e+02, 9 | 0.0000000000000000e+00, 7.2997424837496476e+02, 5.0021984755002904e+02, 10 | 0.0000000000000000e+00, 0.0000000000000000e+00, 1.0000000000000000e+00] 11 | -------------------------------------------------------------------------------- /data/flags/alg_odom.flags: -------------------------------------------------------------------------------- 1 | # keyframing options 2 | -keyframe_mode=AUTO 3 | -dist_threshold=0.6 4 | -inlier_threshold=0.7 5 | -frame_dist_threshold=0.03 6 | 7 | # connection options 8 | -connection_mode=LASTN 9 | -max_back_connections=2 10 | 11 | # tracking options 12 | -tracking_mode=LAST 13 | -tracking_huber_delta=0.03 14 | -tracking_dist_threshold=3.5 15 | -tracking_error_threshold=0.5 16 | 17 | # init/visualize 18 | -init_type=ONEFRAME 19 | -vis_mode=LAST_N 20 | 21 | # mapping parameters 22 | -pose_prior=0.1 23 | -code_prior=1 24 | -partial_relin_check=true 25 | -relinearize_threshold=0.03 26 | -------------------------------------------------------------------------------- /data/flags/alg_refine.flags: -------------------------------------------------------------------------------- 1 | # keyframing options 2 | -keyframe_mode=NEVER 3 | 4 | # connection options 5 | -connection_mode=LAST 6 | -max_back_connections=4 7 | 8 | # tracking options 9 | -tracking_mode=LAST 10 | -tracking_huber_delta=0.1 11 | -tracking_dist_threshold=3 12 | -tracking_error_threshold=0.5 13 | 14 | # init/visualize 15 | -init_type=ONEFRAME 16 | -vis_mode=ALL 17 | 18 | # mapping parameters 19 | -pose_prior=0.1 20 | -code_prior=5 21 | -partial_relin_check=false 22 | -relinearize_threshold=0.01 23 | 24 | -loop_closure=true 25 | -------------------------------------------------------------------------------- /data/flags/common.flags: -------------------------------------------------------------------------------- 1 | ########### NETWORK ############# 2 | -network_path=data/nets/scannet256_32.cfg 3 | -predict_code=true 4 | 5 | ########### ALGORITHM ############# 6 | -vocab_path=data/voc/small_voc.yml.gz 7 | 8 | # coarse to fine iterations 9 | -tracking_iters=5,5,10 10 | -pho_iters=4,8,15 11 | 12 | # photometric error 13 | -use_photometric=true 14 | -huber_delta=0.1 15 | -normalize_image=false 16 | 17 | # reprojection error 18 | -use_reprojection=true 19 | -rep_nfeatures=500 20 | -rep_scale_factor=1.2 21 | -rep_nlevels=1 22 | -rep_max_dist=50 23 | -rep_huber=4 24 | -rep_iters=15 25 | -rep_sigma=1.0 26 | -rep_ransac_maxiters=10000 27 | -rep_ransac_threshold=0.000001 28 | 29 | # geometric error 30 | -use_geometric=false 31 | -geo_npoints=3000 32 | -geo_stochastic=false 33 | -geo_huber=0.1 34 | -geo_iters=15 35 | 36 | # loop closure 37 | -loop_closure=true 38 | -loop_max_dist=0.1 39 | -loop_min_similarity=0.4 40 | -loop_detection_freq=10 41 | -loop_active_window=10 42 | -------------------------------------------------------------------------------- /data/flags/dataset_odom.flags: -------------------------------------------------------------------------------- 1 | ########## INCLUDES ########## 2 | --flagfile=data/flags/common.flags 3 | --flagfile=data/flags/alg_odom.flags 4 | 5 | ########## DATASET SPECIFIC ########## 6 | -interleave_mapping=false 7 | -init_on_start=true 8 | -quit_on_finish=true 9 | -------------------------------------------------------------------------------- /data/flags/live_odom.flags: -------------------------------------------------------------------------------- 1 | ########## INCLUDES ########## 2 | --flagfile=data/flags/common.flags 3 | --flagfile=data/flags/alg_odom.flags 4 | -------------------------------------------------------------------------------- /data/flags/live_refine.flags: -------------------------------------------------------------------------------- 1 | ########## INCLUDES ########## 2 | --flagfile=data/flags/common.flags 3 | --flagfile=data/flags/alg_refine.flags 4 | -------------------------------------------------------------------------------- /data/sequences.json: -------------------------------------------------------------------------------- 1 | { 2 | "cameras": { 3 | "scenenet": [277.12, 289.70, 160.0, 120.0] 4 | }, 5 | 6 | "sequences": { 7 | "scenenet_0": { 8 | "base_dir": "data/testimg", 9 | "images": ["0.jpg", "25.jpg"], 10 | "depth_images": ["0.png", "25.png"], 11 | "camera": "scenenet" 12 | }, 13 | 14 | "scenenet_1": { 15 | "base_dir": "data/testimg", 16 | "images": ["1047.jpg", "1052.jpg"], 17 | "depth_images": ["1047.png", "1052.png"], 18 | "camera": "scenenet" 19 | } 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /data/testimg/0.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/data/testimg/0.jpg -------------------------------------------------------------------------------- /data/testimg/0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/data/testimg/0.png -------------------------------------------------------------------------------- /data/testimg/1047.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/data/testimg/1047.jpg -------------------------------------------------------------------------------- /data/testimg/1047.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/data/testimg/1047.png -------------------------------------------------------------------------------- /data/testimg/1052.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/data/testimg/1052.jpg -------------------------------------------------------------------------------- /data/testimg/1052.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/data/testimg/1052.png -------------------------------------------------------------------------------- /data/testimg/25.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/data/testimg/25.jpg -------------------------------------------------------------------------------- /data/testimg/25.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/data/testimg/25.png -------------------------------------------------------------------------------- /data/voc/small_voc.yml.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/data/voc/small_voc.yml.gz -------------------------------------------------------------------------------- /docs/logo3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/docs/logo3.png -------------------------------------------------------------------------------- /docs/ui_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/docs/ui_example.png -------------------------------------------------------------------------------- /scripts/docker/build_docker.sh: -------------------------------------------------------------------------------- 1 | docker build -t deepfactors -f Dockerfile.ubuntu . -------------------------------------------------------------------------------- /scripts/docker/run_docker.sh: -------------------------------------------------------------------------------- 1 | # You can change this mount directory to your environment. 2 | DATA_PATH="/home/$(id --user --name)" 3 | 4 | xhost +local: 5 | docker run -it \ 6 | --rm \ 7 | --runtime=nvidia \ 8 | -v ${DATA_PATH}:${DATA_PATH} \ 9 | -e DISPLAY=$DISPLAY \ 10 | -v /tmp/.X11-unix/:/tmp/.X11-unix:ro \ 11 | deepfactors 12 | xhost -local: -------------------------------------------------------------------------------- /scripts/download_network.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | URL="https://drive.google.com/file/d/1CQetOB7yVUl8MZ2_hNBnJh3G4bPGGdr6" 3 | DEST_DIR="data/nets" 4 | 5 | if [[ ! -f ${DEST_DIR}/scannet256_32.cfg || 6 | ! -f ${DEST_DIR}/scannet256_32_frozen.pb || 7 | ! -f ${DEST_DIR}/scannet256_32_graphdef.pb ]]; then 8 | # Download pretrained weights 9 | mkdir -p ${DEST_DIR} 10 | wget -qO - ${URL} | tar xvz -C ${DEST_DIR} 11 | else 12 | echo "The pretrained weights already exist" 13 | fi 14 | -------------------------------------------------------------------------------- /scripts/download_scannet_python3.patch: -------------------------------------------------------------------------------- 1 | --- download-scannet.py 2020-05-05 10:08:35.608257161 +0100 2 | +++ download-scannet_new.py 2020-05-05 10:09:52.701293293 +0100 3 | @@ -4,8 +4,7 @@ 4 | # -*- coding: utf-8 -*- 5 | import argparse 6 | import os 7 | -#import urllib.request (for python3) 8 | -import urllib 9 | +import urllib.request 10 | import tempfile 11 | 12 | BASE_URL = 'http://kaldir.vc.in.tum.de/scannet/' 13 | @@ -27,8 +26,7 @@ 14 | 15 | 16 | def get_release_scans(release_file): 17 | - #scan_lines = urllib.request.urlopen(release_file) 18 | - scan_lines = urllib.urlopen(release_file) 19 | + scan_lines = urllib.request.urlopen(release_file) 20 | scans = [] 21 | for scan_line in scan_lines: 22 | scan_id = scan_line.decode('utf8').rstrip('\n') 23 | @@ -55,8 +53,7 @@ 24 | fh, out_file_tmp = tempfile.mkstemp(dir=out_dir) 25 | f = os.fdopen(fh, 'w') 26 | f.close() 27 | - #urllib.request.urlretrieve(url, out_file_tmp) 28 | - urllib.urlretrieve(url, out_file_tmp) 29 | + urllib.request.urlretrieve(url, out_file_tmp) 30 | os.rename(out_file_tmp, out_file) 31 | else: 32 | print('WARNING: skipping download of existing file ' + out_file) 33 | @@ -119,7 +116,7 @@ 34 | print(TOS_URL) 35 | print('***') 36 | print('Press any key to continue, or CTRL-C to exit.') 37 | - key = raw_input('') 38 | + key = input('') 39 | 40 | if args.v1: 41 | global RELEASE 42 | @@ -176,7 +173,7 @@ 43 | use_v1_sens = not is_test_scan 44 | if not is_test_scan and not args.v1 and '.sens' in scan_file_types: 45 | print('Note: ScanNet v2 uses the same .sens files as ScanNet v1: Press \'n\' to exclude downloading .sens files for each scan') 46 | - key = raw_input('') 47 | + key = input('') 48 | if key.strip().lower() == 'n': 49 | scan_file_types.remove('.sens') 50 | download_scan(scan_id, out_dir, scan_file_types, use_v1_sens) 51 | @@ -188,10 +185,10 @@ 52 | print('Note that existing scan directories will be skipped. Delete partially downloaded directories to re-download.') 53 | print('***') 54 | print('Press any key to continue, or CTRL-C to exit.') 55 | - key = raw_input('') 56 | + key = input('') 57 | if not args.v1 and '.sens' in file_types: 58 | print('Note: ScanNet v2 uses the same .sens files as ScanNet v1: Press \'n\' to exclude downloading .sens files for each scan') 59 | - key = raw_input('') 60 | + key = input('') 61 | if key.strip().lower() == 'n': 62 | file_types.remove('.sens') 63 | download_release(release_scans, out_dir_scans, file_types, use_v1_sens=True) 64 | -------------------------------------------------------------------------------- /scripts/sens_reader/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2017 2 | Angela Dai, Angel X. Chang, Manolis Savva, Maciej Halber, Thomas Funkhouser, 3 | Matthias Niessner 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 9 | of the Software, and to permit persons to whom the Software is furnished to do 10 | so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /scripts/sens_reader/README.md: -------------------------------------------------------------------------------- 1 | # Sens Reader 2 | 3 | This code is a derivative work based on a fragment of the official ScanNet 4 | toolkit, which can be found here: 5 | 6 | https://github.com/ScanNet/ScanNet 7 | 8 | 9 | Please see the LICENSE file for licensing information. 10 | -------------------------------------------------------------------------------- /scripts/sens_reader/SensorData.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jczarnowski/DeepFactors/bffc78afe8bd1e1673ab26878350450949ff3e56/scripts/sens_reader/SensorData.pyc -------------------------------------------------------------------------------- /scripts/sens_reader/reader.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import os, sys 3 | 4 | from SensorData import SensorData 5 | 6 | # params 7 | parser = argparse.ArgumentParser() 8 | # data paths 9 | parser.add_argument('--filename', required=True, help='path to sens file to read') 10 | parser.add_argument('--output_path', required=True, help='path to output folder') 11 | parser.add_argument('--export_depth_images', dest='export_depth_images', action='store_true') 12 | parser.add_argument('--export_color_images', dest='export_color_images', action='store_true') 13 | parser.add_argument('--export_poses', dest='export_poses', action='store_true') 14 | parser.add_argument('--export_intrinsics', dest='export_intrinsics', action='store_true') 15 | parser.set_defaults(export_depth_images=False, export_color_images=False, export_poses=False, export_intrinsics=False) 16 | 17 | opt = parser.parse_args() 18 | print(opt) 19 | 20 | 21 | def main(): 22 | if not os.path.exists(opt.output_path): 23 | os.makedirs(opt.output_path) 24 | # load the data 25 | sys.stdout.write('loading %s...' % opt.filename) 26 | sd = SensorData(opt.filename) 27 | sys.stdout.write('loaded!\n') 28 | if opt.export_depth_images: 29 | sd.export_depth_images(os.path.join(opt.output_path, 'depth')) 30 | if opt.export_color_images: 31 | sd.export_color_images(os.path.join(opt.output_path, 'color')) 32 | if opt.export_poses: 33 | sd.export_poses(os.path.join(opt.output_path, 'pose')) 34 | if opt.export_intrinsics: 35 | sd.export_intrinsics(os.path.join(opt.output_path, 'intrinsic')) 36 | 37 | 38 | if __name__ == '__main__': 39 | main() -------------------------------------------------------------------------------- /sources/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(common) 2 | add_subdirectory(core) 3 | add_subdirectory(cuda) 4 | 5 | if(DF_BUILD_DEMO) 6 | add_subdirectory(demo) 7 | endif() 8 | 9 | if(DF_BUILD_TOOLS) 10 | add_subdirectory(tools) 11 | endif() 12 | 13 | if(DF_BUILD_TOOLS OR DF_BUILD_DEMO) 14 | add_subdirectory(drivers) 15 | add_subdirectory(gui) 16 | endif() 17 | 18 | 19 | -------------------------------------------------------------------------------- /sources/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ------------------------------------------------------------------------------ 2 | # List sources 3 | # ------------------------------------------------------------------------------ 4 | set(common_sources 5 | image_sequences.cpp 6 | timing.cpp 7 | logutils.cpp 8 | display_utils.cpp 9 | interp.cpp 10 | ) 11 | set(common_headers 12 | # algorithm 13 | algorithm/pinhole_camera_impl.h 14 | algorithm/m_estimators.h 15 | algorithm/dense_sfm.h 16 | algorithm/camera_pyramid.h 17 | algorithm/warping.h 18 | algorithm/pinhole_camera.h 19 | algorithm/nearest_psd.h 20 | algorithm/lucas_kanade_se3.h 21 | # top-level 22 | image_sequences.h 23 | indexed_map.h 24 | tum_io.h 25 | interp.h 26 | timing.h 27 | display_utils.h 28 | logutils.h 29 | intrinsics.h 30 | ) 31 | 32 | # ------------------------------------------------------------------------------ 33 | # Build target 34 | # ------------------------------------------------------------------------------ 35 | add_library(df_common SHARED ${common_sources} ${common_headers}) 36 | target_include_directories(df_common PUBLIC 37 | ${CMAKE_CURRENT_SOURCE_DIR} 38 | ${CMAKE_CURRENT_SOURCE_DIR}/algorithm 39 | ) 40 | target_link_libraries(df_common PUBLIC 41 | Eigen3::Eigen 42 | Sophus::Sophus 43 | VisionCore 44 | opencv_core 45 | opencv_highgui 46 | jsoncpp 47 | ) 48 | -------------------------------------------------------------------------------- /sources/common/algorithm/camera_pyramid.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_CAMERA_PYRAMID_H_ 19 | #define DF_CAMERA_PYRAMID_H_ 20 | 21 | #include 22 | #include "pinhole_camera.h" 23 | 24 | namespace df 25 | { 26 | 27 | template 28 | class CameraPyramid 29 | { 30 | typedef df::PinholeCamera CameraT; 31 | 32 | public: 33 | CameraPyramid() {} 34 | 35 | CameraPyramid(const CameraT& cam, std::size_t levels) : levels_(levels) 36 | { 37 | for (std::size_t i = 0; i < levels; ++i) 38 | { 39 | cameras_.emplace_back(cam); 40 | 41 | if (i != 0) 42 | { 43 | std::size_t new_width = cameras_[i-1].width() / 2; 44 | std::size_t new_height = cameras_[i-1].height() / 2; 45 | cameras_[i].ResizeViewport(new_width, new_height); 46 | } 47 | } 48 | } 49 | 50 | const CameraT& operator[](int i) const 51 | { 52 | return cameras_[i]; 53 | } 54 | 55 | CameraT& operator[](int i) 56 | { 57 | return cameras_[i]; 58 | } 59 | 60 | std::size_t Levels() const { return levels_; } 61 | 62 | private: 63 | std::vector cameras_; 64 | std::size_t levels_; 65 | }; 66 | 67 | } // namespace df 68 | 69 | template 70 | std::ostream& operator<<(std::ostream& os, const df::CameraPyramid& pyr) 71 | { 72 | os << "CameraPyramid:" << std::endl; 73 | for (std::size_t i = 0; i < pyr.Levels(); ++i) 74 | os << "Level " << i << " " << pyr[i] << std::endl; 75 | return os; 76 | } 77 | 78 | #endif // DF_CAMERA_PYRAMID_H_ 79 | -------------------------------------------------------------------------------- /sources/common/algorithm/lucas_kanade_se3.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_JACOBIANS_H_ 19 | #define DF_JACOBIANS_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "reduction_items.h" 26 | #include "warping.h" 27 | #include "m_estimators.h" 28 | 29 | namespace df 30 | { 31 | 32 | /** 33 | * Full, per-pixel SE3 LucasKanade algorithm 34 | */ 35 | template , 37 | typename CamT=df::PinholeCamera, 38 | typename ReductionItem=df::JTJJrReductionItem, 39 | typename GradT=Eigen::Matrix> 40 | EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE 41 | ReductionItem LucasKanadeSE3(std::size_t x, std::size_t y, 42 | const SE3T& se3, 43 | const CamT& cam, 44 | const vc::Image2DView& img0, 45 | const vc::Image2DView& img1, 46 | const vc::Image2DView& dpt0, 47 | const vc::Image2DView& grad1, 48 | const Scalar huber_delta) 49 | { 50 | ReductionItem result; 51 | const Correspondence corresp = FindCorrespondence(x, y, dpt0(x,y), cam, se3); 52 | 53 | if (corresp.valid) 54 | { 55 | // calculate jacobians 56 | const Eigen::Matrix corresp_jac = FindCorrespondenceJacobianPose(corresp, dpt0(x,y), cam, se3); 57 | const Eigen::Matrix grad = grad1.template getBilinear>(corresp.pix1); 58 | Eigen::Matrix J = -grad * corresp_jac; // (1x6) = (1x2)x(2x6) 59 | 60 | // get the photometric error 61 | Scalar diff = static_cast(img0(x,y)) - img1.template getBilinear(corresp.pix1); 62 | 63 | // apply weighting 64 | const Scalar hbr_wgt = HuberWeight(diff, huber_delta); 65 | diff *= hbr_wgt; 66 | J *= hbr_wgt; 67 | // if (abs(diff) > huber_delta) 68 | // return result; 69 | 70 | // fill the ReductionItem 71 | result.inliers = 1; 72 | result.residual = diff * diff; 73 | result.Jtr = J.transpose() * diff; 74 | result.JtJ = ReductionItem::HessianType(J.transpose()); 75 | } 76 | return result; 77 | } 78 | 79 | /* 80 | * Solves the normal equations system described by JtJ and Jtr 81 | * and updates the current pose estimate with the solution. 82 | * We split the translation and rotation for the update here. 83 | */ 84 | template 85 | void SE3SolveAndUpdate(const Eigen::Matrix& JtJ, const Eigen::Matrix& Jtr, Sophus::SE3& curr_est) 86 | { 87 | // solve for the update 88 | Eigen::Matrix update = -JtJ.ldlt().solve(Jtr); 89 | 90 | // split and apply the update 91 | Eigen::Matrix trs_update = update.template head<3>(); 92 | Eigen::Matrix rot_update = update.template tail<3>(); 93 | curr_est.translation() += trs_update; 94 | curr_est.so3() = Sophus::SO3::exp(rot_update) * curr_est.so3(); 95 | } 96 | 97 | } // namespace df 98 | 99 | #endif // DF_JACOBIANS_H_ 100 | -------------------------------------------------------------------------------- /sources/common/algorithm/m_estimators.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_M_ESTIMATORS_H 19 | #define DF_M_ESTIMATORS_H 20 | 21 | #include 22 | 23 | namespace df 24 | { 25 | 26 | template 27 | EIGEN_DEVICE_FUNC 28 | Scalar TukeyWeight(Scalar x, Scalar delta) 29 | { 30 | Scalar a = delta / x; 31 | if (abs(x) <= delta) 32 | { 33 | Scalar first = 1 - 1 / a / a; // 1 - (delta/x)^2 34 | return abs(a) * sqrt((1 - first * first * first) / 6.); 35 | } 36 | else 37 | { 38 | return abs(a) * sqrt(1. / 6.); 39 | } 40 | } 41 | 42 | template 43 | EIGEN_DEVICE_FUNC 44 | Scalar CauchyWeight(Scalar x, Scalar delta) 45 | { 46 | Scalar a = delta / x; 47 | return abs(a) / sqrt(2.) * sqrt(log(1 + 1 / a / a)); 48 | } 49 | 50 | template 51 | EIGEN_DEVICE_FUNC 52 | Scalar HuberWeight(Scalar x, Scalar delta) 53 | { 54 | const Scalar aa = abs(x); 55 | return aa <= delta ? Scalar(1) : sqrt(delta * (2*aa - delta)) / aa; 56 | } 57 | 58 | } // namespace df 59 | 60 | #endif // DF_M_ESTIMATORS_H 61 | -------------------------------------------------------------------------------- /sources/common/algorithm/nearest_psd.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_NEAREST_PSD_H_ 19 | #define DF_NEAREST_PSD_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | template 28 | bool IsPsd(const Eigen::MatrixBase& M) 29 | { 30 | // size_t maxrank; 31 | // bool success; 32 | // gtsam::Matrix mat = M.template cast(); 33 | // boost::tie(maxrank, success) = gtsam::choleskyCareful(mat); 34 | // return success; 35 | 36 | // Eigen::LLT llt(M); 37 | // if (llt.info() == Eigen::NumericalIssue) 38 | // return false; 39 | // return true; 40 | 41 | Eigen::LDLT ldlt(M); 42 | return ldlt.isPositive(); 43 | } 44 | 45 | /* 46 | * Find nearest positive semi-definite matrix to M 47 | * 48 | * [1] https://www.mathworks.com/matlabcentral/fileexchange/42885-nearestspd 49 | * 50 | * [2] N.J. Higham, "Computing a nearest symmetric positive semidefinite 51 | * matrix" (1988): https://doi.org/10.1016/0024-3795(88)90223-6 52 | * 53 | */ 54 | template 55 | T NearestPsd(const Eigen::MatrixBase& M) 56 | { 57 | typedef typename T::value_type Scalar; 58 | 59 | T B = (M + M.transpose()) / 2; 60 | 61 | Eigen::JacobiSVD svd(B, Eigen::ComputeThinV); 62 | T H = svd.matrixV().transpose() * svd.singularValues().asDiagonal() * svd.matrixV(); 63 | 64 | T A2 = (B + H) / 2; 65 | T A3 = (A2 + A2.transpose()) / 2; 66 | 67 | int k = 1; 68 | Scalar spacing = 1e-15; 69 | T I = T::Identity(M.rows(), M.cols()); 70 | while (!IsPsd(A3)) 71 | { 72 | Eigen::SelfAdjointEigenSolver es(A3); 73 | Scalar min_eig = es.eigenvalues().minCoeff(); 74 | A3 += I * (-min_eig * k + spacing); 75 | k *= 2; 76 | } 77 | std::cout << "had to do " << log2(k) << " iterations" << std::endl; 78 | return A3; 79 | } 80 | 81 | // correct JtJ to be PSD by zeroing out negative eigenvalues 82 | template 83 | T CorrectPSD(const Eigen::MatrixBase& M) 84 | { 85 | typedef Eigen::SelfAdjointEigenSolver Solver; 86 | typedef typename Solver::EigenvectorsType EigenVecs; 87 | typedef typename Solver::RealVectorType RealVec; 88 | 89 | Solver es; 90 | es.compute(M); 91 | 92 | RealVec D = es.eigenvalues(); 93 | EigenVecs V = es.eigenvectors(); 94 | 95 | D = (D.array() < 0).select(0, D); // zero out negative elems 96 | return V * D.asDiagonal() * V.inverse(); // reconstruct matrix 97 | } 98 | 99 | #endif // DF_NEAREST_PSD_H_ 100 | -------------------------------------------------------------------------------- /sources/common/display_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include 19 | #include "display_utils.h" 20 | 21 | 22 | cv::Mat apply_colormap(cv::Mat mat, cv::ColormapTypes cmap) 23 | { 24 | cv::Mat ret; 25 | mat.convertTo(ret, CV_8UC1, 255.0); 26 | cv::applyColorMap(ret, ret, cmap); 27 | return ret; 28 | } 29 | 30 | cv::Mat GetOpenCV(const vc::Buffer2DView& buf) 31 | { 32 | return cv::Mat(buf.height(), buf.width(), CV_32FC1, buf.rawPtr()); 33 | } 34 | 35 | cv::Mat prx_to_color(vc::Buffer2DView prxbuf, cv::ColormapTypes cmap) 36 | { 37 | return apply_colormap(GetOpenCV(prxbuf), cmap); 38 | } 39 | 40 | /** 41 | * @brief CreateMosaic 42 | * Creates a mosaic that tiles images from the input array in a specified shape. 43 | * This function attempts to unify all input images. All images are resized to match 44 | * the first array element. 45 | * @param array A linear vector of input images 46 | * @param rows Number of rows 47 | * @param cols 48 | * @return CV_8UC3 mosaic image 49 | */ 50 | cv::Mat CreateMosaic(const std::vector& array, int rows, int cols) 51 | { 52 | cv::Mat mosaic; 53 | 54 | if (array.empty()) 55 | return mosaic; 56 | 57 | int width = array[0].cols; 58 | int height = array[0].rows; 59 | 60 | mosaic.create(rows * height, cols * width, CV_8UC3); 61 | 62 | int idx = 0; 63 | for (int i = 0; i < rows; ++i) 64 | { 65 | for (int j = 0; j < cols; ++j) 66 | { 67 | cv::Mat img = array[idx++]; 68 | 69 | // resize input image to match the first image in array 70 | if (width != img.cols || height != img.rows) 71 | cv::resize(img, img, cv::Size(width, height)); 72 | 73 | cv::Mat img_disp; 74 | switch (img.type()) 75 | { 76 | case CV_8UC3: 77 | img_disp = img; 78 | break; 79 | case CV_8UC1: 80 | cv::cvtColor(img, img_disp, cv::COLOR_GRAY2BGR); 81 | break; 82 | case CV_32FC1: 83 | cv::cvtColor(img, img_disp, cv::COLOR_GRAY2BGR); 84 | img_disp.convertTo(img_disp, CV_8UC3, 255.0); 85 | break; 86 | default: 87 | LOG(FATAL) << "Unexpected mat type: " << img.type(); 88 | } 89 | 90 | cv::Rect roi(j*width, i*height, width, height); 91 | cv::Mat dst = mosaic(roi); 92 | img_disp.copyTo(dst); 93 | } 94 | } 95 | 96 | return mosaic; 97 | } 98 | 99 | cv::Mat CreateMosaic(const std::vector& array) 100 | { 101 | int cols = (int) sqrt(array.size()); 102 | int rows = array.size() / cols; 103 | return CreateMosaic(array, rows, cols); 104 | } 105 | -------------------------------------------------------------------------------- /sources/common/display_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_DISPLAY_UTILS_H_ 19 | #define DF_DISPLAY_UTILS_H_ 20 | 21 | #include 22 | #include 23 | 24 | cv::Mat apply_colormap(cv::Mat mat, cv::ColormapTypes cmap = cv::COLORMAP_JET); 25 | 26 | cv::Mat GetOpenCV(const vc::Buffer2DView& buf); 27 | 28 | cv::Mat prx_to_color(vc::Buffer2DView prxbuf, cv::ColormapTypes cmap=cv::COLORMAP_JET); 29 | 30 | cv::Mat CreateMosaic(const std::vector& array, int rows, int cols); 31 | 32 | cv::Mat CreateMosaic(const std::vector& array); 33 | 34 | #endif // DF_DISPLAY_UTILS_H_ 35 | -------------------------------------------------------------------------------- /sources/common/image_sequences.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_IMAGE_SEQUENCES_H_ 19 | #define DF_IMAGE_SEQUENCES_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "pinhole_camera.h" 26 | 27 | struct Intrinsics 28 | { 29 | float fx; 30 | float fy; 31 | float ux; 32 | float uy; 33 | }; 34 | 35 | struct ImageSequence 36 | { 37 | std::string base_dir; 38 | std::vector filenames; 39 | std::vector depth_filenames; 40 | Intrinsics intrinsics; 41 | bool has_depth = false; 42 | }; 43 | 44 | class SequenceCollection 45 | { 46 | public: 47 | ImageSequence Get(const std::string& name); 48 | bool Has(const std::string& name); 49 | void Add(const std::string& name, const ImageSequence& seq); 50 | 51 | private: 52 | std::map map_; 53 | }; 54 | 55 | ImageSequence GetImageSequence(const std::string& name, 56 | const std::string& cfgpath, 57 | df::PinholeCamera& cam); 58 | 59 | 60 | SequenceCollection ParseSequencesFromJson(const std::string& path); 61 | 62 | 63 | #endif // DF_IMAGE_SEQUENCES_H_ 64 | -------------------------------------------------------------------------------- /sources/common/indexed_map.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_INDEXED_MAP_H_ 19 | #define DF_INDEXED_MAP_H_ 20 | 21 | namespace df 22 | { 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | template 30 | class IndexedMap 31 | { 32 | public: 33 | typedef std::map ContainerT; 34 | typedef std::vector IdList; 35 | 36 | /* Default constructor, does nothing */ 37 | IndexedMap() : lastid_(0) {} 38 | 39 | /* Get an item by an id */ 40 | virtual Item Get(const IdType& id) 41 | { 42 | if (!Exists(id)) 43 | LOG(FATAL) << "Requesting item with id " << id << " which is not in the container"; 44 | return map_[id]; 45 | } 46 | 47 | virtual const Item Get(const IdType& id) const 48 | { 49 | if (!Exists(id)) 50 | LOG(FATAL) << "Requesting item with id " << id << " which is not in the container"; 51 | return map_.at(id); 52 | } 53 | 54 | /* Check whether an item with this id exists */ 55 | virtual bool Exists(const IdType& id) const 56 | { 57 | return map_.find(id) != map_.end(); 58 | } 59 | 60 | /* 61 | * Add an existing item to the map. Assign a new id to it. 62 | * Return the id. 63 | */ 64 | virtual IdType Add(const Item& item) 65 | { 66 | map_[++lastid_] = item; 67 | ids_.push_back(lastid_); 68 | return lastid_; 69 | } 70 | 71 | /* Remove a value by Id */ 72 | virtual void Remove(IdType id) 73 | { 74 | if (!Exists(id)) 75 | LOG(FATAL) << "Attempting to remove non-existent item with id " << id; 76 | map_.erase(id); 77 | std::remove(ids_.begin(), ids_.end(), lastid_); 78 | } 79 | 80 | /* Delete everything */ 81 | virtual void Clear() 82 | { 83 | map_.clear(); 84 | ids_.clear(); 85 | lastid_ = 0; 86 | } 87 | 88 | virtual std::size_t Size() const { return map_.size(); } 89 | virtual const IdList& Ids() const { return ids_; } 90 | virtual const IdType LastId() const { return lastid_; } 91 | 92 | protected: 93 | IdType lastid_; 94 | ContainerT map_; 95 | IdList ids_; 96 | }; 97 | 98 | } // namespace df 99 | 100 | #endif // DF_INDEXED_MAP_H_ 101 | -------------------------------------------------------------------------------- /sources/common/interp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include "interp.h" 19 | 20 | Eigen::Quaternionf QuatDamp(const Eigen::Quaternionf& from, 21 | const Eigen::Quaternionf& to, 22 | Eigen::Vector4f& vel, 23 | float delta_time, 24 | float damping, 25 | float max_speed) 26 | { 27 | auto dot = from.dot(to); 28 | if (dot >= 0.9999) 29 | return to; 30 | 31 | // Use the closest rotation 32 | Eigen::Quaternionf q_from = dot < 0 ? Eigen::Quaternionf(-from.coeffs()) : from; 33 | 34 | // n1 = velocity - ( currentRot - targerRot ) * ( omega * omega * dt ); 35 | // n2 = 1 + omega * dt; 36 | // newVelocity = n1 / ( n2 * n2 ); 37 | // currentRot += newVelocity * dt; 38 | float dSqrDt = damping * damping * delta_time; 39 | float n2 = 1 + damping * delta_time; 40 | float n2Sqr = n2 * n2; 41 | vel = (vel - (q_from.coeffs() - to.coeffs()) * dSqrDt) / n2Sqr; 42 | 43 | //......................................... 44 | Eigen::Quaternionf q_new(q_from); 45 | q_new.coeffs() += vel * delta_time; 46 | q_new.normalize(); 47 | return q_new; 48 | } 49 | 50 | Sophus::SE3f Interpolate(float x, const Sophus::SE3f& pose0, const Sophus::SE3f& pose1) 51 | { 52 | Eigen::Vector3f trs = lerp(x, pose0.translation(), pose1.translation()); 53 | Eigen::Quaternionf rot = pose0.so3().unit_quaternion().slerp(x, pose1.so3().unit_quaternion()); 54 | return Sophus::SE3f(rot, trs); 55 | } 56 | -------------------------------------------------------------------------------- /sources/common/interp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_INTERP_H_ 19 | #define DF_INTERP_H_ 20 | 21 | #include 22 | #include 23 | 24 | template 25 | T lerp(float x, const T& a, const T& b) 26 | { 27 | return x*a + (1-x)*b; 28 | } 29 | 30 | template 31 | T SmoothDecay(const T& from, const T& to, float rate, float dt) 32 | { 33 | return from + (to-from) * rate * dt; 34 | } 35 | 36 | /* 37 | * Game Programming Gems 4, Chapter 1.10 38 | * Critically damped spring 39 | */ 40 | template 41 | T SmoothDamp(const T& from, 42 | const T& to, 43 | T& vel, 44 | float delta_time, 45 | float smooth_time, 46 | float max_speed) 47 | { 48 | float omega = 2.f / smooth_time; 49 | float x = omega * delta_time; 50 | float exp = 1.f / (1.f + x + 0.48f * x * x + 0.235f * x * x * x); 51 | 52 | T change = from - to; 53 | float max_change = max_speed * delta_time; 54 | if (change.norm() > max_change) 55 | change = (change / change.norm()) * max_change; 56 | T temp = (vel + omega * change) * delta_time; 57 | vel = (vel - omega * temp) * exp; 58 | return to + (change + temp) * exp; 59 | } 60 | 61 | /* 62 | * Hacky damped spring for Quaternions but it works 63 | * (tracks velocity over x y z and w of the quat...) 64 | * https://gist.github.com/sketchpunk/3568150a04b973430dfe8fd29bf470c8 65 | */ 66 | Eigen::Quaternionf QuatDamp(const Eigen::Quaternionf& from, 67 | const Eigen::Quaternionf& to, 68 | Eigen::Vector4f& vel, 69 | float delta_time, 70 | float damping, 71 | float max_speed); 72 | 73 | /** 74 | * @brief Interpolate Linear interpolation for poses. 75 | */ 76 | Sophus::SE3f Interpolate(float x, const Sophus::SE3f& pose0, const Sophus::SE3f& pose1); 77 | 78 | #endif // DF_INTERP_H_ 79 | -------------------------------------------------------------------------------- /sources/common/intrinsics.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_INTRINSICS_H_ 19 | #define DF_INTRINSICS_H_ 20 | 21 | #include 22 | #include 23 | 24 | #include "pinhole_camera.h" 25 | 26 | namespace df 27 | { 28 | 29 | inline 30 | cv::Mat ChangeIntrinsics(const cv::Mat& img, 31 | const df::PinholeCamera& cam_in, 32 | const df::PinholeCamera& cam_out) 33 | { 34 | cv::Mat inK, outK; 35 | cv::eigen2cv(cam_in.Matrix(), inK); 36 | cv::eigen2cv(cam_out.Matrix(), outK); 37 | 38 | cv::Mat map1, map2; 39 | cv::Size out_size(cam_out.width(), cam_out.height()); 40 | cv::initUndistortRectifyMap(inK, cv::Mat{}, cv::Mat{}, outK, out_size, CV_32FC1, map1, map2); 41 | 42 | cv::Mat remapped; 43 | cv::remap(img, remapped, map1, map2, cv::INTER_LINEAR); 44 | return remapped; 45 | } 46 | 47 | } // namespace df 48 | 49 | #endif // DF_INTRINSICS_H_ 50 | -------------------------------------------------------------------------------- /sources/common/logutils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include 19 | #include 20 | 21 | #include "logutils.h" 22 | 23 | namespace fs = boost::filesystem; 24 | 25 | namespace df 26 | { 27 | 28 | std::string GetTimeStamp(std::string format) 29 | { 30 | std::time_t t = std::time(nullptr); 31 | auto localTime = std::localtime(&t); 32 | std::stringstream ss; 33 | ss << std::put_time(localTime, format.c_str()); 34 | return ss.str(); 35 | } 36 | 37 | void CreateDirIfNotExists(std::string dir) 38 | { 39 | if (!fs::exists(dir)) 40 | fs::create_directory(dir); 41 | } 42 | 43 | std::string CreateLogDirForRun(std::string logdir, std::string run_dir_name) 44 | { 45 | // check if the logging dir exists. If not, create it 46 | CreateDirIfNotExists(logdir); 47 | 48 | // create a timestamped rundir inside log_dir or use whatever the user specified in 49 | // run_dir_name 50 | std::string dir_name = run_dir_name.empty() ? GetTimeStamp() : run_dir_name; 51 | std::string rundir = (fs::path(logdir) / dir_name).string(); 52 | CreateDirIfNotExists(rundir); // shouldn't exist really 53 | 54 | return rundir; 55 | } 56 | 57 | } // namespace df 58 | -------------------------------------------------------------------------------- /sources/common/logutils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_LOGUTILS_H_ 19 | #define DF_LOGUTILS_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace df 26 | { 27 | 28 | std::string GetTimeStamp(std::string format = "%y%m%d%H%M%S"); 29 | void CreateDirIfNotExists(std::string dir); 30 | std::string CreateLogDirForRun(std::string logdir, std::string run_dir_name); 31 | 32 | } // namespace df 33 | 34 | #endif // DF_LOGUTILS_H_ 35 | -------------------------------------------------------------------------------- /sources/common/timing.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | std::map __clocks; 24 | std::mutex clocks_mutex; 25 | bool enable_timing = false; 26 | 27 | void EnableTiming(bool enable) 28 | { 29 | enable_timing = enable; 30 | } 31 | 32 | void tic(std::string name) 33 | { 34 | if (enable_timing) 35 | { 36 | #ifdef ENABLE_TIMING 37 | std::lock_guard lock(clocks_mutex); 38 | if (__clocks.find(name) != __clocks.end()) 39 | LOG(INFO) << "Warning: restarting an existing clock (" << name << ")"; 40 | __clocks[name] = std::clock(); 41 | #else 42 | LOG(FATAL) << "Requesting enable_timing but it was disabled during compilation!"; 43 | #endif 44 | } 45 | } 46 | 47 | void toc(std::string name) 48 | { 49 | if (enable_timing) 50 | { 51 | #ifdef ENABLE_TIMING 52 | std::lock_guard lock(clocks_mutex); 53 | if (__clocks.find(name) == __clocks.end()) 54 | LOG(FATAL) << "Trying to use measure a clock that was not started with tic (" << name << ")"; 55 | 56 | // print and remove from clocks 57 | double elapsed_ms = double(std::clock() - __clocks[name]) / CLOCKS_PER_SEC * 1000.0f; 58 | LOG(INFO) << name << " time: " << elapsed_ms; 59 | 60 | __clocks.erase(name); 61 | #else 62 | LOG(FATAL) << "Requesting enable_timing but it was disabled during compilation!"; 63 | #endif 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /sources/common/timing.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_TIMING_H_ 19 | #define DF_TIMING_H_ 20 | 21 | #include 22 | #include 23 | 24 | void EnableTiming(bool enable); 25 | void tic(std::string name); 26 | void toc(std::string name); 27 | 28 | template 29 | double MeasureTime(Func f) 30 | { 31 | auto start = std::clock(); 32 | f(); 33 | return (std::clock()-start) / (double)( CLOCKS_PER_SEC / 1000); 34 | } 35 | 36 | template 37 | double MeasureTimeAverage(Func f, std::size_t ntests, bool skip_first=true) 38 | { 39 | if (skip_first) 40 | MeasureTime(f); 41 | double total = 0; 42 | for (std::size_t i = 0; i < ntests; ++i) 43 | total += MeasureTime(f); 44 | return total / ntests; 45 | } 46 | 47 | #endif // DF_TIMING_H_ 48 | -------------------------------------------------------------------------------- /sources/common/tum_io.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_TUM_IO_H_ 19 | #define DF_TUM_IO_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | struct TumPose 26 | { 27 | double timestamp; 28 | Eigen::Quaternionf rot; 29 | Eigen::Vector3f trs; 30 | }; 31 | 32 | inline std::istream& operator>>(std::istream& is, TumPose& pose) 33 | { 34 | is >> std::setprecision(16) >> pose.timestamp >> pose.trs(0) >> pose.trs(1) >> pose.trs(2) 35 | >> pose.rot.x() >> pose.rot.y() >> pose.rot.z() >> pose.rot.w(); 36 | return is; 37 | } 38 | 39 | inline std::ostream& operator<<(std::ostream& is, TumPose& pose) 40 | { 41 | is << std::setprecision(16) << std::to_string(pose.timestamp) << " " << pose.trs(0) << " " << pose.trs(1) << " " 42 | << pose.trs(2) << " " << pose.rot.x() << " " << pose.rot.y() << " " 43 | << pose.rot.z() << " " << pose.rot.w(); 44 | return is; 45 | } 46 | 47 | #endif // DF_TUM_IO_H_ 48 | -------------------------------------------------------------------------------- /sources/core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # So this is a big hacky. Brisk leaks architecture specific 2 | # calls in public headers, which forces us to compile for that specific architecture 3 | # (for most cases, march=native will work). GT-SAM has problems with march=native 4 | # due to eigen, so we isolate brisk in a separate library which we compile for 5 | # the native architecture and build the rest of our system without it. 6 | # For this to work, we need to make sure that the interface between the native and 7 | # non-native optimised code is as simple as possible -- no eigen! 8 | add_library(core_brisk_wrap SHARED system/fbrisk.cpp system/fbrisk.h) 9 | target_compile_options(core_brisk_wrap PRIVATE -march=native -mtune=native) 10 | target_link_libraries(core_brisk_wrap PUBLIC 11 | brisk 12 | DBoW2 13 | opencv_core) 14 | target_include_directories(core_brisk_wrap PUBLIC 15 | ${CMAKE_CURRENT_SOURCE_DIR}/system 16 | ${BRISK_INCLUDE_DIRS} 17 | ) 18 | 19 | # ------------------------------------------------------------------------------ 20 | # List sources 21 | # ------------------------------------------------------------------------------ 22 | set(core_sources 23 | # gtsam 24 | gtsam/sparse_geometric_factor.cpp 25 | gtsam/photometric_factor.cpp 26 | gtsam/reprojection_factor.cpp 27 | gtsam/depth_prior_factor.cpp 28 | gtsam/uniform_sampler.cpp 29 | # system 30 | system/camera_tracker.cpp 31 | system/loop_detector.cpp 32 | # features 33 | features/matching.cpp 34 | # mapping 35 | mapping/work.cpp 36 | mapping/work_manager.cpp 37 | mapping/mapper.cpp 38 | mapping/df_work.cpp 39 | # network 40 | network/decoder_network.cpp 41 | # top-level 42 | deepfactors.cpp 43 | deepfactors_options.cpp 44 | ) 45 | 46 | set(core_headers 47 | # gtsam 48 | gtsam/photometric_factor.h 49 | gtsam/reprojection_factor.h 50 | gtsam/gtsam_traits.h 51 | gtsam/sparse_geometric_factor.h 52 | gtsam/gtsam_utils.h 53 | gtsam/uniform_sampler.h 54 | gtsam/factor_graph.h 55 | gtsam/depth_prior_factor.h 56 | # system 57 | system/camera_tracker.h 58 | system/loop_detector.h 59 | # features 60 | features/feature_detection.h 61 | features/matching.h 62 | # mapping 63 | mapping/keyframe_map.h 64 | mapping/keyframe.h 65 | mapping/work_manager.h 66 | mapping/df_work.h 67 | mapping/mapper.h 68 | mapping/frame.h 69 | mapping/work.h 70 | # network 71 | network/tfwrap.h 72 | network/decoder_network.h 73 | # top-level 74 | deepfactors.h 75 | deepfactors_options.h 76 | ) 77 | 78 | # ------------------------------------------------------------------------------ 79 | # Build target 80 | # ------------------------------------------------------------------------------ 81 | add_library(df_core SHARED ${core_sources} ${core_headers}) 82 | target_include_directories(df_core PUBLIC 83 | ${CMAKE_CURRENT_SOURCE_DIR}/features 84 | ${CMAKE_CURRENT_SOURCE_DIR}/gtsam 85 | ${CMAKE_CURRENT_SOURCE_DIR}/mapping 86 | ${CMAKE_CURRENT_SOURCE_DIR}/network 87 | ${CMAKE_CURRENT_SOURCE_DIR}/system 88 | ${CMAKE_CURRENT_SOURCE_DIR} 89 | ) 90 | target_link_libraries(df_core 91 | PUBLIC 92 | df_common 93 | df_cuda 94 | Eigen3::Eigen 95 | VisionCore 96 | DBoW2 97 | gtsam 98 | opengv 99 | glog 100 | core_brisk_wrap 101 | PRIVATE 102 | TensorFlow 103 | jsoncpp 104 | ) 105 | 106 | -------------------------------------------------------------------------------- /sources/core/deepfactors_options.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include "deepfactors_options.h" 19 | #include "glog/logging.h" 20 | 21 | namespace df 22 | { 23 | 24 | DeepFactorsOptions::KeyframeMode DeepFactorsOptions::KeyframeModeTranslator(const std::string& s) 25 | { 26 | if (s == "AUTO") 27 | return KeyframeMode::AUTO; 28 | else if (s == "NEVER") 29 | return KeyframeMode::NEVER; 30 | else if (s == "AUTO_COMBINED") 31 | return KeyframeMode::AUTO_COMBINED; 32 | else 33 | LOG(FATAL) << "Unknown KeyframeMode " << s; 34 | } 35 | 36 | std::string DeepFactorsOptions::KeyframeModeTranslator(KeyframeMode mode) 37 | { 38 | std::string name; 39 | switch (mode) 40 | { 41 | case KeyframeMode::AUTO: 42 | name = "AUTO"; 43 | break; 44 | case KeyframeMode::NEVER: 45 | name = "NEVER"; 46 | break; 47 | case KeyframeMode::AUTO_COMBINED: 48 | name = "AUTO_COMBINED"; 49 | break; 50 | default: 51 | name = "UNKNOWN"; 52 | break; 53 | } 54 | return name; 55 | } 56 | 57 | DeepFactorsOptions::TrackingMode DeepFactorsOptions::TrackingModeTranslator(const std::string& s) 58 | { 59 | if (s == "FIRST") 60 | return TrackingMode::FIRST; 61 | else if (s == "LAST") 62 | return TrackingMode::LAST; 63 | else if (s == "CLOSEST") 64 | return TrackingMode::CLOSEST; 65 | else 66 | LOG(FATAL) << "Invalid tracking mode: " << s; 67 | } 68 | 69 | std::string DeepFactorsOptions::TrackingModeTranslator(TrackingMode mode) 70 | { 71 | std::string name; 72 | switch (mode) 73 | { 74 | case TrackingMode::FIRST: 75 | name = "FIRST"; 76 | break; 77 | case TrackingMode::LAST: 78 | name = "LAST"; 79 | break; 80 | case TrackingMode::CLOSEST: 81 | name = "CLOSEST"; 82 | break; 83 | default: 84 | name = "UNKNOWN"; 85 | break; 86 | } 87 | return name; 88 | } 89 | 90 | DeepFactorsOptions::ConnMode DeepFactorsOptions::ConnectionModeTranslator(const std::string& s) 91 | { 92 | if (s == "FIRST") 93 | return ConnMode::FIRST; 94 | else if (s == "LAST") 95 | return ConnMode::LAST; 96 | else if (s == "LASTN") 97 | return ConnMode::LASTN; 98 | else if (s == "FULL") 99 | return ConnMode::FULL; 100 | else 101 | LOG(FATAL) << "Invalid connection mode: " << s; 102 | } 103 | 104 | std::string DeepFactorsOptions::ConnectionModeTranslator(DeepFactorsOptions::ConnMode mode) 105 | { 106 | std::string name; 107 | switch (mode) 108 | { 109 | case ConnMode::FIRST: 110 | name = "FIRST"; 111 | break; 112 | case ConnMode::LAST: 113 | name = "LAST"; 114 | break; 115 | case ConnMode::LASTN: 116 | name = "LASTN"; 117 | break; 118 | case ConnMode::FULL: 119 | name = "FULL"; 120 | break; 121 | default: 122 | name = "UNKNOWN"; 123 | break; 124 | } 125 | return name; 126 | } 127 | 128 | } // namespace df 129 | -------------------------------------------------------------------------------- /sources/core/deepfactors_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_OPTIONS_H_ 19 | #define DF_OPTIONS_H_ 20 | 21 | #include 22 | #include 23 | #include "mapper.h" 24 | 25 | namespace df 26 | { 27 | 28 | struct DeepFactorsOptions 29 | { 30 | typedef std::vector IntVector; 31 | typedef MapperOptions::ConnectionMode ConnMode; 32 | enum KeyframeMode { AUTO=0, AUTO_COMBINED, NEVER }; 33 | enum TrackingMode { CLOSEST=0, LAST, FIRST }; 34 | 35 | /* required parameters */ 36 | std::size_t gpu = 0; 37 | std::string network_path; 38 | std::string vocabulary_path; 39 | 40 | /* iterations */ 41 | 42 | /* camera tracking */ 43 | IntVector tracking_iterations = {4, 6, 5, 10}; 44 | TrackingMode tracking_mode = CLOSEST; 45 | float tracking_huber_delta = 0.3f; 46 | float tracking_error_threshold = 0.3f; 47 | float tracking_dist_threshold = 2; 48 | 49 | /* misc */ 50 | bool debug = false; 51 | std::string debug_dir; 52 | 53 | /* keyframe connection */ 54 | ConnMode connection_mode = ConnMode::LAST; 55 | std::size_t max_back_connections = 4; 56 | 57 | /* keyframe adding */ 58 | KeyframeMode keyframe_mode = AUTO; 59 | float inlier_threshold = 0.5f; 60 | float dist_threshold = 2.f; 61 | float frame_dist_threshold = 0.2f; 62 | float combined_threshold = 2.0f; 63 | 64 | /* loop closure */ 65 | bool loop_closure = true; 66 | float loop_max_dist = 0.5f; 67 | int loop_active_window = 10; 68 | float loop_sigma = 1.0f; 69 | float loop_min_similarity = 0.35f; 70 | int loop_max_candidates = 10; 71 | 72 | /* mapping params */ 73 | bool interleave_mapping = false; 74 | float relinearize_skip = 1; 75 | float relinearize_threshold = 0.05f; 76 | float partial_relin_check = true; 77 | float pose_prior = 0.3f; 78 | float code_prior = 1.0f; 79 | bool predict_code = true; 80 | 81 | /* photometric error */ 82 | bool use_photometric = true; 83 | IntVector pho_iters = {15, 15, 15, 30}; 84 | float huber_delta = 0.3f; 85 | int sfm_step_threads = 32; 86 | int sfm_step_blocks = 11; 87 | int sfm_eval_threads = 224; 88 | int sfm_eval_blocks = 66; 89 | bool normalize_image = false; // normalize image with: (I-mean)/std 90 | 91 | /* reprojection error */ 92 | bool use_reprojection = false; 93 | int rep_nfeatures = 500; 94 | float rep_scale_factor = 1.2f; 95 | int rep_nlevels = 1; 96 | float rep_max_dist = 30.0; 97 | float rep_huber = 0.1f; 98 | int rep_iters = 15; 99 | float rep_sigma = 1.0f; 100 | int rep_ransac_maxiters = 1000; 101 | float rep_ransac_threshold = 0.0001f; 102 | 103 | /* sparse geometric error */ 104 | bool use_geometric = false; 105 | int geo_npoints = 500; 106 | bool geo_stochastic = false; 107 | float geo_huber = 0.1f; 108 | int geo_iters = 15; 109 | 110 | static KeyframeMode KeyframeModeTranslator(const std::string& s); 111 | static std::string KeyframeModeTranslator(KeyframeMode mode); 112 | static TrackingMode TrackingModeTranslator(const std::string& s); 113 | static std::string TrackingModeTranslator(TrackingMode mode); 114 | static ConnMode ConnectionModeTranslator(const std::string& s); 115 | static std::string ConnectionModeTranslator(ConnMode mode); 116 | }; 117 | 118 | } // namespace df 119 | 120 | #endif // DF_OPTIONS_H_ 121 | -------------------------------------------------------------------------------- /sources/core/features/feature_detection.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_FEATURE_DETECTION_H_ 19 | #define DF_FEATURE_DETECTION_H_ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace df 30 | { 31 | 32 | struct Features 33 | { 34 | enum Type { ORB, BRISK }; 35 | typedef std::vector Keypoints; 36 | 37 | Features() {} 38 | 39 | Features(Type type) 40 | : type(type) {} 41 | 42 | Keypoints keypoints; 43 | cv::Mat descriptors; 44 | Type type; 45 | }; 46 | 47 | class FeatureDetector 48 | { 49 | public: 50 | FeatureDetector() {} 51 | virtual ~FeatureDetector() {} 52 | 53 | virtual Features DetectAndCompute(const cv::Mat& img) = 0; 54 | }; 55 | 56 | struct BriskConfig 57 | { 58 | // detector params 59 | double uniformity_rad = 5; 60 | std::size_t octaves = 4; 61 | double absolute_threshold = 200; 62 | std::size_t max_num_kpt = 400; 63 | 64 | // extractor params 65 | bool rotation_invariant = true; 66 | bool scale_invariant = false; 67 | }; 68 | 69 | class BriskDetector : public FeatureDetector 70 | { 71 | public: 72 | typedef brisk::ScaleSpaceFeatureDetector Detector; 73 | typedef brisk::BriskDescriptorExtractor Extractor; 74 | 75 | BriskDetector(BriskConfig cfg = BriskConfig{}) 76 | { 77 | detector_ = std::make_unique(cfg.uniformity_rad, cfg.octaves, 78 | cfg.absolute_threshold, 79 | cfg.max_num_kpt); 80 | extractor_ = std::make_unique(cfg.rotation_invariant, 81 | cfg.scale_invariant); 82 | } 83 | 84 | virtual ~BriskDetector() override {} 85 | 86 | virtual Features DetectAndCompute(const cv::Mat& img) override 87 | { 88 | CHECK_EQ(img.channels(), 1) << "Non-grayscale image passed to BriskDetector"; 89 | Features f{Features::BRISK}; 90 | detector_->detect(img, f.keypoints); 91 | extractor_->compute(img, f.keypoints, f.descriptors); 92 | return f; 93 | } 94 | 95 | private: 96 | std::unique_ptr detector_; 97 | std::unique_ptr extractor_; 98 | }; 99 | 100 | class OrbDetector : public FeatureDetector 101 | { 102 | public: 103 | OrbDetector(int nfeatures, float scale_factor, int nlevels) 104 | { 105 | orb_ = cv::ORB::create(nfeatures, scale_factor, nlevels); 106 | } 107 | 108 | virtual ~OrbDetector() override {} 109 | 110 | virtual Features DetectAndCompute(const cv::Mat& img) override 111 | { 112 | CHECK_EQ(img.channels(), 1) << "Non-grayscale image passed to BriskDetector"; 113 | Features f{Features::ORB}; 114 | orb_->detectAndCompute(img, cv::Mat{}, f.keypoints, f.descriptors); 115 | return f; 116 | } 117 | 118 | private: 119 | cv::Ptr orb_; 120 | }; 121 | 122 | } // namespace df 123 | 124 | #endif // DF_FEATURE_DETECTION_H_ 125 | -------------------------------------------------------------------------------- /sources/core/features/matching.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_MATCHING_H_ 19 | #define DF_MATCHING_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace df 26 | { 27 | 28 | std::vector PruneMatchesByThreshold(const std::vector& matches, 29 | float max_dist = 40.f); 30 | 31 | void ConvertoToBearingVectors(const std::vector& points0, 32 | const std::vector& points1, 33 | const cv::Mat& cameraMatrix, 34 | opengv::bearingVectors_t* bearingVectors0, 35 | opengv::bearingVectors_t* bearingVectors1); 36 | 37 | void MatchesToPointVectors(const std::vector& keypoints0, 38 | const std::vector& keypoints1, 39 | const std::vector& matches, 40 | std::vector* points0, 41 | std::vector* points1); 42 | 43 | std::vector 44 | PruneMatchesEightPoint(const std::vector& keypoints0, 45 | const std::vector& keypoints1, 46 | std::vector matches, 47 | cv::Mat cameraMatrix, 48 | double threshold = 0.0001, 49 | int max_iterations = 1000, 50 | double prob = 0.99); 51 | 52 | } // namespace df 53 | 54 | #endif // DF_MATCHING_H_ 55 | -------------------------------------------------------------------------------- /sources/core/gtsam/depth_prior_factor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_DEPTH_PRIOR_FACTOR_H_ 19 | #define DF_DEPTH_PRIOR_FACTOR_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "keyframe.h" 26 | #include "cu_depthaligner.h" 27 | 28 | namespace df 29 | { 30 | 31 | template 32 | class DepthPriorFactor : public gtsam::NonlinearFactor 33 | { 34 | typedef gtsam::Vector CodeT; 35 | typedef gtsam::NonlinearFactor Base; 36 | typedef df::Keyframe KeyframeT; 37 | typedef typename KeyframeT::Ptr KeyframePtr; 38 | typedef vc::Buffer2DManaged DepthBufferDevice; 39 | typedef vc::Buffer2DManaged DepthBufferHost; 40 | typedef df::DepthAligner AlignerT; 41 | typedef typename AlignerT::Ptr AlignerPtr; 42 | typedef typename AlignerT::ReductionItem StepResult; 43 | 44 | public: 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | 47 | DepthPriorFactor(const cv::Mat& dpt, 48 | const KeyframePtr& kf, 49 | const gtsam::Key& code_key, 50 | Scalar sigma, 51 | std::size_t pyrlevels, 52 | Scalar avgdpt, 53 | AlignerPtr aligner); 54 | virtual ~DepthPriorFactor(); 55 | 56 | /** 57 | * Calculate the error of the factor 58 | * This is equal to the log of gaussian likelihood, e.g. \f$ 0.5(h(x)-z)^2/sigma^2 \f$ 59 | */ 60 | double error(const gtsam::Values& c) const override; 61 | 62 | /* 63 | * Linearize the factor around values c. Returns a HessianFactor 64 | */ 65 | boost::shared_ptr linearize(const gtsam::Values& c) const override; 66 | 67 | /** 68 | * Get the dimension of the factor (number of rows on linearization) 69 | */ 70 | size_t dim() const override { return CS; }; 71 | 72 | private: 73 | /** 74 | * Run the alignment on all pyramid levels 75 | */ 76 | StepResult RunAlignment(const CodeT& code) const; 77 | 78 | /** 79 | * Update the keyframe depth to match a given code 80 | */ 81 | void UpdateKfDepth(const CodeT& code) const; 82 | 83 | private: 84 | gtsam::Key code_key_; 85 | Scalar avgdpt_; 86 | Scalar sigma_; 87 | std::size_t pyrlevels_; 88 | KeyframePtr kf_; 89 | AlignerPtr aligner_; 90 | vc::RuntimeBufferPyramidManaged pyr_tgtdpt_; 91 | }; 92 | 93 | } // namespace df 94 | 95 | #endif // DF_DEPTH_PRIOR_FACTOR_H_ 96 | -------------------------------------------------------------------------------- /sources/core/gtsam/gtsam_traits.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_GTSAM_TRAITS_H_ 19 | #define DF_GTSAM_TRAITS_H_ 20 | 21 | #include 22 | 23 | namespace gtsam 24 | { 25 | 26 | template 27 | struct traits> 28 | { 29 | typedef Sophus::SE3 SE3T; 30 | 31 | /** Return the dimensionality of the tangent space of this value */ 32 | static size_t GetDimension(const SE3T& pose) 33 | { 34 | return SE3T::DoF; 35 | } 36 | 37 | static void Print(const SE3T& pose, const std::string& str) 38 | { 39 | std::cout << str << pose.log().transpose(); 40 | } 41 | 42 | /** Increment the value, by mapping3 from the vector delta in the tangent 43 | * space of the current value back to the manifold to produce a new, 44 | * incremented value. 45 | * @param delta The delta vector in the tangent space of this value, by 46 | * which to increment this value. 47 | */ 48 | static SE3T Retract(const SE3T& pose, const gtsam::Vector& delta) 49 | { 50 | Eigen::Matrix update = delta.cast(); 51 | Eigen::Matrix trs_update = update.template head<3>(); 52 | Eigen::Matrix rot_update = update.template tail<3>(); 53 | 54 | SE3T pose_new; 55 | pose_new.translation() = pose.translation() + trs_update; 56 | pose_new.so3() = SE3T::SO3Type::exp(rot_update) * pose.so3(); 57 | return pose_new; 58 | } 59 | 60 | /** Compute the coordinates in the tangent space of this value that 61 | * retract() would map to \c value. 62 | * @param value The value whose coordinates should be determined in the 63 | * tangent space of the value on which this function is called. 64 | * @return The coordinates of \c value in the tangent space of \c this. 65 | */ 66 | static gtsam::Vector Local(const SE3T& first, const SE3T& second) 67 | { 68 | typename SE3T::Tangent tangent; 69 | tangent.template head<3>() = second.translation() - first.translation(); 70 | tangent.template tail<3>() = (second.so3() * first.so3().inverse()).log(); 71 | return tangent.template cast(); 72 | } 73 | 74 | /** Compare this Value with another for equality. */ 75 | static bool Equals(const SE3T& first, const SE3T& second, Scalar tol) 76 | { 77 | return Local(first, second).norm() < tol; 78 | } 79 | }; 80 | 81 | /* 82 | * instantiate SE3 traits for double and float 83 | */ 84 | template struct traits>; 85 | template struct traits>; 86 | 87 | } // namespace gtsam 88 | 89 | #endif // DF_GTSAM_TRAITS_H_ 90 | -------------------------------------------------------------------------------- /sources/core/gtsam/gtsam_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_GTSAM_UTILS_H_ 19 | #define DF_GTSAM_UTILS_H_ 20 | 21 | #include 22 | #include 23 | 24 | /* 25 | * Shorthand functions to get keys for certain keyframe id 26 | */ 27 | inline gtsam::Key AuxPoseKey(std::uint64_t j) { return gtsam::Symbol('a', j); } 28 | inline gtsam::Key PoseKey(std::uint64_t j) { return gtsam::Symbol('p', j); } 29 | inline gtsam::Key CodeKey(std::uint64_t j) { return gtsam::Symbol('c', j); } 30 | 31 | #endif // DF_GTSAM_UTILS_H_ 32 | -------------------------------------------------------------------------------- /sources/core/gtsam/sparse_geometric_factor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_SPARSE_GEOMETRIC_FACTOR_H_ 19 | #define DF_SPARSE_GEOMETRIC_FACTOR_H_ 20 | 21 | #include 22 | #include 23 | 24 | #include "uniform_sampler.h" 25 | #include "pinhole_camera.h" 26 | #include "keyframe.h" 27 | 28 | namespace df 29 | { 30 | 31 | template 32 | class SparseGeometricFactor : public gtsam::NonlinearFactor 33 | { 34 | typedef SparseGeometricFactor This; 35 | typedef gtsam::NonlinearFactor Base; 36 | typedef Sophus::SE3 PoseT; 37 | typedef gtsam::Vector CodeT; 38 | typedef df::Keyframe KeyframeT; 39 | typedef typename KeyframeT::Ptr KeyframePtr; 40 | 41 | public: 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 43 | 44 | /*! 45 | * \brief Constructor calculating the sparse sampling 46 | */ 47 | SparseGeometricFactor(const df::PinholeCamera& cam, 48 | const KeyframePtr& kf0, 49 | const KeyframePtr& kf1, 50 | const gtsam::Key& pose0_key, 51 | const gtsam::Key& pose1_key, 52 | const gtsam::Key& code0_key, 53 | const gtsam::Key& code1_key, 54 | int num_points, 55 | Scalar huber_delta, 56 | bool stochastic); 57 | 58 | /*! 59 | * \brief Constructor that takes sampled points (for clone) 60 | */ 61 | SparseGeometricFactor(const df::PinholeCamera& cam, 62 | const std::vector& points, 63 | const KeyframePtr& kf0, 64 | const KeyframePtr& kf1, 65 | const gtsam::Key& pose0_key, 66 | const gtsam::Key& pose1_key, 67 | const gtsam::Key& code0_key, 68 | const gtsam::Key& code1_key, 69 | Scalar huber_delta, 70 | bool stochastic); 71 | 72 | virtual ~SparseGeometricFactor(); 73 | 74 | /*! 75 | * \brief Calculate the error of the factor 76 | * This is equal to the log of gaussian likelihood, e.g. \f$ 0.5(h(x)-z)^2/sigma^2 \f$ 77 | * \param c Values to evaluate the error at 78 | * \return The error 79 | */ 80 | double error(const gtsam::Values& c) const override; 81 | 82 | /*! 83 | * \brief Linearizes this factor at a linearization point 84 | * \param c Linearization point 85 | * \return Linearized factor 86 | */ 87 | boost::shared_ptr linearize(const gtsam::Values& c) const override; 88 | 89 | /*! 90 | * \brief Get the dimension of the factor (number of rows on linearization) 91 | * \return 92 | */ 93 | size_t dim() const override { return points_.size(); } 94 | 95 | /*! 96 | * \brief Return a string describing this factor 97 | * \return Factor description string 98 | */ 99 | std::string Name() const; 100 | 101 | /*! 102 | * \brief Clone the factor 103 | * \return shared ptr to a cloned factor 104 | */ 105 | virtual shared_ptr clone() const; 106 | 107 | /*! 108 | * \brief Return an image of drawn matches for debug 109 | * \return 110 | * TODO 111 | */ 112 | // cv::Mat DrawMatches() const; 113 | 114 | private: 115 | /* variables we tie with this factor */ 116 | gtsam::Key pose0_key_; 117 | gtsam::Key pose1_key_; 118 | gtsam::Key code0_key_; 119 | gtsam::Key code1_key_; 120 | 121 | /* data required to do the warping */ 122 | df::PinholeCamera cam_; 123 | mutable std::vector points_; 124 | Scalar huber_delta_; 125 | 126 | /* we do modify the keyframes in the factor */ 127 | mutable KeyframePtr kf0_; 128 | mutable KeyframePtr kf1_; 129 | 130 | bool stochastic_; 131 | }; 132 | 133 | } // namespace df 134 | 135 | #endif // DF_SPARSE_GEOMETRIC_FACTOR_H_ 136 | -------------------------------------------------------------------------------- /sources/core/gtsam/uniform_sampler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include "uniform_sampler.h" 19 | 20 | namespace df 21 | { 22 | 23 | UniformSampler::UniformSampler(int im_width, int im_height) 24 | : uniform01_(0.0, 1.0), im_width_(im_width), im_height_(im_height) 25 | { 26 | unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); 27 | generator_ = std::mt19937(seed); 28 | } 29 | 30 | std::vector UniformSampler::SamplePoints(int num_points) 31 | { 32 | std::vector pts; 33 | for (int i = 0; i < num_points; ++i) 34 | { 35 | Point p; 36 | p.x = im_width_ * uniform01_(generator_); 37 | p.y = im_height_ * uniform01_(generator_); 38 | pts.push_back(p); 39 | } 40 | return pts; 41 | } 42 | 43 | } // namespace df 44 | -------------------------------------------------------------------------------- /sources/core/gtsam/uniform_sampler.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_UNIFORM_SAMPLER_H_ 19 | #define DF_UNIFORM_SAMPLER_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | namespace df 26 | { 27 | 28 | struct Point 29 | { 30 | int x; 31 | int y; 32 | }; 33 | 34 | class UniformSampler 35 | { 36 | public: 37 | UniformSampler(int im_width, int im_height); 38 | 39 | std::vector SamplePoints(int num_points); 40 | 41 | private: 42 | std::mt19937 generator_; 43 | std::uniform_real_distribution uniform01_; 44 | 45 | int im_width_; 46 | int im_height_; 47 | }; 48 | 49 | } // namespace df 50 | 51 | #endif // DF_UNIFORM_SAMPLER_H_ 52 | -------------------------------------------------------------------------------- /sources/core/mapping/frame.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_FRAME_H_ 19 | #define DF_FRAME_H_ 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "synced_pyramid.h" 29 | #include "cu_image_proc.h" 30 | #include "feature_detection.h" 31 | 32 | namespace df 33 | { 34 | 35 | template 36 | class Frame 37 | { 38 | public: 39 | typedef Frame This; 40 | typedef std::shared_ptr Ptr; 41 | typedef Sophus::SE3 SE3T; 42 | typedef Eigen::Matrix GradT; 43 | typedef df::SyncedBufferPyramid ImagePyramid; 44 | typedef df::SyncedBufferPyramid GradPyramid; 45 | typedef std::size_t IdType; 46 | 47 | Frame() = delete; 48 | Frame(std::size_t pyrlevels, std::size_t w, std::size_t h) 49 | : pyr_img(pyrlevels, w, h), 50 | pyr_grad(pyrlevels, w, h), 51 | id(0), 52 | width(w), 53 | height(h) {} 54 | 55 | Frame(const Frame& other) 56 | : pyr_img(other.pyr_img), 57 | pyr_grad(other.pyr_grad), 58 | pose_wk(other.pose_wk), 59 | id(other.id), 60 | features(other.features), 61 | timestamp(other.timestamp), 62 | bow_vec(other.bow_vec), 63 | has_keypoints(other.has_keypoints), 64 | marginalized(other.marginalized) 65 | { 66 | color_img = other.color_img.clone(); 67 | } 68 | 69 | virtual ~Frame() {} 70 | 71 | virtual Ptr Clone() 72 | { 73 | return std::make_shared(*this); 74 | } 75 | 76 | virtual std::string Name() { return "fr" + std::to_string(id); } 77 | 78 | virtual bool IsKeyframe() { return false; } 79 | 80 | void FillPyramids(const cv::Mat& img, uint pyrlevels) 81 | { 82 | for(uint i = 0; i < pyrlevels; ++i) 83 | { 84 | if (i == 0) 85 | { 86 | vc::Image2DView tmp1(img); 87 | pyr_img.GetGpuLevel(0).copyFrom(tmp1); 88 | df::SobelGradients(pyr_img.GetGpuLevel(0), pyr_grad.GetGpuLevel(0)); 89 | continue; 90 | } 91 | df::GaussianBlurDown(pyr_img.GetGpuLevel(i-1), pyr_img.GetGpuLevel(i)); 92 | df::SobelGradients(pyr_img.GetGpuLevel(i), pyr_grad.GetGpuLevel(i)); 93 | } 94 | } 95 | 96 | // buffers that can exist on CPU or GPU 97 | ImagePyramid pyr_img; 98 | GradPyramid pyr_grad; 99 | 100 | // data only on CPU 101 | SE3T pose_wk; 102 | cv::Mat color_img; // original color image for display 103 | IdType id; 104 | 105 | // size 106 | std::size_t width; 107 | std::size_t height; 108 | 109 | // sparse features 110 | Features features; 111 | 112 | // time of rgb image acquisition 113 | double timestamp; 114 | 115 | // bag of words representation 116 | DBoW2::BowVector bow_vec; 117 | 118 | bool has_keypoints = false; 119 | bool marginalized = false; 120 | }; 121 | 122 | } // namespace df 123 | 124 | #endif // DF_FRAME_H_ 125 | -------------------------------------------------------------------------------- /sources/core/mapping/keyframe.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_KEYFRAME_H_ 19 | #define DF_KEYFRAME_H_ 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include "synced_pyramid.h" 28 | #include "frame.h" 29 | 30 | namespace df 31 | { 32 | 33 | template 34 | class Keyframe : public df::Frame 35 | { 36 | public: 37 | typedef Keyframe This; 38 | typedef Frame Base; 39 | typedef typename Base::GradT GradT; 40 | typedef Eigen::Matrix CodeT; 41 | typedef df::SyncedBufferPyramid ImagePyramid; 42 | typedef vc::Buffer2DManaged CpuGradBuffer; 43 | typedef std::shared_ptr> Ptr; 44 | 45 | Keyframe() = delete; 46 | Keyframe(std::size_t pyrlevels, std::size_t w, std::size_t h, std::size_t cs) 47 | : Base(pyrlevels, w, h), 48 | pyr_dpt(pyrlevels, w, h), 49 | pyr_vld(pyrlevels, w, h), 50 | pyr_stdev(pyrlevels, w, h), 51 | pyr_prx_orig(pyrlevels, w, h), 52 | pyr_jac(pyrlevels, cs*w, h), 53 | dpt_grad(w,h) 54 | { 55 | code = CodeT::Zero(cs, 1); 56 | } 57 | 58 | Keyframe(const Keyframe& other) 59 | : Base(other), 60 | pyr_dpt(other.pyr_dpt), 61 | pyr_vld(other.pyr_vld), 62 | pyr_stdev(other.pyr_stdev), 63 | pyr_prx_orig(other.pyr_prx_orig), 64 | pyr_jac(other.pyr_jac), 65 | dpt_grad(other.pyr_img.Width(), other.pyr_img.Height()) 66 | { 67 | code = other.code; 68 | dpt_grad.copyFrom(other.dpt_grad); 69 | } 70 | 71 | virtual ~Keyframe() {} 72 | 73 | virtual typename Base::Ptr Clone() override 74 | { 75 | return std::make_shared(*this); 76 | } 77 | 78 | virtual std::string Name() override { return "kf" + std::to_string(this->id); } 79 | 80 | // virtual bool operator==(const Frame& other) override 81 | // { 82 | // return other.IsKeyframe() && (id == other.id); 83 | // } 84 | 85 | virtual bool IsKeyframe() override { return true; } 86 | 87 | // buffers that can exist on CPU or GPU 88 | ImagePyramid pyr_dpt; 89 | ImagePyramid pyr_vld; 90 | ImagePyramid pyr_stdev; 91 | ImagePyramid pyr_prx_orig; 92 | ImagePyramid pyr_jac; 93 | 94 | // data only on CPU 95 | CpuGradBuffer dpt_grad; 96 | CodeT code; 97 | }; 98 | 99 | } // namespace df 100 | 101 | #endif // DF_KEYFRAME_H_ 102 | -------------------------------------------------------------------------------- /sources/core/mapping/work.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include "work.h" 19 | 20 | namespace df 21 | { 22 | namespace work 23 | { 24 | 25 | int Work::next_id = 0; 26 | 27 | Work::~Work() {} 28 | 29 | Work::Work() 30 | { 31 | id = "[" + std::to_string(next_id++) + "]"; 32 | } 33 | 34 | Work::Ptr Work::AddChild(Ptr child) 35 | { 36 | child_ = child; 37 | return child; 38 | } 39 | 40 | Work::Ptr Work::RemoveChild() 41 | { 42 | auto child = child_; 43 | child_.reset(); 44 | return child; 45 | } 46 | 47 | } // namespace work 48 | } // namespace df 49 | -------------------------------------------------------------------------------- /sources/core/mapping/work.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_WORK_H_ 19 | #define DF_WORK_H_ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | // typedef FactorIndices so that we don't have to include ISAM2 header here 28 | namespace gtsam 29 | { 30 | typedef gtsam::FastVector FactorIndices; 31 | } 32 | 33 | namespace df 34 | { 35 | namespace work 36 | { 37 | 38 | class Work 39 | { 40 | public: 41 | typedef std::shared_ptr Ptr; 42 | 43 | Work(); 44 | 45 | virtual ~Work(); 46 | 47 | // interface that needs to be implemented 48 | // by child functions 49 | virtual void Bookkeeping(gtsam::NonlinearFactorGraph& new_factors, 50 | gtsam::FastVector& remove_indices, 51 | gtsam::Values& var_init) = 0; 52 | virtual void Update() = 0; 53 | virtual bool Finished() const = 0; 54 | virtual std::string Name() = 0; 55 | 56 | virtual void SignalNoRelinearize() {} 57 | virtual void SignalRemove() {} 58 | virtual void LastFactorIndices(gtsam::FactorIndices& indices) {} 59 | 60 | template 61 | Ptr AddChild(Args&&... args) 62 | { 63 | auto child = std::make_shared(std::forward(args)...); 64 | return AddChild(child); 65 | } 66 | 67 | Ptr AddChild(Ptr child); 68 | Ptr RemoveChild(); 69 | 70 | std::string Id() const { return id; } 71 | 72 | private: 73 | Ptr child_; 74 | std::string id; 75 | 76 | static int next_id; 77 | }; 78 | 79 | } // namespace work 80 | } // namespace df 81 | 82 | #endif // DF_WORK_H_ 83 | -------------------------------------------------------------------------------- /sources/core/mapping/work_manager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include "work_manager.h" 19 | 20 | namespace df 21 | { 22 | namespace work 23 | { 24 | 25 | WorkManager::WorkPtr WorkManager::AddWork(WorkPtr work) 26 | { 27 | work_.push_back(work); 28 | work_map_.insert({work->Id(), work}); 29 | return work; 30 | } 31 | 32 | void WorkManager::Bookkeeping(gtsam::NonlinearFactorGraph &new_factors, 33 | gtsam::FactorIndices &remove_indices, 34 | gtsam::Values &var_init) 35 | { 36 | // factors added this iteration 37 | // will be used in consuming the factor indices after ISAM2::update step 38 | last_new_factors_.clear(); 39 | 40 | // collect bookkeeping from all work 41 | for (auto& work : work_) 42 | { 43 | gtsam::NonlinearFactorGraph add_factors; 44 | gtsam::FactorIndices rem_factors; 45 | gtsam::Values init; 46 | work->Bookkeeping(add_factors, rem_factors, init); 47 | 48 | // keep track which work items added factors 49 | if (!add_factors.empty()) 50 | last_new_factors_.insert({work->Id(), add_factors.size()}); 51 | 52 | // add new factors, vars, etc 53 | new_factors += add_factors; 54 | remove_indices.insert(remove_indices.end(), rem_factors.begin(), rem_factors.end()); 55 | var_init.insert(init); 56 | } 57 | } 58 | 59 | void WorkManager::DistributeIndices(gtsam::FactorIndices indices) 60 | { 61 | for (auto& kv : last_new_factors_) 62 | { 63 | std::string id = kv.first; 64 | auto work = work_map_[id]; 65 | int n = kv.second; 66 | 67 | // first N goes to id 68 | gtsam::FactorIndices ind(indices.begin(), indices.begin() + n); 69 | if (work) 70 | work->LastFactorIndices(ind); 71 | 72 | // remove 73 | indices.erase(indices.begin(), indices.begin() + n); 74 | } 75 | last_new_factors_.clear(); 76 | } 77 | 78 | void WorkManager::Remove(std::function f) 79 | { 80 | auto it = std::remove_if(work_.begin(), work_.end(), f); 81 | for (auto ii = it; ii != work_.end(); ++ii) 82 | (*ii)->SignalRemove(); 83 | } 84 | 85 | void WorkManager::Erase(std::function f) 86 | { 87 | auto it = std::remove_if(work_.begin(), work_.end(), f); 88 | work_.erase(it, work_.end()); 89 | } 90 | 91 | void WorkManager::Update() 92 | { 93 | VLOG(1) << "[WorkManager::Update] Current work:"; 94 | for (auto& work : work_) 95 | VLOG(1) << work->Name(); 96 | 97 | auto it = work_.begin(); 98 | while (it != work_.end()) 99 | { 100 | WorkPtr work = *it; 101 | 102 | // in case we need to remove this work 103 | auto this_it = it; 104 | it++; 105 | 106 | work->Update(); 107 | 108 | // check if the work has finished 109 | if (work->Finished()) 110 | { 111 | VLOG(1) << "Work " << work->Id() << " has finished"; 112 | 113 | // add its child if it has one 114 | auto child = work->RemoveChild(); 115 | if (child) 116 | { 117 | VLOG(1) << "Adding its child: " << child->Id(); 118 | AddWork(child); 119 | } 120 | work_map_.erase(work->Id()); 121 | work_.erase(this_it); 122 | } 123 | } 124 | } 125 | 126 | void WorkManager::SignalNoRelinearize() 127 | { 128 | for (auto& work : work_) 129 | work->SignalNoRelinearize(); 130 | } 131 | 132 | void WorkManager::PrintWork() 133 | { 134 | for (auto& work : work_) 135 | LOG(INFO) << work->Name(); 136 | } 137 | 138 | void WorkManager::Clear() 139 | { 140 | work_.clear(); 141 | work_map_.clear(); 142 | last_new_factors_.clear(); 143 | } 144 | 145 | } // namespace work 146 | } // namespace df 147 | -------------------------------------------------------------------------------- /sources/core/mapping/work_manager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_WORK_MANAGER_H_ 19 | #define DF_WORK_MANAGER_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "work.h" 26 | #include "df_work.h" 27 | 28 | namespace df 29 | { 30 | namespace work 31 | { 32 | 33 | class WorkManager 34 | { 35 | public: 36 | typedef Work::Ptr WorkPtr; 37 | 38 | template 39 | WorkPtr AddWork(Args&&... args) 40 | { 41 | auto work = std::make_shared(std::forward(args)...); 42 | return AddWork(work); 43 | } 44 | 45 | WorkPtr AddWork(WorkPtr work); 46 | 47 | void Bookkeeping(gtsam::NonlinearFactorGraph& new_factors, 48 | gtsam::FactorIndices& remove_indices, 49 | gtsam::Values& var_init); 50 | 51 | void DistributeIndices(gtsam::FactorIndices indices); 52 | void Remove(std::function f); 53 | void Erase(std::function f); 54 | 55 | void Update(); 56 | void SignalNoRelinearize(); 57 | 58 | void PrintWork(); 59 | bool Empty() const { return work_.empty(); } 60 | void Clear(); 61 | 62 | private: 63 | std::list work_; 64 | std::map work_map_; 65 | std::map last_new_factors_; 66 | }; 67 | 68 | } // namespace work 69 | } // namespace df 70 | 71 | #endif // DF_WORK_MANAGER_H_ 72 | -------------------------------------------------------------------------------- /sources/core/network/decoder_network.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DECODER_NETWORK_H_ 19 | #define DECODER_NETWORK_H_ 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace df 28 | { 29 | 30 | /* 31 | * Decoder network class 32 | */ 33 | class DecoderNetwork 34 | { 35 | public: 36 | typedef std::shared_ptr Ptr; 37 | 38 | struct NetworkConfig 39 | { 40 | struct Cam { double fx, fy, u0, v0; }; 41 | Cam camera; 42 | 43 | std::string graph_path; 44 | std::size_t input_width; 45 | std::size_t input_height; 46 | std::size_t pyramid_levels; 47 | std::size_t code_size; 48 | bool grayscale; 49 | double avg_dpt; 50 | 51 | bool depth_pred; 52 | 53 | // input tensor names 54 | std::string input_image_name; 55 | std::string input_code_name; 56 | 57 | // output tensor names 58 | std::string code_pred_name; 59 | std::vector depth_est_names; 60 | std::vector depth_jac_names; 61 | std::vector depth_std_names; 62 | std::vector depth_pred_names; 63 | }; 64 | 65 | DecoderNetwork(const NetworkConfig &cfg); 66 | ~DecoderNetwork(); 67 | 68 | void Decode(const vc::Buffer2DView& image, 69 | const Eigen::MatrixXf& code, 70 | vc::RuntimeBufferPyramidView* prx_out = nullptr, 71 | vc::RuntimeBufferPyramidView* stdev_out = nullptr, 72 | vc::RuntimeBufferPyramidView* jac = nullptr); 73 | 74 | void PredictAndDecode(const vc::Buffer2DView& image, 75 | const Eigen::MatrixXf& code, 76 | Eigen::MatrixXf* pred_code, 77 | vc::RuntimeBufferPyramidView* prx_out = nullptr, 78 | vc::RuntimeBufferPyramidView* stdev_out = nullptr, 79 | vc::RuntimeBufferPyramidView* jac = nullptr); 80 | 81 | private: 82 | NetworkConfig cfg_; 83 | vc::Buffer2DView image_view; 84 | vc::Buffer1DView code_view; 85 | 86 | // tensorflow in pimpl 87 | struct Impl; 88 | std::unique_ptr impl_; 89 | 90 | // create vc wrappers around tensor data 91 | vc::Buffer2DView image_view_; 92 | vc::Buffer1DView code_view_; 93 | }; 94 | 95 | /* 96 | * Network config loading functions 97 | */ 98 | DecoderNetwork::NetworkConfig LoadJsonNetworkConfig(const std::string& cfgpath); 99 | 100 | } // namespace df 101 | 102 | #endif // DECODER_NETWORK_H_ 103 | -------------------------------------------------------------------------------- /sources/core/system/camera_tracker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_CAMERA_TRACKER_H_ 19 | #define DF_CAMERA_TRACKER_H_ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include "cu_se3aligner.h" 28 | #include "keyframe.h" 29 | #include "camera_pyramid.h" 30 | 31 | // forward declarations 32 | namespace cv { class Mat; } 33 | 34 | namespace df 35 | { 36 | 37 | /** 38 | * Tracks against a specified keyframe 39 | * Implements gaussian pyramid scheme 40 | * Uses SO3Aligner and SE3Aligner 41 | */ 42 | class CameraTracker 43 | { 44 | public: 45 | typedef Eigen::Matrix GradType; 46 | typedef df::Keyframe KeyframeT; 47 | typedef vc::RuntimeBufferPyramidManaged ImageBufferPyramid; 48 | typedef vc::RuntimeBufferPyramidManaged GradBufferPyramid; 49 | 50 | struct TrackerConfig 51 | { 52 | std::size_t pyramid_levels; 53 | std::vector iterations_per_level = {10, 5, 4}; 54 | double huber_delta; 55 | }; 56 | 57 | CameraTracker() = delete; 58 | CameraTracker(const df::CameraPyramid& camera_pyr, const TrackerConfig& config); 59 | virtual ~CameraTracker(); 60 | 61 | void TrackFrame(const ImageBufferPyramid& img1, const GradBufferPyramid& grad1); 62 | void Reset(); 63 | 64 | void SetKeyframe(std::shared_ptr kf); 65 | void SetPose(const Sophus::SE3f& pose_wc); 66 | void SetConfig(const TrackerConfig& new_cfg); 67 | 68 | Sophus::SE3f GetPoseEstimate(); 69 | float GetInliers() { return inliers_; } 70 | float GetError() { return error_; } 71 | cv::Mat GetResidualImage() { return residual_; } 72 | 73 | private: 74 | float inliers_; 75 | float error_; 76 | Sophus::SE3f pose_ck_; 77 | TrackerConfig config_; 78 | CameraPyramid camera_pyr_; 79 | std::shared_ptr kf_; 80 | cv::Mat residual_; 81 | 82 | df::SE3Aligner se3aligner_; 83 | // df::SO3Aligner so3aligner_; 84 | }; 85 | 86 | } // namespace df 87 | 88 | #endif // DF_CAMERA_TRACKER_H_ 89 | -------------------------------------------------------------------------------- /sources/core/system/fbrisk.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | namespace DBoW2 { 26 | 27 | void FBrisk::meanValue(const std::vector& descriptors, 28 | TDescriptor& mean) 29 | { 30 | mean.resize(0); 31 | mean.resize(L, 0); 32 | 33 | uint64_t s = descriptors.size() / 2; 34 | std::vector sum(L*8, 0); 35 | 36 | for(auto it = descriptors.begin(); it != descriptors.end(); ++it) 37 | { 38 | const auto& desc = **it; 39 | for(int i = 0; i < L; i++) 40 | { 41 | for(int bit = 0; bit < 8; ++bit) 42 | { 43 | sum[i*8 + bit] += (desc[i] & (0x01 << bit)) ? 1 : 0; 44 | } 45 | } 46 | } 47 | 48 | for(int i = 0; i < L; i++) 49 | { 50 | for(int bit = 0; bit < 8; ++bit) 51 | { 52 | if(sum[i*8 + bit] > s) 53 | mean[i] |= (0x01 << bit); 54 | } 55 | } 56 | } 57 | 58 | /* ************************************************************************* */ 59 | double FBrisk::distance(const FBrisk::TDescriptor &a, 60 | const FBrisk::TDescriptor &b) 61 | { 62 | return (double)brisk::Hamming::PopcntofXORed(&a.front(),&b.front(),L/16); 63 | } 64 | 65 | /* ************************************************************************* */ 66 | std::string FBrisk::toString(const FBrisk::TDescriptor &a) 67 | { 68 | std::stringstream ss; 69 | for(int i = 0; i < L; ++i) 70 | ss << int(a[i]) << " "; 71 | return ss.str(); 72 | } 73 | 74 | /* ************************************************************************* */ 75 | void FBrisk::fromString(FBrisk::TDescriptor &a, const std::string &s) 76 | { 77 | a.resize(L); 78 | 79 | std::stringstream ss(s); 80 | for(int i = 0; i < L; ++i) 81 | { 82 | int v; 83 | ss >> v; 84 | a[i] = v; 85 | } 86 | } 87 | 88 | /* ************************************************************************* */ 89 | void FBrisk::toMat32F(const std::vector &descriptors, 90 | cv::Mat &mat) 91 | { 92 | if(descriptors.empty()) 93 | { 94 | mat.release(); 95 | return; 96 | } 97 | 98 | const int N = descriptors.size(); 99 | mat.create(N, L*8, CV_32F); 100 | for(int i = 0; i < N; ++i) 101 | { 102 | const TDescriptor& desc = descriptors[i]; 103 | float *p = mat.ptr(i); 104 | for(int j = 0; j < L*8; j += 8, p += 8) 105 | { 106 | for(int bit = 0; bit < 8; ++bit) 107 | *(p + bit) = (desc[j] & (0x01 << bit)); 108 | } 109 | } 110 | } 111 | 112 | } // namespace DBoW2 113 | 114 | 115 | -------------------------------------------------------------------------------- /sources/core/system/fbrisk.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_FBRISK_H_ 19 | #define DF_FBRISK_H_ 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace DBoW2 { 30 | 31 | /** 32 | * @brief Interface for Brisk features to work with DBoW2. 33 | * Inherits from FClass and overrides the required methods. 34 | */ 35 | struct FBrisk : protected FClass 36 | { 37 | typedef std::vector TDescriptor; 38 | typedef const TDescriptor *pDescriptor; 39 | static const int L = 48; 40 | 41 | static void meanValue(const std::vector &descriptors, 42 | TDescriptor &mean); 43 | 44 | static double distance(const TDescriptor &a, 45 | const TDescriptor &b); 46 | 47 | static std::string toString(const TDescriptor &a); 48 | 49 | static void fromString(TDescriptor& a, const std::string& s); 50 | 51 | static void toMat32F(const std::vector& descriptors, 52 | cv::Mat& mat); 53 | }; 54 | 55 | } // namespace DBoW2 56 | 57 | #endif // DF_FBRISK_H_ 58 | -------------------------------------------------------------------------------- /sources/core/system/loop_detector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_LOOP_DETECTOR_H_ 19 | #define DF_LOOP_DETECTOR_H_ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "fbrisk.h" 30 | #include "keyframe.h" 31 | #include "keyframe_map.h" 32 | #include "feature_detection.h" 33 | #include "camera_tracker.h" 34 | #include "camera_pyramid.h" 35 | 36 | namespace df 37 | { 38 | 39 | typedef DBoW2::TemplatedVocabulary 40 | BriskVocabulary; 41 | typedef DBoW2::TemplatedDatabase 42 | BriskDatabase; 43 | 44 | struct LoopDetectorConfig 45 | { 46 | CameraTracker::TrackerConfig tracker_cfg; 47 | std::vector iters; 48 | float min_similarity = 0.35f; 49 | float max_error = 0.5f; 50 | float max_dist = 0.1f; 51 | int max_candidates = 3; 52 | int active_window = 5; 53 | }; 54 | 55 | template 56 | class LoopDetector 57 | { 58 | public: 59 | typedef Keyframe KeyframeT; 60 | typedef typename KeyframeT::Ptr KeyframePtr; 61 | typedef typename Map::Ptr MapPtr; 62 | typedef std::shared_ptr TrackerPtr; 63 | typedef CameraTracker::TrackerConfig TrackerConfig; 64 | typedef CameraPyramid CameraPyr; 65 | typedef typename KeyframeT::IdType IdType; 66 | typedef std::map DescriptionMap; 67 | typedef Eigen::Matrix ImageGrad; 68 | typedef vc::RuntimeBufferPyramidManaged ImagePyramid; 69 | typedef vc::RuntimeBufferPyramidManaged GradPyramid; 70 | typedef Sophus::SE3 SE3T; 71 | 72 | struct LoopInfo 73 | { 74 | int loop_id = 1; 75 | SE3T pose_wc; 76 | bool detected = false; 77 | }; 78 | 79 | LoopDetector(std::string voc_path, MapPtr map, LoopDetectorConfig cfg, CameraPyr cam); 80 | 81 | void AddKeyframe(KeyframePtr kf); 82 | int DetectLoop(KeyframePtr kf); 83 | LoopInfo DetectLoop(const ImagePyramid& pyr_img, const GradPyramid& pyr_grad, 84 | const Features& ft, KeyframePtr curr_kf, SE3T pose_cam); 85 | int DetectLocalLoop(const ImagePyramid& pyr_img, const GradPyramid& pyr_grad, 86 | const Features& ft, KeyframePtr curr_kf, SE3T pose_cam); 87 | 88 | void Reset(); 89 | 90 | private: 91 | std::vector> ChangeStructure(cv::Mat mat); 92 | 93 | DescriptionMap dmap_; 94 | BriskVocabulary voc_; 95 | BriskDatabase db_; 96 | MapPtr map_; 97 | TrackerPtr tracker_; 98 | LoopDetectorConfig cfg_; 99 | }; 100 | 101 | } // namespace df 102 | 103 | #endif // DF_LOOP_DETECTOR_H_ 104 | -------------------------------------------------------------------------------- /sources/cuda/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ------------------------------------------------------------------------------ 2 | # Set NVCC flags 3 | # ------------------------------------------------------------------------------ 4 | set(DF_CUDA_ARCH Auto CACHE STRING "A list of CUDA architectures to compile for. Specifying 'Auto' will attempt to autodetect available GPU devices") 5 | CUDA_SELECT_NVCC_ARCH_FLAGS(CUDA_NVCC_ARCH_FLAGS ${DF_CUDA_ARCH}) 6 | set(CMAKE_CUDA_FLAGS ${CUDA_NVCC_ARCH_FLAGS};--expt-relaxed-constexpr;--expt-extended-lambda;--use_fast_math) 7 | message("Compiling for CUDA architectures: ${CUDA_NVCC_ARCH_FLAGS}") 8 | 9 | # suppress NVCC warning triggered in Sophus: 10 | # "__device__ annotation is ignored on a function that is explicitly defaulted on its first declaration" 11 | # see: https://github.com/kokkos/kokkos/issues/1473 12 | list(APPEND CMAKE_CUDA_FLAGS -Xcudafe;--diag_suppress=esa_on_defaulted_function_ignored) 13 | 14 | # append debug flags 15 | if(CMAKE_BUILD_TYPE MATCHES Debug) 16 | #list(APPEND CMAKE_CUDA_FLAGS --device-debug;--debug;-Xcompiler;-rdynamic;)#--ptxas-options=-v) 17 | elseif(CMAKE_BUILD_TYPE MATCHES RelWithDebInfo) 18 | list(APPEND CMAKE_CUDA_FLAGS -g;-lineinfo) 19 | endif() 20 | string(REPLACE ";" " " _TMP_STR "${CMAKE_CUDA_FLAGS}") 21 | set(CMAKE_CUDA_FLAGS "${_TMP_STR}") 22 | 23 | # ------------------------------------------------------------------------------ 24 | # List sources 25 | # ------------------------------------------------------------------------------ 26 | set(cuda_sources 27 | cu_sfmaligner.cpp 28 | cu_image_proc.cpp 29 | cuda_context.cpp 30 | cu_se3aligner.cpp 31 | cu_depthaligner.cpp 32 | ) 33 | set(cuda_headers 34 | kernel_utils.h 35 | cuda_context.h 36 | synced_pyramid.h 37 | cu_sfmaligner.h 38 | launch_utils.h 39 | cu_se3aligner.h 40 | cu_depthaligner.h 41 | device_info.h 42 | cu_image_proc.h 43 | reduction_items.h 44 | ) 45 | 46 | set_source_files_properties(${cuda_sources} PROPERTIES LANGUAGE CUDA) 47 | 48 | # ------------------------------------------------------------------------------ 49 | # Build target 50 | # ------------------------------------------------------------------------------ 51 | add_library(df_cuda SHARED ${cuda_sources} ${cuda_headers}) 52 | target_include_directories(df_cuda PUBLIC 53 | ${CMAKE_CURRENT_SOURCE_DIR} 54 | ) 55 | target_link_libraries(df_cuda PUBLIC 56 | df_common 57 | Sophus::Sophus 58 | VisionCore 59 | Eigen3::Eigen 60 | cuda 61 | glog 62 | ) 63 | 64 | #target_compile_features(df_cuda PUBLIC cxx_std_11) 65 | set_property(TARGET df_cuda PROPERTY CUDA_STANDARD 11) 66 | set_target_properties(df_cuda PROPERTIES POSITION_INDEPENDENT_CODE ON) 67 | set_target_properties(df_cuda PROPERTIES CUDA_SEPARABLE_COMPILATION ON) 68 | set_target_properties(df_cuda PROPERTIES RELOCATABLE_DEVICE_CODE ON) 69 | -------------------------------------------------------------------------------- /sources/cuda/cu_depthaligner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_CU_DEPTHALIGNER_H_ 19 | #define DF_CU_DEPTHALIGNER_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "reduction_items.h" 26 | 27 | namespace df 28 | { 29 | 30 | // forward declarations 31 | template 32 | class PinholeCamera; 33 | 34 | template 35 | class DepthAligner 36 | { 37 | public: 38 | typedef std::shared_ptr> Ptr; 39 | typedef Eigen::Matrix CodeT; 40 | typedef vc::Image2DView ImageBuffer; 41 | typedef JTJJrReductionItem ReductionItem; 42 | 43 | DepthAligner(); 44 | virtual ~DepthAligner(); 45 | 46 | ReductionItem RunStep(const CodeT& code, 47 | const ImageBuffer& target_dpt, 48 | const ImageBuffer& prx_orig, 49 | const ImageBuffer& prx_jac); 50 | 51 | private: 52 | vc::Buffer1DManaged bscratch_; 53 | }; 54 | 55 | } // namespace df 56 | 57 | #endif // DF_CU_DEPTHALIGNER_H_ 58 | -------------------------------------------------------------------------------- /sources/cuda/cu_image_proc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_CU_IMAGE_PROC_H_ 19 | #define DF_CU_IMAGE_PROC_H_ 20 | 21 | #include 22 | #include 23 | 24 | namespace df 25 | { 26 | 27 | template 28 | void GaussianBlurDown(const vc::Buffer2DView& in, 29 | vc::Buffer2DView& out); 30 | 31 | template 32 | void SobelGradients(const vc::Buffer2DView& img, 33 | vc::Buffer2DView,vc::TargetDeviceCUDA>& grad); 34 | 35 | template 36 | T SquaredError(const vc::Buffer2DView& buf1, 37 | const vc::Buffer2DView& buf2); 38 | 39 | template > 40 | void UpdateDepth(const Eigen::Matrix& code, 41 | const ImageBuf& prx_orig, 42 | const ImageBuf& prx_jac, 43 | T avg_dpt, 44 | ImageBuf& dpt_out); 45 | 46 | } // namespace df 47 | 48 | #endif // DF_CU_IMAGE_PROC_H_ 49 | -------------------------------------------------------------------------------- /sources/cuda/cu_se3aligner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_CU_SE3ALIGNER_H_ 19 | #define DF_CU_SE3ALIGNER_H_ 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "reduction_items.h" 30 | 31 | namespace df 32 | { 33 | 34 | // forward declarations 35 | template 36 | class PinholeCamera; 37 | 38 | template 39 | class SE3Aligner 40 | { 41 | public: 42 | typedef std::shared_ptr> Ptr; 43 | typedef Sophus::SE3 UpdateType; 44 | typedef Eigen::Matrix ImageGrad; 45 | typedef JTJJrReductionItem ReductionItem; 46 | typedef CorrespondenceReductionItem CorrespondenceItem; 47 | typedef vc::Image2DView ImageBuffer; 48 | typedef vc::Image2DView GradBuffer; 49 | typedef vc::Buffer1DManaged CorrespondenceReductionBuffer; 50 | typedef vc::Buffer1DManaged StepReductionBuffer; 51 | 52 | SE3Aligner(); 53 | virtual ~SE3Aligner(); 54 | 55 | /** 56 | * Renders img0 from image img1 at pose T_01 57 | */ 58 | CorrespondenceItem Warp(const Sophus::SE3& se3, 59 | const df::PinholeCamera& cam, 60 | const ImageBuffer& img0, 61 | const ImageBuffer& img1, 62 | const ImageBuffer& dpt0, 63 | ImageBuffer& img2); // img1 warped into img0 64 | 65 | ReductionItem RunStep(const Sophus::SE3& se3, 66 | const df::PinholeCamera& cam, 67 | const ImageBuffer& img0, 68 | const ImageBuffer& img1, 69 | const ImageBuffer& dpt0, 70 | const GradBuffer& grad1); 71 | 72 | void SetHuberDelta(float val) { huber_delta_ = val; } 73 | 74 | private: 75 | /* 76 | * Maximum number of blocks we launch the kernel with. 77 | * This should be max 1024 as we can finalize the reduction 78 | * in a single block of 1024 threads (max number of threads per block) 79 | */ 80 | int max_blocks = 1024; 81 | 82 | // buffers for per-block reduced items 83 | CorrespondenceReductionBuffer bscratch1_; 84 | StepReductionBuffer bscratch2_; 85 | float huber_delta_ = 0.1f; 86 | }; 87 | 88 | } // namespace df 89 | 90 | #endif // DF_CU_SE3ALIGNER_H_ 91 | -------------------------------------------------------------------------------- /sources/cuda/cu_sfmaligner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_CU_SFMALIGNER_H_ 19 | #define DF_CU_SFMALIGNER_H_ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "reduction_items.h" 31 | #include "device_info.h" 32 | #include "dense_sfm.h" 33 | 34 | namespace df 35 | { 36 | 37 | // forward declarations 38 | template 39 | class PinholeCamera; 40 | 41 | struct SfmAlignerParams 42 | { 43 | DenseSfmParams sfmparams; 44 | int step_threads = 32; // NOTE: threads must be a multiple of 32! 45 | int step_blocks = 11; 46 | int eval_threads = 224; 47 | int eval_blocks = 66; 48 | }; 49 | 50 | template 51 | class SfmAligner 52 | { 53 | public: 54 | typedef std::shared_ptr> Ptr; 55 | typedef Eigen::Matrix CodeT; 56 | typedef Eigen::Matrix ImageGrad; 57 | typedef Sophus::SE3 SE3T; 58 | typedef vc::Image2DView ImageBuffer; 59 | typedef vc::Image2DView GradBuffer; 60 | typedef JTJJrReductionItem ReductionItem; 61 | typedef CorrespondenceReductionItem ErrorReductionItem; 62 | typedef Eigen::Matrix RelposeJac; 63 | 64 | SfmAligner(SfmAlignerParams params = SfmAlignerParams()); 65 | virtual ~SfmAligner(); 66 | 67 | ErrorReductionItem EvaluateError(const Sophus::SE3& pose0, 68 | const Sophus::SE3& pose1, 69 | const df::PinholeCamera& cam, 70 | const ImageBuffer& img0, 71 | const ImageBuffer& img1, 72 | const ImageBuffer& dpt0, 73 | const ImageBuffer& std0, 74 | const GradBuffer& grad1); 75 | 76 | ReductionItem RunStep(const SE3T& pose0, 77 | const SE3T& pose1, 78 | const CodeT& code0, 79 | const df::PinholeCamera& cam, 80 | const ImageBuffer& img0, 81 | const ImageBuffer& img1, 82 | const ImageBuffer& dpt0, 83 | const ImageBuffer& std0, 84 | ImageBuffer& valid0, 85 | const ImageBuffer& prx0_jac, 86 | const GradBuffer& grad1); 87 | 88 | void SetEvalThreadsBlocks(int threads, int blocks); 89 | void SetStepThreadsBlocks(int threads, int blocks); 90 | 91 | private: 92 | static const int max_blocks = 1024; 93 | SfmAlignerParams params_; 94 | cuda::DeviceInfo devinfo_; 95 | vc::Buffer1DManaged bscratch_; 96 | vc::Buffer1DManaged bscratch2_; 97 | }; 98 | 99 | } // namespace df 100 | 101 | #endif // DF_CU_SFMALIGNER_H_ 102 | -------------------------------------------------------------------------------- /sources/cuda/cuda_context.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef CUDA_CONTEXT_H_ 19 | #define CUDA_CONTEXT_H_ 20 | 21 | #include // driver api 22 | #include "device_info.h" 23 | 24 | namespace cuda 25 | { 26 | 27 | DeviceInfo Init(uint gpuId = 0); 28 | 29 | DeviceInfo GetDeviceInfo(uint gpuId); 30 | DeviceInfo GetCurrentDeviceInfo(); 31 | 32 | CUcontext CreateRuntimeContext(std::size_t device_id); 33 | 34 | CUcontext CreateAndBindContext(std::size_t device_id); 35 | 36 | CUcontext GetCurrentContext(); 37 | 38 | void SetCurrentContext(const CUcontext& ctx); 39 | 40 | void PushContext(const CUcontext& ctx); 41 | 42 | CUcontext PopContext(); 43 | 44 | class ScopedContextPop 45 | { 46 | public: 47 | ScopedContextPop() 48 | { 49 | ctx_ = PopContext(); 50 | } 51 | 52 | ~ScopedContextPop() 53 | { 54 | PushContext(ctx_); 55 | } 56 | 57 | private: 58 | CUcontext ctx_; 59 | }; 60 | 61 | 62 | } // namespace df 63 | 64 | #endif // CUDA_CONTEXT_H_ 65 | 66 | -------------------------------------------------------------------------------- /sources/cuda/device_info.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_DEVICE_INFO_H_ 19 | #define DF_DEVICE_INFO_H_ 20 | 21 | namespace cuda 22 | { 23 | 24 | struct DeviceInfo 25 | { 26 | std::string Name; 27 | std::size_t TotalGlobalMem; 28 | std::size_t FreeGlobalMem; 29 | std::size_t SharedMemPerBlock; 30 | int RegistersPerBlock; 31 | int WarpSize; 32 | std::size_t MemPitch; 33 | int MaxThreadsPerBlock; 34 | int MaxThreadsDim[3]; 35 | int MaxGridSize[3]; 36 | std::size_t TotalConstMem; 37 | int Major; 38 | int Minor; 39 | int ClockRate; 40 | std::size_t TextureAlignment; 41 | int DeviceOverlap; 42 | int MultiProcessorCount; 43 | int KernelExecTimeoutEnabled; 44 | int Integrated; 45 | int CanMapHostMemory; 46 | int ComputeMode; 47 | int ConcurrentKernels; 48 | int ECCEnabled; 49 | int PCIBusID; 50 | int PCIDeviceID; 51 | int MemoryClockRate; 52 | int MemoryBusWidth; 53 | int MaxThreadsPerMultiprocessor; 54 | }; 55 | 56 | } // namespace cuda 57 | 58 | #endif // DF_DEVICE_INFO_H_ 59 | -------------------------------------------------------------------------------- /sources/cuda/kernel_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_KERNEL_UTILS_H_ 19 | #define DF_KERNEL_UTILS_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace df 27 | { 28 | 29 | #ifdef __CUDACC__ 30 | 31 | /* 32 | * Traits needed to use kernel_finalize_reduction 33 | */ 34 | template 35 | struct reduction_traits 36 | { 37 | static inline EIGEN_PURE_DEVICE_FUNC void WarpReduceSum(T& item) 38 | { 39 | for(unsigned int offset = warpSize / 2; offset > 0; offset /= 2) 40 | { 41 | item += vc::shfl_down(item, offset); 42 | } 43 | } 44 | 45 | static inline EIGEN_DEVICE_FUNC T zero() 46 | { 47 | return T(0); 48 | } 49 | }; 50 | 51 | template 52 | __global__ void kernel_finalize_reduction(const vc::Buffer1DView in, 53 | vc::Buffer1DView out, 54 | const int nblocks) 55 | { 56 | T sum; 57 | 58 | // this uses grid-stride loop running your lambda 59 | // each thread sums up multiples of data 60 | vc::runReductions(nblocks, [&] __device__ (unsigned int i) 61 | { 62 | sum += in[i]; 63 | }); 64 | 65 | // reduce blocks and store to memory 66 | // we assume here this kernel is always run with one block 67 | // so this will be the final sum 68 | vc::finalizeReduction(out.ptr(), &sum, &reduction_traits::WarpReduceSum, reduction_traits::zero()); 69 | } 70 | 71 | #endif 72 | 73 | } //namespace df 74 | 75 | 76 | template 77 | EIGEN_DEVICE_FUNC void print_mat(const Eigen::MatrixBase& mat) 78 | { 79 | for (int i = 0; i < Derived::RowsAtCompileTime; ++i) 80 | { 81 | for (int j = 0; j < Derived::ColsAtCompileTime; ++j) 82 | printf("%f ", mat(i,j)); 83 | printf("\n"); 84 | } 85 | } 86 | 87 | #endif // DF_KERNEL_UTILS_H_ 88 | -------------------------------------------------------------------------------- /sources/cuda/launch_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_LAUNCH_UTILS_H_ 19 | #define DF_LAUNCH_UTILS_H_ 20 | 21 | #include 22 | 23 | namespace df 24 | { 25 | 26 | inline void CudaCheckLastError(const std::string& msg) 27 | { 28 | cudaDeviceSynchronize(); 29 | cudaError_t err = cudaPeekAtLastError(); 30 | if (cudaSuccess != err) 31 | throw vc::CUDAException(err, msg); 32 | } 33 | 34 | } // namespace df 35 | 36 | #endif // DF_LAUNCH_UTILS_H_ 37 | -------------------------------------------------------------------------------- /sources/demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ------------------------------------------------------------------------------ 2 | # List sources 3 | # ------------------------------------------------------------------------------ 4 | set(demo_sources 5 | main.cpp 6 | live_demo.cpp 7 | ) 8 | set(demo_headers 9 | live_demo.h 10 | ) 11 | 12 | # ------------------------------------------------------------------------------ 13 | # Build target 14 | # ------------------------------------------------------------------------------ 15 | add_executable(df_demo ${demo_sources} ${demo_headers}) 16 | target_include_directories(df_demo PUBLIC 17 | ${CMAKE_CURRENT_SOURCE_DIR} 18 | ) 19 | target_link_libraries(df_demo PUBLIC 20 | df_core 21 | df_gui 22 | df_drivers 23 | df_common 24 | opencv_core 25 | opencv_highgui 26 | boost_filesystem 27 | gflags 28 | glog 29 | ) 30 | -------------------------------------------------------------------------------- /sources/demo/live_demo.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_LIVE_DEMO_H_ 19 | #define DF_LIVE_DEMO_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "deepfactors.h" 26 | #include "visualizer.h" // for enum GuiEvent 27 | 28 | namespace df 29 | { 30 | 31 | // forward declarations 32 | class Visualizer; 33 | enum GuiEvent; 34 | namespace drivers { class CameraInterface; } 35 | 36 | struct LiveDemoOptions 37 | { 38 | using VisMode = VisualizerConfig::VisualizeMode; 39 | enum InitType {ONEFRAME, TWOFRAME}; 40 | 41 | VisMode vis_mode = VisMode::ALL; 42 | InitType init_type = TWOFRAME; 43 | df::DeepFactorsOptions df_opts; 44 | std::string source_url; 45 | std::string calib_path; 46 | bool record_input; 47 | std::string record_path; 48 | bool init_on_start; 49 | bool pause_step; 50 | std::string log_dir; 51 | std::string res_dir_name; 52 | bool enable_timing; 53 | bool quit_on_finish; 54 | int frame_limit; 55 | int skip_frames; 56 | bool demo_mode; 57 | 58 | static InitType InitTypeTranslator(const std::string& s); 59 | }; 60 | 61 | /* 62 | Class representing the live demo program 63 | Handles system things: 64 | Starting/stopping threads 65 | Fetching new frame from camera 66 | Feeding the slam system 67 | */ 68 | template 69 | class LiveDemo 70 | { 71 | public: 72 | enum DemoState { RUNNING, PAUSED, INIT }; 73 | 74 | typedef df::DeepFactors SlamT; 75 | 76 | LiveDemo(LiveDemoOptions opts); 77 | ~LiveDemo(); 78 | 79 | void Run(); 80 | 81 | private: 82 | void Init(); 83 | void ProcessingLoop(); 84 | void VisualizerLoop(); 85 | void StatsCallback(const DeepFactorsStatistics &stats); 86 | void HandleGuiEvent(GuiEvent evt); 87 | 88 | void StartThreads(); 89 | void JoinThreads(); 90 | 91 | void CreateLogDirs(); 92 | void SavePostCrashInfo(bool crashed=false); 93 | 94 | 95 | df::LiveDemoOptions opts_; 96 | std::unique_ptr slam_system_; 97 | std::unique_ptr visualizer_; 98 | std::unique_ptr caminterface_; 99 | 100 | double live_timestamp_; 101 | double saved_timestamp_; 102 | cv::Mat live_frame_; 103 | cv::Mat saved_frame_; 104 | std::atomic state_; 105 | bool quit_ = false; 106 | 107 | std::thread vis_thread_; 108 | std::mutex slam_mutex_; 109 | df::PinholeCamera cam_; // connected camera intrinsics 110 | df::PinholeCamera netcam_; // network camera intrinsics 111 | 112 | // directories that demo is logging to 113 | std::string dir_input; 114 | std::string dir_crash; 115 | }; 116 | 117 | } // namespace df 118 | 119 | #endif // DF_LIVE_DEMO_H_ 120 | 121 | -------------------------------------------------------------------------------- /sources/drivers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ------------------------------------------------------------------------------ 2 | # Dependencies 3 | # For the downloaded deps, we rely here on XXX_DIR cache variables 4 | # that were set by thirdparty/CMakeLists.txt file parsed earlier 5 | # ------------------------------------------------------------------------------ 6 | find_package(CameraDrivers REQUIRED) 7 | 8 | # ------------------------------------------------------------------------------ 9 | # List sources 10 | # ------------------------------------------------------------------------------ 11 | set(drivers_sources 12 | # camera 13 | camera/openni_interface.cpp 14 | # dataset 15 | dataset/file_interface.cpp 16 | dataset/icl_interface.cpp 17 | dataset/scannet_interface.cpp 18 | dataset/tum_interface.cpp 19 | # top-level 20 | camera_interface_factory.cpp 21 | ) 22 | set(drivers_headers 23 | # camera 24 | camera/live_interface.h 25 | camera/openni_interface.h 26 | # dataset 27 | dataset/dataset_interface.h 28 | dataset/file_interface.h 29 | dataset/icl_interface.h 30 | dataset/scannet_interface.h 31 | dataset/tum_interface.h 32 | # top-level 33 | camera_interface.h 34 | camera_interface_factory.h 35 | ) 36 | 37 | if (DF_WITH_FLYCAP) 38 | list(APPEND drivers_sources camera/pointgrey_interface.cpp) 39 | list(APPEND drivers_headers camera/pointgrey_interface.h) 40 | endif() 41 | 42 | # ------------------------------------------------------------------------------ 43 | # Build target 44 | # ------------------------------------------------------------------------------ 45 | add_library(df_drivers SHARED ${drivers_sources} ${drivers_headers}) 46 | target_include_directories(df_drivers PUBLIC 47 | ${CMAKE_CURRENT_SOURCE_DIR}/camera 48 | ${CMAKE_CURRENT_SOURCE_DIR}/dataset 49 | ${CMAKE_CURRENT_SOURCE_DIR} 50 | ) 51 | target_link_libraries(df_drivers PUBLIC 52 | df_common 53 | CameraDrivers 54 | boost_filesystem 55 | glog 56 | ) 57 | -------------------------------------------------------------------------------- /sources/drivers/camera/live_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_LIVE_INTERFACE_H_ 19 | #define DF_LIVE_INTERFACE_H_ 20 | 21 | #include "camera_interface.h" 22 | #include "pinhole_camera.h" 23 | 24 | // forward declarations 25 | namespace cv { class Mat; } 26 | 27 | namespace df 28 | { 29 | namespace drivers 30 | { 31 | 32 | /** 33 | * Interface class for all cameras 34 | * that will work with this system 35 | */ 36 | class LiveInterface : public CameraInterface 37 | { 38 | public: 39 | LiveInterface() {} 40 | virtual ~LiveInterface() {} 41 | 42 | virtual bool HasMore() override { return true; } 43 | 44 | virtual void SetGain(float gain) { throw UnsupportedFeatureException("Gain not supported by this camera"); } 45 | virtual void SetGainAuto(bool on) { throw UnsupportedFeatureException("Gain not supported by this camera"); } 46 | virtual float GetGain() { throw UnsupportedFeatureException("Gain not supported by this camera"); } 47 | virtual void SetShutter(float shutter) { throw UnsupportedFeatureException("Shutter not supported by this camera"); } 48 | virtual void SetShutterAuto(bool on) { throw UnsupportedFeatureException("Shutter not supported by this camera"); } 49 | virtual float GetShutter() { throw UnsupportedFeatureException("Shutter not supported by this camera"); } 50 | }; 51 | 52 | } // namespace drivers 53 | } // namespace df 54 | 55 | #endif // DF_LIVE_INTERFACE_H_ 56 | 57 | -------------------------------------------------------------------------------- /sources/drivers/camera/openni_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_OPENNI_INTERFACE_H_ 19 | #define DF_OPENNI_INTERFACE_H_ 20 | 21 | #include 22 | #include "live_interface.h" 23 | #include "camera_interface_factory.h" 24 | 25 | namespace drivers { namespace camera { class OpenNI; } } 26 | 27 | namespace df 28 | { 29 | namespace drivers 30 | { 31 | 32 | class OpenNIInterface : public LiveInterface 33 | { 34 | public: 35 | OpenNIInterface(std::size_t camera_id); 36 | virtual ~OpenNIInterface(); 37 | 38 | virtual void SetGain(float gain) override; 39 | virtual void SetGainAuto(bool on) override; 40 | virtual float GetGain() override; 41 | virtual void SetShutter(float shutter) override; 42 | virtual void SetShutterAuto(bool on) override; 43 | virtual float GetShutter() override; 44 | 45 | virtual void GrabFrames(double& timestamp, cv::Mat* img, cv::Mat* dpt = nullptr) override; 46 | 47 | virtual bool SupportsDepth() override { return true; } 48 | virtual bool HasIntrinsics() override { return true; } 49 | 50 | virtual df::PinholeCamera GetIntrinsics() override; 51 | 52 | private: 53 | std::unique_ptr<::drivers::camera::OpenNI> driver; 54 | }; 55 | 56 | class OpenNIInterfaceFactory : public SpecificInterfaceFactory 57 | { 58 | public: 59 | virtual std::unique_ptr FromUrlParams(const std::string& url_params) override; 60 | virtual std::string GetUrlPattern(const std::string& prefix_tag) override; 61 | virtual std::string GetPrefix() override; 62 | 63 | private: 64 | const std::string url_prefix = "openni"; 65 | const std::string url_params = "[cameraId]"; 66 | }; 67 | 68 | } // namespace drivers 69 | } // namespace df 70 | 71 | #endif // DF_OPENNI_INTERFACE_H_ 72 | -------------------------------------------------------------------------------- /sources/drivers/camera/pointgrey_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_POINTGREY_INTERFACE_H_ 19 | #define DF_POINTGREY_INTERFACE_H_ 20 | 21 | #include 22 | #include "live_interface.h" 23 | #include "camera_interface_factory.h" 24 | 25 | namespace drivers { namespace camera { class PointGrey; } } 26 | 27 | namespace df 28 | { 29 | namespace drivers 30 | { 31 | 32 | class PointGreyInterface : public LiveInterface 33 | { 34 | public: 35 | PointGreyInterface(uint camera_id); 36 | virtual ~PointGreyInterface() override; 37 | 38 | virtual void SetGain(float gain) override; 39 | virtual void SetGainAuto(bool on) override; 40 | virtual float GetGain() override; 41 | virtual void SetShutter(float shutter) override; 42 | virtual void SetShutterAuto(bool on) override; 43 | virtual float GetShutter() override; 44 | 45 | virtual void GrabFrames(double& timestamp, cv::Mat* img, cv::Mat* dpt = nullptr) override; 46 | 47 | private: 48 | std::unique_ptr<::drivers::camera::PointGrey> driver; 49 | }; 50 | 51 | class PointGreyInterfaceFactory : public SpecificInterfaceFactory 52 | { 53 | public: 54 | virtual std::unique_ptr FromUrlParams(const std::string& url_params) override; 55 | virtual std::string GetUrlPattern(const std::string& prefix_tag) override; 56 | virtual std::string GetPrefix() override; 57 | 58 | private: 59 | const std::string url_prefix = "flycap"; 60 | const std::string url_params = "[cameraId]"; 61 | }; 62 | 63 | } // namespace drivers 64 | } // namespace df 65 | 66 | #endif // DF_POINTGREY_INTERFACE_H_ 67 | -------------------------------------------------------------------------------- /sources/drivers/camera_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_CAMERA_INTERFACE_H_ 19 | #define DF_CAMERA_INTERFACE_H_ 20 | 21 | #include "pinhole_camera.h" 22 | 23 | // forward declarations 24 | namespace cv { class Mat; } 25 | 26 | /** 27 | * Custom exception to detect whether a certain 28 | * feature is not supported by your camera 29 | */ 30 | class UnsupportedFeatureException : public std::runtime_error 31 | { 32 | public: 33 | using std::runtime_error::runtime_error; 34 | }; 35 | 36 | namespace df 37 | { 38 | namespace drivers 39 | { 40 | 41 | /** 42 | * Interface class for all cameras 43 | * that will work with this system 44 | */ 45 | class CameraInterface 46 | { 47 | public: 48 | CameraInterface() {} 49 | virtual ~CameraInterface() {} 50 | 51 | virtual void SetGain(float gain) { throw UnsupportedFeatureException("Gain not supported by this camera"); } 52 | virtual void SetGainAuto(bool on) { throw UnsupportedFeatureException("Gain not supported by this camera"); } 53 | virtual float GetGain() { throw UnsupportedFeatureException("Gain not supported by this camera"); } 54 | virtual void SetShutter(float shutter) { throw UnsupportedFeatureException("Shutter not supported by this camera"); } 55 | virtual void SetShutterAuto(bool on) { throw UnsupportedFeatureException("Shutter not supported by this camera"); } 56 | virtual float GetShutter() { throw UnsupportedFeatureException("Shutter not supported by this camera"); } 57 | 58 | virtual bool SupportsDepth() { return false; } 59 | virtual bool HasIntrinsics() { return false; } 60 | virtual bool HasMore() = 0; 61 | 62 | virtual df::PinholeCamera GetIntrinsics() { return df::PinholeCamera{}; } 63 | 64 | /** 65 | * Grabs new frames from the camera and preprocesses 66 | * them to a standard form 67 | * 68 | * @param timestamp timestamp of the rgb frame 69 | * @param img Output 3-channel color image (uint8) 70 | * @param dpt Optional output depth map, one channel float 71 | */ 72 | virtual void GrabFrames(double& timestamp, cv::Mat* img, cv::Mat* dpt = nullptr) = 0; 73 | }; 74 | 75 | } // namespace drivers 76 | } // namespace df 77 | 78 | #endif // DF_CAMERA_INTERFACE_H_ 79 | 80 | -------------------------------------------------------------------------------- /sources/drivers/camera_interface_factory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include 19 | 20 | #include "camera_interface_factory.h" 21 | #include "camera_interface.h" 22 | 23 | namespace df 24 | { 25 | namespace drivers 26 | { 27 | 28 | // out of line declaration for static members 29 | std::shared_ptr CameraInterfaceFactory::ptr_; 30 | 31 | std::shared_ptr CameraInterfaceFactory::Get() 32 | { 33 | if (!ptr_) 34 | ptr_ = std::make_shared(); 35 | return ptr_; 36 | } 37 | 38 | std::unique_ptr CameraInterfaceFactory::GetInterfaceFromUrl(const std::string& url) 39 | { 40 | // Find the URL type prefix "://" 41 | auto url_parts = PartitionUrl(url); 42 | std::string prefix = url_parts[0]; 43 | std::string remainder = url_parts[1]; 44 | 45 | LOG(INFO) << "prefix: " << url_parts[0] << ", remainder: " << url_parts[1]; 46 | 47 | if (factory_map_.find(prefix) == factory_map_.end()) 48 | { 49 | std::stringstream ss; 50 | ss << "Interface for prefix " << prefix << " not registered" << std::endl; 51 | ss << "Supported interfaces: "; 52 | for (auto& pref : supported_interfaces_) 53 | ss << pref << ", "; 54 | throw MalformedUrlException(url, ss.str()); 55 | } 56 | 57 | return factory_map_[prefix]->FromUrlParams(remainder); 58 | } 59 | 60 | std::string CameraInterfaceFactory::GetUrlHelp() 61 | { 62 | std::stringstream ss; 63 | ss << "Supported URLs:" << std::endl; 64 | for (auto& url_form : url_forms_) 65 | ss << url_form << std::endl; 66 | return ss.str(); 67 | } 68 | 69 | std::vector CameraInterfaceFactory::PartitionUrl(const std::string& url) 70 | { 71 | auto pos = url.find(prefix_tag_); 72 | if (pos > url.length()) 73 | throw MalformedUrlException(url, "Missing tag: " + prefix_tag_); 74 | 75 | std::string prefix = url.substr(0, pos); 76 | std::string remainder = url.substr(pos + prefix_tag_.length()); 77 | return {prefix, remainder}; 78 | } 79 | 80 | } // namespace drivers 81 | } //namespace df 82 | -------------------------------------------------------------------------------- /sources/drivers/camera_interface_factory.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_CAMERA_INTERFACE_FACTORY_H_ 19 | #define DF_CAMERA_INTERFACE_FACTORY_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "camera_interface.h" 27 | 28 | namespace df 29 | { 30 | namespace drivers 31 | { 32 | 33 | /** 34 | * Exception to indicate that user has passed 35 | * an URL that does not match accepted pattern URL_PATTERN 36 | */ 37 | class MalformedUrlException : public std::runtime_error 38 | { 39 | public: 40 | MalformedUrlException(const std::string& pattern, const std::string& reason) 41 | : std::runtime_error("Invalid source URL " + pattern + ": " + reason) {} 42 | }; 43 | 44 | /** 45 | * Base class for a specific interface factory like pointgrey, files, etc 46 | */ 47 | class SpecificInterfaceFactory 48 | { 49 | public: 50 | virtual std::unique_ptr FromUrlParams(const std::string& url_params) = 0; 51 | virtual std::string GetUrlPattern(const std::string& prefix_tag) = 0; 52 | virtual std::string GetPrefix() = 0; 53 | }; 54 | 55 | /** 56 | * Singleton class that registers our supported supported interfaces 57 | * and produces CameraInterfaces based on URL 58 | */ 59 | class CameraInterfaceFactory 60 | { 61 | public: 62 | typedef std::map> FactoryMapT; 63 | 64 | std::unique_ptr GetInterfaceFromUrl(const std::string& url); 65 | 66 | template 67 | void RegisterInterface() 68 | { 69 | auto factory_obj = std::make_shared(); 70 | typename FactoryMapT::value_type pair(factory_obj->GetPrefix(), factory_obj); 71 | factory_map_.insert(pair); 72 | url_forms_.push_back(factory_obj->GetUrlPattern(prefix_tag_)); 73 | supported_interfaces_.push_back(factory_obj->GetPrefix()); 74 | } 75 | 76 | std::string GetUrlHelp(); 77 | 78 | static std::shared_ptr Get(); 79 | 80 | private: 81 | std::vector PartitionUrl(const std::string& url); 82 | 83 | FactoryMapT factory_map_; 84 | std::vector supported_interfaces_; 85 | std::vector url_forms_; 86 | 87 | const std::string prefix_tag_ = "://"; 88 | static std::shared_ptr ptr_; 89 | }; 90 | 91 | /** 92 | * Helper class to register new camera interfaces 93 | * Declare as static variable 94 | */ 95 | template 96 | struct InterfaceRegistrar { 97 | InterfaceRegistrar () { 98 | CameraInterfaceFactory::Get()->RegisterInterface(); 99 | } 100 | }; 101 | 102 | } // namespace drivers 103 | } //namespace df 104 | 105 | #endif // DF_CAMERA_INTERFACE_FACTORY_H_ 106 | -------------------------------------------------------------------------------- /sources/drivers/dataset/dataset_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_DATASET_INTERFACE_H_ 19 | #define DF_DATASET_INTERFACE_H_ 20 | 21 | #include 22 | #include 23 | 24 | #include "camera_interface.h" 25 | 26 | namespace df 27 | { 28 | namespace drivers 29 | { 30 | 31 | struct DatasetFrame 32 | { 33 | double timestamp; 34 | cv::Mat img; 35 | cv::Mat dpt; 36 | Sophus::SE3f pose_wf; 37 | }; 38 | 39 | class DatasetInterface : public CameraInterface 40 | { 41 | public: 42 | DatasetInterface() {} 43 | virtual ~DatasetInterface() {} 44 | 45 | virtual std::vector GetAll() = 0; 46 | virtual std::vector GetPoses() { return std::vector{}; } 47 | virtual bool HasPoses() = 0; 48 | virtual bool HasMore() = 0; 49 | }; 50 | 51 | } // namespace drivers 52 | } // namespace df 53 | 54 | #endif // DF_DATASET_INTERFACE_H_ 55 | 56 | -------------------------------------------------------------------------------- /sources/drivers/dataset/file_interface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include "file_interface.h" 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | namespace df 28 | { 29 | namespace drivers 30 | { 31 | 32 | static InterfaceRegistrar automatic; 33 | 34 | inline std::vector glob(const std::string& pat) { 35 | using namespace std; 36 | glob_t glob_result; 37 | glob(pat.c_str(),GLOB_TILDE,NULL,&glob_result); 38 | vector ret; 39 | for(unsigned int i=0;i> fx >> fy >> u0 >> v0 >> w >> h; 59 | file.close(); 60 | 61 | cam_ = df::PinholeCamera(fx, fy, u0, v0, w, h); 62 | } 63 | 64 | FileInterface::~FileInterface() 65 | { 66 | } 67 | 68 | void FileInterface::GrabFrames(double& timestamp, cv::Mat* img, cv::Mat* dpt) 69 | { 70 | if (dpt != nullptr) 71 | throw UnsupportedFeatureException("Depth not supported by this camera"); 72 | 73 | auto filename = files_[count_]; 74 | timestamp = static_cast(count_); 75 | if (count_ < (int)files_.size()-1) 76 | count_++; 77 | *img = cv::imread(filename, cv::IMREAD_COLOR); 78 | if (img->empty()) 79 | throw std::runtime_error("Failed to load image " + std::to_string(count_-1) + ": " + filename); 80 | } 81 | 82 | bool FileInterface::HasMore() 83 | { 84 | return count_ < (int)files_.size()-2; 85 | } 86 | 87 | //////////////////////////////////////////////////////////////////////////////////////////////////////////// 88 | // Factory class for this interface 89 | //////////////////////////////////////////////////////////////////////////////////////////////////////////// 90 | 91 | std::unique_ptr FileInterfaceFactory::FromUrlParams(const std::string& url_params) 92 | { 93 | // easy enough: we expect url_params to be a file glob so pass it directly to the class 94 | return std::make_unique(url_params); 95 | } 96 | 97 | std::string FileInterfaceFactory::GetUrlPattern(const std::string& prefix_tag) 98 | { 99 | return url_prefix + prefix_tag + url_params; 100 | } 101 | 102 | std::string FileInterfaceFactory::GetPrefix() 103 | { 104 | return url_prefix; 105 | } 106 | 107 | } // namespace drivers 108 | } // namespace df 109 | -------------------------------------------------------------------------------- /sources/drivers/dataset/file_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_FILE_INTERFACE_H_ 19 | #define DF_FILE_INTERFACE_H_ 20 | 21 | #include 22 | #include 23 | #include "dataset_interface.h" 24 | #include "camera_interface_factory.h" 25 | 26 | namespace drivers { namespace camera { class PointGrey; } } 27 | 28 | namespace df 29 | { 30 | namespace drivers 31 | { 32 | 33 | class FileInterface : public DatasetInterface 34 | { 35 | public: 36 | FileInterface(const std::string& glob_path); 37 | virtual ~FileInterface(); 38 | 39 | virtual void GrabFrames(double& timestamp, cv::Mat* img, cv::Mat* dpt = nullptr) override; 40 | virtual bool HasMore() override; 41 | virtual bool HasPoses() override { return false; } 42 | virtual bool HasIntrinsics() override { return true; } 43 | 44 | virtual df::PinholeCamera GetIntrinsics() override { return cam_; } 45 | virtual std::vector GetAll() override { return std::vector{}; } // unimplemented 46 | 47 | private: 48 | int count_; 49 | std::vector files_; 50 | df::PinholeCamera cam_; 51 | }; 52 | 53 | class FileInterfaceFactory : public SpecificInterfaceFactory 54 | { 55 | public: 56 | virtual std::unique_ptr FromUrlParams(const std::string& url_params) override; 57 | virtual std::string GetUrlPattern(const std::string& prefix_tag) override; 58 | virtual std::string GetPrefix() override; 59 | 60 | private: 61 | const std::string url_prefix = "filesrc"; 62 | const std::string url_params = "path_with_glob"; 63 | }; 64 | 65 | 66 | } // namespace drivers 67 | } // namespace df 68 | 69 | #endif // DF_FILE_INTERFACE_H_ 70 | -------------------------------------------------------------------------------- /sources/drivers/dataset/icl_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_ICL_INTERFACE_H 19 | #define DF_ICL_INTERFACE_H 20 | 21 | #include 22 | 23 | #include "tum_io.h" 24 | #include "dataset_interface.h" 25 | #include "camera_interface_factory.h" 26 | 27 | namespace df 28 | { 29 | namespace drivers 30 | { 31 | 32 | struct FrameInfo 33 | { 34 | double img_timestamp; 35 | double dpt_timestamp; 36 | std::string img_path; 37 | std::string dpt_path; 38 | Sophus::SE3f pose_wf; 39 | }; 40 | 41 | class IclInterface : public DatasetInterface 42 | { 43 | public: 44 | IclInterface(std::string data_path); 45 | virtual ~IclInterface(); 46 | 47 | virtual void GrabFrames(double& timestamp, cv::Mat* img, cv::Mat* dpt = nullptr) override; 48 | virtual std::vector GetAll() override; 49 | virtual std::vector GetPoses() override { return poses_; } 50 | 51 | virtual bool SupportsDepth() override { return true; } 52 | virtual bool HasIntrinsics() override { return true; } 53 | virtual bool HasPoses() override { return true; } 54 | virtual bool HasMore() override { return curr_frame_ < (int)frames_.size()-1; } 55 | 56 | virtual df::PinholeCamera GetIntrinsics() override; 57 | 58 | private: 59 | void LoadFrame(int idx, double& timestamp, cv::Mat* img, cv::Mat* dpt); 60 | 61 | std::vector LoadPoses(std::string data_path); 62 | std::vector ParseFrames(std::string data_path); 63 | void AssignPoses(std::vector& frames, const std::vector& rawposes); 64 | 65 | std::vector raw_poses_; 66 | std::vector frames_; 67 | std::vector poses_; // caching these to for GetPoses 68 | std::string data_path_; 69 | int curr_frame_; 70 | }; 71 | 72 | class IclInterfaceFactory : public SpecificInterfaceFactory 73 | { 74 | public: 75 | virtual std::unique_ptr FromUrlParams(const std::string& url_params) override; 76 | virtual std::string GetUrlPattern(const std::string& prefix_tag) override; 77 | virtual std::string GetPrefix() override; 78 | 79 | private: 80 | const std::string url_prefix = "icl"; 81 | const std::string url_params = "sequence_path"; 82 | }; 83 | 84 | } // namespace drivers 85 | } // namespace df 86 | 87 | #endif // DF_ICL_INTERFACE_H 88 | -------------------------------------------------------------------------------- /sources/drivers/dataset/scannet_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_SCANNET_INETRFACE_H_ 19 | #define DF_SCANNET_INETRFACE_H_ 20 | 21 | #include "dataset_interface.h" 22 | #include "camera_interface_factory.h" 23 | 24 | namespace df 25 | { 26 | namespace drivers 27 | { 28 | 29 | class ScanNetInterface : public DatasetInterface 30 | { 31 | public: 32 | ScanNetInterface(std::string seq_path); 33 | virtual ~ScanNetInterface(); 34 | 35 | virtual void GrabFrames(double& timestamp, cv::Mat* img, cv::Mat* dpt = nullptr) override; 36 | virtual std::vector GetAll() override; 37 | 38 | virtual bool SupportsDepth() override { return has_depth_; } 39 | virtual bool HasIntrinsics() override { return true; } 40 | virtual bool HasPoses() override { return true; } 41 | virtual bool HasMore() override { return current_frame_ < (int)img_files_.size()-1; } 42 | virtual std::vector GetPoses() override { return poses_; } 43 | 44 | virtual df::PinholeCamera GetIntrinsics() override; 45 | 46 | private: 47 | void LoadFrame(int i, double& timestamp, cv::Mat* img, cv::Mat* dpt, Sophus::SE3f& pose); 48 | df::PinholeCamera LoadIntrinsics(std::string seq_dir, int width, int height); 49 | std::vector LoadPoses(); 50 | std::size_t ProbeNumberOfFrames(std::string dir); 51 | 52 | std::string seq_path_; 53 | std::vector img_files_; 54 | std::vector dpt_files_; 55 | std::vector pose_files_; 56 | 57 | std::vector poses_; 58 | df::PinholeCamera cam_; 59 | int current_frame_; 60 | bool has_depth_; 61 | }; 62 | 63 | class ScanNetInterfaceFactory : public SpecificInterfaceFactory 64 | { 65 | public: 66 | virtual std::unique_ptr FromUrlParams(const std::string& url_params) override; 67 | virtual std::string GetUrlPattern(const std::string& prefix_tag) override; 68 | virtual std::string GetPrefix() override; 69 | 70 | private: 71 | const std::string url_prefix = "scannet"; 72 | const std::string url_params = "sequence_path"; 73 | }; 74 | 75 | } // namespace drivers 76 | } // namespace df 77 | 78 | #endif // DF_SCANNET_INETRFACE_H_ 79 | -------------------------------------------------------------------------------- /sources/drivers/dataset/tum_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_TUM_INTERFACE_H_ 19 | #define DF_TUM_INTERFACE_H_ 20 | 21 | #include "dataset_interface.h" 22 | #include "camera_interface_factory.h" 23 | #include "tum_io.h" 24 | 25 | namespace df 26 | { 27 | namespace drivers 28 | { 29 | 30 | struct FrameInfo 31 | { 32 | double img_timestamp; 33 | double dpt_timestamp; 34 | std::string img_path; 35 | std::string dpt_path; 36 | Sophus::SE3f pose_wf; 37 | }; 38 | 39 | class TumInterface : public DatasetInterface 40 | { 41 | public: 42 | TumInterface(std::string data_path); 43 | virtual ~TumInterface(); 44 | 45 | virtual void GrabFrames(double& timestamp, cv::Mat* img, cv::Mat* dpt = nullptr) override; 46 | virtual std::vector GetAll() override; 47 | virtual std::vector GetPoses() override { return poses_; } 48 | 49 | virtual bool SupportsDepth() override { return true; } 50 | virtual bool HasIntrinsics() override { return true; } 51 | virtual bool HasPoses() override { return true; } 52 | virtual bool HasMore() override { return curr_frame_ < (int)frames_.size()-1; } 53 | 54 | virtual df::PinholeCamera GetIntrinsics() override; 55 | 56 | private: 57 | void LoadFrame(int idx, double& timestamp, cv::Mat* img, cv::Mat* dpt); 58 | 59 | std::vector LoadPoses(std::string data_path); 60 | std::vector ParseFrames(std::string data_path); 61 | void InterpolatePoses(std::vector& frames, const std::vector& rawposes); 62 | 63 | std::vector raw_poses_; 64 | std::vector frames_; 65 | std::vector poses_; // caching these to for GetPoses 66 | std::string data_path_; 67 | int curr_frame_; 68 | }; 69 | 70 | class TumInterfaceFactory : public SpecificInterfaceFactory 71 | { 72 | public: 73 | virtual std::unique_ptr FromUrlParams(const std::string& url_params) override; 74 | virtual std::string GetUrlPattern(const std::string& prefix_tag) override; 75 | virtual std::string GetPrefix() override; 76 | 77 | private: 78 | const std::string url_prefix = "tum"; 79 | const std::string url_params = "sequence_path"; 80 | }; 81 | 82 | } // namespace drivers 83 | } // namespace df 84 | 85 | #endif // DF_TUM_INTERFACE_H_ 86 | -------------------------------------------------------------------------------- /sources/gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ------------------------------------------------------------------------------ 2 | # List sources 3 | # ------------------------------------------------------------------------------ 4 | set(gui_sources 5 | visualizer.cpp 6 | keyframe_renderer.cpp 7 | ) 8 | set(gui_headers 9 | visualizer.h 10 | keyframe_renderer.h 11 | ) 12 | 13 | # ------------------------------------------------------------------------------ 14 | # Build 15 | # ------------------------------------------------------------------------------ 16 | add_library(df_gui SHARED ${gui_sources} ${gui_headers}) 17 | target_include_directories(df_gui PUBLIC 18 | ${CMAKE_CURRENT_SOURCE_DIR} 19 | ${Pangolin_INCLUDE_DIR} 20 | ) 21 | target_link_libraries(df_gui PUBLIC 22 | df_common 23 | df_core # for keyframe_map definition 24 | gflags 25 | glog 26 | ${Pangolin_LIBRARY} 27 | ) 28 | target_compile_definitions(df_gui PUBLIC 29 | -DDF_SHADER_DIR="${CMAKE_CURRENT_SOURCE_DIR}/shaders") 30 | -------------------------------------------------------------------------------- /sources/gui/keyframe_renderer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include 19 | #include 20 | 21 | #include "keyframe_renderer.h" 22 | 23 | namespace df 24 | { 25 | 26 | void KeyframeRenderer::Init(const df::PinholeCamera& cam) 27 | { 28 | cam_ = cam; 29 | width_ = cam.width(); 30 | height_ = cam.height(); 31 | 32 | // Load and compile shader 33 | std::string shader_dir(DF_SHADER_DIR); 34 | shader_.AddShaderFromFile(pangolin::GlSlVertexShader, shader_dir + "/empty.vert"); 35 | shader_.AddShaderFromFile(pangolin::GlSlGeometryShader, shader_dir + "/drawkf.geom"); 36 | shader_.AddShaderFromFile(pangolin::GlSlFragmentShader, shader_dir + "/phong.frag"); 37 | if (!shader_.Link()) 38 | LOG(FATAL) << "Failed to compile shader"; 39 | 40 | // initialize textures 41 | col_tex_.Reinitialise(width_, height_, GL_RGBA8, true, 0, GL_BGR, GL_UNSIGNED_BYTE); 42 | dpt_tex_.Reinitialise(width_, height_, GL_R32F, true, 0, GL_RED, GL_FLOAT); 43 | val_tex_.Reinitialise(width_, height_, GL_R32F, true, 0, GL_RED, GL_FLOAT); 44 | std_tex_.Reinitialise(width_, height_, GL_R32F, true, 0, GL_RED, GL_FLOAT); 45 | } 46 | 47 | void KeyframeRenderer::RenderKeyframe(const pangolin::OpenGlMatrix& vp, const DisplayData &data) 48 | { 49 | // upload data from keyframe to buffers 50 | // TODO: check if a keyframe is currently on GPU, if yes then use that! 51 | col_tex_.Upload(data.color_img.data, GL_BGR, GL_UNSIGNED_BYTE); 52 | dpt_tex_.Upload(data.dpt.rawPtr(), GL_RED, GL_FLOAT); 53 | val_tex_.Upload(data.vld.rawPtr(), GL_RED, GL_FLOAT); 54 | std_tex_.Upload(data.std.rawPtr(), GL_RED, GL_FLOAT); 55 | 56 | // activate program 57 | shader_.Bind(); 58 | 59 | pangolin::OpenGlMatrix mvp = vp * pangolin::OpenGlMatrix(data.pose_wk.matrix()); 60 | 61 | // fill uniforms 62 | shader_.SetUniform("mvp", mvp); 63 | shader_.SetUniform("cam", cam_.fx(), cam_.fy(), cam_.u0(), cam_.v0()); 64 | shader_.SetUniform("lightpos", light_pos_.x, light_pos_.y, light_pos_.z); 65 | shader_.SetUniform("width", (int) width_); 66 | shader_.SetUniform("height", (int) height_); 67 | shader_.SetUniform("phong_enabled", phong_enabled_); 68 | shader_.SetUniform("draw_noisy_pixels", draw_noisy_pixels_); 69 | shader_.SetUniform("stdev_thresh", stdev_thresh_); 70 | shader_.SetUniform("slt_thresh", slt_thresh_); 71 | shader_.SetUniform("crop_pix", crop_pix_); 72 | 73 | // texture bank ids 74 | shader_.SetUniform("image", 0); 75 | shader_.SetUniform("depth", 1); 76 | shader_.SetUniform("valid", 2); 77 | 78 | // setup texture banks 79 | glActiveTexture(GL_TEXTURE0); 80 | col_tex_.Bind(); 81 | glActiveTexture(GL_TEXTURE1); 82 | dpt_tex_.Bind(); 83 | glActiveTexture(GL_TEXTURE2); 84 | val_tex_.Bind(); 85 | 86 | // draw keyframe 87 | glDrawArrays(GL_POINTS, 0, width_ * height_); 88 | 89 | // TODO: go back to previous active texture 90 | glActiveTexture(GL_TEXTURE0); 91 | 92 | // deactivate shader 93 | shader_.Unbind(); 94 | } 95 | 96 | void KeyframeRenderer::SetPhong(bool enabled) 97 | { 98 | phong_enabled_ = enabled; 99 | } 100 | 101 | void KeyframeRenderer::SetDrawNoisyPixels(bool enabled) 102 | { 103 | draw_noisy_pixels_ = enabled; 104 | } 105 | 106 | void KeyframeRenderer::SetLightPos(float x, float y, float z) 107 | { 108 | light_pos_ = make_float3(x,y,z); 109 | } 110 | 111 | void KeyframeRenderer::SetStdevThresh(float thresh) 112 | { 113 | stdev_thresh_ = thresh; 114 | } 115 | 116 | void KeyframeRenderer::SetSltThresh(float thresh) 117 | { 118 | slt_thresh_ = thresh; 119 | } 120 | 121 | void KeyframeRenderer::SetCropPix(int pix) 122 | { 123 | crop_pix_ = pix; 124 | } 125 | 126 | } // namespace df 127 | 128 | -------------------------------------------------------------------------------- /sources/gui/keyframe_renderer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_KEYFRAME_RENDERER_H_ 19 | #define DF_KEYFRAME_RENDERER_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "pinhole_camera.h" 26 | 27 | namespace df 28 | { 29 | 30 | class KeyframeRenderer 31 | { 32 | public: 33 | struct DisplayData 34 | { 35 | typedef vc::Image2DManaged CpuBuffer; 36 | 37 | DisplayData(int width, int height) 38 | : dpt(width,height), 39 | dpt_orig(width,height), 40 | vld(width,height), 41 | std(width,height) {} 42 | 43 | cv::Mat color_img; 44 | CpuBuffer dpt; 45 | CpuBuffer dpt_orig; 46 | CpuBuffer vld; 47 | CpuBuffer std; 48 | Sophus::SE3f pose_wk; 49 | }; 50 | 51 | void Init(const df::PinholeCamera& cam); 52 | void RenderKeyframe(const pangolin::OpenGlMatrix& vp, const DisplayData& data); 53 | 54 | void SetPhong(bool enabled); 55 | void SetDrawNoisyPixels(bool enabled); 56 | void SetLightPos(float x, float y, float z); 57 | void SetStdevThresh(float thresh); 58 | void SetSltThresh(float thresh); 59 | void SetCropPix(int pix); 60 | 61 | private: 62 | std::size_t width_; 63 | std::size_t height_; 64 | df::PinholeCamera cam_; 65 | 66 | // options 67 | bool phong_enabled_; 68 | bool draw_noisy_pixels_; 69 | float3 light_pos_; 70 | float stdev_thresh_; 71 | float slt_thresh_; 72 | int crop_pix_; 73 | 74 | pangolin::GlSlProgram shader_; 75 | pangolin::GlTexture col_tex_; 76 | pangolin::GlTexture dpt_tex_; 77 | pangolin::GlTexture val_tex_; 78 | pangolin::GlTexture std_tex_; 79 | }; 80 | 81 | } // namespace df 82 | 83 | #endif // DF_KEYFRAME_RENDERER_H_ 84 | -------------------------------------------------------------------------------- /sources/gui/shaders/empty.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | void main() 4 | { 5 | } 6 | -------------------------------------------------------------------------------- /sources/gui/shaders/phong.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | 3 | uniform vec3 lightpos; 4 | uniform bool phong_enabled; 5 | uniform mat4 mvp; 6 | 7 | in fData 8 | { 9 | vec3 color; 10 | vec3 pos; 11 | vec3 normal; 12 | } frag; 13 | 14 | out vec4 out_color; 15 | 16 | // TODO: paramatrize some stuff inside here 17 | vec3 phong_shading(vec3 incolor, vec3 normal) 18 | { 19 | vec3 lightpos_cam = (mvp * vec4(lightpos, 1.0f)).xyz; 20 | 21 | vec3 diffuse = vec3(0.0); 22 | vec3 specular = vec3(0.0); 23 | 24 | // ambient term 25 | vec3 ambient = 0.3 * incolor; 26 | 27 | // diffuse color 28 | vec3 kd = incolor; 29 | 30 | // specular color 31 | vec3 ks = vec3(1.0, 1.0, 1.0); 32 | 33 | // diffuse term 34 | vec3 lightDir = normalize(lightpos_cam - frag.pos); 35 | float NdotL = dot(normal, lightDir); 36 | 37 | if (NdotL > 0.0) 38 | diffuse = kd * NdotL; 39 | 40 | // specular term 41 | vec3 rVector = normalize(2.0 * normal * dot(normal, lightDir) - lightDir); 42 | vec3 viewVector = normalize(-frag.pos); 43 | float RdotV = dot(rVector, viewVector); 44 | 45 | if (RdotV > 0.0) 46 | specular = ks * pow(RdotV, 32) * .05f; 47 | 48 | return ambient + diffuse + specular; 49 | } 50 | 51 | void main() 52 | { 53 | if (phong_enabled) 54 | { 55 | out_color = vec4(phong_shading(frag.color, frag.normal), 1.); 56 | } 57 | else 58 | { 59 | out_color = vec4(frag.color, 1.); 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /sources/tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ------------------------------------------------------------------------------ 2 | # decode_image 3 | # Decode zero code for a given RGB image 4 | # ------------------------------------------------------------------------------ 5 | add_executable(decode_image decode_image.cpp) 6 | target_link_libraries(decode_image PUBLIC 7 | df_core 8 | df_common 9 | opencv_highgui 10 | opencv_imgproc 11 | opencv_core 12 | Eigen3::Eigen 13 | glog 14 | gflags 15 | ) 16 | 17 | # ------------------------------------------------------------------------------ 18 | # kernel_benchmark 19 | # Benchmarking threads/blocks for key CUDA kernels 20 | # ------------------------------------------------------------------------------ 21 | add_executable(kernel_benchmark kernel_benchmark.cpp) 22 | target_include_directories(kernel_benchmark PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) 23 | target_link_libraries(kernel_benchmark PUBLIC 24 | df_cuda 25 | df_core 26 | df_common 27 | opencv_highgui 28 | opencv_core 29 | opencv_imgproc 30 | gflags 31 | ) 32 | 33 | # ------------------------------------------------------------------------------ 34 | # result_viewer 35 | # Display a ground-truth trajectory and reprojected depth from a dataset and 36 | # an estimated trajectory 37 | # ------------------------------------------------------------------------------ 38 | add_executable(result_viewer result_viewer.cpp) 39 | target_include_directories(result_viewer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) 40 | target_link_libraries(result_viewer PUBLIC 41 | df_gui 42 | df_drivers 43 | opencv_core 44 | opencv_highgui 45 | gflags 46 | glog 47 | ) 48 | 49 | # ------------------------------------------------------------------------------ 50 | # voc_builder 51 | # Builds a vocabulary of Brisk features extracted from a set of TUM sequences 52 | # ------------------------------------------------------------------------------ 53 | add_executable(voc_builder voc_builder.cpp) 54 | target_include_directories(voc_builder PUBLIC 55 | ${CMAKE_CURRENT_SOURCE_DIR} 56 | ${BRISK_INCLUDE_DIRS} 57 | ) 58 | target_link_libraries(voc_builder PUBLIC 59 | df_drivers 60 | df_core # for FBrisk.cpp TODO 61 | opencv_core 62 | opencv_highgui 63 | opencv_features2d 64 | brisk 65 | gflags 66 | glog 67 | DBoW2 68 | ) 69 | 70 | # ------------------------------------------------------------------------------ 71 | # voc_test 72 | # Test a BoW vocabulary by calculating similarity between example images 73 | # ------------------------------------------------------------------------------ 74 | add_executable(voc_test voc_test.cpp) 75 | target_include_directories(voc_test PUBLIC 76 | ${CMAKE_CURRENT_SOURCE_DIR} 77 | ${BRISK_INCLUDE_DIRS} 78 | ) 79 | target_link_libraries(voc_test PUBLIC 80 | df_core # for FBrisk.cpp TODO 81 | opencv_core 82 | opencv_highgui 83 | opencv_features2d 84 | brisk 85 | DBoW2 86 | ) 87 | 88 | 89 | -------------------------------------------------------------------------------- /sources/tools/voc_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "fbrisk.h" 25 | 26 | typedef DBoW2::TemplatedVocabulary 27 | FBriskVocabulary; 28 | typedef DBoW2::TemplatedDatabase 29 | FBriskDatabase; 30 | 31 | typedef std::vector> DescriptorVec; 32 | typedef brisk::ScaleSpaceFeatureDetector BriskDetector; 33 | typedef brisk::BriskDescriptorExtractor BriskExtractor; 34 | 35 | DescriptorVec changeStructure(const cv::Mat& mat) 36 | { 37 | // const int L = 48; 38 | 39 | // DescriptorVec out(mat.rows); 40 | 41 | // unsigned int j = 0; 42 | // for(unsigned int i = 0; i < mat.rows*mat.cols; i += L, ++j) 43 | // { 44 | // out[j].resize(L); 45 | // std::copy(mat.data + i, mat.data + i + L, out[j].begin()); 46 | // } 47 | int num_keypoints = mat.rows; 48 | 49 | DescriptorVec out(num_keypoints); 50 | for (int k = 0; k < num_keypoints; ++k) 51 | { 52 | out.at(k).resize(mat.cols); 53 | memcpy(out.at(k).data(), mat.data + mat.cols * k, mat.cols * sizeof(uchar)); 54 | } 55 | 56 | return out; 57 | } 58 | 59 | DescriptorVec LoadExtract(std::string file, BriskDetector& detector, 60 | BriskExtractor& extractor) 61 | { 62 | std::cout << "Loading image from: " << file << std::endl; 63 | cv::Mat img = cv::imread(file, cv::IMREAD_GRAYSCALE); 64 | if (img.empty()) 65 | throw std::runtime_error("Failed to open: " + file); 66 | 67 | cv::Mat descriptors; 68 | std::vector keypoints; 69 | detector.detect(img, keypoints); 70 | extractor.compute(img, keypoints, descriptors); 71 | 72 | cv::drawKeypoints(img, keypoints, img, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS); 73 | cv::imshow(file, img); 74 | 75 | return changeStructure(descriptors); 76 | } 77 | 78 | int main(int argc, char *argv[]) 79 | { 80 | if (argc < 4) 81 | { 82 | std::cout << "usage: " << argv[0] << " voc_path img0 img1 [img2..]" << std::endl; 83 | return -1; 84 | } 85 | int nimg = argc-2; 86 | 87 | // load voc 88 | std::cout << "Loading vocabulary from: " << argv[1] << std::endl; 89 | FBriskVocabulary voc(argv[1]); 90 | 91 | // Brisk detector/extractor 92 | BriskDetector detector(20, 2, 200, 400); 93 | BriskExtractor extractor(true, true); 94 | 95 | // load images and extract feautres 96 | std::vector features(nimg); 97 | std::vector bow_vecs(nimg); 98 | for (int i = 2; i < argc; ++i) 99 | { 100 | features[i-2] = LoadExtract(argv[i], detector, extractor); 101 | voc.transform(features[i-2], bow_vecs[i-2]); 102 | } 103 | 104 | // compute confusion matrix 105 | std::cout << "Confusion matrix: " << std::endl; 106 | for (int i = 0; i < nimg; ++i) 107 | { 108 | for (int j = 0; j < nimg; ++j) 109 | std::cout << std::setw(10) << std::setprecision(3) << voc.score(bow_vecs[i], bow_vecs[j]) << " "; 110 | std::cout << std::endl; 111 | } 112 | 113 | cv::waitKey(0); 114 | return 0; 115 | } 116 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(googletest) 2 | 3 | # ------------------------------------------------------------------------------ 4 | # List sources 5 | # ------------------------------------------------------------------------------ 6 | set(test_sources 7 | main.cpp 8 | ut_decoder.cpp 9 | ut_cuda_utils.cpp 10 | ut_se3aligner.cpp 11 | ut_sfmaligner.cpp 12 | ut_warping.cpp 13 | ut_pinhole_camera.cpp 14 | ut_nearestpsd.cpp 15 | ut_sampling.cpp 16 | ) 17 | 18 | set(test_headers random_machine.h testing_utils.h) 19 | 20 | # ------------------------------------------------------------------------------ 21 | # Build target 22 | # ------------------------------------------------------------------------------ 23 | add_executable(ut_df ${test_sources} ${test_headers}) 24 | target_include_directories(ut_df PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) 25 | target_link_libraries(ut_df PUBLIC 26 | gtest 27 | glog 28 | df_cuda 29 | df_core 30 | Eigen3::Eigen 31 | VisionCore 32 | ) 33 | -------------------------------------------------------------------------------- /tests/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include 19 | #include 20 | #include "cuda_context.h" 21 | 22 | extern "C" int main(int argc, char** argv) 23 | { 24 | ::testing::InitGoogleTest(&argc, argv); 25 | google::InitGoogleLogging(argv[0]); 26 | google::LogToStderr(); 27 | 28 | cuda::Init(); 29 | 30 | int rc = RUN_ALL_TESTS(); 31 | return rc; 32 | } 33 | -------------------------------------------------------------------------------- /tests/random_machine.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_RANDOM_MACHINE_H_ 19 | #define DF_RANDOM_MACHINE_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "pinhole_camera.h" 26 | 27 | namespace df 28 | { 29 | 30 | namespace dist 31 | { 32 | struct normal; 33 | struct uniform; 34 | } 35 | 36 | template 37 | struct random_distribution; 38 | 39 | #define SPECIALIZE_DIST(TYPE, FUNC) \ 40 | template \ 41 | struct random_distribution { \ 42 | typedef std::FUNC Type; \ 43 | }; 44 | 45 | SPECIALIZE_DIST(dist::normal, normal_distribution) 46 | SPECIALIZE_DIST(dist::uniform, uniform_real_distribution) 47 | 48 | template 49 | class RandomMachine 50 | { 51 | public: 52 | typedef typename random_distribution::Type DistT; 53 | 54 | RandomMachine(RandomScalar first, RandomScalar second) 55 | : mt_(rd_()), dist_(first, second) { } 56 | 57 | template 58 | T RandomNumber() 59 | { 60 | return dist_(mt_); 61 | } 62 | 63 | template 64 | Eigen::Matrix RandomMatrix(int rows, int cols) 65 | { 66 | Eigen::Matrix mat(rows, cols); 67 | for (int i = 0; i < rows; ++i) 68 | for (int j = 0; j < cols; ++j) 69 | mat(i,j) = RandomNumber(); 70 | return mat; 71 | } 72 | 73 | template 74 | Eigen::Matrix RandomPoint() 75 | { 76 | return RandomMatrix(3,1); 77 | } 78 | 79 | template 80 | Eigen::Matrix RandomPixel() 81 | { 82 | return RandomMatrix(2,1); 83 | } 84 | 85 | template 86 | Sophus::SE3 RandomPose() 87 | { 88 | return Sophus::SE3::exp(RandomMatrix(6,1)); 89 | } 90 | 91 | template 92 | df::PinholeCamera RandomCamera() 93 | { 94 | int w = (int) RandomNumber(); 95 | int h = (int) RandomNumber(); 96 | T u0 = w / 2.; 97 | T v0 = h / 2.; 98 | T fx = RandomNumber(); 99 | T fy = RandomNumber(); 100 | return df::PinholeCamera(fx,fy,u0,v0,w,h); 101 | } 102 | 103 | private: 104 | std::random_device rd_; 105 | std::mt19937 mt_; 106 | DistT dist_; 107 | }; 108 | 109 | } // namespace df 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /tests/testing_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #ifndef DF_TESTING_UTILS_H_ 19 | #define DF_TESTING_UTILS_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "pinhole_camera.h" 28 | #include "intrinsics.h" 29 | 30 | namespace df 31 | { 32 | 33 | template 34 | df::PinholeCamera GetSceneNetCam(std::size_t w, std::size_t h) 35 | { 36 | // camera information 37 | float fx = w / 2 / 0.5773502691896257; 38 | float fy = h / 2 / 0.41421356237309503; 39 | return df::PinholeCamera(fx, fy, w/2, h/2, w, h); 40 | } 41 | 42 | inline 43 | cv::Mat LoadPreprocessImage(std::string path, 44 | const df::PinholeCamera& in_cam, 45 | const df::PinholeCamera& out_cam) 46 | { 47 | CHECK(in_cam.width() != 0 && in_cam.height() != 0) << "Invalid input camera provided"; 48 | CHECK(out_cam.width() != 0 && out_cam.height() != 0) << "Invalid input camera provided"; 49 | 50 | // load image 51 | cv::Mat img = cv::imread(path, cv::IMREAD_GRAYSCALE); 52 | CHECK(!img.empty()) << "Failed loading test image " << path; 53 | CHECK_EQ(img.type(), CV_8UC1); 54 | 55 | // make sure the intrinsics are scaled to input image size 56 | df::PinholeCamera in_cam_resized(in_cam); 57 | in_cam_resized.ResizeViewport(img.cols, img.rows); 58 | 59 | // preprocess and return 60 | img.convertTo(img, CV_32FC1, 1/255.0); 61 | return df::ChangeIntrinsics(img, in_cam_resized, out_cam); 62 | } 63 | 64 | template 65 | void CompareWithTol(const Eigen::DenseBase& m1, const Eigen::DenseBase& m2, Scalar tol) 66 | { 67 | for (int i = 0; i < m1.rows(); ++i) 68 | for (int j = 0; j < m1.cols(); ++j) 69 | EXPECT_NEAR(m1(i,j), m2(i,j), tol); 70 | } 71 | 72 | template 73 | Sophus::SE3 GetPerturbedPose(const Sophus::SE3& se3, std::size_t idx, T eps) 74 | { 75 | assert(idx >= 0 && idx < 6 && "invalid perturbation index"); 76 | Sophus::SE3 pose_perturbed(se3); 77 | if (idx < 3) 78 | { 79 | pose_perturbed.translation()[idx] += eps; 80 | } 81 | else 82 | { 83 | Eigen::Matrix omega = Eigen::Matrix::Zero(); 84 | omega(idx-3) = eps; 85 | pose_perturbed.so3() = Sophus::SE3::SO3Type::exp(omega) * pose_perturbed.so3(); 86 | } 87 | return pose_perturbed; 88 | } 89 | 90 | } // namespace df 91 | 92 | #endif // DF_TESTING_UTILS_H_ 93 | -------------------------------------------------------------------------------- /tests/ut_nearestpsd.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include 19 | #include "nearest_psd.h" 20 | 21 | class TestNearestPsd : public ::testing::Test 22 | { 23 | public: 24 | TestNearestPsd() 25 | { 26 | 27 | } 28 | 29 | 30 | }; 31 | 32 | TEST_F(TestNearestPsd, UnitTest) 33 | { 34 | Eigen::MatrixXf M = Eigen::MatrixXf::Random(50,50); 35 | Eigen::MatrixXf B = NearestPsd(M); 36 | 37 | EXPECT_TRUE(IsPsd(B)); 38 | } 39 | -------------------------------------------------------------------------------- /tests/ut_sampling.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is part of DeepFactors. 3 | * 4 | * Copyright (C) 2020 Imperial College London 5 | * 6 | * The use of the code within this file and all code within files that make up 7 | * the software that is DeepFactors is permitted for non-commercial purposes 8 | * only. The full terms and conditions that apply to the code within this file 9 | * are detailed within the LICENSE file and at 10 | * 11 | * unless explicitly stated. By downloading this file you agree to comply with 12 | * these terms. 13 | * 14 | * If you wish to use any of this code for commercial purposes then please 15 | * email researchcontracts.engineering@imperial.ac.uk. 16 | * 17 | */ 18 | #include 19 | #include 20 | 21 | #include "uniform_sampler.h" 22 | 23 | TEST(UniformSampling, ShowSampling) 24 | { 25 | int w = 800; 26 | int h = 600; 27 | 28 | df::UniformSampler sampler(w, h); 29 | 30 | for (int npts = 500; npts < 1000; npts += 100) 31 | { 32 | cv::Mat img(h,w,CV_8UC3); 33 | auto pts = sampler.SamplePoints(npts); 34 | 35 | for (auto p : pts) 36 | cv::circle(img, {p.x, p.y}, 1, {255,0,0}); 37 | 38 | cv::imshow("sampling" + std::to_string(npts), img); 39 | } 40 | 41 | cv::waitKey(0); 42 | } 43 | -------------------------------------------------------------------------------- /thirdparty/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ------------------------------------------------------------------------------ 2 | # Find all the cmake-based dependencies 3 | # If you haven't built them, this should also be able to find other installations 4 | # You can point find_package to them by setting e.g. the Sophus_DIR variable 5 | # Note that we're using HINTS instead of PATHS to make sure to search the 6 | # local build first before looking into cmake package registry 7 | # See: https://cmake.org/cmake/help/v3.0/command/find_package.html 8 | # ------------------------------------------------------------------------------ 9 | set(INSTALL_DIR "${CMAKE_CURRENT_SOURCE_DIR}/install") 10 | set(BUILD_DIR "${CMAKE_CURRENT_SOURCE_DIR}/build") 11 | set(CMAKE_PREFIX_PATH ${INSTALL_DIR}) 12 | # Sophus needs to be before VisionCore as it looks for it 13 | find_package(Sophus NO_DEFAULT_PATH PATHS ${BUILD_DIR}/Sophus REQUIRED) 14 | find_package(VisionCore NO_DEFAULT_PATH PATHS ${BUILD_DIR}/vision_core REQUIRED) 15 | find_package(brisk PATHS ${INSTALL_DIR}/lib/CMake REQUIRED) # Stefan's "CMake" instead of "cmake" 16 | find_package(CameraDrivers QUIET REQUIRED) 17 | find_package(DBoW2 QUIET REQUIRED) 18 | find_package(GTSAM QUIET REQUIRED) 19 | find_package(opengv QUIET REQUIRED) 20 | find_package(Eigen3 QUIET REQUIRED) 21 | find_package(Pangolin QUIET REQUIRED) 22 | 23 | ## ------------------------------------------------------------------------------ 24 | ## Modify brisk's imported target to add their include directories 25 | ## ------------------------------------------------------------------------------ 26 | #set_property(TARGET brisk APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES 27 | # ${BRISK_INCLUDE_DIRS}) 28 | 29 | # ------------------------------------------------------------------------------ 30 | # Add a DBoW2 imported target because it does something weird in its config 31 | # ------------------------------------------------------------------------------ 32 | add_library(DBoW2 SHARED IMPORTED GLOBAL) 33 | set_property(TARGET DBoW2 APPEND PROPERTY IMPORTED_LOCATION 34 | ${DBoW2_LIBRARY}) 35 | set_property(TARGET DBoW2 APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES 36 | ${DBoW2_INCLUDE_DIR}) 37 | -------------------------------------------------------------------------------- /thirdparty/patch/brisk-missing-functional.patch: -------------------------------------------------------------------------------- 1 | --- brisk/src/brisk-feature-detector.cc 2020-03-26 18:25:39.743655315 +0000 2 | +++ brisk/src/brisk-feature-detector.cc_fixed 2020-03-26 18:26:11.330322280 +0000 3 | @@ -44,6 +44,7 @@ 4 | */ 5 | 6 | #include 7 | +#include 8 | #include 9 | 10 | #include 11 | -------------------------------------------------------------------------------- /thirdparty/patch/brisk-opencv4.patch: -------------------------------------------------------------------------------- 1 | --- brisk/src/demo-free.cc 2020-03-26 18:37:13.866995120 +0000 2 | +++ brisk/src/demo-free_new.cc 2019-07-18 14:05:47.733377625 +0100 3 | @@ -102,10 +102,10 @@ 4 | 5 | // Convert to grayscale. 6 | cv::Mat imgGray1; 7 | - cv::cvtColor(imgRGB1, imgGray1, CV_BGR2GRAY); 8 | + cv::cvtColor(imgRGB1, imgGray1, cv::COLOR_BGR2GRAY); 9 | cv::Mat imgGray2; 10 | if (!do_rot) { 11 | - cv::cvtColor(imgRGB2, imgGray2, CV_BGR2GRAY); 12 | + cv::cvtColor(imgRGB2, imgGray2, cv::COLOR_BGR2GRAY); 13 | } 14 | 15 | // Run FAST in first image. 16 | --------------------------------------------------------------------------------