├── .clang-format ├── .clang-tidy ├── .cmake-format.yaml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── CPM.cmake ├── CompilerWarnings.cmake ├── Conan.cmake ├── Doxygen.cmake ├── Functions.cmake ├── Open3D.cmake ├── Sanitizers.cmake ├── StandardProjectSettings.cmake ├── StaticAnalyzers.cmake └── xtensor.cmake ├── misc └── object-slam.gif ├── msg ├── CMakeLists.txt └── image.proto ├── python ├── README.md ├── convert_trajectory_kitti.py ├── evaluate_ate.py ├── gt_poses.txt ├── inference.py ├── main.py └── pipeline │ ├── __init__.py │ ├── pipeline.py │ ├── predict.py │ ├── read_images.py │ ├── save_image.py │ └── store_masks.py ├── results └── scene-12 │ ├── gt_poses.txt │ ├── gt_poses_v1.txt │ ├── gt_poses_v10.txt │ ├── gt_poses_v11.txt │ ├── gt_poses_v12.txt │ ├── gt_poses_v13.txt │ ├── gt_poses_v16.txt │ ├── gt_poses_v2.txt │ ├── gt_poses_v3.txt │ ├── poses.txt │ ├── poses_v1.txt │ ├── poses_v10.txt │ ├── poses_v11.txt │ ├── poses_v12.txt │ ├── poses_v13.txt │ ├── poses_v16.txt │ ├── poses_v2.txt │ └── poses_v3.txt ├── src ├── CMakeLists.txt ├── main.cpp ├── object-slam │ ├── CMakeLists.txt │ ├── controller │ │ ├── CMakeLists.txt │ │ ├── controller.cpp │ │ └── controller.h │ ├── module │ │ ├── CMakeLists.txt │ │ ├── display.cpp │ │ ├── display.h │ │ ├── image_transport.cpp │ │ ├── image_transport.h │ │ ├── mapper.cpp │ │ ├── mapper.h │ │ ├── renderer.cpp │ │ ├── renderer.h │ │ ├── segmenter.cpp │ │ ├── segmenter.h │ │ ├── tracker.cpp │ │ └── tracker.h │ ├── payload │ │ ├── CMakeLists.txt │ │ ├── display_payload.h │ │ ├── graph_optimizer_payload.h │ │ ├── mapper_payload.h │ │ ├── renderer_payload.h │ │ ├── segmenter_payload.h │ │ └── tracker_payload.h │ ├── reader │ │ ├── CMakeLists.txt │ │ ├── data_provider.cpp │ │ ├── data_provider.h │ │ ├── data_reader.cpp │ │ └── data_reader.h │ ├── struct │ │ ├── CMakeLists.txt │ │ ├── frame.h │ │ ├── instance_image.h │ │ ├── map.cpp │ │ ├── map.h │ │ ├── tsdf_object.cpp │ │ └── tsdf_object.h │ └── utils │ │ ├── CMakeLists.txt │ │ ├── macros.h │ │ ├── pipeline_module.cpp │ │ ├── pipeline_module.h │ │ ├── pipeline_payload.h │ │ ├── queue_synchroniser.h │ │ ├── thread_class.h │ │ ├── thread_data.h │ │ ├── thread_safe_queue.h │ │ ├── thread_sync_var.h │ │ ├── timer.h │ │ ├── types.h │ │ └── utils.h └── show-objects.cpp └── test └── CMakeLists.txt /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AlignConsecutiveMacros: true 4 | AlignConsecutiveAssignments: true 5 | AllowShortCaseLabelsOnASingleLine: true 6 | AllowShortBlocksOnASingleLine: true 7 | AllowShortIfStatementsOnASingleLine: Never 8 | AllowShortLoopsOnASingleLine: false 9 | AllowShortFunctionsOnASingleLine: true 10 | AllowShortBlocksOnASingleLine: true 11 | AlignTrailingComments: true 12 | BreakBeforeBraces: Allman 13 | Cpp11BracedListStyle: false 14 | ColumnLimit: 125 15 | IndentWidth: 4 16 | IncludeBlocks: Preserve 17 | FixNamespaceComments: true 18 | NamespaceIndentation: All 19 | SpaceAfterTemplateKeyword: false 20 | SpaceBeforeCtorInitializerColon: true 21 | SpaceBeforeInheritanceColon: true 22 | SpaceBeforeRangeBasedForLoopColon: true 23 | SpaceInEmptyParentheses: false 24 | AlignAfterOpenBracket: Align 25 | BinPackArguments: false 26 | BinPackParameters: false 27 | Standard: Cpp11 28 | ... 29 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | --- 2 | Checks: '-fuchsia-*,-google-*,-zircon-*,-abseil-*,-modernize-use-trailing-return-type,-llvm-*,-hicpp-signed-bitwise' 3 | WarningsAsErrors: '' 4 | HeaderFilterRegex: '.*' 5 | FormatStyle: none 6 | -------------------------------------------------------------------------------- /.cmake-format.yaml: -------------------------------------------------------------------------------- 1 | additional_commands: 2 | foo: 3 | flags: 4 | - BAR 5 | - BAZ 6 | kwargs: 7 | DEPENDS: '*' 8 | HEADERS: '*' 9 | SOURCES: '*' 10 | bullet_char: '*' 11 | dangle_parens: false 12 | enum_char: . 13 | line_ending: unix 14 | line_width: 120 15 | max_subargs_per_line: 3 16 | separate_ctrl_name_with_space: false 17 | separate_fn_name_with_space: false 18 | tab_size: 2 19 | 20 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeFiles 2 | build/ 3 | results/ 4 | 5 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "3rdparty/detectron2"] 2 | path = 3rdparty/detectron2 3 | url = https://github.com/akashsharma02/detectron2.git 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15) 2 | 3 | # ------------------------ Object SLAM ------------------------------ 4 | project(object-slam LANGUAGES CXX CUDA) 5 | 6 | # ---------- Standard Project settings and warnings ----------------- 7 | include(cmake/StandardProjectSettings.cmake) 8 | # Project options to be applicable for all the targets in the folder 9 | add_library(project_options INTERFACE) 10 | target_compile_features(project_options INTERFACE cxx_std_17) 11 | target_include_directories(project_options INTERFACE ${CMAKE_CURRENT_BINARY_DIR}) 12 | 13 | # Interface library for warnings 14 | # standard compiler warnings 15 | add_library(project_warnings INTERFACE) 16 | include(cmake/CompilerWarnings.cmake) 17 | set_project_warnings(project_warnings) 18 | 19 | # enable doxygen 20 | include(cmake/Doxygen.cmake) 21 | enable_doxygen() 22 | 23 | # ---------- Options for debugging ----------- 24 | 25 | # ---------- Sanitizer and Static Analyzers to help for debugging ----------- 26 | # sanitizer options if supported by compiler 27 | include(cmake/Sanitizers.cmake) 28 | enable_sanitizers(project_options) 29 | # allow for static analysis options 30 | include(cmake/StaticAnalyzers.cmake) 31 | 32 | option(WITH_DEBUG_VIS "With Visualization for debugging" ON) 33 | 34 | if(WITH_DEBUG_VIS) 35 | target_compile_definitions(project_options INTERFACE OSLAM_DEBUG_VIS) 36 | endif() 37 | 38 | # ----------------------- Dependencies -------------------------------------- 39 | 40 | include(cmake/Conan.cmake) 41 | run_conan() 42 | find_package(Threads REQUIRED) 43 | find_package(Boost REQUIRED) 44 | find_package(Eigen3 3.3 REQUIRED) 45 | find_package( 46 | OpenCV 4.0 REQUIRED 47 | COMPONENTS core imgproc highgui rgbd 48 | ) 49 | find_package(GTSAM REQUIRED) 50 | 51 | # ----------------------- Build options ------------------------------------- 52 | option(BUILD_SHARED_LIBS "Enable compilation of shared libraries" OFF) 53 | option(ENABLE_TESTING "Enable Test Builds" OFF) 54 | option(ENABLE_PCH "Enable Precompiled Headers" OFF) 55 | option(WITH_OPENMP "Use OpenMP" ON) 56 | if(ENABLE_PCH) 57 | target_precompile_headers(project_options INTERFACE ) 58 | endif() 59 | 60 | # ----------------------- Subdirectories ------------------------------------ 61 | add_subdirectory(msg) 62 | add_subdirectory(src) 63 | 64 | # ----------------------- Testing ------------------------------------------- 65 | if(ENABLE_TESTING) 66 | enable_testing() 67 | message("Building Tests. Be sure to check out test/constexpr_tests for constexpr testing") 68 | add_subdirectory(test) 69 | endif() 70 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Robot Perception Lab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Compositional and Scalable Object SLAM 2 | 3 | ## Introduction 4 | Compositional and Scalable Object SLAM is an implementation of semantic RGB-D SLAM that employs recognizable objects in the scene as landmarks. As a RGBD camera moves through a scene containing objects, a pre-trained Deep Neural Network (DNN) (PointRend) produces instance segmentations of the observed objects. These instances are tracked simultaneously and volumetrically integrated in realtime to represent the scene in a sparse object map. 5 | 6 |
7 | 8 | ## Requirements and Dependencies 9 | 10 | - C++17 (Compiler: GCC 7+, CMake: 3.15+) (std::optional, make_unique, etc) 11 | - Open3D CUDA branch: My fork of CUDA branch preferable 12 | - GTSAM library: (Not yet used) 13 | - Eigen > 3.3 14 | - Boost 1.65+ 15 | - Conan package manager (docopt, fmt, spdlog, cppzmq, protobuf) 16 | 17 | ## Dataset requirement 18 | 19 | The dataset is required to have the following folder structure: 20 | ```bash 21 | . 22 | ├── camera-intrinsics.json 23 | ├── color [723 entries] 24 | ├── depth [723 entries] 25 | └── preprocessed [1446 entries] 26 | ``` 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /cmake/CompilerWarnings.cmake: -------------------------------------------------------------------------------- 1 | function(set_project_warnings project_name) 2 | option(WARNINGS_AS_ERRORS "Treat compiler warnings as errors" FALSE) 3 | 4 | set(MSVC_WARNINGS 5 | /W4 # Baseline reasonable warnings 6 | /w14242 # 'identfier': conversion from 'type1' to 'type1', possible loss 7 | # of data 8 | /w14254 # 'operator': conversion from 'type1:field_bits' to 9 | # 'type2:field_bits', possible loss of data 10 | /w14263 # 'function': member function does not override any base class 11 | # virtual member function 12 | /w14265 # 'classname': class has virtual functions, but destructor is not 13 | # virtual instances of this class may not be destructed correctly 14 | /w14287 # 'operator': unsigned/negative constant mismatch 15 | /we4289 # nonstandard extension used: 'variable': loop control variable 16 | # declared in the for-loop is used outside the for-loop scope 17 | /w14296 # 'operator': expression is always 'boolean_value' 18 | /w14311 # 'variable': pointer truncation from 'type1' to 'type2' 19 | /w14545 # expression before comma evaluates to a function which is missing 20 | # an argument list 21 | /w14546 # function call before comma missing argument list 22 | /w14547 # 'operator': operator before comma has no effect; expected 23 | # operator with side-effect 24 | /w14549 # 'operator': operator before comma has no effect; did you intend 25 | # 'operator'? 26 | /w14555 # expression has no effect; expected expression with side- effect 27 | /w14619 # pragma warning: there is no warning number 'number' 28 | /w14640 # Enable warning on thread un-safe static member initialization 29 | /w14826 # Conversion from 'type1' to 'type_2' is sign-extended. This may 30 | # cause unexpected runtime behavior. 31 | /w14905 # wide string literal cast to 'LPSTR' 32 | /w14906 # string literal cast to 'LPWSTR' 33 | /w14928 # illegal copy-initialization; more than one user-defined 34 | # conversion has been implicitly applied 35 | ) 36 | 37 | set(CLANG_WARNINGS 38 | -Wall 39 | -Wextra # reasonable and standard 40 | -Wshadow # warn the user if a variable declaration shadows one from a 41 | # parent context 42 | -Wnon-virtual-dtor # warn the user if a class with virtual functions has a 43 | # non-virtual destructor. This helps catch hard to 44 | # track down memory errors 45 | -Wold-style-cast # warn for c-style casts 46 | -Wcast-align # warn for potential performance problem casts 47 | -Wunused # warn on anything being unused 48 | -Woverloaded-virtual # warn if you overload (not override) a virtual 49 | # function 50 | -Wpedantic # warn if non-standard C++ is used 51 | -Wconversion # warn on type conversions that may lose data 52 | -Wsign-conversion # warn on sign conversions 53 | -Wnull-dereference # warn if a null dereference is detected 54 | -Wdouble-promotion # warn if float is implicit promoted to double 55 | -Wformat=2 # warn on security issues around functions that format output 56 | # (ie printf) 57 | ) 58 | 59 | if (WARNINGS_AS_ERRORS) 60 | set(CLANG_WARNINGS ${CLANG_WARNINGS} -Werror) 61 | set(MSVC_WARNINGS ${MSVC_WARNINGS} /WX) 62 | endif() 63 | 64 | set(GCC_WARNINGS 65 | ${CLANG_WARNINGS} 66 | -Wmisleading-indentation # warn if identation implies blocks where blocks do not exist 67 | -Wduplicated-cond # warn if if / else chain has duplicated conditions 68 | # TODO(Akash): Figure out how to suppress this for Eigen 69 | # -Wduplicated-branches # warn if if / else branches have duplicated code 70 | -Wlogical-op # warn about logical operations being used where bitwise were probably wanted 71 | -Wuseless-cast # warn if you perform a cast to the same type 72 | ) 73 | 74 | if(MSVC) 75 | set(PROJECT_WARNINGS ${MSVC_WARNINGS}) 76 | elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") 77 | set(PROJECT_WARNINGS ${CLANG_WARNINGS}) 78 | else() 79 | set(PROJECT_WARNINGS ${GCC_WARNINGS}) 80 | endif() 81 | 82 | target_compile_options(${project_name} INTERFACE ${PROJECT_WARNINGS}) 83 | 84 | endfunction() 85 | -------------------------------------------------------------------------------- /cmake/Conan.cmake: -------------------------------------------------------------------------------- 1 | macro(run_conan) 2 | # Download automatically, you can also just copy the conan.cmake file 3 | if(NOT EXISTS "${CMAKE_BINARY_DIR}/conan.cmake") 4 | message( 5 | STATUS 6 | "Downloading conan.cmake from https://github.com/conan-io/cmake-conan" 7 | ) 8 | file( 9 | DOWNLOAD "https://github.com/conan-io/cmake-conan/raw/v0.15/conan.cmake" 10 | "${CMAKE_BINARY_DIR}/conan.cmake" 11 | ) 12 | endif() 13 | 14 | include(${CMAKE_BINARY_DIR}/conan.cmake) 15 | 16 | conan_add_remote( 17 | NAME bincrafters URL 18 | https://api.bintray.com/conan/bincrafters/public-conan 19 | ) 20 | 21 | conan_add_remote( 22 | NAME omaralvarez URL 23 | https://api.bintray.com/conan/omaralvarez/public-conan 24 | ) 25 | 26 | conan_cmake_run( 27 | REQUIRES 28 | ${CONAN_EXTRA_REQUIRES} 29 | catch2/2.11.0 30 | CLI11/1.9.1@cliutils/stable 31 | fmt/7.0.3 32 | spdlog/1.8.0 33 | cppzmq/4.6.0 34 | protoc_installer/3.9.1@bincrafters/stable 35 | protobuf/3.9.1@bincrafters/stable 36 | xtensor/0.20.10@omaralvarez/public-conan 37 | OPTIONS 38 | ${CONAN_EXTRA_OPTIONS} 39 | BASIC_SETUP 40 | CMAKE_TARGETS # individual targets to link to 41 | BUILD 42 | missing 43 | ) 44 | endmacro() 45 | -------------------------------------------------------------------------------- /cmake/Doxygen.cmake: -------------------------------------------------------------------------------- 1 | function(enable_doxygen) 2 | option(ENABLE_DOXYGEN "Enable doxygen doc builds of source" OFF) 3 | if(ENABLE_DOXYGEN) 4 | set(DOXYGEN_CALLER_GRAPH ON) 5 | set(DOXYGEN_CALL_GRAPH ON) 6 | set(DOXYGEN_EXTRACT_ALL ON) 7 | find_package(Doxygen REQUIRED dot) 8 | doxygen_add_docs(doxygen-docs ${PROJECT_SOURCE_DIR}) 9 | 10 | endif() 11 | endfunction() 12 | -------------------------------------------------------------------------------- /cmake/Functions.cmake: -------------------------------------------------------------------------------- 1 | # ------------- Suppress warnings from third party included libraries 2 | # Source: https://stackoverflow.com/questions/52135983/cmake-target-link-libraries-include-as-system-to-suppress-compiler-warnings 3 | function(target_link_libraries_system target) 4 | set(options PRIVATE PUBLIC INTERFACE) 5 | cmake_parse_arguments(TLLS "${options}" "" "" ${ARGN}) 6 | foreach(op ${options}) 7 | if(TLLS_${op}) 8 | set(scope ${op}) 9 | endif() 10 | endforeach(op) 11 | set(libs ${TLLS_UNPARSED_ARGUMENTS}) 12 | 13 | foreach(lib ${libs}) 14 | get_target_property(lib_include_dirs ${lib} INTERFACE_INCLUDE_DIRECTORIES) 15 | if(lib_include_dirs) 16 | if(scope) 17 | target_include_directories(${target} SYSTEM ${scope} ${lib_include_dirs}) 18 | else() 19 | target_include_directories(${target} SYSTEM PRIVATE ${lib_include_dirs}) 20 | endif() 21 | else() 22 | message("Warning: ${lib} doesn't set INTERFACE_INCLUDE_DIRECTORIES. No include_directories set.") 23 | endif() 24 | if(scope) 25 | target_link_libraries(${target} ${scope} ${lib}) 26 | else() 27 | target_link_libraries(${target} ${lib}) 28 | endif() 29 | endforeach() 30 | endfunction(target_link_libraries_system) 31 | -------------------------------------------------------------------------------- /cmake/Open3D.cmake: -------------------------------------------------------------------------------- 1 | 2 | include (${CMAKE_CURRENT_LIST_DIR}/CPM.cmake) 3 | 4 | CPMFindPackage( 5 | NAME Open3D 6 | GITHUB_REPOSITORY akashsharma02/Open3D 7 | VERSION 0.9.0 8 | GIT_TAG object-slam 9 | OPTIONS 10 | "BUILD_CPP_EXAMPLES OFF" 11 | "BUILD_CUDA_EXAMPLES OFF" 12 | "BUILD_CUDA_MODULE ON" 13 | "BUILD_EIGEN3 OFF" 14 | "CUDA_WITH_OPENCV ON" 15 | "BUILD_PYBIND11 OFF" 16 | "BUILD_PYTHON_MODULE OFF" 17 | "BUILD_WITH_MARCH_NATIVE OFF" 18 | "ENABLE_JUPYTER OFF" 19 | ) 20 | -------------------------------------------------------------------------------- /cmake/Sanitizers.cmake: -------------------------------------------------------------------------------- 1 | function(enable_sanitizers project_name) 2 | 3 | if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID STREQUAL 4 | "Clang") 5 | option(ENABLE_COVERAGE "Enable coverage reporting for gcc/clang" FALSE) 6 | 7 | if(ENABLE_COVERAGE) 8 | target_compile_options(project_options INTERFACE --coverage -O0 -g) 9 | target_link_libraries(project_options INTERFACE --coverage) 10 | endif() 11 | 12 | set(SANITIZERS "") 13 | 14 | option(ENABLE_SANITIZER_ADDRESS "Enable address sanitizer" FALSE) 15 | if(ENABLE_SANITIZER_ADDRESS) 16 | list(APPEND SANITIZERS "address") 17 | endif() 18 | 19 | option(ENABLE_SANITIZER_MEMORY "Enable memory sanitizer" FALSE) 20 | if(ENABLE_SANITIZER_MEMORY) 21 | list(APPEND SANITIZERS "memory") 22 | endif() 23 | 24 | option(ENABLE_SANITIZER_UNDEFINED_BEHAVIOR 25 | "Enable undefined behavior sanitizer" FALSE) 26 | if(ENABLE_SANITIZER_UNDEFINED_BEHAVIOR) 27 | list(APPEND SANITIZERS "undefined") 28 | endif() 29 | 30 | option(ENABLE_SANITIZER_THREAD "Enable thread sanitizer" FALSE) 31 | if(ENABLE_SANITIZER_THREAD) 32 | list(APPEND SANITIZERS "thread") 33 | endif() 34 | 35 | list(JOIN SANITIZERS "," LIST_OF_SANITIZERS) 36 | 37 | endif() 38 | 39 | if(LIST_OF_SANITIZERS) 40 | if(NOT "${LIST_OF_SANITIZERS}" STREQUAL "") 41 | target_compile_options(${project_name} 42 | INTERFACE -fsanitize=${LIST_OF_SANITIZERS}) 43 | target_link_libraries(${project_name} 44 | INTERFACE -fsanitize=${LIST_OF_SANITIZERS}) 45 | endif() 46 | endif() 47 | 48 | endfunction() 49 | -------------------------------------------------------------------------------- /cmake/StandardProjectSettings.cmake: -------------------------------------------------------------------------------- 1 | include(CheckLanguage) 2 | 3 | # Set a default build type if none was specified 4 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 5 | message( 6 | STATUS "Setting build type to 'RelWithDebInfo' as none was specified.") 7 | set(CMAKE_BUILD_TYPE 8 | RelWithDebInfo 9 | CACHE STRING "Choose the type of build." FORCE) 10 | # Set the possible values of build type for cmake-gui, ccmake 11 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" 12 | "MinSizeRel" "RelWithDebInfo") 13 | endif() 14 | 15 | find_program(CCACHE ccache) 16 | if(CCACHE) 17 | message("using ccache") 18 | set(CMAKE_CXX_COMPILER_LAUNCHER ${CCACHE}) 19 | else() 20 | message("ccache not found cannot use") 21 | endif() 22 | 23 | # Generate compile_commands.json to make it easier to work with clang based 24 | # tools 25 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 26 | 27 | option(ENABLE_IPO 28 | "Enable Iterprocedural Optimization, aka Link Time Optimization (LTO)" 29 | OFF) 30 | 31 | if(ENABLE_IPO) 32 | include(CheckIPOSupported) 33 | check_ipo_supported(RESULT result OUTPUT output) 34 | if(result) 35 | set(CMAKE_INTERPROCEDURAL_OPTIMIZATION TRUE) 36 | else() 37 | message(SEND_ERROR "IPO is not supported: ${output}") 38 | endif() 39 | endif() 40 | -------------------------------------------------------------------------------- /cmake/StaticAnalyzers.cmake: -------------------------------------------------------------------------------- 1 | option(ENABLE_CPPCHECK "Enable static analysis with cppcheck" OFF) 2 | option(ENABLE_CLANG_TIDY "Enable static analysis with clang-tidy" OFF) 3 | 4 | if(ENABLE_CPPCHECK) 5 | find_program(CPPCHECK cppcheck) 6 | if(CPPCHECK) 7 | set(CMAKE_CXX_CPPCHECK ${CPPCHECK} --suppress=missingInclude --enable=all 8 | --inconclusive -i ${CMAKE_SOURCE_DIR}/imgui/lib) 9 | else() 10 | message(SEND_ERROR "cppcheck requested but executable not found") 11 | endif() 12 | endif() 13 | 14 | if(ENABLE_CLANG_TIDY) 15 | find_program(CLANGTIDY clang-tidy) 16 | if(CLANGTIDY) 17 | set(CMAKE_CXX_CLANG_TIDY ${CLANGTIDY} -extra-arg=-Wno-unknown-warning-option) 18 | else() 19 | message(SEND_ERROR "clang-tidy requested but executable not found") 20 | endif() 21 | endif() 22 | 23 | 24 | -------------------------------------------------------------------------------- /cmake/xtensor.cmake: -------------------------------------------------------------------------------- 1 | include (${CMAKE_CURRENT_LIST_DIR}/CPM.cmake) 2 | 3 | CPMFindPackage( 4 | NAME xtensor 5 | GITHUB_REPOSITORY xtensor-stack/xtensor 6 | GIT_TAG master 7 | ) 8 | 9 | -------------------------------------------------------------------------------- /misc/object-slam.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpl-cmu/object-slam/7baecfc995d4bbd6441585742df810e38568732f/misc/object-slam.gif -------------------------------------------------------------------------------- /msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Protobuf REQUIRED) 2 | 3 | include(CMakePrintHelpers) 4 | 5 | protobuf_generate_cpp(PROTO_SRCS PROTO_HEADERS image.proto) 6 | protobuf_generate_python(PY_SRCS image.proto) 7 | 8 | add_library(proto ${PROTO_SRCS} ${PROTO_HEADERS}) 9 | target_include_directories( 10 | proto PRIVATE 11 | ${Protobuf_INCLUDE_DIR} 12 | PUBLIC 13 | ${CMAKE_CURRENT_BINARY_DIR} 14 | ) 15 | target_link_libraries(proto PUBLIC ${Protobuf_LIBRARY}) 16 | 17 | add_custom_target(proto_py ALL DEPENDS ${PY_SRCS}) 18 | -------------------------------------------------------------------------------- /msg/image.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto3"; 2 | 3 | package oslam; 4 | 5 | // Protobuf message for color image 6 | message ColorImage { 7 | int32 width = 1; 8 | int32 height = 2; 9 | int32 num_channels = 3; 10 | int32 bytes_per_channel = 4; 11 | bytes data = 5; 12 | } 13 | 14 | message MaskImage { 15 | int32 width = 1; 16 | int32 height = 2; 17 | int32 num_channels = 3; 18 | int32 bytes_per_channel = 4; 19 | bytes data = 5; 20 | bytes features = 9; 21 | repeated uint32 labels = 6; 22 | repeated double scores = 7; 23 | 24 | message BoundingBox { 25 | repeated float coordinates = 1 [packed=true]; 26 | } 27 | 28 | repeated BoundingBox bboxes = 8; 29 | } 30 | -------------------------------------------------------------------------------- /python/README.md: -------------------------------------------------------------------------------- 1 | # Pre-process dataset 2 | 3 | This pipeline pre-processes a dataset containing images into the top 3 masks predictions found by Mask-RCNN. The masks are stored as the 3 channels of the image, and the mask class is stored separately as txt file. 4 | 5 | The projects depends on [detectron2](https://github.com/facebookresearch/detectron2), and was heavily inspired from [detectron2-pipeline](https://github.com/jagin/detectron2-pipeline) 6 | 7 | ## Getting started 8 | 9 | Install detectron2 on a folder above this project. Refer to [INSTALL.md](https://github.com/facebookresearch/detectron2/blob/master/INSTALL.md) for detailed instructions. 10 | 11 | ## Running 12 | 13 | Run the `process_dataset` script to run the inference on the image folder 14 | 15 | $ python process_dataset.py -i ~/Documents/datasets/sun3d/seq-01/ 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /python/convert_trajectory_kitti.py: -------------------------------------------------------------------------------- 1 | import quaternion 2 | import pathlib 3 | import argparse 4 | import numpy as np 5 | import math 6 | 7 | def files_in_path(directory): 8 | """TODO: Docstring for files_in_path. 9 | 10 | :function: TODO 11 | :returns: TODO 12 | 13 | """ 14 | directory_path = pathlib.Path(directory) 15 | assert(directory_path.is_dir()) 16 | file_list = [] 17 | for x in sorted(directory_path.iterdir()): 18 | if x.is_file(): 19 | file_list.append(x) 20 | elif x.is_dir(): 21 | file_list.extend(files_in_path(x)) 22 | return file_list 23 | 24 | def load_file_list(file_list): 25 | """Loads the files into a list of numpy arrays 26 | 27 | :function: TODO 28 | :returns: TODO 29 | 30 | """ 31 | trajectory_list = [] 32 | for filename in file_list: 33 | trajectory_list.append(np.genfromtxt(filename, delimiter="\t", autostrip=True, usecols=(0, 1, 2, 3), max_rows=4)) 34 | 35 | return trajectory_list 36 | 37 | def load_trajectory_file(poses_path): 38 | poses_path = pathlib.Path(poses_path) 39 | if not poses_path.exists(): 40 | assert False, f"Camera trajectory file does not exist at: {str(poses_path)}" 41 | 42 | trajectory = np.loadtxt(poses_path) 43 | trajectory = np.vsplit(trajectory, trajectory.shape[0]/trajectory.shape[1]) 44 | 45 | return trajectory 46 | 47 | def parse_rgbd_dataset(args): 48 | """TODO: Docstring for parse_rgbd_dataset. 49 | 50 | :function: TODO 51 | :returns: TODO 52 | 53 | """ 54 | trajectory_list = load_trajectory_file(args.poses_path) 55 | gt_file_list = files_in_path(args.gt_poses_path) 56 | gt_trajectory_list = load_file_list(gt_file_list) 57 | 58 | num_poses = len(gt_trajectory_list); 59 | print(f"Number of poses in Ground truth trajectory: {num_poses}") 60 | 61 | gt_poses_list, poses_list = [], [] 62 | for idx, (gt_pose, pose) in enumerate(zip(gt_trajectory_list, trajectory_list)): 63 | gt_pose = gt_pose[0:3, 0:4] 64 | pose = pose[0:3, 0:4] 65 | gt_poses_list.append(gt_pose.flatten()) 66 | poses_list.append(pose.flatten()) 67 | 68 | gt_poses_list = np.vstack(gt_poses_list) 69 | poses_list = np.vstack(poses_list) 70 | print(f"Number of associated files: {gt_poses_list.shape[0]}") 71 | np.savetxt("gt_poses.txt", gt_poses_list) 72 | np.savetxt("poses.txt", poses_list) 73 | 74 | def associate(first_list, second_list, offset,max_difference): 75 | """ 76 | Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 77 | to find the closest match for every input tuple. 78 | 79 | Input: 80 | first_list -- first dictionary of (stamp,data) tuples 81 | second_list -- second dictionary of (stamp,data) tuples 82 | offset -- time offset between both dictionaries (e.g., to model the delay between the sensors) 83 | max_difference -- search radius for candidate generation 84 | Output: 85 | matches -- list of matched tuples ((stamp1,data1),(stamp2,data2)) 86 | 87 | """ 88 | first_keys = list(first_list) 89 | second_keys = list(second_list) 90 | potential_matches = [(abs(a - (b + offset)), a, b) 91 | for a in first_keys 92 | for b in second_keys 93 | if abs(a - (b + offset)) < max_difference] 94 | potential_matches.sort() 95 | matches = [] 96 | for diff, a, b in potential_matches: 97 | if a in first_keys and b in second_keys: 98 | idx1 = first_keys.index(a) 99 | idx2 = second_keys.index(b) 100 | matches.append((idx1, idx2)) 101 | 102 | matches.sort() 103 | return matches 104 | 105 | def read_file_list(filename): 106 | """ 107 | Reads a trajectory from a text file. 108 | 109 | File format: 110 | The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched) 111 | and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 112 | 113 | Input: 114 | filename -- File name 115 | 116 | Output: 117 | dict -- dictionary of (stamp,data) tuples 118 | 119 | """ 120 | file = open(filename) 121 | data = file.read() 122 | lines = data.replace(","," ").replace("\t"," ").split("\n") 123 | list1 = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line) > 0 and line[0] != "#"] 124 | 125 | list1 = [(float(l[0]),l[1:]) for l in list1 if len(l)>1] 126 | return dict(list1) 127 | 128 | def parse_tum_dataset(args): 129 | """TODO: Docstring for parse_tum_dataset. 130 | 131 | :function: TODO 132 | :returns: TODO 133 | 134 | """ 135 | gt_file = args.gt_pose_file 136 | association_file = args.association_file 137 | gt_list = read_file_list(gt_file) 138 | files_list = read_file_list(association_file) 139 | 140 | num_poses = len(gt_list); 141 | print(f"Number of poses in Ground truth trajectory: {num_poses}") 142 | matches = associate(gt_list, files_list, 0.0, 0.02) 143 | print(len(matches)) 144 | trajectory_list = load_trajectory_file(args.poses_path) 145 | print(len(trajectory_list)) 146 | 147 | gt_list = list(gt_list.values()) 148 | gt_poses_list, poses_list = [], [] 149 | for idx1, idx2 in matches: 150 | gt_pose = gt_list[idx1] 151 | gt_pose = np.array(gt_pose).astype(np.float) 152 | t = np.expand_dims(gt_pose[0:3], 1) 153 | q = quaternion.as_quat_array(gt_pose[3:]) 154 | R = quaternion.as_rotation_matrix(q) 155 | gt_pose = np.hstack((R, t)) 156 | pose = np.asarray(trajectory_list[idx2])[0:3, 0:4] 157 | gt_poses_list.append(gt_pose.flatten()) 158 | poses_list.append(pose.flatten()) 159 | 160 | gt_poses_list = np.vstack(gt_poses_list) 161 | poses_list = np.vstack(poses_list) 162 | print(gt_poses_list.shape) 163 | print(poses_list.shape) 164 | np.savetxt("gt_poses.txt", gt_poses_list) 165 | np.savetxt("poses.txt", poses_list) 166 | 167 | 168 | def parse_tum_associate(args): 169 | """TODO: Docstring for parse_tum_associate. 170 | 171 | :function: TODO 172 | :returns: TODO 173 | 174 | """ 175 | gt_file = args.gt_pose_file 176 | association_file = args.association_file 177 | gt_list = read_file_list(gt_file) 178 | files_list = read_file_list(association_file) 179 | pose_list = read_file_list(args.poses_path) 180 | 181 | num_poses = len(gt_list); 182 | print(f"Number of poses in Ground truth trajectory: {num_poses}") 183 | matches = associate(gt_list, files_list, 0.0, 0.02) 184 | print(len(matches)) 185 | 186 | gt_list = list(gt_list.values()) 187 | pose_list = list(pose_list.values()) 188 | print(len(pose_list)) 189 | gt_poses_list, poses_list = [], [] 190 | for idx1, idx2 in matches: 191 | gt_pose = gt_list[idx1] 192 | gt_pose = np.array(gt_pose).astype(np.float) 193 | t = np.expand_dims(gt_pose[0:3], 1) 194 | q = quaternion.as_quat_array(gt_pose[3:]) 195 | R = quaternion.as_rotation_matrix(q) 196 | gt_pose = np.hstack((R, t)) 197 | gt_poses_list.append(gt_pose.flatten()) 198 | 199 | pose = pose_list[idx2] 200 | pose = np.array(pose).astype(np.float) 201 | t = np.expand_dims(pose[0:3], 1) 202 | q = quaternion.as_quat_array(pose[3:]) 203 | R = quaternion.as_rotation_matrix(q) 204 | pose = np.hstack((R, t)) 205 | poses_list.append(pose.flatten()) 206 | 207 | gt_poses_list = np.vstack(gt_poses_list) 208 | poses_list = np.vstack(poses_list) 209 | print(gt_poses_list.shape) 210 | print(poses_list.shape) 211 | np.savetxt("gt_poses.txt", gt_poses_list) 212 | np.savetxt("poses.txt", poses_list) 213 | 214 | 215 | 216 | 217 | if __name__ == "__main__": 218 | parser = argparse.ArgumentParser(description="Calculate absolute trajectory error for dataset") 219 | 220 | parser.add_argument("poses_path", help="Path to the poses generated from object-slam") 221 | subparsers = parser.add_subparsers(title="Dataset type", help="Subparsers for dataset") 222 | 223 | parser_rgbd = subparsers.add_parser("rgbd-scenes", help="Arguments for RGBD scenes dataset type") 224 | parser_rgbd.add_argument("gt_poses_path", help="Path to ground truth poses of the dataset") 225 | parser_rgbd.set_defaults(func=parse_rgbd_dataset) 226 | 227 | parser_tum = subparsers.add_parser("tum", help="Arguments for TUM dataset type") 228 | parser_tum.add_argument("gt_pose_file", help="Path to the ground truth poses txt file") 229 | parser_tum.add_argument("association_file", help="Path to the synchronized rgbd file list") 230 | parser_tum.set_defaults(func=parse_tum_dataset) 231 | 232 | parser_tum_associate = subparsers.add_parser("associate_tum") 233 | parser_tum_associate.add_argument("gt_pose_file", help="Path to the ground truth poses txt file") 234 | parser_tum_associate.add_argument("association_file", help="Path to the synchronized rgbd file list") 235 | parser_tum.set_defaults(func=parse_tum_associate) 236 | 237 | args = parser.parse_args() 238 | args.func(args) 239 | 240 | -------------------------------------------------------------------------------- /python/evaluate_ate.py: -------------------------------------------------------------------------------- 1 | import pathlib 2 | import argparse 3 | import numpy as np 4 | import math 5 | 6 | def files_in_path(directory): 7 | """TODO: Docstring for files_in_path. 8 | 9 | :function: TODO 10 | :returns: TODO 11 | 12 | """ 13 | directory_path = pathlib.Path(directory) 14 | assert(directory_path.is_dir()) 15 | file_list = [] 16 | for x in sorted(directory_path.iterdir()): 17 | if x.is_file(): 18 | file_list.append(x) 19 | elif x.is_dir(): 20 | file_list.extend(files_in_path(x)) 21 | return file_list 22 | 23 | def load_file_list(file_list): 24 | """Loads the files into a list of numpy arrays 25 | 26 | :function: TODO 27 | :returns: TODO 28 | 29 | """ 30 | trajectory_list = [] 31 | for filename in file_list: 32 | trajectory_list.append(np.genfromtxt(filename, delimiter="\t", autostrip=True, usecols=(0, 1, 2, 3), max_rows=4)) 33 | 34 | return trajectory_list 35 | 36 | def load_trajectory_file(poses_path): 37 | poses_path = pathlib.Path(poses_path) 38 | if not poses_path.exists(): 39 | assert False, f"Camera trajectory file does not exist at: {str(poses_path)}" 40 | 41 | trajectory = np.loadtxt(poses_path) 42 | trajectory = np.vsplit(trajectory, trajectory.shape[0]/trajectory.shape[1]) 43 | 44 | return trajectory 45 | 46 | 47 | 48 | if __name__ == "__main__": 49 | parser = argparse.ArgumentParser(description="Calculate absolute trajectory error for dataset") 50 | 51 | parser.add_argument("gt_poses_path", help="Path to ground truth poses of the dataset") 52 | parser.add_subparsers 53 | parser.add_argument("poses_path", help="Path to the poses generated from object-slam") 54 | 55 | args = parser.parse_args(); 56 | 57 | gt_file_list = files_in_path(args.gt_poses_path) 58 | gt_trajectory_list = load_file_list(gt_file_list) 59 | 60 | trajectory_list = load_trajectory_file(args.poses_path) 61 | 62 | num_poses = len(gt_trajectory_list); 63 | delta = 30 64 | 65 | total_absolute_trajectory_error = 0.0 66 | for idx, (gt_pose, pose) in enumerate(zip(gt_trajectory_list, trajectory_list)): 67 | gt_pose_inv = np.linalg.inv(gt_pose) 68 | error = gt_pose_inv * pose 69 | print(f"Error at {idx}: \n {error}") 70 | translation = error[0:3, 3] 71 | total_absolute_trajectory_error += np.dot(translation, translation) 72 | 73 | ate_rmse = np.sqrt(total_absolute_trajectory_error / num_poses) 74 | 75 | 76 | total_relative_pose_error = 0.0 77 | for idx, (gt_pose, pose, gt_pose_delta, pose_delta) in enumerate(zip(gt_trajectory_list[:-delta], trajectory_list[:-delta], 78 | gt_trajectory_list[delta:], trajectory_list[delta:])): 79 | gt_pose_inv = np.linalg.inv(gt_pose) 80 | pose_inv = np.linalg.inv(pose) 81 | 82 | relative_pose_error = np.linalg.inv(gt_pose_inv * gt_pose_delta) * (pose_inv * pose_delta) 83 | 84 | translation = relative_pose_error[0:3, 3] 85 | total_relative_pose_error += np.dot(translation, translation) 86 | 87 | rpe_rmse = 1 / (num_poses - delta) * np.sqrt(total_absolute_trajectory_error) 88 | 89 | 90 | print(f"Absolute Trajectory error: {ate_rmse}\n") 91 | print(f"Relative pose error: {rpe_rmse}\n") 92 | 93 | -------------------------------------------------------------------------------- /python/inference.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import zmq 5 | import struct 6 | 7 | import torch 8 | from detectron2.config import get_cfg 9 | from detectron2.engine.defaults import DefaultPredictor 10 | 11 | import sys 12 | sys.path.insert(1, "../build/msg/") 13 | from image_pb2 import * 14 | 15 | sys.path.insert(1, "../3rdparty/detectron2/projects/PointRend") 16 | import point_rend 17 | 18 | 19 | def create_cfg(config_file, weights_file, cpu=False): 20 | """Create configuration to obtain masks from detectron2 model 21 | :returns: TODO 22 | 23 | """ 24 | cfg = get_cfg() 25 | point_rend.add_pointrend_config(cfg) 26 | cfg.merge_from_file(config_file) 27 | 28 | cfg.MODEL.WEIGHTS = weights_file 29 | 30 | cfg.MODEL.RETINANET.SCORE_THRESH_TEST = 0.75 31 | cfg.MODEL.ROI_HEADS.SCORE_THRESH_TEST = 0.75 32 | cfg.MODEL.PANOPTIC_FPN.COMBINE.INSTANCES_CONFIDENCE_THRESH = 0.75 33 | 34 | if cpu or not torch.cuda.is_available(): 35 | cfg.MODEL.DEVICE = "cpu" 36 | cfg.freeze() 37 | 38 | return cfg 39 | 40 | def get_masks(predictions, image): 41 | """TODO: Docstring for get_masks. 42 | 43 | :arg1: TODO 44 | :returns: TODO 45 | 46 | """ 47 | if "instances" not in predictions: 48 | return 49 | 50 | instances = predictions["instances"] 51 | if not instances.has("pred_masks"): 52 | return 53 | 54 | if not instances.has("pred_boxes"): 55 | return 56 | 57 | if not instances.has("pred_features"): 58 | return 59 | 60 | mask = instances.pred_masks.cpu() 61 | # Convert from (N, H, W) to (H, W, N) tensor 62 | mask = mask.permute(1, 2, 0).numpy() 63 | 64 | features = instances.pred_features.cpu() 65 | features = features.permute(1, 0).numpy() 66 | 67 | dst_mask = np.zeros((image).shape[:2], dtype=np.uint16) 68 | # bitwise encode a max of 16 masks in 16-bit image 69 | for i in range(min(16, mask.shape[2])): 70 | current_mask = np.where(mask[:, :, i] > 0, 2**i, 0).astype(np.uint16) 71 | dst_mask = np.bitwise_or(dst_mask, current_mask) 72 | 73 | classes, scores, bboxes = [], [], [] 74 | if instances.has("pred_classes") and instances.has("scores"): 75 | if len(instances.pred_classes.cpu().numpy()) > 0: 76 | classes = instances.pred_classes.cpu().numpy() 77 | if len(instances.scores.cpu().numpy()) > 0: 78 | scores = instances.scores.cpu().numpy() 79 | if len(instances.pred_boxes.tensor.cpu().numpy()) > 0: 80 | bboxes = instances.pred_boxes.tensor.cpu().numpy() 81 | processed_image = dst_mask.astype(np.uint16) 82 | 83 | return processed_image, features, classes, scores, bboxes 84 | 85 | 86 | def main(): 87 | """TODO: Docstring for main. 88 | 89 | :args: TODO 90 | :returns: TODO 91 | 92 | """ 93 | context = zmq.Context() 94 | reply_sock = context.socket(zmq.REP) 95 | reply_sock.bind("tcp://*:5555") 96 | 97 | config_file = "../3rdparty/detectron2/projects/PointRend/configs/InstanceSegmentation/pointrend_rcnn_R_50_FPN_3x_coco.yaml" 98 | weights_file = "https://dl.fbaipublicfiles.com/detectron2/PointRend/InstanceSegmentation/pointrend_rcnn_R_50_FPN_3x_coco/164955410/model_final_3c3198.pkl" 99 | cfg = create_cfg(config_file, weights_file) 100 | predictor = DefaultPredictor(cfg) 101 | idx = 0 102 | while True: 103 | try: 104 | # Receiving image in bytes 105 | color_image = ColorImage() 106 | data = reply_sock.recv() 107 | color_image.ParseFromString(data) 108 | image_bytes = np.frombuffer(color_image.data, dtype=np.uint8) 109 | image = np.reshape(image_bytes, (color_image.height, color_image.width, color_image.num_channels)) 110 | image = image[20:-20, 20:-20] 111 | print(f"Processing request: {idx}") 112 | predictions = predictor(image) 113 | processed_image, features, classes, scores, bboxes = get_masks(predictions, image) 114 | processed_image = np.pad(processed_image, 20, 'constant') 115 | mask_image = MaskImage() 116 | mask_image.width = color_image.width 117 | mask_image.height = color_image.height 118 | mask_image.num_channels = 1 119 | mask_image.bytes_per_channel = 2 120 | mask_image.data = processed_image.tobytes() 121 | mask_image.labels.extend(classes) 122 | mask_image.scores.extend(scores) 123 | mask_image.features = features.tobytes() 124 | for bbox in bboxes: 125 | bbox = bbox + 20; 126 | boundingbox = mask_image.bboxes.add() 127 | boundingbox.coordinates.extend(bbox) 128 | 129 | reply_sock.send(mask_image.SerializeToString()) 130 | idx += 1 131 | except KeyboardInterrupt: 132 | print("Keyboard interrupt received stopping") 133 | reply_sock.close() 134 | context.term() 135 | break 136 | 137 | 138 | if __name__ == "__main__": 139 | main() 140 | -------------------------------------------------------------------------------- /python/main.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import os 3 | import argparse 4 | from tqdm import tqdm 5 | import multiprocessing as mp 6 | import torch 7 | 8 | from detectron2.config import get_cfg 9 | import sys 10 | sys.path.insert(1, "/home/akashsharma/Documents/projects/detectron2/projects/PointRend") 11 | import point_rend 12 | 13 | from pipeline.read_images import ReadImages 14 | from pipeline.predict import Predict 15 | from pipeline.store_masks import StoreMasks 16 | from pipeline.save_image import SaveImage 17 | 18 | 19 | def create_cfg(config_file, weights_file, cpu=False): 20 | """Create configuration to obtain masks from detectron2 model 21 | :returns: TODO 22 | 23 | """ 24 | cfg = get_cfg() 25 | point_rend.add_pointrend_config(cfg) 26 | cfg.merge_from_file(config_file) 27 | 28 | cfg.MODEL.WEIGHTS= weights_file 29 | 30 | 31 | cfg.MODEL.RETINANET.SCORE_THRESH_TEST = 0.6 32 | cfg.MODEL.ROI_HEADS.SCORE_THRESH_TEST = 0.6 33 | cfg.MODEL.PANOPTIC_FPN.COMBINE.INSTANCES_CONFIDENCE_THRESH = 0.6 34 | 35 | if cpu or not torch.cuda.is_available(): 36 | cfg.MODEL.DEVICE = "cpu" 37 | cfg.freeze() 38 | 39 | return cfg 40 | 41 | 42 | def create_arg_parser(): 43 | """TODO: Docstring for create_arg_parse. 44 | :returns: TODO 45 | 46 | """ 47 | parser = argparse.ArgumentParser(description="Process dataset to obtain instance masks") 48 | parser.add_argument("-i", "--input", required=True, help="path to dataset containing images") 49 | parser.add_argument("-o", "--output", default="../../output", help="path to processed dataset (default: ../../output)") 50 | 51 | # parser.add_argument("--config-file", default="/home/akashsharma/Documents/projects/detectron2/configs/quick_schedules/mask_rcnn_R_50_FPN_instant_test.yaml", 52 | # help="path to config file (default: home/akashsharma/Documents/projects/detectron2/configs/COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml)") 53 | parser.add_argument("--config-file", default="/home/akashsharma/Documents/projects/detectron2/projects/PointRend/configs/InstanceSegmentation/pointrend_rcnn_R_50_FPN_3x_coco.yaml", 54 | help="path to config file (default: home/akashsharma/Documents/projects/detectron2/configs/COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml)") 55 | 56 | 57 | # parser.add_argument("--weights-file", default="detectron2://COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x/137849600/model_final_f10217.pkl", 58 | # help="weights file for config (default: detectron2://COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x/137849600/model_final_f10217.pkl)") 59 | parser.add_argument("--weights-file", default="https://dl.fbaipublicfiles.com/detectron2/PointRend/InstanceSegmentation/pointrend_rcnn_R_50_FPN_3x_coco/164955410/model_final_3c3198.pkl", 60 | help="weights file for config (default: detectron2://COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x/137849600/model_final_f10217.pkl)") 61 | return parser 62 | 63 | def main(args): 64 | """TODO: Docstring for main. 65 | 66 | :args: TODO 67 | :returns: TODO 68 | 69 | """ 70 | # Create output directory if doesn't exist 71 | os.makedirs(args.output, exist_ok=True) 72 | cfg = create_cfg(args.config_file, args.weights_file) 73 | 74 | read_images = ReadImages(args.input) 75 | predict = Predict(cfg) 76 | store_masks = StoreMasks("mask") 77 | save_image = SaveImage("mask", args.output) 78 | 79 | pipeline = (read_images | predict | store_masks | save_image) 80 | try: 81 | for _ in tqdm(pipeline, desc="Processing"): 82 | pass 83 | except StopIteration: 84 | return 85 | except KeyboardInterrupt: 86 | return 87 | 88 | if __name__ == "__main__": 89 | parser = create_arg_parser() 90 | args = parser.parse_args() 91 | main(args) 92 | 93 | 94 | -------------------------------------------------------------------------------- /python/pipeline/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpl-cmu/object-slam/7baecfc995d4bbd6441585742df810e38568732f/python/pipeline/__init__.py -------------------------------------------------------------------------------- /python/pipeline/pipeline.py: -------------------------------------------------------------------------------- 1 | 2 | class Pipeline: 3 | """Pipeline baseclass to be inherited for all tasks""" 4 | 5 | def __init__(self, source=None): 6 | """TODO: to be defined. """ 7 | self.source = source 8 | 9 | def __iter__(self): 10 | return self.generator() 11 | 12 | def generator(self): 13 | """process and yields the next element in the iterator 14 | :returns: TODO 15 | 16 | """ 17 | while True: 18 | try: 19 | data = next(self.source) 20 | if self.filter(data): 21 | yield self.map(data) 22 | except StopIteration: 23 | return 24 | 25 | def __or__(self, other): 26 | """allows for pipelining tasks using | operator 27 | 28 | :object: pipeline object to be |'d 29 | :returns: TODO 30 | 31 | """ 32 | if other is not None: 33 | # post pipelining 34 | other.source = self.generator() 35 | return other 36 | else: 37 | return self 38 | 39 | def filter(self, data): 40 | """TODO: Docstring for filter. 41 | 42 | :arg1: TODO 43 | :returns: TODO 44 | 45 | """ 46 | return True 47 | 48 | def map(self, data): 49 | """overwrite to map the data 50 | 51 | :data: TODO 52 | :returns: TODO 53 | 54 | """ 55 | return data 56 | 57 | 58 | -------------------------------------------------------------------------------- /python/pipeline/predict.py: -------------------------------------------------------------------------------- 1 | from pipeline.pipeline import Pipeline 2 | 3 | from detectron2.engine.defaults import DefaultPredictor 4 | 5 | class Predict(Pipeline): 6 | 7 | """Pipeline task to perform inference""" 8 | 9 | def __init__(self, cfg): 10 | """ 11 | :cfg: TODO 12 | """ 13 | self.predictor = DefaultPredictor(cfg) 14 | Pipeline.__init__(self) 15 | 16 | def map(self, data): 17 | """ 18 | 19 | :data: TODO 20 | :returns: TODO 21 | 22 | """ 23 | image = data["image"] 24 | predictions = self.predictor(image) 25 | data["predictions"] = predictions 26 | 27 | return data 28 | -------------------------------------------------------------------------------- /python/pipeline/read_images.py: -------------------------------------------------------------------------------- 1 | import os 2 | import glob 3 | from itertools import chain 4 | import cv2 5 | 6 | from pipeline.pipeline import Pipeline 7 | 8 | 9 | class ReadImages(Pipeline): 10 | """ReadImages pipeline task: read the image with valid extension from a folder structure, and return an iterable image""" 11 | def __init__(self, path, valid_exts=[".png", ".jpg"]): 12 | """TODO: to be defined. 13 | 14 | :valid_exts: TODO 15 | :".jpg"]: TODO 16 | :: TODO 17 | 18 | """ 19 | self.path = path 20 | self.valid_exts = valid_exts 21 | 22 | Pipeline.__init__(self) 23 | 24 | def generator(self): 25 | """ Yield iterable file after reading from folder structure 26 | :returns: TODO 27 | 28 | """ 29 | 30 | filelist = (chain.from_iterable(chain.from_iterable(glob.glob(os.path.join(x[0], "*.color") + ext) 31 | for ext in self.valid_exts) 32 | for x in os.walk(self.path))) 33 | 34 | for filename in filelist: 35 | image_file = filename 36 | image = cv2.imread(image_file) 37 | data = { 38 | 'image_id': image_file, 39 | 'image': image 40 | } 41 | if self.filter(data): 42 | yield self.map(data) 43 | -------------------------------------------------------------------------------- /python/pipeline/save_image.py: -------------------------------------------------------------------------------- 1 | import os 2 | import cv2 3 | 4 | from pipeline.pipeline import Pipeline 5 | 6 | class SaveImage(Pipeline): 7 | 8 | """Docstring for SaveImage. """ 9 | 10 | def __init__(self, src, path): 11 | """TODO: to be defined. 12 | 13 | :dst: TODO 14 | 15 | """ 16 | self.src = src 17 | self.path = path 18 | Pipeline.__init__(self) 19 | 20 | def map(self, data): 21 | """TODO: Docstring for map. 22 | 23 | :data: TODO 24 | :returns: TODO 25 | 26 | """ 27 | image = data[self.src] 28 | image_id = data["image_id"] 29 | 30 | output = image_id.split(os.path.sep) 31 | dirname = output[:-1] 32 | if len(dirname) > 0: 33 | dirname = os.path.join(*dirname) 34 | dirname = os.path.join(self.path, dirname) 35 | os.makedirs(dirname, exist_ok=True) 36 | else: 37 | dirname = self.path 38 | img_filename = f"{output[-1].rsplit('.', 1)[0]}.png" 39 | txt_filename = f"{output[-1].rsplit('.', 1)[0]}.txt" 40 | img_path = os.path.join(dirname, img_filename) 41 | 42 | cv2.imwrite(img_path, image) 43 | 44 | # if "classes" not in data: 45 | # return data 46 | 47 | txt_path = os.path.join(dirname, txt_filename) 48 | 49 | with open(txt_path, "w") as txt_file: 50 | if "classes" not in data: 51 | return data 52 | for i, c in enumerate(data["classes"]): 53 | p = data["scores"][i] 54 | txt_file.write(f"{c},{p}\n") 55 | return data 56 | -------------------------------------------------------------------------------- /python/pipeline/store_masks.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from pipeline.pipeline import Pipeline 4 | 5 | class StoreMasks(Pipeline): 6 | 7 | """Docstring for StoreMasks. """ 8 | 9 | def __init__(self, dst): 10 | """TODO: to be defined. """ 11 | self.dst = dst 12 | self.dst_classes = "classes" 13 | self.dst_scores = "scores" 14 | self.NUM_MASKS = 16 15 | Pipeline.__init__(self) 16 | 17 | def map(self, data): 18 | """TODO: Docstring for map. 19 | 20 | :data: TODO 21 | :returns: TODO 22 | 23 | """ 24 | self.store_mask(data) 25 | 26 | return data 27 | 28 | def store_mask(self, data): 29 | """TODO: Docstring for store_mask. 30 | 31 | :data: TODO 32 | :returns: TODO 33 | 34 | """ 35 | if "predictions" not in data: 36 | return 37 | 38 | predictions = data["predictions"] 39 | if "instances" not in predictions: 40 | return 41 | 42 | instances = predictions["instances"] 43 | if not instances.has("pred_masks"): 44 | return 45 | 46 | mask = instances.pred_masks.cpu() 47 | mask = mask.permute(1, 2, 0).numpy() 48 | 49 | dst_mask = np.zeros((data["image"]).shape[:2], dtype=np.uint16) 50 | # bitwise encode a max of 16 masks in 16-bit image 51 | for i in range(min(self.NUM_MASKS, mask.shape[2])): 52 | current_mask = np.where(mask[:, :, i] > 0, 2**i, 0).astype(np.uint16) 53 | dst_mask = np.bitwise_or(dst_mask, current_mask) 54 | 55 | if instances.has("pred_classes") and instances.has("scores"): 56 | if len(instances.pred_classes.cpu().numpy()) > 0: 57 | data[self.dst_classes] = instances.pred_classes.cpu().numpy() 58 | if len(instances.scores.cpu().numpy()) > 0: 59 | data[self.dst_scores] = instances.scores.cpu().numpy() 60 | # plt.imshow(dst_mask, cmap="gray", vmin=0, vmax=4096) 61 | # plt.show() 62 | data[self.dst] = dst_mask.astype(np.uint16) 63 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(object-slam) 2 | 3 | add_executable( 4 | reconstruct 5 | ${CMAKE_CURRENT_SOURCE_DIR}/main.cpp 6 | ) 7 | 8 | add_executable( 9 | show_objects 10 | ${CMAKE_CURRENT_SOURCE_DIR}/show-objects.cpp 11 | ) 12 | 13 | target_link_libraries( 14 | reconstruct 15 | PRIVATE 16 | ${PROJECT_NAME} 17 | ) 18 | 19 | target_link_libraries( 20 | show_objects 21 | PRIVATE 22 | ${PROJECT_NAME} 23 | ) 24 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: main.cpp 3 | * 4 | * Author: Akash Sharma 5 | * Created: 04/07/20 6 | * Description: main 7 | *****************************************************************************/ 8 | 9 | #ifndef NDEBUG 10 | #define SPDLOG_ACTIVE_LEVEL SPDLOG_LEVEL_TRACE 11 | #define SPDLOG_DEBUG_ON 12 | #define SPDLOG_TRACE_ON 13 | #endif 14 | 15 | #include 16 | 17 | #include 18 | 19 | #include "object-slam/utils/types.h" 20 | #include "object-slam/controller/controller.h" 21 | 22 | int main(int argc, char* argv[]) 23 | { 24 | CLI::App app{ "Object SLAM" }; 25 | spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] [thread: %t] %^[%L] %v%$"); 26 | std::string dataset_path; 27 | app.add_option("dataset_path", dataset_path, "Path to the dataset")->required(); 28 | 29 | int type; 30 | app.add_set("--dataset_type", type, {1, 2}, "Dataset type: 1 -> TUM, 1 -> RGBD_SCENES")->required(); 31 | app.add_flag( 32 | "-d, --debug", 33 | [](size_t /*unused*/) { 34 | #ifndef NDEBUG 35 | spdlog::set_level(spdlog::level::trace); 36 | #else 37 | spdlog::set_level(spdlog::level::info); 38 | #endif 39 | }, 40 | "Print debug logs"); 41 | 42 | spdlog::set_error_handler([](const std::string& msg) -> void { 43 | std::cerr << "Error in SPDLOG: " << msg << std::endl; 44 | raise(SIGABRT); 45 | }); 46 | CLI11_PARSE(app, argc, argv); 47 | 48 | oslam::DatasetType dataset_type = static_cast(type); 49 | spdlog::info("Dataset type: {} {}", type, dataset_type); 50 | 51 | oslam::Controller controller(dataset_path, dataset_type); 52 | 53 | if (controller.start()) 54 | { 55 | return EXIT_SUCCESS; 56 | } 57 | return EXIT_FAILURE; 58 | } 59 | -------------------------------------------------------------------------------- /src/object-slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(${PROJECT_SOURCE_DIR}/cmake/Functions.cmake) 2 | # --------- Dependencies ----------------------------------------- 3 | 4 | # include Open3D library by downloading if not already present 5 | include(${PROJECT_SOURCE_DIR}/cmake/Open3D.cmake) 6 | # find_package(Open3D REQUIRED 7 | # COMPONENTS Open3DCuda) 8 | 9 | # --------- Create object-slam library ---------------------------- 10 | add_library(${PROJECT_NAME}) 11 | 12 | add_subdirectory(utils) 13 | add_subdirectory(struct) 14 | add_subdirectory(controller) 15 | add_subdirectory(reader) 16 | add_subdirectory(module) 17 | 18 | # Treat Eigen as system library to suppress warnings 19 | target_include_directories( 20 | ${PROJECT_NAME} PUBLIC 21 | ${PROJECT_SOURCE_DIR}/src/ 22 | SYSTEM PUBLIC 23 | ${EIGEN3_INCLUDE_DIR} 24 | ${CMAKE_BINARY_DIR} 25 | ) 26 | 27 | target_link_libraries_system( 28 | ${PROJECT_NAME} PUBLIC 29 | project_options 30 | project_warnings 31 | proto 32 | CONAN_PKG::CLI11 33 | CONAN_PKG::fmt 34 | CONAN_PKG::spdlog 35 | CONAN_PKG::cppzmq 36 | CONAN_PKG::protobuf 37 | CONAN_PKG::xtensor 38 | Threads::Threads 39 | Boost::filesystem 40 | opencv_core 41 | opencv_highgui 42 | opencv_imgproc 43 | opencv_rgbd 44 | Open3D::Open3D 45 | Open3D::Open3DCuda 46 | gtsam 47 | ) 48 | 49 | # target_link_libraries(${PROJECT_NAME} PUBLIC Open3DCuda) 50 | 51 | # TODO: Is this Required? 52 | if(WITH_OPENMP) 53 | find_package(OpenMP) 54 | if(OpenMP_CXX_FOUND) 55 | target_link_libraries( 56 | ${PROJECT_NAME} PUBLIC 57 | OpenMP::OpenMP_CXX 58 | ) 59 | endif() 60 | endif() 61 | -------------------------------------------------------------------------------- /src/object-slam/controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources( 2 | ${PROJECT_NAME} 3 | PRIVATE 4 | controller.h 5 | controller.cpp 6 | ) 7 | -------------------------------------------------------------------------------- /src/object-slam/controller/controller.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: controller.cpp 3 | * 4 | * Author: Akash Sharma 5 | * Created: 04/13/20 6 | * Description: Main loop for object-slam 7 | *****************************************************************************/ 8 | #include "controller.h" 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include "object-slam/struct/map.h" 18 | 19 | namespace oslam 20 | { 21 | Controller::Controller(std::string dataset_path, DatasetType dataset_type) 22 | : transport_input_queue_("TransportFrameQueue"), 23 | transport_output_queue_("MaskedImageQueue"), 24 | tracker_output_queue_("TrackerOutputQueue"), 25 | renderer_input_queue_("RendererInputQueue") 26 | { 27 | spdlog::trace("CONSTRUCT: Controller"); 28 | spdlog::info("Thread ({}, {}) started", "ControllerThread", std::this_thread::get_id()); 29 | 30 | //! TODO: Required? 31 | CheckCuda(cudaSetDevice(0)); 32 | 33 | map_ = std::make_shared(); 34 | data_reader_ = std::make_shared(std::move(dataset_path), std::move(dataset_type)); 35 | image_transport_ = std::make_shared(&transport_input_queue_, &transport_output_queue_); 36 | tracker_ = std::make_shared(&tracker_output_queue_); 37 | renderer_ = std::make_shared(map_, &renderer_input_queue_); 38 | display_ = std::make_shared("Object SLAM"); 39 | mapper_ = 40 | std::make_shared(map_, &tracker_output_queue_, &transport_output_queue_, &renderer_input_queue_); 41 | } 42 | 43 | bool Controller::start() 44 | { 45 | spdlog::trace("Controller::start()"); 46 | std::string figlet = 47 | R"( 48 | ____ __ _ __ ______ ___ __ ___ 49 | / __ \/ / (_)__ ____/ /_/ __/ / / _ | / |/ / 50 | / /_/ / _ \ / / -_) __/ __/\ \/ /__/ __ |/ /|_/ / 51 | \____/_.__/_/ /\__/\__/\__/___/____/_/ |_/_/ /_/ 52 | |___/ 53 | )"; 54 | std::cout << figlet << "\n"; 55 | //! TODO: Add license 56 | if (setup()) 57 | { 58 | run(); 59 | return true; 60 | } 61 | return false; 62 | } 63 | 64 | void Controller::shutdown() 65 | { 66 | // TODO(Akash): Check whether the modules are running / queues are empty, etc. 67 | spdlog::trace("Controller::shutdown()"); 68 | data_reader_->shutdown(); 69 | image_transport_->shutdown(); 70 | tracker_->shutdown(); 71 | mapper_->shutdown(); 72 | renderer_->shutdown(); 73 | display_->shutdown(); 74 | map_->shutdown(); 75 | } 76 | 77 | bool Controller::setup() 78 | { 79 | spdlog::trace("Controller::setup()"); 80 | data_reader_->registerShutdownCallback([this](Timestamp timestamp) { tracker_->setMaxTimestamp(timestamp); }); 81 | data_reader_->registerShutdownCallback([this](Timestamp timestamp) { mapper_->setMaxTimestamp(timestamp); }); 82 | data_reader_->registerShutdownCallback([this](Timestamp timestamp) { display_->setMaxTimestamp(timestamp); }); 83 | 84 | 85 | //! DataReader fills the ImageTransporter Input Queue 86 | auto& transport_frame_queue = transport_input_queue_; 87 | data_reader_->registerOutputCallback([&transport_frame_queue](const Frame::Ptr& frame) { 88 | if (frame->is_keyframe_) 89 | { 90 | // Explicitly copy the shared frame memory 91 | transport_frame_queue.push(std::make_unique(*frame)); 92 | } 93 | }); 94 | //! DataReader also fills the Tracker Frame Queue and Display Frame Queue 95 | data_reader_->registerOutputCallback( 96 | [this](Frame::Ptr frame) { tracker_->fillFrameQueue(std::make_unique(*frame)); }); 97 | data_reader_->registerOutputCallback( 98 | [this](Frame::Ptr frame) { display_->fillFrameQueue(std::make_unique(*frame)); }); 99 | 100 | renderer_->registerOutputCallback([this](const RendererOutput::Ptr& render_payload) { 101 | tracker_->fillModelQueue(std::make_unique(render_payload->timestamp_, 102 | render_payload->model_render_.color_map_, 103 | render_payload->model_render_.vertex_map_, 104 | render_payload->model_render_.normal_map_)); 105 | }); 106 | renderer_->registerOutputCallback([this](const RendererOutput::Ptr& render_payload) { 107 | mapper_->fillRendersQueue(std::make_unique(render_payload->timestamp_, render_payload->object_renders_)); 108 | }); 109 | 110 | renderer_->registerOutputCallback([this](const RendererOutput::Ptr& render_payload) { 111 | std::vector display_img; 112 | display_->fillDisplay3dQueue( 113 | std::make_unique(render_payload->timestamp_, display_img, render_payload->widgets_map_)); 114 | }); 115 | 116 | return (data_reader_ && image_transport_ && tracker_ && mapper_ && renderer_); 117 | } 118 | 119 | bool Controller::shutdownWhenComplete() 120 | { 121 | spdlog::trace("Controller::shutdownWhenComplete()"); 122 | // clang-format off 123 | while (!shutdown_ && 124 | ((image_transport_->isWorking() || (!transport_input_queue_.isShutdown() && !transport_input_queue_.empty())) || 125 | (tracker_->isWorking() || (!transport_output_queue_.isShutdown() && !transport_output_queue_.empty())) || 126 | (mapper_->isWorking() || (!tracker_output_queue_.isShutdown() && !tracker_output_queue_.empty())) || 127 | (renderer_->isWorking() || (!renderer_input_queue_.isShutdown() && !renderer_input_queue_.empty())) 128 | ) 129 | ) 130 | { 131 | spdlog::trace("\n" 132 | "shutdown_ : {}\n" 133 | "image_transport_ working : {}\n" 134 | "transport_input_queue_ shutdown : {}\n" 135 | "transport_input_queue_ empty : {}\n" 136 | "tracker_ working : {}\n" 137 | "transport_output_queue_ shutdown : {}\n" 138 | "transport_output_queue_ empty : {}\n" 139 | "mapper_ working : {}\n" 140 | "tracker_output_queue_ shutdown : {}\n" 141 | "tracker_output_queue_ empty : {}", 142 | shutdown_, 143 | image_transport_->isWorking(), 144 | transport_input_queue_.isShutdown(), 145 | transport_input_queue_.empty(), 146 | tracker_->isWorking(), 147 | transport_output_queue_.isShutdown(), 148 | transport_output_queue_.empty(), 149 | mapper_->isWorking(), 150 | tracker_output_queue_.isShutdown(), 151 | tracker_output_queue_.empty()); 152 | 153 | static constexpr int half_s = 500; 154 | std::chrono::milliseconds sleep_duration{ half_s }; 155 | std::this_thread::sleep_for(sleep_duration); 156 | } 157 | spdlog::trace("\n" 158 | "shutdown_ : {}\n" 159 | "image_transport_ working : {}\n" 160 | "transport_input_queue_ shutdown : {}\n" 161 | "transport_input_queue_ empty : {}\n" 162 | "tracker_ working : {}\n" 163 | "transport_output_queue_ shutdown : {}\n" 164 | "transport_output_queue_ empty : {}\n" 165 | "mapper_ working : {}\n" 166 | "tracker_output_queue_ shutdown : {}\n" 167 | "tracker_output_queue_ empty : {}", 168 | shutdown_, 169 | image_transport_->isWorking(), 170 | transport_input_queue_.isShutdown(), 171 | transport_input_queue_.empty(), 172 | tracker_->isWorking(), 173 | transport_output_queue_.isShutdown(), 174 | transport_output_queue_.empty(), 175 | mapper_->isWorking(), 176 | tracker_output_queue_.isShutdown(), 177 | tracker_output_queue_.empty()); 178 | // clang-format on 179 | if (!shutdown_) 180 | { 181 | shutdown(); 182 | shutdown_ = true; 183 | } 184 | return true; 185 | } 186 | 187 | void Controller::run() 188 | { 189 | // Start thread for each component 190 | auto handle_dataset = std::async(std::launch::async, &oslam::DataReader::run, data_reader_); 191 | auto handle_image_transport = std::async(std::launch::async, &oslam::ImageTransporter::run, image_transport_); 192 | auto handle_tracker = std::async(std::launch::async, &oslam::Tracker::run, tracker_); 193 | auto handle_mapper = std::async(std::launch::async, &oslam::Mapper::run, mapper_); 194 | auto handle_renderer = std::async(std::launch::async, &oslam::Renderer::run, renderer_); 195 | auto handle_shutdown = std::async(std::launch::async, &oslam::Controller::shutdownWhenComplete, this); 196 | 197 | bool handle_display = display_->run(); 198 | if(!handle_display) 199 | { 200 | spdlog::trace("Shutting down entire pipeline..."); 201 | shutdown_ = true; 202 | shutdown(); 203 | } 204 | 205 | try 206 | { 207 | spdlog::info("Dataset reader successful: {}", handle_dataset.get()); 208 | spdlog::info("Shutdown successful: {}", handle_shutdown.get()); 209 | spdlog::info("Image Transporter successful: {}", handle_image_transport.get()); 210 | spdlog::info("Tracker successful: {}", handle_tracker.get()); 211 | spdlog::info("Mapper successful: {}", handle_mapper.get()); 212 | spdlog::info("Renderer successful: {}", handle_renderer.get()); 213 | } 214 | catch (std::exception& ex) 215 | { 216 | spdlog::error("{}", ex.what()); 217 | assert(false); 218 | } 219 | } 220 | 221 | } // namespace oslam 222 | -------------------------------------------------------------------------------- /src/object-slam/controller/controller.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: controller.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 04/13/20 6 | * Description: Main loop for object-slam 7 | *****************************************************************************/ 8 | #ifndef OSLAM_CONTROLLER_H 9 | #define OSLAM_CONTROLLER_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "object-slam/struct/map.h" 18 | #include "object-slam/utils/types.h" 19 | #include "object-slam/utils/macros.h" 20 | 21 | #include "object-slam/module/image_transport.h" 22 | #include "object-slam/module/tracker.h" 23 | #include "object-slam/module/mapper.h" 24 | #include "object-slam/module/renderer.h" 25 | #include "object-slam/module/display.h" 26 | #include "object-slam/reader/data_provider.h" 27 | #include "object-slam/reader/data_reader.h" 28 | 29 | namespace oslam 30 | { 31 | /*! \class Controller 32 | * \brief Creates and manages all the submodules in the SLAM System 33 | */ 34 | class Controller 35 | { 36 | public: 37 | OSLAM_DELETE_COPY_CONSTRUCTORS(Controller); 38 | OSLAM_DELETE_MOVE_CONSTRUCTORS(Controller); 39 | 40 | explicit Controller(std::string dataset_path, DatasetType dataset_type); 41 | virtual ~Controller() = default; 42 | 43 | bool start(); 44 | 45 | private: 46 | bool setup(); 47 | void run(); 48 | bool shutdownWhenComplete(); 49 | void shutdown(); 50 | 51 | std::atomic_bool shutdown_ = { false }; 52 | 53 | std::shared_ptr map_; 54 | 55 | DataReader::Ptr data_reader_; 56 | DataProvider::Ptr data_provider_; 57 | ImageTransporter::Ptr image_transport_; 58 | Tracker::Ptr tracker_; 59 | Mapper::Ptr mapper_; 60 | Renderer::Ptr renderer_; 61 | Display::Ptr display_; 62 | 63 | ImageTransporter::InputQueue transport_input_queue_; 64 | ImageTransporter::OutputQueue transport_output_queue_; 65 | Tracker::OutputQueue tracker_output_queue_; 66 | Renderer::InputQueue renderer_input_queue_; 67 | }; 68 | } // namespace oslam 69 | #endif /* ifndef OSLAM_CONTROLLER_H */ 70 | -------------------------------------------------------------------------------- /src/object-slam/module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources( 2 | ${PROJECT_NAME} 3 | PRIVATE 4 | image_transport.h 5 | image_transport.cpp 6 | tracker.h 7 | tracker.cpp 8 | mapper.h 9 | mapper.cpp 10 | renderer.h 11 | renderer.cpp 12 | display.h 13 | display.cpp 14 | ) 15 | -------------------------------------------------------------------------------- /src/object-slam/module/display.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: display.cpp 3 | * 4 | * Author: Akash Sharma 5 | * Created: 09/01/20 6 | * Description: Implementation of display 7 | *****************************************************************************/ 8 | #include "display.h" 9 | #include 10 | 11 | #include "object-slam/payload/display_payload.h" 12 | namespace oslam 13 | { 14 | Display::Display(std::string window_name) 15 | : MISO(nullptr, "Display"), 16 | window_name_(std::move(window_name)), 17 | window_3d_(window_name + " map"), 18 | background_color_(), 19 | frame_queue_("DisplayFrameQueue"), 20 | display_3d_queue_("Display3dQueue"), 21 | display_input_queue_("DisplayInputQueue") 22 | { 23 | spdlog::trace("Display::Display()"); 24 | cv::startWindowThread(); 25 | #ifndef NDEBUG 26 | window_3d_.setGlobalWarnings(true); 27 | #else 28 | window_3d_.setGlobalWarnings(false); 29 | #endif 30 | window_3d_.setBackgroundColor(background_color_); 31 | window_3d_.showWidget("Camera origin", cv::viz::WCameraPosition(0.25)); 32 | window_3d_.setViewerPose(cv::Affine3d::Identity()); 33 | } 34 | 35 | Display::~Display() { } 36 | 37 | Display::InputUniquePtr Display::getInputPacket() 38 | { 39 | spdlog::trace("Display::getInputPacket()"); 40 | 41 | Frame::UniquePtr input_frame; 42 | bool queue_state = frame_queue_.popBlocking(input_frame); 43 | if (!input_frame || !queue_state) 44 | { 45 | spdlog::error("Module: {} {} returned null", name_id_, frame_queue_.queue_id_); 46 | return nullptr; 47 | } 48 | curr_timestamp_ = input_frame->timestamp_; 49 | 50 | Display::InputUniquePtr display_input; 51 | 52 | display_input = std::make_unique(curr_timestamp_); 53 | display_input->display_images_.emplace_back("Input Color Frame", input_frame->color_); 54 | cv::Mat scaled_depth; 55 | cv::convertScaleAbs(input_frame->depth_, scaled_depth, 0.25 * 256.0 / 1000); 56 | display_input->display_images_.emplace_back("Input Depth Frame", scaled_depth); 57 | 58 | if (curr_timestamp_ == 1) 59 | { 60 | spdlog::debug("Display input only contains input frame"); 61 | } 62 | else 63 | { 64 | DisplayInput::UniquePtr display_3d; 65 | //! TODO: Add color map to display etc and mesh 66 | if (!syncQueue(curr_timestamp_, &display_3d_queue_, &display_3d)) 67 | { 68 | spdlog::error("Missing 3D display widget for requested timestamp: {}", curr_timestamp_); 69 | return nullptr; 70 | } 71 | if (!display_3d) 72 | { 73 | spdlog::error("Module: {} {} returned null", name_id_, display_3d_queue_.queue_id_); 74 | } 75 | display_input->display_images_.insert(display_input->display_images_.end(), 76 | display_3d->display_images_.begin(), 77 | display_3d->display_images_.end()); 78 | 79 | display_input->widgets_map_ = display_3d->widgets_map_; 80 | } 81 | return display_input; 82 | } 83 | 84 | Display::OutputUniquePtr Display::runOnce(InputUniquePtr input) 85 | { 86 | spdlog::trace("Display::runOnce()"); 87 | auto start_time = Timer::tic(); 88 | const std::vector& images_to_display = input->display_images_; 89 | const WidgetsMap& widgets_map = input->widgets_map_; 90 | show2dWindow(images_to_display); 91 | show3dWindow(widgets_map); 92 | spdlog::info("Display took {} ms", Timer::toc(start_time).count()); 93 | return nullptr; 94 | } 95 | 96 | void Display::show2dWindow(const std::vector& images_to_display) const 97 | { 98 | spdlog::trace("Display::show2dWindow()"); 99 | for (const auto& curr_img : images_to_display) 100 | { 101 | cv::namedWindow(curr_img.name_); 102 | cv::imshow(curr_img.name_, curr_img.image_); 103 | } 104 | } 105 | 106 | void Display::show3dWindow(const WidgetsMap& widgets_map) 107 | { 108 | spdlog::trace("Display::show3dWindow()"); 109 | if(window_3d_.wasStopped()) 110 | { 111 | //! TODO: Callback to shutdown entire pipeline! 112 | spdlog::trace("Stopping entire pipeline"); 113 | shutdown(); 114 | } 115 | window_3d_.removeAllWidgets(); 116 | for(auto it = widgets_map.begin(); it != widgets_map.end(); ++it) 117 | { 118 | spdlog::debug("Showed widget: {}", it->first); 119 | window_3d_.showWidget(it->first, *(it->second), it->second->getPose()); 120 | } 121 | window_3d_.spinOnce(1, true); 122 | } 123 | 124 | void Display::shutdownQueues() 125 | { 126 | spdlog::trace("Display::shutDownQueues"); 127 | MISOPipelineModule::shutdownQueues(); 128 | frame_queue_.shutdown(); 129 | display_3d_queue_.shutdown(); 130 | display_input_queue_.shutdown(); 131 | } 132 | 133 | } // namespace oslam 134 | -------------------------------------------------------------------------------- /src/object-slam/module/display.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: display.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 09/01/20 6 | * Description: Display 3D and 2D data 7 | *****************************************************************************/ 8 | #ifndef OSLAM_DISPLAY_H 9 | #define OSLAM_DISPLAY_H 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "object-slam/payload/display_payload.h" 16 | #include "object-slam/payload/renderer_payload.h" 17 | #include "object-slam/struct/frame.h" 18 | #include "object-slam/utils/macros.h" 19 | #include "object-slam/utils/pipeline_module.h" 20 | #include "object-slam/utils/thread_safe_queue.h" 21 | 22 | namespace oslam 23 | { 24 | /*! \class Display 25 | * \brief Brief class description 26 | * 27 | * Detailed description 28 | */ 29 | class Display : public MISOPipelineModule 30 | { 31 | public: 32 | OSLAM_POINTER_TYPEDEFS(Display); 33 | OSLAM_DELETE_COPY_CONSTRUCTORS(Display); 34 | OSLAM_DELETE_MOVE_CONSTRUCTORS(Display); 35 | 36 | using MISO = MISOPipelineModule; 37 | using DisplayInputQueue = ThreadsafeQueue; 38 | using FrameInputQueue = ThreadsafeQueue; 39 | 40 | explicit Display(std::string window_name); 41 | virtual ~Display(); 42 | 43 | virtual OutputUniquePtr runOnce(InputUniquePtr input) override; 44 | 45 | inline void fillFrameQueue(Frame::UniquePtr frame) { frame_queue_.push(std::move(frame)); } 46 | inline void fillDisplay3dQueue(DisplayInput::UniquePtr display_input) 47 | { 48 | display_3d_queue_.push(std::move(display_input)); 49 | } 50 | 51 | virtual bool hasWork() const override { return (curr_timestamp_ < max_timestamp_); } 52 | inline void setMaxTimestamp(Timestamp timestamp) { max_timestamp_ = timestamp; } 53 | 54 | private: 55 | virtual InputUniquePtr getInputPacket() override; 56 | 57 | virtual void shutdownQueues() override; 58 | 59 | void show2dWindow(const std::vector& images_to_display) const; 60 | void show3dWindow(const WidgetsMap& widgets_map); 61 | 62 | Timestamp curr_timestamp_ = 0; 63 | Timestamp max_timestamp_ = std::numeric_limits::max(); 64 | const std::string window_name_; 65 | cv::viz::Viz3d window_3d_; //! 3D visualization window 66 | cv::viz::Color background_color_; 67 | 68 | FrameInputQueue frame_queue_; 69 | DisplayInputQueue display_3d_queue_; 70 | DisplayInputQueue display_input_queue_; 71 | }; 72 | } // namespace oslam 73 | #endif /* ifndef OSLAM_DISPLAY_H */ 74 | -------------------------------------------------------------------------------- /src/object-slam/module/image_transport.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: image_transport.cpp 3 | * 4 | * Author: Akash Sharma 5 | * Created: 04/11/20 6 | * Description: ImageTransporter Module implementation 7 | *****************************************************************************/ 8 | #include "image_transport.h" 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace oslam 23 | { 24 | ImageTransporter::ImageTransporter(SISO::InputQueue* input_queue, SISO::OutputQueue* output_queue) 25 | : SISO(input_queue, output_queue, "ImageTransporter"), req_sock_(context_, ZMQ_REQ) 26 | { 27 | spdlog::debug("CONSTRUCT: ImageTransporter"); 28 | req_sock_.connect(SERVER_ENDPOINT); 29 | spdlog::info("Connected C++ client to ENDPOINT: {}", SERVER_ENDPOINT); 30 | } 31 | 32 | ImageTransportOutput::UniquePtr ImageTransporter::runOnce(Frame::UniquePtr frame) 33 | { 34 | if (frame) 35 | { 36 | return process(std::move(frame)); 37 | } 38 | return nullptr; 39 | } 40 | 41 | ImageTransportOutput::UniquePtr ImageTransporter::process(Frame::UniquePtr frame) 42 | { 43 | std::string str = serializeFrame(*frame); 44 | Timestamp curr_timestamp = frame->timestamp_; 45 | 46 | zmq::message_t message(str.c_str(), str.length()); 47 | req_sock_.send(message, zmq::send_flags::dontwait); 48 | spdlog::debug("Send request from client"); 49 | zmq::message_t recv_msg; 50 | oslam::MaskImage mask_pbuf; 51 | 52 | // Blocking call. 53 | auto success = req_sock_.recv(recv_msg); 54 | if (success) 55 | { 56 | mask_pbuf.ParseFromString(recv_msg.to_string()); 57 | ImageTransportOutput::UniquePtr output = deserializeFrame(curr_timestamp, mask_pbuf); 58 | if (output) 59 | return output; 60 | } 61 | return nullptr; 62 | } 63 | 64 | std::string ImageTransporter::serializeFrame(const Frame& frame) 65 | { 66 | oslam::ColorImage image_pbuf; 67 | image_pbuf.set_width(frame.width_); // 640 68 | image_pbuf.set_height(frame.height_); // 480 69 | image_pbuf.set_num_channels(frame.color_.channels()); 70 | image_pbuf.set_bytes_per_channel(1); // CV_8UC3 71 | image_pbuf.set_data(std::string(frame.color_.datastart, frame.color_.dataend)); 72 | 73 | std::string str; 74 | image_pbuf.SerializeToString(&str); 75 | return str; 76 | } 77 | 78 | ImageTransportOutput::UniquePtr ImageTransporter::deserializeFrame(const Timestamp timestamp, 79 | const oslam::MaskImage& mask_pbuf) 80 | { 81 | std::string input_data(mask_pbuf.data()); 82 | cv::Mat image(mask_pbuf.height(), mask_pbuf.width(), CV_16UC1, input_data.data()); 83 | 84 | std::string input_features(mask_pbuf.features()); 85 | cv::Mat feature_vector(1024, mask_pbuf.labels_size(), CV_32FC1, input_features.data()); 86 | ImageTransportOutput::UniquePtr output = std::make_unique(timestamp); 87 | for (int i = 0; i < mask_pbuf.labels_size(); i++) 88 | { 89 | unsigned int curr_label = mask_pbuf.labels(i); 90 | double curr_score = mask_pbuf.scores(i); 91 | BoundingBox curr_bbox = { static_cast(mask_pbuf.bboxes(i).coordinates(0)), 92 | static_cast(mask_pbuf.bboxes(i).coordinates(1)), 93 | static_cast(mask_pbuf.bboxes(i).coordinates(2)), 94 | static_cast(mask_pbuf.bboxes(i).coordinates(3)) }; 95 | cv::Mat curr_mask; 96 | cv::bitwise_and(image, static_cast(1U << i), curr_mask); 97 | curr_mask = (curr_mask >= 1); 98 | 99 | Feature feature = feature_vector.col(i); 100 | output->instance_images_.emplace_back(curr_mask, curr_bbox, curr_label, curr_score, feature); 101 | } 102 | return output; 103 | } 104 | } // namespace oslam 105 | -------------------------------------------------------------------------------- /src/object-slam/module/image_transport.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: image_transport.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 04/11/20 6 | * Description: Transport image to and fro between cpp and python 7 | *****************************************************************************/ 8 | #ifndef OSLAM_IMAGE_TRANSPORT_H 9 | #define OSLAM_IMAGE_TRANSPORT_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "object-slam/utils/macros.h" 19 | #include "object-slam/utils/pipeline_module.h" 20 | #include "object-slam/utils/thread_safe_queue.h" 21 | 22 | #include "object-slam/struct/frame.h" 23 | #include "object-slam/struct/instance_image.h" 24 | 25 | static constexpr auto SERVER_ENDPOINT = "tcp://localhost:5555"; 26 | 27 | namespace oslam 28 | { 29 | /*! \class ImageTransporter 30 | * \brief Transport frame object to Python process running PointRend inference for segmentation 31 | */ 32 | class ImageTransporter : public SISOPipelineModule 33 | { 34 | public: 35 | OSLAM_POINTER_TYPEDEFS(ImageTransporter); 36 | OSLAM_DELETE_COPY_CONSTRUCTORS(ImageTransporter); 37 | using SISO = SISOPipelineModule; 38 | 39 | explicit ImageTransporter(SISO::InputQueue* input_queue, SISO::OutputQueue* output_queue); 40 | virtual ~ImageTransporter() = default; 41 | 42 | virtual ImageTransportOutput::UniquePtr runOnce(Frame::UniquePtr frame) override; 43 | 44 | protected: 45 | ImageTransportOutput::UniquePtr process(Frame::UniquePtr frame); 46 | 47 | //! \brief Serialize the image to send to server 48 | static std::string serializeFrame(const Frame& frame); 49 | 50 | //! \brief Deserialize the received image from server 51 | static ImageTransportOutput::UniquePtr deserializeFrame(Timestamp timestamp, const MaskImage& mask_pbuf); 52 | 53 | zmq::context_t context_{ 1 }; //!< Shared ZMQ context for thread to receive and send data 54 | zmq::socket_t req_sock_; //!< ZMQ socket to send request to Python server 55 | }; 56 | } // namespace oslam 57 | #endif /* ifndef OSLAM_IMAGE_TRANSPORT_H */ 58 | -------------------------------------------------------------------------------- /src/object-slam/module/mapper.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: mapper.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 06/26/20 6 | * Description: Mapper module 7 | *****************************************************************************/ 8 | #ifndef OSLAM_MAPPER_H 9 | #define OSLAM_MAPPER_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "object-slam/utils/macros.h" 18 | #include "object-slam/utils/pipeline_module.h" 19 | 20 | #include "object-slam/struct/map.h" 21 | 22 | #include "object-slam/payload/mapper_payload.h" 23 | #include "object-slam/payload/renderer_payload.h" 24 | #include "object-slam/utils/thread_safe_queue.h" 25 | 26 | namespace oslam 27 | { 28 | /*! \class mapper 29 | * \brief Incrementally builds the map from the incoming frames, segmentation information 30 | * and the tracked camera pose. 31 | * Also creates a posegraph with the object volumes, which is then optimized regularly 32 | */ 33 | class Mapper : public MISOPipelineModule 34 | { 35 | public: 36 | OSLAM_POINTER_TYPEDEFS(Mapper); 37 | OSLAM_DELETE_COPY_CONSTRUCTORS(Mapper); 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | 40 | using MISO = MISOPipelineModule; 41 | using TrackerOutputQueue = ThreadsafeQueue; 42 | using TransportOutputQueue = ThreadsafeQueue; 43 | using ObjectRendersQueue = ThreadsafeQueue; 44 | using Matches = std::unordered_map; 45 | 46 | Mapper(Map::Ptr map, 47 | TrackerOutputQueue* tracker_output_queue, 48 | TransportOutputQueue* transport_output_queue, 49 | OutputQueue* output_queue); 50 | 51 | virtual ~Mapper() = default; 52 | 53 | virtual OutputUniquePtr runOnce(InputUniquePtr input) override; 54 | 55 | virtual bool hasWork() const override { return (curr_timestamp_ < max_timestamp_); } 56 | virtual void setMaxTimestamp(Timestamp timestamp) { max_timestamp_ = timestamp; } 57 | void fillRendersQueue(ObjectRenders::UniquePtr renders) { object_renders_queue_.push(std::move(renders)); } 58 | 59 | private: 60 | constexpr static double SCORE_THRESHOLD = 0.7; 61 | constexpr static double IOU_OVERLAP_THRESHOLD = 0.2; 62 | constexpr static int MASK_AREA_THRESHOLD = 2000; 63 | constexpr static int BACKGROUND_RESOLUTION = 256; 64 | constexpr static int OBJECT_RESOLUTION = 128; 65 | 66 | virtual InputUniquePtr getInputPacket() override; 67 | 68 | virtual void shutdownQueues() override; 69 | void initializeMapAndGraph(const Frame& frame, 70 | const InstanceImages& instance_images, 71 | const Eigen::Matrix4d& camera_pose); 72 | 73 | void projectInstanceImages(const Timestamp& keyframe_timestamp, 74 | const Frame& frame, 75 | const Render& background_render, 76 | const InstanceImages& instance_images, 77 | InstanceImages& projected_instance_images); 78 | 79 | InstanceImage createBgInstanceImage(const Frame& frame, 80 | const Renders& object_renders, 81 | const InstanceImages& instance_images, 82 | const Matches& object_to_instance_matches) const; 83 | 84 | bool shouldCreateNewBackground(Timestamp timestamp); 85 | static TSDFObject::UniquePtr createBackground(const Frame& frame, const Eigen::Matrix4d& camera_pose); 86 | static TSDFObject::UniquePtr createObject(const Frame& frame, 87 | const InstanceImage& instance_image, 88 | const Eigen::Matrix4d& camera_pose); 89 | 90 | InstanceImages::const_iterator associateObjects(const ObjectId& id, 91 | const cv::Mat& object_render_color, 92 | const InstanceImages& instance_images, 93 | std::vector& instance_matches) const; 94 | 95 | void updateMap(const gtsam::Values& values); 96 | 97 | Matches integrateObjects(const Renders& object_renders, 98 | const InstanceImages& frame_instance_images, 99 | const Frame& frame, 100 | const Eigen::Matrix4d& camera_pose); 101 | 102 | unsigned int createUnmatchedObjects(const Matches& object_to_instance_matches, 103 | const InstanceImages& instance_images, 104 | const Frame& frame, 105 | const Eigen::Matrix4d& camera_pose); 106 | 107 | void addCameraPriorFactor(const gtsam::Key& camera_key, const Eigen::Matrix4d& camera_pose); 108 | void addCameraCameraBetweenFactor(const Timestamp& time_source_camera, 109 | const Timestamp& time_target_camera, 110 | const Eigen::Matrix4d& T_source_camera_to_target_camera); 111 | 112 | void addObjectCameraBetweenFactor(const gtsam::Key& object_key, 113 | const Timestamp& camera_timestamp, 114 | const Eigen::Matrix4d& T_object_to_camera); 115 | 116 | gtsam::Key addCameraValue(const Timestamp& timestamp, const Eigen::Matrix4d& camera_pose); 117 | void addObjectValue(const gtsam::Key& object_key, const Eigen::Matrix4d& object_pose); 118 | 119 | 120 | Map::Ptr map_; 121 | ObjectId active_bg_id_; 122 | 123 | TrackerOutputQueue* tracker_output_queue_; 124 | TransportOutputQueue* transport_output_queue_; 125 | ObjectRendersQueue object_renders_queue_; 126 | ImageTransportOutput::UniquePtr prev_transport_output_; 127 | 128 | Timestamp curr_timestamp_ = 0; 129 | std::vector keyframe_timestamps_; 130 | 131 | Timestamp max_timestamp_ = std::numeric_limits::max(); 132 | 133 | gtsam::NonlinearFactorGraph pose_graph_; 134 | gtsam::Values pose_values_; 135 | }; 136 | } // namespace oslam 137 | #endif /* ifndef OSLAM_MAPPER_H */ 138 | -------------------------------------------------------------------------------- /src/object-slam/module/renderer.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: renderer.cpp 3 | * 4 | * Author: Akash Sharma 5 | * Created: 07/07/20 6 | * Description: Implementation file for renderer 7 | *****************************************************************************/ 8 | #include "renderer.h" 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | namespace oslam 24 | { 25 | Renderer::Renderer(const Map::Ptr& map, InputQueue* input_queue) : SIMO(input_queue, "Renderer"), map_(map) 26 | { 27 | spdlog::trace("CONSTRUCT: Renderer"); 28 | } 29 | 30 | Renderer::OutputUniquePtr Renderer::runOnce(Renderer::InputUniquePtr input) 31 | { 32 | spdlog::trace("Renderer::runOnce()"); 33 | curr_timestamp_ = input->timestamp_; 34 | const RendererInput& renderer_payload = *input; 35 | const Frame& frame = renderer_payload.frame_; 36 | const Eigen::Matrix3d& intrinsic_matrix = frame.intrinsic_.intrinsic_matrix_ * 4.0; 37 | const InstanceImage& bg_instance = renderer_payload.bg_instance_; 38 | 39 | std::vector camera_trajectory_3d = fillCameraTrajectory(map_->getCameraTrajectory()); 40 | 41 | if (renderer_payload.mapper_status_ == MapperStatus::VALID || 42 | renderer_payload.mapper_status_ == MapperStatus::OPTIMIZED) 43 | { 44 | auto render_start_time = Timer::tic(); 45 | 46 | Render background_render = map_->renderBackground(frame, map_->getCameraPose(curr_timestamp_)); 47 | Renders all_renders = map_->renderObjects(frame, map_->getCameraPose(curr_timestamp_)); 48 | spdlog::info("Rendering objects took {} ms", Timer::toc(render_start_time).count()); 49 | 50 | cv::Mat color_map, vertex_map, normal_map; 51 | background_render.color_map_.copyTo(color_map, bg_instance.maskb_); 52 | background_render.vertex_map_.copyTo(vertex_map, bg_instance.maskb_); 53 | background_render.normal_map_.copyTo(normal_map, bg_instance.maskb_); 54 | 55 | cv::imshow("Background render", color_map); 56 | 57 | all_renders.emplace_back(map_->getBackgroundId(), Render(color_map, vertex_map, normal_map)); 58 | 59 | Render model_render = background_render; 60 | //! Use compositional render only after 25 frames 61 | if(camera_trajectory_3d.size() > 25) 62 | model_render = composeSceneRenders(all_renders); 63 | 64 | cv::imshow("Composed render", model_render.color_map_); 65 | all_renders.pop_back(); 66 | all_renders.emplace_back(map_->getBackgroundId(), background_render); 67 | 68 | RendererOutput::UniquePtr render_output = 69 | std::make_unique(curr_timestamp_ + 1, model_render, all_renders); 70 | 71 | auto object_bboxes = map_->getAllObjectBoundingBoxes(); 72 | auto point_planes = map_->getCameraFrustumPlanes(map_->getCameraPose(curr_timestamp_)); 73 | 74 | render_output->widgets_map_.emplace("Trajectory", render3dTrajectory(camera_trajectory_3d)); 75 | render_output->widgets_map_.emplace( 76 | "Frustum", render3dFrustumWithColorMap(camera_trajectory_3d, intrinsic_matrix, model_render.color_map_)); 77 | renderObjectCubes(object_bboxes, render_output->widgets_map_); 78 | renderFrustumPlanes(point_planes, render_output->widgets_map_); 79 | 80 | /* if(renderer_payload.mapper_status_ == MapperStatus::OPTIMIZED) */ 81 | /* { */ 82 | /* auto object_meshes = map_->meshAllObjects(); */ 83 | /* renderObjectMeshes(object_meshes, render_output->widgets_map_); */ 84 | /* } */ 85 | 86 | spdlog::info("Renderer took {} ms", Timer::toc(render_start_time).count()); 87 | return render_output; 88 | } 89 | return nullptr; 90 | } 91 | 92 | Render Renderer::composeSceneRenders(const Renders& renders) const 93 | { 94 | std::vector depth_array, color_array, vertex_array, normal_array; 95 | 96 | for (auto iter = renders.begin(); iter != renders.end(); ++iter) 97 | { 98 | const Render& render = iter->second; 99 | 100 | cv::Mat depth; 101 | cv::extractChannel(render.vertex_map_, depth, 2); 102 | depth_array.push_back(depth); 103 | 104 | color_array.push_back(render.color_map_); 105 | vertex_array.push_back(render.vertex_map_); 106 | normal_array.push_back(render.normal_map_); 107 | } 108 | cv::Mat object_depths; 109 | cv::merge(depth_array, object_depths); 110 | 111 | //! TODO: Using xtensor only for this one task is not warranted. 112 | std::vector shape = { object_depths.rows, object_depths.cols, object_depths.channels() }; 113 | xt::xarray depth_xarray = xt::adapt((float*)object_depths.data, 114 | object_depths.total() * static_cast(object_depths.channels()), 115 | xt::no_ownership(), 116 | shape); 117 | 118 | //! Use a constant greater than max depth threshold 119 | depth_xarray = xt::where(xt::isfinite(depth_xarray), depth_xarray, 100); 120 | depth_xarray = xt::where(depth_xarray <= 0, 100, depth_xarray); 121 | 122 | xt::xarray min_idx = xt::argmin(depth_xarray, depth_xarray.dimension() - 1); 123 | 124 | cv::Mat layered_color = cv::Mat::zeros(object_depths.rows, object_depths.cols, CV_8UC3); 125 | cv::Mat layered_vertex = cv::Mat::zeros(object_depths.rows, object_depths.cols, CV_32FC3); 126 | cv::Mat layered_normal = cv::Mat::zeros(object_depths.rows, object_depths.cols, CV_32FC3); 127 | 128 | for (int row = 0; row < object_depths.rows; ++row) 129 | { 130 | for (int col = 0; col < object_depths.cols; ++col) 131 | { 132 | size_t layer = min_idx(row, col); 133 | layered_color.at(row, col) = color_array.at(layer).at(row, col); 134 | layered_vertex.at(row, col) = vertex_array.at(layer).at(row, col); 135 | layered_normal.at(row, col) = normal_array.at(layer).at(row, col); 136 | } 137 | } 138 | return Render(layered_color, layered_vertex, layered_normal); 139 | } 140 | 141 | std::vector Renderer::fillCameraTrajectory(const PoseTrajectory& camera_trajectory) 142 | { 143 | std::vector camera_trajectory_3d; 144 | camera_trajectory_3d.reserve(camera_trajectory.size()); 145 | for (const auto& camera_pose : camera_trajectory) 146 | { 147 | cv::Matx44d cv_camera; 148 | cv::eigen2cv(camera_pose, cv_camera); 149 | cv::Affine3d cv_camera_pose(cv_camera); 150 | camera_trajectory_3d.push_back(cv_camera_pose); 151 | } 152 | return camera_trajectory_3d; 153 | } 154 | 155 | WidgetPtr Renderer::render3dTrajectory(const std::vector& camera_trajectory_3d) const 156 | { 157 | //! TODO: Limit trajectory length 158 | return std::make_shared( 159 | camera_trajectory_3d, cv::viz::WTrajectory::PATH, 1.0, cv::viz::Color::green()); 160 | } 161 | 162 | WidgetPtr Renderer::render3dFrustumTraj(const std::vector& camera_trajectory_3d, 163 | const Eigen::Matrix3d& intrinsic_matrix, 164 | const size_t& num_prev_frustums) const 165 | { 166 | cv::Matx33d K; 167 | cv::eigen2cv(intrinsic_matrix, K); 168 | 169 | std::vector trajectory_frustums; 170 | trajectory_frustums.reserve(num_prev_frustums); 171 | size_t count = 0; 172 | for (auto it = camera_trajectory_3d.end(); it != camera_trajectory_3d.begin() && count < num_prev_frustums; --it) 173 | { 174 | trajectory_frustums.push_back(*it); 175 | count++; 176 | } 177 | return std::make_unique(trajectory_frustums, K, 0.2, cv::viz::Color::red()); 178 | } 179 | 180 | WidgetPtr Renderer::render3dFrustumWithColorMap(const std::vector& camera_trajectory_3d, 181 | const Eigen::Matrix3d& intrinsic_matrix, 182 | const cv::Mat& color_map) const 183 | { 184 | cv::Matx33d K; 185 | cv::eigen2cv(intrinsic_matrix, K); 186 | std::unique_ptr frustum_widget = 187 | std::make_unique(K, color_map, 0.4, cv::viz::Color::red()); 188 | frustum_widget->setPose(camera_trajectory_3d.back()); 189 | return frustum_widget; 190 | } 191 | 192 | void Renderer::renderObjectCubes(const ObjectBoundingBoxes& object_bboxes, std::map& widget_map) 193 | { 194 | for (const auto& point_pair : object_bboxes) 195 | { 196 | cv::Vec3d min_pt, max_pt; 197 | cv::eigen2cv(point_pair.second.first, min_pt); 198 | cv::eigen2cv(point_pair.second.second, max_pt); 199 | std::string object_id_string = fmt::format("{}", point_pair.first); 200 | widget_map.emplace(object_id_string, std::make_unique(cv::Point3d(min_pt), cv::Point3d(max_pt))); 201 | } 202 | } 203 | 204 | void Renderer::renderObjectMeshes(const IdToObjectMesh& object_meshes, std::map& widget_map) 205 | { 206 | for (const auto& mesh_pair : object_meshes) 207 | { 208 | cv::Mat colors, vertices, polygons; 209 | const auto& mesh = mesh_pair.second; 210 | std::string object_id_string = fmt::format("{}temp.ply", mesh_pair.first); 211 | io::WriteTriangleMesh(object_id_string, *mesh); 212 | 213 | auto cv_mesh = cv::viz::Mesh::load(object_id_string); 214 | auto cv_mesh_widget = std::make_unique(cv_mesh); 215 | 216 | widget_map.emplace(object_id_string, std::move(cv_mesh_widget)); 217 | } 218 | } 219 | 220 | void Renderer::renderFrustumPlanes(const PointPlanes& point_planes, std::map& widget_map) 221 | { 222 | { 223 | int i = 0; 224 | for (const auto& point_plane : point_planes) 225 | { 226 | const Plane3d& plane = point_plane.first; 227 | const Eigen::Vector3d plane_normal = plane.normal(); 228 | const Eigen::Vector3d& plane_center = point_plane.second; 229 | 230 | cv::Vec3d cv_plane_normal, cv_plane_center; 231 | cv::eigen2cv(plane_normal, cv_plane_normal); 232 | cv::eigen2cv(plane_center, cv_plane_center); 233 | std::string plane_string = fmt::format("plane_{}", i); 234 | std::string arrow_string = fmt::format("Arrow_{}", i); 235 | cv::Vec3d second_point = cv_plane_center + 0.25 * cv_plane_normal; 236 | widget_map.emplace( 237 | arrow_string, 238 | std::make_unique(cv_plane_center, second_point, 0.02, cv::viz::Color::yellow())); 239 | /* widget_map.emplace(plane_string, */ 240 | /* std::make_unique(cv_plane_center, */ 241 | /* cv_plane_normal, */ 242 | /* cv::Vec3d(0, 1, 0), */ 243 | /* cv::Size2d(0.25, 0.25), */ 244 | /* cv::viz::Color::red())); */ 245 | i++; 246 | } 247 | } 248 | } 249 | } // namespace oslam 250 | -------------------------------------------------------------------------------- /src/object-slam/module/renderer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: renderer.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 07/07/20 6 | * Description: Renderer which raycasts vertices and normals / generates visualization 7 | *****************************************************************************/ 8 | #ifndef OSLAM_RENDERER_H 9 | #define OSLAM_RENDERER_H 10 | 11 | #include 12 | #include "object-slam/utils/macros.h" 13 | #include "object-slam/utils/pipeline_module.h" 14 | 15 | #include "object-slam/payload/display_payload.h" 16 | #include "object-slam/payload/renderer_payload.h" 17 | #include "object-slam/struct/map.h" 18 | 19 | namespace oslam 20 | { 21 | /*! \class Renderer 22 | * \brief Brief class description 23 | * 24 | * Detailed description 25 | */ 26 | class Renderer : public SIMOPipelineModule 27 | { 28 | public: 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | OSLAM_POINTER_TYPEDEFS(Renderer); 31 | OSLAM_DELETE_COPY_CONSTRUCTORS(Renderer); 32 | OSLAM_DELETE_MOVE_CONSTRUCTORS(Renderer); 33 | 34 | using SIMO = SIMOPipelineModule; 35 | 36 | Renderer(const Map::Ptr& map, InputQueue* input_queue); 37 | virtual ~Renderer() = default; 38 | 39 | virtual OutputUniquePtr runOnce(InputUniquePtr input) override; 40 | 41 | private: 42 | Render composeSceneRenders(const Renders& renders) const; 43 | 44 | static std::vector fillCameraTrajectory(const PoseTrajectory& camera_trajectory); 45 | 46 | WidgetPtr render3dTrajectory(const std::vector& camera_trajectory_3d) const; 47 | WidgetPtr render3dFrustumTraj(const std::vector& camera_trajectory_3d, 48 | const Eigen::Matrix3d& intrinsic_matrix, 49 | const size_t& num_prev_frustums) const; 50 | WidgetPtr render3dFrustumWithColorMap(const std::vector& camera_trajectory_3d, 51 | const Eigen::Matrix3d& intrinsic_matrix, 52 | const cv::Mat& color_map) const; 53 | void renderFrustumPlanes(const PointPlanes& point_planes, std::map& widget_map); 54 | 55 | void renderObjectCubes(const ObjectBoundingBoxes& object_bboxes, std::map& widget_map); 56 | void renderObjectMeshes(const IdToObjectMesh& object_meshes, std::map& widget_map); 57 | Timestamp curr_timestamp_ = 0; 58 | 59 | Map::Ptr map_; 60 | }; 61 | } // namespace oslam 62 | #endif /* ifndef OSLAM_RENDERER_H */ 63 | -------------------------------------------------------------------------------- /src/object-slam/module/segmenter.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: segmenter.cpp 3 | * 4 | * Author: Akash Sharma 5 | * Created: 06/19/20 6 | * Description: Segmentation thread 7 | *****************************************************************************/ 8 | #include "segmenter.h" 9 | #include 10 | 11 | namespace oslam { 12 | 13 | Segmenter::Segmenter(FrameQueue* p_frame_queue, MaskedImageQueue* p_masked_image_queue) 14 | : MIMO("Segmenter"), mp_frame_queue(p_frame_queue), mp_masked_image_queue(p_masked_image_queue) 15 | {} 16 | 17 | Segmenter::InputUniquePtr Segmenter::get_input_packet() 18 | { 19 | auto start_time = Timer::tic(); 20 | 21 | Frame::UniquePtr p_frame; 22 | bool frame_queue_state = mp_frame_queue->popBlocking(p_frame); 23 | if (!frame_queue_state) 24 | { 25 | spdlog::error("Module: {} {} is down", name_id_, mp_frame_queue->queue_id_); 26 | return nullptr; 27 | } 28 | if (!p_frame) 29 | { 30 | spdlog::error("Module: {} {} returned null", name_id_, mp_frame_queue->queue_id_); 31 | return nullptr; 32 | } 33 | 34 | m_curr_timestamp = p_frame->m_timestamp; 35 | const bool is_keyframe = p_frame->is_keyframe_; 36 | 37 | Segmenter::InputUniquePtr p_segmenter_payload; 38 | if (!is_keyframe) 39 | { 40 | spdlog::debug("SegmenterInputPayload: frame + old mask"); 41 | //! Simply use the previous masked image to create tracker payload 42 | //! TODO(Akash): This creates a copy of the data. Should i use a pointer here? 43 | p_segmenter_payload = std::make_unique(m_curr_timestamp, *p_frame, *mp_prev_masked_image, m_prev_maskframe_timestamp); 44 | } 45 | else 46 | { 47 | MaskedImage::UniquePtr p_masked_image; 48 | //! Try to synchronize the Masked Image queue and search for image with same timestamp as 49 | //! current frame 50 | if (!syncQueue(m_curr_timestamp, mp_masked_image_queue, &p_masked_image)) 51 | { 52 | spdlog::error("Missing masked image with requested timestamp: {}", m_curr_timestamp); 53 | return nullptr; 54 | } 55 | if (!p_masked_image) 56 | { 57 | spdlog::error("Module: {} {} returned null", name_id_, mp_masked_image_queue->queue_id_); 58 | return nullptr; 59 | } 60 | p_segmenter_payload = 61 | std::make_unique(m_curr_timestamp, *p_frame, *p_masked_image, m_curr_timestamp); 62 | mp_prev_masked_image = std::move(p_masked_image); 63 | m_prev_maskframe_timestamp = m_curr_timestamp; 64 | } 65 | if(!p_segmenter_payload) 66 | { 67 | spdlog::error("Unable to create SegmenterInput: {}", name_id_); 68 | return nullptr; 69 | } 70 | auto duration = Timer::toc(start_time).count(); 71 | spdlog::info("Processed tracker payload: {}, took {} ms", m_curr_timestamp, duration); 72 | return p_segmenter_payload; 73 | } 74 | 75 | Segmenter::OutputUniquePtr Segmenter::run_once(Segmenter::InputUniquePtr p_input) 76 | { 77 | const Frame curr_frame = p_input->m_frame; 78 | const MaskedImage curr_masked_image = p_input->m_masked_image; 79 | 80 | //!TODO(Akash): Segmenter can run only after initial transformation is obtained, to transform and project semantic segmentation 81 | 82 | 83 | } 84 | 85 | } // namespace oslam 86 | -------------------------------------------------------------------------------- /src/object-slam/module/segmenter.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: segmenter.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 06/19/20 6 | * Description: Segmentation thread which prepares TrackerInputPayload 7 | *****************************************************************************/ 8 | #ifndef OSLAM_SEGMENTER_H 9 | #define OSLAM_SEGMENTER_H 10 | 11 | #include "segmenter_payload.h" 12 | #include "utils/macros.h" 13 | #include "utils/pipeline_module.h" 14 | #include "utils/thread_safe_queue.h" 15 | 16 | #include "frame.h" 17 | #include "masked_image.h" 18 | 19 | namespace oslam 20 | { 21 | /*! \class Segmenter 22 | * \brief Brief class description 23 | * 24 | * Detailed description 25 | */ 26 | class Segmenter : public MIMOPipelineModule 27 | { 28 | public: 29 | OSLAM_POINTER_TYPEDEFS(Segmenter); 30 | OSLAM_DELETE_COPY_CONSTRUCTORS(Segmenter); 31 | 32 | using MIMO = MIMOPipelineModule; 33 | using MaskedImageQueue = ThreadSafeQueue; 34 | using FrameQueue = ThreadSafeQueue; 35 | 36 | explicit Segmenter(MaskedImageQueue* p_masked_image_queue, FrameQueue* p_frame_queue); 37 | virtual ~Segmenter() = default; 38 | 39 | //! \brief: Reads in the SegmenterInput payload, adds depth segmentation 40 | //! and generates the SegmenterOutput payload 41 | //! being a MIMO package, the SegmenterOutput payload is transferred by callbacks to requisite modules 42 | virtual OutputUniquePtr run_once(InputUniquePtr p_input) override; 43 | 44 | private: 45 | //! \brief: override the get_input_packet interface 46 | //! Segmenter reads from the FrameQueue (prepared by DataReader) and MaskedImageQueue (prepared by ImageTransporter) 47 | //! Synchronizes the packets and generates a SegmenterInput payload 48 | virtual InputUniquePtr get_input_packet() override; 49 | 50 | FrameQueue* mp_frame_queue; 51 | MaskedImageQueue* mp_masked_image_queue; 52 | 53 | MaskedImage::UniquePtr mp_prev_masked_image; 54 | 55 | Timestamp m_curr_timestamp; 56 | Timestamp m_prev_maskframe_timestamp; 57 | 58 | }; 59 | } // namespace oslam 60 | 61 | #endif /* ifndef OSLAM_SEGMENTER_H */ 62 | -------------------------------------------------------------------------------- /src/object-slam/module/tracker.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: tracker.cpp 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/10/20 6 | * Description: Tracker thread 7 | *****************************************************************************/ 8 | #include "tracker.h" 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "object-slam/utils/utils.h" 25 | 26 | namespace oslam 27 | { 28 | Tracker::Tracker(OutputQueue* output_queue) 29 | : MISO(output_queue, "Tracker"), frame_queue_("TrackerFrameQueue"), model_queue_("TrackerModelQueue") 30 | { 31 | spdlog::trace("CONSTRUCT: Tracker"); 32 | } 33 | 34 | Tracker::InputUniquePtr Tracker::getInputPacket() 35 | { 36 | auto start_time = Timer::tic(); 37 | 38 | Frame::UniquePtr input_frame = nullptr; 39 | bool queue_state = frame_queue_.popBlocking(input_frame); 40 | if (!input_frame || !queue_state) 41 | { 42 | spdlog::error("Module: {} {} returned null", name_id_, frame_queue_.queue_id_); 43 | return nullptr; 44 | } 45 | curr_timestamp_ = input_frame->timestamp_; 46 | 47 | Tracker::InputUniquePtr tracker_input; 48 | if (curr_timestamp_ == 1) 49 | { 50 | spdlog::debug("Tracker input payload: Timestamp: {}, Null Vertices, Null Normals", curr_timestamp_); 51 | tracker_input = std::make_unique(curr_timestamp_, *input_frame, cv::Mat(), cv::Mat(), cv::Mat()); 52 | } 53 | else 54 | { 55 | Model::UniquePtr model; 56 | //! Try to synchronize the ImageTransportOutputQueue and search for image with same timestamp as 57 | //! current frame 58 | if (!syncQueue(curr_timestamp_, &model_queue_, &model)) 59 | { 60 | spdlog::error("Missing Render output with requested timestamp: {}", curr_timestamp_); 61 | return nullptr; 62 | } 63 | if (!model) 64 | { 65 | spdlog::error("Module: {} {} returned null", name_id_, model_queue_.queue_id_); 66 | return nullptr; 67 | } 68 | tracker_input = std::make_unique( 69 | curr_timestamp_, *input_frame, model->colors_, model->vertices_, model->normals_); 70 | } 71 | if (!tracker_input) 72 | { 73 | spdlog::error("Unable to create TrackerInputPayload"); 74 | return nullptr; 75 | } 76 | spdlog::info("Processed tracker payload: {}, took {} ms", curr_timestamp_, Timer::toc(start_time).count()); 77 | return tracker_input; 78 | } 79 | 80 | Tracker::OutputUniquePtr Tracker::runOnce(Tracker::InputUniquePtr input) 81 | { 82 | using namespace open3d; 83 | const auto& frame = input->frame_; 84 | const auto& model_color_map = input->model_color_map_; 85 | const auto& model_vertices = input->model_vertices_; 86 | const auto& model_normals = input->model_normals_; 87 | 88 | cuda::ImageCuda frame_raw_depth_cuda; 89 | cuda::ImageCuda frame_color_cuda; 90 | 91 | frame_raw_depth_cuda.Upload(frame.depth_); 92 | frame_color_cuda.Upload(frame.color_); 93 | 94 | cuda::RGBDImageCuda frame_rgbd(frame.max_depth_, frame.depth_factor_); 95 | frame_rgbd.Build(frame_raw_depth_cuda, frame_color_cuda); 96 | 97 | TrackerStatus tracker_status = TrackerStatus::INVALID; 98 | Eigen::Matrix4d relative_camera_pose = Eigen::Matrix4d::Identity(); 99 | Eigen::Matrix4d camera_pose; 100 | 101 | if (curr_timestamp_ == 1) 102 | { 103 | camera_pose = relative_camera_pose; 104 | tracker_status = TrackerStatus::VALID; 105 | } 106 | else 107 | { 108 | auto odo_start_time = Timer::tic(); 109 | 110 | cuda::ImageCuda model_color_map_cuda; 111 | cuda::ImageCuda model_vertices_cuda; 112 | cuda::ImageCuda model_normals_cuda; 113 | 114 | model_color_map_cuda.Upload(model_color_map); 115 | model_vertices_cuda.Upload(model_vertices); 116 | model_normals_cuda.Upload(model_normals); 117 | 118 | cuda::RGBDOdometryCuda<3> odometry; 119 | odometry.SetIntrinsics(frame.intrinsic_); 120 | odometry.SetParameters(odometry::OdometryOption({ 20, 10, 5 }, 0.07), 0.5F, cuda::OdometryType::FRAME_TO_MODEL); 121 | 122 | spdlog::debug("Initializing Odometry for frame: {}", curr_timestamp_); 123 | odometry.Initialize(frame_rgbd, model_vertices_cuda, model_normals_cuda, model_color_map_cuda); 124 | 125 | auto result = odometry.ComputeMultiScale(); 126 | 127 | auto success = std::get<0>(result); 128 | relative_camera_pose = std::get<1>(result); 129 | 130 | if (success) 131 | { 132 | tracker_status = TrackerStatus::VALID; 133 | } 134 | 135 | spdlog::info("Odometry took {} ms", Timer::toc(odo_start_time).count()); 136 | camera_pose = prev_camera_pose * relative_camera_pose; 137 | } 138 | 139 | spdlog::info("Current camera pose: \n{}\n", camera_pose); 140 | prev_camera_pose = camera_pose; 141 | 142 | return std::make_unique(curr_timestamp_, tracker_status, frame, relative_camera_pose); 143 | } 144 | 145 | void Tracker::shutdownQueues() 146 | { 147 | MISOPipelineModule::shutdownQueues(); 148 | frame_queue_.shutdown(); 149 | model_queue_.shutdown(); 150 | } 151 | } // namespace oslam 152 | -------------------------------------------------------------------------------- /src/object-slam/module/tracker.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: tracker.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/10/20 6 | * Description: Tracker Thread 7 | *****************************************************************************/ 8 | #ifndef OSLAM_TRACKER_H 9 | #define OSLAM_TRACKER_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "object-slam/utils/macros.h" 23 | #include "object-slam/utils/pipeline_module.h" 24 | #include "object-slam/utils/thread_safe_queue.h" 25 | 26 | #include "object-slam/payload/renderer_payload.h" 27 | #include "object-slam/payload/tracker_payload.h" 28 | 29 | #include "object-slam/struct/map.h" 30 | 31 | namespace oslam 32 | { 33 | /*! \class Tracker 34 | * \brief Tracks the incoming camera frame via frame-to-model to obtain relative camera pose 35 | */ 36 | class Tracker : public MISOPipelineModule 37 | { 38 | public: 39 | OSLAM_POINTER_TYPEDEFS(Tracker); 40 | OSLAM_DELETE_COPY_CONSTRUCTORS(Tracker); 41 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 42 | 43 | using MISO = MISOPipelineModule; 44 | using ModelInputQueue = ThreadsafeQueue; 45 | using FrameInputQueue = ThreadsafeQueue; 46 | 47 | explicit Tracker(OutputQueue* output_queue); 48 | virtual ~Tracker() = default; 49 | 50 | void fillFrameQueue(Frame::UniquePtr frame) { frame_queue_.push(std::move(frame)); } 51 | void fillModelQueue(Model::UniquePtr model_payload) { model_queue_.push(std::move(model_payload)); } 52 | 53 | virtual OutputUniquePtr runOnce(InputUniquePtr input) override; 54 | 55 | virtual bool hasWork() const override { return (curr_timestamp_ < max_timestamp_); } 56 | virtual void setMaxTimestamp(Timestamp timestamp) { max_timestamp_ = timestamp; } 57 | 58 | private: 59 | virtual InputUniquePtr getInputPacket() override; 60 | 61 | virtual void shutdownQueues() override; 62 | 63 | //! Input Queues which are to be synchronised 64 | FrameInputQueue frame_queue_; 65 | ModelInputQueue model_queue_; 66 | 67 | Timestamp curr_timestamp_ = 0; 68 | Timestamp max_timestamp_ = std::numeric_limits::max(); 69 | 70 | Eigen::Matrix4d prev_camera_pose; //!< T_camera_to_world_ at prev timestep 71 | 72 | cuda::PinholeCameraIntrinsicCuda intrinsic_cuda_; 73 | }; 74 | } // namespace oslam 75 | 76 | #endif /* ifndef OSLAM_TRACKER_H */ 77 | -------------------------------------------------------------------------------- /src/object-slam/payload/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources( 2 | slam 3 | PRIVATE 4 | tracker_payload.h 5 | mapper_payload.h 6 | renderer_payload.h 7 | display_payload.h 8 | ) 9 | -------------------------------------------------------------------------------- /src/object-slam/payload/display_payload.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: display_payload.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 09/02/20 6 | * Description: Input payload for Display 7 | *****************************************************************************/ 8 | #ifndef OSLAM_DISPLAY_PAYLOAD_H 9 | #define OSLAM_DISPLAY_PAYLOAD_H 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "object-slam/utils/pipeline_payload.h" 16 | 17 | namespace oslam 18 | { 19 | struct NamedImage 20 | { 21 | NamedImage(std::string name, cv::Mat image) : name_(std::move(name)), image_(std::move(image)) {} 22 | ~NamedImage() = default; 23 | std::string name_; 24 | cv::Mat image_; 25 | }; 26 | 27 | using WidgetPtr = std::shared_ptr; 28 | using WidgetsMap = std::map; 29 | 30 | /*! \class DisplayInput : public PipelinePayload 31 | * \brief Brief class description 32 | * 33 | * Detailed description 34 | */ 35 | struct DisplayInput : public PipelinePayload 36 | { 37 | public: 38 | OSLAM_POINTER_TYPEDEFS(DisplayInput); 39 | 40 | explicit DisplayInput(Timestamp timestamp) : PipelinePayload(timestamp) {} 41 | 42 | DisplayInput(Timestamp timestamp, const std::vector& display_images, const WidgetsMap& widgets_map) 43 | : PipelinePayload(timestamp), display_images_(display_images), widgets_map_(widgets_map) 44 | { 45 | } 46 | ~DisplayInput() override = default; 47 | 48 | public: 49 | std::vector display_images_; 50 | WidgetsMap widgets_map_; 51 | }; 52 | } // namespace oslam 53 | #endif /* ifndef OSLAM_DISPLAY_PAYLOAD_H */ 54 | -------------------------------------------------------------------------------- /src/object-slam/payload/graph_optimizer_payload.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: graph_optimizer_payload.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 06/16/20 6 | * Description: Payload structure for graph optimizer module 7 | *****************************************************************************/ 8 | #ifndef OSLAM_GRAPH_OPTIMIZER_PAYLOAD_H 9 | #define OSLAM_GRAPH_OPTIMIZER_PAYLOAD_H 10 | 11 | #include "utils/pipeline_payload.h" 12 | namespace oslam { 13 | 14 | /*! \class OptimizerInputPayload 15 | * \brief Brief class description 16 | * 17 | * Detailed description 18 | */ 19 | class OptimizerInputPayload 20 | { 21 | public: 22 | OptimizerInputPayload(const Timestamp& r_timestamp_kf); 23 | virtual ~OptimizerInputPayload(); 24 | 25 | protected: 26 | const Timestamp m_timestamp; 27 | }; 28 | } 29 | #endif /* ifndef OSLAM_GRAPH_OPTIMIZER_PAYLOAD_H */ 30 | -------------------------------------------------------------------------------- /src/object-slam/payload/mapper_payload.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: mapper_payload.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 07/07/20 6 | * Description: Mapper payload data 7 | *****************************************************************************/ 8 | #ifndef OSLAM_MAPPER_PAYLOAD_H 9 | #define OSLAM_MAPPER_PAYLOAD_H 10 | 11 | #include "object-slam/utils/pipeline_payload.h" 12 | 13 | #include "object-slam/payload/tracker_payload.h" 14 | #include "object-slam/struct/frame.h" 15 | #include "object-slam/struct/instance_image.h" 16 | 17 | namespace oslam 18 | { 19 | struct MapperInput : public PipelinePayload 20 | { 21 | public: 22 | OSLAM_POINTER_TYPEDEFS(MapperInput); 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | 25 | MapperInput(const Timestamp timestamp, 26 | const Timestamp prev_maskframe_timestamp, 27 | const TrackerStatus& tracker_status, 28 | const Frame& frame, 29 | const InstanceImages& instance_images, 30 | const Renders& renders, 31 | const Eigen::Matrix4d& relative_camera_pose) 32 | : PipelinePayload(timestamp), 33 | prev_maskframe_timestamp_(prev_maskframe_timestamp), 34 | tracker_status_(tracker_status), 35 | frame_(frame), 36 | instance_images_(instance_images), 37 | object_renders_(renders), 38 | relative_camera_pose_(relative_camera_pose) 39 | { 40 | } 41 | 42 | public: 43 | const Timestamp prev_maskframe_timestamp_; 44 | const TrackerStatus tracker_status_; 45 | const Frame frame_; 46 | const InstanceImages instance_images_; 47 | const Renders object_renders_; 48 | const Eigen::Matrix4d relative_camera_pose_; 49 | }; 50 | } // namespace oslam 51 | #endif /* ifndef OSLAM_MAPPER_PAYLOAD_H */ 52 | -------------------------------------------------------------------------------- /src/object-slam/payload/renderer_payload.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: renderer_payload.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 07/07/20 6 | * Description: Payload definitions for Renderer 7 | *****************************************************************************/ 8 | #ifndef OSLAM_RENDERER_PAYLOAD_H 9 | #define OSLAM_RENDERER_PAYLOAD_H 10 | 11 | #include 12 | 13 | #include 14 | 15 | #include "object-slam/struct/instance_image.h" 16 | #include "object-slam/utils/pipeline_payload.h" 17 | #include "object-slam/utils/types.h" 18 | 19 | #include "object-slam/payload/display_payload.h" 20 | #include "object-slam/struct/frame.h" 21 | 22 | namespace oslam 23 | { 24 | using namespace open3d; 25 | 26 | enum class MapperStatus 27 | { 28 | VALID = 0, 29 | OPTIMIZED = 1, 30 | INVALID = 2 31 | }; 32 | 33 | struct RendererInput : public PipelinePayload 34 | { 35 | public: 36 | OSLAM_POINTER_TYPEDEFS(RendererInput); 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | RendererInput(Timestamp timestamp, 39 | const MapperStatus& mapper_status, 40 | const InstanceImage& bg_instance, 41 | const Renders& object_renders, 42 | const Frame& frame) 43 | : PipelinePayload(timestamp), mapper_status_(mapper_status), bg_instance_(bg_instance), object_renders_(object_renders), frame_(frame) 44 | { 45 | } 46 | 47 | ~RendererInput() = default; 48 | 49 | const MapperStatus mapper_status_; 50 | const InstanceImage bg_instance_; 51 | const Renders object_renders_; 52 | const Frame frame_; 53 | }; 54 | 55 | struct RendererOutput : public PipelinePayload 56 | { 57 | public: 58 | OSLAM_POINTER_TYPEDEFS(RendererOutput); 59 | 60 | RendererOutput(Timestamp timestamp, Render model_render, Renders object_renders) 61 | : PipelinePayload(timestamp), model_render_(model_render), object_renders_(object_renders) 62 | { 63 | } 64 | 65 | ~RendererOutput() = default; 66 | 67 | const Render model_render_; 68 | const Renders object_renders_; 69 | std::map widgets_map_{}; 70 | }; 71 | 72 | } // namespace oslam 73 | #endif /* ifndef OSLAM_RENDERER_PAYLOAD_H */ 74 | -------------------------------------------------------------------------------- /src/object-slam/payload/segmenter_payload.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: segmenter-payload.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 06/19/20 6 | * Description: Segmenter payload definitions 7 | *****************************************************************************/ 8 | #ifndef OSLAM_SEGMENTER_PAYLOAD_H 9 | #define OSLAM_SEGMENTER_PAYLOAD_H 10 | 11 | #include "utils/macros.h" 12 | #include "utils/pipeline_payload.h" 13 | #include "utils/macros.h" 14 | 15 | #include "frame.h" 16 | 17 | namespace oslam { 18 | 19 | struct SegmenterInput : public PipelinePayload 20 | { 21 | OSLAM_POINTER_TYPEDEFS(SegmenterInput); 22 | OSLAM_DELETE_COPY_CONSTRUCTORS(SegmenterInput); 23 | 24 | SegmenterInput(Timestamp timestamp, const Frame& r_frame, const MaskedImage& r_masked_image, Timestamp prev_maskframe_timestamp) 25 | : PipelinePayload(timestamp), m_frame(r_frame), m_masked_image(r_masked_image), m_prev_maskframe_timestamp(prev_maskframe_timestamp) 26 | {} 27 | 28 | public: 29 | const Frame m_frame; 30 | const MaskedImage m_masked_image; 31 | const Timestamp m_prev_maskframe_timestamp; 32 | 33 | }; 34 | } 35 | #endif /* ifndef OSLAM_SEGMENTER_PAYLOAD_H */ 36 | -------------------------------------------------------------------------------- /src/object-slam/payload/tracker_payload.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: trackepayload.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/16/20 6 | * Description: Tracker Payload 7 | *****************************************************************************/ 8 | #ifndef OSLAM_TRACKER_PAYLOAD_H 9 | #define OSLAM_TRACKER_PAYLOAD_H 10 | 11 | #include 12 | 13 | #include "object-slam/utils/pipeline_payload.h" 14 | 15 | #include "object-slam/struct/frame.h" 16 | #include "object-slam/struct/instance_image.h" 17 | 18 | namespace oslam 19 | { 20 | struct Model : public PipelinePayload 21 | { 22 | public: 23 | OSLAM_POINTER_TYPEDEFS(Model); 24 | Model(Timestamp timestamp, cv::Mat colors, cv::Mat vertices, cv::Mat normals) 25 | : PipelinePayload(timestamp), colors_(std::move(colors)), vertices_(std::move(vertices)), normals_(std::move(normals)) 26 | { 27 | } 28 | ~Model() override = default; 29 | 30 | public: 31 | const cv::Mat colors_; 32 | const cv::Mat vertices_; 33 | const cv::Mat normals_; 34 | }; 35 | 36 | /*! \class TrackerInput 37 | * \brief Brief class description 38 | * 39 | * Detailed description 40 | */ 41 | struct TrackerInput : public PipelinePayload 42 | { 43 | public: 44 | OSLAM_POINTER_TYPEDEFS(TrackerInput); 45 | TrackerInput(const Timestamp timestamp, 46 | const Frame& frame, 47 | const cv::Mat& model_color_map, 48 | const cv::Mat& model_vertices, 49 | const cv::Mat& model_normals) 50 | : PipelinePayload(timestamp), 51 | frame_(frame), 52 | model_color_map_(model_color_map), 53 | model_vertices_(model_vertices), 54 | model_normals_(model_normals) 55 | { 56 | } 57 | ~TrackerInput() = default; 58 | 59 | public: 60 | const Frame frame_; 61 | const cv::Mat model_color_map_; 62 | const cv::Mat model_vertices_; 63 | const cv::Mat model_normals_; 64 | }; 65 | 66 | enum class TrackerStatus 67 | { 68 | VALID, 69 | INVALID, 70 | DISABLED 71 | }; 72 | 73 | struct TrackerOutput : public PipelinePayload 74 | { 75 | public: 76 | OSLAM_POINTER_TYPEDEFS(TrackerOutput); 77 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 78 | TrackerOutput(const Timestamp timestamp, 79 | const TrackerStatus& tracker_status, 80 | const Frame& frame, 81 | const Eigen::Matrix4d& relative_camera_pose) 82 | : PipelinePayload(timestamp), 83 | tracker_status_(tracker_status), 84 | frame_(frame), 85 | relative_camera_pose_(relative_camera_pose) 86 | { 87 | } 88 | ~TrackerOutput() = default; 89 | 90 | public: 91 | const TrackerStatus tracker_status_; 92 | const Frame frame_; 93 | const Eigen::Matrix4d relative_camera_pose_; 94 | }; 95 | } // namespace oslam 96 | #endif /* ifndef OSLAM_TRACKER_PAYLOAD_H */ 97 | -------------------------------------------------------------------------------- /src/object-slam/reader/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources( 2 | ${PROJECT_NAME} 3 | PRIVATE 4 | data_reader.h 5 | data_reader.cpp 6 | data_provider.h 7 | data_provider.cpp 8 | ) 9 | -------------------------------------------------------------------------------- /src/object-slam/reader/data_provider.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: data_provider.cpp 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/15/20 6 | * Description: Data Provider module implementation 7 | *****************************************************************************/ 8 | #include "data_provider.h" 9 | 10 | namespace oslam { 11 | DataProvider::DataProvider(InputQueue* p_input_queue) 12 | : SIMO(p_input_queue, "DataProvider") 13 | { 14 | } 15 | 16 | //Simply pass the input to all output callbacks 17 | DataProvider::OutputUniquePtr DataProvider::run_once(Frame::UniquePtr input) 18 | { 19 | return input; 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /src/object-slam/reader/data_provider.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: data_provider.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/15/20 6 | * Description: Data Provider module header 7 | *****************************************************************************/ 8 | #ifndef OSLAM_DATA_PROVIDER_H 9 | #define OSLAM_DATA_PROVIDER_H 10 | 11 | #include "object-slam/utils/macros.h" 12 | #include "object-slam/utils/pipeline_module.h" 13 | #include "object-slam/utils/thread_safe_queue.h" 14 | 15 | #include "object-slam/struct/frame.h" 16 | 17 | namespace oslam 18 | { 19 | /*! \class DataProvider 20 | * \brief Brief class description 21 | * 22 | * Detailed description 23 | */ 24 | class DataProvider : public SIMOPipelineModule 25 | { 26 | public: 27 | OSLAM_POINTER_TYPEDEFS(DataProvider); 28 | OSLAM_DELETE_COPY_CONSTRUCTORS(DataProvider); 29 | 30 | using SIMO = SIMOPipelineModule; 31 | using InputQueue = ThreadsafeQueue; 32 | 33 | explicit DataProvider(InputQueue *p_input_queue); 34 | virtual ~DataProvider() = default; 35 | 36 | virtual OutputUniquePtr run_once(Frame::UniquePtr input); 37 | }; 38 | } // namespace oslam 39 | #endif /* ifndef OSLAM_DATA_PROVIDER_H */ 40 | -------------------------------------------------------------------------------- /src/object-slam/reader/data_reader.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: dataset.cpp 3 | * 4 | * Author: Akash Sharma 5 | * Created: 04/07/20 6 | * Description: Implementation for RGBDdataset 7 | *****************************************************************************/ 8 | #include "data_reader.h" 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace oslam 22 | { 23 | DataReader::DataReader(std::string root_dir, DatasetType dataset_type) 24 | : dataset_type_(dataset_type), root_dir_(std::move(root_dir)) 25 | { 26 | spdlog::trace("CONSTRUCT: DataReader"); 27 | } 28 | 29 | bool DataReader::run() 30 | { 31 | spdlog::trace("DataReader::run()"); 32 | if (!shutdown_ && !dataset_parsed_) 33 | { 34 | dataset_parsed_ = parseDataset(); 35 | } 36 | 37 | if (dataset_parsed_) 38 | { 39 | while (!shutdown_ && readFrame()) 40 | { 41 | ++curr_idx_; 42 | } 43 | } 44 | 45 | if (shutdown_) 46 | { 47 | spdlog::info("DataReader Shutdown requested"); 48 | } 49 | return false; 50 | } 51 | 52 | bool DataReader::parseDataset() 53 | { 54 | spdlog::trace("DataReader::parseDataset()"); 55 | if (!fs::exists(root_dir_) || !fs::is_directory(root_dir_)) 56 | { 57 | spdlog::error("Incorrect dataset path"); 58 | return false; 59 | } 60 | 61 | if (dataset_type_ == DatasetType::RGBD_SCENES) 62 | { 63 | return parseRgbdScenes(); 64 | } 65 | else 66 | { 67 | return parseTum(); 68 | } 69 | return false; 70 | } 71 | 72 | bool DataReader::parseRgbdScenes() 73 | { 74 | spdlog::trace("DataReader::parseRgbdScenes()"); 75 | bool mask_files_exist = true; 76 | 77 | fs::path color_files_path = root_dir_ / "color"; 78 | fs::path depth_files_path = root_dir_ / "depth"; 79 | fs::path mask_files_path = root_dir_ / "preprocessed"; 80 | fs::path intrinsic_path = root_dir_ / "camera-intrinsics.json"; 81 | 82 | if (!fs::exists(intrinsic_path) || !fs::is_regular_file(intrinsic_path)) 83 | { 84 | spdlog::error("Could not find camera intrinsics"); 85 | return false; 86 | } 87 | 88 | spdlog::info("Found camera intrinsics"); 89 | if (!fs::exists(color_files_path) || !fs::exists(depth_files_path) || !fs::is_directory(color_files_path) || 90 | !fs::is_directory(depth_files_path)) 91 | { 92 | spdlog::error("Could not find color and depth images at path \n or path {}\n {}\n is not a directory", 93 | color_files_path.string(), 94 | depth_files_path.string()); 95 | return false; 96 | } 97 | spdlog::info("Found files in the color and depth folder"); 98 | 99 | if (!fs::exists(mask_files_path) || !fs::is_directory(mask_files_path)) 100 | { 101 | spdlog::info("Could not find mask files at path:", mask_files_path.string()); 102 | mask_files_exist = false; 103 | } 104 | 105 | spdlog::info("Found Color files path : {}", color_files_path.string()); 106 | spdlog::info("Found Depth files path : {}", depth_files_path.string()); 107 | 108 | copy(fs::directory_iterator(color_files_path), fs::directory_iterator(), std::back_inserter(rgb_files_)); 109 | copy(fs::directory_iterator(depth_files_path), fs::directory_iterator(), std::back_inserter(depth_files_)); 110 | sort(rgb_files_.begin(), rgb_files_.end()); 111 | sort(depth_files_.begin(), depth_files_.end()); 112 | 113 | if (mask_files_exist) 114 | { 115 | for (auto &file : fs::directory_iterator(mask_files_path)) 116 | { 117 | if (file.path().extension() == ".png") 118 | { 119 | mask_files_.push_back(file); 120 | } 121 | else if (file.path().extension() == ".txt") 122 | { 123 | label_files_.push_back(file); 124 | } 125 | else 126 | { 127 | spdlog::error("Groundtruth segmentation has incorrect extension", file.path().string()); 128 | return false; 129 | } 130 | } 131 | sort(mask_files_.begin(), mask_files_.end()); 132 | } 133 | 134 | if (rgb_files_.size() != depth_files_.size()) 135 | { 136 | spdlog::error("Number of Color images and Depth images do not match"); 137 | return false; 138 | } 139 | 140 | open3d::io::ReadIJsonConvertible(intrinsic_path.string(), intrinsic_); 141 | 142 | depth_factor_ = 1000.0f; 143 | max_depth_ = 3.0f; 144 | size_ = rgb_files_.size(); 145 | spdlog::info("Total number of files: {}", rgb_files_.size()); 146 | 147 | return true; 148 | } 149 | 150 | bool DataReader::parseTum() 151 | { 152 | spdlog::trace("DataReader::parseTum()"); 153 | fs::path color_files_path = root_dir_ / "rgb"; 154 | fs::path depth_files_path = root_dir_ / "depth"; 155 | fs::path associated_files_path = root_dir_ / "files.txt"; 156 | fs::path intrinsic_path = root_dir_ / "camera-intrinsics.json"; 157 | 158 | if (!fs::exists(associated_files_path) || !fs::is_regular_file(associated_files_path)) 159 | { 160 | spdlog::error("Please generate associated files, since data is unsynchronized"); 161 | return false; 162 | } 163 | 164 | if (!fs::exists(intrinsic_path) || !fs::is_regular_file(intrinsic_path)) 165 | { 166 | spdlog::error("Could not find camera intrinsics"); 167 | return false; 168 | } 169 | 170 | spdlog::info("Found camera intrinsics"); 171 | if (!fs::exists(color_files_path) || !fs::exists(depth_files_path) || !fs::is_directory(color_files_path) || 172 | !fs::is_directory(depth_files_path)) 173 | { 174 | spdlog::error("Could not find color and depth images at path \n or path {}\n {}\n is not a directory", 175 | color_files_path.string(), 176 | depth_files_path.string()); 177 | return false; 178 | } 179 | 180 | spdlog::info("Found files in the color and depth folder"); 181 | 182 | spdlog::info("Found Color files path : {}", color_files_path.string()); 183 | spdlog::info("Found Depth files path : {}", depth_files_path.string()); 184 | 185 | fs::ifstream associated_files{ associated_files_path }; 186 | 187 | spdlog::info("Opened file: {}", associated_files_path.string()); 188 | 189 | std::string s; 190 | std::string delimiter{ " " }; 191 | while (std::getline(associated_files, s)) 192 | { 193 | size_t pos_start = 0, pos_end, delim_len = delimiter.length(); 194 | std::string token; 195 | std::vector tokens; 196 | 197 | while ((pos_end = s.find(delimiter, pos_start)) != std::string::npos) 198 | { 199 | token = s.substr(pos_start, pos_end - pos_start); 200 | pos_start = pos_end + delim_len; 201 | tokens.push_back(token); 202 | } 203 | tokens.push_back(s.substr(pos_start)); 204 | if (tokens.size() != 4) 205 | { 206 | spdlog::error("Check file {} whether it has 4 columns of data", associated_files_path.string()); 207 | return false; 208 | } 209 | const std::string &rgb_filename = tokens.at(1); 210 | const std::string &depth_filename = tokens.at(3); 211 | 212 | fs::path rgb_file = root_dir_ / rgb_filename; 213 | fs::path depth_file = root_dir_ / depth_filename; 214 | 215 | if (!fs::exists(rgb_file) || !fs::is_regular_file(rgb_file)) 216 | { 217 | spdlog::error("Could not find {}", (rgb_file).string()); 218 | return false; 219 | } 220 | if (!fs::exists(depth_file) || !fs::is_regular_file(depth_file)) 221 | { 222 | spdlog::error("Could not find {}", (depth_file).string()); 223 | return false; 224 | } 225 | rgb_files_.push_back(rgb_file); 226 | depth_files_.push_back(depth_file); 227 | } 228 | 229 | if (rgb_files_.size() != depth_files_.size()) 230 | { 231 | spdlog::error("Number of Color images and Depth images do not match"); 232 | return false; 233 | } 234 | 235 | open3d::io::ReadIJsonConvertible(intrinsic_path.string(), intrinsic_); 236 | 237 | depth_factor_ = 5000.0f; 238 | max_depth_ = 3.0f; 239 | spdlog::trace("Setting depth factor = 5000.0"); 240 | size_ = rgb_files_.size(); 241 | spdlog::info("Total number of files: {}", rgb_files_.size()); 242 | 243 | return true; 244 | } 245 | bool DataReader::readFrame() 246 | { 247 | spdlog::trace("DataReader::readFrame()"); 248 | if (curr_idx_ >= size_) 249 | { 250 | for (const auto &callback : shutdown_callbacks_) 251 | { 252 | callback(curr_idx_); 253 | } 254 | return false; 255 | } 256 | 257 | using namespace open3d::io; 258 | 259 | spdlog::debug("Reading files: \n{} \n{}", rgb_files_.at(curr_idx_).string(), depth_files_.at(curr_idx_).string()); 260 | 261 | cv::Mat color = cv::imread(rgb_files_.at(curr_idx_).string(), cv::IMREAD_COLOR); 262 | cv::Mat depth = cv::imread(depth_files_.at(curr_idx_).string(), cv::IMREAD_ANYDEPTH); 263 | 264 | // Every 10th frame requires semantic segmentation 265 | for (const auto &callback : output_callbacks_) 266 | { 267 | callback(std::make_unique( 268 | curr_idx_ + 1, color, depth, intrinsic_, (curr_idx_ % KEYFRAME_LENGTH) == 0, depth_factor_, max_depth_)); 269 | } 270 | return true; 271 | } 272 | 273 | void DataReader::shutdown() 274 | { 275 | spdlog::info("Shutting down DataReader on demand"); 276 | shutdown_ = true; 277 | } 278 | 279 | } // namespace oslam 280 | -------------------------------------------------------------------------------- /src/object-slam/reader/data_reader.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: data_reader.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 04/07/20 6 | * Description: DataReader header: Reads the dataset from folder 7 | *****************************************************************************/ 8 | #ifndef OSLAM_DATA_READER_H 9 | #define OSLAM_DATA_READER_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "object-slam/struct/frame.h" 19 | #include "object-slam/utils/types.h" 20 | 21 | namespace oslam 22 | { 23 | namespace fs = boost::filesystem; 24 | 25 | /*! \class DataReader 26 | * \brief Reads from a given dataset folder given the following structure 27 | * . 28 | * ├── camera-intrinsics.json 29 | * ├── color [723 entries] 30 | * ├── depth [723 entries] 31 | * └── preprocessed [1446 entries] 32 | * 33 | */ 34 | class DataReader 35 | { 36 | public: 37 | OSLAM_POINTER_TYPEDEFS(DataReader); 38 | OSLAM_DELETE_COPY_CONSTRUCTORS(DataReader); 39 | 40 | using FrameCallback = std::function; 41 | using ShutdownCallback = std::function; 42 | 43 | explicit DataReader(std::string root_dir, DatasetType dataset_type = DatasetType::RGBD_SCENES); 44 | virtual ~DataReader() = default; 45 | 46 | //! \brief Runs through the dataset folder to read all files 47 | //! - unless explicitly stopped 48 | //! - all the files have been read 49 | //! 50 | bool run(); 51 | 52 | //! \brief Shutdown the data_reader on demand 53 | void shutdown(); 54 | 55 | //! \brief Register callbacks for output queues and shutdown notification 56 | inline void registerOutputCallback(const FrameCallback &r_callback) { output_callbacks_.emplace_back(r_callback); } 57 | inline void registerShutdownCallback(const ShutdownCallback &r_callback) 58 | { 59 | shutdown_callbacks_.emplace_back(r_callback); 60 | } 61 | 62 | private: 63 | bool parseDataset(); 64 | bool parseRgbdScenes(); 65 | bool parseTum(); 66 | bool readFrame(); 67 | 68 | constexpr static int KEYFRAME_LENGTH = 10; 69 | DatasetType dataset_type_ = DatasetType::INVALID; 70 | fs::path root_dir_; //!< Root directory of the dataset folder 71 | std::size_t size_ = 0; //!< Number of images color/depth in the dataset folder 72 | Timestamp curr_idx_ = 0; //!< Current index of files being read 73 | float depth_factor_ = 1000.0f; 74 | float max_depth_ = 3.0f; 75 | bool dataset_parsed_ = false; //!< is dataset parsed 76 | std::atomic_bool shutdown_ = false; //!< Shutdown switch 77 | 78 | //! Vector of filepaths in the dataset folder 79 | std::vector rgb_files_; 80 | std::vector depth_files_; 81 | std::vector pose_files_; 82 | std::vector mask_files_; 83 | std::vector label_files_; 84 | 85 | open3d::camera::PinholeCameraIntrinsic intrinsic_; //!< Camera intrinsics 86 | 87 | std::vector output_callbacks_; //!< Called when data available 88 | std::vector shutdown_callbacks_; //!< Forwards max num frames in dataset when shutdown 89 | }; 90 | 91 | } /* namespace oslam */ 92 | #endif /* OSLAM_DATA_READER_H */ 93 | -------------------------------------------------------------------------------- /src/object-slam/struct/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources( 2 | ${PROJECT_NAME} 3 | PRIVATE 4 | frame.h 5 | instance_image.h 6 | tsdf_object.h 7 | tsdf_object.cpp 8 | map.h 9 | map.cpp 10 | ) 11 | -------------------------------------------------------------------------------- /src/object-slam/struct/frame.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: frame.h 3 | * 4 | * Author: 5 | * Created: 04/07/20 6 | * Description: Frame header: Logical RGBD Frame object 7 | *****************************************************************************/ 8 | #ifndef OSLAM_FRAME_H 9 | #define OSLAM_FRAME_H 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include "object-slam/utils/macros.h" 19 | #include "object-slam/utils/pipeline_payload.h" 20 | 21 | namespace oslam 22 | { 23 | using namespace open3d; 24 | 25 | /*! \class Frame 26 | * \brief Encapsulation data structure for RGB + Depth information 27 | */ 28 | struct Frame : public PipelinePayload 29 | { 30 | public: 31 | OSLAM_POINTER_TYPEDEFS(Frame); 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | 34 | explicit Frame(Timestamp timestamp, 35 | const cv::Mat& color, 36 | const cv::Mat& depth, 37 | const camera::PinholeCameraIntrinsic& intrinsic, 38 | bool is_keyframe = false, 39 | float depth_factor = 1000.0f, 40 | float max_depth = 3.0f) 41 | : PipelinePayload(timestamp), 42 | width_(color.cols), 43 | height_(color.rows), 44 | color_(color), 45 | depth_(depth), 46 | intrinsic_(intrinsic), 47 | is_keyframe_(is_keyframe), 48 | depth_factor_(depth_factor), 49 | max_depth_(max_depth){}; 50 | 51 | ~Frame() override = default; 52 | 53 | //! Copy constructor and assignment 54 | Frame(const Frame& frame) 55 | : PipelinePayload(frame.timestamp_), 56 | width_(frame.width_), 57 | height_(frame.height_), 58 | color_(frame.color_), 59 | depth_(frame.depth_), 60 | intrinsic_(frame.intrinsic_), 61 | is_keyframe_(frame.is_keyframe_), 62 | depth_factor_(frame.depth_factor_), 63 | max_depth_(frame.max_depth_){}; 64 | 65 | public: 66 | //! Frame size 67 | const int width_ = -1; 68 | const int height_ = -1; 69 | 70 | //! Color and depth images 71 | const cv::Mat color_; 72 | const cv::Mat depth_; 73 | const camera::PinholeCameraIntrinsic intrinsic_; 74 | const bool is_keyframe_ = { false }; //!< False if frame doesn't have segmentation 75 | const float depth_factor_ = 1000.0f; 76 | const float max_depth_ = 3.0f; 77 | Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(); //!< Pose of the frame to current local submap TODO: Required? 78 | }; 79 | 80 | //! TODO: Possibly add a new file for this struct 81 | struct Render 82 | { 83 | public: 84 | OSLAM_POINTER_TYPEDEFS(Render); 85 | Render(cv::Mat color_map, cv::Mat vertex_map, cv::Mat normal_map) 86 | : color_map_(std::move(color_map)), vertex_map_(std::move(vertex_map)), normal_map_(std::move(normal_map)) 87 | { 88 | } 89 | 90 | ~Render() = default; 91 | cv::Mat color_map_; 92 | cv::Mat vertex_map_; 93 | cv::Mat normal_map_; 94 | }; 95 | 96 | using Renders = std::vector>; 97 | using RendersUniquePtr = std::unique_ptr; 98 | 99 | struct ObjectRenders : public PipelinePayload 100 | { 101 | public: 102 | OSLAM_POINTER_TYPEDEFS(ObjectRenders); 103 | ObjectRenders(Timestamp timestamp, const Renders& renders) : PipelinePayload(timestamp), renders_(renders) {} 104 | ~ObjectRenders() = default; 105 | Renders renders_; 106 | }; 107 | } // namespace oslam 108 | #endif /* ifndef OSLAM_FRAME_H */ 109 | -------------------------------------------------------------------------------- /src/object-slam/struct/instance_image.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: masked_image.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/16/5 6 | * Description: Masked Image payload 7 | *****************************************************************************/ 8 | #ifndef OSLAM_MASKED_IMAGE_H 9 | #define OSLAM_MASKED_IMAGE_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "object-slam/utils/macros.h" 18 | #include "object-slam/utils/pipeline_payload.h" 19 | #include "object-slam/utils/types.h" 20 | 21 | namespace oslam 22 | { 23 | struct InstanceImage 24 | { 25 | OSLAM_POINTER_TYPEDEFS(InstanceImage); 26 | 27 | static constexpr int BORDER_WIDTH = 20; 28 | cv::Mat maskb_; //!< Store the float value of the object confidence in the image pixels in the mask 29 | Feature feature_; 30 | BoundingBox bbox_; //!< Binary image which masks the rectangular box of the image 31 | cv::Mat bbox_mask_; 32 | unsigned int label_; 33 | double score_; 34 | 35 | explicit InstanceImage(const cv::Mat& mask, 36 | const BoundingBox& bbox, 37 | unsigned int label, 38 | double score, 39 | const Feature& feature = Feature()) 40 | : maskb_(mask), 41 | feature_(feature), 42 | bbox_(bbox), 43 | bbox_mask_(mask.size(), CV_8UC1, cv::Scalar(0)), 44 | label_(label), 45 | score_(score) 46 | { 47 | //! Sanity shrinking 48 | bbox_[0] = std::min(std::max(bbox_[0], 0), mask.cols); 49 | bbox_[1] = std::min(std::max(bbox_[1], 0), mask.rows); 50 | bbox_[2] = std::min(std::max(bbox_[2], 0), mask.cols); 51 | bbox_[3] = std::min(std::max(bbox_[3], 0), mask.rows); 52 | 53 | cv::Point2i left_top = cv::Point2i(bbox_[0], bbox_[1]); 54 | cv::Point2i right_bottom = cv::Point2i(bbox_[2], bbox_[3]); 55 | 56 | // clang-format off 57 | if ((left_top.x - 2 < mask.cols) && (left_top.x - 2 >= 0) && 58 | (left_top.y - 2 < mask.rows) && (left_top.y - 2 >= 0)) 59 | left_top -= cv::Point2i(2, 2); 60 | if ((right_bottom.x + 2 < mask.cols) && (right_bottom.x + 2 >= 0) && 61 | (right_bottom.y + 2 < mask.rows) && (right_bottom.y + 2 >= 0)) 62 | right_bottom += cv::Point2i(2, 2); 63 | // clang-format on 64 | bbox_mask_(cv::Rect(left_top, right_bottom)) = 255; 65 | bbox_mask_ = (bbox_mask_ >= 1); 66 | } 67 | 68 | InstanceImage(int width, int height) 69 | : maskb_(height, width, CV_8UC1, 1), 70 | feature_(), 71 | bbox_({ 0, 0, width, height }), 72 | bbox_mask_(height, width, CV_8UC1, 255), 73 | label_(0), 74 | score_(0) 75 | { 76 | maskb_ = (maskb_ >= 1); 77 | } 78 | ~InstanceImage() = default; 79 | }; 80 | 81 | typedef std::vector InstanceImages; 82 | 83 | struct ImageTransportOutput : public PipelinePayload 84 | { 85 | OSLAM_POINTER_TYPEDEFS(ImageTransportOutput); 86 | InstanceImages instance_images_; 87 | 88 | explicit ImageTransportOutput(Timestamp timestamp) : PipelinePayload(timestamp) {} 89 | ~ImageTransportOutput() = default; 90 | }; 91 | } // namespace oslam 92 | 93 | #endif /* ifndef OSLAM_MASKED_IMAGE_H */ 94 | -------------------------------------------------------------------------------- /src/object-slam/struct/map.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: map.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/01/20 6 | * Description: Global map containing objects 7 | *****************************************************************************/ 8 | #ifndef OSLAM_MAP_H 9 | #define OSLAM_MAP_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include "object-slam/utils/macros.h" 21 | #include "object-slam/utils/types.h" 22 | 23 | #include "object-slam/struct/frame.h" 24 | #include "object-slam/struct/instance_image.h" 25 | #include "object-slam/struct/tsdf_object.h" 26 | 27 | namespace oslam 28 | { 29 | /*! \class Map 30 | * \brief A map representing a scene consists of a hashtable of object volumes, each with their own poses 31 | * and hence forming a posegraph 32 | */ 33 | namespace fs = boost::filesystem; 34 | 35 | using ObjectBoundingBoxes = std::unordered_map>; 36 | using IdToObjectMesh = std::unordered_map>; 37 | using Plane3d = Eigen::Hyperplane; 38 | using PointPlanes = std::vector, Eigen::Vector3d>>; 39 | 40 | struct Map 41 | { 42 | public: 43 | OSLAM_POINTER_TYPEDEFS(Map); 44 | OSLAM_DELETE_COPY_CONSTRUCTORS(Map); 45 | OSLAM_DELETE_MOVE_CONSTRUCTORS(Map); 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | using IdToObjectMap = std::unordered_map; 49 | 50 | explicit Map() = default; 51 | virtual ~Map() = default; 52 | 53 | void addBackground(TSDFObject::UniquePtr bg); 54 | void integrateBackground(const Frame& frame, 55 | const InstanceImage& instance_image, 56 | const Eigen::Matrix4d& camera_pose); 57 | Render renderBackground(const Frame& frame, const Eigen::Matrix4d& camera_pose); 58 | ObjectId getBackgroundId() const 59 | { 60 | std::scoped_lock lock_get_bg_id(mutex_); 61 | return background_volume_->id_; 62 | } 63 | double getBackgroundVisibilityRatio(const Timestamp& timestamp) const; 64 | 65 | bool addObject(TSDFObject::UniquePtr object); 66 | void integrateObject(const ObjectId& id, 67 | const Frame& frame, 68 | const InstanceImage& instance_image, 69 | const Eigen::Matrix4d& camera_pose); 70 | Renders renderObjects(const Frame& frame, const Eigen::Matrix4d& camera_pose); 71 | std::uint64_t getObjectHash(const ObjectId& id) const; 72 | Eigen::Matrix4d getObjectPose(const ObjectId& id) const; 73 | ObjectBoundingBoxes getAllObjectBoundingBoxes() const; 74 | double computeObjectMatch(const ObjectId& id, const Feature& feature) const; 75 | 76 | IdToObjectMesh meshAllObjects() const; 77 | 78 | size_t getNumObjects() const 79 | { 80 | std::scoped_lock get_num_objects(mutex_); 81 | return id_to_object_.size(); 82 | } 83 | 84 | std::vector deleteBadObjects(); 85 | 86 | void incrementExistence(const ObjectId& id); 87 | void incrementNonExistence(const ObjectId& id); 88 | 89 | void addCameraPose(const Eigen::Matrix4d& camera_pose); 90 | Eigen::Matrix4d getCameraPose(const Timestamp& camera_timestamp); 91 | PointPlanes getCameraFrustumPlanes(const Eigen::Matrix4d& camera_pose) const; 92 | 93 | bool isObjectInFrustum(const TSDFObject::Ptr& object, const Eigen::Matrix4d& camera_pose); 94 | std::vector objectsNotInFrustum(Timestamp timestamp); 95 | 96 | PoseTrajectory getCameraTrajectory() const; 97 | 98 | void update(const gtsam::Values& values, const std::vector& keyframe_timestamps); 99 | 100 | void shutdown(); 101 | 102 | void setMaxDepth(double max_depth) { max_depth_ = max_depth; } 103 | private: 104 | double min_depth_ = 0.01; 105 | double max_depth_ = 4.0; 106 | 107 | bool removeObject(const ObjectId& id); 108 | void getObjectBoundingBox(const ObjectId& id, Eigen::Vector3d& min_pt, Eigen::Vector3d& max_pt) const; 109 | 110 | void writeCameraTrajectory(const fs::path& output_path) const; 111 | void writeObjectVolumeToBin(const fs::path& output_path) const; 112 | 113 | std::vector> deleted_objects; 114 | 115 | TSDFObject::UniquePtr background_volume_; 116 | PoseTrajectory camera_trajectory_; //! Sequence of the camera poses traversed in the map 117 | IdToObjectMap id_to_object_; //! Map is a hashtable of different objects 118 | mutable std::mutex mutex_; 119 | }; 120 | } // namespace oslam 121 | #endif /* ifndef OSLAM_MAP_H */ 122 | -------------------------------------------------------------------------------- /src/object-slam/struct/tsdf_object.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: object.hpp 3 | * 4 | * Author: Akash Sharma 5 | * Created: 04/29/20 6 | * Description: TSDFObject implementation 7 | *****************************************************************************/ 8 | #include "tsdf_object.h" 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "object-slam/utils/utils.h" 27 | 28 | namespace oslam 29 | { 30 | TSDFObject::TSDFObject(const ObjectId &id, 31 | const Frame &frame, 32 | const InstanceImage &instance_image, 33 | const Eigen::Matrix4d &camera_pose, 34 | int resolution) 35 | : id_(id), 36 | resolution_(resolution), 37 | instance_image_(instance_image), 38 | pose_(camera_pose), 39 | intrinsic_(frame.intrinsic_), 40 | intrinsic_cuda_(intrinsic_) 41 | { 42 | auto color = frame.color_; 43 | auto object_depth = frame.depth_.clone(); 44 | object_depth.setTo(0, ~instance_image_.bbox_mask_); 45 | 46 | // Construct Object RGBD to obtain center and estimate object size 47 | open3d::cuda::RGBDImageCuda object_rgbd(frame.max_depth_, frame.depth_factor_); 48 | object_rgbd.Upload(object_depth, color); 49 | open3d::cuda::PointCloudCuda object_point_cloud(open3d::cuda::VertexWithColor, frame.width_ * frame.height_); 50 | object_point_cloud.Build(object_rgbd, intrinsic_cuda_); 51 | // Move object point cloud to F_{W} 52 | object_point_cloud.Transform(camera_pose); 53 | 54 | /* auto cpu_pcl = object_point_cloud.Download(); */ 55 | 56 | /* open3d::visualization::DrawGeometries({cpu_pcl}, "Point cloud"); */ 57 | 58 | object_max_pt_ = Eigen::Affine3d(pose_) * object_point_cloud.GetMaxBound(); 59 | object_min_pt_ = Eigen::Affine3d(pose_) * object_point_cloud.GetMinBound(); 60 | 61 | // Translate the object pose to point cloud center 62 | // T_wo = T_wc * T_co 63 | Eigen::Matrix4d T_camera_to_object = Eigen::Matrix4d::Identity(); 64 | T_camera_to_object.block<3, 1>(0, 3) = object_min_pt_; 65 | pose_ = camera_pose * T_camera_to_object.inverse(); 66 | 67 | // Appropriately size the voxel_length of volume for appropriate resolution of the object 68 | voxel_length_ = VOLUME_SIZE_SCALE * float((object_max_pt_ - object_min_pt_).maxCoeff() / resolution); 69 | 70 | spdlog::debug("Volume pose\n {}", pose_); 71 | spdlog::debug("Voxel length: {}", voxel_length_); 72 | 73 | // TODO: Check if this should be camera pose 74 | cuda::TransformCuda object_pose_cuda; 75 | object_pose_cuda.FromEigen(pose_); 76 | 77 | // truncation distance = 4 * voxel length 78 | // Allocate lower memory since we will create multiple TSDF objects 79 | if (instance_image.label_ == 0) 80 | { 81 | BUCKET_COUNT_ = 15000; 82 | VALUE_CAPACITY_ = 15000; 83 | spdlog::debug("Created new background instance"); 84 | } 85 | else 86 | { 87 | spdlog::debug("Created new object instance"); 88 | } 89 | volume_ = cuda::ScalableTSDFVolumeCuda(SUBVOLUME_RES, 90 | voxel_length_, 91 | TSDF_TRUNCATION_SCALE * voxel_length_, 92 | object_pose_cuda, 93 | BUCKET_COUNT_, 94 | VALUE_CAPACITY_); 95 | //! Empty until the first time the object goes out of camera frustum 96 | volume_cpu_ = std::nullopt; 97 | } 98 | 99 | void TSDFObject::integrate(const Frame &frame, const InstanceImage &instance_image, const Eigen::Matrix4d &camera_pose) 100 | { 101 | std::scoped_lock lock_integration(mutex_); 102 | spdlog::trace("Entered integrate for object: {}", id_); 103 | if (volume_.device_ == nullptr) 104 | { 105 | spdlog::error("Volume downloaded into CPU, cannot integrate"); 106 | assert(false); 107 | } 108 | auto color = frame.color_; 109 | auto object_depth = frame.depth_.clone(); 110 | object_depth.setTo(0, ~instance_image.bbox_mask_); 111 | 112 | 113 | if (!isBackground()) 114 | { 115 | auto difference = instance_image_.feature_ - instance_image.feature_; 116 | double match_score = cv::norm(difference, cv::NORM_L1); 117 | 118 | if (match_score < 100.0) 119 | { 120 | instance_image_.feature_ = instance_image.feature_; 121 | spdlog::debug("Updated feature map for the object"); 122 | } 123 | } 124 | 125 | open3d::cuda::RGBDImageCuda object_rgbd_cuda(frame.max_depth_, frame.depth_factor_); 126 | object_rgbd_cuda.Upload(object_depth, color); 127 | 128 | open3d::cuda::TransformCuda camera_to_object_cuda; 129 | camera_to_object_cuda.FromEigen(camera_pose); 130 | 131 | open3d::cuda::ImageCuda mask_cuda; 132 | mask_cuda.Upload(instance_image.maskb_); 133 | 134 | volume_.Integrate( 135 | object_rgbd_cuda, intrinsic_cuda_, camera_to_object_cuda, static_cast(frame.timestamp_), mask_cuda); 136 | 137 | #ifdef OSLAM_DEBUG_VIS 138 | if (frame.timestamp_ % 100 == 0) 139 | { 140 | volume_.GetAllSubvolumes(); 141 | open3d::cuda::ScalableMeshVolumeCuda mesher( 142 | open3d::cuda::VertexWithColor, 16, volume_.active_subvolume_entry_array_.size(), 200000, 400000); 143 | mesher.MarchingCubes(volume_); 144 | auto mesh = mesher.mesh().Download(); 145 | auto aabb = 146 | std::make_shared(volume_.GetMinBound(), volume_.GetMaxBound()); 147 | open3d::visualization::DrawGeometries({ mesh, aabb }, "Mesh after integration"); 148 | mesher.Release(); 149 | } 150 | #endif 151 | } 152 | 153 | void TSDFObject::raycast(open3d::cuda::ImageCuda &vertex, 154 | open3d::cuda::ImageCuda &normal, 155 | open3d::cuda::ImageCuda &color, 156 | const Eigen::Matrix4d &camera_pose) 157 | { 158 | std::scoped_lock lock_raycast(mutex_); 159 | if (volume_.device_ == nullptr) 160 | { 161 | spdlog::error("Volume downloaded into CPU, cannot raycast"); 162 | } 163 | open3d::cuda::TransformCuda camera_to_object_cuda; 164 | /* Eigen::Matrix4d camera_to_object = pose_.inverse() * camera_pose; */ 165 | camera_to_object_cuda.FromEigen(camera_pose); 166 | volume_.RayCasting(vertex, normal, color, intrinsic_cuda_, camera_to_object_cuda); 167 | } 168 | 169 | int TSDFObject::getActiveSubvolumesInFrame(Timestamp timestamp) const 170 | { 171 | if(volume_.device_ == nullptr) 172 | return 0; 173 | 174 | return volume_.GetVisibleSubvolumesCount(static_cast(timestamp), 0); 175 | } 176 | 177 | double TSDFObject::getVisibilityRatio(Timestamp timestamp) const 178 | { 179 | int visible_blocks = volume_.GetVisibleSubvolumesCount(static_cast(timestamp), RETROSPECT_VISIBILITY_THRESH); 180 | int total_blocks = volume_.GetTotalAllocatedSubvolumesCount(); 181 | spdlog::debug("Visible blocks: {}, total_blocks: {}", visible_blocks, total_blocks); 182 | return double(visible_blocks) / double(total_blocks); 183 | } 184 | 185 | bool TSDFObject::downloadVolumeToCPU() 186 | { 187 | if(!volume_cpu_.has_value()) 188 | { 189 | volume_cpu_ = std::make_optional(volume_.DownloadVolumes()); 190 | if(volume_cpu_->first.size() <= 0) 191 | return false; 192 | volume_.Release(); 193 | return true; 194 | } 195 | spdlog::trace("{} already downloaded to CPU", id_); 196 | return true; 197 | } 198 | 199 | bool TSDFObject::uploadVolumeToGPU() 200 | { 201 | assert(volume_cpu_.has_value()); 202 | if (volume_.device_) 203 | { 204 | spdlog::warn("Object already on GPU, not uploading"); 205 | return true; 206 | } 207 | auto keys = volume_cpu_->first; 208 | auto volumes = volume_cpu_->second; 209 | 210 | spdlog::info("Length of keys: {}, length of volumes: {}", keys.size(), volumes.size()); 211 | if(keys.size() <= 0) 212 | { 213 | return false; 214 | } 215 | cuda::TransformCuda object_pose_cuda; 216 | object_pose_cuda.FromEigen(pose_); 217 | 218 | cuda::ScalableTSDFVolumeCuda new_volume = cuda::ScalableTSDFVolumeCuda(SUBVOLUME_RES, 219 | voxel_length_, 220 | TSDF_TRUNCATION_SCALE * voxel_length_, 221 | object_pose_cuda, 222 | BUCKET_COUNT_, 223 | VALUE_CAPACITY_); 224 | 225 | new_volume.UploadVolumes(keys, volumes); 226 | std::swap(volume_, new_volume); 227 | volume_cpu_.reset(); 228 | return true; 229 | } 230 | 231 | Eigen::Vector3d TSDFObject::getMinBound() 232 | { 233 | if (volume_.device_ != nullptr) 234 | { 235 | object_min_pt_ = volume_.GetMinBound(); 236 | return object_min_pt_; 237 | } 238 | return object_min_pt_; 239 | } 240 | 241 | Eigen::Vector3d TSDFObject::getMaxBound() 242 | { 243 | if (volume_.device_ != nullptr) 244 | { 245 | object_max_pt_ = volume_.GetMaxBound(); 246 | return object_max_pt_; 247 | } 248 | return object_max_pt_; 249 | } 250 | } // namespace oslam 251 | -------------------------------------------------------------------------------- /src/object-slam/struct/tsdf_object.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: object.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 04/29/20 6 | * Description: TSDFObject class 7 | *****************************************************************************/ 8 | #ifndef OSLAM_OBJECT_H 9 | #define OSLAM_OBJECT_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "object-slam/utils/macros.h" 23 | #include "object-slam/utils/thread_sync_var.h" 24 | 25 | #include "object-slam/struct/frame.h" 26 | #include "object-slam/struct/instance_image.h" 27 | 28 | namespace oslam 29 | { 30 | /*! \class TSDFObject 31 | * \brief Brief class description 32 | * 33 | * Detailed description 34 | */ 35 | class TSDFObject 36 | { 37 | public: 38 | OSLAM_POINTER_TYPEDEFS(TSDFObject); 39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 40 | 41 | using ScalableTSDFVolumeCPU = std::pair, std::vector>; 42 | 43 | //! Constructor accepts masked depth image to size the object volume during 44 | //! allocation/initialization 45 | TSDFObject(const ObjectId &id, 46 | const Frame &frame, 47 | const InstanceImage &instance_image, 48 | const Eigen::Matrix4d &camera_pose = Eigen::Matrix4d::Identity(), 49 | int resolution = 64); 50 | 51 | virtual ~TSDFObject() = default; 52 | 53 | void integrate(const Frame &frame, const InstanceImage &instance_image, const Eigen::Matrix4d &camera_pose); 54 | 55 | void raycast(open3d::cuda::ImageCuda &vertex, 56 | open3d::cuda::ImageCuda &normal, 57 | open3d::cuda::ImageCuda &color, 58 | const Eigen::Matrix4d &camera_pose); 59 | 60 | bool downloadVolumeToCPU(); 61 | bool uploadVolumeToGPU(); 62 | 63 | Eigen::Vector3d getMinBound(); 64 | Eigen::Vector3d getMaxBound(); 65 | 66 | [[nodiscard]] bool isBackground() const { return (instance_image_.label_ == 0); } 67 | [[nodiscard]] unsigned int getLabel() const { return instance_image_.label_; } 68 | [[nodiscard]] Eigen::Matrix4d getPose() const { return pose_; } 69 | 70 | void setPose(const Eigen::Matrix4d &pose) 71 | { 72 | std::scoped_lock lock_setpose(mutex_); 73 | pose_ = pose; 74 | } 75 | [[nodiscard]] cuda::PinholeCameraIntrinsicCuda getIntrinsicCuda() const { return intrinsic_cuda_; } 76 | [[nodiscard]] double getExistExpectation() const { return double(existence_) / double(existence_ + non_existence_); } 77 | [[nodiscard]] double getVisibilityRatio(Timestamp timestamp) const; 78 | [[nodiscard]] int getActiveSubvolumesInFrame(Timestamp timestamp) const; 79 | 80 | const ObjectId id_; //!< Const public object ID Cannot be modified 81 | std::hash hash; //!< Functor for obtaining hash from object ID 82 | 83 | private: 84 | constexpr static int SUBVOLUME_RES = 16; //!< Each subvolume unit has 16^3 voxels 85 | constexpr static float VOLUME_SIZE_SCALE = 0.7F; 86 | constexpr static float TSDF_TRUNCATION_SCALE = 5.0F; 87 | constexpr static int RETROSPECT_VISIBILITY_THRESH = 5; 88 | int BUCKET_COUNT_ = 3500; 89 | int VALUE_CAPACITY_ = 3500; 90 | 91 | int resolution_; //!< Resolution for the object volume (about 128^3 voxels) 92 | float voxel_length_ = 0.0f; 93 | InstanceImage instance_image_; //!< Object semantic information 94 | Eigen::Matrix4d pose_; //!< Object pose w.r.t world frame T_o_w 95 | open3d::camera::PinholeCameraIntrinsic intrinsic_; 96 | open3d::cuda::PinholeCameraIntrinsicCuda intrinsic_cuda_; 97 | std::mutex mutex_; //!< Protection against integration/raycasting from multiple threads 98 | 99 | open3d::cuda::ScalableTSDFVolumeCuda volume_; 100 | std::optional volume_cpu_; 101 | 102 | std::atomic_int existence_ = 1; 103 | std::atomic_int non_existence_ = 1; 104 | 105 | Eigen::Vector3d object_max_pt_; 106 | Eigen::Vector3d object_min_pt_; 107 | 108 | friend struct Map; 109 | }; 110 | } // namespace oslam 111 | #endif /* ifndef OSLAM_OBJECT_H */ 112 | -------------------------------------------------------------------------------- /src/object-slam/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | target_sources( 2 | ${PROJECT_NAME} 3 | PRIVATE 4 | macros.h 5 | types.h 6 | timer.h 7 | pipeline_module.h 8 | pipeline_module.cpp 9 | thread_safe_queue.h 10 | queue_synchroniser.h 11 | ) 12 | -------------------------------------------------------------------------------- /src/object-slam/utils/macros.h: -------------------------------------------------------------------------------- 1 | #ifndef OSLAM_MACROS_H 2 | #define OSLAM_MACROS_H 3 | 4 | #include 5 | 6 | #define OSLAM_POINTER_TYPEDEFS(TypeName) \ 7 | typedef std::shared_ptr Ptr; \ 8 | typedef std::shared_ptr ConstPtr; \ 9 | typedef std::unique_ptr UniquePtr; \ 10 | typedef std::unique_ptr ConstUniquePtr; \ 11 | typedef std::weak_ptr WeakPtr; \ 12 | typedef std::weak_ptr WeakConstPtr; \ 13 | void definePointerTypedefs##__FILE__##__LINE__(void) 14 | 15 | #define OSLAM_DELETE_COPY_CONSTRUCTORS(TypeName) \ 16 | TypeName(const TypeName&) = delete; \ 17 | void operator=(const TypeName&) = delete 18 | 19 | #define OSLAM_DELETE_MOVE_CONSTRUCTORS(TypeName) \ 20 | TypeName(const TypeName&&) = delete; \ 21 | void operator=(const TypeName&&) = delete 22 | #endif /* ifndef OSLAM_MACROS_H */ 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/object-slam/utils/pipeline_module.cpp: -------------------------------------------------------------------------------- 1 | /* ---------------------------------------------------------------------------- 2 | * Copyright 2017, Massachusetts Institute of Technology, 3 | * Cambridge, MA 02139 4 | * All Rights Reserved 5 | * Authors: Luca Carlone, et al. (see THANKS for the full author list) 6 | * See LICENSE for the license information 7 | * -------------------------------------------------------------------------- */ 8 | 9 | #include "pipeline_module.h" 10 | 11 | namespace oslam {} // namespace oslam 12 | -------------------------------------------------------------------------------- /src/object-slam/utils/pipeline_payload.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file PipelinePayload.h 3 | * @brief Base class for the payloads shared between pipeline modules. 4 | * @author Antoni Rosinol 5 | */ 6 | 7 | #ifndef OSLAM_PIPELINE_PAYLOAD 8 | #define OSLAM_PIPELINE_PAYLOAD 9 | 10 | #include "macros.h" 11 | #include "types.h" 12 | #include 13 | 14 | namespace oslam { 15 | 16 | struct PipelinePayload 17 | { 18 | OSLAM_POINTER_TYPEDEFS(PipelinePayload); 19 | OSLAM_DELETE_COPY_CONSTRUCTORS(PipelinePayload); 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | 22 | explicit PipelinePayload(const Timestamp ×tamp) 23 | : timestamp_(timestamp) {}; 24 | virtual ~PipelinePayload() = default; 25 | 26 | // Untouchable timestamp of the payload. 27 | const Timestamp timestamp_; 28 | }; 29 | 30 | /** 31 | * @brief The NullPipelinePayload is an empty payload, used for those modules 32 | * that do not return a payload, such as the display module, which only 33 | * displays images and returns nothing. 34 | */ 35 | struct NullPipelinePayload : public PipelinePayload 36 | { 37 | OSLAM_POINTER_TYPEDEFS(NullPipelinePayload); 38 | OSLAM_DELETE_COPY_CONSTRUCTORS(NullPipelinePayload); 39 | explicit NullPipelinePayload() : PipelinePayload(Timestamp()) {} 40 | virtual ~NullPipelinePayload() = default; 41 | }; 42 | 43 | }// namespace oslam 44 | 45 | #endif /* ifndef OSLAM_PIPELINE_PAYLOAD */ 46 | -------------------------------------------------------------------------------- /src/object-slam/utils/queue_synchroniser.h: -------------------------------------------------------------------------------- 1 | /* ---------------------------------------------------------------------------- 2 | * Copyright 2017, Massachusetts Institute of Technology, 3 | * Cambridge, MA 02139 4 | * All Rights Reserved 5 | * Authors: Luca Carlone, et al. (see THANKS for the full author list) 6 | * See LICENSE for the license information 7 | * -------------------------------------------------------------------------- */ 8 | 9 | /** 10 | * @file QueueSynchronizer.h 11 | * @brief Implements temporal synchronization of queues. 12 | * @author Antoni Rosinol 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | #include // for numeric_limits 21 | #include 22 | 23 | #include "macros.h" 24 | #include "pipeline_payload.h" 25 | #include "thread_safe_queue.h" 26 | #include "timer.h" 27 | 28 | namespace oslam 29 | { 30 | // TODO: Move this to types.h file 31 | using Timestamp = std::uint64_t; 32 | /** 33 | * @brief The QueueSynchronizer class: a singleton class meant to 34 | * synchronize threadsafe queues (ThreadsafeQueue). 35 | */ 36 | template 37 | class QueueSynchronizerBase 38 | { 39 | public: 40 | OSLAM_POINTER_TYPEDEFS(QueueSynchronizerBase); 41 | /** 42 | * @brief Utility function to synchronize threadsafe queues. 43 | * 44 | * @param[in] timestamp Timestamp of the payload we want to retrieve from the 45 | * queue 46 | * @param[in] queue Threadsafe queue templated on a POINTER to a class that 47 | * is derived from PipelinePayload (otw we cannot query what is its timestamp) 48 | * @param[out] pipeline_payload Returns payload to be found in the given queue 49 | * at the given timestamp. 50 | * @param[in] max_iterations Number of times we try to find the payload at the 51 | * given timestamp in the given queue. 52 | * @param[in] callback User defined function to be called on each successful 53 | * retrieval of a payload in the queue, the callback should be lighting fast! 54 | * @return a boolean indicating whether the synchronizing was successful (i.e. 55 | * we found a payload with the requested timestamp) or we failed because a 56 | * payload with an older timestamp was retrieved. 57 | */ 58 | virtual bool syncQueue(const Timestamp ×tamp, 59 | ThreadsafeQueue *queue, 60 | T *pipeline_payload, 61 | std::string name_id, 62 | int max_iterations = 10, 63 | std::function *callback = nullptr) = 0; 64 | virtual ~QueueSynchronizerBase() = default; 65 | }; 66 | 67 | template 68 | class SimpleQueueSynchronizer : public QueueSynchronizerBase 69 | { 70 | public: 71 | OSLAM_POINTER_TYPEDEFS(SimpleQueueSynchronizer); 72 | OSLAM_DELETE_COPY_CONSTRUCTORS(SimpleQueueSynchronizer); 73 | 74 | /** 75 | * @brief getInstance of a SimpleQueueSynchronizer 76 | * @return a unique ptr to a simple queue synchronizer which can be casted 77 | * to its base class for common interface as a QueueSynchronizer. 78 | */ 79 | static SimpleQueueSynchronizer &getInstance() 80 | { 81 | static SimpleQueueSynchronizer synchronizer_instance_; 82 | return synchronizer_instance_; 83 | } 84 | 85 | /** 86 | * @brief Utility function to synchronize threadsafe queues. 87 | * For now we are doing a very simple naive sync approach: 88 | * Just loop over the messages in the queue until you find the matching 89 | * timestamp. If we are at a timestamp greater than the queried one 90 | * 91 | * @param[in] timestamp Timestamp of the payload we want to retrieve from the 92 | * queue 93 | * @param[in] queue Threadsafe queue templated on a POINTER to a class that 94 | * is derived from PipelinePayload (otw we cannot query what is its timestamp) 95 | * @param[out] pipeline_payload Returns payload to be found in the given queue 96 | * at the given timestamp. 97 | * @param[in] max_iterations Number of times we try to find the payload at the 98 | * given timestamp in the given queue. 99 | * @param[in] callback User defined function to be called on each successful 100 | * retrieval of a payload in the queue, the callback should be lighting fast! 101 | * @return a boolean indicating whether the synchronizing was successful (i.e. 102 | * we found a payload with the requested timestamp) or we failed because a 103 | * payload with an older timestamp was retrieved. 104 | */ 105 | bool syncQueue(const Timestamp ×tamp, 106 | ThreadsafeQueue *queue, 107 | T *pipeline_payload, 108 | std::string name_id, 109 | int max_iterations = 10, 110 | std::function *callback = nullptr) 111 | { 112 | if (!queue) 113 | { 114 | spdlog::error("Queue is null"); 115 | } 116 | if (!pipeline_payload) 117 | { 118 | spdlog::error("pipeline_payload is null"); 119 | } 120 | static_assert(std::is_base_of::element_type>::value, 121 | "T must be a pointer to a class that derives from PipelinePayload."); 122 | // Look for the synchronized packet in payload queue 123 | Timestamp payload_timestamp = std::numeric_limits::min(); 124 | // Loop over payload timestamps until we reach the query timestamp 125 | // or we are past the asked timestamp (in which case, we failed). 126 | int i = 0; 127 | static constexpr size_t timeout_ms = 1500000U; // Wait 1500ms at most! 128 | for (; i < max_iterations && timestamp > payload_timestamp; ++i) 129 | { 130 | // TODO(Toni): add a timer to avoid waiting forever... 131 | if (!queue->popBlockingWithTimeout(*pipeline_payload, timeout_ms)) 132 | { 133 | spdlog::error("Queue sync failed for module: {} with queue: {}\n Reason: {}", 134 | name_id, 135 | queue->queue_id_, 136 | (queue->isShutdown() ? "Shutdown" : "Timeout")); 137 | return false; 138 | } 139 | if (*pipeline_payload) 140 | { 141 | payload_timestamp = (*pipeline_payload)->timestamp_; 142 | // Call any user defined callback at this point (should be fast!!). 143 | if (callback) 144 | (*callback)(*pipeline_payload); 145 | } 146 | else 147 | { 148 | spdlog::warn("Payload synchronization failed. Missing payload for Module: {}", name_id); 149 | } 150 | } 151 | if (timestamp != payload_timestamp) 152 | { 153 | spdlog::error( 154 | "Syncing queue {} in module {} failed\n Could not retrieve exact timestamp requested " 155 | "- Requested timestamp {}\n- Actual timestamp {}\n", 156 | queue->queue_id_, 157 | name_id, 158 | timestamp, 159 | payload_timestamp); 160 | if (i >= max_iterations) 161 | spdlog::error("Reached max number of sync attempts: {}", max_iterations); 162 | } 163 | return true; 164 | } 165 | 166 | private: 167 | // TODO(Toni): don't make this guy a singleton... Rather send it around. 168 | /// Non-constructible class, so make ctors private (copy ctors are deleted): 169 | SimpleQueueSynchronizer() = default; 170 | ~SimpleQueueSynchronizer() = default; 171 | }; 172 | 173 | } // namespace oslam 174 | -------------------------------------------------------------------------------- /src/object-slam/utils/thread_class.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: threadclass.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/11/20 6 | * Description: Thread Wrapper class for safety 7 | *****************************************************************************/ 8 | #ifndef OSLAM_THREAD 9 | #define OSLAM_THREAD 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace oslam { 21 | 22 | class Thread 23 | { 24 | public: 25 | Thread(const std::string &r_threadname) :m_threadname(r_threadname) {} 26 | 27 | // Non-copyable 28 | Thread(const Thread&) = delete; 29 | Thread& operator=(const Thread&) = delete; 30 | 31 | virtual ~Thread() = default; 32 | 33 | virtual bool start() 34 | { 35 | is_running = true; 36 | return run(); 37 | } 38 | 39 | 40 | protected: 41 | bool run(void) 42 | { 43 | spdlog::info("Thread ({},{}) started", m_threadname, std::this_thread::get_id()); 44 | //TODO: Add running conditional variable 45 | 46 | while (process()) 47 | { 48 | //TODO: Add Timer 49 | } 50 | is_running = false; 51 | spdlog::info("Thread ({}, {}), ended", m_threadname, std::this_thread::get_id()); 52 | return false; 53 | } 54 | 55 | virtual bool process(void) = 0; 56 | 57 | /* data */ 58 | std::string m_threadname; 59 | //TODO: Reconsider this to conditional variable 60 | std::atomic_bool is_running = {false}; 61 | }; 62 | }// namespace oslam 63 | #endif /* ifndef OSLAM_THREAD */ 64 | -------------------------------------------------------------------------------- /src/object-slam/utils/thread_data.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: thread_data.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/11/20 6 | * Description: Thread synchronisation data 7 | *****************************************************************************/ 8 | #ifndef OSLAM_THREAD_DATA_H 9 | #define OSLAM_THREAD_DATA_H 10 | 11 | #include 12 | #include 13 | namespace oslam 14 | { 15 | /*! \class ThreadSyncVar 16 | * \brief Thread synchronisation helper variable with mutex and condition variable 17 | * 18 | * Detailed description 19 | */ 20 | template 21 | class ThreadSyncVar 22 | { 23 | public: 24 | ThreadSyncVar(){}; 25 | 26 | ThreadSyncVar(T init_value) : m_var(init_value), m_var_copy(init_value) {} 27 | virtual ~ThreadSyncVar() = default; 28 | 29 | void assign_value(T value) 30 | { 31 | std::scoped_lock lock(m_mutex); 32 | m_var = value; 33 | m_var_copy = value; 34 | } 35 | 36 | void assign_notify_all(T value) 37 | { 38 | { 39 | std::scoped_lock lock(m_mutex); 40 | m_var = value; 41 | } 42 | m_cv.notify_all(); 43 | } 44 | 45 | void assign_notify_one(T value) 46 | { 47 | { 48 | std::scoped_lock lock(m_mutex); 49 | m_var = value; 50 | } 51 | m_cv.notify_one(); 52 | } 53 | 54 | void notify_all() 55 | { 56 | m_cv.notify_all(); 57 | } 58 | void notify_one() 59 | { 60 | m_cv.notify_one(); 61 | } 62 | 63 | void wait(T val) 64 | { 65 | std::unique_lock lock(m_mutex); 66 | m_cv.wait(lock, [&]{ return (m_var == val); }); 67 | } 68 | protected: 69 | T m_var; 70 | T m_var_copy; 71 | std::mutex m_mutex; 72 | std::condition_variable m_cv; 73 | }; 74 | } // namespace oslam 75 | #endif /* ifndef OSLAM_THREAD_DATA_H */ 76 | -------------------------------------------------------------------------------- /src/object-slam/utils/thread_sync_var.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: thread_data.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/11/20 6 | * Description: Thread synchronisation data 7 | *****************************************************************************/ 8 | #ifndef OSLAM_THREAD_DATA_H 9 | #define OSLAM_THREAD_DATA_H 10 | 11 | #include 12 | #include 13 | namespace oslam 14 | { 15 | /*! \class ThreadSyncVar 16 | * \brief Thread synchronisation helper variable with mutex and condition variable 17 | * 18 | * Detailed description 19 | */ 20 | template 21 | class ThreadSyncVar 22 | { 23 | public: 24 | ThreadSyncVar(){}; 25 | 26 | ThreadSyncVar(T init_value) : m_var(init_value), m_var_copy(init_value) {} 27 | virtual ~ThreadSyncVar() = default; 28 | 29 | void assign_value(T value) 30 | { 31 | std::scoped_lock lock(m_mutex); 32 | m_var = value; 33 | m_var_copy = value; 34 | } 35 | 36 | void assign_notify_all(T value) 37 | { 38 | { 39 | std::scoped_lock lock(m_mutex); 40 | m_var = value; 41 | } 42 | m_cv.notify_all(); 43 | } 44 | 45 | void assign_notify_one(T value) 46 | { 47 | { 48 | std::scoped_lock lock(m_mutex); 49 | m_var = value; 50 | } 51 | m_cv.notify_one(); 52 | } 53 | 54 | void notify_all() 55 | { 56 | m_cv.notify_all(); 57 | } 58 | void notify_one() 59 | { 60 | m_cv.notify_one(); 61 | } 62 | 63 | void wait(T val) 64 | { 65 | std::unique_lock lock(m_mutex); 66 | m_cv.wait(lock, [&]{ return (m_var == val); }); 67 | } 68 | protected: 69 | T m_var; 70 | T m_var_copy; 71 | std::mutex m_mutex; 72 | std::condition_variable m_cv; 73 | }; 74 | } // namespace oslam 75 | #endif /* ifndef OSLAM_THREAD_DATA_H */ 76 | -------------------------------------------------------------------------------- /src/object-slam/utils/timer.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: timer.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 05/12/20 6 | * Description: Timer util class 7 | *****************************************************************************/ 8 | #ifndef OSLAM_TIMER_H 9 | #define OSLAM_TIMER_H 10 | 11 | #include 12 | #include 13 | 14 | 15 | namespace oslam { 16 | 17 | class Timer { 18 | public: 19 | static std::chrono::high_resolution_clock::time_point tic() { 20 | return std::chrono::high_resolution_clock::now(); 21 | } 22 | 23 | // Returns duration in milliseconds by default. 24 | // call .count() on returned duration to have number of ticks. 25 | template 26 | static T toc(const std::chrono::high_resolution_clock::time_point& start) { 27 | return std::chrono::duration_cast( 28 | std::chrono::high_resolution_clock::now() - start); 29 | } 30 | }; 31 | 32 | // Usage: measure<>::execution(function, arguments) 33 | template 34 | struct Measure { 35 | template 36 | static typename T::rep execution(F&& func, Args&&... args) { 37 | auto start = std::chrono::steady_clock::now(); 38 | std::forward(func)(std::forward(args)...); 39 | auto duration = 40 | std::chrono::duration_cast(std::chrono::steady_clock::now() - start); 41 | return duration.count(); 42 | } 43 | }; 44 | 45 | } // namespace oslam 46 | #endif /* ifndef OSLAM_TIMER_H */ 47 | -------------------------------------------------------------------------------- /src/object-slam/utils/types.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: types.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 06/16/20 6 | * Description: Utility types for Object SLAM 7 | *****************************************************************************/ 8 | #ifndef OSLAM_TYPES_H 9 | #define OSLAM_TYPES_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace oslam 20 | { 21 | using Timestamp = std::uint64_t; 22 | 23 | using FrameId = std::uint64_t; 24 | 25 | using BoundingBox = std::array; 26 | using Feature = cv::Vec; 27 | 28 | using PoseTrajectory = std::vector>; 29 | 30 | enum DatasetType 31 | { 32 | INVALID = 0, 33 | TUM = 1, 34 | RGBD_SCENES = 2 35 | }; 36 | 37 | //! Hashable object id struct 38 | struct ObjectId 39 | { 40 | unsigned int label; 41 | Timestamp frame_created; 42 | BoundingBox bbx; 43 | 44 | ObjectId(unsigned int _label, Timestamp _frame_created, BoundingBox _bbx) 45 | : label(_label), frame_created(_frame_created), bbx(_bbx) 46 | { 47 | } 48 | ObjectId() : label(0), frame_created(0), bbx({ 0, 0, 0, 0 }) {} 49 | 50 | ~ObjectId() = default; 51 | 52 | bool operator==(const ObjectId& r_id) const 53 | { 54 | return label == r_id.label && frame_created == r_id.frame_created && 55 | (bbx[0] == r_id.bbx[0] && bbx[1] == r_id.bbx[1] && bbx[2] == r_id.bbx[2] && bbx[3] == r_id.bbx[3]); 56 | } 57 | 58 | friend std::ostream& operator<<(std::ostream& os, const ObjectId& r_id) 59 | { 60 | return os << "L:" << r_id.label << "-T:" << r_id.frame_created << "-BBX:{" << r_id.bbx[0] << "," << r_id.bbx[1] << "}"; 61 | } 62 | }; 63 | } // namespace oslam 64 | 65 | namespace std 66 | { 67 | template<> 68 | struct hash 69 | { 70 | std::size_t operator()(const oslam::ObjectId& r_id) const noexcept 71 | { 72 | std::size_t seed = 0; 73 | constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; 74 | seed ^= std::hash()(r_id.label) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); 75 | seed ^= std::hash()(r_id.frame_created) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); 76 | seed ^= std::hash()(r_id.bbx[0]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); 77 | seed ^= std::hash()(r_id.bbx[1]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); 78 | 79 | return seed; 80 | } 81 | }; 82 | } // namespace std 83 | #endif /* ifndef OSLAM_TYPES_H */ 84 | -------------------------------------------------------------------------------- /src/object-slam/utils/utils.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * File: utils.h 3 | * 4 | * Author: Akash Sharma 5 | * Created: 06/16/20 6 | * Description: Utility functions required in Object SLAM 7 | *****************************************************************************/ 8 | #ifndef OSLAM_UTILS_H 9 | #define OSLAM_UTILS_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include "object-slam/struct/instance_image.h" 17 | 18 | namespace oslam 19 | { 20 | inline Eigen::Vector2d project_point(const Eigen::Vector3d& r_point3d, 21 | const open3d::camera::PinholeCameraIntrinsic& r_intrinsic) 22 | { 23 | auto fx = r_intrinsic.intrinsic_matrix_(0, 0); 24 | auto fy = r_intrinsic.intrinsic_matrix_(1, 1); 25 | auto cx = r_intrinsic.intrinsic_matrix_(0, 2); 26 | auto cy = r_intrinsic.intrinsic_matrix_(1, 2); 27 | 28 | return Eigen::Vector2d((fx * r_point3d(0)) / r_point3d(2) + cx, (fy * r_point3d(1)) / r_point3d(2) + cy); 29 | } 30 | 31 | inline Eigen::Vector3d inverse_project_point(const Eigen::Vector2i& r_point2, 32 | const open3d::camera::PinholeCameraIntrinsic& r_intrinsic, 33 | double depth) 34 | { 35 | auto fx = r_intrinsic.intrinsic_matrix_(0, 0); 36 | auto fy = r_intrinsic.intrinsic_matrix_(1, 1); 37 | auto cx = r_intrinsic.intrinsic_matrix_(0, 2); 38 | auto cy = r_intrinsic.intrinsic_matrix_(1, 2); 39 | 40 | return Eigen::Vector3d((depth * (r_point2(0) - cx)) / fx, (depth * (r_point2(1) - cy)) / fy, depth); 41 | } 42 | 43 | inline bool is_valid_depth(float depth) 44 | { 45 | if (std::isnan(depth)) 46 | return false; 47 | return (depth >= 0.0F) && (depth <= 4.0F); 48 | } 49 | 50 | inline bool transform_project_bbox(const BoundingBox& bbox, 51 | BoundingBox& transformed_bbox, 52 | const cv::Mat& r_depth, 53 | const open3d::camera::PinholeCameraIntrinsic& r_intrinsic, 54 | const Eigen::Matrix4d& r_transform) 55 | { 56 | Eigen::Vector2i left_top_point(bbox[0], bbox[1]); 57 | Eigen::Vector2i right_bottom_point(bbox[2], bbox[3]); 58 | Eigen::Vector2i center_point((left_top_point(0) + right_bottom_point(0)) / 2, 59 | (left_top_point(1) + right_bottom_point(1)) / 2); 60 | 61 | /* float depth_left_top = r_depth.at(bbox[1], bbox[0]); */ 62 | /* float depth_right_bottom = r_depth.at(bbox[3], bbox[2]); */ 63 | float depth_center = r_depth.at(center_point(1), center_point(0)); 64 | 65 | transformed_bbox = bbox; 66 | if(!is_valid_depth(depth_center)) 67 | return false; 68 | 69 | int width = center_point(0) - left_top_point(0); 70 | int height = center_point(1) - left_top_point(1); 71 | 72 | if (!(left_top_point(0) >= InstanceImage::BORDER_WIDTH && 73 | left_top_point(0) < r_depth.cols - InstanceImage::BORDER_WIDTH && 74 | left_top_point(1) >= InstanceImage::BORDER_WIDTH && 75 | left_top_point(1) < r_depth.rows - InstanceImage::BORDER_WIDTH)) 76 | { 77 | return false; 78 | } 79 | if (!(right_bottom_point(0) >= InstanceImage::BORDER_WIDTH && 80 | right_bottom_point(0) < r_depth.cols - InstanceImage::BORDER_WIDTH && 81 | right_bottom_point(1) >= InstanceImage::BORDER_WIDTH && 82 | right_bottom_point(1) < r_depth.rows - InstanceImage::BORDER_WIDTH)) 83 | { 84 | return false; 85 | } 86 | Eigen::Vector3d center_3dpoint = inverse_project_point(center_point, r_intrinsic, static_cast(depth_center)); 87 | Eigen::Vector4d center_4dpoint{center_3dpoint(0), center_3dpoint(1), center_3dpoint(2), 1}; 88 | Eigen::Vector4d tform_center = r_transform * center_4dpoint; 89 | Eigen::Vector2d center = project_point(tform_center.head(3), r_intrinsic); 90 | 91 | Eigen::Vector2d left_top{ center(0) - width, center(1) - height }; 92 | Eigen::Vector2d right_bottom{ center(0) + width, center(1) + height }; 93 | spdlog::debug("Center: {}", center); 94 | spdlog::debug("Left top: {}, right bottom: {}", left_top, right_bottom); 95 | 96 | /* if (!is_valid_depth(depth_left_top) || !is_valid_depth(depth_right_bottom)) */ 97 | /* { */ 98 | /* spdlog::debug("Depth at left point: {}, Depth at right point: {}\n", depth_left_top, depth_right_bottom); */ 99 | /* //! Try once more */ 100 | /* depth_left_top = r_depth.at(bbox[1] - 2, bbox[0] - 2); */ 101 | /* depth_right_bottom = r_depth.at(bbox[3] - 2, bbox[2] - 2); */ 102 | 103 | /* if (!is_valid_depth(depth_left_top) || !is_valid_depth(depth_right_bottom)) */ 104 | /* return false; */ 105 | /* } */ 106 | /* Eigen::Vector3d left_top_3dpoint = */ 107 | /* inverse_project_point(left_top_point, r_intrinsic, static_cast(r_depth.at(bbox[1], bbox[0]))); */ 108 | /* Eigen::Vector3d right_bottom_3dpoint = */ 109 | /* inverse_project_point(right_bottom_point, r_intrinsic, static_cast(r_depth.at(bbox[3], bbox[2]))); */ 110 | 111 | /* Eigen::Vector4d left_top_4dpoint{ left_top_3dpoint(0), left_top_3dpoint(1), left_top_3dpoint(2), 1 }; */ 112 | /* Eigen::Vector4d right_bottom_4dpoint{ right_bottom_3dpoint(0), right_bottom_3dpoint(1), right_bottom_3dpoint(2), 1 }; */ 113 | 114 | /* Eigen::Vector4d tform_left_top = r_transform * left_top_4dpoint; */ 115 | /* Eigen::Vector4d tform_right_bottom = r_transform * right_bottom_4dpoint; */ 116 | 117 | /* Eigen::Vector2d left_top = project_point(tform_left_top.head(3), r_intrinsic); */ 118 | /* Eigen::Vector2d right_bottom = project_point(tform_right_bottom.head(3), r_intrinsic); */ 119 | transformed_bbox = BoundingBox({ int(std::round(left_top(0))), 120 | int(std::round(left_top(1))), 121 | int(std::round(right_bottom(0))), 122 | int(std::round(right_bottom(1))) }); 123 | return true; 124 | } 125 | 126 | } // namespace oslam 127 | #endif /* ifndef OSLAM_UTILS_H */ 128 | -------------------------------------------------------------------------------- /src/show-objects.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace open3d; 12 | using namespace open3d::registration; 13 | using namespace open3d::geometry; 14 | using namespace open3d::io; 15 | using namespace open3d::utility; 16 | namespace fs = boost::filesystem; 17 | 18 | std::shared_ptr visualizeCameraTrajectory(const std::vector& camera_trajectory) { 19 | std::shared_ptr cam_traj_vis = 20 | std::make_shared(); 21 | 22 | int cnt = 0; 23 | 24 | const int kPointsPerFrustum = 5; 25 | const int kEdgesPerFrustum = 8; 26 | 27 | std::vector subsampled_cam_traj; 28 | for(size_t i = 0; i < camera_trajectory.size(); i++) 29 | { 30 | if(i %10 == 0) 31 | subsampled_cam_traj.push_back(camera_trajectory.at(i)); 32 | } 33 | Eigen::Matrix4d previous_camera_pose = Eigen::Matrix4d::Identity(); 34 | int i = 0; 35 | for (auto& pose : subsampled_cam_traj) { 36 | 37 | double norm = 0.1; 38 | Eigen::Vector4d ph; 39 | 40 | ph = pose * Eigen::Vector4d(0, 0, 0, 1); 41 | cam_traj_vis->points_.emplace_back(ph.hnormalized()); 42 | ph = pose * (norm * Eigen::Vector4d(1, 1, 2, 1/norm)); 43 | cam_traj_vis->points_.emplace_back(ph.hnormalized()); 44 | ph = pose * (norm * Eigen::Vector4d(1, -1, 2, 1/norm)); 45 | cam_traj_vis->points_.emplace_back(ph.hnormalized()); 46 | ph = pose * (norm * Eigen::Vector4d(-1, -1, 2, 1/norm)); 47 | cam_traj_vis->points_.emplace_back(ph.hnormalized()); 48 | ph = pose * (norm * Eigen::Vector4d(-1, 1, 2, 1/norm)); 49 | cam_traj_vis->points_.emplace_back(ph.hnormalized()); 50 | 51 | cam_traj_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 0, cnt + 1)); 52 | cam_traj_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 0, cnt + 2)); 53 | cam_traj_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 0, cnt + 3)); 54 | cam_traj_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 0, cnt + 4)); 55 | cam_traj_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 1, cnt + 2)); 56 | cam_traj_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 2, cnt + 3)); 57 | cam_traj_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 3, cnt + 4)); 58 | cam_traj_vis->lines_.emplace_back(Eigen::Vector2i(cnt + 4, cnt + 1)); 59 | 60 | for (int k = 0; k < kEdgesPerFrustum; ++k) { 61 | cam_traj_vis->colors_.emplace_back(Eigen::Vector3d(1, 0, 0)); 62 | } 63 | 64 | 65 | if(i != 0) 66 | { 67 | cam_traj_vis->lines_.emplace_back( 68 | Eigen::Vector2i((i-1) * kPointsPerFrustum, i * kPointsPerFrustum)); 69 | cam_traj_vis->colors_.emplace_back(Eigen::Vector3d(0, 0, 1)); 70 | } 71 | cnt += kPointsPerFrustum; 72 | i++; 73 | } 74 | 75 | /* for (auto &edge : pose_graph.edges_) { */ 76 | /* int s = edge.source_node_id_; */ 77 | /* int t = edge.target_node_id_; */ 78 | 79 | /* if (edge.uncertain_) { */ 80 | /* cam_traj_vis->lines_.emplace_back( */ 81 | /* Eigen::Vector2i(s * kPointsPerFrustum, t * kPointsPerFrustum)); */ 82 | /* cam_traj_vis->colors_.emplace_back(Eigen::Vector3d(0, 1, 0)); */ 83 | /* } else { */ 84 | /* cam_traj_vis->lines_.emplace_back( */ 85 | /* Eigen::Vector2i(s * kPointsPerFrustum, t * kPointsPerFrustum)); */ 86 | /* cam_traj_vis->colors_.emplace_back(Eigen::Vector3d(0, 0, 1)); */ 87 | /* } */ 88 | /* } */ 89 | 90 | return cam_traj_vis; 91 | } 92 | 93 | std::vector readCameraTrajectory(const fs::path& filename) 94 | { 95 | fs::ifstream cam_traj_file{filename}; 96 | spdlog::info("Read file"); 97 | std::string line; 98 | Eigen::Matrix4d matrix4d = Eigen::Matrix4d::Zero(); 99 | std::vector camera_trajectory; 100 | for(int count = 0; std::getline(cam_traj_file, line);) 101 | { 102 | std::stringstream ss(line); 103 | spdlog::info("Processed line: {}", line); 104 | ss >> matrix4d(count + 0*4) >> matrix4d(count + 1*4) >> matrix4d(count + 2*4) >> matrix4d(count + 3*4); 105 | 106 | count = (count + 1) % 4; 107 | if(count % 4 == 0) 108 | { 109 | camera_trajectory.push_back(matrix4d); 110 | spdlog::info("Camera pose: {}", matrix4d); 111 | matrix4d = Eigen::Matrix4d::Zero(); 112 | 113 | } 114 | } 115 | return camera_trajectory; 116 | } 117 | std::shared_ptr readFragment(const fs::path& filename) { 118 | 119 | Timer timer; 120 | timer.Start(); 121 | cuda::ScalableTSDFVolumeCuda tsdf_volume = 122 | io::ReadScalableTSDFVolumeFromBIN(filename.string(), true); 123 | std::cout << tsdf_volume.GetMinBound() << "\n"; 124 | std::cout << tsdf_volume.GetMaxBound() << "\n"; 125 | timer.Stop(); 126 | utility::LogInfo("Read takes {} ms\n", timer.GetDuration()); 127 | auto aabb = std::make_shared( 128 | tsdf_volume.GetMinBound(), tsdf_volume.GetMaxBound()); 129 | 130 | tsdf_volume.GetAllSubvolumes(); 131 | cuda::ScalableMeshVolumeCuda mesher( 132 | cuda::VertexWithNormalAndColor, 16, 133 | tsdf_volume.active_subvolume_entry_array_.size()); 134 | mesher.MarchingCubes(tsdf_volume); 135 | auto mesh = mesher.mesh().Download(); 136 | for(auto& color : mesh->vertex_colors_) 137 | { 138 | Eigen::Vector3d temp_color = color; 139 | color(0) = temp_color(2); 140 | color(1) = temp_color(1); 141 | color(2) = temp_color(0); 142 | } 143 | /* std::cout << mesh->GetMinBound() << "\n"; */ 144 | /* std::cout << mesh->GetMaxBound() << "\n"; */ 145 | return mesh; 146 | /* visualization::DrawGeometries({mesh, aabb}); */ 147 | } 148 | 149 | std::vector> visualizeObjectMeshes(fs::path object_path) 150 | { 151 | std::vector> objects; 152 | for (auto &file : fs::directory_iterator(object_path)) 153 | { 154 | if(fs::is_regular_file(file)) 155 | { 156 | objects.push_back(readFragment(file)); 157 | } 158 | } 159 | return objects; 160 | } 161 | 162 | int main(int argc, char* argv[]) { 163 | CLI::App app{ "Read objects BIN file and display mesh" }; 164 | spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] [thread: %t] %^[%L] %v%$"); 165 | std::string output_folder; 166 | app.add_option("outout_folder", output_folder, "Path to the output folder")->required(); 167 | 168 | CLI11_PARSE(app, argc, argv); 169 | fs::path output_path{output_folder}; 170 | 171 | if(!fs::exists(output_path) && !fs::is_directory(output_path)) 172 | { 173 | spdlog::error("Output folder does not exist"); 174 | return EXIT_FAILURE; 175 | } 176 | 177 | fs::path camera_trajectory_file = output_path / "camera_trajectory.txt"; 178 | if(!fs::exists(camera_trajectory_file) && !fs::is_regular_file(camera_trajectory_file)) 179 | { 180 | spdlog::error("Camera trajectory file does not exist"); 181 | return EXIT_FAILURE; 182 | } 183 | 184 | fs::path object_path = output_path / "objects"; 185 | if(!fs::exists(object_path) && !fs::is_directory(object_path)) 186 | { 187 | spdlog::error("Objects folder absent"); 188 | return EXIT_FAILURE; 189 | } 190 | 191 | auto camera_trajectory = readCameraTrajectory(camera_trajectory_file); 192 | std::shared_ptr camera_traj_vis = visualizeCameraTrajectory(camera_trajectory); 193 | 194 | auto mesh_vis = visualizeObjectMeshes(object_path); 195 | std::vector> vis{}; 196 | vis.push_back(camera_traj_vis); 197 | vis.insert(vis.end(), mesh_vis.begin(), mesh_vis.end()); 198 | visualization::DrawGeometries(vis); 199 | /* readFragment(bin_file); */ 200 | } 201 | 202 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpl-cmu/object-slam/7baecfc995d4bbd6441585742df810e38568732f/test/CMakeLists.txt --------------------------------------------------------------------------------