├── .gitattributes ├── .gitignore ├── CONTRIBUTING.md ├── LICENSE ├── README.md ├── gxf_isaac_bi3d ├── CMakeLists.txt ├── gxf │ ├── extensions │ │ └── bi3d │ │ │ ├── bi3d.cpp │ │ │ ├── components │ │ │ ├── bi3d_depthview.cpp │ │ │ ├── bi3d_depthview.hpp │ │ │ ├── bi3d_inference.cpp │ │ │ ├── bi3d_inference.hpp │ │ │ ├── bi3d_postprocessor.cpp │ │ │ ├── bi3d_postprocessor.cu │ │ │ ├── bi3d_postprocessor.cu.hpp │ │ │ ├── bi3d_postprocessor.hpp │ │ │ └── bi3d_postprocessor_utils.hpp │ │ │ └── inference │ │ │ ├── Bi3D.cpp │ │ │ ├── Bi3D.h │ │ │ ├── Bi3DPostProcessor.cpp │ │ │ ├── Bi3DPreProcessor.cpp │ │ │ ├── Bi3D_detail.cpp │ │ │ ├── Bi3D_detail.hpp │ │ │ └── Model.h │ └── gems │ │ ├── dnn_inferencer │ │ └── inferencer │ │ │ ├── Errors.cpp │ │ │ ├── Errors.h │ │ │ ├── IInferenceBackend.h │ │ │ ├── Inferencer.cpp │ │ │ ├── Inferencer.h │ │ │ ├── TensorRTInferencer.cpp │ │ │ ├── TensorRTInferencer.h │ │ │ ├── TensorRTUtils.cpp │ │ │ ├── TensorRTUtils.h │ │ │ ├── TritonGrpcInferencer.cpp │ │ │ ├── TritonGrpcInferencer.h │ │ │ ├── TritonUtils.cpp │ │ │ └── TritonUtils.h │ │ └── hash │ │ ├── hash_file.cpp │ │ ├── hash_file.hpp │ │ ├── sha256.cpp │ │ └── sha256.hpp └── package.xml ├── isaac_ros_bi3d ├── CMakeLists.txt ├── config │ ├── bi3d_node.yaml │ ├── isaac_ros_bi3d_isaac_sim.rviz │ ├── isaac_ros_bi3d_zed.rviz │ ├── realsense.yaml │ └── zed.yaml ├── include │ └── isaac_ros_bi3d │ │ └── bi3d_node.hpp ├── isaac_ros_bi3d │ └── __init__.py ├── launch │ ├── isaac_ros_bi3d.launch.py │ ├── isaac_ros_bi3d_core.launch.py │ ├── isaac_ros_bi3d_hawk.launch.py │ ├── isaac_ros_bi3d_isaac_sim.launch.py │ └── isaac_ros_bi3d_realsense.launch.py ├── package.xml ├── scripts │ └── isaac_ros_bi3d_visualizer.py ├── src │ └── bi3d_node.cpp └── test │ ├── bi3d_node_test.cpp │ ├── camera_info.json │ ├── dummy_featnet_model.onnx │ ├── dummy_segnet_model.onnx │ └── isaac_ros_bi3d_test.py └── isaac_ros_nitros_bi3d_inference_param_array_type ├── CMakeLists.txt ├── include └── isaac_ros_nitros_bi3d_inference_param_array_type │ └── nitros_bi3d_inference_param_array.hpp ├── package.xml ├── src └── nitros_bi3d_inference_param_array.cpp └── test ├── config └── test_forward_node.yaml ├── isaac_ros_nitros_bi3d_inference_param_array_type_test_pol.py └── src └── nitros_bi3d_inference_param_array_forward_node.cpp /.gitattributes: -------------------------------------------------------------------------------- 1 | # Ignore Python files in linguist 2 | *.py linguist-detectable=false 3 | 4 | # Images 5 | *.gif filter=lfs diff=lfs merge=lfs -text 6 | *.jpg filter=lfs diff=lfs merge=lfs -text 7 | *.png filter=lfs diff=lfs merge=lfs -text 8 | *.psd filter=lfs diff=lfs merge=lfs -text 9 | 10 | # Archives 11 | *.gz filter=lfs diff=lfs merge=lfs -text 12 | *.tar filter=lfs diff=lfs merge=lfs -text 13 | *.zip filter=lfs diff=lfs merge=lfs -text 14 | 15 | # Documents 16 | *.pdf filter=lfs diff=lfs merge=lfs -text 17 | 18 | # Shared libraries 19 | *.so filter=lfs diff=lfs merge=lfs -text 20 | *.so.* filter=lfs diff=lfs merge=lfs -text 21 | 22 | # ROS Bags 23 | **/resources/**/*.db3 filter=lfs diff=lfs merge=lfs -text 24 | **/resources/**/*.yaml filter=lfs diff=lfs merge=lfs -text 25 | 26 | # DNN Model files 27 | *.onnx filter=lfs diff=lfs merge=lfs -text 28 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore all pycache files 2 | **/__pycache__/** 3 | 4 | # Ignore TensorRT plan files 5 | *.plan 6 | *.engine 7 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Isaac ROS Contribution Rules 2 | 3 | Any contribution that you make to this repository will 4 | be under the Apache 2 License, as dictated by that 5 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 6 | 7 | > **5. Submission of Contributions.** Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. 8 | 9 | Contributors must sign-off each commit by adding a `Signed-off-by: ...` 10 | line to commit messages to certify that they have the right to submit 11 | the code they are contributing to the project according to the 12 | [Developer Certificate of Origin (DCO)](https://developercertificate.org/). 13 | 14 | [//]: # (202201002) 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Isaac ROS Depth Segmentation 2 | 3 | NVIDIA-accelerated packages for depth segmentation. 4 | 5 |
image
6 | 7 | --- 8 | 9 | ## Webinar Available 10 | 11 | Learn how to use this package by watching our on-demand webinar: [Using ML Models in ROS 2 to Robustly Estimate Distance to Obstacles](https://gateway.on24.com/wcc/experience/elitenvidiabrill/1407606/3998202/isaac-ros-webinar-series) 12 | 13 | --- 14 | 15 | ### Overview 16 | 17 | Isaac ROS Depth Segmentation provides NVIDIA NVIDIA-accelerated packages for 18 | depth segmentation. The `isaac_ros_bi3d` package uses the 19 | optimized [Bi3D DNN 20 | model](https://catalog.ngc.nvidia.com/orgs/nvidia/teams/isaac/models/bi3d_proximity_segmentation) 21 | to perform stereo-depth estimation via binary classification, which is 22 | used for depth segmentation. Depth segmentation can be used to 23 | determine whether an obstacle is within a proximity field and to avoid 24 | collisions with obstacles during navigation. 25 | 26 |
image
27 | 28 | [Bi3D](https://arxiv.org/abs/2005.07274) is used in a graph of nodes 29 | to provide depth segmentation from a time-synchronized input left 30 | and right stereo image pair. Images to Bi3D need to be rectified and 31 | resized to the appropriate input resolution. The aspect ratio of the 32 | image needs to be maintained; hence, a crop and resize may be required 33 | to maintain the input aspect ratio. The graph for DNN encode, to DNN 34 | inference, to DNN decode is part of the Bi3D node. Inference is 35 | performed using TensorRT, as the Bi3D DNN model is designed to use 36 | optimizations supported by TensorRT. 37 | 38 | Compared to other stereo disparity functions, depth segmentation 39 | provides a prediction of whether an obstacle is within a proximity 40 | field, as opposed to continuous depth, while simultaneously predicting 41 | freespace from the ground plane, which other functions typically do not 42 | provide. Also unlike other stereo disparity functions in Isaac ROS, 43 | depth segmentation runs on NVIDIA DLA (deep learning accelerator), 44 | which is separate and independent from the GPU. For more information on 45 | disparity, refer to [this 46 | page](https://en.wikipedia.org/wiki/Binocular_disparity). 47 | 48 | > [!Note] 49 | > This DNN is optimized for and evaluated with RGB global shutter camera images, 50 | > and accuracy may vary on monochrome images. 51 | 52 | ### Isaac ROS NITROS Acceleration 53 | 54 | This package is powered by [NVIDIA Isaac Transport for ROS (NITROS)](https://developer.nvidia.com/blog/improve-perception-performance-for-ros-2-applications-with-nvidia-isaac-transport-for-ros/), which leverages type adaptation and negotiation to optimize message formats and dramatically accelerate communication between participating nodes. 55 | 56 | ### Performance 57 | 58 | | Sample Graph

| Input Size

| AGX Orin

| Orin NX

| x86_64 w/ RTX 4090

| 59 | |-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------| 60 | | [Depth Segmentation Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_bi3d_benchmark/scripts/isaac_ros_bi3d_node.py)



| 576p



| [41.4 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_node-agx_orin.json)


80 ms @ 30Hz

| [28.1 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_node-orin_nx.json)


98 ms @ 30Hz

| [105 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_bi3d_node-x86-4090.json)


25 ms @ 30Hz

| 61 | 62 | --- 63 | 64 | ### Documentation 65 | 66 | Please visit the [Isaac ROS Documentation](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_depth_segmentation/index.html) to learn how to use this repository. 67 | 68 | --- 69 | 70 | ### Packages 71 | 72 | * [`isaac_ros_bi3d`](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_depth_segmentation/isaac_ros_bi3d/index.html) 73 | * [Quickstart](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_depth_segmentation/isaac_ros_bi3d/index.html#quickstart) 74 | * [Try More Examples](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_depth_segmentation/isaac_ros_bi3d/index.html#try-more-examples) 75 | * [Model Preparation](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_depth_segmentation/isaac_ros_bi3d/index.html#model-preparation) 76 | * [Troubleshooting](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_depth_segmentation/isaac_ros_bi3d/index.html#troubleshooting) 77 | * [API](https://nvidia-isaac-ros.github.io/repositories_and_packages/isaac_ros_depth_segmentation/isaac_ros_bi3d/index.html#api) 78 | 79 | ### Latest 80 | 81 | Update 2024-12-10: Update to be compatible with JetPack 6.1 82 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | cmake_minimum_required(VERSION 3.22.1) 19 | project(gxf_isaac_bi3d LANGUAGES C CXX) 20 | 21 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 22 | add_compile_options(-fPIC -w) 23 | endif() 24 | 25 | find_package(ament_cmake_auto REQUIRED) 26 | ament_auto_find_build_dependencies() 27 | 28 | enable_language(CUDA) 29 | 30 | # Dependencies 31 | find_package(CUDAToolkit) 32 | find_package(yaml-cpp) 33 | find_package(TENSORRT) 34 | find_package(Eigen3) 35 | 36 | # Bi3D extension 37 | ament_auto_add_library(${PROJECT_NAME} SHARED 38 | gxf/extensions/bi3d/bi3d.cpp 39 | gxf/extensions/bi3d/components/bi3d_postprocessor.cpp 40 | gxf/extensions/bi3d/components/bi3d_inference.cpp 41 | gxf/extensions/bi3d/components/bi3d_postprocessor.cu 42 | gxf/extensions/bi3d/components/bi3d_depthview.cpp 43 | 44 | gxf/extensions/bi3d/inference/Bi3D_detail.cpp 45 | gxf/extensions/bi3d/inference/Bi3D_detail.hpp 46 | gxf/extensions/bi3d/inference/Bi3D.cpp 47 | gxf/extensions/bi3d/inference/Bi3D.h 48 | gxf/extensions/bi3d/inference/Bi3DPreProcessor.cpp 49 | gxf/extensions/bi3d/inference/Bi3DPostProcessor.cpp 50 | 51 | # Inferencer (Bi3D only) 52 | gxf/gems/dnn_inferencer/inferencer/TensorRTInferencer.cpp 53 | gxf/gems/dnn_inferencer/inferencer/TensorRTUtils.cpp 54 | gxf/gems/dnn_inferencer/inferencer/Inferencer.cpp 55 | gxf/gems/dnn_inferencer/inferencer/Errors.cpp 56 | ) 57 | 58 | target_include_directories(${PROJECT_NAME} PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/gxf") 59 | 60 | target_link_libraries(${PROJECT_NAME} 61 | Eigen3::Eigen 62 | CUDA::cudart 63 | CUDA::nppc 64 | CUDA::nppial 65 | CUDA::nppicc 66 | CUDA::nppidei 67 | CUDA::nppif 68 | CUDA::nppig 69 | CUDA::nppisu 70 | CUDA::nppitc 71 | TENSORRT::nvinfer 72 | yaml-cpp 73 | ) 74 | 75 | set_target_properties(${PROJECT_NAME} PROPERTIES 76 | BUILD_WITH_INSTALL_RPATH TRUE 77 | BUILD_RPATH_USE_ORIGIN TRUE 78 | INSTALL_RPATH_USE_LINK_PATH TRUE 79 | ) 80 | 81 | # Install the binary file 82 | install(TARGETS ${PROJECT_NAME} DESTINATION share/${PROJECT_NAME}/gxf/lib) 83 | 84 | 85 | # Embed versioning information into installed files 86 | ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common) 87 | include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake") 88 | generate_version_info(${PROJECT_NAME}) 89 | 90 | ament_auto_package(INSTALL_TO_SHARE) 91 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/bi3d.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #include "extensions/bi3d/components/bi3d_depthview.hpp" 18 | #include "extensions/bi3d/components/bi3d_inference.hpp" 19 | #include "extensions/bi3d/components/bi3d_postprocessor.hpp" 20 | #include "gxf/std/extension_factory_helper.hpp" 21 | 22 | GXF_EXT_FACTORY_BEGIN() 23 | 24 | GXF_EXT_FACTORY_SET_INFO(0xce7c6985267a4ec7, 0xa073030e16e49f29, "Bi3D", 25 | "Extension containing Bi3D related components", 26 | "Isaac SDK", "2.0.0", "LICENSE"); 27 | 28 | GXF_EXT_FACTORY_ADD(0x9cfc86f71c7c4102, 0xade57d2d4faf6453, 29 | nvidia::isaac::bi3d::DepthView, nvidia::gxf::Codelet, 30 | "Bi3d depth map generator"); 31 | 32 | GXF_EXT_FACTORY_ADD(0x2eb361c045894aec, 0x831550ff5f177d87, 33 | nvidia::isaac::bi3d::Bi3DPostprocessor, nvidia::gxf::Codelet, 34 | "Bi3D post processor"); 35 | 36 | GXF_EXT_FACTORY_ADD(0xdcba0cf83a5340d2, 0x8404788350ad2324, 37 | nvidia::isaac::Bi3DInference, nvidia::gxf::Codelet, 38 | "Bi3D Inference"); 39 | 40 | GXF_EXT_FACTORY_END() 41 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/components/bi3d_depthview.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #include "extensions/bi3d/components/bi3d_depthview.hpp" 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "gems/core/image/image.hpp" 24 | #include "gems/core/tensor/tensor.hpp" 25 | #include "gems/image/io.hpp" 26 | #include "gems/image/utils.hpp" 27 | #include "gxf/multimedia/video.hpp" 28 | 29 | namespace nvidia { 30 | namespace isaac { 31 | namespace bi3d { 32 | 33 | // Indicates Depth View Color Scheme. 34 | enum DepthViewColorScheme { 35 | DepthViewColorScheme_ErrorColor = 1, 36 | DepthViewColorScheme_RedGreenBlack, 37 | DepthViewColorScheme_DistanceMap, 38 | DepthViewColorScheme_StarryNight, 39 | DepthViewColorScheme_BlackAndWhite 40 | }; 41 | 42 | gxf_result_t DepthView::registerInterface(gxf::Registrar* registrar) { 43 | gxf::Expected result; 44 | result &= registrar->parameter( 45 | data_receiver_, "data_receiver", 46 | "Receiver to get the data", ""); 47 | result &= registrar->parameter( 48 | output_tensor_, "output_tensor", 49 | "The name of the tensor to be transmitted", ""); 50 | result &= registrar->parameter( 51 | output_as_tensor_, "output_as_tensor", 52 | "The flag to indicate that output is a tensor or video buffer", "", true); 53 | result &= registrar->parameter(data_transmitter_, "data_transmitter", 54 | "Transmitter to send the data", ""); 55 | result &= registrar->parameter( 56 | allocator_, "allocator", 57 | "Memory pool for allocating output data", ""); 58 | result &= registrar->parameter( 59 | color_scheme_, "color_scheme", 60 | "The color scheme for Bi3d depth", 61 | "Following are supported color schemes: " 62 | "1 - ErrorColor" 63 | "2 - RedGreenBlack" 64 | "3 - DistanceMap" 65 | "4 - StarryNight" 66 | "5 - BlackAndWhite", 67 | 5); 68 | result &= registrar->parameter( 69 | min_disparity_, "min_disparity", 70 | "Minimum disparity used for colorization", "0.0f"); 71 | result &= registrar->parameter( 72 | max_disparity_, "max_disparity", 73 | "Maximum disparity used for colorization", "255.0f"); 74 | return gxf::ToResultCode(result); 75 | } 76 | 77 | gxf_result_t DepthView::start() { 78 | switch (color_scheme_) { 79 | case DepthViewColorScheme_ErrorColor: 80 | gradient_ = ::nvidia::isaac::ErrorColorGradient(); 81 | break; 82 | 83 | case DepthViewColorScheme_RedGreenBlack: 84 | gradient_ = ::nvidia::isaac::ColorGradient({ 85 | ::nvidia::isaac::Pixel3ub{0, 0, 0}, ::nvidia::isaac::Pixel3ub{118, 185, 0}, 86 | ::nvidia::isaac::Pixel3ub{221, 86, 47}, ::nvidia::isaac::Pixel3ub{255, 0, 0}}); 87 | break; 88 | 89 | case DepthViewColorScheme_DistanceMap: 90 | gradient_ = ::nvidia::isaac::DistanceColorGradient(); 91 | break; 92 | 93 | case DepthViewColorScheme_StarryNight: 94 | gradient_ = ::nvidia::isaac::StarryNightColorGradient(); 95 | break; 96 | 97 | case DepthViewColorScheme_BlackAndWhite: 98 | gradient_ = ::nvidia::isaac::BlackWhiteColorGradient(); 99 | break; 100 | 101 | default: 102 | gradient_ = ::nvidia::isaac::BlackWhiteColorGradient(); 103 | break; 104 | } 105 | return GXF_SUCCESS; 106 | } 107 | 108 | gxf_result_t DepthView::tick() { 109 | auto entity = data_receiver_->receive(); 110 | if (!entity) { 111 | return gxf::ToResultCode(entity); 112 | } 113 | 114 | // Get input 115 | auto input_frame = entity.value().get(); 116 | if (!input_frame) { 117 | GXF_LOG_ERROR("Failed to get input frame from message"); 118 | return gxf::ToResultCode(input_frame); 119 | } 120 | 121 | if (input_frame.value()->video_frame_info().color_format != 122 | gxf::VideoFormat::GXF_VIDEO_FORMAT_D32F) { 123 | GXF_LOG_ERROR("Only supports D32F images"); 124 | return GXF_FAILURE; 125 | } 126 | 127 | if (input_frame.value()->storage_type() != gxf::MemoryStorageType::kHost) { 128 | GXF_LOG_ERROR("Only supports video buffer on Host."); 129 | return GXF_FAILURE; 130 | } 131 | 132 | int inputWidth = input_frame.value()->video_frame_info().width; 133 | int inputHeight = input_frame.value()->video_frame_info().height; 134 | ::nvidia::isaac::ImageView1f input_depthview = ::nvidia::isaac::CreateImageView( 135 | reinterpret_cast(input_frame.value()->pointer()), 136 | inputHeight, inputWidth); 137 | 138 | // Create output message 139 | auto output_message = gxf::Entity::New(context()); 140 | if (!output_message) { 141 | return gxf::ToResultCode(output_message); 142 | } 143 | 144 | auto output_name = output_tensor_.try_get() ? output_tensor_.try_get()->c_str() : nullptr; 145 | ::nvidia::isaac::ImageView3ub output_image; 146 | 147 | if (output_as_tensor_) { 148 | // Create tensor 149 | auto output_tensor = output_message.value().add(output_name); 150 | if (!output_tensor) { 151 | return gxf::ToResultCode(output_tensor); 152 | } 153 | 154 | const gxf::Shape shape({inputHeight, inputWidth, 3}); 155 | auto result = 156 | output_tensor.value()->reshapeCustom(shape, gxf::PrimitiveType::kUnsigned8, 1, 157 | gxf::Unexpected{GXF_UNINITIALIZED_VALUE}, 158 | gxf::MemoryStorageType::kHost, allocator_); 159 | if (!result) { 160 | GXF_LOG_ERROR("reshape tensor failed."); 161 | return GXF_FAILURE; 162 | } 163 | 164 | output_image = ::nvidia::isaac::CreateImageView( 165 | output_tensor.value()->data().value(), 166 | inputHeight, inputWidth); 167 | } else { 168 | // Create video buffer 169 | auto output_frame = output_message.value().add(output_name); 170 | if (!output_frame) { 171 | return gxf::ToResultCode(output_frame); 172 | } 173 | 174 | std::array planes{ 175 | gxf::ColorPlane("RGB", 3, inputWidth * 3)}; 176 | gxf::VideoFormatSize video_format_size; 177 | const uint64_t size = video_format_size.size(inputWidth, inputHeight, planes); 178 | const gxf::VideoBufferInfo buffer_info{static_cast(inputWidth), 179 | static_cast(inputHeight), 180 | gxf::VideoFormat::GXF_VIDEO_FORMAT_RGB, 181 | {planes.begin(), planes.end()}, 182 | gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR}; 183 | output_frame.value()->resizeCustom( 184 | buffer_info, size, 185 | gxf::MemoryStorageType::kHost, allocator_); 186 | output_image = ::nvidia::isaac::CreateImageView( 187 | output_frame.value()->pointer(), 188 | inputHeight, inputWidth); 189 | } 190 | 191 | ::nvidia::isaac::Colorize(input_depthview, gradient_, min_disparity_, 192 | max_disparity_, output_image); 193 | 194 | // Send the data 195 | return gxf::ToResultCode(data_transmitter_->publish(output_message.value())); 196 | } 197 | 198 | } // namespace bi3d 199 | } // namespace isaac 200 | } // namespace nvidia 201 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/components/bi3d_depthview.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include "gems/image/color.hpp" 23 | #include "gxf/std/codelet.hpp" 24 | #include "gxf/std/receiver.hpp" 25 | #include "gxf/std/tensor.hpp" 26 | #include "gxf/std/transmitter.hpp" 27 | 28 | namespace nvidia { 29 | namespace isaac { 30 | namespace bi3d { 31 | 32 | class DepthView : public gxf::Codelet { 33 | public: 34 | gxf_result_t registerInterface(gxf::Registrar* registrar) override; 35 | gxf_result_t start() override; 36 | gxf_result_t tick() override; 37 | gxf_result_t stop() override {return GXF_SUCCESS;}; 38 | 39 | private: 40 | // Data receiver to get data 41 | gxf::Parameter> data_receiver_; 42 | // The name of the output tensor 43 | gxf::Parameter output_tensor_; 44 | // The flag that indicates if output as a tensor 45 | gxf::Parameter output_as_tensor_; 46 | // Data transmitter to send the data 47 | gxf::Parameter> data_transmitter_; 48 | // Allocator 49 | gxf::Parameter> allocator_; 50 | // Color scheme 51 | gxf::Parameter color_scheme_; 52 | // Minimum disparity value 53 | gxf::Parameter min_disparity_; 54 | // maximum disparity value 55 | gxf::Parameter max_disparity_; 56 | 57 | // Color gradient used to color different depths 58 | ::nvidia::isaac::ColorGradient gradient_; 59 | }; 60 | 61 | } // namespace bi3d 62 | } // namespace isaac 63 | } // namespace nvidia 64 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/components/bi3d_inference.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "extensions/bi3d/inference/Bi3D.h" 27 | #include "gxf/cuda/cuda_stream_pool.hpp" 28 | #include "gxf/std/allocator.hpp" 29 | #include "gxf/std/codelet.hpp" 30 | #include "gxf/std/receiver.hpp" 31 | #include "gxf/std/transmitter.hpp" 32 | 33 | namespace nvidia { 34 | namespace isaac { 35 | // CV-Core Bi3D GXF Codelet 36 | class Bi3DInference : public gxf::Codelet { 37 | public: 38 | Bi3DInference() = default; 39 | ~Bi3DInference() = default; 40 | 41 | gxf_result_t registerInterface(gxf::Registrar* registrar) override; 42 | gxf_result_t initialize() override { 43 | return GXF_SUCCESS; 44 | } 45 | gxf_result_t deinitialize() override { 46 | return GXF_SUCCESS; 47 | } 48 | 49 | gxf_result_t start() override; 50 | gxf_result_t tick() override; 51 | gxf_result_t stop() override; 52 | 53 | private: 54 | // gxf stream handling 55 | gxf::Handle cuda_stream_ = nullptr; 56 | 57 | // cvcore image pre-processing params for Bi3D 58 | cvcore::tensor_ops::ImagePreProcessingParams preProcessorParams; 59 | // cvcore model input params for Bi3D 60 | bi3d::ModelInputParams modelInputParams; 61 | // cvcore inference params for Bi3D 62 | cvcore::inferencer::TensorRTInferenceParams featureInferenceParams; 63 | cvcore::inferencer::TensorRTInferenceParams featureHRInferenceParams; 64 | cvcore::inferencer::TensorRTInferenceParams refinementInferenceParams; 65 | cvcore::inferencer::TensorRTInferenceParams segmentationInferenceParams; 66 | // extra params for Bi3D 67 | bi3d::Bi3D::Bi3DParams extraParams; 68 | // cvcore Bi3D object 69 | std::unique_ptr objBi3D; 70 | cvcore::tensor_ops::Array disparityValues; 71 | 72 | // The name of the input left image tensor 73 | gxf::Parameter left_image_name_; 74 | // The name of the input right image tensor 75 | gxf::Parameter right_image_name_; 76 | // The name of the output tensor 77 | gxf::Parameter output_name_; 78 | // Cuda Stream Pool 79 | gxf::Parameter> stream_pool_; 80 | // Data allocator to create a tensor 81 | gxf::Parameter> pool_; 82 | // Data allocator to forward the disparity array 83 | gxf::Parameter> forward_pool_; 84 | // Data receiver to get left image data 85 | gxf::Parameter> left_image_receiver_; 86 | // Data receiver to get right image data 87 | gxf::Parameter> right_image_receiver_; 88 | // Optional receiver for dynamic disparity values input 89 | gxf::Parameter> disparity_receiver_; 90 | // Data transmitter to send the data 91 | gxf::Parameter> output_transmitter_; 92 | 93 | // Pre-processing params for Bi3D 94 | gxf::Parameter image_type_; 95 | gxf::Parameter> pixel_mean_; 96 | gxf::Parameter> normalization_; 97 | gxf::Parameter> standard_deviation_; 98 | 99 | // Model input params for Bi3D 100 | gxf::Parameter max_batch_size_; 101 | gxf::Parameter input_layer_width_; 102 | gxf::Parameter input_layer_height_; 103 | gxf::Parameter model_input_type_; 104 | 105 | // Inference params for Bi3D 106 | gxf::Parameter featnet_engine_file_path_; 107 | gxf::Parameter> featnet_input_layers_name_; 108 | gxf::Parameter> featnet_output_layers_name_; 109 | 110 | gxf::Parameter featnet_hr_engine_file_path_; 111 | gxf::Parameter> featnet_hr_input_layers_name_; 112 | gxf::Parameter> featnet_hr_output_layers_name_; 113 | 114 | gxf::Parameter refinenet_engine_file_path_; 115 | gxf::Parameter> refinenet_input_layers_name_; 116 | gxf::Parameter> refinenet_output_layers_name_; 117 | 118 | gxf::Parameter segnet_engine_file_path_; 119 | gxf::Parameter> segnet_input_layers_name_; 120 | gxf::Parameter> segnet_output_layers_name_; 121 | 122 | // Extra params for Bi3D 123 | gxf::Parameter engine_type_; 124 | gxf::Parameter apply_sigmoid_; 125 | gxf::Parameter apply_thresholding_; 126 | gxf::Parameter threshold_value_low_; 127 | gxf::Parameter threshold_value_high_; 128 | gxf::Parameter threshold_; 129 | gxf::Parameter apply_edge_refinement_; 130 | gxf::Parameter max_disparity_levels_; 131 | gxf::Parameter> disparity_values_; 132 | 133 | // Decide which timestamp to pass down 134 | gxf::Parameter timestamp_policy_; 135 | }; 136 | 137 | } // namespace isaac 138 | } // namespace nvidia 139 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/components/bi3d_postprocessor.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #include "extensions/bi3d/components/bi3d_postprocessor.hpp" 18 | 19 | #include 20 | 21 | #include "extensions/bi3d/components/bi3d_postprocessor.cu.hpp" 22 | #include "gems/video_buffer/allocator.hpp" 23 | #include "gxf/core/parameter_parser_std.hpp" 24 | #include "gxf/std/timestamp.hpp" 25 | 26 | namespace nvidia { 27 | namespace isaac { 28 | namespace bi3d { 29 | 30 | gxf_result_t Bi3DPostprocessor::registerInterface(gxf::Registrar* registrar) noexcept { 31 | gxf::Expected result; 32 | result &= registrar->parameter( 33 | bi3d_receiver_, "bi3d_receiver", "Bi3D receiver", 34 | "Tensor from Bi3D"); 35 | result &= registrar->parameter( 36 | output_transmitter_, "output_transmitter", "Output transmitter", 37 | "The collapsed output of Bi3D"); 38 | result &= registrar->parameter( 39 | pool_, "pool", "Pool", 40 | "Allocator instance for output videobuffer"); 41 | result &= registrar->parameter( 42 | disparity_values_, "disparity_values", "Disparity values" 43 | "Disparities values used for Bi3D inference"); 44 | return gxf::ToResultCode(result); 45 | } 46 | 47 | gxf_result_t Bi3DPostprocessor::tick() noexcept { 48 | // Receive bi3d output 49 | auto bi3d_message = bi3d_receiver_->receive(); 50 | if (!bi3d_message) { 51 | GXF_LOG_ERROR("Failed to get message"); 52 | return gxf::ToResultCode(bi3d_message); 53 | } 54 | 55 | // Get Bi3D disparity tensor 56 | auto bi3d_disparity_tensor = bi3d_message->get("disparity"); 57 | if (!bi3d_disparity_tensor) { 58 | GXF_LOG_ERROR("Failed to get disparity image tensor"); 59 | return gxf::ToResultCode(bi3d_disparity_tensor); 60 | } 61 | 62 | if (!bi3d_disparity_tensor.value()->data()) { 63 | GXF_LOG_ERROR("Failed to get pointer to bi3d disparity data"); 64 | return gxf::ToResultCode(bi3d_disparity_tensor.value()->data()); 65 | } 66 | const float* bi3d_disparity_data_ptr = bi3d_disparity_tensor.value()->data().value(); 67 | 68 | // Get dimensions of tensor 69 | gxf::Shape dims = bi3d_disparity_tensor.value()->shape(); 70 | const int image_height = dims.dimension(1); 71 | const int image_width = dims.dimension(2); 72 | const int image_size = dims.dimension(1) * dims.dimension(2); 73 | 74 | // Create output message 75 | auto out_message = gxf::Entity::New(context()); 76 | if (!out_message) { 77 | GXF_LOG_ERROR("Failed to allocate message"); 78 | return gxf::ToResultCode(out_message); 79 | } 80 | 81 | // Create video buffer 82 | auto out_video_buffer = out_message->add(); 83 | if (!out_video_buffer) { 84 | GXF_LOG_ERROR("Failed to allocate output video buffer"); 85 | return gxf::ToResultCode(out_video_buffer); 86 | } 87 | 88 | // Allocate video buffer on device (unpadded d32f) 89 | auto maybe_allocation = AllocateUnpaddedVideoBuffer( 90 | out_video_buffer.value(), image_width, image_height, gxf::MemoryStorageType::kDevice, 91 | pool_.get()); 92 | if (!maybe_allocation) { 93 | GXF_LOG_ERROR("Failed to allocate output video buffer's memory."); 94 | return gxf::ToResultCode(maybe_allocation); 95 | } 96 | 97 | cudaMemset(out_video_buffer.value()->pointer(), 0, 98 | image_size * bi3d_disparity_tensor.value()->bytes_per_element()); 99 | 100 | for (uint32_t i = 0; i < disparity_values_.get().size(); i++) { 101 | cuda_postprocess(bi3d_disparity_data_ptr + (i * image_size), 102 | reinterpret_cast(out_video_buffer.value()->pointer()), 103 | disparity_values_.get().at(i), image_height, image_width); 104 | } 105 | 106 | // Add timestamp to message if it is available from input message 107 | std::string timestamp_name{"timestamp"}; 108 | auto maybe_bi3d_timestamp = bi3d_message->get(); 109 | if (maybe_bi3d_timestamp) { 110 | auto out_timestamp = out_message.value().add(timestamp_name.c_str()); 111 | if (!out_timestamp) { 112 | return GXF_FAILURE; 113 | } 114 | *out_timestamp.value() = *maybe_bi3d_timestamp.value(); 115 | } 116 | 117 | // Publish message 118 | auto result = output_transmitter_->publish(out_message.value()); 119 | if (!result) { 120 | return gxf::ToResultCode(result); 121 | } 122 | 123 | return GXF_SUCCESS; 124 | } 125 | 126 | } // namespace bi3d 127 | } // namespace isaac 128 | } // namespace nvidia 129 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/components/bi3d_postprocessor.cu: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2022, NVIDIA CORPORATION. All rights reserved. 3 | 4 | NVIDIA CORPORATION and its licensors retain all intellectual property 5 | and proprietary rights in and to this software, related documentation 6 | and any modifications thereto. Any use, reproduction, disclosure or 7 | distribution of this software and related documentation without an express 8 | license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | #include "extensions/bi3d/components/bi3d_postprocessor.cu.hpp" 11 | 12 | namespace nvidia { 13 | namespace isaac { 14 | namespace bi3d { 15 | 16 | __global__ void postprocessing_kernel(const float* input, float* output, int disparity, 17 | int imageHeight, int imageWidth) { 18 | const uint32_t x = blockIdx.x * blockDim.x + threadIdx.x; 19 | const uint32_t y = blockIdx.y * blockDim.y + threadIdx.y; 20 | const uint32_t index = y * imageWidth + x; 21 | if (x < imageWidth && y < imageHeight) { 22 | if (input[index] == 1.0) { 23 | output[index] = input[index] * disparity; 24 | } 25 | } 26 | } 27 | 28 | uint16_t ceil_div(uint16_t numerator, uint16_t denominator) { 29 | uint32_t accumulator = numerator + denominator - 1; 30 | return accumulator / denominator; 31 | } 32 | 33 | void cuda_postprocess(const float* input, float* output, int disparity, int imageHeight, 34 | int imageWidth) { 35 | dim3 block(16, 16); 36 | dim3 grid(ceil_div(imageWidth, 16), ceil_div(imageHeight, 16), 1); 37 | postprocessing_kernel<<>>(input, output, disparity, imageHeight, imageWidth); 38 | } 39 | 40 | } // namespace bi3d 41 | } // namespace isaac 42 | } // namespace nvidia 43 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/components/bi3d_postprocessor.cu.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include "cuda.h" 23 | #include "cuda_runtime.h" 24 | 25 | namespace nvidia { 26 | namespace isaac { 27 | namespace bi3d { 28 | 29 | void cuda_postprocess(const float* input, float* output, int disparity, int imageHeight, 30 | int imageWidth); 31 | 32 | } // namespace bi3d 33 | } // namespace isaac 34 | } // namespace nvidia 35 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/components/bi3d_postprocessor.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #pragma once 18 | 19 | #include 20 | 21 | #include "gxf/std/allocator.hpp" 22 | #include "gxf/std/codelet.hpp" 23 | #include "gxf/std/receiver.hpp" 24 | #include "gxf/std/transmitter.hpp" 25 | 26 | namespace nvidia { 27 | namespace isaac { 28 | namespace bi3d { 29 | 30 | // GXF codelet that takes Bi3D output tensor list and collapses it into single segmentation image 31 | class Bi3DPostprocessor : public gxf::Codelet { 32 | public: 33 | gxf_result_t registerInterface(gxf::Registrar* registrar) noexcept override; 34 | gxf_result_t start() noexcept override { return GXF_SUCCESS; } 35 | gxf_result_t tick() noexcept override; 36 | gxf_result_t stop() noexcept override { return GXF_SUCCESS; } 37 | 38 | private: 39 | gxf::Parameter> bi3d_receiver_; 40 | gxf::Parameter> output_transmitter_; 41 | 42 | gxf::Parameter> pool_; 43 | gxf::Parameter> disparity_values_; 44 | }; 45 | 46 | } // namespace bi3d 47 | } // namespace isaac 48 | } // namespace nvidia 49 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/components/bi3d_postprocessor_utils.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #pragma once 18 | 19 | #include 20 | 21 | #include "gxf/multimedia/video.hpp" 22 | #include "gxf/std/allocator.hpp" 23 | 24 | namespace nvidia { 25 | namespace isaac { 26 | namespace bi3d { 27 | 28 | template 29 | inline constexpr uint8_t GetChannelSize(); 30 | 31 | template <> 32 | inline constexpr uint8_t GetChannelSize() { 33 | return 1; 34 | } 35 | 36 | template <> 37 | inline constexpr uint8_t GetChannelSize() { 38 | return 4; 39 | } 40 | 41 | template <> 42 | inline constexpr uint8_t GetChannelSize() { 43 | return 3; 44 | } 45 | 46 | template 47 | inline const char* GetColorName(); 48 | 49 | template <> 50 | inline const char* GetColorName() { 51 | return "gray"; 52 | } 53 | 54 | template <> 55 | inline const char* GetColorName() { 56 | return "D"; 57 | } 58 | 59 | template <> 60 | inline const char* GetColorName() { 61 | return "RGB"; 62 | } 63 | 64 | template 65 | gxf::Expected AllocateVideoBuffer(gxf::Handle video_buffer, size_t width, 66 | size_t height, gxf::MemoryStorageType memory_storage_type, 67 | gxf::Handle allocator) { 68 | if (width % 2 != 0 || height % 2 != 0) { 69 | GXF_LOG_ERROR("Image width and height must be even, but received width: %zu, height: %zu", 70 | width, height); 71 | return gxf::Unexpected{GXF_FAILURE}; 72 | } 73 | 74 | std::array planes{ 75 | gxf::ColorPlane(GetColorName(), GetChannelSize(), GetChannelSize() * width)}; 76 | gxf::VideoFormatSize video_format_size; 77 | const uint64_t size = video_format_size.size(width, height, planes); 78 | const std::vector planes_filled{planes.begin(), planes.end()}; 79 | const gxf::VideoBufferInfo buffer_info{static_cast(width), 80 | static_cast(height), T, planes_filled, 81 | gxf::SurfaceLayout::GXF_SURFACE_LAYOUT_PITCH_LINEAR}; 82 | return video_buffer->resizeCustom(buffer_info, size, memory_storage_type, allocator); 83 | } 84 | 85 | } // namespace bi3d 86 | } // namespace isaac 87 | } // namespace nvidia 88 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/inference/Bi3D.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | 23 | #include "extensions/bi3d/inference/Model.h" 24 | #include "extensions/tensorops/core/Array.h" 25 | #include "extensions/tensorops/core/ComputeEngine.h" 26 | #include "extensions/tensorops/core/Tensor.h" 27 | #include "gems/dnn_inferencer/inferencer/Inferencer.h" 28 | 29 | namespace nvidia { 30 | namespace isaac { 31 | namespace bi3d { 32 | 33 | using TensorLayout = cvcore::tensor_ops::TensorLayout; 34 | using ChannelType = cvcore::tensor_ops::ChannelType; 35 | using ChannelCount = cvcore::tensor_ops::ChannelCount; 36 | using ImagePreProcessingParams = cvcore::tensor_ops::ImagePreProcessingParams; 37 | using Tensor_HWC_C3_U8 = cvcore::tensor_ops::Tensor; 39 | using Tensor_CHW_C3_F32 = cvcore::tensor_ops::Tensor; 41 | using Tensor_CHW_CX_U8 = cvcore::tensor_ops::Tensor; 43 | using Tensor_CHW_CX_F32 = cvcore::tensor_ops::Tensor; 45 | using DisparityLevels = cvcore::tensor_ops::Array; 46 | using TensorBase = cvcore::tensor_ops::TensorBase; 47 | 48 | enum class ProcessingControl : std::uint8_t { 49 | DISABLE = 0, 50 | ENABLE = 1 51 | }; 52 | 53 | /** 54 | * Interface for Loading and running inference on bi3d network. 55 | */ 56 | class Bi3D { 57 | public: 58 | using InferencerParams = cvcore::inferencer::TensorRTInferenceParams; 59 | using TRTInferenceType = cvcore::inferencer::TRTInferenceType; 60 | 61 | /** 62 | * Default Image Processing Params for Bi3D. 63 | */ 64 | static const ImagePreProcessingParams defaultPreProcessorParams; 65 | 66 | /** 67 | * Default Model Input Params for Bi3D. 68 | */ 69 | static const ModelInputParams defaultModelInputParams; 70 | 71 | /** 72 | * Default inference Params for Bi3D. 73 | */ 74 | static const InferencerParams defaultFeatureParams; 75 | static const InferencerParams defaultHRParams; 76 | static const InferencerParams defaultRefinementParams; 77 | static const InferencerParams defaultSegmentationParams; 78 | 79 | /** 80 | * Bi3D extra params 81 | */ 82 | struct Bi3DParams { 83 | // Maximum pixel-wise shift between left and right images 84 | std::size_t maxDisparityLevels; 85 | // Switch to turn on/off edge refinement models (FeatNetHR and RefineNet) 86 | // in inferencing 87 | ProcessingControl edgeRefinement; 88 | // Switch to turn on/off sigmoid operation in postprocessing 89 | ProcessingControl sigmoidPostProcessing; 90 | // Switch to turn on/off thresholding in postprocessing 91 | ProcessingControl thresholdPostProcessing; 92 | // The low value set by threshold postprocessing 93 | float thresholdValueLow; 94 | // The high value set by threshold postprocessing 95 | float thresholdValueHigh; 96 | // The threshold value that casts pixel values to either low or high 97 | float threshold; 98 | }; 99 | static const Bi3DParams defaultBi3DParams; 100 | 101 | /** 102 | * Defaule Constructor of Bi3D. 103 | */ 104 | Bi3D() = delete; 105 | 106 | /** 107 | * Constructor of Bi3D. 108 | * @param params custom params for the network. 109 | */ 110 | Bi3D(const ImagePreProcessingParams & preProcessorParams, 111 | const ModelInputParams & modelInputParams, 112 | const InferencerParams & featureParams, 113 | const InferencerParams & hrParams, 114 | const InferencerParams & refinementParams, 115 | const InferencerParams & segmentationParams, 116 | const Bi3DParams & bi3dParams, 117 | cudaStream_t stream = 0); 118 | 119 | /** 120 | * Destructor of Bi3D. 121 | */ 122 | ~Bi3D(); 123 | 124 | /** 125 | * Main interface to run inference. 126 | * @param quantizedDisparity depth segmentation for input left and right images 127 | * @param leftImage left image tensor in RGB HWC format. 128 | * @param rightImage right image tensor in RGB HWC format. 129 | * @param disparityLevels diparity value to run depth estimation based on. 130 | */ 131 | void execute(Tensor_CHW_CX_U8 & quantizedDisparity, 132 | const Tensor_HWC_C3_U8 & leftImage, 133 | const Tensor_HWC_C3_U8 & rightImage, 134 | const DisparityLevels& disparityLevels, 135 | cudaStream_t stream = 0); 136 | 137 | /** 138 | * Main interface to run inference. 139 | * @param disparityConfidence depth segmentation for input left and right images 140 | * @param leftImage left image tensor in RGB HWC format. 141 | * @param rightImage right image tensor in RGB HWC format. 142 | * @param disparityLevels diparity value to run depth estimation based on. 143 | */ 144 | void execute(Tensor_CHW_CX_F32 & disparityConfidence, 145 | const Tensor_HWC_C3_U8 & leftImage, 146 | const Tensor_HWC_C3_U8 & rightImage, 147 | const DisparityLevels& disparityLevels, 148 | cudaStream_t stream = 0); 149 | 150 | /** 151 | * Main interface to run inference. 152 | * @param disparityConfidence depth segmentation for input left and right images 153 | * @param leftImage left image tensor in RGB CHW format. 154 | * @param rightImage right image tensor in RGB CHW format. 155 | * @param disparityLevels diparity value to run depth estimation based on. 156 | */ 157 | void execute(Tensor_CHW_CX_F32 & disparityConfidence, 158 | const Tensor_CHW_C3_F32 & leftImage, 159 | const Tensor_CHW_C3_F32 & rightImage, 160 | const DisparityLevels& disparityLevels, 161 | cudaStream_t stream = 0); 162 | 163 | private: 164 | struct Bi3DImpl; 165 | 166 | std::unique_ptr m_pImpl; 167 | }; 168 | 169 | /** 170 | * Interface for running pre-processing for Bi3D. 171 | */ 172 | class Bi3DPreProcessor { 173 | public: 174 | /** 175 | * Removing the default constructor for Bi3DPreProcessor. 176 | */ 177 | Bi3DPreProcessor() = delete; 178 | 179 | /** 180 | * Constructor for Bi3DPreProcessor. 181 | * @param preProcessorParams Image preprocessing parameters. 182 | * @param modelInputParams Model input parameters. 183 | */ 184 | Bi3DPreProcessor(const ImagePreProcessingParams & preProcessorParams, 185 | const ModelInputParams & modelInputParams, 186 | cudaStream_t stream = 0); 187 | 188 | /** 189 | * Destructor for Bi3DPreProcessor. 190 | */ 191 | ~Bi3DPreProcessor(); 192 | 193 | void execute(Tensor_CHW_C3_F32 & preprocessedLeftImage, 194 | Tensor_CHW_C3_F32 & preprocessedRightImage, 195 | const Tensor_HWC_C3_U8 & leftImage, 196 | const Tensor_HWC_C3_U8 & rightImage, 197 | cudaStream_t stream = 0); 198 | 199 | void execute(Tensor_CHW_C3_F32 & preprocessedLeftImage, 200 | Tensor_CHW_C3_F32 & preprocessedRightImage, 201 | const Tensor_CHW_C3_F32 & leftImage, 202 | const Tensor_CHW_C3_F32 & rightImage, 203 | cudaStream_t stream = 0); 204 | 205 | private: 206 | struct Bi3DPreProcessorImpl; 207 | 208 | std::unique_ptr m_pImpl; 209 | }; 210 | 211 | /** 212 | * Interface for running post-processing for Bi3D. 213 | */ 214 | class Bi3DPostProcessor { 215 | public: 216 | /** 217 | * Removing the default constructor for Bi3DPostProcessor. 218 | */ 219 | Bi3DPostProcessor() = delete; 220 | 221 | /** 222 | * Constructor for Bi3DPostProcessor. 223 | * @param modelInputParams Model input parameters. 224 | * @param bi3dParams Model parameters unique to this model. 225 | */ 226 | Bi3DPostProcessor(const ModelInputParams & modelInputParams, 227 | const Bi3D::Bi3DParams & bi3dParams, 228 | cudaStream_t stream = 0); 229 | 230 | /** 231 | * Destructor for Bi3DPostProcessor. 232 | */ 233 | ~Bi3DPostProcessor(); 234 | 235 | void execute(Tensor_CHW_CX_F32 & disparityConfidence, 236 | Tensor_CHW_CX_F32 & rawDisparityConfidence, 237 | cudaStream_t stream = 0); 238 | 239 | private: 240 | struct Bi3DPostProcessorImpl; 241 | 242 | std::unique_ptr m_pImpl; 243 | }; 244 | 245 | } // namespace bi3d 246 | } // namespace isaac 247 | } // namespace nvidia 248 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/inference/Bi3DPostProcessor.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include 19 | 20 | #include 21 | #include "extensions/bi3d/inference/Bi3D.h" 22 | #include "extensions/bi3d/inference/Bi3D_detail.hpp" 23 | #include "extensions/tensorops/core/Memory.h" 24 | 25 | namespace nvidia { 26 | namespace isaac { 27 | namespace bi3d { 28 | 29 | struct Bi3DPostProcessor::Bi3DPostProcessorImpl { 30 | mutable DisparityConfidence m_disparityConfidenceDevice; 31 | mutable DisparityConfidence m_disparityConfidenceThresholdDevice; 32 | mutable DisparityConfidence m_disparityConfidenceResizeDevice; 33 | 34 | ModelInputParams m_modelParams; 35 | Bi3D::Bi3DParams m_params; 36 | 37 | Bi3DPostProcessorImpl(const ModelInputParams & modelInputParams, 38 | const Bi3D::Bi3DParams & bi3dParams, 39 | cudaStream_t stream) 40 | : m_modelParams{modelInputParams}, m_params{bi3dParams} 41 | { 42 | m_disparityConfidenceDevice = {modelInputParams.inputLayerWidth, 43 | modelInputParams.inputLayerHeight, 44 | bi3dParams.maxDisparityLevels, 45 | false}; 46 | 47 | m_disparityConfidenceThresholdDevice = {modelInputParams.inputLayerWidth, 48 | modelInputParams.inputLayerHeight, 49 | bi3dParams.maxDisparityLevels, 50 | false}; 51 | } 52 | 53 | void resizeBuffers(std::size_t width, std::size_t height) { 54 | if (m_disparityConfidenceResizeDevice.getDimCount() > 0 && 55 | m_disparityConfidenceResizeDevice.getWidth() == width && 56 | m_disparityConfidenceResizeDevice.getHeight() == height) { 57 | return; 58 | } 59 | 60 | m_disparityConfidenceResizeDevice = {width, height, 61 | m_disparityConfidenceDevice.getChannelCount(), 62 | false}; 63 | } 64 | }; 65 | 66 | // ============================================================================= 67 | // Bi3D Frontend 68 | // ============================================================================= 69 | 70 | Bi3DPostProcessor::Bi3DPostProcessor(const ModelInputParams & modelInputParams, 71 | const Bi3D::Bi3DParams & bi3dParams, 72 | cudaStream_t stream) 73 | : m_pImpl{new Bi3DPostProcessorImpl(modelInputParams, bi3dParams, stream)} {} 74 | 75 | Bi3DPostProcessor::~Bi3DPostProcessor() {} 76 | 77 | void Bi3DPostProcessor::execute(DisparityConfidence & disparityConfidence, 78 | DisparityConfidence & rawDisparityConfidence, 79 | cudaStream_t stream) { 80 | // Ensuring the buffers are appropreately allocated 81 | m_pImpl->resizeBuffers(disparityConfidence.getWidth(), disparityConfidence.getHeight()); 82 | if (m_pImpl->m_params.sigmoidPostProcessing == ProcessingControl::ENABLE) { 83 | Sigmoid(m_pImpl->m_disparityConfidenceDevice, 84 | rawDisparityConfidence, stream); 85 | 86 | if (m_pImpl->m_params.thresholdPostProcessing == ProcessingControl::ENABLE) { 87 | Threshold(m_pImpl->m_disparityConfidenceThresholdDevice, 88 | m_pImpl->m_disparityConfidenceDevice, 89 | m_pImpl->m_params.thresholdValueLow, 90 | m_pImpl->m_params.thresholdValueHigh, 91 | m_pImpl->m_params.threshold, stream); 92 | 93 | Resize(m_pImpl->m_disparityConfidenceResizeDevice, 94 | m_pImpl->m_disparityConfidenceThresholdDevice, 95 | cvcore::tensor_ops::InterpolationType::INTERP_LINEAR, stream); 96 | } else { 97 | Resize(m_pImpl->m_disparityConfidenceResizeDevice, 98 | m_pImpl->m_disparityConfidenceDevice, 99 | cvcore::tensor_ops::InterpolationType::INTERP_LINEAR, stream); 100 | } 101 | } else { 102 | if (m_pImpl->m_params.thresholdPostProcessing == ProcessingControl::ENABLE) { 103 | Threshold(m_pImpl->m_disparityConfidenceThresholdDevice, 104 | rawDisparityConfidence, 105 | m_pImpl->m_params.thresholdValueLow, 106 | m_pImpl->m_params.thresholdValueHigh, 107 | m_pImpl->m_params.threshold, stream); 108 | 109 | Resize(m_pImpl->m_disparityConfidenceResizeDevice, 110 | m_pImpl->m_disparityConfidenceThresholdDevice, 111 | cvcore::tensor_ops::InterpolationType::INTERP_LINEAR, stream); 112 | } else { 113 | Resize(m_pImpl->m_disparityConfidenceResizeDevice, 114 | rawDisparityConfidence, 115 | cvcore::tensor_ops::InterpolationType::INTERP_LINEAR, stream); 116 | } 117 | } 118 | DisparityConfidence partial{disparityConfidence.getWidth(), 119 | disparityConfidence.getHeight(), 120 | disparityConfidence.getChannelCount(), 121 | m_pImpl->m_disparityConfidenceResizeDevice.getData(), 122 | m_pImpl->m_disparityConfidenceResizeDevice.isCPU()}; 123 | Copy(disparityConfidence, partial, stream); 124 | } 125 | } // namespace bi3d 126 | } // namespace isaac 127 | } // namespace nvidia 128 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/inference/Bi3DPreProcessor.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include 19 | 20 | #include 21 | #include "extensions/bi3d/inference/Bi3D.h" 22 | #include "extensions/bi3d/inference/Bi3D_detail.hpp" 23 | #include "extensions/bi3d/inference/Model.h" 24 | #include "extensions/tensorops/core/Memory.h" 25 | 26 | namespace nvidia { 27 | namespace isaac { 28 | namespace bi3d { 29 | 30 | struct Bi3DPreProcessor::Bi3DPreProcessorImpl { 31 | mutable InputImage m_leftImageDevice; 32 | mutable InputImage m_rightImageDevice; 33 | mutable PreprocessedImage m_preLeftImageDevice; 34 | mutable PreprocessedImage m_preRightImageDevice; 35 | mutable InputImage m_leftImageResizeDevice; 36 | mutable InputImage m_rightImageResizeDevice; 37 | 38 | // Operation(s) 39 | NormalizeFunctor m_normalizer; 40 | 41 | // Needed parameters for pre-processing 42 | ImagePreProcessingParams m_preProcessorParams; 43 | ModelInputParams m_modelParams; 44 | 45 | Bi3DPreProcessorImpl(const ImagePreProcessingParams & preProcessorParams, 46 | const ModelInputParams & modelInputParams, 47 | cudaStream_t stream) 48 | : m_preProcessorParams{preProcessorParams}, 49 | m_modelParams{modelInputParams}, 50 | m_normalizer{static_cast(modelInputParams.inputLayerWidth), 51 | static_cast(modelInputParams.inputLayerHeight)} { 52 | // Configuring the top-level model input(s) 53 | m_leftImageResizeDevice = {modelInputParams.inputLayerWidth, 54 | modelInputParams.inputLayerHeight, 55 | false}; 56 | m_rightImageResizeDevice = {modelInputParams.inputLayerWidth, 57 | modelInputParams.inputLayerHeight, 58 | m_leftImageResizeDevice.isCPU()}; 59 | } 60 | 61 | void resizeBuffers(std::size_t width, std::size_t height) { 62 | if (m_leftImageDevice.getDimCount() > 0 && 63 | m_leftImageDevice.getWidth() == width && 64 | m_leftImageDevice.getHeight() == height) { 65 | return; 66 | } 67 | 68 | m_leftImageDevice = {width, height, false}; 69 | m_rightImageDevice = {width, height, false}; 70 | m_preLeftImageDevice = {width, height, false}; 71 | m_preRightImageDevice = {width, height, false}; 72 | } 73 | }; 74 | 75 | // ============================================================================= 76 | // Bi3D Frontend 77 | // ============================================================================= 78 | 79 | Bi3DPreProcessor::Bi3DPreProcessor(const ImagePreProcessingParams & preProcessorParams, 80 | const ModelInputParams & modelInputParams, 81 | cudaStream_t stream) 82 | : m_pImpl{new Bi3DPreProcessorImpl(preProcessorParams, modelInputParams, stream)} {} 83 | 84 | Bi3DPreProcessor::~Bi3DPreProcessor() {} 85 | 86 | void Bi3DPreProcessor::execute(PreprocessedImage & preprocessedLeftImage, 87 | PreprocessedImage & preprocessedRightImage, 88 | const InputImage & leftImage, 89 | const InputImage & rightImage, 90 | cudaStream_t stream) { 91 | // Ensuring the buffers are appropreately allocated 92 | m_pImpl->resizeBuffers(leftImage.getWidth(), leftImage.getHeight()); 93 | 94 | // Ensuring data is on the GPU 95 | // FIXME: Rename so this is not confused with cvcore::Copy 96 | Copy(m_pImpl->m_leftImageDevice, leftImage, stream); 97 | Copy(m_pImpl->m_rightImageDevice, rightImage, stream); 98 | 99 | // Resizing the data to model input size 100 | Resize(m_pImpl->m_leftImageResizeDevice, m_pImpl->m_leftImageDevice, 101 | false, cvcore::tensor_ops::InterpolationType::INTERP_LINEAR, stream); 102 | Resize(m_pImpl->m_rightImageResizeDevice, m_pImpl->m_rightImageDevice, 103 | false, cvcore::tensor_ops::InterpolationType::INTERP_LINEAR, stream); 104 | 105 | // Normalize (data whiten) the images 106 | m_pImpl->m_normalizer(preprocessedLeftImage, m_pImpl->m_leftImageResizeDevice, 107 | m_pImpl->m_preProcessorParams.pixelMean, 108 | m_pImpl->m_preProcessorParams.stdDev, 109 | stream); 110 | m_pImpl->m_normalizer(preprocessedRightImage, m_pImpl->m_rightImageResizeDevice, 111 | m_pImpl->m_preProcessorParams.pixelMean, 112 | m_pImpl->m_preProcessorParams.stdDev, 113 | stream); 114 | } 115 | 116 | void Bi3DPreProcessor::execute(PreprocessedImage & preprocessedLeftImage, 117 | PreprocessedImage & preprocessedRightImage, 118 | const PreprocessedImage & leftImage, 119 | const PreprocessedImage & rightImage, 120 | cudaStream_t stream) { 121 | // Ensuring the buffers are appropreately allocated 122 | m_pImpl->resizeBuffers(leftImage.getWidth(), leftImage.getHeight()); 123 | 124 | // FIXME: Rename so this is not confused with cvcore::Copy 125 | Copy(m_pImpl->m_preLeftImageDevice, leftImage, stream); 126 | Copy(m_pImpl->m_preRightImageDevice, rightImage, stream); 127 | 128 | // Resizing the data to model input size 129 | Resize(preprocessedLeftImage, m_pImpl->m_preLeftImageDevice, 130 | cvcore::tensor_ops::InterpolationType::INTERP_LINEAR, stream); 131 | Resize(preprocessedLeftImage, m_pImpl->m_preRightImageDevice, 132 | cvcore::tensor_ops::InterpolationType::INTERP_LINEAR, stream); 133 | } 134 | } // namespace bi3d 135 | } // namespace isaac 136 | } // namespace nvidia 137 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/inference/Bi3D_detail.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include "extensions/bi3d/inference/Bi3D_detail.hpp" 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace nvidia { 28 | namespace isaac { 29 | namespace bi3d { 30 | 31 | void _pad(Tensor_CHW_C1_F32& dst, const Tensor_CHW_C1_F32& src, 32 | int topBorderHeight, int leftBorderWidth, cudaStream_t stream) { 33 | // Using NPP function: ``nppiCopyConstBorder_32f_C1R_Ctx'' 34 | // Spec ==> NppStatus nppiCopyConstBorder_32f_C1R_Ctx(const Npp32f * pSrc, 35 | // int nSrcStep, 36 | // NppiSize oSrcSizeROI, 37 | // Npp32f * pDst, 38 | // int nDstStep, 39 | // NppiSize oDstSizeROI, 40 | // int nTopBorderHeight, 41 | // int nLeftBorderWidth, 42 | // Npp32f nValue, 43 | // NppStreamContext nppStreamCtx) 44 | // 45 | // Parameters 46 | // pSrc Source-Image Pointer. 47 | // nSrcStep Source-Image Stride in bytes. 48 | // oSrcSizeROI Size (width, height) of the source region in pixels. 49 | // pDst Destination-Image Pointer. 50 | // nDstStep Destination-Image Stride in bytes. 51 | // oDstSizeROI Size (width, height) of the destination region, i.e. the region 52 | // that gets filled with data from the source image (inner part) 53 | // and constant border color (outer part). 54 | // nTopBorderHeight Height (in pixels) of the top border. The number of pixel rows at 55 | // the top of the destination ROI that will be filled with the 56 | // constant border color. nBottomBorderHeight = oDstSizeROI.height - 57 | // nTopBorderHeight - oSrcSizeROI.height. 58 | // nLeftBorderWidth Width (in pixels) of the left border. The width of the border at 59 | // the right side of the destination ROI is implicitly defined by the 60 | // size of the source ROI: nRightBorderWidth = oDstSizeROI.width - 61 | // nLeftBorderWidth - oSrcSizeROI.width. 62 | // nValue The pixel value to be set for border pixels for single channel 63 | // functions. 64 | // nppStreamCtx NPP Application Managed Stream Context. 65 | nppiCopyConstBorder_32f_C1R_Ctx( 66 | static_cast(src.getData()), 67 | src.getStride(TensorDimension::HEIGHT) * sizeof(float), 68 | {static_cast(src.getWidth()), static_cast(src.getHeight())}, 69 | static_cast(dst.getData()), 70 | dst.getStride(TensorDimension::HEIGHT) * sizeof(float), 71 | {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, 72 | topBorderHeight, leftBorderWidth, 0.0, cvcore::tensor_ops::GetNppStreamContext(stream)); 73 | } 74 | 75 | void _threshold(Tensor_CHW_C1_F32& dst, const Tensor_CHW_C1_F32& src, 76 | float valueLow, float valueHigh, float thresholdValue, 77 | cudaStream_t stream) { 78 | // Using NPP function: ``nppiThreshold_LTValGTVal_32f_C1R_Ctx'' 79 | // Spec ==> NppStatus nppiThreshold_LTValGTVal_32f_C1R_Ctx(const Npp32f * pSrc, 80 | // int nSrcStep, 81 | // Npp32f * pDst, 82 | // int nDstStep, 83 | // NppiSize oSizeROI, 84 | // Npp32f nThresholdLT, 85 | // Npp32f nValueLT, 86 | // Npp32f nThresholdGT, 87 | // Npp32f nValueGT, 88 | // NppStreamContext nppStreamCtx) 89 | // 90 | // Parameters 91 | // pSrc Source-Image Pointer. 92 | // nSrcStep Source-Image Stride in bytes. 93 | // pDst Destination-Image Pointer. 94 | // nDstStep Destination-Image Stride in bytes. 95 | // oSizeROI Size (width, height) of the destination region, i.e. the region 96 | // that gets filled with data from the source image 97 | // nThresholdLT The thresholdLT value. 98 | // nValueLT The thresholdLT replacement value. 99 | // nThresholdGT The thresholdGT value. 100 | // nValueGT The thresholdGT replacement value. 101 | // nppStreamCtx NPP Application Managed Stream Context. 102 | nppiThreshold_LTValGTVal_32f_C1R_Ctx( 103 | static_cast(src.getData()), 104 | src.getStride(TensorDimension::HEIGHT) * sizeof(float), 105 | static_cast(dst.getData()), 106 | dst.getStride(TensorDimension::HEIGHT) * sizeof(float), 107 | {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, 108 | thresholdValue * (1.0 + std::numeric_limits::epsilon()), valueLow, 109 | thresholdValue, valueHigh, 110 | cvcore::tensor_ops::GetNppStreamContext(stream)); 111 | } 112 | 113 | void _clear(Tensor_CHW_C1_F32& dst, cudaStream_t stream) { 114 | // Using NPP function: ``nppiSet_32f_C1R_Ctx'' 115 | // Spec ==> NppStatus nppiSet_32f_C1R_Ctx(const Npp32f nValue 116 | // Npp32f * pDst, 117 | // int nDstStep, 118 | // NppiSize oSizeROI, 119 | // NppStreamContext nppStreamCtx) 120 | // 121 | // Parameters 122 | // nValue The pixel value to be set. 123 | // pDst Destination-Image Pointer. 124 | // nDstStep Destination-Image Stride in bytes. 125 | // oSizeROI Size (width, height) of the destination region, i.e. the region that 126 | // gets filled with nValue. 127 | // nppStreamCtx NPP Application Managed Stream Context. 128 | nppiSet_32f_C1R_Ctx( 129 | 0.0, 130 | static_cast(dst.getData()), 131 | dst.getStride(TensorDimension::HEIGHT) * sizeof(float), 132 | {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, 133 | cvcore::tensor_ops::GetNppStreamContext(stream)); 134 | } 135 | 136 | void _sigmoid(Tensor_CHW_C1_F32& dst, Tensor_CHW_C1_F32& src, 137 | cudaStream_t stream) { 138 | auto context = cvcore::tensor_ops::GetNppStreamContext(stream); 139 | 140 | // This is a bit of a complex series of operations, here's an exact model 141 | // 142 | // Explicit: 143 | // DST = exp(-1 * ln(1 + exp(-1 * SRC))) 144 | // 145 | // Properties: 146 | // 1) exp(ln(x)) = x 147 | // 2) ln(x^y) = y * ln(x) 148 | // 3) x^(-1) = 1 / x 149 | // 150 | // Expected: 151 | // DST = 1 / (1 + exp(-1 * SRC)) 152 | nppiMulC_32f_C1R_Ctx( 153 | static_cast(src.getData()), 154 | src.getStride(TensorDimension::HEIGHT) * sizeof(float), 155 | -1.0, 156 | static_cast(dst.getData()), 157 | dst.getStride(TensorDimension::HEIGHT) * sizeof(float), 158 | {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, 159 | context); 160 | nppiExp_32f_C1IR_Ctx( 161 | static_cast(dst.getData()), 162 | dst.getStride(TensorDimension::HEIGHT) * sizeof(float), 163 | {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, 164 | context); 165 | nppiAddC_32f_C1IR_Ctx( 166 | 1.0, 167 | static_cast(dst.getData()), 168 | dst.getStride(TensorDimension::HEIGHT) * sizeof(float), 169 | {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, 170 | context); 171 | nppiLn_32f_C1IR_Ctx( 172 | static_cast(dst.getData()), 173 | dst.getStride(TensorDimension::HEIGHT) * sizeof(float), 174 | {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, 175 | context); 176 | nppiMulC_32f_C1IR_Ctx( 177 | -1.0, 178 | static_cast(dst.getData()), 179 | dst.getStride(TensorDimension::HEIGHT) * sizeof(float), 180 | {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, 181 | context); 182 | nppiExp_32f_C1IR_Ctx( 183 | static_cast(dst.getData()), 184 | dst.getStride(TensorDimension::HEIGHT) * sizeof(float), 185 | {static_cast(dst.getWidth()), static_cast(dst.getHeight())}, 186 | context); 187 | } 188 | 189 | void _sigmoid(Tensor_CHW_C1_F32& dst, Tensor_CHW_C1_F32& src) { 190 | std::transform(src.getData(), src.getData() + src.getDataSize() / sizeof(float), 191 | dst.getData(), [](float value) -> float { 192 | return 1.0 / (1.0 + std::exp(-1.0 * value));}); 193 | } 194 | 195 | } // namespace bi3d 196 | } // namespace isaac 197 | } // namespace nvidia 198 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/extensions/bi3d/inference/Model.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2020-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | 23 | #include "extensions/tensorops/core/Image.h" 24 | 25 | namespace nvidia { 26 | namespace isaac { 27 | namespace bi3d { 28 | 29 | /** 30 | * Struct to describe input type required by the model 31 | */ 32 | struct ModelInputParams { 33 | size_t maxBatchSize; /**< maxbatchSize supported by network*/ 34 | size_t inputLayerWidth; /**< Input layer width */ 35 | size_t inputLayerHeight; /**< Input layer Height */ 36 | cvcore::tensor_ops::ImageType modelInputType; /**< Input Layout type */ 37 | }; 38 | 39 | /** 40 | * Struct to describe the model 41 | */ 42 | struct ModelInferenceParams { 43 | std::string engineFilePath; /**< Engine file path. */ 44 | std::vector inputLayers; /**< names of input layers. */ 45 | std::vector outputLayers; /**< names of output layers. */ 46 | }; 47 | 48 | } // namespace bi3d 49 | } // namespace isaac 50 | } // namespace nvidia 51 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/Errors.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include "gems/dnn_inferencer/inferencer/Errors.h" 19 | #include 20 | 21 | #ifndef __cpp_lib_to_underlying 22 | namespace std { 23 | template 24 | constexpr underlying_type_t to_underlying(Enum e) noexcept { 25 | return static_cast>(e); 26 | } 27 | }; // namespace std 28 | #endif // __cpp_lib_to_underlying 29 | 30 | namespace cvcore { 31 | namespace inferencer { 32 | 33 | namespace detail { 34 | struct InferencerErrorCategory : std::error_category { 35 | const char* name() const noexcept final { 36 | return "cvcore-inferencer-error"; 37 | } 38 | 39 | std::string message(int value) const final { 40 | std::string result; 41 | 42 | switch (value) { 43 | case std::to_underlying(InferencerErrorCode::SUCCESS): 44 | result = "(SUCCESS) No errors detected"; 45 | break; 46 | case std::to_underlying(InferencerErrorCode::INVALID_ARGUMENT): 47 | result = "(INVALID_ARGUMENT) Invalid config parameter or input argument"; 48 | break; 49 | case std::to_underlying(InferencerErrorCode::INVALID_OPERATION): 50 | result = "(INVALID_OPERATION) Invalid operation performed"; 51 | break; 52 | case std::to_underlying(InferencerErrorCode::TRITON_SERVER_NOT_READY): 53 | result = "(TRITON_SERVER_NOT_READY) Triton server is not live or the serverUrl" 54 | "is incorrect"; 55 | break; 56 | case std::to_underlying(InferencerErrorCode::TRITON_CUDA_SHARED_MEMORY_ERROR): 57 | result = "(TRITON_CUDA_SHARED_MEMORY_ERROR) Unable to map/unmap cuda shared memory" 58 | "for triton server"; 59 | break; 60 | case std::to_underlying(InferencerErrorCode::TRITON_INFERENCE_ERROR): 61 | result = "(TRITON_INFERENCE_ERROR) Error during inference using triton API"; 62 | break; 63 | case std::to_underlying(InferencerErrorCode::TRITON_REGISTER_LAYER_ERROR): 64 | result = "(TRITON_REGISTER_LAYER_ERROR) Error when setting input or output layers"; 65 | break; 66 | case std::to_underlying(InferencerErrorCode::TENSORRT_INFERENCE_ERROR): 67 | result = "(TENSORRT_INFERENCE_ERROR) Error when running TensorRT enqueue/execute"; 68 | break; 69 | default: 70 | result = "(Unrecognized Condition) Value " + std::to_string(value) + 71 | " does not map to known error code literal " + 72 | " defined by cvcore::inferencer::InferencerErrorCode"; 73 | break; 74 | } 75 | 76 | return result; 77 | } 78 | 79 | std::error_condition default_error_condition(int code) const noexcept final { 80 | std::error_condition result; 81 | using ErrorCode = cvcore::tensor_ops::ErrorCode; 82 | 83 | switch (code) { 84 | case std::to_underlying(InferencerErrorCode::SUCCESS): 85 | result = ErrorCode::SUCCESS; 86 | break; 87 | case std::to_underlying(InferencerErrorCode::INVALID_ARGUMENT): 88 | result = ErrorCode::INVALID_ARGUMENT; 89 | break; 90 | case std::to_underlying(InferencerErrorCode::INVALID_OPERATION): 91 | result = ErrorCode::INVALID_OPERATION; 92 | break; 93 | case std::to_underlying(InferencerErrorCode::NOT_IMPLEMENTED): 94 | result = ErrorCode::NOT_IMPLEMENTED; 95 | break; 96 | case std::to_underlying(InferencerErrorCode::TRITON_SERVER_NOT_READY): 97 | result = ErrorCode::NOT_READY; 98 | break; 99 | case std::to_underlying(InferencerErrorCode::TRITON_CUDA_SHARED_MEMORY_ERROR): 100 | result = ErrorCode::DEVICE_ERROR; 101 | break; 102 | case std::to_underlying(InferencerErrorCode::TRITON_INFERENCE_ERROR): 103 | result = ErrorCode::INVALID_OPERATION; 104 | break; 105 | case std::to_underlying(InferencerErrorCode::TENSORRT_INFERENCE_ERROR): 106 | result = ErrorCode::INVALID_OPERATION; 107 | break; 108 | case std::to_underlying(InferencerErrorCode::TRITON_REGISTER_LAYER_ERROR): 109 | result = ErrorCode::INVALID_OPERATION; 110 | break; 111 | default: 112 | result = ErrorCode::NOT_IMPLEMENTED; 113 | break; 114 | } 115 | 116 | return result; 117 | } 118 | }; 119 | } // namespace detail 120 | 121 | const detail::InferencerErrorCategory errorCategory{}; 122 | 123 | std::error_code make_error_code(InferencerErrorCode ec) noexcept { 124 | return {std::to_underlying(ec), errorCategory}; 125 | } 126 | 127 | } // namespace inferencer 128 | } // namespace cvcore 129 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/Errors.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021-2023, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | * property and proprietary rights in and to this material, related 6 | * documentation and any modifications thereto. Any use, reproduction, 7 | * disclosure or distribution of this material and related documentation 8 | * without an express license agreement from NVIDIA CORPORATION or 9 | * its affiliates is strictly prohibited. 10 | * 11 | * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES 12 | * SPDX-License-Identifier: LicenseRef-NvidiaProprietary 13 | */ 14 | 15 | #pragma once 16 | 17 | #include "extensions/tensorops/core/CVError.h" 18 | 19 | namespace cvcore { 20 | namespace inferencer { 21 | 22 | /* 23 | * Enum class describing the inference error codes. 24 | */ 25 | enum class InferencerErrorCode : std::int32_t { 26 | SUCCESS = 0, 27 | INVALID_ARGUMENT, 28 | INVALID_OPERATION, 29 | NOT_IMPLEMENTED, 30 | TRITON_SERVER_NOT_READY, 31 | TRITON_CUDA_SHARED_MEMORY_ERROR, 32 | TRITON_INFERENCE_ERROR, 33 | TRITON_REGISTER_LAYER_ERROR, 34 | TENSORRT_INFERENCE_ERROR, 35 | TENSORRT_ENGINE_ERROR 36 | }; 37 | 38 | } // namespace inferencer 39 | } // namespace cvcore 40 | 41 | namespace std { 42 | 43 | template<> 44 | struct is_error_code_enum : true_type { 45 | }; 46 | 47 | } // namespace std 48 | 49 | namespace cvcore { 50 | namespace inferencer { 51 | 52 | std::error_code make_error_code(InferencerErrorCode) noexcept; 53 | 54 | } // namespace inferencer 55 | } // namespace cvcore 56 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/IInferenceBackend.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021-2023, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | * property and proprietary rights in and to this material, related 6 | * documentation and any modifications thereto. Any use, reproduction, 7 | * disclosure or distribution of this material and related documentation 8 | * without an express license agreement from NVIDIA CORPORATION or 9 | * its affiliates is strictly prohibited. 10 | * 11 | * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES 12 | * SPDX-License-Identifier: LicenseRef-NvidiaProprietary 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include "extensions/tensorops/core/Tensor.h" 27 | 28 | namespace cvcore { 29 | namespace inferencer { 30 | 31 | /** 32 | * Struct type to describe input and output layers. 33 | */ 34 | struct LayerInfo { 35 | size_t index; /**< Block Index of layer */ 36 | std::string name; /**< Name of layer */ 37 | std::vector shape; /**< Shape of layer */ 38 | cvcore::tensor_ops::ChannelType dataType; /**< Datatype of layer */ 39 | cvcore::tensor_ops::TensorLayout layout; /**< Tensor layour of layer */ 40 | size_t layerSize; 41 | }; 42 | 43 | /** 44 | * Enum class to describe the backend protocol for triton 45 | */ 46 | enum class BackendProtocol { 47 | GRPC, 48 | HTTP 49 | }; 50 | 51 | /** 52 | * Struct type to describe the input for triton inference. 53 | */ 54 | struct TritonRemoteInferenceParams { 55 | std::string serverUrl; /**< Server url created by running the triton server */ 56 | bool verbose; /**< Verbose log from backend */ 57 | BackendProtocol protocolType; /**< Backend protocol type */ 58 | std::string modelName; /**< Model name as per model respoitory */ 59 | std::string modelVersion; /** Model version as per model repository */ 60 | }; 61 | 62 | /** 63 | * Struct type to describe the model metadata parsed by the inference backend. 64 | */ 65 | struct ModelMetaData { 66 | std::string modelName; /**< Model name */ 67 | std::string modelVersion; /**< Model version */ 68 | std::unordered_map inputLayers; /**< Map of input layer */ 69 | std::unordered_map 70 | outputLayers; /**< Map of output layer information indexed by layer name */ 71 | size_t maxBatchSize; /**< Maximum batch size */ 72 | }; 73 | 74 | /** 75 | * Enum type for TensorRT inference type 76 | */ 77 | enum class TRTInferenceType { 78 | TRT_ENGINE, /**< Inference using TRT engine file */ 79 | TRT_ENGINE_IN_MEMORY /**< Inference using TRT Cuda Engine */ 80 | }; 81 | 82 | /** 83 | * TRT Logger 84 | */ 85 | class TRTLogger : public nvinfer1::ILogger { 86 | public: 87 | void log(Severity severity, const char* msg) noexcept { 88 | if ((severity == Severity::kERROR) || (severity == Severity::kWARNING)) { 89 | std::cerr << msg << std::endl; 90 | } 91 | } 92 | }; 93 | 94 | /** 95 | * Enum class to describe the model build flags 96 | */ 97 | enum OnnxModelBuildFlag { 98 | NONE = 0, 99 | kINT8 = 1, 100 | kFP16 = 2, 101 | kGPU_FALLBACK = 4, 102 | }; 103 | 104 | /** 105 | * Struct type to describe the input for triton inference. 106 | */ 107 | struct TensorRTInferenceParams { 108 | TRTInferenceType inferType; /**< TensorRT inference type */ 109 | nvinfer1::ICudaEngine* engine; /**< Cuda engine file for TRT_ENGINE_IN_MEMORY type. */ 110 | // Nullptr if TRT_ENGINE is used 111 | std::string onnxFilePath; /**< ONNX model file path. */ 112 | std::string engineFilePath; /**< Engine file path for TRT_ENGINE type. */ 113 | bool force_engine_update; 114 | int32_t buildFlags; 115 | int64_t max_workspace_size; 116 | std::size_t maxBatchSize; /**< Max Batch size */ 117 | std::vector inputLayerNames; /** Input layer names */ 118 | std::vector outputLayerNames; /** Output layer names */ 119 | int32_t dlaID{-1}; 120 | }; 121 | 122 | /** 123 | * Interface for TritonRemote , Triton C and Native TensorRT inference backend. 124 | */ 125 | class IInferenceBackendClient { 126 | public: 127 | virtual ~IInferenceBackendClient() noexcept = default; 128 | 129 | /** 130 | * Function to set input layer data 131 | * @param trtInputBuffer Input GPU buffer 132 | * @param inputLayerName Input Layer name 133 | * @return error code 134 | */ 135 | virtual std::error_code setInput(const cvcore::tensor_ops::TensorBase& trtInputBuffer, 136 | std::string inputLayerName) = 0; 137 | 138 | /** 139 | * Function to set output layer data 140 | * @param trtInputBuffer Output GPU buffer 141 | * @param outputLayerName Output Layer name 142 | * @return error code 143 | */ 144 | virtual std::error_code setOutput(cvcore::tensor_ops::TensorBase& trtOutputBuffer, 145 | std::string outputLayerName) = 0; 146 | 147 | /** 148 | * Returns the model metadata parsed by the inference backend. 149 | * @return ModelMetaData 150 | */ 151 | virtual ModelMetaData getModelMetaData() const = 0; 152 | 153 | /** 154 | * Inference based on input and output set. enqueueV2 for TensorRT and inferSync for Triton. 155 | * @param Batch size of input for inference. Default set to 1. Must be <= Max Batch Size . 156 | * @return error code 157 | */ 158 | virtual std::error_code infer(size_t batchSize = 1) = 0; 159 | 160 | /** 161 | * Sets the cuda stream applicable only for TensorRT backend. 162 | * @param cudaStream_t cuda input stream 163 | * @return error code 164 | */ 165 | virtual std::error_code setCudaStream(cudaStream_t) = 0; 166 | 167 | /** 168 | * Unregisters the tensor mapped from the inference backend 169 | * @param input/output layer name 170 | * @return error code 171 | */ 172 | virtual std::error_code unregister(std::string layerName) = 0; 173 | 174 | /** 175 | * Unregisters all the tensor mappeds from the inference backend 176 | * @return error code 177 | */ 178 | virtual std::error_code unregister() = 0; 179 | 180 | protected: 181 | IInferenceBackendClient() = default; 182 | IInferenceBackendClient(const IInferenceBackendClient&) = default; 183 | IInferenceBackendClient& operator=(const IInferenceBackendClient&) = default; 184 | IInferenceBackendClient(IInferenceBackendClient &&) noexcept = default; 185 | IInferenceBackendClient& operator=(IInferenceBackendClient &&) noexcept = default; 186 | }; 187 | 188 | using InferenceBackendClient = IInferenceBackendClient *; 189 | 190 | } // namespace inferencer 191 | } // namespace cvcore 192 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/Inferencer.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include "gems/dnn_inferencer/inferencer/Inferencer.h" 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "Errors.h" 26 | #include "IInferenceBackend.h" 27 | #include "TensorRTInferencer.h" 28 | #include "TritonGrpcInferencer.h" 29 | 30 | namespace cvcore { 31 | namespace inferencer { 32 | using ErrorCode = cvcore::tensor_ops::ErrorCode; 33 | 34 | std::mutex InferenceBackendFactory::inferenceMutex; 35 | 36 | #ifdef ENABLE_TRITON 37 | std::unordered_map> 38 | InferenceBackendFactory::tritonRemoteMap; 39 | 40 | std::error_code InferenceBackendFactory::CreateTritonRemoteInferenceBackendClient( 41 | InferenceBackendClient& client, const TritonRemoteInferenceParams& params) { 42 | std::lock_guard instanceLock(inferenceMutex); 43 | 44 | if (params.protocolType == BackendProtocol::HTTP) { 45 | return ErrorCode::NOT_IMPLEMENTED; 46 | } 47 | std::error_code result = ErrorCode::SUCCESS; 48 | std::string hashString = params.serverUrl + params.modelName + params.modelVersion; 49 | 50 | try { 51 | if (tritonRemoteMap.find(hashString) != tritonRemoteMap.end()) { 52 | client = tritonRemoteMap[hashString].second; 53 | tritonRemoteMap[hashString].first++; 54 | } else { 55 | tritonRemoteMap[hashString] = 56 | std::pair(1, 57 | new TritonGrpcInferencer(params)); 58 | } 59 | } 60 | catch (std::error_code &e) { 61 | result = e; 62 | } 63 | catch (...) { 64 | result = ErrorCode::INVALID_ARGUMENT; 65 | } 66 | client = tritonRemoteMap[hashString].second; 67 | return result; 68 | } 69 | 70 | std::error_code InferenceBackendFactory::DestroyTritonRemoteInferenceBackendClient( 71 | InferenceBackendClient& client) { 72 | std::lock_guard instanceLock(inferenceMutex); 73 | for (auto &it : tritonRemoteMap) { 74 | if (it.second.second == client) { 75 | it.second.first--; 76 | if (it.second.first == 0) { 77 | tritonRemoteMap.erase(it.first); 78 | client->unregister(); 79 | delete client; 80 | client = nullptr; 81 | } 82 | break; 83 | } 84 | } 85 | client = nullptr; 86 | return ErrorCode::SUCCESS; 87 | } 88 | #endif 89 | 90 | std::error_code InferenceBackendFactory::CreateTensorRTInferenceBackendClient( 91 | InferenceBackendClient& client, const TensorRTInferenceParams& params) { 92 | 93 | std::lock_guard instanceLock(inferenceMutex); 94 | std::error_code result = ErrorCode::SUCCESS; 95 | try { 96 | client = new TensorRTInferencer(params); 97 | } 98 | catch (std::error_code &e) { 99 | result = e; 100 | } 101 | catch (...) { 102 | result = ErrorCode::INVALID_ARGUMENT; 103 | } 104 | return result; 105 | } 106 | 107 | std::error_code InferenceBackendFactory::DestroyTensorRTInferenceBackendClient( 108 | InferenceBackendClient& client) { 109 | 110 | std::lock_guard instanceLock(inferenceMutex); 111 | if (client != nullptr) { 112 | client->unregister(); 113 | delete client; 114 | client = nullptr; 115 | } 116 | client = nullptr; 117 | 118 | return ErrorCode::SUCCESS; 119 | } 120 | } // namespace inferencer 121 | } // namespace cvcore 122 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/Inferencer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021-2023, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | * property and proprietary rights in and to this material, related 6 | * documentation and any modifications thereto. Any use, reproduction, 7 | * disclosure or distribution of this material and related documentation 8 | * without an express license agreement from NVIDIA CORPORATION or 9 | * its affiliates is strictly prohibited. 10 | * 11 | * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES 12 | * SPDX-License-Identifier: LicenseRef-NvidiaProprietary 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "extensions/tensorops/core/Tensor.h" 24 | #include "IInferenceBackend.h" 25 | 26 | namespace cvcore { 27 | namespace inferencer { 28 | 29 | /** 30 | * A class to create and destroy a client for a given inference backend type 31 | */ 32 | class InferenceBackendFactory { 33 | public: 34 | #ifdef ENABLE_TRITON 35 | 36 | /** 37 | * Function to create client for Triton remote inference backend based on the Triton remote 38 | * inference paramaters. 39 | * @param client client object created 40 | * @param Triton remote inference config parameters. 41 | * @return error code 42 | */ 43 | static std::error_code CreateTritonRemoteInferenceBackendClient(InferenceBackendClient& client, 44 | const TritonRemoteInferenceParams&); 45 | 46 | /** 47 | * Function to Destroy the triton grpc client 48 | * @param client client object created 49 | * @return error code 50 | */ 51 | static std::error_code DestroyTritonRemoteInferenceBackendClient( 52 | InferenceBackendClient& client); 53 | #endif 54 | 55 | /** 56 | * Function to create client for TensorRT inference backend based on the TensorRT 57 | * inference paramaters. 58 | * @param client client object created 59 | * @param TensorRT inference config parameters. 60 | * @return error code 61 | */ 62 | static std::error_code CreateTensorRTInferenceBackendClient(InferenceBackendClient& client, 63 | const TensorRTInferenceParams&); 64 | 65 | /** 66 | * Function to Destroy the tensorrt client 67 | * @param client client object created 68 | * @return error code 69 | */ 70 | static std::error_code DestroyTensorRTInferenceBackendClient(InferenceBackendClient& client); 71 | 72 | private: 73 | #ifdef ENABLE_TRITON 74 | // Keeps track of any changes in url/model repo path and returns an existing / new client 75 | // instance. 76 | static std::unordered_map> tritonRemoteMap; 78 | #endif 79 | static std::mutex inferenceMutex; 80 | }; 81 | 82 | } // namespace inferencer 83 | } // namespace cvcore 84 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/TensorRTInferencer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021-2024, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | * property and proprietary rights in and to this material, related 6 | * documentation and any modifications thereto. Any use, reproduction, 7 | * disclosure or distribution of this material and related documentation 8 | * without an express license agreement from NVIDIA CORPORATION or 9 | * its affiliates is strictly prohibited. 10 | * 11 | * SPDX-FileCopyrightText: Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES 12 | * SPDX-License-Identifier: LicenseRef-NvidiaProprietary 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include "Errors.h" 25 | #include "IInferenceBackend.h" 26 | #include "Inferencer.h" 27 | 28 | namespace cvcore { 29 | namespace inferencer { 30 | 31 | using TensorBase = cvcore::tensor_ops::TensorBase; 32 | class TensorRTInferencer : public IInferenceBackendClient { 33 | public: 34 | TensorRTInferencer(const TensorRTInferenceParams& params); 35 | 36 | // Set input layer tensor 37 | std::error_code setInput(const TensorBase& trtInputBuffer, 38 | std::string inputLayerName) override; 39 | 40 | // Sets output layer tensor 41 | std::error_code setOutput(TensorBase& trtOutputBuffer, 42 | std::string outputLayerName) override; 43 | 44 | // Get the model metadata parsed based on the model file 45 | // This would be done in initialize call itself. User can access the 46 | // modelMetaData created using this API. 47 | ModelMetaData getModelMetaData() const override; 48 | 49 | // Convert onnx mode to engine file 50 | std::error_code convertModelToEngine(int32_t dla_core, 51 | const char* model_file, int64_t max_workspace_size, int32_t buildFlags, 52 | std::size_t max_batch_size); 53 | 54 | // TensorRT will use infer and TensorRT would use enqueueV2 55 | std::error_code infer(size_t batchSize = 1) override; 56 | 57 | // Applicable only for Native TRT 58 | std::error_code setCudaStream(cudaStream_t) override; 59 | 60 | // Unregister shared memory for layer 61 | std::error_code unregister(std::string layerName) override; 62 | 63 | // Unregister all shared memory 64 | std::error_code unregister() override; 65 | 66 | private: 67 | ~TensorRTInferencer(); 68 | std::unique_ptr m_logger; 69 | std::unique_ptr m_inferenceRuntime; 70 | size_t m_maxBatchSize; 71 | std::vector m_inputLayers; 72 | std::vector m_outputLayers; 73 | cudaStream_t m_cudaStream; 74 | nvinfer1::ICudaEngine* m_inferenceEngine; 75 | std::unique_ptr m_ownedInferenceEngine; 76 | std::unique_ptr m_inferenceContext; 77 | size_t m_ioTensorsCount; 78 | ModelMetaData m_modelInfo; 79 | bool m_hasImplicitBatch; 80 | std::vector m_modelEngineStream; 81 | size_t m_modelEngineStreamSize = 0; 82 | 83 | std::error_code ParseTRTModel(); 84 | std::error_code getLayerInfo(LayerInfo& layer, std::string layerName); 85 | }; 86 | 87 | } // namespace inferencer 88 | } // namespace cvcore 89 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/TensorRTUtils.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include "gems/dnn_inferencer/inferencer/TensorRTUtils.h" 19 | #include 20 | 21 | namespace cvcore { 22 | namespace inferencer { 23 | using TensorLayout = cvcore::tensor_ops::TensorLayout; 24 | using ChannelType = cvcore::tensor_ops::ChannelType; 25 | using ChannelCount = cvcore::tensor_ops::ChannelCount; 26 | using ErrorCode = cvcore::tensor_ops::ErrorCode; 27 | 28 | std::error_code getCVCoreChannelTypeFromTensorRT(ChannelType& channelType, 29 | nvinfer1::DataType dtype) { 30 | if (dtype == nvinfer1::DataType::kINT8) { 31 | channelType = ChannelType::U8; 32 | } else if (dtype == nvinfer1::DataType::kHALF) { 33 | channelType = ChannelType::F16; 34 | } else if (dtype == nvinfer1::DataType::kFLOAT) { 35 | channelType = ChannelType::F32; 36 | } else { 37 | return ErrorCode::INVALID_OPERATION; 38 | } 39 | 40 | return ErrorCode::SUCCESS; 41 | } 42 | 43 | std::error_code getCVCoreChannelLayoutFromTensorRT(TensorLayout& channelLayout, 44 | nvinfer1::TensorFormat tensorFormat) { 45 | if (tensorFormat == nvinfer1::TensorFormat::kLINEAR) { 46 | channelLayout = TensorLayout::NCHW; 47 | } else if (tensorFormat == nvinfer1::TensorFormat::kHWC) { 48 | channelLayout = TensorLayout::HWC; 49 | } else { 50 | return ErrorCode::INVALID_OPERATION; 51 | } 52 | 53 | return ErrorCode::SUCCESS; 54 | } 55 | 56 | } // namespace inferencer 57 | } // namespace cvcore 58 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/TensorRTUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021-2023, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | * property and proprietary rights in and to this material, related 6 | * documentation and any modifications thereto. Any use, reproduction, 7 | * disclosure or distribution of this material and related documentation 8 | * without an express license agreement from NVIDIA CORPORATION or 9 | * its affiliates is strictly prohibited. 10 | * 11 | * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES 12 | * SPDX-License-Identifier: LicenseRef-NvidiaProprietary 13 | */ 14 | #pragma once 15 | 16 | #include "NvInferRuntime.h" 17 | #include "extensions/tensorops/core/Tensor.h" 18 | #include "Errors.h" 19 | 20 | namespace cvcore { 21 | namespace inferencer { 22 | 23 | /* 24 | * Maps tensorrt datatype to cvcore Channel type. 25 | * @param channelType cvcore channel type. 26 | * @param dtype tensorrt datatype 27 | * return error code 28 | */ 29 | std::error_code getCVCoreChannelTypeFromTensorRT( 30 | cvcore::tensor_ops::ChannelType& channelType, 31 | nvinfer1::DataType dtype); 32 | 33 | /* 34 | * Maps tensorrt datatype to cvcore Channel type. 35 | * @param channelLayout cvcore channel type. 36 | * @param dtype tensorrt layout 37 | * return error code 38 | */ 39 | std::error_code getCVCoreChannelLayoutFromTensorRT( 40 | cvcore::tensor_ops::TensorLayout& channelLayout, 41 | nvinfer1::TensorFormat tensorFormat); 42 | 43 | } // namespace inferencer 44 | } // namespace cvcore 45 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/TritonGrpcInferencer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021-2023, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | * property and proprietary rights in and to this material, related 6 | * documentation and any modifications thereto. Any use, reproduction, 7 | * disclosure or distribution of this material and related documentation 8 | * without an express license agreement from NVIDIA CORPORATION or 9 | * its affiliates is strictly prohibited. 10 | * 11 | * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES 12 | * SPDX-License-Identifier: LicenseRef-NvidiaProprietary 13 | */ 14 | #pragma once 15 | #ifdef ENABLE_TRITON 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "cv/inferencer/Errors.h" 23 | #include "cv/inferencer/IInferenceBackend.h" 24 | #include "cv/inferencer/Inferencer.h" 25 | #include "grpc_client.h" 26 | 27 | namespace cvcore { 28 | namespace inferencer { 29 | namespace tc = triton::client; 30 | 31 | class TritonGrpcInferencer : public IInferenceBackendClient { 32 | public: 33 | TritonGrpcInferencer(const TritonRemoteInferenceParams& params); 34 | 35 | // Set input layer tensor 36 | std::error_code setInput(const cvcore::TensorBase& trtInputBuffer, 37 | std::string inputLayerName) override; 38 | // Sets output layer tensor 39 | std::error_code setOutput(cvcore::TensorBase& trtOutputBuffer, 40 | std::string outputLayerName) override; 41 | 42 | // Get the model metadata parsed based on the model file 43 | // This would be done in initialize call itself. User can access the modelMetaData 44 | // created using this API. 45 | ModelMetaData getModelMetaData() const override; 46 | 47 | // Triton will use infer and TensorRT would use enqueueV2 48 | std::error_code infer(size_t batchSize = 1) override; 49 | 50 | // Applicable only for Native TRT 51 | std::error_code setCudaStream(cudaStream_t) override; 52 | 53 | // Unregister shared memory for layer 54 | std::error_code unregister(std::string layerName) override; 55 | 56 | // Unregister all shared memory 57 | std::error_code unregister() override; 58 | 59 | private: 60 | // Parse grpc model 61 | std::error_code ParseGrpcModel(); 62 | std::unique_ptr client; 63 | ModelMetaData modelInfo; 64 | std::vector> inputRequests; 65 | std::vector> outputRequests; 66 | std::vector inputMap; 67 | std::vector inputMapHistory; 68 | std::vector outputMapHistory; 69 | std::vector outputMap; 70 | std::string modelVersion, modelName; 71 | }; 72 | 73 | } // namespace inferencer 74 | } // namespace cvcore 75 | #endif 76 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/TritonUtils.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #ifdef ENABLE_TRITON 18 | 19 | #include 20 | #include 21 | 22 | #include "gems/dnn_inferencer/inferencer/TritonUtils.h" 23 | 24 | namespace cvcore { 25 | namespace inferencer { 26 | using ChannelType = cvcore::tensor_ops::ChannelType; 27 | bool getCVCoreChannelType(ChannelType& channelType, std::string dtype) { 28 | if (dtype.compare("UINT8") == 0) { 29 | channelType = ChannelType::U8; 30 | } else if (dtype.compare("UINT16") == 0) { 31 | channelType = ChannelType::U16; 32 | } else if (dtype.compare("FP16") == 0) { 33 | channelType = ChannelType::F16; 34 | } else if (dtype.compare("FP32") == 0) { 35 | channelType = ChannelType::F32; 36 | } else if (dtype.compare("FP64") == 0) { 37 | channelType = ChannelType::F64; 38 | } else { 39 | return false; 40 | } 41 | 42 | return true; 43 | } 44 | 45 | bool getTritonChannelType(std::string& dtype, ChannelType channelType) { 46 | if (channelType == ChannelType::U8) { 47 | dtype = "UINT8"; 48 | } else if (channelType == ChannelType::U16) { 49 | dtype = "UINT16"; 50 | } else if (channelType == ChannelType::F16) { 51 | dtype = "FP16"; 52 | } else if (channelType == ChannelType::F32) { 53 | dtype = "FP32"; 54 | } else if (channelType == ChannelType::F64) { 55 | dtype = "FP64"; 56 | } else { 57 | return false; 58 | } 59 | 60 | return true; 61 | } 62 | 63 | } // namespace inferencer 64 | } // namespace cvcore 65 | #endif 66 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/dnn_inferencer/inferencer/TritonUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2021-2023, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION, its affiliates and licensors retain all intellectual 5 | * property and proprietary rights in and to this material, related 6 | * documentation and any modifications thereto. Any use, reproduction, 7 | * disclosure or distribution of this material and related documentation 8 | * without an express license agreement from NVIDIA CORPORATION or 9 | * its affiliates is strictly prohibited. 10 | * 11 | * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES 12 | * SPDX-License-Identifier: LicenseRef-NvidiaProprietary 13 | */ 14 | 15 | #pragma once 16 | 17 | #ifdef ENABLE_TRITON 18 | #include 19 | 20 | #include "grpc_client.h" 21 | 22 | #include "cv/core/Tensor.h" 23 | #include "cv/inferencer/Errors.h" 24 | 25 | namespace cvcore { 26 | namespace inferencer { 27 | 28 | /* 29 | * Maps triton datatype to cvcore Channel type. 30 | * @param channelType cvcore channel type. 31 | * @param dtype String representing triton datatype 32 | * return bool returns false if mapping was not successful. 33 | */ 34 | bool getCVCoreChannelType(cvcore::ChannelType& channelType, std::string dtype); 35 | 36 | /* 37 | * Maps triton datatype to cvcore Channel type. 38 | * @param dtype String representing triton datatype 39 | * @param channelType cvcore channel type. 40 | * return bool returns false if mapping was not successful. 41 | */ 42 | bool getTritonChannelType(std::string& dtype, cvcore::ChannelType channelType); 43 | 44 | } // namespace inferencer 45 | } // namespace cvcore 46 | #endif // ENABLE_TRITON 47 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/hash/hash_file.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include 19 | #include // NOLINT(build/include_order) 20 | 21 | #include "common/fixed_vector.hpp" 22 | #include "common/span.hpp" 23 | #include "gems/gxf_helpers/expected_macro_gxf.hpp" 24 | #include "gems/hash/hash_file.hpp" 25 | 26 | namespace nvidia { 27 | namespace isaac { 28 | 29 | namespace { 30 | 31 | constexpr size_t kBlockSize = 8096; 32 | 33 | } // namespace 34 | 35 | // helper to get the hash of a file 36 | gxf::Expected hash_file(const char* path) { 37 | std::ifstream file; 38 | SHA256 hash; 39 | FixedVector buffer; 40 | 41 | size_t bytes_remaining = std::filesystem::file_size(path); 42 | 43 | file.open(path, std::ios_base::in | std::ios_base::binary); 44 | 45 | if (!file.good()) { 46 | return nvidia::gxf::Unexpected{GXF_FAILURE}; 47 | } 48 | 49 | while (true) { 50 | size_t current_bytes = std::min(kBlockSize, bytes_remaining); 51 | 52 | file.read(reinterpret_cast(buffer.data()), current_bytes); 53 | 54 | if (!file.good()) { 55 | return nvidia::gxf::Unexpected{GXF_FAILURE}; 56 | } 57 | 58 | RETURN_IF_ERROR(hash.hashData(Span(buffer.data(), current_bytes))); 59 | 60 | bytes_remaining -= current_bytes; 61 | 62 | if (bytes_remaining == 0) { 63 | RETURN_IF_ERROR(hash.finalize()); 64 | 65 | return hash.toString(); 66 | } 67 | } 68 | } 69 | 70 | } // namespace isaac 71 | } // namespace nvidia 72 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/hash/hash_file.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #pragma once 18 | 19 | #include "gems/hash/sha256.hpp" 20 | 21 | namespace nvidia { 22 | namespace isaac { 23 | 24 | // helper to get the hash of a file 25 | gxf::Expected hash_file(const char* path); 26 | 27 | } // namespace isaac 28 | } // namespace nvidia 29 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/hash/sha256.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #include "gems/hash/sha256.hpp" 18 | 19 | #include 20 | 21 | namespace nvidia { 22 | namespace isaac { 23 | 24 | namespace { 25 | 26 | constexpr uint32_t K[64] = { 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, 27 | 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, 28 | 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, 29 | 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, 30 | 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, 31 | 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, 32 | 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, 33 | 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, 34 | 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, 35 | 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, 36 | 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, 37 | 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, 38 | 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, 39 | 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, 40 | 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, 41 | 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 }; 42 | 43 | #define DBL_INT_ADD(a, b, c) \ 44 | if ((a) > 0xFFFFFFFF - (c)) { \ 45 | ++(b); \ 46 | } \ 47 | (a) += (c); 48 | 49 | #define ROTLEFT(a, b) (((a) << (b)) | ((a) >> (32 - (b)))) 50 | #define ROTRIGHT(a, b) (((a) >> (b)) | ((a) << (32 - (b)))) 51 | 52 | #define CH(x, y, z) (((x) & (y)) ^ (~(x) & (z))) 53 | #define MAJ(x, y, z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z))) 54 | 55 | #define EP0(x) (ROTRIGHT((x), 2) ^ ROTRIGHT((x), 13) ^ ROTRIGHT((x), 22)) 56 | #define EP1(x) (ROTRIGHT((x), 6) ^ ROTRIGHT((x), 11) ^ ROTRIGHT((x), 25)) 57 | #define SIG0(x) (ROTRIGHT((x), 7) ^ ROTRIGHT((x), 18) ^ ((x) >> 3)) 58 | #define SIG1(x) (ROTRIGHT((x), 17) ^ ROTRIGHT((x), 19) ^ ((x) >> 10)) 59 | 60 | } // namespace 61 | 62 | gxf::Expected SHA256::Hash(const Span data) { 63 | SHA256 hasher; 64 | return hasher.hashData(data) 65 | .and_then([&]() { return hasher.finalize(); }) 66 | .substitute(hasher.hash()); 67 | } 68 | 69 | gxf::Expected SHA256::Hash(const Span data) { 70 | auto span = Span(reinterpret_cast(data.data()), data.size()); 71 | return Hash(span).map(ToString); 72 | } 73 | 74 | void SHA256::Initialize(SHA256_CTX& ctx) { 75 | ctx.datalen = 0; 76 | ctx.bitlen[0] = 0; 77 | ctx.bitlen[1] = 0; 78 | ctx.state[0] = 0x6a09e667; 79 | ctx.state[1] = 0xbb67ae85; 80 | ctx.state[2] = 0x3c6ef372; 81 | ctx.state[3] = 0xa54ff53a; 82 | ctx.state[4] = 0x510e527f; 83 | ctx.state[5] = 0x9b05688c; 84 | ctx.state[6] = 0x1f83d9ab; 85 | ctx.state[7] = 0x5be0cd19; 86 | } 87 | 88 | gxf::Expected SHA256::Update(SHA256_CTX& ctx, const Span data) { 89 | for (auto element : data) { 90 | if (!element) { 91 | return gxf::Unexpected{GXF_FAILURE}; 92 | } 93 | ctx.data[ctx.datalen] = element.value(); 94 | ctx.datalen++; 95 | if (ctx.datalen == 64) { 96 | auto result = Transform(ctx, Span(ctx.data)); 97 | if (!result) { 98 | return gxf::ForwardError(result); 99 | } 100 | DBL_INT_ADD(ctx.bitlen[0], ctx.bitlen[1], 512); 101 | ctx.datalen = 0; 102 | } 103 | } 104 | return gxf::Success; 105 | } 106 | 107 | gxf::Expected SHA256::Transform(SHA256_CTX& ctx, const Span data) { 108 | uint32_t m[64]; 109 | uint32_t i = 0; 110 | uint32_t j = 0; 111 | for (; i < 16; i++) { 112 | if (!data[j] || !data[j + 1] || !data[j + 2] || !data[j + 3]) { 113 | return gxf::Unexpected{GXF_EXCEEDING_PREALLOCATED_SIZE}; 114 | } 115 | m[i] = (static_cast(data[j].value()) << 24 & 0xFF000000) 116 | | (static_cast(data[j + 1].value()) << 16 & 0x00FF0000) 117 | | (static_cast(data[j + 2].value()) << 8 & 0x0000FF00) 118 | | (static_cast(data[j + 3].value()) << 0 & 0x000000FF); 119 | j += 4; 120 | } 121 | for (; i < 64; i++) { 122 | m[i] = SIG1(m[i - 2]) + m[i - 7] + SIG0(m[i - 15]) + m[i - 16]; 123 | } 124 | 125 | uint32_t a = ctx.state[0]; 126 | uint32_t b = ctx.state[1]; 127 | uint32_t c = ctx.state[2]; 128 | uint32_t d = ctx.state[3]; 129 | uint32_t e = ctx.state[4]; 130 | uint32_t f = ctx.state[5]; 131 | uint32_t g = ctx.state[6]; 132 | uint32_t h = ctx.state[7]; 133 | uint32_t t1; 134 | uint32_t t2; 135 | for (i = 0; i < 64; ++i) { 136 | t1 = h + EP1(e) + CH(e, f, g) + K[i] + m[i]; 137 | t2 = EP0(a) + MAJ(a, b, c); 138 | h = g; 139 | g = f; 140 | f = e; 141 | e = d + t1; 142 | d = c; 143 | c = b; 144 | b = a; 145 | a = t1 + t2; 146 | } 147 | 148 | ctx.state[0] += a; 149 | ctx.state[1] += b; 150 | ctx.state[2] += c; 151 | ctx.state[3] += d; 152 | ctx.state[4] += e; 153 | ctx.state[5] += f; 154 | ctx.state[6] += g; 155 | ctx.state[7] += h; 156 | 157 | return gxf::Success; 158 | } 159 | 160 | gxf::Expected SHA256::Finalize(SHA256_CTX& ctx) { 161 | uint32_t i = ctx.datalen; 162 | 163 | if (ctx.datalen < 56) { 164 | ctx.data[i] = 0x80; 165 | i++; 166 | 167 | while (i < 56) { 168 | ctx.data[i] = 0x00; 169 | i++; 170 | } 171 | } else { 172 | ctx.data[i] = 0x80; 173 | i++; 174 | 175 | while (i < 64) { 176 | ctx.data[i] = 0x00; 177 | i++; 178 | } 179 | 180 | auto result = Transform(ctx, Span(ctx.data)); 181 | if (!result) { 182 | return gxf::ForwardError(result); 183 | } 184 | std::memset(ctx.data, 0, 56); 185 | } 186 | 187 | DBL_INT_ADD(ctx.bitlen[0], ctx.bitlen[1], ctx.datalen * 8); 188 | ctx.data[63] = static_cast(ctx.bitlen[0] >> 0 & 0x000000FF); 189 | ctx.data[62] = static_cast(ctx.bitlen[0] >> 8 & 0x000000FF); 190 | ctx.data[61] = static_cast(ctx.bitlen[0] >> 16 & 0x000000FF); 191 | ctx.data[60] = static_cast(ctx.bitlen[0] >> 24 & 0x000000FF); 192 | ctx.data[59] = static_cast(ctx.bitlen[1] >> 0 & 0x000000FF); 193 | ctx.data[58] = static_cast(ctx.bitlen[1] >> 8 & 0x000000FF); 194 | ctx.data[57] = static_cast(ctx.bitlen[1] >> 16 & 0x000000FF); 195 | ctx.data[56] = static_cast(ctx.bitlen[1] >> 24 & 0x000000FF); 196 | 197 | auto result = Transform(ctx, Span(ctx.data)); 198 | if (!result) { 199 | return gxf::ForwardError(result); 200 | } 201 | 202 | Result hash; 203 | for (i = 0; i < 4; ++i) { 204 | hash[i] = static_cast(ctx.state[0] >> (24 - i * 8) & 0x000000FF); 205 | hash[i + 4] = static_cast(ctx.state[1] >> (24 - i * 8) & 0x000000FF); 206 | hash[i + 8] = static_cast(ctx.state[2] >> (24 - i * 8) & 0x000000FF); 207 | hash[i + 12] = static_cast(ctx.state[3] >> (24 - i * 8) & 0x000000FF); 208 | hash[i + 16] = static_cast(ctx.state[4] >> (24 - i * 8) & 0x000000FF); 209 | hash[i + 20] = static_cast(ctx.state[5] >> (24 - i * 8) & 0x000000FF); 210 | hash[i + 24] = static_cast(ctx.state[6] >> (24 - i * 8) & 0x000000FF); 211 | hash[i + 28] = static_cast(ctx.state[7] >> (24 - i * 8) & 0x000000FF); 212 | } 213 | 214 | return hash; 215 | } 216 | 217 | gxf::Expected SHA256::ToString(const Result& hash) { 218 | constexpr char INT2HEX[] = { '0', '1', '2', '3', '4', '5', '6', '7', 219 | '8', '9', 'a', 'b', 'c', 'd', 'e', 'f' }; 220 | 221 | String text; 222 | for (auto&& value : hash) { 223 | const uint8_t upper = static_cast(value >> 4 & 0x0F); 224 | const uint8_t lower = static_cast(value >> 0 & 0x0F); 225 | 226 | auto result = text.append(INT2HEX[upper]) 227 | .and_then([&]() { return text.append(INT2HEX[lower]); }); 228 | if (!result) { 229 | return gxf::Unexpected{GXF_EXCEEDING_PREALLOCATED_SIZE}; 230 | } 231 | } 232 | 233 | return text; 234 | } 235 | 236 | } // namespace isaac 237 | } // namespace nvidia 238 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/gxf/gems/hash/sha256.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | #pragma once 18 | 19 | #include 20 | 21 | #include "common/fixed_string.hpp" 22 | #include "common/span.hpp" 23 | #include "gxf/core/expected.hpp" 24 | 25 | namespace nvidia { 26 | namespace isaac { 27 | 28 | /// The SHA256 class Generates the SHA256 sum of a given string 29 | /// Adapted from https://www.programmingalgorithms.com/algorithm/sha256 30 | class SHA256 { 31 | public: 32 | static constexpr size_t HASH_LENGTH = 32; 33 | using Result = std::array; 34 | using String = FixedString; 35 | 36 | static gxf::Expected Hash(const Span data); 37 | static gxf::Expected Hash(const Span data); 38 | 39 | SHA256() : context_{}, hash_{} { Initialize(context_); } 40 | 41 | /// Reset hasher to be fed again 42 | void reset() { Initialize(context_); } 43 | 44 | /// Hash a given array 45 | gxf::Expected hashData(const Span data) { return Update(context_, data); } 46 | 47 | /// Finalize computation of the hash, i.e. make solution available through `hash()` 48 | gxf::Expected finalize() { 49 | return Finalize(context_) 50 | .map([&](Result result) { 51 | hash_ = result; 52 | return gxf::Success; 53 | }); 54 | } 55 | 56 | /// Return hashed result 57 | const Result& hash() const { return hash_; } 58 | /// Return base64 encoding of the hash 59 | gxf::Expected toString() const { return ToString(hash_); } 60 | 61 | private: 62 | struct SHA256_CTX { 63 | uint8_t data[64]; 64 | uint32_t datalen; 65 | uint32_t bitlen[2]; 66 | uint32_t state[8]; 67 | }; 68 | 69 | static void Initialize(SHA256_CTX& ctx); 70 | static gxf::Expected Update(SHA256_CTX& ctx, const Span data); 71 | static gxf::Expected Transform(SHA256_CTX& ctx, const Span data); 72 | static gxf::Expected Finalize(SHA256_CTX& ctx); 73 | static gxf::Expected ToString(const Result& hash); 74 | 75 | SHA256_CTX context_; 76 | Result hash_; 77 | }; 78 | 79 | } // namespace isaac 80 | } // namespace nvidia 81 | -------------------------------------------------------------------------------- /gxf_isaac_bi3d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 20 | 21 | 22 | 23 | gxf_isaac_bi3d 24 | 3.2.5 25 | GXF extension containing Bi3D related components. 26 | 27 | Isaac ROS Maintainers 28 | Apache-2.0 29 | https://developer.nvidia.com/isaac-ros-gems/ 30 | CY Chen 31 | 32 | ament_cmake_auto 33 | 34 | isaac_ros_common 35 | isaac_ros_gxf 36 | gxf_isaac_messages 37 | gxf_isaac_tensorops 38 | 39 | gxf_isaac_gems 40 | 41 | ament_lint_auto 42 | ament_lint_common 43 | 44 | 45 | ament_cmake 46 | 47 | 48 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | cmake_minimum_required(VERSION 3.22.1) 19 | project(isaac_ros_bi3d LANGUAGES C CXX) 20 | 21 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 22 | add_compile_options(-Wall -Wextra -Wpedantic) 23 | endif() 24 | 25 | find_package(ament_cmake_auto REQUIRED) 26 | ament_auto_find_build_dependencies() 27 | 28 | # Bi3DNode 29 | ament_auto_add_library(bi3d_node SHARED src/bi3d_node.cpp) 30 | rclcpp_components_register_nodes(bi3d_node "nvidia::isaac_ros::bi3d::Bi3DNode") 31 | set(node_plugins "${node_plugins}nvidia::isaac_ros::bi3d::Bi3DNode;$\n") 32 | set_target_properties(bi3d_node PROPERTIES 33 | BUILD_WITH_INSTALL_RPATH TRUE 34 | BUILD_RPATH_USE_ORIGIN TRUE 35 | INSTALL_RPATH_USE_LINK_PATH TRUE) 36 | 37 | install(PROGRAMS scripts/isaac_ros_bi3d_visualizer.py DESTINATION lib/${PROJECT_NAME}) 38 | 39 | if(BUILD_TESTING) 40 | find_package(ament_lint_auto REQUIRED) 41 | ament_lint_auto_find_test_dependencies() 42 | 43 | # Gtest for bi3d_node 44 | ament_add_gtest(bi3d_node_test test/bi3d_node_test.cpp) 45 | target_link_libraries(bi3d_node_test bi3d_node) 46 | target_include_directories(bi3d_node_test PUBLIC include/isaac_ros_bi3d/) 47 | target_include_directories(bi3d_node_test PUBLIC /usr/src/googletest/googlemock/include/) 48 | ament_target_dependencies(bi3d_node_test rclcpp) 49 | ament_target_dependencies(bi3d_node_test isaac_ros_nitros) 50 | 51 | # The FindPythonInterp and FindPythonLibs modules are removed 52 | if(POLICY CMP0148) 53 | cmake_policy(SET CMP0148 OLD) 54 | endif() 55 | 56 | find_package(launch_testing_ament_cmake REQUIRED) 57 | add_launch_test(test/isaac_ros_bi3d_test.py TIMEOUT "1000") 58 | endif() 59 | 60 | 61 | # Embed versioning information into installed files 62 | ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common) 63 | include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake") 64 | generate_version_info(${PROJECT_NAME}) 65 | 66 | ament_auto_package(INSTALL_TO_SHARE launch config) 67 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/config/isaac_ros_bi3d_zed.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 466 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: PointCloud2 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Alpha: 1 51 | Autocompute Intensity Bounds: true 52 | Autocompute Value Bounds: 53 | Max Value: 10 54 | Min Value: -10 55 | Value: true 56 | Axis: Z 57 | Channel Name: intensity 58 | Class: rviz_default_plugins/PointCloud2 59 | Color: 255; 255; 255 60 | Color Transformer: RGB8 61 | Decay Time: 0 62 | Enabled: true 63 | Invert Rainbow: false 64 | Max Color: 255; 255; 255 65 | Max Intensity: 4096 66 | Min Color: 0; 0; 0 67 | Min Intensity: 0 68 | Name: PointCloud2 69 | Position Transformer: XYZ 70 | Selectable: true 71 | Size (Pixels): 3 72 | Size (m): 0.009999999776482582 73 | Style: Flat Squares 74 | Topic: 75 | Depth: 5 76 | Durability Policy: Volatile 77 | Filter size: 10 78 | History Policy: Keep Last 79 | Reliability Policy: Reliable 80 | Value: /points2 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: true 84 | - Class: rviz_default_plugins/Image 85 | Enabled: true 86 | Max Value: 1 87 | Median window: 5 88 | Min Value: 0 89 | Name: Image 90 | Normalize Range: true 91 | Topic: 92 | Depth: 5 93 | Durability Policy: Volatile 94 | History Policy: Keep Last 95 | Reliability Policy: Reliable 96 | Value: /zed_node/left/image_rect_color_rgb 97 | Value: true 98 | - Class: rviz_default_plugins/Image 99 | Enabled: true 100 | Max Value: 1 101 | Median window: 5 102 | Min Value: 0 103 | Name: Image 104 | Normalize Range: true 105 | Topic: 106 | Depth: 5 107 | Durability Policy: Volatile 108 | History Policy: Keep Last 109 | Reliability Policy: Reliable 110 | Value: /zed_node/right/image_rect_color_rgb 111 | Value: true 112 | Enabled: true 113 | Global Options: 114 | Background Color: 48; 48; 48 115 | Fixed Frame: zed2i_left_camera_optical_frame 116 | Frame Rate: 30 117 | Name: root 118 | Tools: 119 | - Class: rviz_default_plugins/Interact 120 | Hide Inactive Objects: true 121 | - Class: rviz_default_plugins/MoveCamera 122 | - Class: rviz_default_plugins/Select 123 | - Class: rviz_default_plugins/FocusCamera 124 | - Class: rviz_default_plugins/Measure 125 | Line color: 128; 128; 0 126 | - Class: rviz_default_plugins/SetInitialPose 127 | Covariance x: 0.25 128 | Covariance y: 0.25 129 | Covariance yaw: 0.06853891909122467 130 | Topic: 131 | Depth: 5 132 | Durability Policy: Volatile 133 | History Policy: Keep Last 134 | Reliability Policy: Reliable 135 | Value: /initialpose 136 | - Class: rviz_default_plugins/SetGoal 137 | Topic: 138 | Depth: 5 139 | Durability Policy: Volatile 140 | History Policy: Keep Last 141 | Reliability Policy: Reliable 142 | Value: /goal_pose 143 | - Class: rviz_default_plugins/PublishPoint 144 | Single click: true 145 | Topic: 146 | Depth: 5 147 | Durability Policy: Volatile 148 | History Policy: Keep Last 149 | Reliability Policy: Reliable 150 | Value: /clicked_point 151 | Transformation: 152 | Current: 153 | Class: rviz_default_plugins/TF 154 | Value: true 155 | Views: 156 | Current: 157 | Class: rviz_default_plugins/Orbit 158 | Distance: 14.81318187713623 159 | Enable Stereo Rendering: 160 | Stereo Eye Separation: 0.05999999865889549 161 | Stereo Focal Distance: 1 162 | Swap Stereo Eyes: false 163 | Value: false 164 | Focal Point: 165 | X: -0.536247968673706 166 | Y: 0.23787975311279297 167 | Z: 1.2995383739471436 168 | Focal Shape Fixed Size: true 169 | Focal Shape Size: 0.05000000074505806 170 | Invert Z Axis: false 171 | Name: Current View 172 | Near Clip Distance: 0.009999999776482582 173 | Pitch: -0.7852040529251099 174 | Target Frame: 175 | Value: Orbit (rviz) 176 | Yaw: 4.648578643798828 177 | Saved: ~ 178 | Window Geometry: 179 | Displays: 180 | collapsed: false 181 | Height: 1136 182 | Hide Left Dock: false 183 | Hide Right Dock: false 184 | Image: 185 | collapsed: false 186 | QMainWindow State: 000000ff00000000fd0000000400000000000003b1000003d6fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b0000025b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003b000001e70000002800fffffffb0000000a0049006d0061006700650100000228000001e90000002800ffffff000000010000010f000003d6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003d6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d0065010000000000000450000000000000000000000381000003d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 187 | Selection: 188 | collapsed: false 189 | Time: 190 | collapsed: false 191 | Tool Properties: 192 | collapsed: false 193 | Views: 194 | collapsed: false 195 | Width: 1848 196 | X: 72 197 | Y: 27 198 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/config/realsense.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | device_type: '' 20 | serial_no: '' 21 | usb_port_id: '' 22 | 23 | rgb_camera: 24 | profile: '640x480x15' 25 | color_qos: "SENSOR_DATA" 26 | 27 | depth_module: 28 | profile: '640x480x60' 29 | emitter_enabled: 0 30 | emitter_on_off: false 31 | depth_qos: "SENSOR_DATA" 32 | depth_info_qos: "SYSTEM_DEFAULT" 33 | 34 | infra_qos: "SENSOR_DATA" 35 | 36 | enable_accel: false 37 | enable_color: false 38 | enable_depth: false 39 | enable_gyro: false 40 | enable_infra1: true 41 | enable_infra2: true 42 | 43 | pointcloud: 44 | enable: false 45 | 46 | pointcloud_texture_index: 0 47 | pointcloud_texture_stream: RS2_STREAM_ANY 48 | 49 | enable_sync: false 50 | align_depth: false 51 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/include/isaac_ros_bi3d/bi3d_node.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #ifndef ISAAC_ROS_BI3D__BI3D_NODE_HPP_ 19 | #define ISAAC_ROS_BI3D__BI3D_NODE_HPP_ 20 | 21 | #include 22 | #include 23 | 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "rclcpp/parameter_event_handler.hpp" 26 | #include "isaac_ros_nitros/nitros_node.hpp" 27 | 28 | namespace nvidia 29 | { 30 | namespace isaac_ros 31 | { 32 | namespace bi3d 33 | { 34 | 35 | class Bi3DNode : public nitros::NitrosNode 36 | { 37 | public: 38 | explicit Bi3DNode(const rclcpp::NodeOptions &); 39 | 40 | ~Bi3DNode(); 41 | 42 | Bi3DNode(const Bi3DNode &) = delete; 43 | 44 | Bi3DNode & operator=(const Bi3DNode &) = delete; 45 | 46 | // The callback to be implemented by users for any required initialization 47 | void preLoadGraphCallback() override; 48 | void postLoadGraphCallback() override; 49 | 50 | // Callback for setting correct VideoBuffer name 51 | void Bi3DVideoBufferNameCallback( 52 | const gxf_context_t, nitros::NitrosTypeBase &, std::string name, bool pub_disp_values); 53 | 54 | private: 55 | // Image parameters 56 | const uint16_t image_height_; 57 | const uint16_t image_width_; 58 | 59 | // Bi3D model input paramters 60 | const std::string featnet_engine_file_path_; 61 | const std::vector featnet_input_layers_name_; 62 | const std::vector featnet_output_layers_name_; 63 | 64 | const std::string segnet_engine_file_path_; 65 | const std::vector segnet_input_layers_name_; 66 | const std::vector segnet_output_layers_name_; 67 | 68 | // Bi3D extra parameters 69 | int64_t max_disparity_values_; 70 | std::vector disparity_values_; 71 | 72 | // Dynamic parameter callbacks and handles 73 | OnSetParametersCallbackHandle::SharedPtr on_set_param_cb_handle_{nullptr}; 74 | }; 75 | 76 | } // namespace bi3d 77 | } // namespace isaac_ros 78 | } // namespace nvidia 79 | 80 | #endif // ISAAC_ROS_BI3D__BI3D_NODE_HPP_ 81 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/isaac_ros_bi3d/__init__.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2023-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/launch/isaac_ros_bi3d.launch.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import launch 19 | from launch.actions import DeclareLaunchArgument 20 | from launch.substitutions import LaunchConfiguration 21 | from launch_ros.actions import ComposableNodeContainer 22 | from launch_ros.descriptions import ComposableNode 23 | 24 | 25 | def generate_launch_description(): 26 | launch_args = [ 27 | DeclareLaunchArgument( 28 | 'image_height', 29 | default_value='576', 30 | description='The height of the input image'), 31 | DeclareLaunchArgument( 32 | 'image_width', 33 | default_value='960', 34 | description='The width of the input image'), 35 | DeclareLaunchArgument( 36 | 'featnet_engine_file_path', 37 | default_value='', 38 | description='The absolute path to the Bi3D Featnet TensorRT engine plan'), 39 | DeclareLaunchArgument( 40 | 'segnet_engine_file_path', 41 | default_value='', 42 | description='The absolute path to the Bi3D Segnet TensorRT engine plan'), 43 | DeclareLaunchArgument( 44 | 'max_disparity_values', 45 | default_value='64', 46 | description='The maximum number of disparity values given for Bi3D inference'), 47 | ] 48 | 49 | # Bi3DNode parameters 50 | image_height = LaunchConfiguration('image_height') 51 | image_width = LaunchConfiguration('image_width') 52 | featnet_engine_file_path = LaunchConfiguration('featnet_engine_file_path') 53 | segnet_engine_file_path = LaunchConfiguration('segnet_engine_file_path') 54 | max_disparity_values = LaunchConfiguration('max_disparity_values') 55 | 56 | bi3d_node = ComposableNode( 57 | name='bi3d_node', 58 | package='isaac_ros_bi3d', 59 | plugin='nvidia::isaac_ros::bi3d::Bi3DNode', 60 | parameters=[{ 61 | 'image_height': image_height, 62 | 'image_width': image_width, 63 | 'featnet_engine_file_path': featnet_engine_file_path, 64 | 'segnet_engine_file_path': segnet_engine_file_path, 65 | 'max_disparity_values': max_disparity_values}], 66 | remappings=[('left_image_bi3d', 'rgb_left'), 67 | ('right_image_bi3d', 'rgb_right'), 68 | ('left_camera_info_bi3d', 'camera_info_left'), 69 | ('right_camera_info_bi3d', 'camera_info_right')] 70 | ) 71 | 72 | container = ComposableNodeContainer( 73 | name='bi3d_container', 74 | namespace='bi3d', 75 | package='rclcpp_components', 76 | executable='component_container_mt', 77 | composable_node_descriptions=[bi3d_node], 78 | output='screen' 79 | ) 80 | 81 | final_launch_description = launch_args + [container] 82 | return (launch.LaunchDescription(final_launch_description)) 83 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/launch/isaac_ros_bi3d_core.launch.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | from typing import Any, Dict 19 | 20 | from isaac_ros_examples import IsaacROSLaunchFragment 21 | import launch 22 | from launch.actions import DeclareLaunchArgument 23 | from launch.substitutions import LaunchConfiguration 24 | from launch_ros.actions import ComposableNodeContainer 25 | from launch_ros.descriptions import ComposableNode 26 | 27 | 28 | class IsaacROSBi3DLaunchFragment(IsaacROSLaunchFragment): 29 | 30 | @staticmethod 31 | def get_composable_nodes(interface_specs: Dict[str, Any]) -> Dict[str, ComposableNode]: 32 | 33 | # Bi3DNode parameters 34 | featnet_engine_file_path = LaunchConfiguration('featnet_engine_file_path') 35 | segnet_engine_file_path = LaunchConfiguration('segnet_engine_file_path') 36 | max_disparity_values = LaunchConfiguration('max_disparity_values') 37 | 38 | return { 39 | 'bi3d_node': ComposableNode( 40 | name='bi3d_node', 41 | package='isaac_ros_bi3d', 42 | plugin='nvidia::isaac_ros::bi3d::Bi3DNode', 43 | parameters=[{ 44 | 'image_height': interface_specs['image_resolution']['height'], 45 | 'image_width': interface_specs['image_resolution']['width'], 46 | 'featnet_engine_file_path': featnet_engine_file_path, 47 | 'segnet_engine_file_path': segnet_engine_file_path, 48 | 'max_disparity_values': max_disparity_values}], 49 | remappings=[('left_image_bi3d', 'left/image_rect'), 50 | ('right_image_bi3d', 'right/image_rect'), 51 | ('left_camera_info_bi3d', 'left/camera_info_rect'), 52 | ('right_camera_info_bi3d', 'right/camera_info_rect')] 53 | ) 54 | } 55 | 56 | def get_launch_actions(interface_specs: Dict[str, Any]) -> \ 57 | Dict[str, launch.actions.OpaqueFunction]: 58 | return { 59 | 'featnet_engine_file_path': DeclareLaunchArgument( 60 | 'featnet_engine_file_path', 61 | default_value='', 62 | description='The absolute path to the Bi3D Featnet TensorRT engine plan' 63 | ), 64 | 'segnet_engine_file_path': DeclareLaunchArgument( 65 | 'segnet_engine_file_path', 66 | default_value='', 67 | description='The absolute path to the Bi3D Segnet TensorRT engine plan' 68 | ), 69 | 'max_disparity_values': DeclareLaunchArgument( 70 | 'max_disparity_values', 71 | default_value='64', 72 | description='The maximum number of disparity values given for Bi3D inference' 73 | ), 74 | } 75 | 76 | def generate_launch_description(): 77 | bi3d_container = ComposableNodeContainer( 78 | package='rclcpp_components', 79 | name='bi3d_container', 80 | namespace='', 81 | executable='component_container_mt', 82 | composable_node_descriptions=IsaacROSBi3DLaunchFragment 83 | .get_composable_nodes().values(), 84 | output='screen' 85 | ) 86 | 87 | return launch.LaunchDescription( 88 | [bi3d_container] + IsaacROSBi3DLaunchFragment.get_launch_actions().values()) 89 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/launch/isaac_ros_bi3d_hawk.launch.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2021-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import launch 19 | from launch.actions import DeclareLaunchArgument 20 | from launch.substitutions import LaunchConfiguration 21 | from launch_ros.actions import ComposableNodeContainer 22 | from launch_ros.descriptions import ComposableNode 23 | 24 | 25 | def generate_launch_description(): 26 | launch_args = [ 27 | DeclareLaunchArgument( 28 | 'featnet_engine_file_path', 29 | default_value='', 30 | description='The absolute path to the Bi3D Featnet TensorRT engine plan'), 31 | DeclareLaunchArgument( 32 | 'segnet_engine_file_path', 33 | default_value='', 34 | description='The absolute path to the Bi3D Segnet TensorRT engine plan'), 35 | DeclareLaunchArgument( 36 | 'max_disparity_values', 37 | default_value='64', 38 | description='The maximum number of disparity values given for Bi3D inference'), 39 | DeclareLaunchArgument( 40 | 'module_id', 41 | default_value='2', 42 | description='Index specifying the stereo camera module to use.'), 43 | ] 44 | 45 | module_id = LaunchConfiguration('module_id') 46 | 47 | # Bi3DNode parameters 48 | featnet_engine_file_path = LaunchConfiguration('featnet_engine_file_path') 49 | segnet_engine_file_path = LaunchConfiguration('segnet_engine_file_path') 50 | max_disparity_values = LaunchConfiguration('max_disparity_values') 51 | 52 | argus_stereo_node = ComposableNode( 53 | name='argus_stereo', 54 | package='isaac_ros_argus_camera', 55 | plugin='nvidia::isaac_ros::argus::ArgusStereoNode', 56 | parameters=[{ 57 | 'module_id': module_id 58 | }], 59 | ) 60 | 61 | left_resize_node = ComposableNode( 62 | name='left_resize_node', 63 | package='isaac_ros_image_proc', 64 | plugin='nvidia::isaac_ros::image_proc::ResizeNode', 65 | parameters=[{ 66 | 'output_width': 960, 67 | 'output_height': 576, 68 | 'keep_aspect_ratio': True 69 | }], 70 | remappings=[ 71 | ('camera_info', '/left/camera_info'), 72 | ('image', '/left/image_raw'), 73 | ('resize/camera_info', '/left/camera_info_resize'), 74 | ('resize/image', '/left/image_resize')] 75 | ) 76 | 77 | right_resize_node = ComposableNode( 78 | name='right_resize_node', 79 | package='isaac_ros_image_proc', 80 | plugin='nvidia::isaac_ros::image_proc::ResizeNode', 81 | parameters=[{ 82 | 'output_width': 960, 83 | 'output_height': 576, 84 | 'keep_aspect_ratio': True 85 | }], 86 | remappings=[ 87 | ('camera_info', '/right/camera_info'), 88 | ('image', '/right/image_raw'), 89 | ('resize/camera_info', '/right/camera_info_resize'), 90 | ('resize/image', '/right/image_resize')] 91 | ) 92 | 93 | left_rectify_node = ComposableNode( 94 | name='left_rectify_node', 95 | package='isaac_ros_image_proc', 96 | plugin='nvidia::isaac_ros::image_proc::RectifyNode', 97 | parameters=[{ 98 | 'output_width': 960, 99 | 'output_height': 576, 100 | }], 101 | remappings=[ 102 | ('image_raw', '/left/image_resize'), 103 | ('camera_info', '/left/camera_info_resize'), 104 | ('image_rect', '/left/image_rect'), 105 | ('camera_info_rect', '/left/camera_info_rect') 106 | ] 107 | ) 108 | 109 | right_rectify_node = ComposableNode( 110 | name='right_rectify_node', 111 | package='isaac_ros_image_proc', 112 | plugin='nvidia::isaac_ros::image_proc::RectifyNode', 113 | parameters=[{ 114 | 'output_width': 960, 115 | 'output_height': 576, 116 | }], 117 | remappings=[ 118 | ('image_raw', '/right/image_resize'), 119 | ('camera_info', '/right/camera_info_resize'), 120 | ('image_rect', '/right/image_rect'), 121 | ('camera_info_rect', '/right/camera_info_rect') 122 | ] 123 | ) 124 | 125 | bi3d_node = ComposableNode( 126 | name='bi3d_node', 127 | package='isaac_ros_bi3d', 128 | plugin='nvidia::isaac_ros::bi3d::Bi3DNode', 129 | parameters=[{ 130 | 'featnet_engine_file_path': featnet_engine_file_path, 131 | 'segnet_engine_file_path': segnet_engine_file_path, 132 | 'max_disparity_values': max_disparity_values, 133 | 'image_width': 960, 134 | 'image_height': 576 135 | }], 136 | remappings=[('bi3d_node/bi3d_output', 'bi3d_mask'), 137 | ('left_image_bi3d', 'left/image_rect'), 138 | ('left_camera_info_bi3d', 'left/camera_info_rect'), 139 | ('right_image_bi3d', 'right/image_rect'), 140 | ('right_camera_info_bi3d', 'right/camera_info_rect')] 141 | ) 142 | 143 | bi3d_launch_container = ComposableNodeContainer( 144 | name='bi3d_launch_container', 145 | namespace='', 146 | package='rclcpp_components', 147 | executable='component_container', 148 | composable_node_descriptions=[ 149 | argus_stereo_node, left_resize_node, right_resize_node, 150 | left_rectify_node, right_rectify_node, bi3d_node 151 | ], 152 | output='screen' 153 | ) 154 | 155 | return (launch.LaunchDescription(launch_args + [bi3d_launch_container])) 156 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/launch/isaac_ros_bi3d_isaac_sim.launch.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import os 19 | 20 | from ament_index_python.packages import get_package_share_directory 21 | import launch 22 | from launch.actions import DeclareLaunchArgument 23 | from launch.substitutions import LaunchConfiguration 24 | from launch_ros.actions import ComposableNodeContainer, Node 25 | from launch_ros.descriptions import ComposableNode 26 | 27 | 28 | def generate_launch_description(): 29 | launch_args = [ 30 | DeclareLaunchArgument( 31 | 'featnet_engine_file_path', 32 | default_value='', 33 | description='The absolute path to the Bi3D Featnet TensorRT engine plan'), 34 | DeclareLaunchArgument( 35 | 'segnet_engine_file_path', 36 | default_value='', 37 | description='The absolute path to the Bi3D Segnet TensorRT engine plan'), 38 | DeclareLaunchArgument( 39 | 'max_disparity_values', 40 | default_value='64', 41 | description='The maximum number of disparity values given for Bi3D inference'), 42 | ] 43 | 44 | # Bi3DNode parameters 45 | featnet_engine_file_path = LaunchConfiguration('featnet_engine_file_path') 46 | segnet_engine_file_path = LaunchConfiguration('segnet_engine_file_path') 47 | max_disparity_values = LaunchConfiguration('max_disparity_values') 48 | 49 | image_resize_node_right = ComposableNode( 50 | package='isaac_ros_image_proc', 51 | plugin='nvidia::isaac_ros::image_proc::ResizeNode', 52 | name='image_resize_node_right', 53 | parameters=[{ 54 | 'output_width': 960, 55 | 'output_height': 576, 56 | 'encoding_desired': 'rgb8', 57 | }], 58 | remappings=[ 59 | ('camera_info', 'front_stereo_camera/right/camera_info'), 60 | ('image', 'front_stereo_camera/right/image_rect_color'), 61 | ('resize/camera_info', 'front_stereo_camera/right/camera_info_resize'), 62 | ('resize/image', 'front_stereo_camera/right/image_resize')] 63 | ) 64 | 65 | image_resize_node_left = ComposableNode( 66 | package='isaac_ros_image_proc', 67 | plugin='nvidia::isaac_ros::image_proc::ResizeNode', 68 | name='image_resize_node_left', 69 | parameters=[{ 70 | 'output_width': 960, 71 | 'output_height': 576, 72 | 'encoding_desired': 'rgb8', 73 | }], 74 | remappings=[ 75 | ('camera_info', 'front_stereo_camera/left/camera_info'), 76 | ('image', 'front_stereo_camera/left/image_rect_color'), 77 | ('resize/camera_info', 'front_stereo_camera/left/camera_info_resize'), 78 | ('resize/image', 'front_stereo_camera/left/image_resize')] 79 | ) 80 | 81 | bi3d_node = ComposableNode( 82 | name='bi3d_node', 83 | package='isaac_ros_bi3d', 84 | plugin='nvidia::isaac_ros::bi3d::Bi3DNode', 85 | parameters=[{ 86 | 'featnet_engine_file_path': featnet_engine_file_path, 87 | 'segnet_engine_file_path': segnet_engine_file_path, 88 | 'max_disparity_values': max_disparity_values, 89 | 'disparity_values': [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 20, 30, 40, 50, 60], 90 | 'image_width': 960, 91 | 'image_height': 576 92 | }], 93 | remappings=[('left_image_bi3d', 'front_stereo_camera/left/image_resize'), 94 | ('left_camera_info_bi3d', 95 | 'front_stereo_camera/left/camera_info_resize'), 96 | ('right_image_bi3d', 'front_stereo_camera/right/image_resize'), 97 | ('right_camera_info_bi3d', 98 | 'front_stereo_camera/right/camera_info_resize')]) 99 | 100 | pointcloud_node = ComposableNode( 101 | package='isaac_ros_stereo_image_proc', 102 | plugin='nvidia::isaac_ros::stereo_image_proc::PointCloudNode', 103 | parameters=[{ 104 | 'use_color': True, 105 | 'unit_scaling': 0.3 106 | }], 107 | remappings=[('left/image_rect_color', 'front_stereo_camera/left/image_resize'), 108 | ('left/camera_info', 109 | 'front_stereo_camera/left/camera_info_resize'), 110 | ('right/camera_info', 111 | 'front_stereo_camera/right/camera_info_resize'), 112 | ('disparity', 'bi3d_node/bi3d_output')]) 113 | 114 | container = ComposableNodeContainer( 115 | name='bi3d_container', 116 | namespace='bi3d', 117 | package='rclcpp_components', 118 | executable='component_container_mt', 119 | composable_node_descriptions=[bi3d_node, pointcloud_node, 120 | image_resize_node_left, image_resize_node_right], 121 | output='screen' 122 | ) 123 | 124 | rviz_config_path = os.path.join(get_package_share_directory( 125 | 'isaac_ros_bi3d'), 'config', 'isaac_ros_bi3d_isaac_sim.rviz') 126 | 127 | rviz_node = Node( 128 | package='rviz2', 129 | executable='rviz2', 130 | arguments=['-d', rviz_config_path], 131 | output='screen') 132 | 133 | final_launch_description = launch_args + [container, rviz_node] 134 | return (launch.LaunchDescription(final_launch_description)) 135 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/launch/isaac_ros_bi3d_realsense.launch.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import os 19 | 20 | from ament_index_python.packages import get_package_share_directory 21 | 22 | import launch 23 | from launch.actions import DeclareLaunchArgument 24 | from launch.substitutions import LaunchConfiguration 25 | from launch_ros.actions import ComposableNodeContainer 26 | from launch_ros.descriptions import ComposableNode 27 | 28 | 29 | def generate_launch_description(): 30 | launch_args = [ 31 | DeclareLaunchArgument( 32 | 'image_height', 33 | default_value='720', 34 | description='The height of the input image'), 35 | DeclareLaunchArgument( 36 | 'image_width', 37 | default_value='1280', 38 | description='The width of the input image'), 39 | DeclareLaunchArgument( 40 | 'featnet_engine_file_path', 41 | default_value='', 42 | description='The absolute path to the Bi3D Featnet TensorRT engine plan'), 43 | DeclareLaunchArgument( 44 | 'segnet_engine_file_path', 45 | default_value='', 46 | description='The absolute path to the Bi3D Segnet TensorRT engine plan'), 47 | DeclareLaunchArgument( 48 | 'max_disparity_values', 49 | default_value='64', 50 | description='The maximum number of disparity values given for Bi3D inference'), 51 | ] 52 | 53 | # Bi3DNode parameters 54 | image_height = LaunchConfiguration('image_height') 55 | image_width = LaunchConfiguration('image_width') 56 | featnet_engine_file_path = LaunchConfiguration('featnet_engine_file_path') 57 | segnet_engine_file_path = LaunchConfiguration('segnet_engine_file_path') 58 | max_disparity_values = LaunchConfiguration('max_disparity_values') 59 | 60 | bi3d_node = ComposableNode( 61 | name='bi3d_node', 62 | package='isaac_ros_bi3d', 63 | plugin='nvidia::isaac_ros::bi3d::Bi3DNode', 64 | parameters=[{ 65 | 'image_height': image_height, 66 | 'image_width': image_width, 67 | 'featnet_engine_file_path': featnet_engine_file_path, 68 | 'segnet_engine_file_path': segnet_engine_file_path, 69 | 'max_disparity_values': max_disparity_values}], 70 | remappings=[ 71 | ('left_image_bi3d', 'infra1/image_rect_raw'), 72 | ('right_image_bi3d', 'infra2/image_rect_raw'), 73 | ('left_camera_info_bi3d', 'infra1/camera_info'), 74 | ('right_camera_info_bi3d', 'infra2/camera_info')] 75 | ) 76 | 77 | image_format_converter_node_left = ComposableNode( 78 | package='isaac_ros_image_proc', 79 | plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', 80 | name='image_format_node_left', 81 | parameters=[{ 82 | 'encoding_desired': 'rgb8', 83 | 'image_height': image_height, 84 | 'image_width': image_width, 85 | }], 86 | remappings=[ 87 | ('image_raw', 'infra1/image_rect_raw_mono'), 88 | ('image', 'infra1/image_rect_raw')] 89 | ) 90 | 91 | image_format_converter_node_right = ComposableNode( 92 | package='isaac_ros_image_proc', 93 | plugin='nvidia::isaac_ros::image_proc::ImageFormatConverterNode', 94 | name='image_format_node_right', 95 | parameters=[{ 96 | 'encoding_desired': 'rgb8', 97 | 'image_height': image_height, 98 | 'image_width': image_width, 99 | }], 100 | remappings=[ 101 | ('image_raw', 'infra2/image_rect_raw_mono'), 102 | ('image', 'infra2/image_rect_raw')] 103 | ) 104 | 105 | # RealSense 106 | realsense_config_file_path = os.path.join( 107 | get_package_share_directory('isaac_ros_bi3d'), 108 | 'config', 'realsense.yaml' 109 | ) 110 | 111 | realsense_node = ComposableNode( 112 | package='realsense2_camera', 113 | plugin='realsense2_camera::RealSenseNodeFactory', 114 | parameters=[realsense_config_file_path], 115 | remappings=[ 116 | ('infra1/image_rect_raw', 'infra1/image_rect_raw_mono'), 117 | ('infra2/image_rect_raw', 'infra2/image_rect_raw_mono') 118 | ] 119 | ) 120 | 121 | container = ComposableNodeContainer( 122 | name='bi3d_container', 123 | namespace='bi3d', 124 | package='rclcpp_components', 125 | executable='component_container_mt', 126 | composable_node_descriptions=[bi3d_node, 127 | image_format_converter_node_left, 128 | image_format_converter_node_right, 129 | realsense_node], 130 | output='screen', 131 | remappings=[ 132 | ('left_image_bi3d', 'infra1/image_rect_raw'), 133 | ('right_image_bi3d', 'infra2/image_rect_raw')] 134 | ) 135 | 136 | final_launch_description = launch_args + [container] 137 | return (launch.LaunchDescription(final_launch_description)) 138 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 20 | 21 | 22 | 23 | isaac_ros_bi3d 24 | 3.2.5 25 | Bi3D inference network for ROS 26 | 27 | Isaac ROS Maintainers 28 | Apache-2.0 29 | https://developer.nvidia.com/isaac-ros-gems/ 30 | Peter Du 31 | 32 | ament_cmake_auto 33 | 34 | rclcpp 35 | rclcpp_components 36 | isaac_ros_nitros 37 | isaac_ros_image_proc 38 | isaac_ros_stereo_image_proc 39 | isaac_ros_nitros_image_type 40 | isaac_ros_nitros_camera_info_type 41 | isaac_ros_nitros_disparity_image_type 42 | isaac_ros_nitros_bi3d_inference_param_array_type 43 | 44 | isaac_ros_common 45 | 46 | gxf_isaac_bi3d 47 | gxf_isaac_sgm 48 | 49 | ament_lint_auto 50 | ament_lint_common 51 | isaac_ros_test 52 | isaac_ros_bi3d_interfaces 53 | ament_cmake_gtest 54 | 55 | 56 | ament_cmake 57 | 58 | 59 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/scripts/isaac_ros_bi3d_visualizer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 4 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 5 | # 6 | # Licensed under the Apache License, Version 2.0 (the "License"); 7 | # you may not use this file except in compliance with the License. 8 | # You may obtain a copy of the License at 9 | # 10 | # http://www.apache.org/licenses/LICENSE-2.0 11 | # 12 | # Unless required by applicable law or agreed to in writing, software 13 | # distributed under the License is distributed on an "AS IS" BASIS, 14 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | # See the License for the specific language governing permissions and 16 | # limitations under the License. 17 | # 18 | # SPDX-License-Identifier: Apache-2.0 19 | 20 | # This script plays a rosbag with prerecorded images and sends them to the Bi3DNode for inference, 21 | # then either saves the output prediction to spcified location as an image, or displays 22 | # it to the screen 23 | 24 | import argparse 25 | import subprocess 26 | 27 | import cv2 28 | import cv_bridge 29 | import rclpy 30 | from rclpy.node import Node 31 | from stereo_msgs.msg import DisparityImage 32 | 33 | 34 | def get_args(): 35 | parser = argparse.ArgumentParser(description='Bi3D Node Visualizer') 36 | parser.add_argument('--save_image', action='store_true', help='Save output or display it', 37 | default=False) 38 | parser.add_argument('--enable_rosbag', action='store_true', help='Save output or display it', 39 | default=False) 40 | parser.add_argument('--max_disparity_value', type=int, 41 | help='Maximium disparity value given to Bi3D Node', default=18) 42 | parser.add_argument('--result_path', default='/workspaces/isaac_ros-dev/src/bi3d_output.png', 43 | help='Absolute path to save your result.') 44 | parser.add_argument('--disparity_topic', default='bi3d_node/bi3d_output', 45 | help='Topic name for disparity output.') 46 | parser.add_argument('--rosbag_path', 47 | default='/workspaces/isaac_ros-dev/src/' 48 | 'isaac_ros_depth_segmentation/resources/' 49 | 'rosbags/bi3dnode_rosbag', 50 | help='Absolute path to your rosbag.') 51 | args = parser.parse_args() 52 | return args 53 | 54 | 55 | class Bi3DVisualizer(Node): 56 | 57 | def __init__(self, args): 58 | super().__init__('bi3d_visualizer') 59 | self.args = args 60 | self.encoding = 'rgb8' 61 | self._bridge = cv_bridge.CvBridge() 62 | self._bi3d_sub = self.create_subscription( 63 | DisparityImage, self.args.disparity_topic, self.bi3d_callback, 10) 64 | if self.args.enable_rosbag: 65 | subprocess.Popen('ros2 bag play --loop ' + self.args.rosbag_path, shell=True) 66 | self.image_scale = 255.0 67 | 68 | def bi3d_callback(self, data): 69 | self.get_logger().info('Receiving Bi3D output') 70 | bi3d_image = self._bridge.imgmsg_to_cv2(data.image) 71 | bi3d_image = bi3d_image/self.args.max_disparity_value 72 | if self.args.save_image: 73 | cv2.imwrite(self.args.result_path, bi3d_image*self.image_scale) 74 | else: 75 | cv2.imshow('bi3d_output', bi3d_image) 76 | cv2.waitKey(1) 77 | 78 | 79 | def main(): 80 | args = get_args() 81 | rclpy.init() 82 | rclpy.spin(Bi3DVisualizer(args)) 83 | 84 | 85 | if __name__ == '__main__': 86 | main() 87 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/test/bi3d_node_test.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include 19 | #include "bi3d_node.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | // Objective: to cover code lines where exceptions are thrown 23 | // Approach: send Invalid Arguments for node parameters to trigger the exception 24 | 25 | 26 | TEST(bi3d_node_test, test_featnet_engine_file_path) 27 | { 28 | rclcpp::init(0, nullptr); 29 | rclcpp::NodeOptions options; 30 | options.append_parameter_override("featnet_engine_file_path", ""); 31 | EXPECT_THROW( 32 | { 33 | try { 34 | nvidia::isaac_ros::bi3d::Bi3DNode bi3d_node(options); 35 | } catch (const std::invalid_argument & e) { 36 | EXPECT_THAT(e.what(), testing::HasSubstr("Empty featnet_engine_file_path")); 37 | throw; 38 | } catch (const rclcpp::exceptions::InvalidParameterValueException & e) { 39 | EXPECT_THAT(e.what(), testing::HasSubstr("No parameter value set")); 40 | throw; 41 | } 42 | }, std::invalid_argument); 43 | rclcpp::shutdown(); 44 | } 45 | 46 | TEST(bi3d_node_test, test_segnet_engine_file_path) 47 | { 48 | rclcpp::init(0, nullptr); 49 | rclcpp::NodeOptions options; 50 | options.append_parameter_override("featnet_engine_file_path", "path_to_featnet_engine"); 51 | options.append_parameter_override("segnet_engine_file_path", ""); 52 | EXPECT_THROW( 53 | { 54 | try { 55 | nvidia::isaac_ros::bi3d::Bi3DNode bi3d_node(options); 56 | } catch (const std::invalid_argument & e) { 57 | EXPECT_THAT(e.what(), testing::HasSubstr("Empty segnet_engine_file_path")); 58 | throw; 59 | } catch (const rclcpp::exceptions::InvalidParameterValueException & e) { 60 | EXPECT_THAT(e.what(), testing::HasSubstr("No parameter value set")); 61 | throw; 62 | } 63 | }, std::invalid_argument); 64 | rclcpp::shutdown(); 65 | } 66 | 67 | TEST(bi3d_node_test, test_disparity_values) 68 | { 69 | rclcpp::init(0, nullptr); 70 | rclcpp::NodeOptions options; 71 | options.append_parameter_override("featnet_engine_file_path", "path_to_featnet_engine"); 72 | options.append_parameter_override("segnet_engine_file_path", "path_to_segnet_engine"); 73 | options.append_parameter_override("max_disparity_values", 3); 74 | EXPECT_THROW( 75 | { 76 | try { 77 | nvidia::isaac_ros::bi3d::Bi3DNode bi3d_node(options); 78 | } catch (const std::invalid_argument & e) { 79 | EXPECT_THAT(e.what(), testing::HasSubstr("Invalid disparity values:")); 80 | throw; 81 | } catch (const rclcpp::exceptions::InvalidParameterValueException & e) { 82 | EXPECT_THAT(e.what(), testing::HasSubstr("No parameter value set")); 83 | throw; 84 | } 85 | }, std::invalid_argument); 86 | rclcpp::shutdown(); 87 | } 88 | 89 | 90 | int main(int argc, char ** argv) 91 | { 92 | testing::InitGoogleTest(&argc, argv); 93 | return RUN_ALL_TESTS(); 94 | } 95 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/test/camera_info.json: -------------------------------------------------------------------------------- 1 | { 2 | "header": { 3 | "frame_id": "tf_camera" 4 | }, 5 | "width": 960, 6 | "height": 576, 7 | "distortion_model": "plumb_bob", 8 | "D": [ 9 | 0.0, 10 | 0.0, 11 | 0.0, 12 | 0.0, 13 | 0.0 14 | ], 15 | "K": [ 16 | 434.943999, 17 | 0.000000, 18 | 651.073921, 19 | 0.0, 20 | 431.741273, 21 | 441.878037, 22 | 0.0, 23 | 0.0, 24 | 1.0 25 | ], 26 | "R": [ 27 | 1.0, 28 | 0.0, 29 | 0.0, 30 | 0.0, 31 | 1.0, 32 | 0.0, 33 | 0.0, 34 | 0.0, 35 | 1.0 36 | ], 37 | "P": [ 38 | 434.943999, 39 | 0.0, 40 | 651.073921, 41 | 160.0, 42 | 0.0, 43 | 431.741273, 44 | 441.878037, 45 | 0.0, 46 | 0.0, 47 | 0.0, 48 | 1.0, 49 | 0.0 50 | ] 51 | } -------------------------------------------------------------------------------- /isaac_ros_bi3d/test/dummy_featnet_model.onnx: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:6a354f4bd6c7165bf962341f1520a0918547a58e4466ab8481f3d0efa8c80867 3 | size 768860 4 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/test/dummy_segnet_model.onnx: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:a737e215b87bc87b6a33ba0c49e9416938305bdbf3bb8d5b3e0871cbd57bf392 3 | size 32288957 4 | -------------------------------------------------------------------------------- /isaac_ros_bi3d/test/isaac_ros_bi3d_test.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import os 19 | import subprocess 20 | import time 21 | 22 | from isaac_ros_test import IsaacROSBaseTest, JSONConversion 23 | 24 | from launch_ros.actions import ComposableNodeContainer 25 | from launch_ros.descriptions import ComposableNode 26 | 27 | import pytest 28 | import rclpy 29 | 30 | from sensor_msgs.msg import CameraInfo, Image 31 | from stereo_msgs.msg import DisparityImage 32 | 33 | 34 | @pytest.mark.rostest 35 | def generate_test_description(): 36 | dir_path = os.path.dirname(os.path.realpath(__file__)) 37 | featnet_engine_file_path = '/tmp/dummy_bi3dnet_featnet.engine' 38 | segnet_engine_file_path = '/tmp/dummy_bi3dnet_segnet.engine' 39 | 40 | if not os.path.isfile(featnet_engine_file_path): 41 | args = [ 42 | '/usr/src/tensorrt/bin/trtexec', 43 | f'--saveEngine={featnet_engine_file_path}', 44 | f'--onnx={dir_path}/dummy_featnet_model.onnx' 45 | ] 46 | print('Generating model engine file by command: ', ' '.join(args)) 47 | result = subprocess.run( 48 | args, 49 | env=os.environ, 50 | stdout=subprocess.PIPE, 51 | stderr=subprocess.PIPE 52 | ) 53 | if result.returncode != 0: 54 | raise Exception( 55 | f'Failed to convert with status: {result.returncode}.\n' 56 | f'stderr:\n' + result.stderr.decode('utf-8') 57 | ) 58 | if not os.path.isfile(segnet_engine_file_path): 59 | args = [ 60 | '/usr/src/tensorrt/bin/trtexec', 61 | f'--saveEngine={segnet_engine_file_path}', 62 | f'--onnx={dir_path}/dummy_segnet_model.onnx' 63 | ] 64 | print('Generating model engine file by command: ', ' '.join(args)) 65 | result = subprocess.run( 66 | args, 67 | env=os.environ, 68 | stdout=subprocess.PIPE, 69 | stderr=subprocess.PIPE 70 | ) 71 | if result.returncode != 0: 72 | raise Exception( 73 | f'Failed to convert with status: {result.returncode}.\n' 74 | f'stderr:\n' + result.stderr.decode('utf-8') 75 | ) 76 | 77 | bi3d_node = ComposableNode( 78 | name='bi3d', 79 | package='isaac_ros_bi3d', 80 | plugin='nvidia::isaac_ros::bi3d::Bi3DNode', 81 | namespace=IsaacROSBi3DTest.generate_namespace(), 82 | parameters=[{'featnet_engine_file_path': featnet_engine_file_path, 83 | 'segnet_engine_file_path': segnet_engine_file_path, 84 | 'featnet_output_layers_name': ['97'], 85 | 'segnet_output_layers_name': ['294'], 86 | 'max_disparity_values': 1, 87 | 'disparity_values': [18]}] 88 | ) 89 | 90 | container = ComposableNodeContainer( 91 | name='bi3d_container', 92 | namespace='', 93 | package='rclcpp_components', 94 | executable='component_container_mt', 95 | composable_node_descriptions=[bi3d_node], 96 | output='screen', 97 | arguments=['--ros-args', '--log-level', 'info'] 98 | ) 99 | return IsaacROSBi3DTest.generate_test_description([container]) 100 | 101 | 102 | class IsaacROSBi3DTest(IsaacROSBaseTest): 103 | IMAGE_HEIGHT = 576 104 | IMAGE_WIDTH = 960 105 | TIMEOUT = 300 106 | GXF_WAIT_SEC = 10 107 | FEATNET_ENGINE_FILE_PATH = '/tmp/dummy_bi3dnet_featnet.engine' 108 | SEGNET_ENGINE_FILE_PATH = '/tmp/dummy_bi3dnet_segnet.engine' 109 | CAMERA_INFO_PATH = os.path.dirname(os.path.realpath(__file__)) + '/camera_info.json' 110 | 111 | def _create_image(self, name): 112 | image = Image() 113 | image.height = self.IMAGE_HEIGHT 114 | image.width = self.IMAGE_WIDTH 115 | image.encoding = 'rgb8' 116 | image.is_bigendian = False 117 | image.step = self.IMAGE_WIDTH * 3 118 | image.data = [0] * self.IMAGE_HEIGHT * self.IMAGE_WIDTH * 3 119 | image.header.frame_id = name 120 | return image 121 | 122 | def test_image_bi3d(self): 123 | end_time = time.time() + self.TIMEOUT 124 | while time.time() < end_time: 125 | if os.path.isfile(self.FEATNET_ENGINE_FILE_PATH) and \ 126 | os.path.isfile(self.SEGNET_ENGINE_FILE_PATH): 127 | break 128 | self.assertTrue(os.path.isfile(self.FEATNET_ENGINE_FILE_PATH), 129 | 'Featnet engine file was not generated in time.') 130 | self.assertTrue(os.path.isfile(self.SEGNET_ENGINE_FILE_PATH), 131 | 'Segnet engine file was not generated in time.') 132 | 133 | time.sleep(self.GXF_WAIT_SEC) 134 | 135 | received_messages = {} 136 | 137 | self.generate_namespace_lookup( 138 | ['left_image_bi3d', 'right_image_bi3d', 'left_camera_info_bi3d', 139 | 'right_camera_info_bi3d', 'bi3d_node/bi3d_output']) 140 | 141 | subs = self.create_logging_subscribers( 142 | [('bi3d_node/bi3d_output', DisparityImage)], received_messages) 143 | 144 | image_left_pub = self.node.create_publisher( 145 | Image, self.namespaces['left_image_bi3d'], self.DEFAULT_QOS 146 | ) 147 | image_right_pub = self.node.create_publisher( 148 | Image, self.namespaces['right_image_bi3d'], self.DEFAULT_QOS 149 | ) 150 | camera_info_left = self.node.create_publisher( 151 | CameraInfo, self.namespaces['left_camera_info_bi3d'], self.DEFAULT_QOS 152 | ) 153 | camera_info_right = self.node.create_publisher( 154 | CameraInfo, self.namespaces['right_camera_info_bi3d'], self.DEFAULT_QOS 155 | ) 156 | 157 | try: 158 | left_image = self._create_image('left_image') 159 | right_image = self._create_image('right_image') 160 | camera_info = JSONConversion.load_camera_info_from_json(self.CAMERA_INFO_PATH) 161 | 162 | end_time = time.time() + self.TIMEOUT 163 | done = False 164 | 165 | while time.time() < end_time: 166 | image_left_pub.publish(left_image) 167 | image_right_pub.publish(right_image) 168 | camera_info_left.publish(camera_info) 169 | camera_info_right.publish(camera_info) 170 | 171 | rclpy.spin_once(self.node, timeout_sec=0.1) 172 | 173 | if 'bi3d_node/bi3d_output' in received_messages: 174 | done = True 175 | break 176 | self.assertTrue(done, 'Didnt recieve output on bi3d_node/bi3d_output topic') 177 | 178 | bi3d_output = received_messages['bi3d_node/bi3d_output'] 179 | self.assertEqual(bi3d_output.image.encoding, '32FC1') 180 | self.assertEqual(bi3d_output.image.height, self.IMAGE_HEIGHT) 181 | self.assertEqual(bi3d_output.image.width, self.IMAGE_WIDTH) 182 | self.assertAlmostEqual(bi3d_output.f, 434.9440002) 183 | self.assertAlmostEqual(bi3d_output.t, -0.3678634) 184 | 185 | finally: 186 | [self.node.destroy_subscription(sub) for sub in subs] 187 | self.node.destroy_publisher(image_left_pub) 188 | self.node.destroy_publisher(image_right_pub) 189 | -------------------------------------------------------------------------------- /isaac_ros_nitros_bi3d_inference_param_array_type/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | cmake_minimum_required(VERSION 3.22.1) 19 | project(isaac_ros_nitros_bi3d_inference_param_array_type LANGUAGES C CXX) 20 | 21 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 22 | add_compile_options(-Wall -Wextra -Wpedantic) 23 | endif() 24 | 25 | find_package(ament_cmake_auto REQUIRED) 26 | ament_auto_find_build_dependencies() 27 | 28 | # NVTX 29 | option(USE_NVTX "Enable NVTX markers for improved profiling (if available)" ON) 30 | if(USE_NVTX) 31 | add_definitions(-DUSE_NVTX) 32 | link_directories("${CUDA_TOOLKIT_ROOT_DIR}/lib64") 33 | link_libraries("nvToolsExt") 34 | endif() 35 | 36 | # NitrosBi3DInferenceParamArray 37 | ament_auto_add_library(${PROJECT_NAME} SHARED src/nitros_bi3d_inference_param_array.cpp) 38 | 39 | if(BUILD_TESTING) 40 | # Install test/config directory 41 | install(DIRECTORY test/config DESTINATION share/${PROJECT_NAME}/test) 42 | 43 | # NitrosBi3DInferenceParamArrayForwardNode 44 | ament_auto_add_library(nitros_bi3d_inference_param_array_forward_node SHARED test/src/nitros_bi3d_inference_param_array_forward_node.cpp) 45 | target_link_libraries(nitros_bi3d_inference_param_array_forward_node ${PROJECT_NAME}) 46 | set_target_properties(nitros_bi3d_inference_param_array_forward_node PROPERTIES BUILD_RPATH_USE_ORIGIN TRUE) 47 | set_target_properties(nitros_bi3d_inference_param_array_forward_node PROPERTIES INSTALL_RPATH_USE_LINK_PATH TRUE) 48 | rclcpp_components_register_nodes(nitros_bi3d_inference_param_array_forward_node 49 | "nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArrayForwardNode") 50 | set(node_plugins "${node_plugins}nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArrayForwardNode;\ 51 | $\n") 52 | 53 | find_package(ament_lint_auto REQUIRED) 54 | ament_lint_auto_find_test_dependencies() 55 | 56 | 57 | # The FindPythonInterp and FindPythonLibs modules are removed 58 | if(POLICY CMP0148) 59 | cmake_policy(SET CMP0148 OLD) 60 | endif() 61 | 62 | find_package(launch_testing_ament_cmake REQUIRED) 63 | add_launch_test(test/isaac_ros_nitros_bi3d_inference_param_array_type_test_pol.py TIMEOUT "15") 64 | endif() 65 | 66 | 67 | # Embed versioning information into installed files 68 | ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common) 69 | include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake") 70 | generate_version_info(${PROJECT_NAME}) 71 | 72 | ament_auto_package() 73 | -------------------------------------------------------------------------------- /isaac_ros_nitros_bi3d_inference_param_array_type/include/isaac_ros_nitros_bi3d_inference_param_array_type/nitros_bi3d_inference_param_array.hpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #ifndef ISAAC_ROS_NITROS_BI3D_INFERENCE_PARAM_ARRAY_TYPE__NITROS_BI3D_INFERENCE_PARAM_ARRAY_HPP_ 19 | #define ISAAC_ROS_NITROS_BI3D_INFERENCE_PARAM_ARRAY_TYPE__NITROS_BI3D_INFERENCE_PARAM_ARRAY_HPP_ 20 | /* 21 | * Type adaptation for: 22 | * Nitros type: NitrosBi3DInferenceParamArray 23 | * ROS type: isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray 24 | */ 25 | 26 | #include 27 | 28 | #include "isaac_ros_nitros/types/nitros_format_agent.hpp" 29 | #include "isaac_ros_nitros/types/nitros_type_base.hpp" 30 | #include "isaac_ros_bi3d_interfaces/msg/bi3_d_inference_parameters_array.hpp" 31 | 32 | 33 | #include "rclcpp/type_adapter.hpp" 34 | 35 | 36 | namespace nvidia 37 | { 38 | namespace isaac_ros 39 | { 40 | namespace nitros 41 | { 42 | 43 | // Type 44 | struct NitrosBi3DInferenceParamArray; 45 | 46 | // Formats 47 | struct nitros_bi3d_inference_param_array_t 48 | { 49 | using MsgT = NitrosBi3DInferenceParamArray; 50 | static const inline std::string supported_type_name = "nitros_bi3d_inference_param_array"; 51 | }; 52 | 53 | // NITROS data type registration factory 54 | NITROS_TYPE_FACTORY_BEGIN(NitrosBi3DInferenceParamArray) 55 | // Supported data formats 56 | NITROS_FORMAT_FACTORY_BEGIN() 57 | NITROS_FORMAT_ADD(nitros_bi3d_inference_param_array_t) 58 | NITROS_FORMAT_FACTORY_END() 59 | // Required extensions 60 | NITROS_TYPE_EXTENSION_FACTORY_BEGIN() 61 | NITROS_TYPE_EXTENSION_ADD("isaac_ros_gxf", "gxf/lib/std/libgxf_std.so") 62 | NITROS_TYPE_EXTENSION_FACTORY_END() 63 | NITROS_TYPE_FACTORY_END() 64 | 65 | 66 | } // namespace nitros 67 | } // namespace isaac_ros 68 | } // namespace nvidia 69 | 70 | 71 | template<> 72 | struct rclcpp::TypeAdapter< 73 | nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArray, 74 | isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray> 75 | { 76 | using is_specialized = std::true_type; 77 | using custom_type = nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArray; 78 | using ros_message_type = isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray; 79 | 80 | static void convert_to_ros_message( 81 | const custom_type & source, 82 | ros_message_type & destination); 83 | 84 | static void convert_to_custom( 85 | const ros_message_type & source, 86 | custom_type & destination); 87 | }; 88 | 89 | RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE( 90 | nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArray, 91 | isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray); 92 | 93 | #endif // ISAAC_ROS_NITROS_BI3D_INFERENCE_PARAM_ARRAY_TYPE__NITROS_BI3D_INFERENCE_PARAM_ARRAY_HPP_ 94 | -------------------------------------------------------------------------------- /isaac_ros_nitros_bi3d_inference_param_array_type/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 20 | 21 | 22 | 23 | isaac_ros_nitros_bi3d_inference_param_array_type 24 | 3.2.5 25 | Isaac ROS Nitros Bi3D Inferece Param Array Type 26 | 27 | Isaac ROS Maintainers 28 | Apache-2.0 29 | https://developer.nvidia.com/isaac-ros-gems/ 30 | Peter Du 31 | 32 | ament_cmake 33 | 34 | rclcpp 35 | rclcpp_components 36 | isaac_ros_bi3d_interfaces 37 | isaac_ros_nitros 38 | 39 | isaac_ros_common 40 | 41 | ament_lint_auto 42 | ament_lint_common 43 | isaac_ros_test 44 | 45 | 46 | ament_cmake 47 | 48 | 49 | -------------------------------------------------------------------------------- /isaac_ros_nitros_bi3d_inference_param_array_type/src/nitros_bi3d_inference_param_array.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #pragma GCC diagnostic push 24 | #pragma GCC diagnostic ignored "-Wunused-parameter" 25 | #pragma GCC diagnostic ignored "-Wmissing-field-initializers" 26 | #pragma GCC diagnostic ignored "-Wpedantic" 27 | #include "gxf/core/entity.hpp" 28 | #include "gxf/core/gxf.h" 29 | #include "gxf/std/tensor.hpp" 30 | #include "gxf/std/timestamp.hpp" 31 | #pragma GCC diagnostic pop 32 | 33 | #include "isaac_ros_nitros_bi3d_inference_param_array_type/nitros_bi3d_inference_param_array.hpp" 34 | #include "isaac_ros_nitros/types/type_adapter_nitros_context.hpp" 35 | 36 | #include "rclcpp/rclcpp.hpp" 37 | 38 | 39 | constexpr char kEntityName[] = "memory_pool"; 40 | constexpr char kComponentName[] = "unbounded_allocator"; 41 | constexpr char kComponentTypeName[] = "nvidia::gxf::UnboundedAllocator"; 42 | 43 | void rclcpp::TypeAdapter< 44 | nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArray, 45 | isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray>::convert_to_ros_message( 46 | const custom_type & source, 47 | ros_message_type & destination) 48 | { 49 | nvidia::isaac_ros::nitros::nvtxRangePushWrapper( 50 | "NitrosTensorList::convert_to_ros_message", 51 | nvidia::isaac_ros::nitros::CLR_PURPLE); 52 | 53 | RCLCPP_DEBUG( 54 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), 55 | "[convert_to_ros_message] Conversion started for handle = %ld", source.handle); 56 | 57 | auto msg_entity = nvidia::gxf::Entity::Shared( 58 | nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), source.handle); 59 | auto gxf_tensor = msg_entity->get(); 60 | 61 | if (!gxf_tensor) { 62 | std::string error_msg = 63 | "[convert_to_ros_message] No tensor found: conversion from " 64 | "gxf::Tensor to ROS Bi3D Inference Parameters Array failed!"; 65 | RCLCPP_ERROR( 66 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.c_str()); 67 | throw std::runtime_error(error_msg.c_str()); 68 | } 69 | 70 | // Move data from GXF tensor to ROS message 71 | destination.disparity_values.resize(gxf_tensor.value()->size()); 72 | switch (gxf_tensor.value()->storage_type()) { 73 | case nvidia::gxf::MemoryStorageType::kHost: 74 | { 75 | std::memcpy( 76 | destination.disparity_values.data(), gxf_tensor.value()->pointer(), 77 | gxf_tensor.value()->size()); 78 | } 79 | break; 80 | case nvidia::gxf::MemoryStorageType::kDevice: 81 | { 82 | const cudaError_t cuda_error = cudaMemcpy( 83 | destination.disparity_values.data(), gxf_tensor.value()->pointer(), 84 | gxf_tensor.value()->size(), cudaMemcpyDeviceToHost); 85 | if (cuda_error != cudaSuccess) { 86 | std::stringstream error_msg; 87 | error_msg << 88 | "[convert_to_ros_message] cudaMemcpy failed for conversion from " 89 | "gxf::Tensor to ROS Bi3D Inference Parameters Array: " << 90 | cudaGetErrorName(cuda_error) << 91 | " (" << cudaGetErrorString(cuda_error) << ")"; 92 | RCLCPP_ERROR( 93 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); 94 | throw std::runtime_error(error_msg.str().c_str()); 95 | } 96 | } 97 | break; 98 | default: 99 | std::string error_msg = 100 | "[convert_to_ros_message] MemoryStorageType not supported: conversion from " 101 | "gxf::Tensor to ROS Bi3D Inference Parameters Array failed!"; 102 | RCLCPP_ERROR( 103 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.c_str()); 104 | throw std::runtime_error(error_msg.c_str()); 105 | } 106 | 107 | // Populate timestamp information back into ROS header 108 | auto input_timestamp = msg_entity->get("timestamp"); 109 | if (!input_timestamp) { // Fallback to label 'timestamp' 110 | input_timestamp = msg_entity->get(); 111 | } 112 | if (input_timestamp) { 113 | destination.header.stamp.sec = static_cast( 114 | input_timestamp.value()->acqtime / static_cast(1e9)); 115 | destination.header.stamp.nanosec = static_cast( 116 | input_timestamp.value()->acqtime % static_cast(1e9)); 117 | } 118 | 119 | // Set frame ID 120 | destination.header.frame_id = source.frame_id; 121 | 122 | RCLCPP_DEBUG( 123 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), 124 | "[convert_to_ros_message] Conversion completed"); 125 | 126 | nvidia::isaac_ros::nitros::nvtxRangePopWrapper(); 127 | } 128 | 129 | 130 | void rclcpp::TypeAdapter< 131 | nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArray, 132 | isaac_ros_bi3d_interfaces::msg::Bi3DInferenceParametersArray>::convert_to_custom( 133 | const ros_message_type & source, custom_type & destination) 134 | { 135 | nvidia::isaac_ros::nitros::nvtxRangePushWrapper( 136 | "NitrosBi3DInferenceParamArray::convert_to_custom", 137 | nvidia::isaac_ros::nitros::CLR_PURPLE); 138 | 139 | RCLCPP_DEBUG( 140 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), 141 | "[convert_to_custom] Conversion started"); 142 | 143 | // Get pointer to allocator component 144 | gxf_uid_t cid; 145 | nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid( 146 | kEntityName, kComponentName, kComponentTypeName, cid); 147 | 148 | auto maybe_allocator_handle = 149 | nvidia::gxf::Handle::Create( 150 | nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), cid); 151 | if (!maybe_allocator_handle) { 152 | std::stringstream error_msg; 153 | error_msg << 154 | "[convert_to_custom] Failed to get allocator's handle: " << 155 | GxfResultStr(maybe_allocator_handle.error()); 156 | RCLCPP_ERROR( 157 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); 158 | throw std::runtime_error(error_msg.str().c_str()); 159 | } 160 | auto allocator_handle = maybe_allocator_handle.value(); 161 | 162 | auto message = nvidia::gxf::Entity::New( 163 | nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext()); 164 | if (!message) { 165 | std::stringstream error_msg; 166 | error_msg << 167 | "[convert_to_custom] Error initializing new message entity: " << 168 | GxfResultStr(message.error()); 169 | RCLCPP_ERROR( 170 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); 171 | throw std::runtime_error(error_msg.str().c_str()); 172 | } 173 | 174 | auto gxf_tensor = message->add("bi3d_inference_disparities"); 175 | std::array dims = {static_cast(source.disparity_values.size())}; 177 | 178 | nvidia::gxf::Expected result; 179 | 180 | // Bi3D disparity value tensor must be on host 181 | nvidia::gxf::MemoryStorageType storage_type = nvidia::gxf::MemoryStorageType::kHost; 182 | 183 | // Initializing GXF tensor 184 | nvidia::gxf::PrimitiveType type = 185 | static_cast(nvidia::gxf::PrimitiveType::kInt32); 186 | result = gxf_tensor.value()->reshape( 187 | nvidia::gxf::Shape(dims, 1), storage_type, allocator_handle); 188 | 189 | if (!result) { 190 | std::stringstream error_msg; 191 | error_msg << 192 | "[convert_to_custom] Error initializing GXF tensor of type " << 193 | static_cast(type) << ": " << 194 | GxfResultStr(result.error()); 195 | RCLCPP_ERROR( 196 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); 197 | throw std::runtime_error(error_msg.str().c_str()); 198 | } 199 | 200 | // Sort disparity values data 201 | std::vector sorted_data(begin(source.disparity_values), end(source.disparity_values)); 202 | sort(sorted_data.begin(), sorted_data.end()); 203 | 204 | // Copy to GXF tensor 205 | const cudaMemcpyKind operation = cudaMemcpyHostToHost; 206 | const cudaError_t cuda_error = cudaMemcpy( 207 | gxf_tensor.value()->pointer(), 208 | sorted_data.data(), 209 | gxf_tensor.value()->size(), 210 | operation); 211 | if (cuda_error != cudaSuccess) { 212 | std::stringstream error_msg; 213 | error_msg << 214 | "[convert_to_custom] cudaMemcpy failed for copying data from " 215 | "Bi3D ROS Float Array to GXF Tensor: " << 216 | cudaGetErrorName(cuda_error) << 217 | " (" << cudaGetErrorString(cuda_error) << ")"; 218 | RCLCPP_ERROR( 219 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); 220 | throw std::runtime_error(error_msg.str().c_str()); 221 | } 222 | 223 | // Add timestamp to the message 224 | uint64_t input_timestamp = 225 | source.header.stamp.sec * static_cast(1e9) + 226 | source.header.stamp.nanosec; 227 | auto output_timestamp = message->add("timestamp"); 228 | if (!output_timestamp) { 229 | std::stringstream error_msg; 230 | error_msg << "[convert_to_custom] Failed to add a timestamp component to message: " << 231 | GxfResultStr(output_timestamp.error()); 232 | RCLCPP_ERROR( 233 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), error_msg.str().c_str()); 234 | throw std::runtime_error(error_msg.str().c_str()); 235 | } 236 | output_timestamp.value()->acqtime = input_timestamp; 237 | 238 | // Set frame ID 239 | destination.frame_id = source.header.frame_id; 240 | 241 | // Set message entity 242 | destination.handle = message->eid(); 243 | GxfEntityRefCountInc( 244 | nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getContext(), message->eid()); 245 | 246 | RCLCPP_DEBUG( 247 | rclcpp::get_logger("NitrosBi3DInferenceParamArray"), 248 | "[convert_to_custom] Conversion completed"); 249 | 250 | nvidia::isaac_ros::nitros::nvtxRangePopWrapper(); 251 | } 252 | -------------------------------------------------------------------------------- /isaac_ros_nitros_bi3d_inference_param_array_type/test/config/test_forward_node.yaml: -------------------------------------------------------------------------------- 1 | %YAML 1.2 2 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 3 | # Copyright (c) 2022-2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | # SPDX-License-Identifier: Apache-2.0 18 | --- 19 | name: forward 20 | components: 21 | - name: input 22 | type: nvidia::gxf::DoubleBufferReceiver 23 | parameters: 24 | capacity: 1 25 | - name: output 26 | type: nvidia::gxf::DoubleBufferTransmitter 27 | - type: nvidia::isaac_ros::MessageForward 28 | parameters: 29 | in: input 30 | out: output 31 | - type: nvidia::gxf::MessageAvailableSchedulingTerm 32 | parameters: 33 | receiver: input 34 | min_size: 1 35 | --- 36 | name: sink 37 | components: 38 | - name: input 39 | type: nvidia::gxf::DoubleBufferReceiver 40 | parameters: 41 | capacity: 1 42 | - type: nvidia::gxf::MessageAvailableSchedulingTerm 43 | parameters: 44 | receiver: input 45 | min_size: 1 46 | - name: sink 47 | type: nvidia::isaac_ros::MessageRelay 48 | parameters: 49 | source: input 50 | --- 51 | components: 52 | - type: nvidia::gxf::Connection 53 | parameters: 54 | source: forward/output 55 | target: sink/input 56 | --- 57 | components: 58 | - type: nvidia::gxf::GreedyScheduler 59 | parameters: 60 | clock: clock 61 | stop_on_deadlock: false 62 | check_recession_period_us: 100 63 | - name: clock 64 | type: nvidia::gxf::RealtimeClock -------------------------------------------------------------------------------- /isaac_ros_nitros_bi3d_inference_param_array_type/test/isaac_ros_nitros_bi3d_inference_param_array_type_test_pol.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | # Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | # 16 | # SPDX-License-Identifier: Apache-2.0 17 | 18 | import time 19 | 20 | from isaac_ros_bi3d_interfaces.msg import Bi3DInferenceParametersArray 21 | from isaac_ros_test import IsaacROSBaseTest 22 | 23 | import launch 24 | from launch_ros.actions import ComposableNodeContainer 25 | from launch_ros.descriptions import ComposableNode 26 | import launch_testing 27 | import pytest 28 | import rclpy 29 | 30 | 31 | @pytest.mark.rostest 32 | def generate_test_description(): 33 | """Generate launch description with NitrosNode test node.""" 34 | test_ns = IsaacROSNitrosBi3DInferenceParamArrayTest.generate_namespace() 35 | container = ComposableNodeContainer( 36 | name='image_container', 37 | namespace='isaac_ros_nitros_container', 38 | package='rclcpp_components', 39 | executable='component_container_mt', 40 | composable_node_descriptions=[ 41 | ComposableNode( 42 | package='isaac_ros_nitros_bi3d_inference_param_array_type', 43 | plugin='nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArrayForwardNode', 44 | name='NitrosBi3DInferenceParamArrayForwardNode', 45 | namespace=test_ns, 46 | parameters=[{ 47 | 'compatible_format': 'nitros_bi3d_inference_param_array' 48 | }] 49 | ), 50 | ], 51 | output='both', 52 | arguments=['--ros-args', '--log-level', 'info'], 53 | ) 54 | 55 | return IsaacROSNitrosBi3DInferenceParamArrayTest.generate_test_description([ 56 | container, 57 | launch.actions.TimerAction( 58 | period=2.5, actions=[launch_testing.actions.ReadyToTest()]) 59 | ]) 60 | 61 | 62 | class IsaacROSNitrosBi3DInferenceParamArrayTest(IsaacROSBaseTest): 63 | """ 64 | Proof-of-Life Test for Isaac ROS Nitros Node. 65 | 66 | 1. Sets up ROS publisher to send Bi3DInferenceParametersArray values 67 | 2. Sets up ROS subscriber to listen to output channel of NitrosNode 68 | 3. Verify received messages 69 | """ 70 | 71 | def test_forward_node(self) -> None: 72 | self.node._logger.info('Starting Isaac ROS NitrosNode POL Test') 73 | 74 | # Subscriber 75 | received_messages = {} 76 | 77 | subscriber_topic_namespace = self.generate_namespace('topic_forward_output') 78 | test_subscribers = [ 79 | (subscriber_topic_namespace, Bi3DInferenceParametersArray) 80 | ] 81 | 82 | subs = self.create_logging_subscribers( 83 | subscription_requests=test_subscribers, 84 | received_messages=received_messages, 85 | use_namespace_lookup=False, 86 | accept_multiple_messages=True, 87 | add_received_message_timestamps=True 88 | ) 89 | 90 | # Publisher 91 | publisher_topic_namespace = self.generate_namespace('topic_forward_input') 92 | pub = self.node.create_publisher( 93 | Bi3DInferenceParametersArray, 94 | publisher_topic_namespace, 95 | self.DEFAULT_QOS) 96 | 97 | try: 98 | # Construct Bi3D inference parameters array message 99 | INPUT_DATA = [1, 2, 3] 100 | bi3d_inf_params = Bi3DInferenceParametersArray() 101 | bi3d_inf_params.disparity_values = INPUT_DATA 102 | 103 | # Start sending messages 104 | self.node.get_logger().info('Start publishing messages') 105 | sent_count = 0 106 | end_time = time.time() + 2.0 107 | while time.time() < end_time: 108 | sent_count += 1 109 | pub.publish(bi3d_inf_params) 110 | rclpy.spin_once(self.node, timeout_sec=0.2) 111 | 112 | # Conclude the test 113 | received_count = len(received_messages[subscriber_topic_namespace]) 114 | self.node._logger.info( 115 | f'Test Results:\n' 116 | f'# of Messages Sent: {sent_count}\n' 117 | f'# of Messages Received: {received_count}\n' 118 | f'# of Messages Dropped: {sent_count - received_count}\n' 119 | f'Message Drop Rate: {((sent_count-received_count)/sent_count)*100}%' 120 | ) 121 | 122 | self.assertGreater(len(received_messages[subscriber_topic_namespace]), 0) 123 | for i in range(len(INPUT_DATA)): 124 | self.assertEqual( 125 | received_messages[subscriber_topic_namespace][-1][0].disparity_values[i], 126 | bi3d_inf_params.disparity_values[i]) 127 | 128 | self.node._logger.info('Source and received messages are matched.') 129 | 130 | finally: 131 | [self.node.destroy_subscription(sub) for sub in subs] 132 | self.assertTrue(self.node.destroy_publisher(pub)) 133 | -------------------------------------------------------------------------------- /isaac_ros_nitros_bi3d_inference_param_array_type/test/src/nitros_bi3d_inference_param_array_forward_node.cpp: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES 2 | // Copyright (c) 2022-2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | // 16 | // SPDX-License-Identifier: Apache-2.0 17 | 18 | #include \ 19 | "isaac_ros_nitros_bi3d_inference_param_array_type/nitros_bi3d_inference_param_array.hpp" 20 | #include "isaac_ros_nitros/nitros_node.hpp" 21 | 22 | #include "rclcpp_components/register_node_macro.hpp" 23 | 24 | namespace nvidia 25 | { 26 | namespace isaac_ros 27 | { 28 | namespace nitros 29 | { 30 | 31 | constexpr char PACKAE_NAME[] = "isaac_ros_nitros_bi3d_inference_param_array_type"; 32 | constexpr char FORWARD_FORMAT[] = "nitros_bi3d_inference_param_array"; 33 | 34 | #pragma GCC diagnostic push 35 | #pragma GCC diagnostic ignored "-Wpedantic" 36 | class NitrosBi3DInferenceParamArrayForwardNode : public NitrosNode 37 | { 38 | public: 39 | explicit NitrosBi3DInferenceParamArrayForwardNode(const rclcpp::NodeOptions & options) 40 | : NitrosNode( 41 | options, 42 | // Application graph filename 43 | "test/config/test_forward_node.yaml", 44 | // I/O configuration map 45 | { 46 | {"forward/input", 47 | { 48 | .type = NitrosPublisherSubscriberType::NEGOTIATED, 49 | .qos = rclcpp::QoS(1), 50 | .compatible_data_format = FORWARD_FORMAT, 51 | .topic_name = "topic_forward_input", 52 | .use_compatible_format_only = true, 53 | } 54 | }, 55 | {"sink/sink", 56 | { 57 | .type = NitrosPublisherSubscriberType::NEGOTIATED, 58 | .qos = rclcpp::QoS(1), 59 | .compatible_data_format = FORWARD_FORMAT, 60 | .topic_name = "topic_forward_output", 61 | .use_compatible_format_only = true, 62 | } 63 | } 64 | }, 65 | // Extension specs 66 | {}, 67 | // Optimizer's rule filenames 68 | {}, 69 | // Extension so file list 70 | { 71 | {"gxf_isaac_message_compositor", "gxf/lib/libgxf_isaac_message_compositor.so"} 72 | }, 73 | // Test node package name 74 | PACKAE_NAME) 75 | { 76 | std::string compatible_format = declare_parameter("compatible_format", ""); 77 | if (!compatible_format.empty()) { 78 | config_map_["forward/input"].compatible_data_format = compatible_format; 79 | config_map_["sink/sink"].compatible_data_format = compatible_format; 80 | } 81 | 82 | registerSupportedType(); 83 | 84 | startNitrosNode(); 85 | } 86 | }; 87 | #pragma GCC diagnostic pop 88 | 89 | } // namespace nitros 90 | } // namespace isaac_ros 91 | } // namespace nvidia 92 | 93 | RCLCPP_COMPONENTS_REGISTER_NODE( 94 | nvidia::isaac_ros::nitros::NitrosBi3DInferenceParamArrayForwardNode) 95 | --------------------------------------------------------------------------------