├── .github └── workflows │ ├── ci.yml │ └── docker.yaml ├── .gitignore ├── CITATION.cff ├── CMakeLists.txt ├── LICENSE.GPL-2 ├── README.md ├── assets └── scheme.png ├── cmake ├── AddInstallRPATHSupport.cmake ├── AddUninstallTarget.cmake ├── CMakeRC.cmake ├── CMakeRC_LICENSE │ └── LICENSE.txt └── InstallBasicPackageFiles.cmake ├── config ├── classes.txt ├── classes_ho3d.txt ├── config_fast_ycb.cfg └── config_ho3d.cfg ├── dockerfiles └── Dockerfile ├── docs ├── detectron2_requirements.txt ├── detectron2_requirements_cuda11.txt ├── dope_requirements.txt └── installation.md ├── evaluation ├── data_loader.py ├── evaluate.py ├── experiments.py ├── metrics.py ├── objects.py ├── results_renderer.py ├── run_all_experiments_evaluation └── run_paper_evaluation ├── requirements.txt ├── setup.py ├── src ├── roft-lib │ ├── CMakeLists.txt │ ├── include │ │ └── ROFT │ │ │ ├── CameraMeasurement.h │ │ │ ├── CartesianQuaternionMeasurement.h │ │ │ ├── CartesianQuaternionModel.h │ │ │ ├── DatasetImageOpticalFlow.h │ │ │ ├── DatasetImageSegmentation.h │ │ │ ├── DatasetImageSegmentationDelayed.h │ │ │ ├── ImageOpticalFlowMeasurement.hpp │ │ │ ├── ImageOpticalFlowNVOF.h │ │ │ ├── ImageOpticalFlowSource.h │ │ │ ├── ImageSegmentationMeasurement.h │ │ │ ├── ImageSegmentationOFAidedSource.hpp │ │ │ ├── ImageSegmentationOFAidedSourceStamped.hpp │ │ │ ├── MeshResource.h │ │ │ ├── ModelParameters.h │ │ │ ├── OpticalFlowQueueHandler.h │ │ │ ├── OpticalFlowUtilities.h │ │ │ ├── QuaternionModel.h │ │ │ ├── ROFTFilter.h │ │ │ ├── SICAD.h │ │ │ ├── SICADModel.h │ │ │ ├── SICADShader.h │ │ │ ├── SKFCorrection.h │ │ │ ├── SpatialVelocityModel.h │ │ │ └── UKFCorrection.h │ ├── meshes │ │ └── DOPE │ │ │ ├── 003_cracker_box.obj │ │ │ ├── 004_sugar_box.obj │ │ │ ├── 005_tomato_soup_can.obj │ │ │ ├── 006_mustard_bottle.obj │ │ │ ├── 009_gelatin_box.obj │ │ │ ├── 010_potted_meat_can.obj │ │ │ └── 021_bleach_cleanser.obj │ ├── shader │ │ ├── shader_background.frag │ │ ├── shader_background.vert │ │ ├── shader_frame.frag │ │ ├── shader_frame.vert │ │ ├── shader_model.frag │ │ ├── shader_model.vert │ │ └── shader_model_texture.frag │ └── src │ │ ├── CameraMeasurement.cpp │ │ ├── CartesianQuaternionMeasurement.cpp │ │ ├── CartesianQuaternionModel.cpp │ │ ├── DatasetImageOpticalFlow.cpp │ │ ├── DatasetImageSegmentation.cpp │ │ ├── DatasetImageSegmentationDelayed.cpp │ │ ├── ImageOpticalFlowNVOF.cpp │ │ ├── ImageOpticalFlowSource.cpp │ │ ├── ImageSegmentationMeasurement.cpp │ │ ├── ImageSegmentationRenderer.cpp │ │ ├── MeshResource.cpp │ │ ├── ModelParameters.cpp │ │ ├── OpticalFlowQueueHandler.cpp │ │ ├── OpticalFlowUtilities.cpp │ │ ├── QuaternionModel.cpp │ │ ├── ROFTFilter.cpp │ │ ├── SICAD.cpp │ │ ├── SICADModel.cpp │ │ ├── SICADShader.cpp │ │ ├── SKFCorrection.cpp │ │ ├── SpatialVelocityModel.cpp │ │ └── UKFCorrection.cpp └── roft │ ├── CMakeLists.txt │ ├── include │ └── ConfigParser.h │ └── src │ ├── ConfigParser.cpp │ └── main.cpp ├── test ├── init.sh ├── post_process_results.sh ├── post_process_results_ho3d.sh ├── run_all_experiments ├── run_paper_experiments ├── test.sh ├── test_full.sh ├── test_full_all.sh ├── test_full_all_ho3d.sh ├── test_full_gt_mask.sh ├── test_full_gt_mask_all.sh ├── test_full_gt_mask_all_ho3d.sh ├── test_full_gt_mask_gt_pose.sh ├── test_full_gt_mask_gt_pose_all.sh ├── test_full_gt_mask_gt_pose_all_ho3d.sh ├── test_full_gt_mask_gt_pose_ho3d.sh ├── test_full_gt_mask_ho3d.sh ├── test_full_gt_pose.sh ├── test_full_gt_pose_all.sh ├── test_full_gt_pose_all_ho3d.sh ├── test_full_gt_pose_ho3d.sh ├── test_full_ho3d.sh ├── test_full_ho3d_mask_all_ho3d.sh ├── test_full_ho3d_mask_ho3d.sh ├── test_full_noflowaid.sh ├── test_full_noflowaid_all.sh ├── test_full_nooutrej.sh ├── test_full_nooutrej_all.sh ├── test_full_nopose.sh ├── test_full_nopose_all.sh ├── test_full_noposesync.sh ├── test_full_noposesync_all.sh ├── test_full_novel.sh ├── test_full_novel_all.sh └── test_ho3d.sh └── tools ├── CMakeLists.txt ├── dataset ├── conversion │ ├── conversion.py │ ├── convert_ho3d.py │ ├── ho3d_utils.py │ ├── nvdu_poses_to_ycbv.py │ ├── utils.py │ └── ycbv_poses_to_nvdu.py ├── data_txt_generation │ ├── generate_data_txt.py │ └── process_ho3d.sh ├── dope_pose_finder │ └── pose_finder.py ├── fc_masks_converter │ └── fc_masks_converter.py └── results_padding │ └── pad_results.py ├── detectron2 ├── LICENSE_Detectron2 ├── dataset.py ├── dataset_augmentation_tester.py ├── dataset_mapper.py ├── dataset_tester.py ├── evaluate.py ├── evaluate_ho3d.sh ├── generate_ho3d_dataset_description.sh ├── generate_ycbv_bop_pbr_dataset_description.sh ├── inference.py ├── predictor.py ├── process_fastycb.sh ├── process_ho3d.sh ├── results_saver.py ├── test_ho3d_dataset_description.sh ├── test_ycbv_bop_pbr_dataset_augmentation.sh ├── test_ycbv_bop_pbr_dataset_description.sh ├── train.py └── train_ycbv_bop_pbr.sh ├── dope ├── inference.py ├── process_fastycb.sh ├── process_fastycb_qual.sh └── process_ho3d.sh ├── download ├── download_fastycb.sh ├── download_results.sh └── download_ycb_models.sh ├── nvof ├── CMakeLists.txt └── dumper │ ├── CMakeLists.txt │ ├── process_ho3d.sh │ ├── process_ycbv_real.sh │ ├── process_ycbv_synthetic.sh │ └── src │ └── main.cpp ├── object_renderer ├── CMakeLists.txt ├── grid │ └── grid_template.xcf.zip ├── shotcut │ ├── process_ycbvr.sh │ ├── process_ycbvr_comparison.sh │ ├── process_ycbvs.sh │ ├── process_ycbvs_comparison.sh │ ├── project.mlt │ └── project_comparison.mlt └── src │ └── renderer.cpp ├── rs-capture ├── CMakeLists.txt └── src │ └── main.cpp ├── third_party ├── bop_pose_error.py └── download_google_drive_file.sh └── video └── compress /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: C++ CI Workflow 2 | 3 | on: 4 | push: 5 | pull_request: 6 | branches: 7 | - main 8 | schedule: 9 | - cron: '0 2 * * *' 10 | 11 | jobs: 12 | build: 13 | name: '[${{ matrix.os }}@${{ matrix.build_type }}]' 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | build_type: [Release] 18 | os: [ubuntu-20.04] 19 | 20 | steps: 21 | - uses: actions/checkout@v2 22 | 23 | - name: Remove broken apt repos [Ubuntu] 24 | if: contains(matrix.os, 'ubuntu') 25 | run: for apt_file in `grep -lr microsoft /etc/apt/sources.list.d/`; do sudo rm $apt_file; done 26 | 27 | - name: Dependencies [Ubuntu] 28 | if: contains(matrix.os, 'ubuntu') 29 | shell: bash -l {0} 30 | run: | 31 | sudo apt-get update 32 | sudo apt install -y cmake libassimp-dev libconfig++-dev libglfw3-dev libglew-dev libglm-dev libeigen3-dev libopencv-dev libpython3-dev libtclap-dev pybind11-dev 33 | 34 | - name: Dependencies (from source) [Ubuntu] 35 | if: contains(matrix.os, 'ubuntu') 36 | shell: bash -l {0} 37 | run: | 38 | # bayes-filters-lib 39 | cd ${GITHUB_WORKSPACE} 40 | git clone https://github.com/robotology/bayes-filters-lib 41 | cd bayes-filters-lib 42 | mkdir build 43 | cd build 44 | cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE} .. 45 | cmake --build . --config ${{ matrix.build_type }} --target install 46 | 47 | # superimpose-mesh-lib 48 | cd ${GITHUB_WORKSPACE} 49 | git clone https://github.com/robotology/ycm 50 | cd ycm 51 | mkdir build 52 | cd build 53 | cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE} .. 54 | cmake --build . --config ${{ matrix.build_type }} --target install 55 | 56 | cd ${GITHUB_WORKSPACE} 57 | git clone https://github.com/robotology/superimpose-mesh-lib 58 | cd superimpose-mesh-lib 59 | git checkout devel 60 | mkdir build 61 | cd build 62 | cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE} .. 63 | cmake --build . --config ${{ matrix.build_type }} --target install 64 | 65 | # robots-io 66 | cd ${GITHUB_WORKSPACE} 67 | git clone https://github.com/xenvre/robots-io 68 | cd robots-io 69 | mkdir build 70 | cd build 71 | cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE} .. 72 | cmake --build . --config ${{ matrix.build_type }} --target install 73 | 74 | - name: Configure CMake [Ubuntu] 75 | if: contains(matrix.os, 'ubuntu') 76 | shell: bash -l {0} 77 | run: | 78 | mkdir build 79 | cd build 80 | cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE} -DCMAKE_BUILD_TYPE=${{matrix.build_type}} -DBUILD_RENDERER=ON .. 81 | 82 | - name: Build 83 | shell: bash -l {0} 84 | run: | 85 | cd build 86 | cmake --build . --config ${{matrix.build_type}} 87 | -------------------------------------------------------------------------------- /.github/workflows/docker.yaml: -------------------------------------------------------------------------------- 1 | name: Publish Docker Image 2 | 3 | on: [workflow_dispatch] 4 | 5 | jobs: 6 | publish: 7 | runs-on: ubuntu-latest 8 | steps: 9 | - uses: actions/checkout@main 10 | - name: Publish to Registry 11 | uses: elgohr/Publish-Docker-Github-Action@main 12 | with: 13 | name: hsp-iit/roft 14 | username: ${{ github.actor }} 15 | password: ${{ secrets.GITHUB_TOKEN }} 16 | workdir: dockerfiles 17 | registry: ghcr.io 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ 2 | */__pycache__ 3 | build/* -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software, please cite it as below." 3 | authors: 4 | - family-names: "Piga" 5 | given-names: "Nicola Agostino" 6 | orcid: "https://orcid.org/0000-0003-3183-8108" 7 | - family-names: "Onyshchuk" 8 | given-names: "Yuriy" 9 | orcid: "https://orcid.org/0000-0003-1676-1261" 10 | - family-names: "Pasquale" 11 | given-names: "Giulia" 12 | orcid: "https://orcid.org/0000-0002-7221-3553" 13 | - family-names: "Pattacini" 14 | given-names: "Ugo" 15 | orcid: "https://orcid.org/0000-0001-8754-1632" 16 | - family-names: "Natale" 17 | given-names: "Lorenzo" 18 | orcid: "https://orcid.org/0000-0002-8777-5233" 19 | title: "roft" 20 | version: v1.2.1 21 | doi: 10.5281/zenodo.6396283 22 | date-released: 2022-11-21 23 | url: "https://github.com/hsp-iit/roft" 24 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | cmake_minimum_required(VERSION 3.16) 11 | 12 | project(ROFTSuite 13 | LANGUAGES CXX) 14 | 15 | include(GNUInstallDirs) 16 | 17 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") 18 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") 19 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") 20 | 21 | set(CMAKE_C_EXTENSIONS OFF) 22 | set(CMAKE_CXX_EXTENSIONS OFF) 23 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 24 | 25 | option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) 26 | 27 | # Enable RPATH 28 | include(AddInstallRPATHSupport) 29 | add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_FULL_BINDIR}" 30 | LIB_DIRS "${CMAKE_INSTALL_FULL_LIBDIR}" 31 | INSTALL_NAME_DIR "${CMAKE_INSTALL_FULL_LIBDIR}" 32 | USE_LINK_PATH) 33 | 34 | # Default build type to Release 35 | if(NOT CMAKE_CONFIGURATION_TYPES) 36 | if(NOT CMAKE_BUILD_TYPE) 37 | message(STATUS "Setting build type to 'Release' as none was specified.") 38 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release") 39 | endif() 40 | endif() 41 | 42 | # Default build type to Release 43 | if(NOT CMAKE_CONFIGURATION_TYPES) 44 | if(NOT CMAKE_BUILD_TYPE) 45 | message(STATUS "Setting build type to 'Release' as none was specified.") 46 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release") 47 | endif() 48 | endif() 49 | 50 | # Build library, executable and tools 51 | add_subdirectory(src/roft-lib) 52 | add_subdirectory(src/roft) 53 | add_subdirectory(tools) 54 | 55 | # Add standard uninstall target 56 | include(AddUninstallTarget) 57 | -------------------------------------------------------------------------------- /assets/scheme.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hsp-iit/roft/cee79752d9bc3759cc2ed02c1e57a278ae342d6d/assets/scheme.png -------------------------------------------------------------------------------- /cmake/AddUninstallTarget.cmake: -------------------------------------------------------------------------------- 1 | #.rst: 2 | # AddUninstallTarget 3 | # ------------------ 4 | # 5 | # Add the "uninstall" target for your project:: 6 | # 7 | # include(AddUninstallTarget) 8 | # 9 | # 10 | # will create a file ``cmake_uninstall.cmake`` in the build directory and add a 11 | # custom target ``uninstall`` (or ``UNINSTALL`` on Visual Studio and Xcode) that 12 | # will remove the files installed by your package (using 13 | # ``install_manifest.txt``). 14 | # See also 15 | # https://gitlab.kitware.com/cmake/community/wikis/FAQ#can-i-do-make-uninstall-with-cmake 16 | # 17 | # The :module:`AddUninstallTarget` module must be included in your main 18 | # ``CMakeLists.txt``. If included in a subdirectory it does nothing. 19 | # This allows you to use it safely in your main ``CMakeLists.txt`` and include 20 | # your project using ``add_subdirectory`` (for example when using it with 21 | # :cmake:module:`FetchContent`). 22 | # 23 | # If the ``uninstall`` target already exists, the module does nothing. 24 | 25 | #============================================================================= 26 | # Copyright 2008-2013 Kitware, Inc. 27 | # Copyright 2013 Istituto Italiano di Tecnologia (IIT) 28 | # Authors: Daniele E. Domenichelli 29 | # 30 | # Distributed under the OSI-approved BSD License (the "License"); 31 | # see accompanying file Copyright.txt for details. 32 | # 33 | # This software is distributed WITHOUT ANY WARRANTY; without even the 34 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 35 | # See the License for more information. 36 | #============================================================================= 37 | # (To distribute this file outside of CMake, substitute the full 38 | # License text for the above reference.) 39 | 40 | 41 | # AddUninstallTarget works only when included in the main CMakeLists.txt 42 | if(NOT "${CMAKE_CURRENT_BINARY_DIR}" STREQUAL "${CMAKE_BINARY_DIR}") 43 | return() 44 | endif() 45 | 46 | # The name of the target is uppercase in MSVC and Xcode (for coherence with the 47 | # other standard targets) 48 | if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") 49 | set(_uninstall "UNINSTALL") 50 | else() 51 | set(_uninstall "uninstall") 52 | endif() 53 | 54 | # If target is already defined don't do anything 55 | if(TARGET ${_uninstall}) 56 | return() 57 | endif() 58 | 59 | 60 | set(_filename cmake_uninstall.cmake) 61 | 62 | file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/${_filename}" 63 | "if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") 64 | message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") 65 | return() 66 | endif() 67 | 68 | file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files) 69 | string(STRIP \"\${files}\" files) 70 | string(REGEX REPLACE \"\\n\" \";\" files \"\${files}\") 71 | list(REVERSE files) 72 | foreach(file \${files}) 73 | if(IS_SYMLINK \"\$ENV{DESTDIR}\${file}\" OR EXISTS \"\$ENV{DESTDIR}\${file}\") 74 | message(STATUS \"Uninstalling: \$ENV{DESTDIR}\${file}\") 75 | execute_process( 76 | COMMAND \${CMAKE_COMMAND} -E remove \"\$ENV{DESTDIR}\${file}\" 77 | OUTPUT_VARIABLE rm_out 78 | RESULT_VARIABLE rm_retval) 79 | if(NOT \"\${rm_retval}\" EQUAL 0) 80 | message(FATAL_ERROR \"Problem when removing \\\"\$ENV{DESTDIR}\${file}\\\"\") 81 | endif() 82 | else() 83 | message(STATUS \"Not-found: \$ENV{DESTDIR}\${file}\") 84 | endif() 85 | endforeach(file) 86 | ") 87 | 88 | set(_desc "Uninstall the project...") 89 | if(CMAKE_GENERATOR STREQUAL "Unix Makefiles") 90 | set(_comment COMMAND \$\(CMAKE_COMMAND\) -E cmake_echo_color --switch=$\(COLOR\) --cyan "${_desc}") 91 | else() 92 | set(_comment COMMENT "${_desc}") 93 | endif() 94 | add_custom_target(${_uninstall} 95 | ${_comment} 96 | COMMAND ${CMAKE_COMMAND} -P ${_filename} 97 | USES_TERMINAL 98 | BYPRODUCTS uninstall_byproduct 99 | WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") 100 | set_property(SOURCE uninstall_byproduct PROPERTY SYMBOLIC 1) 101 | 102 | set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets") 103 | -------------------------------------------------------------------------------- /cmake/CMakeRC_LICENSE/LICENSE.txt: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 vector-of-bool 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. -------------------------------------------------------------------------------- /config/classes.txt: -------------------------------------------------------------------------------- 1 | 003_cracker_box 2 | 004_sugar_box 3 | 005_tomato_soup_can 4 | 006_mustard_bottle 5 | 009_gelatin_box 6 | 010_potted_meat_can 7 | -------------------------------------------------------------------------------- /config/classes_ho3d.txt: -------------------------------------------------------------------------------- 1 | 003_cracker_box 2 | 004_sugar_box 3 | 006_mustard_bottle 4 | 010_potted_meat_can 5 | -------------------------------------------------------------------------------- /config/config_fast_ycb.cfg: -------------------------------------------------------------------------------- 1 | sample_time = 0.033333333333; 2 | 3 | camera_dataset: 4 | { 5 | width = 1280; 6 | height = 720; 7 | fx = 1229.4285612615463; 8 | fy = 1229.4285612615463; 9 | cx = 640.0; 10 | cy = 360.0; 11 | path = "?"; 12 | data_prefix = "/"; 13 | rgb_prefix = "rgb/"; 14 | depth_prefix = "depth/"; 15 | data_format = "txt"; 16 | rgb_format = "png"; 17 | depth_format = "float"; 18 | heading_zeros = 0; 19 | index_offset = 0; 20 | } 21 | 22 | initial_condition: 23 | { 24 | pose: 25 | { 26 | v = [0.0, 0.0, 0.0]; 27 | w = [0.0, 0.0, 0.0]; 28 | x = [0.0, 0.0, 0.0]; 29 | axis_angle = [1.0, 0.0, 0.0, 0.0]; 30 | 31 | cov_v = [0.001, 0.001, 0.001]; 32 | cov_w = [0.001, 0.001, 0.001]; 33 | cov_x = [0.001, 0.001, 0.001]; 34 | cov_q = [0.001, 0.001, 0.001]; 35 | } 36 | 37 | velocity: 38 | { 39 | v = [0.0, 0.0, 0.0]; 40 | w = [0.0, 0.0, 0.0]; 41 | 42 | cov_v = [0.001, 0.001, 0.001]; 43 | cov_w = [0.001, 0.001, 0.001]; 44 | } 45 | } 46 | 47 | kinematic_model: 48 | { 49 | pose: 50 | { 51 | sigma_linear = [1.0, 1.0, 1.0]; 52 | sigma_angular = [1.0, 1.0, 1.0]; 53 | } 54 | 55 | velocity: 56 | { 57 | sigma_linear = [0.1, 0.1, 0.1]; 58 | sigma_angular = [0.1, 0.1, 0.1]; 59 | } 60 | } 61 | 62 | log: 63 | { 64 | enable = true; 65 | enable_segmentation = false; 66 | path = "?"; 67 | } 68 | 69 | measurement_model: 70 | { 71 | pose: 72 | { 73 | cov_v = [0.1, 0.1, 0.1]; 74 | cov_w = [0.0001, 0.0001, 0.0001]; 75 | cov_x = [0.001, 0.001, 0.001]; 76 | cov_q = [0.0001, 0.0001, 0.0001]; 77 | } 78 | 79 | velocity: 80 | { 81 | cov_flow = [1.0, 1.0]; 82 | depth_maximum = 2.0; 83 | subsampling_radius = 35.0; 84 | weight_flow = true; 85 | } 86 | 87 | use_pose = true; 88 | use_pose_resync = true; 89 | use_velocity = true; 90 | } 91 | 92 | model: 93 | { 94 | name = "?"; 95 | use_internal_db = true; 96 | internal_db_name = "DOPE"; 97 | external_path = "?"; 98 | } 99 | 100 | optical_flow_dataset: 101 | { 102 | path = "?"; 103 | set = "nvof"; 104 | heading_zeros = 0; 105 | index_offset = 0; 106 | } 107 | 108 | outlier_rejection: 109 | { 110 | enable = true; 111 | gain = 0.01; 112 | } 113 | 114 | pose_dataset: 115 | { 116 | path = "?"; 117 | skip_rows = 0; 118 | skip_cols = 0; 119 | fps_reduction = true; 120 | delay = true; 121 | original_fps = 30.0; 122 | desired_fps = 5.0; 123 | } 124 | 125 | segmentation_dataset: 126 | { 127 | path = "?"; 128 | format = "png"; 129 | set = "mrcnn"; 130 | heading_zeros = 0; 131 | index_offset = 0; 132 | fps_reduction = true; 133 | delay = true; 134 | original_fps = 30.0; 135 | desired_fps = 5.0; 136 | flow_aided = true; 137 | } 138 | 139 | unscented_transform: 140 | { 141 | alpha = 1.0; 142 | beta = 2.0; 143 | kappa = 0.0; 144 | } -------------------------------------------------------------------------------- /config/config_ho3d.cfg: -------------------------------------------------------------------------------- 1 | sample_time = 0.033333333333; 2 | 3 | camera_dataset: 4 | { 5 | width = 640; 6 | height = 480; 7 | fx = 0.0; 8 | fy = 0.0; 9 | cx = 0.0; 10 | cy = 0.0; 11 | path = "?"; 12 | data_prefix = "/"; 13 | rgb_prefix = "rgb/"; 14 | depth_prefix = "depth/"; 15 | data_format = "txt"; 16 | rgb_format = "png"; 17 | depth_format = "float"; 18 | heading_zeros = 0; 19 | index_offset = 0; 20 | } 21 | 22 | initial_condition: 23 | { 24 | pose: 25 | { 26 | v = [0.0, 0.0, 0.0]; 27 | w = [0.0, 0.0, 0.0]; 28 | x = [0.0, 0.0, 0.0]; 29 | axis_angle = [1.0, 0.0, 0.0, 0.0]; 30 | 31 | cov_v = [0.001, 0.001, 0.001]; 32 | cov_w = [0.001, 0.001, 0.001]; 33 | cov_x = [0.001, 0.001, 0.001]; 34 | cov_q = [0.001, 0.001, 0.001]; 35 | } 36 | 37 | velocity: 38 | { 39 | v = [0.0, 0.0, 0.0]; 40 | w = [0.0, 0.0, 0.0]; 41 | 42 | cov_v = [0.001, 0.001, 0.001]; 43 | cov_w = [0.001, 0.001, 0.001]; 44 | } 45 | } 46 | 47 | kinematic_model: 48 | { 49 | pose: 50 | { 51 | sigma_linear = [1.0, 1.0, 1.0]; 52 | sigma_angular = [1.0, 1.0, 1.0]; 53 | } 54 | 55 | velocity: 56 | { 57 | sigma_linear = [0.1, 0.1, 0.1]; 58 | sigma_angular = [0.1, 0.1, 0.1]; 59 | } 60 | } 61 | 62 | log: 63 | { 64 | enable = true; 65 | enable_segmentation = false; 66 | path = "?"; 67 | } 68 | 69 | measurement_model: 70 | { 71 | pose: 72 | { 73 | cov_v = [0.1, 0.1, 0.1]; 74 | cov_w = [0.0001, 0.0001, 0.0001]; 75 | cov_x = [0.001, 0.001, 0.001]; 76 | cov_q = [0.0001, 0.0001, 0.0001]; 77 | } 78 | 79 | velocity: 80 | { 81 | cov_flow = [1.0, 1.0]; 82 | depth_maximum = 2.0; 83 | subsampling_radius = 35.0; 84 | weight_flow = true; 85 | } 86 | 87 | use_pose = true; 88 | use_pose_resync = true; 89 | use_velocity = true; 90 | } 91 | 92 | model: 93 | { 94 | name = "?"; 95 | use_internal_db = true; 96 | internal_db_name = "DOPE"; 97 | external_path = "?"; 98 | } 99 | 100 | optical_flow_dataset: 101 | { 102 | path = "?"; 103 | set = "nvof"; 104 | heading_zeros = 0; 105 | index_offset = 0; 106 | } 107 | 108 | outlier_rejection: 109 | { 110 | enable = true; 111 | gain = 0.01; 112 | } 113 | 114 | pose_dataset: 115 | { 116 | path = "?"; 117 | skip_rows = 0; 118 | skip_cols = 0; 119 | fps_reduction = true; 120 | delay = true; 121 | original_fps = 30.0; 122 | desired_fps = 5.0; 123 | } 124 | 125 | segmentation_dataset: 126 | { 127 | path = "?"; 128 | format = "png"; 129 | set = "mrcnn"; 130 | heading_zeros = 0; 131 | index_offset = 0; 132 | fps_reduction = true; 133 | delay = true; 134 | original_fps = 30.0; 135 | desired_fps = 5.0; 136 | flow_aided = true; 137 | } 138 | 139 | unscented_transform: 140 | { 141 | alpha = 1.0; 142 | beta = 2.0; 143 | kappa = 0.0; 144 | } -------------------------------------------------------------------------------- /docs/detectron2_requirements.txt: -------------------------------------------------------------------------------- 1 | torch==1.6.0 2 | torchvision==0.7.0 3 | imgaug==0.4.0 4 | -------------------------------------------------------------------------------- /docs/detectron2_requirements_cuda11.txt: -------------------------------------------------------------------------------- 1 | torch==1.8.0 2 | torchvision==0.9.0 3 | imgaug==0.4.0 4 | -------------------------------------------------------------------------------- /docs/dope_requirements.txt: -------------------------------------------------------------------------------- 1 | pyrr==0.10.3 2 | torch==1.6.0 3 | torchvision==0.7.0 4 | numpy==1.17.4 5 | scipy==1.5.2 6 | opencv_python==4.4.0.44 7 | Pillow==7.2.0 8 | configparser==5.0.0 9 | pyyaml 10 | -------------------------------------------------------------------------------- /docs/installation.md: -------------------------------------------------------------------------------- 1 | We suggest having two seprate virtual environments for `DOPE` and `Detectron 2`. 2 | 3 | We assume that `CUDA 10.1` is adopted. 4 | 5 | ### DOPE 6 | 7 | Just install the provided [requirements](/docs/dope_requirements.txt). 8 | 9 | ### Detectron 2 10 | 11 | Install the provided [requirements](/docs/detectron2_requirements.txt). 12 | 13 | Then proceed with `detectron 2` installation as follows: 14 | 15 | ``` 16 | python -m pip install detectron2 -f https://dl.fbaipublicfiles.com/detectron2/wheels/cu101/torch1.6/index.html 17 | ``` 18 | 19 | -------------------------------------------------------------------------------- /evaluation/objects.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | class Objects(): 11 | 12 | def __init__(self): 13 | """Constructor""" 14 | 15 | self.objects = {} 16 | 17 | self.objects['models'] =\ 18 | [ 19 | '003_cracker_box', 20 | '004_sugar_box', 21 | '005_tomato_soup_can', 22 | '006_mustard_bottle', 23 | '009_gelatin_box', 24 | '010_potted_meat_can' 25 | ] 26 | 27 | self.objects['fastycb'] =\ 28 | [ 29 | '003_cracker_box', 30 | '004_sugar_box', 31 | '005_tomato_soup_can', 32 | '006_mustard_bottle', 33 | '009_gelatin_box', 34 | '010_potted_meat_can', 35 | 'ALL' 36 | ] 37 | 38 | self.objects['fastycb_qual'] =\ 39 | [ 40 | '003_cracker_box', 41 | '006_mustard_bottle' 42 | ] 43 | 44 | self.objects['ho3d'] =\ 45 | [ 46 | '003_cracker_box', 47 | '004_sugar_box', 48 | '006_mustard_bottle', 49 | '010_potted_meat_can', 50 | 'ALL' 51 | ] 52 | -------------------------------------------------------------------------------- /evaluation/run_all_experiments_evaluation: -------------------------------------------------------------------------------- 1 | if [ -f roft_env/bin/activate ]; then 2 | . roft_env/bin/activate 3 | fi 4 | 5 | python evaluation/evaluate.py --experiment-name exp_ho3d --metric-name mix --output-head latex 6 | python evaluation/evaluate.py --experiment-name exp_ho3d_ablation --metric-name mix --output-head latex-summary 7 | python evaluation/evaluate.py --experiment-name exp_ho3d_roft --metric-name time --output-head latex-summary 8 | python evaluation/evaluate.py --experiment-name exp_fastycb --metric-name mix --output-head latex 9 | python evaluation/evaluate.py --experiment-name exp_fastycb --output-head thumbnail 10 | python evaluation/evaluate.py --experiment-name exp_fastycb_velocities --metric-name rmse_velocity --output-head latex 11 | python evaluation/evaluate.py --experiment-name exp_fastycb_roft --metric-name max_velocity --output-head latex 12 | python evaluation/evaluate.py --experiment-name exp_fastycb_roft --metric-name time --output-head latex-summary 13 | python evaluation/evaluate.py --experiment-name exp_fastycb_se3_plain --metric-name add --output-head latex 14 | python evaluation/evaluate.py --experiment-name exp_fastycb_ablation --metric-name mix --output-head latex-summary 15 | # python evaluation/evaluate.py --experiment-name exp_fastycb --output-head video 16 | # python evaluation/evaluate.py --experiment-name exp_fastycb_qual --output-head video 17 | # python evaluation/evaluate.py --experiment-name exp_fastycb_roft --output-head video-segmentation 18 | # python evaluation/evaluate.py --experiment-name exp_fastycb_qual_roft --output-head video-segmentation 19 | 20 | if [ -f roft_env/bin/activate ]; then 21 | deactivate 22 | fi -------------------------------------------------------------------------------- /evaluation/run_paper_evaluation: -------------------------------------------------------------------------------- 1 | if [ -f roft_env/bin/activate ]; then 2 | . roft_env/bin/activate 3 | fi 4 | 5 | # Table I 6 | python evaluation/evaluate.py --experiment-name exp_fastycb --metric-name mix --output-head latex 7 | pdflatex -output-directory=./evaluation_output evaluation_output/exp_fastycb_mix.tex 8 | mv evaluation_output/exp_fastycb_mix.pdf evaluation_output/tableI.pdf 9 | 10 | # Table II 11 | python evaluation/evaluate.py --experiment-name exp_fastycb_velocities --metric-name rmse_velocity --output-head latex 12 | pdflatex -output-directory=./evaluation_output evaluation_output/exp_fastycb_velocities_rmse_velocity.tex 13 | mv evaluation_output/exp_fastycb_velocities_rmse_velocity.pdf evaluation_output/tableII.pdf 14 | 15 | # Table IV 16 | python evaluation/evaluate.py --experiment-name exp_fastycb_ablation --metric-name mix --output-head latex-summary 17 | pdflatex -output-directory=./evaluation_output evaluation_output/exp_fastycb_ablation_mix.tex 18 | mv evaluation_output/exp_fastycb_ablation_mix.pdf evaluation_output/tableIV.pdf 19 | 20 | # Figure 3 21 | python evaluation/evaluate.py --experiment-name exp_fastycb --output-head thumbnail 22 | mv evaluation_output/exp_fastycb/thumb.png evaluation_output/Fig3_left.png 23 | 24 | python evaluation/evaluate.py --experiment-name exp_fastycb_qual --output-head thumbnail 25 | mv evaluation_output/exp_fastycb_qual/thumb.png evaluation_output/Fig3_right.png 26 | 27 | rm evaluation_output/*.log 28 | rm evaluation_output/*.aux 29 | rm -r evaluation_output/exp_fastycb 30 | rm -r evaluation_output/exp_fastycb_qual 31 | 32 | if [ -f roft_env/bin/activate ]; then 33 | deactivate 34 | fi -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | ffmpeg-python 2 | markdown-table 3 | matplotlib 4 | numpy 5 | opencv-python 6 | pybind11 7 | pytransform3d 8 | pyquaternion 9 | pyrender 10 | tqdm 11 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup(name='tools', packages=['tools']) 4 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/CameraMeasurement.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_CAMERAMEASUREMENTS_H 9 | #define ROFT_CAMERAMEASUREMENTS_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | 21 | namespace ROFT { 22 | class CameraMeasurement; 23 | 24 | enum class CameraMeasurementType { RGB, D, RGBD, PC , RGBPC}; 25 | } 26 | 27 | 28 | class ROFT::CameraMeasurement : public bfl::MeasurementModel 29 | { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | 33 | CameraMeasurement(std::shared_ptr camera); 34 | 35 | ~CameraMeasurement(); 36 | 37 | bool freeze(const bfl::Data& type = bfl::Data()) override; 38 | 39 | std::pair measure(const bfl::Data& data = bfl::Data()) const override; 40 | 41 | std::pair predictedMeasure(const Eigen::Ref& cur_states) const override; 42 | 43 | std::pair innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const override; 44 | 45 | std::pair camera_parameters() const; 46 | 47 | std::pair camera_deprojection_matrix() const; 48 | 49 | std::pair camera_time_stamp_rgb() const; 50 | 51 | std::pair camera_time_stamp_depth() const; 52 | 53 | std::int32_t camera_frame_index() const; 54 | 55 | void reset() const; 56 | 57 | using CameraMeasurementTuple = std::tuple, cv::Mat, Eigen::MatrixXf>; 58 | 59 | private: 60 | std::shared_ptr camera_; 61 | 62 | cv::Mat rgb_; 63 | 64 | Eigen::MatrixXf depth_; 65 | 66 | Eigen::MatrixXd point_cloud_; 67 | 68 | Eigen::Transform pose_; 69 | 70 | double time_stamp_rgb_; 71 | 72 | double time_stamp_depth_; 73 | 74 | bool is_time_stamp_rgb_ = false; 75 | 76 | bool is_time_stamp_depth_ = false; 77 | 78 | ROFT::CameraMeasurementType measure_type_; 79 | 80 | bool measurement_available_ = false; 81 | 82 | std::string log_name_ = "CameraMeasurements"; 83 | }; 84 | 85 | #endif /* ROFT_CAMERAMEASUREMENTS_H */ 86 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/CartesianQuaternionMeasurement.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_CARTESIANQUATERNIONMEASUREMENT_H 9 | #define ROFT_CARTESIANQUATERNIONMEASUREMENT_H 10 | 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | namespace ROFT { 25 | class CartesianQuaternionMeasurement; 26 | } 27 | 28 | 29 | class ROFT::CartesianQuaternionMeasurement : public bfl::MeasurementModel 30 | { 31 | public: 32 | CartesianQuaternionMeasurement(std::shared_ptr pose_measurement, std::shared_ptr velocity_measurement, const bool use_screw_velocity, const bool use_pose_measurement, const bool use_velocity_measurement, const Eigen::Ref sigma_position, const Eigen::Ref sigma_quaternion, const Eigen::Ref sigma_linear_velocity, const Eigen::Ref sigma_angular_velocity, const bool enable_log); 33 | 34 | CartesianQuaternionMeasurement(std::shared_ptr pose_measurement, std::shared_ptr velocity_measurement, std::shared_ptr camera_measurement, std::shared_ptr segmentation_measurement, const bool use_screw_velocity, const bool use_pose_measurement, const bool use_velocity_measurement, const Eigen::Ref sigma_position, const Eigen::Ref sigma_quaternion, const Eigen::Ref sigma_linear_velocity, const Eigen::Ref sigma_angular_velocity, const bool wait_source_initialization, const bool enable_log); 35 | 36 | virtual ~CartesianQuaternionMeasurement(); 37 | 38 | bool freeze(const bfl::Data& data = bfl::Data()) override; 39 | 40 | std::pair measure(const bfl::Data& data = bfl::Data()) const override; 41 | 42 | std::pair predictedMeasure(const Eigen::Ref& current_states) const override; 43 | 44 | std::pair innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const override; 45 | 46 | std::pair getNoiseCovarianceMatrix() const override; 47 | 48 | bfl::VectorDescription getInputDescription() const override; 49 | 50 | bfl::VectorDescription getMeasurementDescription() const override; 51 | 52 | bool setProperty(const std::string& property) override; 53 | 54 | enum class MeasurementMode { Standard, RepeatOnlyVelocity, PopBufferedMeasurement }; 55 | 56 | enum class TransformFeedback { None, RGB, DepthSegmentation }; 57 | 58 | protected: 59 | std::vector log_file_names(const std::string& prefix_path, const std::string& prefix_name) override; 60 | 61 | private: 62 | std::shared_ptr pose_measurement_; 63 | 64 | std::shared_ptr velocity_measurement_; 65 | 66 | std::shared_ptr camera_measurement_; 67 | 68 | std::shared_ptr segmentation_measurement_; 69 | 70 | bool is_velocity_measurement_degenerate_ = false; 71 | 72 | Eigen::MatrixXd measurement_; 73 | 74 | Eigen::MatrixXd noise_covariance_; 75 | 76 | Eigen::MatrixXd R_pose_; 77 | 78 | Eigen::MatrixXd R_velocity_; 79 | 80 | Eigen::MatrixXd R_pose_velocity_; 81 | 82 | Eigen::Vector3d last_linear_velocity_; 83 | 84 | Eigen::Vector3d last_angular_velocity_; 85 | 86 | Eigen::Transform last_pose_; 87 | 88 | std::deque buffer_velocities_; 89 | 90 | enum class MeasurementType { Pose, Velocity, PoseVelocity, None }; 91 | 92 | MeasurementType measurement_type_ = MeasurementType::None; 93 | 94 | MeasurementMode measurement_mode_ = MeasurementMode::Standard; 95 | 96 | TransformFeedback transform_feedback_ = TransformFeedback::None; 97 | 98 | bfl::VectorDescription input_description_; 99 | 100 | bfl::VectorDescription measurement_description_; 101 | 102 | bool is_first_velocity_in_ = false; 103 | 104 | bool is_pose_ = false; 105 | 106 | bool use_screw_velocity_; 107 | 108 | bool use_pose_measurement_; 109 | 110 | bool use_velocity_measurement_; 111 | 112 | const bool wait_source_initialization_default_; 113 | 114 | bool wait_source_initialization_; 115 | 116 | int pose_frames_between_iterations_; 117 | 118 | bool enable_log_; 119 | 120 | const std::string log_name_ = "CartesianQuaternionMeasurement"; 121 | }; 122 | 123 | #endif /* ROFT_CARTESIANQUATERNIONMEASUREMENT_H */ 124 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/CartesianQuaternionModel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_CARTESIANQUATERNIONMODEL_H 9 | #define ROFT_CARTESIANQUATERNIONMODEL_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace ROFT { 19 | class CartesianQuaternionModel; 20 | } 21 | 22 | 23 | class ROFT::CartesianQuaternionModel : public bfl::StateModel 24 | { 25 | public: 26 | CartesianQuaternionModel(const Eigen::Ref psd_linear_acceleration, const Eigen::Ref sigma_angular_velocity, const double sample_time); 27 | 28 | virtual ~CartesianQuaternionModel(); 29 | 30 | virtual void propagate(const Eigen::Ref& cur_states, Eigen::Ref mot_states) override; 31 | 32 | virtual void motion(const Eigen::Ref& cur_states, Eigen::Ref mot_states) override; 33 | 34 | bool setSamplingTime(const double& sample_time) override; 35 | 36 | bool setProperty(const std::string& property) override; 37 | 38 | Eigen::MatrixXd getNoiseCovarianceMatrix() override; 39 | 40 | bfl::VectorDescription getInputDescription() override; 41 | 42 | bfl::VectorDescription getStateDescription() override; 43 | 44 | private: 45 | void evaluate_noise_covariance_matrix(const double& T); 46 | 47 | /** 48 | * Input and state descriptions. 49 | */ 50 | bfl::VectorDescription input_description_; 51 | 52 | bfl::VectorDescription state_description_; 53 | 54 | /** 55 | * Noise covariance matrix. 56 | */ 57 | Eigen::MatrixXd Q_; 58 | 59 | /** 60 | * Squared power spectral density for the linear acceleration part 61 | */ 62 | Eigen::MatrixXd psd_linear_acceleration_; 63 | 64 | /** 65 | * Variance for the angular velocity part 66 | */ 67 | Eigen::MatrixXd sigma_angular_velocity_; 68 | 69 | /** 70 | * Sample time estimation 71 | */ 72 | std::chrono::steady_clock::time_point last_time_; 73 | 74 | double sample_time_; 75 | 76 | bool last_time_set_ = false; 77 | }; 78 | 79 | #endif /* ROFT_CARTESIANQUATERNIONMODEL_H */ 80 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/DatasetImageOpticalFlow.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_DATASETIMAGEOPTICALFLOW_H 9 | #define ROFT_DATASETIMAGEOPTICALFLOW_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace ROFT { 17 | class DatasetImageOpticalFlow; 18 | } 19 | 20 | 21 | class ROFT::DatasetImageOpticalFlow : public ROFT::ImageOpticalFlowSource 22 | { 23 | public: 24 | DatasetImageOpticalFlow(const std::string& dataset_path, const std::string& set, const std::size_t width, const std::size_t height, const std::size_t& heading_zeros = 0, const std::size_t& index_offset = 0); 25 | 26 | ~DatasetImageOpticalFlow(); 27 | 28 | bool reset() override; 29 | 30 | bool step_frame() override; 31 | 32 | bool is_stepping_required() const override; 33 | 34 | std::size_t get_grid_size() const override; 35 | 36 | float get_scaling_factor() const override; 37 | 38 | double get_data_loading_time() const override; 39 | 40 | int get_matrix_type() const override; 41 | 42 | std::tuple flow(const bool& blocking) override; 43 | 44 | private: 45 | std::string compose_file_name(const int& index, const std::size_t& number_of_digits); 46 | 47 | std::string dataset_path_; 48 | 49 | const std::size_t width_; 50 | 51 | const std::size_t height_; 52 | 53 | int head_; 54 | 55 | const std::size_t index_offset_; 56 | 57 | const std::size_t heading_zeros_; 58 | 59 | std::size_t grid_size_; 60 | 61 | float scaling_factor_; 62 | 63 | int matrix_type_; 64 | 65 | std::tuple output_; 66 | 67 | double data_loading_time_ = 0.0; 68 | 69 | const std::string log_name_ = "DatasetImageOpticalFlow"; 70 | }; 71 | 72 | #endif /* ROFT_DATASETIMAGEOPTICALFLOW_H */ 73 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/DatasetImageSegmentation.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_DATASETIMAGESEGMENTATION_H 9 | #define ROFT_DATASETIMAGESEGMENTATION_H 10 | 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace ROFT { 19 | class DatasetImageSegmentation; 20 | } 21 | 22 | 23 | class ROFT::DatasetImageSegmentation : public RobotsIO::Utils::Segmentation 24 | { 25 | public: 26 | DatasetImageSegmentation(const std::string& dataset_path, const std::string& format, const std::size_t width, const std::size_t height, const std::string& segmentation_set, const ROFT::ModelParameters& model_parameters, const std::size_t heading_zeros = 0, const std::size_t index_offset = 0, const bool simulate_missing_detections = false); 27 | 28 | ~DatasetImageSegmentation(); 29 | 30 | bool reset() override; 31 | 32 | bool step_frame() override; 33 | 34 | bool is_stepping_required() const override; 35 | 36 | void reset_data_loading_time() override; 37 | 38 | double get_data_loading_time() const override; 39 | 40 | std::pair segmentation(const bool& blocking) override; 41 | 42 | protected: 43 | std::string compose_file_name(const int& index, const std::size_t& number_of_digits); 44 | 45 | std::pair read_file(const std::size_t& file_name); 46 | 47 | std::string dataset_path_; 48 | 49 | const std::string format_; 50 | 51 | const std::size_t width_; 52 | 53 | const std::size_t height_; 54 | 55 | std::string object_name_; 56 | 57 | int head_; 58 | 59 | const std::size_t index_offset_; 60 | 61 | const std::size_t heading_zeros_; 62 | 63 | const bool simulate_missing_detections_; 64 | 65 | std::pair output_; 66 | 67 | double data_loading_time_ = 0.0; 68 | 69 | const std::string log_name_ = "DatasetImageSegmentation"; 70 | }; 71 | 72 | #endif /* ROFT_DATASETIMAGESEGMENTATION_H */ 73 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/DatasetImageSegmentationDelayed.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_DATASETIMAGESEGMENTATIONDELAYED_H 9 | #define ROFT_DATASETIMAGESEGMENTATIONDELAYED_H 10 | 11 | #include 12 | 13 | namespace ROFT { 14 | class DatasetImageSegmentationDelayed; 15 | } 16 | 17 | 18 | class ROFT::DatasetImageSegmentationDelayed : public ROFT::DatasetImageSegmentation 19 | { 20 | public: 21 | DatasetImageSegmentationDelayed(const float& fps, const float& simulated_fps, const bool simulate_inference_time, const std::string& dataset_path, const std::string& format, const std::size_t width, const std::size_t height, const std::string& segmentation_set, const ROFT::ModelParameters& model_parameters, const std::size_t heading_zeros = 0, const std::size_t index_offset = 0); 22 | 23 | ~DatasetImageSegmentationDelayed(); 24 | 25 | std::pair segmentation(const bool& blocking) override; 26 | 27 | void reset_data_loading_time() override; 28 | 29 | double get_data_loading_time() const override; 30 | 31 | int get_frames_between_iterations() const override; 32 | 33 | private: 34 | const float fps_; 35 | 36 | const float simulated_fps_; 37 | 38 | const bool simulate_inference_time_; 39 | 40 | const int head_0_; 41 | 42 | const int delay_; 43 | 44 | double data_loading_time_ = 0.0; 45 | }; 46 | 47 | #endif /* ROFT_DATASETIMAGESEGMENTATIONDELAYED_H */ 48 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/ImageOpticalFlowNVOF.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_IMAGEOPTICALFLOWNVOF_H 9 | #define ROFT_IMAGEOPTICALFLOWNVOF_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace ROFT { 20 | class ImageOpticalFlowNVOF; 21 | } 22 | 23 | 24 | class ROFT::ImageOpticalFlowNVOF : public ROFT::ImageOpticalFlowSource 25 | { 26 | public: 27 | enum class NVOFPerformance_1_0 : std::underlying_type::type 28 | { 29 | Slow = cv::cuda::NvidiaOpticalFlow_1_0::NVIDIA_OF_PERF_LEVEL::NV_OF_PERF_LEVEL_SLOW, 30 | Medium = cv::cuda::NvidiaOpticalFlow_1_0::NVIDIA_OF_PERF_LEVEL::NV_OF_PERF_LEVEL_MEDIUM, 31 | Fast = cv::cuda::NvidiaOpticalFlow_1_0::NVIDIA_OF_PERF_LEVEL::NV_OF_PERF_LEVEL_FAST 32 | }; 33 | 34 | #if CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION == 5 && CV_SUBMINOR_VERSION >= 2 35 | enum class NVOFPerformance_2_0 : std::underlying_type::type 36 | { 37 | Slow = cv::cuda::NvidiaOpticalFlow_2_0::NVIDIA_OF_PERF_LEVEL::NV_OF_PERF_LEVEL_SLOW, 38 | Medium = cv::cuda::NvidiaOpticalFlow_2_0::NVIDIA_OF_PERF_LEVEL::NV_OF_PERF_LEVEL_MEDIUM, 39 | Fast = cv::cuda::NvidiaOpticalFlow_2_0::NVIDIA_OF_PERF_LEVEL::NV_OF_PERF_LEVEL_FAST 40 | }; 41 | #endif 42 | 43 | ImageOpticalFlowNVOF(std::shared_ptr camera_measurement, const NVOFPerformance_1_0& performance_setting, const bool use_temporal_hints = false); 44 | 45 | #if CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION == 5 && CV_SUBMINOR_VERSION >= 2 46 | ImageOpticalFlowNVOF(std::shared_ptr camera_measurement, const NVOFPerformance_2_0& performance_setting, const bool use_temporal_hints = false); 47 | #endif 48 | 49 | ~ImageOpticalFlowNVOF(); 50 | 51 | bool step_frame() override; 52 | 53 | std::tuple flow(const bool& blocking) override; 54 | 55 | bool is_stepping_required() const override; 56 | 57 | std::size_t get_grid_size() const override; 58 | 59 | float get_scaling_factor() const override; 60 | 61 | int get_matrix_type() const override; 62 | 63 | private: 64 | const std::shared_ptr camera_measurement_; 65 | 66 | RobotsIO::Camera::CameraParameters camera_parameters_; 67 | 68 | cv::Ptr nvof_1_0_; 69 | 70 | #if CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION == 5 && CV_SUBMINOR_VERSION >= 2 71 | cv::Ptr nvof_2_0_; 72 | #endif 73 | 74 | cv::cuda::GpuMat gpu_last_frame_; 75 | 76 | cv::Mat flow_; 77 | 78 | bool last_frame_in_ = false; 79 | 80 | bool flow_in_ = false; 81 | 82 | int nvof_version_; 83 | 84 | const std::size_t grid_size_; 85 | 86 | const float scaling_factor_; 87 | 88 | const int matrix_type_; 89 | 90 | const std::string log_name_ = "ImageOpticalFlowNVOF"; 91 | }; 92 | 93 | #endif /* ROFT_IMAGEOPTICALFLOWNVOF_H */ 94 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/ImageOpticalFlowSource.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_OPTICALFLOWSOURCE_H 9 | #define ROFT_OPTICALFLOWSOURCE_H 10 | 11 | #include 12 | 13 | #include 14 | 15 | namespace ROFT { 16 | class ImageOpticalFlowSource; 17 | } 18 | 19 | 20 | class ROFT::ImageOpticalFlowSource 21 | { 22 | public: 23 | 24 | virtual ~ImageOpticalFlowSource(); 25 | 26 | virtual bool reset(); 27 | 28 | virtual bool step_frame(); 29 | 30 | virtual bool is_stepping_required() const = 0; 31 | 32 | virtual double get_data_loading_time() const; 33 | 34 | virtual std::tuple flow(const bool& blocking) = 0; 35 | 36 | /* A flow frame can be per-pixel or block-wise with blocks of size grid_size x grid_size. */ 37 | virtual std::size_t get_grid_size() const = 0; 38 | 39 | /* Values of a flow frame might require additional scaling. */ 40 | virtual float get_scaling_factor() const = 0; 41 | 42 | /* A flow frame can be of type CV_32FC2 or CV_16SC2. */ 43 | virtual int get_matrix_type() const = 0; 44 | 45 | private: 46 | 47 | const std::string log_name_ = "ImageOpticalFlowSource"; 48 | }; 49 | 50 | #endif /* ROFT_IMAGEOPTICALFLOWSOURCE_H */ 51 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/ImageSegmentationMeasurement.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_IMAGESEGMENTATIONMEASUREMENT_H 9 | #define ROFT_IMAGESEGMENTATIONMEASUREMENT_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | #include 19 | 20 | #include 21 | 22 | namespace ROFT { 23 | class ImageSegmentationMeasurement; 24 | } 25 | 26 | 27 | class ROFT::ImageSegmentationMeasurement : public bfl::MeasurementModel 28 | { 29 | public: 30 | 31 | ImageSegmentationMeasurement(std::shared_ptr segmentation_source, std::shared_ptr camera_measurement, const std::size_t& width = 0, const std::size_t& height = 0); 32 | 33 | ~ImageSegmentationMeasurement(); 34 | 35 | bool freeze(const bfl::Data& data = bfl::Data()) override; 36 | 37 | std::pair measure(const bfl::Data& data = bfl::Data()) const override; 38 | 39 | std::pair predictedMeasure(const Eigen::Ref& cur_states) const override; 40 | 41 | std::pair innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const override; 42 | 43 | void reset(); 44 | 45 | void reset_data_loading_time(); 46 | 47 | double get_data_loading_time() const; 48 | 49 | protected: 50 | std::shared_ptr segmentation_source_; 51 | 52 | std::shared_ptr camera_; 53 | 54 | std::size_t width_; 55 | 56 | std::size_t height_; 57 | 58 | cv::Mat segmentation_; 59 | 60 | /* i.e. whether segmentation was received at least one time. */ 61 | bool segmentation_available_ = false; 62 | 63 | /* i.e. if the current segmentation_ is new. */ 64 | bool new_segmentation_ = false; 65 | 66 | double data_loading_time_ = 0.0; 67 | 68 | const std::string log_name_ = "ImageSegmentationMeasurement"; 69 | }; 70 | 71 | #endif /* ROFT_IMAGESEGMENTATIONMEASUREMENT_H */ 72 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/MeshResource.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_MESHRESOURCE_H 9 | #define ROFT_MESHRESOURCE_H 10 | 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace ROFT { 19 | class MeshResource; 20 | } 21 | 22 | 23 | class ROFT::MeshResource 24 | { 25 | public: 26 | MeshResource(const std::string& name, const std::string& set); 27 | 28 | MeshResource(const ROFT::ModelParameters& model_parameters); 29 | 30 | virtual ~MeshResource(); 31 | 32 | virtual const std::string& as_string() const; 33 | 34 | private: 35 | std::string data_; 36 | 37 | const std::string log_name_ = "MeshResource"; 38 | }; 39 | 40 | #endif /* ROFT_MESHRESOURCE_H */ 41 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/ModelParameters.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_MODELPARAMETERS_H 9 | #define ROFT_MODELPARAMETERS_H 10 | 11 | #include 12 | 13 | namespace ROFT { 14 | class ModelParameters; 15 | } 16 | 17 | 18 | class ROFT::ModelParameters : public RobotsIO::Utils::Parameters 19 | { 20 | public: 21 | ModelParameters(); 22 | 23 | virtual ~ModelParameters(); 24 | 25 | robots_io_accessor(ModelParameters); 26 | 27 | robots_io_declare_std_field(ModelParameters, string, name); 28 | 29 | robots_io_declare_field(ModelParameters, bool, use_internal_db); 30 | 31 | robots_io_declare_std_field(ModelParameters, string, internal_db_name); 32 | 33 | robots_io_declare_std_field(ModelParameters, string, mesh_external_path); 34 | 35 | robots_io_declare_std_field(ModelParameters, string, textured_mesh_external_path); 36 | 37 | robots_io_declare_std_field(ModelParameters, string, cloud_external_path); 38 | }; 39 | 40 | #endif /* ROFT_MODELPARAMETERS_H */ 41 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/OpticalFlowQueueHandler.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_OPTICALFLOWQUEUEHANDLER_HPP 9 | #define ROFT_OPTICALFLOWQUEUEHANDLER_HPP 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | namespace ROFT { 17 | class OpticalFlowQueueHandler; 18 | } 19 | 20 | 21 | class ROFT::OpticalFlowQueueHandler 22 | { 23 | public: 24 | OpticalFlowQueueHandler(const std::size_t& window_size); 25 | 26 | void add_flow(const cv::Mat& frame, const double& time_stamp); 27 | 28 | std::vector get_buffer_region(const double& initial_time_stamp); 29 | 30 | void clear(); 31 | 32 | struct Entry 33 | { 34 | public: 35 | cv::Mat frame; 36 | double timestamp; 37 | }; 38 | 39 | private: 40 | std::size_t window_size_; 41 | 42 | std::deque buffer_; 43 | }; 44 | 45 | #endif /* ROFT_FLOWQUEUEHANDLER_HPP */ 46 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/OpticalFlowUtilities.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_OPTICALFLOWUTILITIES_H 9 | #define ROFT_OPTICALFLOWUTILITIES_H 10 | 11 | #include 12 | 13 | #include 14 | 15 | 16 | namespace ROFT { 17 | namespace OpticalFlowUtils { 18 | 19 | inline bool is_flow_valid(const float& f_x, const float& f_y) 20 | { 21 | return !cvIsNaN(f_x) && !cvIsNaN(f_y) && fabs(f_x) < 1e9 && fabs(f_y) < 1e9; 22 | } 23 | 24 | cv::Vec3b compute_flow_color(const float& fx, const float& fy); 25 | 26 | std::pair draw_flow(const cv::Mat& flow, const float& max_motion = 10); 27 | 28 | std::pair read_flow(const std::string& file_name); 29 | 30 | bool save_flow(const cv::Mat& flow, const std::string& output_path); 31 | 32 | std::pair pad_flow(const cv::Mat& flow, const std::size_t& width, const std::size_t& height); 33 | } 34 | } 35 | 36 | #endif /* OPTICALFLOWUTILITIES */ 37 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/QuaternionModel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_QUATERNIONMODEL_H 9 | #define ROFT_QUATERNIONMODEL_H 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace ROFT { 19 | class QuaternionModel; 20 | } 21 | 22 | 23 | class ROFT::QuaternionModel : public bfl::StateModel 24 | { 25 | public: 26 | QuaternionModel(const Eigen::Ref sigma_angular_velocity, const double sample_time); 27 | 28 | virtual ~QuaternionModel(); 29 | 30 | virtual void propagate(const Eigen::Ref& cur_states, Eigen::Ref mot_states) override; 31 | 32 | virtual void motion(const Eigen::Ref& cur_states, Eigen::Ref mot_states) override; 33 | 34 | Eigen::MatrixXd getNoiseCovarianceMatrix() override; 35 | 36 | bfl::VectorDescription getInputDescription() override; 37 | 38 | bfl::VectorDescription getStateDescription() override; 39 | 40 | bool setProperty(const std::string& property); 41 | 42 | bool setSamplingTime(const double& sample_time) override; 43 | 44 | private: 45 | Eigen::MatrixXd propagate_quaternion(const Eigen::Ref& current_quaternion, const Eigen::Ref& angular_velocities); 46 | 47 | /** 48 | * Input and state descriptions. 49 | */ 50 | bfl::VectorDescription input_description_; 51 | 52 | bfl::VectorDescription state_description_; 53 | 54 | /** 55 | * Noise covariance matrix. 56 | */ 57 | Eigen::MatrixXd Q_; 58 | 59 | /** 60 | * Variance for the angular velocity part 61 | */ 62 | Eigen::MatrixXd sigma_angular_velocity_; 63 | 64 | double sample_time_; 65 | }; 66 | 67 | #endif /* ROFT_QUATERNIONMODEL_H */ 68 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/SICADModel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | * 7 | * 8 | * Part of this code is taken from https://github.com/robotology/superimpose-mesh-lib/commits/impl/depth 9 | * 10 | * This is the original BSD 3-Clause LICENSE the original code was provided with: 11 | * 12 | * Copyright (c) 2016-2019, Istituto Italiano di Tecnologia (IIT) All rights reserved. 13 | * 14 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 15 | * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 16 | * - Redistributions in binary form must reproduce the above copyright notice this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * - Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CLAUDIO FANTACCI BE LIABLE FOR ANY DIRECT, 23 | * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef ROFT_SICAD_MODEL_H 29 | #define ROFT_SICAD_MODEL_H 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace ROFT { 38 | class SICADModel; 39 | } 40 | 41 | 42 | class ROFT::SICADModel 43 | { 44 | public: 45 | SICADModel(const GLchar* path); 46 | 47 | SICADModel(const std::basic_istream* model_stream); 48 | 49 | void Draw(ROFT::SICADShader shader); 50 | 51 | bool has_texture(); 52 | 53 | protected: 54 | void loadModel(std::string path); 55 | 56 | void loadModel(const std::basic_istream* model_stream); 57 | 58 | void processNode(aiNode* node, const aiScene* scene); 59 | 60 | Mesh processMesh(aiMesh* mesh, const aiScene* scene); 61 | 62 | GLint TextureFromFile(const char* path, std::string directory); 63 | 64 | std::vector loadMaterialTextures(aiMaterial* mat, aiTextureType type, std::string typeName); 65 | 66 | private: 67 | std::vector meshes_; 68 | 69 | std::string directory_; 70 | 71 | std::vector textures_loaded_; 72 | }; 73 | 74 | #endif /* ROFT_SICAD_MODEL_H */ 75 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/SICADShader.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | * 7 | * 8 | * Part of this code is taken from https://github.com/robotology/superimpose-mesh-lib/commits/impl/depth 9 | * 10 | * This is the original BSD 3-Clause LICENSE the original code was provided with: 11 | * 12 | * Copyright (c) 2016-2019, Istituto Italiano di Tecnologia (IIT) All rights reserved. 13 | * 14 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 15 | * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 16 | * - Redistributions in binary form must reproduce the above copyright notice this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * - Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CLAUDIO FANTACCI BE LIABLE FOR ANY DIRECT, 23 | * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef ROFT_SICAD_SHADER_H 29 | #define ROFT_SICAD_SHADER_H 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | namespace ROFT { 37 | class SICADShader; 38 | } 39 | 40 | 41 | class ROFT::SICADShader 42 | { 43 | public: 44 | /** 45 | * Create a shader program with given vertex and fragment shader paths. 46 | */ 47 | SICADShader(const std::string& vertex_shader_path, const std::string& fragment_shader_path); 48 | 49 | /** 50 | * Activate the shader program. 51 | */ 52 | void install(); 53 | 54 | /** 55 | * Deactivate the shader program. 56 | */ 57 | void uninstall(); 58 | 59 | 60 | inline const GLuint get_program() 61 | { 62 | return shader_program_id_; 63 | } 64 | 65 | private: 66 | /** 67 | * The program ID. 68 | */ 69 | GLuint shader_program_id_; 70 | }; 71 | 72 | #endif /* ROFT_SICAD_SHADER_H */ 73 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/SKFCorrection.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_SKFCORRECTION_H 9 | #define ROFT_SKFCORRECTION_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | namespace ROFT { 19 | class SKFCorrection; 20 | } 21 | 22 | 23 | class ROFT::SKFCorrection : public bfl::GaussianCorrection 24 | { 25 | public: 26 | SKFCorrection(std::unique_ptr measurement_model, const std::size_t measurement_sub_size, const bool use_laplacian_reweighting = false); 27 | 28 | virtual ~SKFCorrection(); 29 | 30 | bfl::MeasurementModel& getMeasurementModel() override; 31 | 32 | protected: 33 | void correctStep(const bfl::GaussianMixture& pred_state, bfl::GaussianMixture& corr_state) override; 34 | 35 | private: 36 | std::unique_ptr measurement_model_; 37 | 38 | /* Eigen::MatrixXd measurement_; */ 39 | 40 | /* Eigen::MatrixXd propagated_sigma_points_; */ 41 | 42 | /* Eigen::MatrixXd innovations_; */ 43 | 44 | /** 45 | * Unscented transform weight. 46 | */ 47 | std::size_t measurement_sub_size_; 48 | 49 | bool use_laplacian_reweighting_; 50 | 51 | const std::string log_name_ = "SKFCorrection"; 52 | }; 53 | 54 | #endif /* SKFCORRECTION_H */ 55 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/SpatialVelocityModel.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_SPATIALVELOCITYMODEL_H 9 | #define ROFT_SPATIALVELOCITYMODEL_H 10 | 11 | #include 12 | 13 | #include 14 | 15 | namespace ROFT { 16 | class SpatialVelocityModel; 17 | } 18 | 19 | 20 | class ROFT::SpatialVelocityModel : public bfl::LinearStateModel 21 | { 22 | public: 23 | SpatialVelocityModel(const Eigen::Ref sigma_v, const Eigen::Ref sigma_w); 24 | 25 | virtual ~SpatialVelocityModel(); 26 | 27 | Eigen::MatrixXd getStateTransitionMatrix() override; 28 | 29 | Eigen::MatrixXd getNoiseCovarianceMatrix() override; 30 | 31 | bfl::VectorDescription getInputDescription() override; 32 | 33 | bfl::VectorDescription getStateDescription() override; 34 | 35 | bool setProperty(const std::string& property) override; 36 | 37 | protected: 38 | /** 39 | * Input and state descriptions. 40 | */ 41 | bfl::VectorDescription input_description_; 42 | 43 | bfl::VectorDescription state_description_; 44 | 45 | /** 46 | * State transition matrix. 47 | */ 48 | Eigen::MatrixXd F_; 49 | 50 | /** 51 | * Noise covariance matrix. 52 | */ 53 | Eigen::MatrixXd Q_; 54 | 55 | const std::string log_name_ = "SpatialVelocityModel"; 56 | }; 57 | 58 | #endif /* ROFT_SPATIALVELOCITYMODEL_H */ 59 | -------------------------------------------------------------------------------- /src/roft-lib/include/ROFT/UKFCorrection.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #ifndef ROFT_UKFCORRECTION_H 9 | #define ROFT_UKFCORRECTION_H 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | namespace ROFT { 20 | class UKFCorrection; 21 | } 22 | 23 | 24 | class ROFT::UKFCorrection : public bfl::GaussianCorrection 25 | { 26 | public: 27 | UKFCorrection(std::unique_ptr meas_model, const double alpha, const double beta, const double kappa) noexcept; 28 | 29 | UKFCorrection(UKFCorrection&& ukf_prediction) noexcept; 30 | 31 | virtual ~UKFCorrection() noexcept = default; 32 | 33 | bfl::MeasurementModel& getMeasurementModel() override; 34 | 35 | std::pair getLikelihood() override; 36 | 37 | protected: 38 | void correctStep(const bfl::GaussianMixture& pred_state, bfl::GaussianMixture& corr_state) override; 39 | 40 | std::unique_ptr measurement_model_; 41 | 42 | /** 43 | * Unscented transform weight. 44 | */ 45 | bfl::sigma_point::UTWeight ut_weight_; 46 | 47 | const double ut_alpha_; 48 | 49 | const double ut_beta_; 50 | 51 | const double ut_kappa_; 52 | }; 53 | 54 | #endif /* ROFT_UKFCORRECTION_H */ 55 | -------------------------------------------------------------------------------- /src/roft-lib/shader/shader_background.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | * 7 | * 8 | * Part of this code is taken from https://github.com/robotology/superimpose-mesh-lib/commits/impl/depth 9 | * 10 | * This is the original BSD 3-Clause LICENSE the original code was provided with: 11 | * 12 | * Copyright (c) 2016-2019, Istituto Italiano di Tecnologia (IIT) All rights reserved. 13 | * 14 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 15 | * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 16 | * - Redistributions in binary form must reproduce the above copyright notice this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * - Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CLAUDIO FANTACCI BE LIABLE FOR ANY DIRECT, 23 | * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #version 330 core 29 | 30 | in vec3 ourColor; 31 | in vec2 TexCoord; 32 | 33 | out vec4 color; 34 | 35 | uniform sampler2D ourTexture; 36 | 37 | void main() 38 | { 39 | color = texture(ourTexture, TexCoord); 40 | } 41 | -------------------------------------------------------------------------------- /src/roft-lib/shader/shader_background.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | * 7 | * 8 | * Part of this code is taken from https://github.com/robotology/superimpose-mesh-lib/commits/impl/depth 9 | * 10 | * This is the original BSD 3-Clause LICENSE the original code was provided with: 11 | * 12 | * Copyright (c) 2016-2019, Istituto Italiano di Tecnologia (IIT) All rights reserved. 13 | * 14 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 15 | * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 16 | * - Redistributions in binary form must reproduce the above copyright notice this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * - Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CLAUDIO FANTACCI BE LIABLE FOR ANY DIRECT, 23 | * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #version 330 core 29 | 30 | layout (location = 0) in vec2 position; 31 | layout (location = 1) in vec3 color; 32 | layout (location = 2) in vec2 texCoord; 33 | 34 | out vec3 ourColor; 35 | out vec2 TexCoord; 36 | 37 | uniform mat4 projection; 38 | 39 | void main() 40 | { 41 | gl_Position = projection * vec4(position, -99999.99, 1.0f); 42 | ourColor = color; 43 | TexCoord = vec2(texCoord.x, 1.0f - texCoord.y); 44 | } 45 | -------------------------------------------------------------------------------- /src/roft-lib/shader/shader_frame.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | * 7 | * 8 | * Part of this code is taken from https://github.com/robotology/superimpose-mesh-lib/commits/impl/depth 9 | * 10 | * This is the original BSD 3-Clause LICENSE the original code was provided with: 11 | * 12 | * Copyright (c) 2016-2019, Istituto Italiano di Tecnologia (IIT) All rights reserved. 13 | * 14 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 15 | * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 16 | * - Redistributions in binary form must reproduce the above copyright notice this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * - Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CLAUDIO FANTACCI BE LIABLE FOR ANY DIRECT, 23 | * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #version 330 core 29 | 30 | in vec3 vert_color; 31 | 32 | out vec4 frag_color; 33 | 34 | void main() 35 | { 36 | frag_color = vec4(vert_color, 1.0f); 37 | } 38 | -------------------------------------------------------------------------------- /src/roft-lib/shader/shader_frame.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | * 7 | * 8 | * Part of this code is taken from https://github.com/robotology/superimpose-mesh-lib/commits/impl/depth 9 | * 10 | * This is the original BSD 3-Clause LICENSE the original code was provided with: 11 | * 12 | * Copyright (c) 2016-2019, Istituto Italiano di Tecnologia (IIT) All rights reserved. 13 | * 14 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 15 | * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 16 | * - Redistributions in binary form must reproduce the above copyright notice this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * - Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CLAUDIO FANTACCI BE LIABLE FOR ANY DIRECT, 23 | * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #version 330 core 29 | 30 | layout (location = 0) in vec3 position; 31 | layout (location = 1) in vec3 color; 32 | 33 | out vec3 vert_color; 34 | 35 | uniform mat4 model; 36 | uniform mat4 view; 37 | uniform mat4 projection; 38 | 39 | void main() 40 | { 41 | gl_Position = projection * view * model * vec4(position, 1.0f); 42 | vert_color = color; 43 | } 44 | -------------------------------------------------------------------------------- /src/roft-lib/shader/shader_model.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | * 7 | * 8 | * Part of this code is taken from https://github.com/robotology/superimpose-mesh-lib/commits/impl/depth 9 | * 10 | * This is the original BSD 3-Clause LICENSE the original code was provided with: 11 | * 12 | * Copyright (c) 2016-2019, Istituto Italiano di Tecnologia (IIT) All rights reserved. 13 | * 14 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 15 | * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 16 | * - Redistributions in binary form must reproduce the above copyright notice this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * - Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CLAUDIO FANTACCI BE LIABLE FOR ANY DIRECT, 23 | * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #version 330 core 29 | 30 | layout (location = 0) out vec4 color; 31 | layout (location = 1) out float depth; 32 | 33 | float near = 0.001f; 34 | float far = 1000.0f; 35 | 36 | float linearize_depth(float coord_z) 37 | { 38 | /* Back to NDC. */ 39 | float z = coord_z * 2.0 - 1.0; 40 | 41 | /* Evaluate real depth. */ 42 | return (2.0 * near * far) / (far + near - z * (far - near)); 43 | } 44 | 45 | void main() 46 | { 47 | /* BGR orange-like color. */ 48 | color = vec4(0.2f, 0.5f, 1.0f, 1.0f); 49 | 50 | /* Real depth. */ 51 | depth = linearize_depth(gl_FragCoord.z); 52 | } 53 | -------------------------------------------------------------------------------- /src/roft-lib/shader/shader_model.vert: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | * 7 | * 8 | * Part of this code is taken from https://github.com/robotology/superimpose-mesh-lib/commits/impl/depth 9 | * 10 | * This is the original BSD 3-Clause LICENSE the original code was provided with: 11 | * 12 | * Copyright (c) 2016-2019, Istituto Italiano di Tecnologia (IIT) All rights reserved. 13 | * 14 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 15 | * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 16 | * - Redistributions in binary form must reproduce the above copyright notice this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * - Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CLAUDIO FANTACCI BE LIABLE FOR ANY DIRECT, 23 | * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #version 330 core 29 | 30 | layout (location = 0) in vec3 position; 31 | layout (location = 1) in vec3 normal; 32 | layout (location = 2) in vec2 texCoords; 33 | 34 | out vec2 TexCoords; 35 | 36 | uniform mat4 model; 37 | uniform mat4 view; 38 | uniform mat4 projection; 39 | 40 | void main() 41 | { 42 | gl_Position = projection * view * model * vec4(position, 1.0f); 43 | TexCoords = texCoords; 44 | } 45 | -------------------------------------------------------------------------------- /src/roft-lib/shader/shader_model_texture.frag: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | * 7 | * 8 | * Part of this code is taken from https://github.com/robotology/superimpose-mesh-lib/commits/impl/depth 9 | * 10 | * This is the original BSD 3-Clause LICENSE the original code was provided with: 11 | * 12 | * Copyright (c) 2016-2019, Istituto Italiano di Tecnologia (IIT) All rights reserved. 13 | * 14 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 15 | * - Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 16 | * - Redistributions in binary form must reproduce the above copyright notice this list of conditions and the following disclaimer in the documentation 17 | * and/or other materials provided with the distribution. 18 | * - Neither the name of the organization nor the names of its contributors may be used to endorse or promote products derived from this software without 19 | * specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CLAUDIO FANTACCI BE LIABLE FOR ANY DIRECT, 23 | * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 24 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 25 | * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #version 330 core 29 | 30 | in vec2 TexCoords; 31 | 32 | out vec4 color; 33 | 34 | uniform sampler2D texture_diffuse1; 35 | 36 | void main() 37 | { 38 | color = texture(texture_diffuse1, TexCoords); 39 | } 40 | -------------------------------------------------------------------------------- /src/roft-lib/src/CameraMeasurement.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | using namespace Eigen; 14 | using namespace ROFT; 15 | using namespace RobotsIO::Camera; 16 | using namespace bfl; 17 | 18 | 19 | CameraMeasurement::CameraMeasurement(std::shared_ptr camera) : 20 | camera_(camera) 21 | {} 22 | 23 | 24 | CameraMeasurement::~CameraMeasurement() 25 | {} 26 | 27 | 28 | bool CameraMeasurement::freeze(const Data& type) 29 | { 30 | /* Step frame in case of an offline camera. */ 31 | if (!camera_->step_frame()) 32 | return false; 33 | 34 | /* Determine type of image required by the user. */ 35 | measure_type_ = bfl::any::any_cast(type); 36 | const bool use_rgb = (measure_type_ == CameraMeasurementType::RGB) || (measure_type_ == CameraMeasurementType::RGBD); 37 | const bool use_d = (measure_type_ == CameraMeasurementType::D) || (measure_type_ == CameraMeasurementType::RGBD); 38 | const bool use_pc = (measure_type_ == CameraMeasurementType::PC) || (measure_type_ == CameraMeasurementType::RGBPC); 39 | const bool use_rgbpc = (measure_type_ == CameraMeasurementType::RGBPC); 40 | 41 | bool valid_data = false; 42 | bool blocking_read = true; 43 | 44 | /* Pick new rgb image. */ 45 | if (use_rgb) 46 | { 47 | std::tie(valid_data, rgb_) = camera_->rgb(blocking_read); 48 | if (!valid_data) 49 | return false; 50 | 51 | std::tie(is_time_stamp_rgb_, time_stamp_rgb_) = camera_->time_stamp_rgb(); 52 | } 53 | valid_data = false; 54 | 55 | /* Pick new depth image. */ 56 | if (use_d) 57 | { 58 | std::tie(valid_data, depth_) = camera_->depth(blocking_read); 59 | if (!valid_data) 60 | return false; 61 | 62 | std::tie(is_time_stamp_depth_, time_stamp_depth_) = camera_->time_stamp_depth(); 63 | } 64 | valid_data = false; 65 | 66 | /* Pick new pose. */ 67 | std::tie(valid_data, pose_) = camera_->pose(blocking_read); 68 | if (!valid_data) 69 | return false; 70 | 71 | valid_data = false; 72 | /* Pick new point cloud. */ 73 | if (use_pc || use_rgbpc) 74 | { 75 | std::tie(valid_data, point_cloud_) = camera_->point_cloud(blocking_read, 10.0, false, use_rgbpc); 76 | if (!valid_data) 77 | return false; 78 | } 79 | 80 | measurement_available_ = true; 81 | 82 | return true; 83 | } 84 | 85 | 86 | std::pair CameraMeasurement::measure(const Data& data) const 87 | { 88 | const bool use_pc = (measure_type_ == CameraMeasurementType::PC) || (measure_type_ == CameraMeasurementType::RGBPC); 89 | 90 | if (use_pc) 91 | return std::make_pair(measurement_available_, std::make_tuple(pose_, point_cloud_)); 92 | 93 | return std::make_pair(measurement_available_, std::make_tuple(pose_, rgb_, depth_)); 94 | } 95 | 96 | 97 | std::pair CameraMeasurement::predictedMeasure(const Ref& cur_states) const 98 | { 99 | /* Not implemented. */ 100 | throw(std::runtime_error(log_name_ + "::step_frame. Not implemented in base class.")); 101 | } 102 | 103 | 104 | std::pair CameraMeasurement::innovation(const Data& predicted_measurements, const Data& measurements) const 105 | { 106 | /* Not implemented. */ 107 | throw(std::runtime_error(log_name_ + "::step_frame. Not implemented in base class.")); 108 | } 109 | 110 | 111 | std::pair CameraMeasurement::camera_parameters() const 112 | { 113 | return camera_->parameters(); 114 | } 115 | 116 | 117 | std::pair CameraMeasurement::camera_deprojection_matrix() const 118 | { 119 | return camera_->deprojection_matrix(); 120 | } 121 | 122 | 123 | std::pair CameraMeasurement::camera_time_stamp_rgb() const 124 | { 125 | return std::make_pair(is_time_stamp_rgb_, time_stamp_rgb_); 126 | } 127 | 128 | 129 | std::pair CameraMeasurement::camera_time_stamp_depth() const 130 | { 131 | return std::make_pair(is_time_stamp_depth_, time_stamp_depth_); 132 | } 133 | 134 | 135 | std::int32_t CameraMeasurement::camera_frame_index() const 136 | { 137 | return camera_->frame_index(); 138 | } 139 | 140 | 141 | void CameraMeasurement::reset() const 142 | { 143 | camera_->reset(); 144 | } 145 | -------------------------------------------------------------------------------- /src/roft-lib/src/DatasetImageOpticalFlow.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | using namespace ROFT; 20 | using namespace ROFT::OpticalFlowUtils; 21 | using namespace cv; 22 | 23 | 24 | DatasetImageOpticalFlow::DatasetImageOpticalFlow(const std::string& dataset_path, const std::string& set, const std::size_t width, const std::size_t height, const std::size_t& heading_zeros, const std::size_t& index_offset) : 25 | width_(width), 26 | height_(height), 27 | head_(-1 + index_offset), 28 | index_offset_(index_offset), 29 | heading_zeros_(heading_zeros) 30 | { 31 | /* Compose dataset path. */ 32 | std::string root = dataset_path; 33 | if (root.back() != '/') 34 | root += '/'; 35 | dataset_path_ = root + "optical_flow/" + set + "/"; 36 | 37 | /* Find parameters of this dataset. */ 38 | bool valid = false; 39 | cv::Mat tmp; 40 | int counter = 0; 41 | while (!valid) 42 | { 43 | std::string file_name = dataset_path_ + compose_file_name(counter++, heading_zeros_) + ".float"; 44 | std::tie(valid, tmp) = read_flow(file_name); 45 | } 46 | grid_size_ = width_ / tmp.cols; 47 | matrix_type_ = tmp.type(); 48 | scaling_factor_ = 1; 49 | if (matrix_type_ == CV_16SC2) 50 | scaling_factor_ = float(1 << 5); 51 | 52 | /* Log loaded information. */ 53 | std::string matrix_type_str = (matrix_type_ == CV_32FC2) ? std::string("CV_32FC2") : std::string("CV_16SC2"); 54 | std::cout << log_name_ + "::ctor." << std::endl; 55 | std::cout << log_name_ + " - grid size: " << grid_size_ << std::endl; 56 | std::cout << log_name_ + " - scaling factor: " << scaling_factor_ << std::endl; 57 | std::cout << log_name_ + " - matrix type: " << matrix_type_str << std::endl; 58 | } 59 | 60 | 61 | DatasetImageOpticalFlow::~DatasetImageOpticalFlow() 62 | {} 63 | 64 | 65 | bool DatasetImageOpticalFlow::reset() 66 | { 67 | head_ = -1 + index_offset_; 68 | 69 | return true; 70 | } 71 | 72 | 73 | bool DatasetImageOpticalFlow::step_frame() 74 | { 75 | head_++; 76 | 77 | std::string file_name = dataset_path_ + compose_file_name(head_, heading_zeros_) + ".float"; 78 | 79 | auto t0 = std::chrono::steady_clock::now(); 80 | 81 | output_ = read_flow(file_name); 82 | 83 | auto t1 = std::chrono::steady_clock::now(); 84 | 85 | data_loading_time_ = std::chrono::duration_cast(t1 - t0).count(); 86 | 87 | return true; 88 | } 89 | 90 | 91 | // bool DatasetImageOpticalFlow::set_frame(const std::size_t& index) 92 | // { 93 | // head_ = index; 94 | 95 | // return true; 96 | // } 97 | 98 | 99 | bool DatasetImageOpticalFlow::is_stepping_required() const 100 | { 101 | return true; 102 | } 103 | 104 | 105 | std::size_t DatasetImageOpticalFlow::get_grid_size() const 106 | { 107 | return grid_size_; 108 | } 109 | 110 | 111 | float DatasetImageOpticalFlow::get_scaling_factor() const 112 | { 113 | return scaling_factor_; 114 | } 115 | 116 | 117 | int DatasetImageOpticalFlow::get_matrix_type() const 118 | { 119 | return matrix_type_; 120 | } 121 | 122 | 123 | double DatasetImageOpticalFlow::get_data_loading_time() const 124 | { 125 | return data_loading_time_; 126 | } 127 | 128 | 129 | std::tuple DatasetImageOpticalFlow::flow(const bool& blocking) 130 | { 131 | return output_; 132 | } 133 | 134 | 135 | std::string DatasetImageOpticalFlow::compose_file_name(const int& index, const std::size_t& number_of_digits) 136 | { 137 | std::ostringstream ss; 138 | ss << std::setw(number_of_digits) << std::setfill('0') << index; 139 | return ss.str(); 140 | } 141 | -------------------------------------------------------------------------------- /src/roft-lib/src/DatasetImageSegmentation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | using namespace ROFT; 18 | using namespace cv; 19 | 20 | 21 | DatasetImageSegmentation::DatasetImageSegmentation 22 | ( 23 | const std::string& dataset_path, 24 | const std::string& format, 25 | const std::size_t width, 26 | const std::size_t height, 27 | const std::string& segmentation_set, 28 | const ModelParameters& model_parameters, 29 | const std::size_t heading_zeros, 30 | const std::size_t index_offset, 31 | const bool simulate_missing_detections 32 | ) : 33 | format_(format), 34 | width_(width), 35 | height_(height), 36 | object_name_(model_parameters.name()), 37 | head_(-1 + index_offset), 38 | index_offset_(index_offset), 39 | heading_zeros_(heading_zeros), 40 | simulate_missing_detections_(simulate_missing_detections) 41 | { 42 | /* Compose dataset path. */ 43 | std::string root = dataset_path; 44 | if (root.back() != '/') 45 | root += '/'; 46 | dataset_path_ = root + "masks/" + segmentation_set + "/"; 47 | 48 | /* Test. */ 49 | // set_frame(0 + index_offset_); 50 | // bool valid_dataset = false; 51 | // std::tie(valid_dataset, std::ignore) = segmentation(false); 52 | // if (!valid_dataset) 53 | // throw(std::runtime_error(log_name_ + "::ctor. Cannot load dataset.")); 54 | // else 55 | // std::cout << log_name_ + "::ctor. Dataset loaded succesfully." << std::endl; 56 | // set_frame(-1 + index_offset_); 57 | } 58 | 59 | 60 | DatasetImageSegmentation::~DatasetImageSegmentation() 61 | {} 62 | 63 | 64 | bool DatasetImageSegmentation::reset() 65 | { 66 | head_ = -1 + index_offset_; 67 | 68 | return true; 69 | } 70 | 71 | 72 | bool DatasetImageSegmentation::step_frame() 73 | { 74 | head_++; 75 | 76 | auto t0 = std::chrono::steady_clock::now(); 77 | 78 | output_ = read_file(head_); 79 | 80 | auto t1 = std::chrono::steady_clock::now(); 81 | 82 | data_loading_time_ = std::chrono::duration_cast(t1 - t0).count(); 83 | 84 | return true; 85 | } 86 | 87 | 88 | // bool DatasetImageSegmentation::set_frame(const std::size_t& index) 89 | // { 90 | // head_ = index; 91 | 92 | // return true; 93 | // } 94 | 95 | 96 | bool DatasetImageSegmentation::is_stepping_required() const 97 | { 98 | return true; 99 | } 100 | 101 | 102 | void DatasetImageSegmentation::reset_data_loading_time() 103 | { 104 | data_loading_time_ = 0.0; 105 | } 106 | 107 | 108 | double DatasetImageSegmentation::get_data_loading_time() const 109 | { 110 | return data_loading_time_; 111 | } 112 | 113 | 114 | std::pair DatasetImageSegmentation::segmentation(const bool& blocking) 115 | { 116 | return output_; 117 | } 118 | 119 | 120 | std::string DatasetImageSegmentation::compose_file_name(const int& index, const std::size_t& number_of_digits) 121 | { 122 | std::ostringstream ss; 123 | ss << std::setw(number_of_digits) << std::setfill('0') << index; 124 | return ss.str(); 125 | } 126 | 127 | 128 | std::pair DatasetImageSegmentation::read_file(const std::size_t& frame_index) 129 | { 130 | std::string file_name = dataset_path_ + object_name_ + "_" + compose_file_name(frame_index, heading_zeros_) + "." + format_; 131 | cv::Mat segmentation = cv::imread(file_name, cv::IMREAD_UNCHANGED); 132 | segmentation.convertTo(segmentation, CV_8UC1); 133 | 134 | if (segmentation.data == nullptr) 135 | { 136 | if (simulate_missing_detections_) 137 | segmentation = cv::Mat(cv::Size(width_, height_), CV_8UC1, cv::Scalar(0.0)); 138 | else 139 | { 140 | std::cout << log_name_ << "::segmentation. Error: cannot load segmentation data for frame" + file_name << std::endl; 141 | 142 | return std::make_pair(false, cv::Mat()); 143 | } 144 | } 145 | 146 | return std::make_pair(true, segmentation); 147 | } 148 | -------------------------------------------------------------------------------- /src/roft-lib/src/DatasetImageSegmentationDelayed.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | using namespace ROFT; 13 | 14 | 15 | DatasetImageSegmentationDelayed::DatasetImageSegmentationDelayed 16 | ( 17 | const float& fps, 18 | const float& simulated_fps, 19 | const bool simulate_inference_time, 20 | const std::string& dataset_path, 21 | const std::string& format, 22 | const std::size_t width, 23 | const std::size_t height, 24 | const std::string& segmentation_set, 25 | const ModelParameters& model_parameters, 26 | const std::size_t heading_zeros, 27 | const std::size_t index_offset 28 | ) : 29 | DatasetImageSegmentation(dataset_path, format, width, height, segmentation_set, model_parameters, heading_zeros, index_offset), 30 | fps_(fps), 31 | simulated_fps_(simulated_fps), 32 | simulate_inference_time_(simulate_inference_time), 33 | head_0_(head_ + 1), 34 | delay_(static_cast(fps_ / simulated_fps_)) 35 | {} 36 | 37 | 38 | DatasetImageSegmentationDelayed::~DatasetImageSegmentationDelayed() 39 | {} 40 | 41 | 42 | std::pair DatasetImageSegmentationDelayed::segmentation(const bool& blocking) 43 | { 44 | int index = head_; 45 | if (simulate_inference_time_) 46 | index -= delay_; 47 | 48 | if (((index - head_0_) % delay_) != 0) 49 | return std::make_pair(false, cv::Mat()); 50 | 51 | if (index < 0) 52 | index = head_0_; 53 | 54 | auto t0 = std::chrono::steady_clock::now(); 55 | 56 | auto output = read_file(index); 57 | 58 | auto t1 = std::chrono::steady_clock::now(); 59 | 60 | data_loading_time_ = std::chrono::duration_cast(t1 - t0).count(); 61 | 62 | return output; 63 | } 64 | 65 | 66 | void DatasetImageSegmentationDelayed::reset_data_loading_time() 67 | { 68 | data_loading_time_ = 0.0; 69 | } 70 | 71 | 72 | double DatasetImageSegmentationDelayed::get_data_loading_time() const 73 | { 74 | return data_loading_time_; 75 | } 76 | 77 | 78 | int DatasetImageSegmentationDelayed::get_frames_between_iterations() const 79 | { 80 | return int(fps_ / simulated_fps_); 81 | } 82 | -------------------------------------------------------------------------------- /src/roft-lib/src/ImageOpticalFlowSource.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | using namespace ROFT; 11 | 12 | 13 | ImageOpticalFlowSource::~ImageOpticalFlowSource() 14 | {} 15 | 16 | 17 | bool ImageOpticalFlowSource::reset() 18 | { 19 | return true; 20 | } 21 | 22 | 23 | bool ImageOpticalFlowSource::step_frame() 24 | { 25 | return true; 26 | } 27 | 28 | 29 | double ImageOpticalFlowSource::get_data_loading_time() const 30 | { 31 | return 0.0; 32 | } 33 | -------------------------------------------------------------------------------- /src/roft-lib/src/ImageSegmentationMeasurement.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | using namespace Eigen; 13 | using namespace ROFT; 14 | using namespace bfl; 15 | using namespace cv; 16 | 17 | 18 | ImageSegmentationMeasurement::ImageSegmentationMeasurement(std::shared_ptr segmentation_source, std::shared_ptr camera_measurement, const std::size_t& width, const std::size_t& height) : 19 | segmentation_source_(segmentation_source), 20 | camera_(camera_measurement), 21 | width_(width), 22 | height_(height) 23 | {} 24 | 25 | 26 | ImageSegmentationMeasurement::~ImageSegmentationMeasurement() 27 | {} 28 | 29 | 30 | bool ImageSegmentationMeasurement::freeze(const Data& data) 31 | { 32 | /* Provide RGB input for Segmentation sources that might require it if the camera measurement is available. */ 33 | if (camera_ != nullptr) 34 | { 35 | bfl::Data camera_data; 36 | bool valid_data = false; 37 | std::tie(valid_data, camera_data) = camera_->measure(); 38 | if (!valid_data) 39 | { 40 | std::cout << log_name_ << "::freeze. Warning: RGB from camera measurement is not available." << std::endl; 41 | return false; 42 | } 43 | cv::Mat rgb; 44 | double rgb_timestamp; 45 | std::tie(std::ignore, rgb, std::ignore) = bfl::any::any_cast(camera_data); 46 | std::tie(std::ignore, rgb_timestamp) = camera_->camera_time_stamp_rgb(); 47 | segmentation_source_->set_rgb_image(rgb, rgb_timestamp); 48 | } 49 | 50 | /* Step frame if required. */ 51 | if (segmentation_source_->is_stepping_required()) 52 | segmentation_source_->step_frame(); 53 | 54 | cv::Mat segmentation; 55 | new_segmentation_ = false; 56 | std::tie(new_segmentation_, segmentation) = segmentation_source_->segmentation(false); 57 | 58 | if (new_segmentation_) 59 | { 60 | segmentation_available_ = true; 61 | segmentation_ = segmentation.clone(); 62 | if (segmentation_.channels() == 3 || segmentation_.channels() == 4) 63 | cv::cvtColor(segmentation_, segmentation_, cv::COLOR_BGR2GRAY); 64 | #if CV_MAJOR_VERSION >= 4 65 | cv::threshold(segmentation_, segmentation_, 1, 255, cv::THRESH_BINARY); 66 | #else 67 | cv::threshold(segmentation_, segmentation_, 1, 255, CV_THRESH_BINARY); 68 | #endif 69 | 70 | if ((width_ != 0) && (height_ != 0)) 71 | cv::resize(segmentation_, segmentation_, cv::Size(width_, height_)); 72 | } 73 | 74 | return segmentation_available_; 75 | } 76 | 77 | 78 | std::pair ImageSegmentationMeasurement::measure(const Data& data) const 79 | { 80 | return std::make_pair(segmentation_available_, std::make_pair(new_segmentation_, segmentation_)); 81 | } 82 | 83 | 84 | std::pair ImageSegmentationMeasurement::predictedMeasure(const Eigen::Ref& cur_states) const 85 | { 86 | /* Not implemented. */ 87 | throw(std::runtime_error(log_name_ + "::innovation. Not implemented.")); 88 | } 89 | 90 | 91 | std::pair ImageSegmentationMeasurement::innovation(const Data& predicted_measurements, const Data& measurements) const 92 | { 93 | /* Not implemented. */ 94 | throw(std::runtime_error(log_name_ + "::innovation. Not implemented.")); 95 | } 96 | 97 | 98 | void ImageSegmentationMeasurement::reset() 99 | { 100 | segmentation_available_ = false; 101 | segmentation_source_->reset(); 102 | } 103 | 104 | 105 | void ImageSegmentationMeasurement::reset_data_loading_time() 106 | { 107 | segmentation_source_->reset_data_loading_time(); 108 | } 109 | 110 | 111 | double ImageSegmentationMeasurement::get_data_loading_time() const 112 | { 113 | return segmentation_source_->get_data_loading_time(); 114 | } 115 | -------------------------------------------------------------------------------- /src/roft-lib/src/ImageSegmentationRenderer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | using namespace ROFT; 11 | 12 | 13 | ImageSegmentationRenderer::~ImageSegmentationRenderer() 14 | {} 15 | -------------------------------------------------------------------------------- /src/roft-lib/src/MeshResource.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | CMRC_DECLARE(resources); 15 | 16 | using namespace ROFT; 17 | 18 | 19 | MeshResource::MeshResource(const std::string& name, const std::string& set) 20 | { 21 | const std::string virtual_path = "__prc/meshes/" + set + "/" + name + ".obj"; 22 | 23 | auto cmrc_fs = cmrc::resources::get_filesystem(); 24 | 25 | if (!(cmrc_fs.exists(virtual_path) && cmrc_fs.is_file(virtual_path))) 26 | throw(std::runtime_error(log_name_ + "::ctor. Cannot find requested mesh among available resources.")); 27 | 28 | auto mesh_cmrc_file = cmrc_fs.open(virtual_path); 29 | data_.assign(mesh_cmrc_file.cbegin(), mesh_cmrc_file.cend()); 30 | } 31 | 32 | 33 | MeshResource::MeshResource(const ROFT::ModelParameters& model_parameters) 34 | { 35 | if (model_parameters.use_internal_db()) 36 | { 37 | const std::string virtual_path = "__prc/meshes/" + model_parameters.internal_db_name() + "/" + model_parameters.name() + ".obj"; 38 | 39 | auto cmrc_fs = cmrc::resources::get_filesystem(); 40 | 41 | if (!(cmrc_fs.exists(virtual_path) && cmrc_fs.is_file(virtual_path))) 42 | throw(std::runtime_error(log_name_ + "::ctor. Cannot find requested mesh among available resources.")); 43 | 44 | auto mesh_cmrc_file = cmrc_fs.open(virtual_path); 45 | data_.assign(mesh_cmrc_file.cbegin(), mesh_cmrc_file.cend()); 46 | } 47 | else 48 | { 49 | std::ifstream in; 50 | in.open(model_parameters.mesh_external_path()); 51 | if (!in.is_open()) 52 | throw(std::runtime_error(log_name_ + "::ctor. Cannot open model from external path " + model_parameters.mesh_external_path() + ".")); 53 | 54 | std::stringstream buffer; 55 | buffer << in.rdbuf(); 56 | in.close(); 57 | 58 | data_ = buffer.str(); 59 | } 60 | } 61 | 62 | 63 | MeshResource::~MeshResource() 64 | {} 65 | 66 | 67 | const std::string& MeshResource::as_string() const 68 | { 69 | return data_; 70 | } 71 | -------------------------------------------------------------------------------- /src/roft-lib/src/ModelParameters.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | using namespace ROFT; 11 | 12 | 13 | robots_io_accessor_impl(ModelParameters); 14 | 15 | 16 | robots_io_declare_std_field_impl(ModelParameters, string, name); 17 | 18 | 19 | robots_io_declare_field_impl(ModelParameters, bool, use_internal_db); 20 | 21 | 22 | robots_io_declare_std_field_impl(ModelParameters, string, internal_db_name); 23 | 24 | 25 | robots_io_declare_std_field_impl(ModelParameters, string, mesh_external_path); 26 | 27 | 28 | robots_io_declare_std_field_impl(ModelParameters, string, textured_mesh_external_path); 29 | 30 | 31 | robots_io_declare_std_field_impl(ModelParameters, string, cloud_external_path); 32 | 33 | 34 | ModelParameters::ModelParameters() 35 | { 36 | /* Set default values */ 37 | name(""); 38 | 39 | use_internal_db(false); 40 | 41 | internal_db_name(""); 42 | 43 | mesh_external_path(""); 44 | 45 | textured_mesh_external_path(""); 46 | 47 | cloud_external_path(""); 48 | } 49 | 50 | 51 | ModelParameters::~ModelParameters() 52 | {} 53 | -------------------------------------------------------------------------------- /src/roft-lib/src/OpticalFlowQueueHandler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | using namespace ROFT; 11 | 12 | 13 | OpticalFlowQueueHandler::OpticalFlowQueueHandler(const std::size_t& window_size) : 14 | window_size_(window_size) 15 | {} 16 | 17 | 18 | void OpticalFlowQueueHandler::add_flow(const cv::Mat& frame, const double& time_stamp) 19 | { 20 | OpticalFlowQueueHandler::Entry entry{frame.clone(), time_stamp}; 21 | buffer_.push_back(entry); 22 | 23 | /* Enforce the maximum size. */ 24 | if (buffer_.size() > window_size_) 25 | buffer_.pop_front(); 26 | } 27 | 28 | 29 | std::vector OpticalFlowQueueHandler::get_buffer_region(const double& initial_time_stamp) 30 | { 31 | std::vector output_region; 32 | output_region.clear(); 33 | 34 | std::size_t index; 35 | bool found = false; 36 | for (index = 0; index < buffer_.size(); index++) 37 | { 38 | if (abs(buffer_[index].timestamp - initial_time_stamp) < 1e-3) 39 | { 40 | found = true; 41 | break; 42 | } 43 | } 44 | 45 | /* Return empty buffer if not found. */ 46 | if (!found) 47 | return output_region; 48 | 49 | /* The optical flow alway refer to the previous RGB image, this we need the next frame. */ 50 | index++; 51 | 52 | for (; index < buffer_.size(); index++) 53 | // output_region.push_back(buffer_[index].frame); 54 | output_region.push_back(buffer_[index].frame.clone()); 55 | 56 | return output_region; 57 | } 58 | 59 | 60 | void OpticalFlowQueueHandler::clear() 61 | { 62 | buffer_.clear(); 63 | } 64 | -------------------------------------------------------------------------------- /src/roft-lib/src/QuaternionModel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | using namespace Eigen; 13 | using namespace ROFT; 14 | using namespace bfl; 15 | 16 | 17 | QuaternionModel::QuaternionModel(const Eigen::Ref sigma_angular_velocity, const double sample_time) : 18 | sigma_angular_velocity_(sigma_angular_velocity), 19 | sample_time_(sample_time) 20 | { 21 | /* Initialize the noise covariance matrix. */ 22 | Q_.resize(3, 3); 23 | Q_ = sigma_angular_velocity; 24 | 25 | /* Set the state description: 3 linear components (w_x, w_y, w_z) and 1 quaternion. */ 26 | state_description_ = VectorDescription(3, 1, 0, VectorDescription::CircularType::Quaternion); 27 | 28 | /* Set the input description: as the state description plus noise components for w_x, w_y, w_z. */ 29 | input_description_ = VectorDescription(3, 1, 3, VectorDescription::CircularType::Quaternion); 30 | } 31 | 32 | 33 | QuaternionModel::~QuaternionModel() 34 | { } 35 | 36 | 37 | void QuaternionModel::motion(const Ref& current, Ref propagated) 38 | { 39 | /* Warning. This class is bfl::StateModel, i.e. the most general form of state model. 40 | For this reason, it is assumed that Q_.rows() = 3 noise components are provided in current.bottomRows(Q_.cols()). */ 41 | 42 | propagated.topRows<3>() = current.topRows<3>() + current.bottomRows<3>(); 43 | propagated.bottomRows<4>() = propagate_quaternion(current.middleRows<4>(3), current.topRows<3>()); 44 | 45 | } 46 | 47 | void QuaternionModel::propagate(const Ref& current, Ref propagated) 48 | { 49 | /* Same implementation as QuaternionModel::motion() but noise-free. */ 50 | 51 | propagated.topRows<3>() = current.topRows<3>(); 52 | propagated.bottomRows<4>() = propagate_quaternion(current.middleRows<4>(3), current.topRows<3>()); 53 | } 54 | 55 | 56 | VectorDescription QuaternionModel::getInputDescription() 57 | { 58 | return input_description_; 59 | } 60 | 61 | 62 | VectorDescription QuaternionModel::getStateDescription() 63 | { 64 | return state_description_; 65 | } 66 | 67 | 68 | Eigen::MatrixXd QuaternionModel::getNoiseCovarianceMatrix() 69 | { 70 | return Q_; 71 | } 72 | 73 | 74 | bool QuaternionModel::setProperty(const std::string& property) 75 | { 76 | return false; 77 | } 78 | 79 | 80 | bool QuaternionModel::setSamplingTime(const double& sample_time) 81 | { 82 | sample_time_ = sample_time; 83 | 84 | return true; 85 | } 86 | 87 | 88 | MatrixXd QuaternionModel::propagate_quaternion(const Ref& current_quaternion, const Ref& angular_velocities) 89 | { 90 | MatrixXd propagated(4, current_quaternion.cols()); 91 | 92 | for (std::size_t i = 0; i < current_quaternion.cols(); i++) 93 | { 94 | /* Take the angular velocity. */ 95 | const Ref w = angular_velocities.col(i); 96 | 97 | /* Angular velocity norm. 98 | 99 | std::numeric_limits::min() used to properly evaluate sin(x)/x when x-> 0. */ 100 | double norm_w = w.norm() + std::numeric_limits::min(); 101 | 102 | /* Left-quaternion-product matrix of the augmented angular velocity quaternion *written in the inertial reference frame*. */ 103 | Matrix4d skew_w = Matrix4d::Zero(); 104 | skew_w.block<1, 3>(0, 1) = -1.0 * w.transpose(); 105 | skew_w.block<3, 1>(1, 0) = w; 106 | /* The following should have a reversed sign if the angular velocity was *written in the body reference frame*. */ 107 | skew_w(1, 2) = -w(2); 108 | skew_w(1, 3) = w(1); 109 | skew_w(2, 1) = w(2); 110 | skew_w(2, 3) = -w(0); 111 | skew_w(3, 1) = -w(1); 112 | skew_w(3, 2) = w(0); 113 | 114 | propagated.col(i) = (std::cos(norm_w * sample_time_ / 2.0) * Matrix4d::Identity() + sin(norm_w * sample_time_ / 2.0) / norm_w * skew_w) * current_quaternion.col(i); 115 | } 116 | 117 | return propagated; 118 | } 119 | -------------------------------------------------------------------------------- /src/roft-lib/src/SpatialVelocityModel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | using namespace Eigen; 11 | using namespace ROFT; 12 | using namespace bfl; 13 | 14 | 15 | SpatialVelocityModel::SpatialVelocityModel(const Ref sigma_v, const Ref sigma_w) 16 | { 17 | F_ = MatrixXd::Identity(6, 6); 18 | 19 | Q_.resize(6, 6); 20 | Q_ = MatrixXd::Zero(6, 6); 21 | 22 | Q_.block<3, 3>(0, 0) = sigma_v; 23 | Q_.block<3, 3>(3, 3) = sigma_w; 24 | 25 | input_description_ = VectorDescription(6, 0, 6); 26 | state_description_ = input_description_.noiseless_description(); 27 | } 28 | 29 | 30 | SpatialVelocityModel::~SpatialVelocityModel() 31 | {} 32 | 33 | 34 | Eigen::MatrixXd SpatialVelocityModel::getStateTransitionMatrix() 35 | { 36 | return F_; 37 | } 38 | 39 | 40 | Eigen::MatrixXd SpatialVelocityModel::getNoiseCovarianceMatrix() 41 | { 42 | return Q_; 43 | } 44 | 45 | 46 | VectorDescription SpatialVelocityModel::getInputDescription() 47 | { 48 | return input_description_; 49 | } 50 | 51 | 52 | VectorDescription SpatialVelocityModel::getStateDescription() 53 | { 54 | return state_description_; 55 | } 56 | 57 | 58 | bool SpatialVelocityModel::setProperty(const std::string& property) 59 | { 60 | return true; 61 | } 62 | -------------------------------------------------------------------------------- /src/roft-lib/src/UKFCorrection.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * BSD 3-Clause license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | using namespace bfl; 12 | using namespace bfl::sigma_point; 13 | using namespace bfl::utils; 14 | using namespace Eigen; 15 | using namespace ROFT; 16 | 17 | 18 | UKFCorrection::UKFCorrection 19 | ( 20 | std::unique_ptr measurement_model, 21 | const double alpha, 22 | const double beta, 23 | const double kappa 24 | ) noexcept : 25 | measurement_model_(std::move(measurement_model)), 26 | ut_weight_(measurement_model_->getInputDescription(), alpha, beta, kappa), 27 | ut_alpha_(alpha), 28 | ut_beta_(beta), 29 | ut_kappa_(kappa) 30 | { } 31 | 32 | 33 | UKFCorrection::UKFCorrection(UKFCorrection&& ukf_correction) noexcept : 34 | measurement_model_(std::move(ukf_correction.measurement_model_)), 35 | ut_weight_(ukf_correction.ut_weight_), 36 | ut_alpha_(ukf_correction.ut_alpha_), 37 | ut_beta_(ukf_correction.ut_beta_), 38 | ut_kappa_(ukf_correction.ut_kappa_) 39 | { } 40 | 41 | 42 | MeasurementModel& UKFCorrection::getMeasurementModel() 43 | { 44 | return *measurement_model_; 45 | } 46 | 47 | 48 | std::pair UKFCorrection::getLikelihood() 49 | { 50 | throw(std::runtime_error("Error: ROFT::UKFCorrection::getLikelihood() is not implemented.")); 51 | } 52 | 53 | 54 | void UKFCorrection::correctStep(const GaussianMixture& pred_state, GaussianMixture& corr_state) 55 | { 56 | /* Pick the correct measurement model. */ 57 | MeasurementModel& model = getMeasurementModel(); 58 | 59 | /* Get the current measurement if available. */ 60 | bool valid_measurement; 61 | Data measurement; 62 | std::tie(valid_measurement, measurement) = model.measure(); 63 | 64 | if (!valid_measurement) 65 | { 66 | corr_state = pred_state; 67 | return; 68 | } 69 | 70 | /* Extract measurement size. */ 71 | std::size_t meas_size = model.getMeasurementDescription().total_size(); 72 | 73 | /* Evaluate the joint state-measurement statistics, if possible. */ 74 | bool valid = false; 75 | MatrixXd Pxy; 76 | 77 | /* Augment the previous state using measurement noise statistics. */ 78 | GaussianMixture pred_state_augmented = pred_state; 79 | MatrixXd noise_covariance_matrix; 80 | std::tie(std::ignore, noise_covariance_matrix) = model.getNoiseCovarianceMatrix(); 81 | pred_state_augmented.augmentWithNoise(noise_covariance_matrix); 82 | 83 | /* Update the UT weights. */ 84 | ut_weight_ = UTWeight(measurement_model_->getInputDescription(), ut_alpha_, ut_beta_, ut_kappa_); 85 | 86 | /* Predict the measurements. */ 87 | GaussianMixture predicted_meas; 88 | std::tie(valid, predicted_meas, Pxy) = sigma_point::unscented_transform(pred_state_augmented, ut_weight_, *measurement_model_); 89 | if (!valid) 90 | { 91 | corr_state = pred_state; 92 | return; 93 | } 94 | 95 | /* Evaluate the innovation if possible. */ 96 | bool valid_innovation; 97 | Data innovation; 98 | /* This temporary is required since some MeasurementModel::innovation methods may try to cast from 99 | const Ref to MatrixXd resulting in a bfl::any::bad_any_cast. 100 | 101 | Hopefully, using std::move, it is possible to steal the memory from predicted_meas.mean(). */ 102 | MatrixXd y_p = std::move(predicted_meas.mean()); 103 | std::tie(valid_innovation, innovation) = model.innovation(y_p, measurement); 104 | 105 | if (!valid_innovation) 106 | { 107 | corr_state = pred_state; 108 | return; 109 | } 110 | 111 | /* Cast innovations once for all. */ 112 | MatrixXd innovations = any::any_cast(std::move(innovation)); 113 | 114 | /* Our mixture only has one component. */ 115 | 116 | /* Evaluate the Kalman Gain 117 | K = Pxy * (Py)^{-1} */ 118 | MatrixXd K = Pxy * predicted_meas.covariance().inverse(); 119 | 120 | /* Evaluate the K * innovation product once for all. */ 121 | MatrixXd K_innovations = K * innovations; 122 | 123 | /* Evaluate the filtered mean. 124 | x_{k}+ = x{k}- + K * innovation */ 125 | corr_state.mean().topRows(corr_state.dim_linear) = pred_state.mean().topRows(corr_state.dim_linear) + K_innovations.topRows(corr_state.dim_linear); 126 | 127 | /* Sum the mean predicted quaternion with rotation vectors resulting from the product between K and the innovations.*/ 128 | corr_state.mean().middleRows(corr_state.dim_linear, 4) = sum_quaternion_rotation_vector(pred_state.mean().middleRows(corr_state.dim_linear, 4), K_innovations.middleRows(corr_state.dim_linear, 3)); 129 | 130 | /* Evaluate the filtered covariance 131 | P_{k}+ = P_{k}- - K * Py * K' */ 132 | corr_state.covariance() = pred_state.covariance() - K * predicted_meas.covariance() * K.transpose(); 133 | } 134 | -------------------------------------------------------------------------------- /src/roft/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | project(ROFT 11 | LANGUAGES CXX 12 | VERSION 1.2.1) 13 | 14 | set(CMAKE_CXX_STANDARD 14) 15 | 16 | # BayesFilters 17 | find_package(BayesFilters REQUIRED) 18 | 19 | # Eigen 3 20 | find_package(Eigen3 REQUIRED) 21 | 22 | # RobotsIO 23 | find_package(RobotsIO REQUIRED) 24 | 25 | # OpenCV 26 | find_package(OpenCV REQUIRED) 27 | 28 | # PkgConfig 29 | find_package(PkgConfig REQUIRED) 30 | 31 | # libconfig 32 | pkg_check_modules(LIBCONFIG++ REQUIRED IMPORTED_TARGET libconfig++) 33 | 34 | # tclap 35 | pkg_check_modules(TCLAP REQUIRED tclap) 36 | 37 | # Support for NVIDIA Optical Flow within OpenCV is optional 38 | if (OpenCV_opencv_cudaoptflow_FOUND) 39 | add_definitions(-DHAS_NVOF) 40 | endif() 41 | 42 | add_executable(ROFT-tracker include/ConfigParser.h src/ConfigParser.cpp src/main.cpp) 43 | 44 | target_include_directories(ROFT-tracker 45 | PRIVATE 46 | ${CMAKE_CURRENT_SOURCE_DIR}/include 47 | ${TCLAP_INCLUDEDIR} 48 | ) 49 | 50 | target_link_libraries(ROFT-tracker 51 | PRIVATE 52 | BayesFilters::BayesFilters 53 | Eigen3::Eigen 54 | PkgConfig::LIBCONFIG++ 55 | RobotsIO::RobotsIO 56 | ROFT 57 | ) 58 | 59 | install(TARGETS ROFT-tracker DESTINATION bin) 60 | -------------------------------------------------------------------------------- /test/init.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | if [ -f roft_env/bin/activate ]; then 11 | . roft_env/bin/activate 12 | fi 13 | 14 | CONVERSION_TOOL_PATH=./tools/dataset/conversion/ 15 | FASTYCB_PATH='./dataset/fast-ycb' 16 | HO3D_PATH='./dataset/ho3d/' 17 | 18 | if [ -d "$FASTYCB_PATH" ]; then 19 | for object_name in `cat config/classes.txt` 20 | do 21 | OBJECT_ID="" 22 | if [ $object_name == "003_cracker_box" ]; then 23 | OBJECT_ID="2"; 24 | elif [ $object_name == "004_sugar_box" ]; then 25 | OBJECT_ID="3" 26 | elif [ $object_name == "005_tomato_soup_can" ]; then 27 | OBJECT_ID="4" 28 | elif [ $object_name == "006_mustard_bottle" ]; then 29 | OBJECT_ID="5" 30 | elif [ $object_name == "009_gelatin_box" ]; then 31 | OBJECT_ID="8" 32 | elif [ $object_name == "010_potted_meat_can" ]; then 33 | OBJECT_ID="9" 34 | fi 35 | 36 | echo "[fastycb] Converting ground truth to YCB-Video format for sequence "$object_name 37 | python $CONVERSION_TOOL_PATH/nvdu_poses_to_ycbv.py\ 38 | --format gt\ 39 | --obj_id $OBJECT_ID\ 40 | --in_path $FASTYCB_PATH/$object_name/gt/poses.txt\ 41 | --out_path $FASTYCB_PATH/$object_name/gt/poses_ycb.txt 42 | 43 | echo "[fastycb] Converting DOPE poses to YCB-Video format for sequence "$object_name 44 | python $CONVERSION_TOOL_PATH/nvdu_poses_to_ycbv.py\ 45 | --format gt\ 46 | --obj_id $OBJECT_ID\ 47 | --in_path $FASTYCB_PATH/$object_name/dope/poses.txt\ 48 | --out_path $FASTYCB_PATH/$object_name/dope/poses_ycb.txt 49 | done 50 | 51 | for object_name in 003_cracker_box 006_mustard_bottle; 52 | do 53 | OBJECT_ID="" 54 | if [ $object_name == "003_cracker_box" ]; then 55 | OBJECT_ID="2"; 56 | elif [ $object_name == "006_mustard_bottle" ]; then 57 | OBJECT_ID="5" 58 | fi 59 | 60 | echo "[fastycb_qual] Converting DOPE poses to YCB-Video format for sequence "$object_name 61 | python $CONVERSION_TOOL_PATH/nvdu_poses_to_ycbv.py\ 62 | --format gt\ 63 | --obj_id $OBJECT_ID\ 64 | --in_path $FASTYCB_PATH/${object_name}_real/dope/poses.txt\ 65 | --out_path $FASTYCB_PATH/${object_name}_real/dope/poses_ycb.txt 66 | done 67 | else 68 | echo "[fastycb_qual] Skipping as the dataset 'fast-ycb' is not available in folder 'dataset'" 69 | fi 70 | 71 | if [ -d "$HO3D_PATH" ]; then 72 | for object_name in `cat config/classes_ho3d.txt` 73 | do 74 | OBJECT_ID="" 75 | if [ $object_name == "003_cracker_box" ]; then 76 | OBJECT_ID="2"; 77 | elif [ $object_name == "004_sugar_box" ]; then 78 | OBJECT_ID="3" 79 | elif [ $object_name == "005_tomato_soup_can" ]; then 80 | OBJECT_ID="4" 81 | elif [ $object_name == "006_mustard_bottle" ]; then 82 | OBJECT_ID="5" 83 | elif [ $object_name == "009_gelatin_box" ]; then 84 | OBJECT_ID="8" 85 | elif [ $object_name == "010_potted_meat_can" ]; then 86 | OBJECT_ID="9" 87 | fi 88 | 89 | for sequence in $HO3D_PATH/$object_name*; do 90 | echo "[ho3d] Converting ground truth to NVDU format for sequence "$sequence 91 | python $CONVERSION_TOOL_PATH/ycbv_poses_to_nvdu.py\ 92 | --format gt\ 93 | --obj_id $OBJECT_ID\ 94 | --in_path $sequence/gt/poses.txt\ 95 | --out_path $sequence/gt/poses_nvdu.txt 96 | 97 | echo "[ho3d] Converting DOPE poses to YCB-Video format for sequence "$sequence 98 | python $CONVERSION_TOOL_PATH/nvdu_poses_to_ycbv.py\ 99 | --format gt\ 100 | --obj_id $OBJECT_ID\ 101 | --in_path $sequence/dope/poses.txt\ 102 | --out_path $sequence/dope/poses_ycb.txt 103 | done 104 | done 105 | else 106 | echo "[ho3d] Skipping as the dataset 'ho3d' is not available in folder 'dataset'" 107 | fi 108 | 109 | if [ -f roft_env/bin/activate ]; then 110 | deactivate 111 | fi 112 | -------------------------------------------------------------------------------- /test/post_process_results.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | INPUT_PATH=$2 12 | 13 | OBJECT_ID="" 14 | if [ $OBJECT_NAME == "003_cracker_box" ]; then 15 | OBJECT_ID="2"; 16 | elif [ $OBJECT_NAME == "004_sugar_box" ]; then 17 | OBJECT_ID="3" 18 | elif [ $OBJECT_NAME == "005_tomato_soup_can" ]; then 19 | OBJECT_ID="4" 20 | elif [ $OBJECT_NAME == "006_mustard_bottle" ]; then 21 | OBJECT_ID="5" 22 | elif [ $OBJECT_NAME == "009_gelatin_box" ]; then 23 | OBJECT_ID="8" 24 | elif [ $OBJECT_NAME == "010_potted_meat_can" ]; then 25 | OBJECT_ID="9" 26 | fi 27 | 28 | rm -f $INPUT_PATH/pose_estimate_ycb.txt 29 | python ./tools/dataset/conversion/nvdu_poses_to_ycbv.py\ 30 | --format pred\ 31 | --obj_id $OBJECT_ID\ 32 | --in_path $INPUT_PATH/pose_estimate.txt\ 33 | --out_path $INPUT_PATH/pose_estimate_ycb.txt 34 | -------------------------------------------------------------------------------- /test/post_process_results_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | SEQ_NAME=$2 12 | INPUT_PATH=$3 13 | 14 | OBJECT_ID="" 15 | if [ $OBJECT_NAME == "003_cracker_box" ]; then 16 | OBJECT_ID="2"; 17 | elif [ $OBJECT_NAME == "004_sugar_box" ]; then 18 | OBJECT_ID="3" 19 | elif [ $OBJECT_NAME == "005_tomato_soup_can" ]; then 20 | OBJECT_ID="4" 21 | elif [ $OBJECT_NAME == "006_mustard_bottle" ]; then 22 | OBJECT_ID="5" 23 | elif [ $OBJECT_NAME == "009_gelatin_box" ]; then 24 | OBJECT_ID="8" 25 | elif [ $OBJECT_NAME == "010_potted_meat_can" ]; then 26 | OBJECT_ID="9" 27 | fi 28 | 29 | rm -f $INPUT_PATH/pose_estimate_ycb.txt 30 | python ./tools/dataset/conversion/nvdu_poses_to_ycbv.py\ 31 | --format pred\ 32 | --obj_id $OBJECT_ID\ 33 | --in_path $INPUT_PATH/pose_estimate.txt\ 34 | --out_path $INPUT_PATH/pose_estimate_ycb.txt 35 | 36 | python ./tools/dataset/results_padding/pad_results.py $INPUT_PATH/pose_estimate_ycb.txt $OBJECT_NAME $SEQ_NAME 37 | -------------------------------------------------------------------------------- /test/run_all_experiments: -------------------------------------------------------------------------------- 1 | bash test/test_full_all.sh 2 | bash test/test_full.sh 003_cracker_box_real 3 | bash test/test_full.sh 006_mustard_bottle_real 4 | bash test/test_full_gt_mask_all.sh 5 | bash test/test_full_gt_mask_gt_pose_all.sh 6 | bash test/test_full_gt_pose_all.sh 7 | bash test/test_full_noflowaid_all.sh 8 | bash test/test_full_nooutrej_all.sh 9 | bash test/test_full_nopose_all.sh 10 | bash test/test_full_noposesync_all.sh 11 | bash test/test_full_novel_all.sh 12 | bash test/test_full_all_ho3d.sh 13 | bash test/test_full_gt_mask_all_ho3d.sh 14 | bash test/test_full_gt_mask_gt_pose_all_ho3d.sh 15 | bash test/test_full_gt_pose_all_ho3d.sh 16 | -------------------------------------------------------------------------------- /test/run_paper_experiments: -------------------------------------------------------------------------------- 1 | # Table I 2 | bash test/test_full_all.sh 3 | 4 | # Table II 5 | bash test/test_full_noflowaid_all.sh 6 | 7 | # Table IV 8 | bash test/test_full_gt_mask_all.sh 9 | bash test/test_full_gt_mask_gt_pose_all.sh 10 | bash test/test_full_gt_pose_all.sh 11 | bash test/test_full_noflowaid_all.sh 12 | bash test/test_full_nooutrej_all.sh 13 | bash test/test_full_nopose_all.sh 14 | bash test/test_full_noposesync_all.sh 15 | bash test/test_full_novel_all.sh 16 | 17 | # Figure 3 18 | bash test/test_full.sh 003_cracker_box_real 19 | bash test/test_full.sh 006_mustard_bottle_real 20 | -------------------------------------------------------------------------------- /test/test_full.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | GT_MASK="false" 12 | GT_OF="false" 13 | GT_POSE="false" 14 | NO_OUT_REJ="false" 15 | NO_POSE_SYNC="false" 16 | NO_FLOW_AID="false" 17 | NO_POSE="false" 18 | NO_VEL="false" 19 | 20 | bash test/test.sh $OBJECT_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL 21 | -------------------------------------------------------------------------------- /test/test_full_all.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | for object_name in `cat config/classes.txt` 11 | do 12 | bash test/test_full.sh $object_name 13 | done 14 | -------------------------------------------------------------------------------- /test/test_full_all_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | HO3D_PATH='./dataset/ho3d/' 11 | 12 | for object_name in `cat config/classes_ho3d.txt` 13 | do 14 | for sequence_name in `ls -d $HO3D_PATH/$object_name*`; do 15 | sequence_number=`echo ${sequence_name##*_}` 16 | bash test/test_full_ho3d.sh $object_name $sequence_number 17 | done 18 | done 19 | -------------------------------------------------------------------------------- /test/test_full_gt_mask.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | GT_MASK="true" 12 | GT_OF="false" 13 | GT_POSE="false" 14 | NO_OUT_REJ="false" 15 | NO_POSE_SYNC="false" 16 | NO_FLOW_AID="false" 17 | NO_POSE="false" 18 | NO_VEL="false" 19 | 20 | bash test/test.sh $OBJECT_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL 21 | -------------------------------------------------------------------------------- /test/test_full_gt_mask_all.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | for object_name in `cat config/classes.txt` 11 | do 12 | bash test/test_full_gt_mask.sh $object_name 13 | done 14 | -------------------------------------------------------------------------------- /test/test_full_gt_mask_all_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | HO3D_PATH='./dataset/ho3d/' 11 | 12 | for object_name in `cat config/classes_ho3d.txt` 13 | do 14 | for sequence_name in `ls -d $HO3D_PATH/$object_name*`; do 15 | sequence_number=`echo ${sequence_name##*_}` 16 | bash test/test_full_gt_mask_ho3d.sh $object_name $sequence_number 17 | done 18 | done 19 | -------------------------------------------------------------------------------- /test/test_full_gt_mask_gt_pose.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | GT_MASK="true" 12 | GT_OF="false" 13 | GT_POSE="true" 14 | NO_OUT_REJ="false" 15 | NO_POSE_SYNC="false" 16 | NO_FLOW_AID="false" 17 | NO_POSE="false" 18 | NO_VEL="false" 19 | 20 | bash test/test.sh $OBJECT_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL 21 | -------------------------------------------------------------------------------- /test/test_full_gt_mask_gt_pose_all.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | for object_name in `cat config/classes.txt` 11 | do 12 | bash test/test_full_gt_mask_gt_pose.sh $object_name 13 | done 14 | -------------------------------------------------------------------------------- /test/test_full_gt_mask_gt_pose_all_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | HO3D_PATH='./dataset/ho3d/' 11 | 12 | for object_name in `cat config/classes_ho3d.txt` 13 | do 14 | for sequence_name in `ls -d $HO3D_PATH/$object_name*`; do 15 | sequence_number=`echo ${sequence_name##*_}` 16 | bash test/test_full_gt_mask_gt_pose_ho3d.sh $object_name $sequence_number 17 | done 18 | done 19 | -------------------------------------------------------------------------------- /test/test_full_gt_mask_gt_pose_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | SEQ_NAME=$2 12 | GT_MASK="true" 13 | GT_OF="false" 14 | GT_POSE="true" 15 | NO_OUT_REJ="false" 16 | NO_POSE_SYNC="false" 17 | NO_FLOW_AID="false" 18 | NO_POSE="false" 19 | NO_VEL="false" 20 | HO3D_MASK="false" 21 | 22 | bash test/test_ho3d.sh $OBJECT_NAME $SEQ_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL $HO3D_MASK 23 | -------------------------------------------------------------------------------- /test/test_full_gt_mask_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | SEQ_NAME=$2 12 | GT_MASK="true" 13 | GT_OF="false" 14 | GT_POSE="false" 15 | NO_OUT_REJ="false" 16 | NO_POSE_SYNC="false" 17 | NO_FLOW_AID="false" 18 | NO_POSE="false" 19 | NO_VEL="false" 20 | HO3D_MASK="false" 21 | 22 | bash test/test_ho3d.sh $OBJECT_NAME $SEQ_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL $HO3D_MASK 23 | -------------------------------------------------------------------------------- /test/test_full_gt_pose.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | GT_MASK="false" 12 | GT_OF="false" 13 | GT_POSE="true" 14 | NO_OUT_REJ="false" 15 | NO_POSE_SYNC="false" 16 | NO_FLOW_AID="false" 17 | NO_POSE="false" 18 | NO_VEL="false" 19 | 20 | bash test/test.sh $OBJECT_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL 21 | -------------------------------------------------------------------------------- /test/test_full_gt_pose_all.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | for object_name in `cat config/classes.txt` 11 | do 12 | bash test/test_full_gt_pose.sh $object_name 13 | done 14 | -------------------------------------------------------------------------------- /test/test_full_gt_pose_all_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | HO3D_PATH='./dataset/ho3d/' 11 | 12 | for object_name in `cat config/classes_ho3d.txt` 13 | do 14 | for sequence_name in `ls -d $HO3D_PATH/$object_name*`; do 15 | sequence_number=`echo ${sequence_name##*_}` 16 | bash test/test_full_gt_pose_ho3d.sh $object_name $sequence_number 17 | done 18 | done 19 | -------------------------------------------------------------------------------- /test/test_full_gt_pose_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | SEQ_NAME=$2 12 | GT_MASK="false" 13 | GT_OF="false" 14 | GT_POSE="true" 15 | NO_OUT_REJ="false" 16 | NO_POSE_SYNC="false" 17 | NO_FLOW_AID="false" 18 | NO_POSE="false" 19 | NO_VEL="false" 20 | HO3D_MASK="false" 21 | 22 | bash test/test_ho3d.sh $OBJECT_NAME $SEQ_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL $HO3D_MASK 23 | -------------------------------------------------------------------------------- /test/test_full_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | SEQ_NAME=$2 12 | GT_MASK="false" 13 | GT_OF="false" 14 | GT_POSE="false" 15 | NO_OUT_REJ="false" 16 | NO_POSE_SYNC="false" 17 | NO_FLOW_AID="false" 18 | NO_POSE="false" 19 | NO_VEL="false" 20 | 21 | bash test/test_ho3d.sh $OBJECT_NAME $SEQ_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL 22 | -------------------------------------------------------------------------------- /test/test_full_ho3d_mask_all_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | HO3D_PATH='./dataset/ho3d/' 11 | 12 | for object_name in `cat config/classes_ho3d.txt` 13 | do 14 | for sequence_name in `ls -d $HO3D_PATH/$object_name*`; do 15 | sequence_number=`echo ${sequence_name##*_}` 16 | bash test/test_full_ho3d_mask_ho3d.sh $object_name $sequence_number 17 | done 18 | done 19 | -------------------------------------------------------------------------------- /test/test_full_ho3d_mask_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | SEQ_NAME=$2 12 | GT_MASK="false" 13 | GT_OF="false" 14 | GT_POSE="false" 15 | NO_OUT_REJ="false" 16 | NO_POSE_SYNC="false" 17 | NO_FLOW_AID="false" 18 | NO_POSE="false" 19 | NO_VEL="false" 20 | HO3D_MASK="true" 21 | 22 | bash test/test_ho3d.sh $OBJECT_NAME $SEQ_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL $HO3D_MASK 23 | -------------------------------------------------------------------------------- /test/test_full_noflowaid.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | GT_MASK="false" 12 | GT_OF="false" 13 | GT_POSE="false" 14 | NO_OUT_REJ="false" 15 | NO_POSE_SYNC="false" 16 | NO_FLOW_AID="true" 17 | NO_POSE="false" 18 | NO_VEL="false" 19 | 20 | bash test/test.sh $OBJECT_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL 21 | -------------------------------------------------------------------------------- /test/test_full_noflowaid_all.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | for object_name in `cat config/classes.txt` 11 | do 12 | bash test/test_full_noflowaid.sh $object_name 13 | done 14 | -------------------------------------------------------------------------------- /test/test_full_nooutrej.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | GT_MASK="false" 12 | GT_OF="false" 13 | GT_POSE="false" 14 | NO_OUT_REJ="true" 15 | NO_POSE_SYNC="false" 16 | NO_FLOW_AID="false" 17 | NO_POSE="false" 18 | NO_VEL="false" 19 | 20 | bash test/test.sh $OBJECT_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL 21 | -------------------------------------------------------------------------------- /test/test_full_nooutrej_all.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | for object_name in `cat config/classes.txt` 11 | do 12 | bash test/test_full_nooutrej.sh $object_name 13 | done 14 | -------------------------------------------------------------------------------- /test/test_full_nopose.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | GT_MASK="false" 12 | GT_OF="false" 13 | GT_POSE="false" 14 | NO_OUT_REJ="false" 15 | NO_POSE_SYNC="false" 16 | NO_FLOW_AID="false" 17 | NO_POSE="true" 18 | NO_VEL="false" 19 | 20 | bash test/test.sh $OBJECT_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL 21 | -------------------------------------------------------------------------------- /test/test_full_nopose_all.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | for object_name in `cat config/classes.txt` 11 | do 12 | bash test/test_full_nopose.sh $object_name 13 | done 14 | -------------------------------------------------------------------------------- /test/test_full_noposesync.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | GT_MASK="false" 12 | GT_OF="false" 13 | GT_POSE="false" 14 | NO_OUT_REJ="false" 15 | NO_POSE_SYNC="true" 16 | NO_FLOW_AID="false" 17 | NO_POSE="false" 18 | NO_VEL="false" 19 | 20 | bash test/test.sh $OBJECT_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL 21 | -------------------------------------------------------------------------------- /test/test_full_noposesync_all.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | for object_name in `cat config/classes.txt` 11 | do 12 | bash test/test_full_noposesync.sh $object_name 13 | done 14 | -------------------------------------------------------------------------------- /test/test_full_novel.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | OBJECT_NAME=$1 11 | GT_MASK="false" 12 | GT_OF="false" 13 | GT_POSE="false" 14 | NO_OUT_REJ="false" 15 | NO_POSE_SYNC="false" 16 | NO_FLOW_AID="false" 17 | NO_POSE="false" 18 | NO_VEL="true" 19 | 20 | bash test/test.sh $OBJECT_NAME $GT_MASK $GT_OF $GT_POSE $NO_OUT_REJ $NO_POSE_SYNC $NO_FLOW_AID $NO_POSE $NO_VEL 21 | -------------------------------------------------------------------------------- /test/test_full_novel_all.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | for object_name in `cat config/classes.txt` 11 | do 12 | bash test/test_full_novel.sh $object_name 13 | done 14 | -------------------------------------------------------------------------------- /tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | option(BUILD_NVOF "Build NVIDIA NVOF-based optical flow dumper" OFF) 11 | option(BUILD_RENDERER "Build OpenGL-based object renderer" OFF) 12 | option(BUILD_RS_CAPTURE "Build realsense-based sequence recording tool" OFF) 13 | 14 | if (BUILD_NVOF) 15 | add_subdirectory(nvof) 16 | endif() 17 | 18 | if (BUILD_RENDERER) 19 | add_subdirectory(object_renderer) 20 | endif() 21 | 22 | if (BUILD_RS_CAPTURE) 23 | add_subdirectory(rs-capture) 24 | endif() 25 | -------------------------------------------------------------------------------- /tools/dataset/conversion/ho3d_utils.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import numpy as np 11 | import imageio 12 | import json 13 | import cv2 14 | import struct 15 | from pyquaternion import Quaternion 16 | 17 | 18 | def get_pose(anns_path, obj_id): 19 | meta_dict = np.load(anns_path, allow_pickle=True) 20 | 21 | pose = np.zeros((4,4)) 22 | pose[3,3] = 1 23 | 24 | """ 25 | Conversion taken from the Function: convertPoseOpenDR_DIRT(rot, trans) of: 26 | https://github.com/shreyashampali/HOnnotate/blob/d94f6b7e1c09a8c7229799a0d48fdc5f2b416780/optimization/ghope/utils.py#L579 27 | """ 28 | coordChangMat = np.array([[1., 0., 0.], [0., -1., 0.], [0., 0., -1.]]) 29 | pose[:3,:3] = coordChangMat.dot(cv2.Rodrigues(meta_dict['objRot'])[0]) 30 | pose[:3,3] = meta_dict['objTrans'].dot(coordChangMat.T) 31 | 32 | return pose 33 | 34 | 35 | def decode_depth_img(inFileName): 36 | ''' 37 | Decode the depth image to depth map in meters 38 | :param inFileName: input file name 39 | :return: depth map (float) in meters 40 | The function taken from: 41 | https://github.com/shreyashampali/ho3d/blob/466b58262b2cec85358ccfc8443ff7ced7366df6/utils/vis_utils.py#L380 42 | ''' 43 | depth_scale = 0.00012498664727900177 44 | depth_img = cv2.imread(inFileName) 45 | dpt = depth_img[:, :, 2] + depth_img[:, :, 1] * 256 46 | dpt = dpt * depth_scale 47 | 48 | return dpt 49 | 50 | 51 | def T_to_aa(T_mat): 52 | aa = np.zeros(7) 53 | aa[:3] = T_mat[:3,3] 54 | try: 55 | q = Quaternion(matrix = T_mat[:3,:3]) 56 | except ValueError: 57 | U, s, VT = np.linalg.svd(T_mat[:3,:3]) 58 | q = Quaternion(matrix = np.dot(U, VT)) 59 | aa[3:6] = q.axis 60 | aa[6] = q.angle 61 | 62 | return aa 63 | 64 | 65 | def read_depth_float(path): 66 | with open(path, 'rb') as dat: 67 | # This will give data as a 1D array 68 | data = list(struct.iter_unpack('f', dat.read())) 69 | depth = (np.array(data[4:])).reshape(480, 640) 70 | 71 | return depth 72 | 73 | 74 | def write_depth_float(path, depth): 75 | f = open(path,'wb') 76 | f.write(struct.pack('=Q', depth.shape[1])) 77 | f.write(struct.pack('=Q', depth.shape[0])) 78 | f.write(depth.astype('float32', order='C').tobytes()) 79 | f.close() 80 | 81 | 82 | def write_depth_png(path, depth, factor=1000): 83 | imageio.imwrite(path, np.array(depth*factor, dtype='uint16')) 84 | 85 | 86 | def write_binary_mask(in_path, out_path): 87 | mask = imageio.imread(in_path) 88 | mask = np.where(mask[:,:,2]>150, 255, 0) 89 | full_mask = cv2.resize(mask, (640, 480), interpolation=cv2.INTER_NEAREST) 90 | imageio.imwrite(out_path, np.array(full_mask, dtype='uint8')) 91 | 92 | 93 | def write_cam_K(out_name, meta_path): 94 | meta_dict = np.load(meta_path, allow_pickle=True) 95 | cam_K = meta_dict['camMat'] 96 | 97 | cam_dict = { 98 | "name": "Camera (640x480)", 99 | "width": 640, 100 | "height": 480, 101 | "fx": str(cam_K[0,0]), 102 | "fy": str(cam_K[1,1]), 103 | "cx": str(cam_K[0,2]), 104 | "cy": str(cam_K[1,2]) 105 | } 106 | 107 | out_file = open(out_name, 'w') 108 | json.dump(cam_dict, out_file, indent=1) 109 | out_file.close() 110 | 111 | 112 | def produce_bbox_txt(depth): 113 | 114 | where = np.array(np.where(depth)) 115 | x1, y1 = np.amin(where, axis=1) # top-left, row-column 116 | x2, y2 = np.amax(where, axis=1) # bottom-right, row-column 117 | bbox = f' {y1} {x1} {y2} {x2}\n' # column-row, top-left, bottom-right 118 | 119 | return bbox 120 | -------------------------------------------------------------------------------- /tools/dataset/conversion/nvdu_poses_to_ycbv.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import utils 11 | import argparse 12 | import numpy 13 | import sys 14 | 15 | 16 | def configure(args): 17 | cfg = dict() 18 | cfg['nvdu_path'] = './tools/third_party/Dataset_Utilities/' 19 | cfg['ycbv_models_path'] = './YCB_Video_Models/models/' 20 | 21 | cfg['out_path'] = args.out_path 22 | cfg['in_path'] = args.in_path 23 | 24 | cfg['obj_id'] = args.obj_id 25 | 26 | return cfg 27 | 28 | 29 | def main(): 30 | parser = argparse.ArgumentParser(description='Convert predicted poses in order to refer to the YCB-Video models instead of the NVDU models.') 31 | parser.add_argument("--out_path", type=str, default="out.txt") 32 | parser.add_argument("--in_path", type=str) 33 | parser.add_argument("--obj_id", type=int) 34 | parser.add_argument("--format", type=str, default='gt', help='Input format: "gt" or "pred"') 35 | args = parser.parse_args() 36 | 37 | cfg = configure(args) 38 | 39 | if args.format=='gt': 40 | velNaN = False 41 | elif args.format=='pred': 42 | velNaN = True 43 | else: 44 | print(f'Input format "{args.format}" not supported.') 45 | sys.exit(1) 46 | 47 | nvdu_aligned_to_ycbv = utils.nvdu_to_ycbv(cfg['nvdu_path'], cfg['ycbv_models_path'], cfg['obj_id']) 48 | poses = utils.get_obj_poses_synthetic(cfg['in_path']) 49 | ycbv_aa = list() 50 | for i, nvdu_T in poses.items(): 51 | ycbv_T = numpy.dot(nvdu_T, nvdu_aligned_to_ycbv) 52 | ycbv_aa.append(utils.T_to_aa(ycbv_T)) 53 | 54 | utils.matrix_to_txt(ycbv_aa, cfg['out_path'], velNaN) 55 | 56 | 57 | if __name__ == '__main__': 58 | main() 59 | -------------------------------------------------------------------------------- /tools/dataset/conversion/ycbv_poses_to_nvdu.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import utils 11 | import argparse 12 | import numpy 13 | import sys 14 | 15 | 16 | def configure(args): 17 | cfg = dict() 18 | cfg['nvdu_path'] = './tools/third_party/Dataset_Utilities/' 19 | cfg['ycbv_models_path'] = './YCB_Video_Models/models/' 20 | 21 | cfg['out_path'] = args.out_path 22 | cfg['in_path'] = args.in_path 23 | 24 | cfg['obj_id'] = args.obj_id 25 | 26 | return cfg 27 | 28 | 29 | def main(): 30 | parser = argparse.ArgumentParser(description='Convert predicted poses in order to refer to the NVDU models instead of the YCB-Video models.') 31 | parser.add_argument("--out_path", type=str, default="out.txt") 32 | parser.add_argument("--in_path", type=str) 33 | parser.add_argument("--obj_id", type=int) 34 | parser.add_argument("--format", type=str, default='gt', help='Input format: "gt" or "pred"') 35 | args = parser.parse_args() 36 | 37 | cfg = configure(args) 38 | 39 | if args.format=='gt': 40 | velNaN = False 41 | elif args.format=='pred': 42 | velNaN = True 43 | else: 44 | print(f'Input format "{args.format}" not supported.') 45 | sys.exit(1) 46 | 47 | nvdu_aligned_to_ycbv = utils.nvdu_to_ycbv(cfg['nvdu_path'], cfg['ycbv_models_path'], cfg['obj_id']) 48 | poses = utils.get_obj_poses_synthetic(cfg['in_path']) 49 | nvdu_aa = list() 50 | for i, ycbv_T in poses.items(): 51 | nvdu_T = numpy.dot(ycbv_T, numpy.linalg.inv(nvdu_aligned_to_ycbv)) 52 | nvdu_aa.append(utils.T_to_aa(nvdu_T)) 53 | 54 | utils.matrix_to_txt(nvdu_aa, cfg['out_path'], velNaN) 55 | 56 | 57 | if __name__ == '__main__': 58 | main() 59 | -------------------------------------------------------------------------------- /tools/dataset/data_txt_generation/generate_data_txt.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import os 11 | import sys 12 | 13 | sequence_path = str(sys.argv[1]) 14 | fps = 30.0 15 | 16 | num_lines = sum(1 for line in open(os.path.join(sequence_path, 'gt', 'poses.txt'))) 17 | 18 | f = open(os.path.join(sequence_path, 'data.txt'), 'w') 19 | 20 | for i in range(num_lines): 21 | stamp_rgb = (1.0 / fps) * i 22 | stamp_depth = stamp_rgb 23 | camera_pose = '0.0 0.0 0.0 1.0 0.0 0.0 0.0' 24 | f.write(str(stamp_rgb) + ' ' + str(stamp_depth) + ' ' + camera_pose + '\n') 25 | 26 | f.close() 27 | -------------------------------------------------------------------------------- /tools/dataset/data_txt_generation/process_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | HO3D_PATH='./dataset/ho3d/' 11 | 12 | for SEQ_NAME in `ls $HO3D_PATH`; do 13 | python ./tools/dataset/data_txt_generation/generate_data_txt.py $HO3D_PATH/$SEQ_NAME 14 | done 15 | -------------------------------------------------------------------------------- /tools/dataset/dope_pose_finder/pose_finder.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import sys 11 | 12 | # Open file 13 | file_path = str(sys.argv[1]) 14 | fps = int(sys.argv[2]) 15 | fps_steps = (1.0 / fps) / (1 / 30.0) 16 | f = open(file_path, 'r') 17 | 18 | # Read all lines 19 | lines = f.readlines() 20 | f.close() 21 | 22 | # Find the firstp pose different from the invalid pose 23 | invalid_pose = ('0.0 ' * 7)[:-1] 24 | 25 | for i, line in enumerate(lines): 26 | line = line.rstrip() 27 | if (line != invalid_pose) and (i % fps_steps == 0): 28 | # The starting frame should be i + 6 if i is not 0 29 | if i != 0: 30 | i += 6 31 | print(str(i) + ' ' + line) 32 | exit(0) 33 | -------------------------------------------------------------------------------- /tools/dataset/fc_masks_converter/fc_masks_converter.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import cv2 11 | import glob 12 | import numpy 13 | import os 14 | import sys 15 | from tqdm import tqdm 16 | 17 | masks_path = str(sys.argv[1]) 18 | ho3d_path = str(sys.argv[2]) 19 | mapping =\ 20 | { 21 | 'MC1' : '003_cracker_box_0', 22 | 'MC2' : '003_cracker_box_1', 23 | 'MC5' : '003_cracker_box_2', 24 | 'ShSu12' : '004_sugar_box_0', 25 | 'ShSu13' : '004_sugar_box_1', 26 | 'ShSu14' : '004_sugar_box_2', 27 | 'SiS1' : '004_sugar_box_3', 28 | 'SS3' : '004_sugar_box_4', 29 | 'SM2' : '006_mustard_bottle_0', 30 | 'SM3' : '006_mustard_bottle_1', 31 | 'SM4' : '006_mustard_bottle_2', 32 | 'SM5' : '006_mustard_bottle_3', 33 | 'MPM10' : '010_potted_meat_can_100', 34 | 'MPM11' : '010_potted_meat_can_101', 35 | 'MPM12' : '010_potted_meat_can_102', 36 | 'MPM13' : '010_potted_meat_can_103', 37 | 'MPM14' : '010_potted_meat_can_104' 38 | } 39 | 40 | 41 | for source_name in mapping: 42 | target_name = mapping[source_name] 43 | object_name = '_'.join(target_name.split('_')[:-1]) 44 | 45 | source_path = os.path.join(masks_path, source_name) 46 | target_path = os.path.join(ho3d_path, target_name, 'masks', 'mrcnn_ho3d') 47 | 48 | os.makedirs(target_path, exist_ok = True) 49 | 50 | # For each frame 51 | for i in tqdm(range(len(glob.glob(os.path.join(ho3d_path, target_name, 'rgb', '*.png'))))): 52 | frame_instances = [] 53 | 54 | instance_idx = 0 55 | 56 | # For each instance 57 | while True: 58 | file_path = os.path.join(source_path, str(i).zfill(4) + '_' + object_name + '_' + str(instance_idx) + '.png') 59 | mask = cv2.imread(file_path, cv2.IMREAD_GRAYSCALE) 60 | 61 | # The instances are over for this frame (or there are none) 62 | if mask is None: 63 | break 64 | 65 | frame_instances.append(mask) 66 | 67 | instance_idx += 1 68 | 69 | output_mask = numpy.zeros((480, 640), dtype = numpy.uint8) 70 | for instance in frame_instances: 71 | output_mask = cv2.add(output_mask, instance) 72 | cv2.imwrite(os.path.join(target_path, object_name + '_' + str(i) + '.png'), output_mask) 73 | -------------------------------------------------------------------------------- /tools/dataset/results_padding/pad_results.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import sys 11 | 12 | # Open file 13 | file_path = str(sys.argv[1]) 14 | object_name = str(sys.argv[2]) 15 | sequence_name = str(sys.argv[3]) 16 | video_name = object_name + '_' + sequence_name 17 | 18 | # For these sequences DOPE predictions are not available starting from the first frame 19 | # Hence, we zeropad the results at the beginning until we receive the first valid DOPE prediction 20 | # and only then we start the tracking process 21 | padding_list =\ 22 | { 23 | '006_mustard_bottle_2' : { 'padding' : 72, 'target_size' : 880 } 24 | } 25 | 26 | if video_name in padding_list: 27 | f = open(file_path, 'r+') 28 | 29 | lines = f.readlines() 30 | if len(lines) == padding_list[video_name]['target_size']: 31 | # nothing to do, the results have been already padded or are already of the right size 32 | f.close() 33 | exit(0) 34 | 35 | f.seek(0, 0) 36 | for i in range(padding_list[video_name]['padding']): 37 | f.write('0.0 ' * 13 + '\n') 38 | for line in lines: 39 | f.write(line) 40 | 41 | f.close() 42 | -------------------------------------------------------------------------------- /tools/detectron2/dataset_augmentation_tester.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import argparse 11 | import cv2 12 | import detectron2 13 | import os 14 | from dataset import DatasetDescription 15 | from dataset_mapper import Mapper 16 | from detectron2 import model_zoo 17 | from detectron2.config import get_cfg 18 | from detectron2.data import DatasetCatalog, MetadataCatalog, build_detection_train_loader 19 | from detectron2.data import detection_utils as utils 20 | from detectron2.engine import DefaultTrainer 21 | from detectron2.utils.visualizer import Visualizer 22 | 23 | 24 | def main(): 25 | 26 | parser = argparse.ArgumentParser() 27 | parser.add_argument('--dataset-name', dest = 'dataset_name', type = str, required = True) 28 | options = parser.parse_args() 29 | 30 | dataset = DatasetDescription(options.dataset_name, './tools/detectron2/' + options.dataset_name + '_dataset_description.pickle', False) 31 | DatasetCatalog.register(options.dataset_name, lambda : dataset.dataset()) 32 | MetadataCatalog.get(options.dataset_name).set(thing_classes = dataset.class_list()) 33 | metadata = MetadataCatalog.get(options.dataset_name) 34 | 35 | cfg = get_cfg() 36 | cfg.merge_from_file(model_zoo.get_config_file('COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml')) 37 | cfg.DATASETS.TRAIN = (options.dataset_name,) 38 | cfg.INPUT.MASK_FORMAT = 'bitmask' 39 | 40 | counter = 0 41 | train_data_loader = build_detection_train_loader(cfg, mapper = Mapper(cfg, True)) 42 | for batch in train_data_loader: 43 | for per_image in batch: 44 | # Pytorch tensor is in (C, H, W) format 45 | img = per_image["image"].permute(1, 2, 0).cpu().detach().numpy() 46 | img = utils.convert_image_to_rgb(img, cfg.INPUT.FORMAT) 47 | 48 | # visualizer = Visualizer(img, metadata = metadata, scale = 1.0) 49 | # target_fields = per_image["instances"].get_fields() 50 | # labels = [metadata.thing_classes[i] for i in target_fields["gt_classes"]] 51 | # img = visualizer.overlay_instances\ 52 | # ( 53 | # labels = labels, 54 | # boxes = target_fields.get("gt_boxes", None), 55 | # masks = target_fields.get("gt_masks", None), 56 | # keypoints = target_fields.get("gt_keypoints", None), 57 | # ) 58 | # cv2.imwrite('./tools/detectron2/dataset_tester_' + options.dataset_name + '_' + str(counter) + '.png', img.get_image()[:, :, ::-1]) 59 | 60 | img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) 61 | cv2.imwrite('./tools/detectron2/dataset_tester_' + options.dataset_name + '_' + str(counter) + '.png', img) 62 | 63 | counter += 1 64 | if counter > 100: 65 | exit(0) 66 | 67 | 68 | if __name__ == '__main__': 69 | main() 70 | -------------------------------------------------------------------------------- /tools/detectron2/dataset_tester.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import argparse 11 | import cv2 12 | import detectron2 13 | import os 14 | import random 15 | from dataset import DatasetDescription 16 | from detectron2 import model_zoo 17 | from detectron2.config import get_cfg 18 | from detectron2.data import MetadataCatalog, DatasetCatalog 19 | from detectron2.engine import DefaultPredictor 20 | from detectron2.utils.visualizer import Visualizer 21 | from detectron2.utils.visualizer import ColorMode 22 | 23 | 24 | def main(): 25 | 26 | parser = argparse.ArgumentParser() 27 | parser.add_argument('--dataset-name', dest = 'dataset_name', type = str, required = True) 28 | options = parser.parse_args() 29 | 30 | dataset = DatasetDescription(options.dataset_name, './tools/detectron2/' + options.dataset_name + '_dataset_description.pickle', False) 31 | DatasetCatalog.register(options.dataset_name, lambda : dataset.dataset()) 32 | MetadataCatalog.get(options.dataset_name).set(thing_classes = dataset.class_list()) 33 | metadata = MetadataCatalog.get(options.dataset_name) 34 | 35 | counter = 0 36 | for d in random.sample(dataset.dataset(), 10): 37 | im = cv2.imread(d["file_name"]) 38 | visualizer = Visualizer(im[:, :, ::-1], metadata = metadata, scale = 1.0) 39 | out = visualizer.draw_dataset_dict(d) 40 | cv2.imwrite('./tools/detectron2/dataset_tester_' + options.dataset_name + '_' + str(counter) + '.png', out.get_image()[:, :, ::-1]) 41 | counter += 1 42 | 43 | 44 | if __name__ == '__main__': 45 | main() 46 | -------------------------------------------------------------------------------- /tools/detectron2/evaluate.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import argparse 11 | import detectron2 12 | import os 13 | from dataset import DatasetDescription 14 | from dataset_mapper import Mapper 15 | from detectron2 import model_zoo 16 | from detectron2.config import get_cfg 17 | from detectron2.evaluation import COCOEvaluator, inference_on_dataset 18 | from detectron2.data import DatasetCatalog, MetadataCatalog, build_detection_test_loader 19 | from detectron2.engine import DefaultTrainer 20 | from tools.detectron2 import results_saver 21 | 22 | 23 | 24 | def evaluate_single_ckpt(options): 25 | 26 | try: 27 | eval_ckpt = str(int(options.eval_ckpt) - 1).zfill(7) 28 | except: 29 | eval_ckpt = options.eval_ckpt 30 | 31 | print(f'--Using checkpoint: {eval_ckpt}!') 32 | weight_paths =\ 33 | { 34 | 'ycbv_bop_pbr' : os.path.join(options.weights_path, f'model_{eval_ckpt}.pth') 35 | } 36 | 37 | training_dataset_name = 'ycbv_bop_pbr' 38 | training_dataset_paths =\ 39 | { 40 | 'ycbv_bop_pbr' : './tools/detectron2/ycbv_bop_pbr_dataset_description.pickle' 41 | } 42 | 43 | training_dataset = DatasetDescription(training_dataset_name, training_dataset_paths[training_dataset_name], False) 44 | try: 45 | DatasetCatalog.register(training_dataset_name, lambda : training_dataset.dataset()) 46 | except: 47 | print(f'--- WARNING: Using existing dataset {training_dataset_name}. If this was not intended, please modify.') 48 | 49 | testing_dataset = DatasetDescription(options.dataset_name, './tools/detectron2/' + options.dataset_name + '_dataset_description.pickle', False) 50 | try: 51 | DatasetCatalog.register(options.dataset_name, lambda : testing_dataset.dataset()) 52 | except: 53 | print(f'--- WARNING: Using existing dataset {training_dataset_name}. If this was not intended, please modify.') 54 | MetadataCatalog.get(options.dataset_name).set(thing_classes = testing_dataset.class_list()) 55 | 56 | cfg = get_cfg() 57 | cfg.merge_from_file(model_zoo.get_config_file('COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml')) 58 | cfg.DATASETS.TRAIN = (training_dataset_name,) 59 | cfg.INPUT.MASK_FORMAT = 'bitmask' 60 | cfg.MODEL.WEIGHTS = weight_paths[training_dataset_name] 61 | cfg.OUTPUT_DIR = '/'.join(weight_paths[training_dataset_name].split('/')[:-1]) 62 | cfg.MODEL.ROI_HEADS.NUM_CLASSES = len(testing_dataset.class_list()) 63 | 64 | trainer = DefaultTrainer(cfg) 65 | trainer.resume_or_load(resume=False) 66 | evaluator = COCOEvaluator(options.dataset_name, ("bbox", "segm"), False, output_dir = './') 67 | validator_loader = build_detection_test_loader(cfg, options.dataset_name, mapper = Mapper(cfg, True, options.augmentation)) 68 | results = inference_on_dataset(trainer.model, validator_loader, evaluator) 69 | 70 | return results 71 | 72 | 73 | def boolean_string(s): 74 | if s not in {'False', 'True', True, False}: 75 | raise ValueError('Not a valid boolean string') 76 | return s == 'True' or s == True 77 | 78 | 79 | def main(): 80 | 81 | parser = argparse.ArgumentParser() 82 | parser.add_argument('--dataset-name', dest = 'dataset_name', type = str, required = True) 83 | parser.add_argument('--gpu-id', dest = 'gpu_id', type = str, required = True) 84 | parser.add_argument('--eval-ckpt', dest = 'eval_ckpt', default='final') 85 | parser.add_argument('--weights-path', dest = 'weights_path', type = str, default='./tools/detectron2/coco_mask_rcnn_R_50_FPN_3x_60k_ycbv_bop_pbr') 86 | parser.add_argument('--augmentation', dest = 'augmentation', default=False) 87 | options = parser.parse_args() 88 | options.augmentation = boolean_string(options.augmentation) 89 | 90 | os.environ['CUDA_DEVICE_ORDER'] = 'PCI_BUS_ID' 91 | os.environ['CUDA_VISIBLE_DEVICES'] = options.gpu_id 92 | 93 | results = list() 94 | if options.eval_ckpt != 'all': 95 | results_dict = evaluate_single_ckpt(options) 96 | results = [results_dict['segm']] 97 | else: 98 | ckpts = sorted([int(ckpt.split('_')[-1].split('.')[0])+1 99 | for ckpt in os.listdir(options.weights_path) 100 | if ckpt.startswith('model_0') and ckpt.endswith('.pth')]) 101 | for ckpt in ckpts: 102 | options.eval_ckpt = ckpt 103 | results_dict = evaluate_single_ckpt(options) 104 | results.append(results_dict['segm']) 105 | 106 | out_dir = os.path.join(os.getcwd(), 'detectron2_results', options.dataset_name + 107 | os.path.basename(os.path.normpath(options.weights_path))) 108 | results_saver.save_res(results, out_dir=out_dir) 109 | 110 | 111 | if __name__ == '__main__': 112 | main() 113 | -------------------------------------------------------------------------------- /tools/detectron2/evaluate_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | DETECTRON2_VENV_PATH='./tools/third_party/detectron2/env' 11 | GPU_ID=$1 12 | 13 | . $DETECTRON2_VENV_PATH/bin/activate 14 | python ./tools/detectron2/evaluate.py --dataset-name ho3d --gpu-id $GPU_ID 15 | -------------------------------------------------------------------------------- /tools/detectron2/generate_ho3d_dataset_description.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | HO3D_PATH='./dataset/ho3d' 11 | DETECTRON2_VENV_PATH='./tools/third_party/detectron2/env' 12 | 13 | . $DETECTRON2_VENV_PATH/bin/activate 14 | python ./tools/detectron2/dataset.py --dataset-name ho3d --dataset-path $HO3D_PATH --generate "true" 15 | -------------------------------------------------------------------------------- /tools/detectron2/generate_ycbv_bop_pbr_dataset_description.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | YCBV_BOP_PBR_PATH='./dataset/ycbv_bop_pbr' 11 | DETECTRON2_VENV_PATH='./tools/third_party/detectron2/env' 12 | 13 | . $DETECTRON2_VENV_PATH/bin/activate 14 | python ./tools/detectron2/dataset.py --dataset-name ycbv_bop_pbr --dataset-path $YCBV_BOP_PBR_PATH --generate "true" 15 | -------------------------------------------------------------------------------- /tools/detectron2/predictor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import torch 5 | from detectron2.modeling import build_model 6 | from detectron2.data import MetadataCatalog 7 | from detectron2.checkpoint import DetectionCheckpointer 8 | 9 | 10 | class CustomPredictor: 11 | """ 12 | Create a simple end-to-end predictor with the given config that runs on 13 | single device for a single input image. 14 | 15 | Compared to using the model directly, this class does the following additions: 16 | 17 | 1. Load checkpoint from `cfg.MODEL.WEIGHTS`. 18 | 2. Always take BGR image as the input and apply conversion defined by `cfg.INPUT.FORMAT`. 19 | 3. Apply resizing defined by `cfg.INPUT.{MIN,MAX}_SIZE_TEST`. 20 | 4. Take one input image and produce a single output, instead of a batch. 21 | 22 | If you'd like to do anything more fancy, please refer to its source code 23 | as examples to build and use the model manually. 24 | 25 | Attributes: 26 | metadata (Metadata): the metadata of the underlying dataset, obtained from 27 | cfg.DATASETS.TEST. 28 | 29 | Examples: 30 | :: 31 | pred = DefaultPredictor(cfg) 32 | inputs = cv2.imread("input.jpg") 33 | outputs = pred(inputs) 34 | """ 35 | 36 | def __init__(self, cfg): 37 | from detectron2.data import transforms as T 38 | self.cfg = cfg.clone() # cfg can be modified by model 39 | self.model = build_model(self.cfg) 40 | self.model.eval() 41 | if len(cfg.DATASETS.TEST): 42 | self.metadata = MetadataCatalog.get(cfg.DATASETS.TEST[0]) 43 | 44 | checkpointer = DetectionCheckpointer(self.model) 45 | checkpointer.load(cfg.MODEL.WEIGHTS) 46 | 47 | self.aug = T.ResizeShortestEdge( 48 | [cfg.INPUT.MIN_SIZE_TEST, cfg.INPUT.MIN_SIZE_TEST], cfg.INPUT.MAX_SIZE_TEST 49 | ) 50 | 51 | self.input_format = cfg.INPUT.FORMAT 52 | assert self.input_format in ["RGB", "BGR"], self.input_format 53 | 54 | def __call__(self, original_image): 55 | """ 56 | Args: 57 | original_image (np.ndarray): an image of shape (H, W, C) (in BGR order). 58 | 59 | Returns: 60 | predictions (dict): 61 | the output of the model for one image only. 62 | See :doc:`/tutorials/models` for details about the format. 63 | """ 64 | with torch.no_grad(): # https://github.com/sphinx-doc/sphinx/issues/4258 65 | # Apply pre-processing to image. 66 | if self.input_format == "RGB": 67 | # whether the model expects BGR inputs or RGB 68 | original_image = original_image[:, :, ::-1] 69 | height, width = original_image.shape[:2] 70 | image = torch.as_tensor(original_image.astype("float32").transpose(2, 0, 1)) 71 | 72 | inputs = {"image": image, "height": height, "width": width} 73 | predictions = self.model([inputs])[0] 74 | return predictions 75 | -------------------------------------------------------------------------------- /tools/detectron2/process_fastycb.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | FASTYCB_PATH='./dataset/fast-ycb' 11 | DETECTRON2_VENV_PATH='./tools/third_party/detectron2/env' 12 | GPU_ID=$1 13 | 14 | . $DETECTRON2_VENV_PATH/bin/activate 15 | for OBJECT_NAME in 003_cracker_box 004_sugar_box 005_tomato_soup_can 006_mustard_bottle 009_gelatin_box 010_potted_meat_can; do 16 | python ./tools/detectron2/inference.py --training-dataset-name ycbv_bop_pbr --dataset-name fastycb --dataset-path $FASTYCB_PATH --gpu-id $GPU_ID --output-path $FASTYCB_PATH --object-name $OBJECT_NAME --rgb-format png 17 | done 18 | -------------------------------------------------------------------------------- /tools/detectron2/process_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | HO3D_PATH='./dataset/ho3d' 11 | DETECTRON2_VENV_PATH='./tools/third_party/detectron2/env' 12 | GPU_ID=$1 13 | 14 | . $DETECTRON2_VENV_PATH/bin/activate 15 | for OBJECT_NAME in 003_cracker_box 004_sugar_box 006_mustard_bottle 010_potted_meat_can; do 16 | python ./tools/detectron2/inference.py --training-dataset-name ycbv_bop_pbr --dataset-name ho3d --dataset-path $HO3D_PATH --gpu-id $GPU_ID --output-path $HO3D_PATH --object-name $OBJECT_NAME --rgb-format png 17 | done 18 | -------------------------------------------------------------------------------- /tools/detectron2/results_saver.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | 11 | import os 12 | from tomark import Tomark 13 | 14 | def save_res(results, out_dir=''): 15 | """ 16 | Save experimental results as an MD table. 17 | 18 | Args: 19 | results_dict (dict): a result dictionary. 20 | out_dir (str): Output directory. If not specified, 21 | 'results' dir will be created in the root dir 22 | 23 | """ 24 | 25 | if not out_dir: 26 | out_dir = os.path.join(os.getcwd(), 'results') 27 | os.makedirs(out_dir, exist_ok=True) 28 | 29 | for i, res_dict in enumerate(results): 30 | keys_to_pop = list() 31 | for key, value in res_dict.items(): 32 | if not key.startswith('AP-'): 33 | keys_to_pop.append(key) 34 | else: 35 | results[i][key] = round(value, 2) 36 | for key in keys_to_pop: 37 | results[i].pop(key) 38 | for old_key in sorted(results[i]): 39 | results[i][old_key.replace('AP-', '')] = results[i].pop(old_key) 40 | 41 | md_table = '## Segmentation results\n\n' 42 | md_table += Tomark.table(results) 43 | 44 | out_file = open(os.path.join(out_dir, 'results.md'), 'w') 45 | out_file.write(md_table) 46 | out_file.close() 47 | -------------------------------------------------------------------------------- /tools/detectron2/test_ho3d_dataset_description.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | DETECTRON2_VENV_PATH='./tools/third_party/detectron2/env' 11 | 12 | . $DETECTRON2_VENV_PATH/bin/activate 13 | python ./tools/detectron2/dataset_tester.py --dataset-name ho3d 14 | -------------------------------------------------------------------------------- /tools/detectron2/test_ycbv_bop_pbr_dataset_augmentation.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | DETECTRON2_VENV_PATH='./tools/third_party/detectron2/env' 11 | 12 | . $DETECTRON2_VENV_PATH/bin/activate 13 | python ./tools/detectron2/dataset_augmentation_tester.py --dataset-name ycbv_bop_pbr 14 | -------------------------------------------------------------------------------- /tools/detectron2/test_ycbv_bop_pbr_dataset_description.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | DETECTRON2_VENV_PATH='./tools/third_party/detectron2/env' 11 | 12 | . $DETECTRON2_VENV_PATH/bin/activate 13 | python ./tools/detectron2/dataset_tester.py --dataset-name ycbv_bop_pbr 14 | -------------------------------------------------------------------------------- /tools/detectron2/train.py: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | import argparse 11 | import detectron2 12 | import os 13 | from dataset import DatasetDescription 14 | from dataset_mapper import Mapper 15 | from detectron2 import model_zoo 16 | from detectron2.config import get_cfg 17 | from detectron2.data import DatasetCatalog, build_detection_train_loader 18 | from detectron2.engine import DefaultTrainer 19 | 20 | 21 | 22 | def boolean_string(s): 23 | if s not in {'False', 'True', True, False}: 24 | raise ValueError('Not a valid boolean string') 25 | return s == 'True' or s == True 26 | 27 | 28 | def main(): 29 | 30 | parser = argparse.ArgumentParser() 31 | parser.add_argument('--dataset-name', dest = 'dataset_name', type = str, required = True) 32 | parser.add_argument('--gpu-id', dest = 'gpu_id', type = str, required = True) 33 | parser.add_argument('--augmentation', dest = 'augmentation', type = bool, default=True) 34 | parser.add_argument('--iterations', dest = 'iterations', type = int, default=60000) 35 | parser.add_argument('--checkpoint-period', dest = 'checkpoint_period', type = int, default=2500) 36 | options = parser.parse_args() 37 | options.augmentation = boolean_string(options.augmentation) 38 | 39 | os.environ['CUDA_DEVICE_ORDER'] = 'PCI_BUS_ID' 40 | os.environ['CUDA_VISIBLE_DEVICES'] = options.gpu_id 41 | 42 | dataset = DatasetDescription(options.dataset_name, './tools/detectron2/' + options.dataset_name + '_dataset_description.pickle', False) 43 | DatasetCatalog.register(options.dataset_name, lambda : dataset.dataset()) 44 | 45 | class Trainer(DefaultTrainer): 46 | @classmethod 47 | def build_train_loader(cls, cfg): 48 | return build_detection_train_loader(cfg, mapper = Mapper(cfg, True, options.augmentation)) 49 | 50 | cfg = get_cfg() 51 | cfg.merge_from_file(model_zoo.get_config_file('COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml')) 52 | cfg.DATASETS.TRAIN = (options.dataset_name,) 53 | cfg.DATASETS.TEST = () 54 | cfg.DATALOADER.NUM_WORKERS = 1 55 | cfg.INPUT.MASK_FORMAT = 'bitmask' 56 | cfg.MODEL.WEIGHTS = model_zoo.get_checkpoint_url('COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml') 57 | cfg.OUTPUT_DIR = './tools/detectron2/coco_mask_rcnn_R_50_FPN_3x_60k_' + options.dataset_name 58 | cfg.SOLVER.IMS_PER_BATCH = 2 59 | cfg.SOLVER.BASE_LR = 0.00025 60 | cfg.SOLVER.MAX_ITER= options.iterations 61 | cfg.SOLVER.CHECKPOINT_PERIOD = options.checkpoint_period 62 | cfg.MODEL.ROI_HEADS.BATCH_SIZE_PER_IMAGE = 512 63 | cfg.MODEL.ROI_HEADS.NUM_CLASSES = len(dataset.class_list()) 64 | 65 | os.makedirs(cfg.OUTPUT_DIR, exist_ok = True) 66 | 67 | tr = Trainer(cfg) 68 | tr.resume_or_load(resume=False) 69 | tr.train() 70 | 71 | 72 | if __name__ == '__main__': 73 | main() 74 | -------------------------------------------------------------------------------- /tools/detectron2/train_ycbv_bop_pbr.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | DETECTRON2_VENV_PATH='./tools/third_party/detectron2/env' 11 | GPU_ID=$1 12 | 13 | . $DETECTRON2_VENV_PATH/bin/activate 14 | python ./tools/detectron2/train.py --dataset-name ycbv_bop_pbr --gpu-id $GPU_ID 15 | -------------------------------------------------------------------------------- /tools/dope/process_fastycb.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | FASTYCB_PATH='./dataset/fast-ycb' 11 | DOPE_PATH='./tools/third_party/Deep_Object_Pose' 12 | DOPE_VENV_PATH='./tools/third_party/Deep_Object_Pose/env' 13 | GPU_ID=$1 14 | 15 | . $DOPE_VENV_PATH/bin/activate 16 | for OBJECT_NAME in 003_cracker_box 004_sugar_box 005_tomato_soup_can 006_mustard_bottle 009_gelatin_box 010_potted_meat_can; do 17 | python ./tools/dope/inference.py --dataset-name fastycb --dataset-path $FASTYCB_PATH --dope-path $DOPE_PATH --gpu-id $GPU_ID --output-path $FASTYCB_PATH --object-name $OBJECT_NAME --rgb-format png 18 | done 19 | -------------------------------------------------------------------------------- /tools/dope/process_fastycb_qual.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | FASTYCB_PATH='./dataset/fast-ycb' 11 | DOPE_PATH='./tools/third_party/Deep_Object_Pose' 12 | DOPE_VENV_PATH='./tools/third_party/Deep_Object_Pose/env' 13 | GPU_ID=$1 14 | 15 | . $DOPE_VENV_PATH/bin/activate 16 | for OBJECT_NAME in 003_cracker_box 006_mustard_bottle; do 17 | python ./tools/dope/inference.py --dataset-name fastycb_qual --dataset-path $FASTYCB_PATH --dope-path $DOPE_PATH --gpu-id $GPU_ID --output-path $FASTYCB_PATH --object-name $OBJECT_NAME --rgb-format png 18 | done 19 | -------------------------------------------------------------------------------- /tools/dope/process_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | HO3D_PATH='./dataset/ho3d' 11 | DOPE_PATH='./tools/third_party/Deep_Object_Pose' 12 | DOPE_VENV_PATH='./tools/third_party/Deep_Object_Pose/env' 13 | GPU_ID=$1 14 | 15 | . $DOPE_VENV_PATH/bin/activate 16 | for OBJECT_NAME in 003_cracker_box 004_sugar_box 006_mustard_bottle 010_potted_meat_can; do 17 | python ./tools/dope/inference.py --dataset-name ho3d --dataset-path $HO3D_PATH --dope-path $DOPE_PATH --gpu-id $GPU_ID --output-path $HO3D_PATH --object-name $OBJECT_NAME --rgb-format png 18 | done 19 | -------------------------------------------------------------------------------- /tools/download/download_fastycb.sh: -------------------------------------------------------------------------------- 1 | mkdir -p dataset/ 2 | [ ! -d dataset/fast-ycb ] && git clone https://github.com/hsp-iit/fast-ycb dataset/fast-ycb 3 | cd dataset/fast-ycb 4 | bash tools/download/download_dataset.sh 5 | -------------------------------------------------------------------------------- /tools/download/download_results.sh: -------------------------------------------------------------------------------- 1 | SERVER=https://dataverse.iit.it 2 | PERSISTENT_ID=doi:10.48557/9FJW42 3 | 4 | echo "Downloading results..." 5 | curl -L $SERVER/api/access/dataset/:persistentId/?persistentId=$PERSISTENT_ID -o results.zip 6 | unzip -qq results.zip 7 | rm results.zip 8 | rm MANIFEST.TXT 9 | rm README.txt 10 | -------------------------------------------------------------------------------- /tools/download/download_ycb_models.sh: -------------------------------------------------------------------------------- 1 | bash tools/third_party/download_google_drive_file.sh https://drive.google.com/file/d/1gmcDD-5bkJfcMKLZb3zGgH_HUFbulQWu/view 2 | unzip -qq YCB_Video_Models.zip -d YCB_Video_Models/ 3 | rm YCB_Video_Models.zip 4 | -------------------------------------------------------------------------------- /tools/nvof/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | add_subdirectory(dumper) 11 | -------------------------------------------------------------------------------- /tools/nvof/dumper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | set(EXE_TARGET_NAME ROFT-of-dumper) 11 | 12 | find_package(OpenCV REQUIRED) 13 | 14 | if (NOT OpenCV_opencv_cudaoptflow_FOUND) 15 | message(FATAL_ERROR "The OpenCV installation on this system is missing the OpenCV_opencv_cudaoptflow contrib module.""") 16 | endif() 17 | 18 | find_package(RobotsIO REQUIRED) 19 | 20 | set(${EXE_TARGET_NAME}_HDR 21 | ) 22 | 23 | set(${EXE_TARGET_NAME}_SRC 24 | src/main.cpp 25 | ) 26 | 27 | add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_SRC}) 28 | 29 | target_include_directories(${EXE_TARGET_NAME} 30 | PRIVATE 31 | ${CMAKE_CURRENT_SOURCE_DIR}/include 32 | ) 33 | 34 | target_link_libraries(${EXE_TARGET_NAME} 35 | PRIVATE 36 | RobotsIO::RobotsIO 37 | ROFT 38 | ) 39 | 40 | install(TARGETS ${EXE_TARGET_NAME} DESTINATION bin) 41 | -------------------------------------------------------------------------------- /tools/nvof/dumper/process_ho3d.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | HO3D_PATH='./dataset/ho3d' 11 | 12 | for SEQ_NAME in `ls $HO3D_PATH`; do 13 | OUTPUT_PATH=$HO3D_PATH/$SEQ_NAME/optical_flow/nvof_1_slow 14 | mkdir -p $OUTPUT_PATH 15 | ./build/bin/ROFT-of-dumper $HO3D_PATH/$SEQ_NAME txt png 0 0 640 480 $OUTPUT_PATH 16 | done 17 | -------------------------------------------------------------------------------- /tools/nvof/dumper/process_ycbv_real.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | YCBVSYN_PATH='./dataset/fast-ycb' 11 | 12 | for OBJECT_NAME in 003_cracker_box_real 006_mustard_bottle_real; do 13 | OUTPUT_PATH=$YCBVSYN_PATH/$OBJECT_NAME/optical_flow/nvof_1_slow 14 | mkdir -p $OUTPUT_PATH 15 | ./build/bin/ROFT-of-dumper $YCBVSYN_PATH/$OBJECT_NAME txt png 0 0 1280 720 $OUTPUT_PATH 16 | done 17 | -------------------------------------------------------------------------------- /tools/nvof/dumper/process_ycbv_synthetic.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | YCBVSYN_PATH='./dataset/fast-ycb' 11 | 12 | for OBJECT_NAME in 003_cracker_box 004_sugar_box 005_tomato_soup_can 006_mustard_bottle 009_gelatin_box 010_potted_meat_can; do 13 | OUTPUT_PATH=$YCBVSYN_PATH/$OBJECT_NAME/optical_flow/nvof_1_slow 14 | mkdir -p $OUTPUT_PATH 15 | ./build/bin/ROFT-of-dumper $YCBVSYN_PATH/$OBJECT_NAME txt png 0 0 1280 720 $OUTPUT_PATH 16 | done 17 | -------------------------------------------------------------------------------- /tools/object_renderer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | set(LIBRARY_NAME object_renderer) 11 | 12 | find_package(Python COMPONENTS Interpreter Development) 13 | 14 | find_package(pybind11 CONFIG) 15 | 16 | find_package(OpenCV REQUIRED) 17 | 18 | find_package(SuperimposeMesh 0.11.100 REQUIRED) 19 | 20 | set(${LIBRARY_NAME}_SRC src/renderer.cpp) 21 | 22 | pybind11_add_module(${LIBRARY_NAME} ${${LIBRARY_NAME}_SRC}) 23 | target_link_libraries(${LIBRARY_NAME} PRIVATE SI::SuperimposeMesh ${OpenCV_LIBS}) 24 | -------------------------------------------------------------------------------- /tools/object_renderer/grid/grid_template.xcf.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hsp-iit/roft/cee79752d9bc3759cc2ed02c1e57a278ae342d6d/tools/object_renderer/grid/grid_template.xcf.zip -------------------------------------------------------------------------------- /tools/object_renderer/shotcut/process_ycbvr.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | REPOSITORY_PATH=$PWD 11 | mkdir -p tools/object_renderer/shotcut/tmp 12 | 13 | for object_name in 003_cracker_box 006_mustard_bottle; do 14 | for name in dope ours; do 15 | ln -sfT $REPOSITORY_PATH/evaluation_output/exp_ycbvr_real/$name/$object_name/sequence_0/output.mp4 $REPOSITORY_PATH/tools/object_renderer/shotcut/tmp/$name.mp4; 16 | done 17 | ln -sfT $REPOSITORY_PATH/evaluation_output/exp_ycbvr/$name/$object_name/sequence_0/segmentation/output.mp4 $REPOSITORY_PATH/tools/object_renderer/shotcut/tmp/segmentation.mp4; 18 | ln -sfT $REPOSITORY_PATH/evaluation_output/exp_ycbvr/$name/$object_name/sequence_0/segmentation_refined/output.mp4 $REPOSITORY_PATH/tools/object_renderer/shotcut/tmp/segmentation_refined.mp4; 19 | shotcut tools/object_renderer/shotcut/project.mlt 20 | done 21 | -------------------------------------------------------------------------------- /tools/object_renderer/shotcut/process_ycbvr_comparison.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | REPOSITORY_PATH=$PWD 11 | mkdir -p tools/object_renderer/shotcut/tmp 12 | 13 | for object_name in 003_cracker_box 006_mustard_bottle; do 14 | for name in se3tracknet poserbpf ours; do 15 | ln -sfT $REPOSITORY_PATH/evaluation_output/exp_ycbvr_real/$name/$object_name/sequence_0/output.mp4 $REPOSITORY_PATH/tools/object_renderer/shotcut/tmp/$name.mp4; 16 | done 17 | shotcut tools/object_renderer/shotcut/project_comparison.mlt 18 | done 19 | -------------------------------------------------------------------------------- /tools/object_renderer/shotcut/process_ycbvs.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | REPOSITORY_PATH=$PWD 11 | mkdir -p tools/object_renderer/shotcut/tmp 12 | 13 | for object_name in 003_cracker_box 004_sugar_box 005_tomato_soup_can 006_mustard_bottle 009_gelatin_box 010_potted_meat_can; do 14 | for name in dope ours; do 15 | ln -sfT $REPOSITORY_PATH/evaluation_output/exp_ycbvs_real/$name/$object_name/sequence_0/output.mp4 $REPOSITORY_PATH/tools/object_renderer/shotcut/tmp/$name.mp4; 16 | done 17 | ln -sfT $REPOSITORY_PATH/evaluation_output/exp_ycbvs/$name/$object_name/sequence_0/segmentation/output.mp4 $REPOSITORY_PATH/tools/object_renderer/shotcut/tmp/segmentation.mp4; 18 | ln -sfT $REPOSITORY_PATH/evaluation_output/exp_ycbvs/$name/$object_name/sequence_0/segmentation_refined/output.mp4 $REPOSITORY_PATH/tools/object_renderer/shotcut/tmp/segmentation_refined.mp4; 19 | shotcut tools/object_renderer/shotcut/project.mlt 20 | done 21 | -------------------------------------------------------------------------------- /tools/object_renderer/shotcut/process_ycbvs_comparison.sh: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | REPOSITORY_PATH=$PWD 11 | mkdir -p tools/object_renderer/shotcut/tmp 12 | 13 | for object_name in 003_cracker_box 004_sugar_box 005_tomato_soup_can 006_mustard_bottle 009_gelatin_box 010_potted_meat_can; do 14 | for name in se3tracknet poserbpf ours; do 15 | ln -sfT $REPOSITORY_PATH/evaluation_output/exp_ycbvs_real/$name/$object_name/sequence_0/output.mp4 $REPOSITORY_PATH/tools/object_renderer/shotcut/tmp/$name.mp4; 16 | done 17 | shotcut tools/object_renderer/shotcut/project_comparison.mlt 18 | done 19 | -------------------------------------------------------------------------------- /tools/rs-capture/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #=============================================================================== 2 | # 3 | # Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # GPL-2+ license. See the accompanying LICENSE file for details. 7 | # 8 | #=============================================================================== 9 | 10 | set(EXE_TARGET_NAME ROFT-rs-capture) 11 | 12 | find_package(OpenCV REQUIRED) 13 | 14 | find_package(RobotsIO REQUIRED) 15 | 16 | find_package(realsense2 REQUIRED) 17 | 18 | set(${EXE_TARGET_NAME}_SRC 19 | src/main.cpp 20 | ) 21 | 22 | add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC}) 23 | 24 | target_link_libraries(${EXE_TARGET_NAME} 25 | PRIVATE 26 | RobotsIO::RobotsIO 27 | ${OpenCV_LIBS} 28 | realsense2::realsense2 29 | ) 30 | -------------------------------------------------------------------------------- /tools/rs-capture/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT) 3 | * 4 | * This software may be modified and distributed under the terms of the 5 | * GPL-2+ license. See the accompanying LICENSE file for details. 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | 18 | int main(int argc, char ** argv) 19 | { 20 | /* Create pipeline. */ 21 | rs2::pipeline pipe; 22 | rs2::config cfg; 23 | cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 30); 24 | cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_RGB8, 30); 25 | cfg.enable_stream(RS2_STREAM_INFRARED, 1, 1280, 720, RS2_FORMAT_Y8, 30); 26 | cfg.enable_stream(RS2_STREAM_INFRARED, 2, 1280, 720, RS2_FORMAT_Y8, 30); 27 | pipe.start(cfg); 28 | 29 | /* Create instance of alignment procedure. */ 30 | rs2::align align_to_color(RS2_STREAM_COLOR); 31 | 32 | /* Storage for rgb, depth and time stamps. */ 33 | std::vector rgb_frames; 34 | std::vector depth_frames; 35 | std::vector stamps; 36 | 37 | /* Take note of current time. */ 38 | auto t0 = std::chrono::steady_clock::now(); 39 | 40 | /* Instantiate global timer. */ 41 | double timer = 0.0; 42 | 43 | /* While loop for data collection. */ 44 | rs2::colorizer c; 45 | while (true) 46 | { 47 | /* Local timer. */ 48 | auto time0 = std::chrono::steady_clock::now(); 49 | 50 | /* Wait for new frames. */ 51 | rs2::frameset frameset = pipe.wait_for_frames(); 52 | 53 | /* Align frames. */ 54 | frameset = align_to_color.process(frameset); 55 | 56 | /* Extract separate frames and colorize. */ 57 | auto depth = frameset.get_depth_frame(); 58 | auto color = frameset.get_color_frame(); 59 | /* Not sure it is required, however it stabilizes frame rate! */ 60 | auto colorized_depth = c.colorize(depth); 61 | 62 | /* Wrap data around OpenCV matrices. */ 63 | const int w = depth.as().get_width(); 64 | const int h = depth.as().get_height(); 65 | cv::Mat cv_rgb(cv::Size(w, h), CV_8UC3, (void*) color.get_data(), cv::Mat::AUTO_STEP); 66 | cv::Mat cv_depth(cv::Size(w, h), CV_16U, (void*) depth.get_data(), cv::Mat::AUTO_STEP); 67 | cv_depth.convertTo(cv_depth, CV_32FC1, 0.001); 68 | 69 | /* Wait some time such that the pipeline reaches staedy state behavior. */ 70 | if (std::chrono::duration_cast(std::chrono::steady_clock::now() - t0).count() > 4.0) 71 | { 72 | rgb_frames.push_back(cv_rgb.clone()); 73 | depth_frames.push_back(cv_depth.clone()); 74 | 75 | double delta = std::chrono::duration_cast(std::chrono::steady_clock::now() - time0).count() / 1000.0 / 1000.0; 76 | timer += delta; 77 | stamps.push_back(timer); 78 | 79 | // std::cout << "Elapsed " << std::chrono::duration_cast(std::chrono::steady_clock::now() - time0).count() / 1000.0 << " (ms)" << std::endl; 80 | } 81 | 82 | /* Collect a one minute long sequence. */ 83 | if (std::chrono::duration_cast(std::chrono::steady_clock::now() - t0).count() > 60.0) 84 | break; 85 | } 86 | 87 | /* Save on disk. */ 88 | std::cout << "Saving..." << std::endl; 89 | 90 | /* Output file for timestamps. */ 91 | std::ofstream data; 92 | data.open("./data.txt"); 93 | 94 | for (std::size_t i = 0; i < rgb_frames.size(); i++) 95 | { 96 | std::cout << "frame " << i << std::endl; 97 | 98 | /* Save RGB frame. */ 99 | cv::cvtColor(rgb_frames.at(i), rgb_frames.at(i), cv::COLOR_BGR2RGB); 100 | cv::imwrite("./rgb/" + std::to_string(i) + ".png", rgb_frames.at(i)); 101 | 102 | /* Save depth frame. */ 103 | RobotsIO::Utils::depth_to_file("./depth/" + std::to_string(i) + ".float", depth_frames.at(i)); 104 | 105 | data << stamps.at(i) << " " << stamps.at(i) << " 0.0 0.0 0.0 0.0 0.0 0.0 0.0" << std::endl; 106 | } 107 | data.close(); 108 | 109 | return EXIT_SUCCESS; 110 | } 111 | -------------------------------------------------------------------------------- /tools/third_party/bop_pose_error.py: -------------------------------------------------------------------------------- 1 | # Author: Tomas Hodan (hodantom@cmp.felk.cvut.cz) 2 | # Center for Machine Perception, Czech Technical University in Prague 3 | 4 | # Implementation of the pose error functions described in: 5 | # Hodan et al., "On Evaluation of 6D Object Pose Estimation", ECCVW 2016 6 | 7 | import math 8 | import numpy as np 9 | from scipy import spatial 10 | 11 | 12 | def VOCap(rec, prec): 13 | index = np.where(np.isfinite(rec))[0] 14 | rec = rec[index] 15 | prec = prec[index] 16 | if len(rec) == 0 or len(prec) == 0: 17 | ap = 0 18 | else: 19 | mrec = np.insert(rec, 0, 0) 20 | mrec = np.append(mrec, 0.1) 21 | mpre = np.insert(prec, 0, 0) 22 | mpre = np.append(mpre, prec[-1]) 23 | for i in range(1, len(mpre)): 24 | mpre[i] = max(mpre[i], mpre[i-1]) 25 | i = np.where(mrec[1:] != mrec[:-1])[0] + 1 26 | ap = np.sum(np.multiply(mrec[i] - mrec[i-1], mpre[i])) * 10 27 | return ap 28 | 29 | 30 | def transform_pts_Rt(pts, R, t): 31 | """ 32 | Applies a rigid transformation to 3D points. 33 | 34 | :param pts: nx3 ndarray with 3D points. 35 | :param R: 3x3 rotation matrix. 36 | :param t: 3x1 translation vector. 37 | :return: nx3 ndarray with transformed 3D points. 38 | """ 39 | assert(pts.shape[1] == 3) 40 | pts_t = R.dot(pts.T) + t.reshape((3, 1)) 41 | return pts_t.T 42 | 43 | def reproj(K, R_est, t_est, R_gt, t_gt, pts): 44 | """ 45 | reprojection error. 46 | :param K intrinsic matrix 47 | :param R_est, t_est: Estimated pose (3x3 rot. matrix and 3x1 trans. vector). 48 | :param R_gt, t_gt: GT pose (3x3 rot. matrix and 3x1 trans. vector). 49 | :param model: Object model given by a dictionary where item 'pts' 50 | is nx3 ndarray with 3D model points. 51 | :return: Error of pose_est w.r.t. pose_gt. 52 | """ 53 | pts_est = transform_pts_Rt(pts, R_est, t_est) 54 | pts_gt = transform_pts_Rt(pts, R_gt, t_gt) 55 | 56 | pixels_est = K.dot(pts_est.T) 57 | pixels_est = pixels_est.T 58 | pixels_gt = K.dot(pts_gt.T) 59 | pixels_gt = pixels_gt.T 60 | 61 | n = pts.shape[0] 62 | est = np.zeros((n, 2), dtype=np.float32); 63 | est[:, 0] = np.divide(pixels_est[:, 0], pixels_est[:, 2]) 64 | est[:, 1] = np.divide(pixels_est[:, 1], pixels_est[:, 2]) 65 | 66 | gt = np.zeros((n, 2), dtype=np.float32); 67 | gt[:, 0] = np.divide(pixels_gt[:, 0], pixels_gt[:, 2]) 68 | gt[:, 1] = np.divide(pixels_gt[:, 1], pixels_gt[:, 2]) 69 | 70 | e = np.linalg.norm(est - gt, axis=1).mean() 71 | return e 72 | 73 | def add(R_est, t_est, R_gt, t_gt, pts): 74 | """ 75 | Average Distance of Model Points for objects with no indistinguishable views 76 | - by Hinterstoisser et al. (ACCV 2012). 77 | 78 | :param R_est, t_est: Estimated pose (3x3 rot. matrix and 3x1 trans. vector). 79 | :param R_gt, t_gt: GT pose (3x3 rot. matrix and 3x1 trans. vector). 80 | :param model: Object model given by a dictionary where item 'pts' 81 | is nx3 ndarray with 3D model points. 82 | :return: Error of pose_est w.r.t. pose_gt. 83 | """ 84 | pts_est = transform_pts_Rt(pts, R_est, t_est) 85 | pts_gt = transform_pts_Rt(pts, R_gt, t_gt) 86 | e = np.linalg.norm(pts_est - pts_gt, axis=1).mean() 87 | return e 88 | 89 | def adi(R_est, t_est, R_gt, t_gt, pts): 90 | """ 91 | Average Distance of Model Points for objects with indistinguishable views 92 | - by Hinterstoisser et al. (ACCV 2012). 93 | 94 | :param R_est, t_est: Estimated pose (3x3 rot. matrix and 3x1 trans. vector). 95 | :param R_gt, t_gt: GT pose (3x3 rot. matrix and 3x1 trans. vector). 96 | :param model: Object model given by a dictionary where item 'pts' 97 | is nx3 ndarray with 3D model points. 98 | :return: Error of pose_est w.r.t. pose_gt. 99 | """ 100 | pts_est = transform_pts_Rt(pts, R_est, t_est) 101 | pts_gt = transform_pts_Rt(pts, R_gt, t_gt) 102 | 103 | # Calculate distances to the nearest neighbors from pts_gt to pts_est 104 | nn_index = spatial.cKDTree(pts_est) 105 | nn_dists, _ = nn_index.query(pts_gt, k=1) 106 | 107 | e = nn_dists.mean() 108 | return e 109 | 110 | def re(R_est, R_gt): 111 | """ 112 | Rotational Error. 113 | 114 | :param R_est: Rotational element of the estimated pose (3x1 vector). 115 | :param R_gt: Rotational element of the ground truth pose (3x1 vector). 116 | :return: Error of t_est w.r.t. t_gt. 117 | """ 118 | assert(R_est.shape == R_gt.shape == (3, 3)) 119 | error_cos = 0.5 * (np.trace(R_est.dot(np.linalg.inv(R_gt))) - 1.0) 120 | error_cos = min(1.0, max(-1.0, error_cos)) # Avoid invalid values due to numerical errors 121 | error = math.acos(error_cos) 122 | error = 180.0 * error / np.pi # [rad] -> [deg] 123 | return error 124 | 125 | def te(t_est, t_gt): 126 | """ 127 | Translational Error. 128 | 129 | :param t_est: Translation element of the estimated pose (3x1 vector). 130 | :param t_gt: Translation element of the ground truth pose (3x1 vector). 131 | :return: Error of t_est w.r.t. t_gt. 132 | """ 133 | assert(t_est.size == t_gt.size == 3) 134 | error = np.linalg.norm(t_gt - t_est) 135 | return error 136 | -------------------------------------------------------------------------------- /tools/third_party/download_google_drive_file.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | checkurl() { 3 | FILEID=$1 4 | if echo $FILEID | sed -n 's#.*\https\:\/\/drive\.google\.com/file/d/\([^.]*\)\/view.*#\1#;p' | wc -l; then 5 | echo "url valida" 6 | return 1 7 | else 8 | echo "Please input 'https://drive.google.com/file/d/xxxxxxx/view'" 9 | return 0 10 | fi 11 | } 12 | check_quota(){ 13 | FILENAME=$1 14 | if grep -q "Quota exceeded" "$FILENAME"; then 15 | rm $FILENAME && \ 16 | echo "Google Drive Limited (Quota Exceeded)" && \ 17 | echo "file $FILENAME can NOT be downloaded" 18 | return 0 19 | else 20 | echo "file $FILENAME has been downloaded" 21 | return 1 22 | fi 23 | 24 | } 25 | if [ $# -eq 1 ];then 26 | FILEID=$1 27 | else 28 | echo "insert the google drive full url to download"; 29 | read FILEID; 30 | fi 31 | FILEID="$(echo $FILEID | sed -n 's#.*\https\:\/\/drive\.google\.com/file/d/\([^.]*\)\/view.*#\1#;p')"; 32 | FILENAME="$(wget -q --show-progress -O - "https://drive.google.com/file/d/$FILEID/view" | sed -n -e 's!.*\(.*\)\ \-\ Google\ Drive.*!\1!p')"; 33 | CONFIRM_CODE=$(wget -q --show-progress --save-cookies /tmp/cookies.txt --keep-session-cookies --no-check-certificate "https://docs.google.com/uc?export=download&id=$FILEID" -O- | sed -rn 's/.*confirm=([0-9A-Za-z_]+).*/\1\n/p') 34 | wget -q --show-progress --load-cookies /tmp/cookies.txt "https://docs.google.com/uc?export=download&confirm=$CONFIRM_CODE&id=$FILEID" -c -O "$FILENAME" && rm -rf /tmp/cookies.txt; 35 | if [ -f "$FILENAME" ]; then 36 | echo "file $FILENAME has been downloaded" 37 | else 38 | echo "file $FILENAME cannot be downloaded,any error appear" 39 | fi 40 | -------------------------------------------------------------------------------- /tools/video/compress: -------------------------------------------------------------------------------- 1 | ffmpeg -y -hwaccel cuda -hwaccel_output_format cuda -i Video.mp4 -c:v h264_nvenc -preset slow -b:v 1400k -pass 1 -f mp4 /dev/null &&\ 2 | ffmpeg -y -hwaccel cuda -hwaccel_output_format cuda -i Video.mp4 -c:v h264_nvenc -preset slow -b:v 1400k -pass 2 ROFT_media.mp4 3 | --------------------------------------------------------------------------------